//  Code for Image-Server Program
/*
This program encodes the black and white image taken by the framegrabber of the server into the run-length encoded image and transmits it over the network using the socket.  
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
 


*/

/*imageserver.cpp*/
/* The run length encoded black & white image server for the Navigator client program */
#include <arpa/inet.h>
#include <assert.h>
#include <errno.h>
#include <netinet/in.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/wait.h>
#include <unistd.h>
#include <fcntl.h>
#include "svsclass.h"
//image size 
#define H 240
#define W 320

svsVideoImages *videoObject; 
svsStereoImage *imageObject;
int sfd,cfd; // file descriptors for the sockets used 
unsigned char left_image[W*H+104];

// Ctrl-C signal handler
void shutdown(int signum)
{
printf("Closing video device and TCP connection ...\n");
close(cfd);
close(sfd);
videoObject->Stop();
videoObject->Close();
exit(0);
}

int main(int argc, char **argv)
{
int k,acount;
unsigned char prev,count,zero=0;
//term signal handler
struct sigaction sa;
memset(&sa,0,sizeof(sa));
sa.sa_handler = &shutdown;
sigaction(SIGINT,&sa,NULL);  
videoObject = getVideoObject(); 
//size
svsImageParams *ip=videoObject->GetIP();
ip->width=W;
ip->height=H;
//sampling parameters
videoObject->decimation=2;
videoObject->binning=2;
svsHasMMX=true;
ip->linelen=ip->width;
ip->lines=ip->height;
ip->vergence=0;
bool ret;
  
//camera parameter file
videoObject->ReadParams("/root/megad-75.ini");

//Opening video framegrabber
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);
}

//Initialising server socket
struct sockaddr_in socket_address;
struct in_addr local_address;
uint16_t port;
int rval;
int server_socket;

/* Create a TCP socket.  */
port=(uint16_t)htons(4325);// port number 
local_address.s_addr=INADDR_ANY;
server_socket = socket (PF_INET, SOCK_STREAM, 0);
sfd=server_socket;

if (server_socket == -1)
 printf ("socket");

/*Construct a socket address structure for the local address on which 
you want to listen for connections. */
memset (&socket_address, 0, sizeof (socket_address));
socket_address.sin_family = AF_INET;
socket_address.sin_port = port;
socket_address.sin_addr = local_address;
/* Bind the socket to that address.  */
rval = bind (server_socket,(sockaddr *)&socket_address, sizeof (socket_address));
if (rval != 0) printf("error binding");
/*  Instruct the socket to accept connections.  */
rval = listen (server_socket, 10);
if (rval != 0) printf("error listening");

//(verbose) 
{
/* Display the local address and port number we're listening on.  */

socklen_t address_length;
    
 /* Find the socket's local address.  */
address_length = sizeof (socket_address);
rval = getsockname (server_socket, (sockaddr *)&socket_address, &address_length);
assert (rval == 0);
/* Print a message.  The port number needs to be converted from 
network byte order (big endian) to host byte order.  */
printf ("Image server listening on %s:%d\n", 
inet_ntoa (socket_address.sin_addr), 
 (int) ntohs (socket_address.sin_port));
}//verbose

//accept a connection from the Navigator client
{
struct sockaddr_in remote_address;
socklen_t address_length;
int connection;
pid_t child_pid;

/* Accept call blocks until a connection is ready.  */
address_length = sizeof (remote_address);
connection = accept (server_socket, (sockaddr *)&remote_address, 
&address_length);
if (connection == -1) {
/* The call to accept failed.  */
/* Something else went wrong.  */
printf("error accepting");
}//if
    
cfd=connection;

/* We have a connection.  Print a message */
socklen_t address_length;

/* Get the remote address of the connection.  */
address_length = sizeof (socket_address);
rval = getpeername (connection, (sockaddr *)& socket_address, 
& address_length);
assert (rval == 0);
/* Print a message.  */
printf ("Connection accepted from %s\n",
inet_ntoa (socket_address.sin_addr));

//client
{
/* Sending Video over the network to the Navigator Client */
videoObject->Start();
	
//Setting Camera parameters
//videoObject->SetRect(true);
/* for rectification using camera correction parameters generated after calibration */
videoObject->SetColor(true);
videoObject->exposure=100;
videoObject->gain=80;
videoObject->blue=0;
videoObject->red=0;
videoObject->SetDigitization();//necessary for MEGA-D camera

char fname[120];

while (1) 
{
//Grab an image form the frame grabber. 400ms is the wait time.
imageObject = videoObject->GetImage(400);
        
if (imageObject == NULL)   {  printf("No image, timed out...\n"); 
continue; }

k=0;count=0;acount=0;
memcpy(left_image,imageObject->left,W*H);
prev=left_image[0];

// Run length encoding algorithm
for(int i=0;i<H;i++)
{for(int j=0;j<W;j++)
{
if ((abs(left_image[k]-prev)>8)||(k>=H*W))
{
//Sending pixel color information to the client socket
write(cfd,&prev,1);
write(cfd,&count,1);
count=0;
prev=left_image[k];
}		  
count++;
k++;
}
//Signalling end of line of the image 
write(cfd,&zero,1);write(cfd,&zero,1);
}
}//end of while
}//end of client
}//end of accept
}//end of main



