/* * server.c - code for a sample remote device server that uses TCP * */
#ifndef unix
    #include <windows.h>
    #include <winsock.h> 
#else
    #define closesocket close
    #include <sys/types.h>
    #include <sys/socket.h>
    #include <netinet/in.h>
    #include <netdb.h>
#endif

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "pai.h"

#define PROTOPORT       6811 /* default protocol port number         */
#define QLEN            0    /* the server connect request queue size*/
#define BUFSIZE         100  /* maximum size for the command buffer  */

#define MaxSpeed        400  /* maximum translational speed of robot */
#define IniSpeed        100  /* initial translational speed of robot */
#define tooClose        10   /* minimum front distnat from obstacle  */

void ServiceClient(const int socket);
int MinSonarDist(int low, int high);

int main(){
  
/*	struct  hostent  *ptrh;  /* pointer to a host table entry        */
    struct  protoent *ptrp;  /* pointer to a protocol table entry    */
    struct  sockaddr_in sad; /* structure to hold server's address   */
    struct  sockaddr_in cad; /* structure to hold client's address   */
    int     sd, sd2;         /* socket descriptors                   */
    int     port;            /* protocol port number                 */
    int     alen;            /* length of address                    */

#ifdef WIN32
    WSADATA wsaData;
    WSAStartup(0x0101, &wsaData);
#endif

    memset((char *)&sad,0,sizeof(sad)); /* clear sockaddr structure */
    sad.sin_family = AF_INET;           /* set family to Internet   */
    sad.sin_addr.s_addr = INADDR_ANY;   /* set the local IP address */
    port = PROTOPORT;                   /* use default port number  */
    sad.sin_port = htons((u_short)port);
    /* Map TCP transport protocol name to protocol number */
    if ( ((int)(ptrp = getprotobyname("tcp"))) == 0) {
        printf("cannot map \"tcp\" to protocol number\n"); exit(1);
    }
    /* Create a socket */
    sd = socket(PF_INET, SOCK_STREAM, ptrp->p_proto);
    if (sd < 0) {
        printf("socket creation failed\n"); exit(1);
    }
    /* Bind a local address to the socket */
    if (bind(sd, (struct sockaddr *)&sad, sizeof(sad)) < 0) {
        printf("bind failed\n"); exit(1);
    }
    /* Specify size of request queue */
    if (listen(sd, QLEN) < 0) {
        printf("listen failed\n"); exit(1);
    }
    /* Main server loop - accept and handle requests */
    while (1) {
        alen = sizeof(cad);
        if ( (sd2=accept(sd, (struct sockaddr *)&cad, &alen)) < 0) {
            printf("accept failed, listening again\n"); continue;
        }
        printf("+OK A client connected\n");
        ServiceClient(sd2);
        printf("+OK A client disconnected\n");
        closesocket(sd2);
    }
    return(0);
}

int ReceiveLine(const int socket, char *line) {
    int     n;
    int     len=0;
    char    bch;
    n = recv(socket,&bch,1,0);
    while (n > 0) {
        /* printf("%c", bch); /* direct console debug */
        if (len>=BUFSIZE) return(-1); /* someone overruns buffer, disconnect! */
		if (bch!='\n'&&bch!='\r')
		{
			line[len]=bch;
			len++;
		}
		else
		{
			line[len]='\0';
            if (len>0) return(len);
            /* else - an empty string, skip it */
		}
        n = recv(socket,&bch,1,0);
    }
	return(n);
}

int SendLine(const int socket, const char *line) {
    return send(socket,line,strlen(line),0);
}

void ProcessCommand(const char *command, char *reply)
{
    int     parint;          /* an integer parameter                */
    float   parfloat;        /* a floating point parameter          */
    
    strcpy(reply,"+OK done\n");
    switch (command[0]) {
    case 's': /* stop */
        paiStopRobot();
        printf("+OK command STOP\n");
        break;
    case 'b': /* request battery voltage */
        sprintf(reply, "!DATA %2.2d\n+OK\n", paiBatteryVoltage());
        printf("+OK command BATTERY - %2.2d\n", paiBatteryVoltage());
      break;
    case 'm': /* move */
        parint=atoi(&command[1]);
        if(parint>MinSonarDist(2,5)+tooClose)
        {
            printf("-ERR command MOVE %d - obstacle too close\n", parint);
            strcpy(reply, "-ERR obstacle too close\n");
        } else {
            paiMoveRobot(parint);
            strcpy(reply, "+OK\n");
            printf("+OK command MOVE %d\n", parint);
        }
        break;
    case 'r': /* rotate */
        parfloat=(float)atof(&command[1]);
        paiRotateRobot(parfloat);
        strcpy(reply, "+OK\n");
        printf("+OK command ROTATE %4.2f\n", parfloat);
        break;
    case 'v': /* velocity */
        parint=atoi(&command[1]);
        if (parint<=0) {
            strcpy(reply, "-ERR too slow\n");
            printf("-ERR command VELOCITY %d\n", parint);
            break;
        } else if (parint>MaxSpeed) {
            strcpy(reply, "-ERR too fast\n");
            printf("-ERR command VELOCITY %d\n", parint);
        } else {
            strcpy(reply, "+OK\n");
            printf("+OK command VELOCITY %d\n", parint);
            paiSetMoveVelocity(parint);
        }
        break;
    case 'p':
        sprintf(reply, "!DATA %d %d %d %d\n+OK\n", paiSonarRange(2), paiSonarRange(3), paiSonarRange(4), paiSonarRange(5));
        printf("+OK command SONAR - $s", reply);
        break;
    default:
        printf("-ERR bad syntax: %s\n", command);
        strcpy(reply,"-ERR bad syntax\n");
    }
}

void ServiceClient(const int socket)
{
    char    command[BUFSIZE];/* buffer for string the server sends  */
    char    reply[BUFSIZE];  /* buffer for the reply string to send */
    int     n;               /* the length of the command received  */

    if(paiRobotStartup(sfTTYPORT, sfCOM1, NULL, 0))
    {
        printf("+OK connected to the robot\n");
        SendLine(socket, "+OK connected\n");
        paiSetMoveVelocity(IniSpeed);
        sfRobotComInt(sfCOMENABLE,1);
        paiSetSonarPolling("\001\002\003\004\005\006");
    }
    else
    {
        printf("-ERR cannot connect to the robot\n");
        SendLine(socket, "-ERR cannot connect to the robot\n");
        return;
    }

    command[0]='\0'; /* set the command string length to zero */
    n = ReceiveLine(socket, command);
    /* n<=0 menas that someone disconnected or there was an error */
    while (n > 0) {
        if (strcmp(command, "exit")==0)
        {
            SendLine(socket, "+OK disconnect");
            break;
        }
        ProcessCommand(command, reply);
        SendLine(socket, reply);
        n = ReceiveLine(socket, command);
    }
    closesocket(socket);
    paiRobotShutdown();
}

int MinSonarDist(int low, int high)
{
    int i;
    int closest=paiSonarRange(low);
    for(i=low+1;i<=high;i++)
        if(paiSonarRange(i)<closest)
            closest=paiSonarRange(i);
        return closest;
}


/*
int   paiRobotStartup(int channel, char *name, HANDLE hInst, int nCmdShow);
void  paiRobotShutdown (void);

void  paiResetRobotPosition (void);
float paiRobotX(void);
float paiRobotY(void);
float paiRobotHeading(void);

float paiRobotTransVel(void);
int   paiRobotStall(void);
int   paiRobotStatus(void);
void  paiSetRobotVelocity(int vel);
void  paiSetMoveVelocity(int vel);
void  paiMoveRobot(int dist);
void  paiStopRobot(void);

float paiRobotRotVel(void);
void  paiSetRobotRotVel(float vel);
void  paiRotateRobot(float radians);
void  paiSetRobotHeading(float radians);

int   paiSonarRange(int Pinger);
void  paiSetSonarPolling (char *sequence);

float paiBatteryVoltage(void);
*/