// Server source code for 3D perception 

/*
Author: Srikanta Patnaik and Team 
Machine Intelligence Laboratory, University College of Engineering, Burla, India
Date of Writing: April, 2003; Version: 1.0
Book: Robot Cognition and Navigation: An Experiment with Mobile Robots
Publisher: Springer
ISBN: 978-3-540-23446-3
URL: http://www.springer.com/3-540-23446-2
*/


//Sampler.cpp
#include<stdio.h>
#include<stdlib.h>
#include<string.h>
#include<math.h>
#include<string>
#include"svsclass.h"
#include"Aria.h"
extern "C" {
#include"BotSpeak.h"
}

#define H 240
#define W 320

ArTime start;
ArTcpConnection tcpConn;
ArSerialConnection serConn;
ArRobot robot;
ArSocket server,client;
ArSonarDevice sonar;
int conType=0;
svsVideoImages *videoObject; // source of images
svsStereoImage *imageObject;
char filename[80],text[100];
double head=0.0;
char *numtext[]={ "first","second","third","forth","fifth","sixth","seventh","eigth","ninth","tenth","eleventh","twelth","thirteenth","fourteenth","fifteenth","sixteenth","seventeenth","eighteenth","ninteenth","twentieth","twenty first"};

char color_image[4*W*H+104];
class ConnHandler
{
public:
// Constructor
ConnHandler(ArRobot *robot);
// Destructor, its just empty
~ConnHandler(void) {}
 // to be called if the connection was made
void connected(void);
// to call if the connection failed
void connFail(void);
// to be called if the connection was lost
void disconnected(void);
protected:
// robot pointer
ArRobot *myRobot;
// the functor callbacks
ArFunctorC<ConnHandler> myConnectedCB;
ArFunctorC<ConnHandler> myConnFailCB;
ArFunctorC<ConnHandler> myDisconnectedCB;
};

/*
The constructor, note its use of contructor chaining to initalize the callbacks.
*/
ConnHandler::ConnHandler(ArRobot *robot) :
myConnectedCB(this, &ConnHandler::connected),  
myConnFailCB(this, &ConnHandler::connFail),
myDisconnectedCB(this, &ConnHandler::disconnected)

{
// set the robot poitner
myRobot = robot;

// add the callbacks to the robot
myRobot->addConnectCB(&myConnectedCB, ArListPos::FIRST);
myRobot->addFailedConnectCB(&myConnFailCB, ArListPos::FIRST);
myRobot->addDisconnectNormallyCB(&myDisconnectedCB, ArListPos::FIRST);
myRobot->addDisconnectOnErrorCB(&myDisconnectedCB, ArListPos::FIRST);
}

// just exit if the connection failed
void ConnHandler::connFail(void)
{
printf("Failed to connect.\n");
myRobot->stopRunning();
Aria::shutdown();
return;
}

// turn on motors, and off sonar, and off amigobot sounds, when connected
void ConnHandler::connected(void) 
{
printf("Connected\n");
myRobot->comInt(ArCommands::ENABLE, 1);
myRobot->comInt(ArCommands::SOUNDTOG, 0);
}

// lost connection, so just exit
void ConnHandler::disconnected(void)
{
printf("Lost connection\n");
printf("Closing video device \n");
myRobot->stopRunning();
videoObject->Stop();
videoObject->Close();  
server.close();
client.close();
Aria::shutdown();
exit(0);
}


void move_f(double d)
{ 
printf("move %.2f\n",d);
sprintf(text,"moving %d mm",(int)d);
bsSpeak(text);
bsFinishSpeaking();
robot.lock();
robot.move(d);
robot.unlock();
start.setToNow();
while (1)
{
robot.lock();
if (robot.isMoveDone())
{
robot.unlock();
break;
}
else
{ 
double range = sonar.currentReadingPolar(-20,20 ) - robot.getRobotRadius();
if(range<200)
{
printf("Unavoidable obstacle, bailing out");
robot.stopRunning();
videoObject->Stop();
videoObject->Close();  
Aria::shutdown();
exit(1); 
}
}
if (start.mSecSince() > 5000)
{
printf("turn timed out\n");
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(100);
}
}//move_f

void turn_f(double d)
{ 
printf("turn %.2f\n",d);
sprintf(text,"turning %d degrees.",abs((int)d));
bsSpeak(text);
bsFinishSpeaking();
head+=d;
robot.lock();
robot.setHeading(head);
robot.unlock();
start.setToNow();
while (1)
{
robot.lock();
if (robot.isHeadingDone(3.0))
{
robot.unlock();
break;
}
if (start.mSecSince() > 5000)
{
printf("turn timed out\n");
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(100);
}
}

void sendImage()
{
int ck = 0;
char data[15];
int size = client.read(data,sizeof(data));
data[size] = '\0';
if(!strcmp(data,"SEND"))
{
printf("\n%s\n",data);
int rep = 2;
while(rep-->0)
{
ck=0;
for(int i=0;i<H;i++)
{for(int j=0;j<W;j++)
{		
sprintf(data,"%c%c%c%c",color_image[ck],color_image[ck+1],color_image[ck+2],char(1));
//printf("\n%s",data);
client.write(data,4);//sizeof(data));
ck=ck+4;
}
} 
sprintf(data,"%c%c%c%c",char(0),char(0),char(0),char(0));
//printf("\n%s",data);
client.write(data,4);//sizeof(data));
}
}	
}

int main(int argc, char **argv) 
{
int num=0,rad;
double phi=0.0,dist=0.0,t;
char name[80];
FILE *fp;
/*  printf("Enter the number of Images:-");
scanf("%d",&num);
printf("Enter the radial distance:-");
scanf("%d",&rad);
printf("Enter file name :-");
scanf("%s",name);*/
 
ConnHandler ch(&robot);
Aria::init(Aria::SIGHANDLE_THREAD);
bsInit();

//socket connection

if (server.open(7774, ArSocket::TCP))
printf("Opened the server port\n");
else
{
printf("Failed to open the server port: %s\n",
server.getErrorStr().c_str());
return(-1);
}

// Lets wait for the client to connect to us.
if (server.accept(&client))
printf("Client has connected\n");
else
printf("Error in accepting a connection from the client: %s\n",
server.getErrorStr().c_str());


// socket connection established

char data[15];
char ackw[] = "OK"; 
int size = client.read(data,sizeof(data));
data[sizeof(data)] = '\0';
if(size>0)
{
printf("\n %s \n",data);
sscanf(data,"%d|%d|",&rad,&num);
printf("\n%d    %d\n",rad,num);
client.write(ackw,sizeof(ackw));
int size = client.read(name,sizeof(name));
name[size] = '\0';
printf("\n%s\n",name);
}
t=360.0/(double)num/3;
phi=(180.0-t)/2.0;
dist=2.0*(double)rad*cos(phi*3.14159/180.0);
printf("phi=%.2f  and dist=%.2f\n",phi,dist);
tcpConn.setPort();
int state=tcpConn.open("127.0.0.1",8101);
printf("TCP connection status = %d.\n",state);
if (state==0)
{
printf("Connecting to simulator through tcp.\n");
robot.setDeviceConnection(&tcpConn);
conType=0;
}
else
{
serConn.setPort();
printf("Could not connect to simulator, connecting to robot through serial.\n");
robot.setDeviceConnection(&serConn);
conType=1;
}

//start up the image grabber
if (conType==1)
{  
videoObject = getVideoObject(); 
//size
svsImageParams *ip=videoObject->GetIP();
ip->width=W;
ip->height=H;
//sample
videoObject->decimation=2;
videoObject->binning=2;
svsHasMMX=true;
ip->linelen=ip->width;
ip->lines=ip->height;
ip->vergence=0;

bool ret;
videoObject->ReadParams("/root/megad-75.ini");

ret = videoObject->Open(0);
if (!ret) {printf("Can't open frame grabber.\n"); return 0; } else printf("Opened frame grabber.\n");
if (!videoObject->CheckParams()) 
{
printf("Incorrect  Params\n");
videoObject->Close();
exit(1);
}
videoObject->Start();
videoObject->SetColor(true);
videoObject->exposure=100;
videoObject->gain=95;  //30;  //Color 30
videoObject->blue=20;
videoObject->red=20;
videoObject->SetDigitization();
}//Image grabber up and running        

robot.addRangeDevice(&sonar);
// try to connect, if we fail, the connection handler should bail
if (!robot.blockingConnect())
{
// this should have been taken care of by the connection handler
// but just in case
printf("asyncConnect failed because robot is not running in its own thread.\n");
Aria::shutdown();
return 1;
}
// run the robot in its own thread, so it gets and processes packets and such

bsSpeak("Starting sampler procedure. Stand aside.");
bsFinishSpeaking();

robot.runAsync(false);
//robot.setMaxTransVel(300);
//robot.setMaxRotVel(30);   

// just a big long set of printfs, direct motion commands and sleeps,
// it should be self-explanatory
head=0.0;
for(int ij=0;ij<num;ij++)
{
//capture_image();capture and store image
if(conType==1)
{ videoObject->Start();
while(1)
{  for(int i=0;i<5;i++) imageObject = videoObject->GetImage(400);        
if (imageObject != NULL)   
{    sprintf(filename,"/home/vidya/sampler/images/%s%d",name,ij);
if(imageObject->haveImages) { memcpy(color_image,imageObject->color,4*W*H);
imageObject->SaveToFile(filename);sendImage();
size = client.read(data,sizeof(data));
data[size]='\0';
printf("\n%s\n",data);
}
videoObject->Stop();
sprintf(text,"Saving %s image.",numtext[ij],name);
strcat(filename,".txt");
fp=fopen(filename,"w");
if (fp!=NULL)
{
robot.lock();
fprintf(fp,"At Image %s.\n",filename);
fprintf(fp,"Robot x-coordinate in mm:%f\n",robot.getX());
fprintf(fp,"Robot y-coordinate in mm:%f\n",robot.getY());
fprintf(fp,"Robot heading in degrees :%f\n",robot.getTh());
sprintf(data,"%4.0lf|%4.0lf|%3.0lf|",robot.getX(),robot.getY(),robot.getTh());
fclose(fp);
robot.unlock();
}
client.write(data,sizeof(data));
		
bsSpeak(text);
bsFinishSpeaking();
break;
}
}
       
}
turn_f(-1.0*phi);
move_f(dist);
turn_f(180.0-phi);
}
robot.comInt(ArCommands::ENABLE, 0);
robot.comInt(ArCommands::SONAR, 0);
printf("Done , exiting\n");
// shutdown
robot.stopRunning();
//Aria::shutdown();
return(0);
}

/*
Compilation, linking and execution: The following text commands are used for compilation, linking and execution. 

g++ -c -I$ARIA/include -I$BOTSPEAK/include -I$SVS/src sampler.cpp
g++ -o sampler -L$ARIA/lib -L$BOTSPEAK/lib -L$SVS/bin -lAria -ldl -lcap -lsvs -lbotspeak -lm -pthread sampler.o
*/