// Listing of server source code

/*
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
*/


// filename : garbage.cpp
#include <math.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include "svsclass.h"
#include "Aria.h"

// size of the image
#define H 240
#define W 320
# define SIZE (long)320*(long)240*(long)4

// the client and server socket
ArSocket server,client;
// the main object the robot
ArRobot robot;
// the gripper object
ArGripper grip(&robot);

char str[100];

// for images 
svsVideoImages *videoObject; 
svsStereoImage *imageObject;

// initially the heading is zero
int head = 0;

// function prototypes
void sendImage();
void run();
void turn(int);
void moveD(int);
void drop();

char left_image[SIZE+104];
size_t size;

// for shuting down the program when CTRL+C is pressed
void shutdown(int signum)
{
printf("Closing video device and TCP connection ...\n");
videoObject->Stop();
videoObject->Close();
client.close();
server.close();
exit(0);
}

// the main function
int main(int argc, char **argv)
{
char left_image[SIZE+104];
struct sigaction sa;
memset(&sa,0,sizeof(sa));
sa.sa_handler = &shutdown;
sigaction(SIGINT,&sa,NULL);
// for serial connection
ArSerialConnection scon;

// running aria in single thread.
Aria::init(Aria::SIGHANDLE_THREAD);

// for opening the connection
if(scon.open() != 0)
{
printf("\n Could not open the connection.");
exit(1);
}
	
// setting the device connection
robot.setDeviceConnection(&scon);
// connecting the robot.
if(!robot.blockingConnect())
printf("\n Could not connect to the robot");

// opening the server socket
if(server.open(9990,ArSocket::TCP))
printf("\n Opened the Server Socket.");
else
{
printf("\n Unable to open the Server Socket.");
Aria::shutdown();
return 1;
}

// accepting the client
if(server.accept(&client))
printf("\n Connected to Client.");

// getting the video object
videoObject = getVideoObject(); 
svsImageParams *ip=videoObject->GetIP();
ip->width=W;
ip->height=H;

// setting the video parameters
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);

// error if happens
if (!ret) 
{
printf("Can't open frame grabber.\n"); 
client.close();
server.close();
return 0; 
} 
else 
printf("Opened frame grabber.\n");

// check param
if (!videoObject->CheckParams()) 
{
printf("Incorrect  Params\n");
videoObject->Close();
client.close();
server.close();
exit(1);
}

// setting the image param
videoObject->Start();
videoObject->SetColor(true);
videoObject->exposure=100;
videoObject->gain=100;
videoObject->blue=30;
videoObject->red=7;
videoObject->SetDigitization();

for ( int i = 0 ; i < 5; i ++ )
videoObject->GetImage(400);
robot.runAsync(true);
// enabling the motors.
robot.enableMotors();

// open the gripper
grip.gripOpen();
while(grip.isGripMoving()){}

sendImage();
client.write("READY",strlen("READY"));
	
while(true)
{
size = client.read(str,sizeof(str));
str[size] = '\0';
	
// image again
if (!strcmp("IMAGE",str))	
{
printf("\n%s",str);
sendImage();	
}

// stop motion
if(!strcmp("STOP",str))	robot.stop();
// exit if necessary
if (!strcmp("EXIT",str))
{
client.close();
server.close();
videoObject->Stop();
videoObject->Close();
robot.disconnect();
Aria::shutdown();
break;
}

// used to start object search
if (!strcmp("GO",str))
{
run();
}	
}
return 0;
}

// function used to send the image to the server
void sendImage()
{
// for one image
for (int j = 0 ; j < 1 ; j ++)
{
for (int z = 0 ; z < 10 ; z++)
imageObject = videoObject->GetImage(400);
if ( !imageObject)
{
printf("\n No image");
continue;
}
	
if ( !imageObject->haveColor)
{
videoObject->Stop();
videoObject->Close();
client.close();
server.close();
robot.disconnect();
Aria::shutdown();
exit(1);
}

memcpy(left_image,imageObject->color,SIZE);
long k = 0;
for (int i = 0; i < H ; i++)
{
for (int j = 0 ; j < W ; j++ )
{
// sending the RGB values
sprintf(str,"%c%c%c",left_image[k],left_image[k+1],left_image[k+2]);
client.write(str,3);
k = k+4;
}	
sprintf(str,"%c%c%c",char(0),char(0),char(0));
client.write(str,3);
}
}
}

// function to drop the object at the origin and come back to initial position
void drop()
{
robot.lock();
float x1 = robot.getX();
float y1 = robot.getY();
float th1 = robot.getTh();
// calculating angle
float th2 = atan2(y,x);

robot.unlock();	
turn(-th2);

// calculating distance
float dis_cal = sqrt(x1*x1+y1*y1);

int max = int(dis_cal)/250+1;
		
for ( int i = 0 ; i <= max ; i ++)
{
moveD(250);	
}

grip.gripOpen();
printf("\n Dropped the object");
while(grip.isGripMoving()){}
printf("\n Turning back.");
moveD(-250);	
turn((int)th1);
for(int i = 0 ; i < max ; i ++ )
moveD(250);
return;	
}


// function to turn robot by a
void turn(int a)
{
robot.lock();
robot.setHeading(a);	
robot.unlock();

while(true)
{
robot.lock();
if ( robot.isHeadingDone())
{
robot.unlock();
break;
}
ArUtil::sleep(50);
robot.unlock();
}	
}

// function to move robot by distance d
void moveD(int d)
{
robot.lock();
robot.move(d);	
robot.unlock();
while(true)
{
robot.lock();
if ( robot.isMoveDone())
{
robot.unlock();
break;
}
ArUtil::sleep(50);
robot.unlock();
}
}

// the function which runs when the robot searches the object
void run()
{
int angle = 0;
while(true)
{
// if beam broken close gripper
if (grip.getBreakBeamState() == 3)
{
robot.stop();
grip.gripClose();
while(grip.isGripMoving()){}
robot.unlock();
printf("\n Got the object");
drop();
}
	
for (int i = 0 ; i < 10 ; i ++ )
videoObject->GetImage(400);
// send image
sendImage();

// get instruction
size = client.read(str,sizeof(str));
str[size] = '\0';
printf("%s",str);
if (!strcmp(str,"STOP"))
break;

// get angle
sscanf(str,"%d",&angle);

printf("\nangle = %d",angle);
//set absolute heading
head += (int)angle;
head %= 360;
// turn the angle
turn(head);

if ( (int)angle == 45)
continue;
moveD(250);
}
return;
}

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

g++ -o garbage I$ARIA/include L$ARIA/lib I$SVS/src L$SVS/bin lAria ldl -pthread  -lm lsvs  lcap garbage.cpp

*/