#include <utility.h>
#include <ansi_c.h>
#include <cvirte.h>		/* Needed if linking in external compiler; harmless otherwise */
#include <userint.h>
#include <tcpsupp.h>
#include <rs232.h>
#include "robotserver.h"

/* --------------- Constants ------------------------------------------------*/
#define VERSION "1.01"	/* Added Allowed hosts */

/* --------------- Global Variabels for this file ---------------------------*/
static int panelHandle, CalPanel;
static unsigned int tcpHandle;

/* --------------- Prototyping of functions defined in this file ------------*/
int  OpenComPort(void);
void CloseComPort(void);
void ShutDown (void);
void CVICALLBACK ComInt (int portNo, int eventMask, void *callbackData);
int  CVICALLBACK ClbTcpRobot (unsigned handle, int event, int error,void *callbackData);
void TcpSendMessage(char *type, char *message);
void DecodeMessage(char *inbuf);
void MoveRobot(void);
void InitRobot(void);
void StartRobot(void); 
void StopRobot(void); 
void SendStatus(void);
void DecodeRobotReply(void);
void InitPositions(void);
int InterpolatePosition(int nb, int *args);
char *NextPosition(int nb, char *buf, int *args, float zoff);
char *ParkPosition(int nb, char *buf, int *args);
char *TesterPosition(int nb,char *buf, int stay);
void FetchChip(void);
void StoreChip(void);
void ReInsert(void);
void TesterMoveChip(void);
void InitPanel(void);
void EnablePanel(void);
void DisablePanel(void);
char *SetHanded(int nb, char *buf, int *args);
char *SetTesterHanded(int nb, char *buf, int *args);
int  DecodeBitPattern(char *head, char *buf);
void Socket(void);
void Push(void);

/* --------------- Type definitions -----------------------------------------*/
typedef struct Trays { /* x,y,z positions for each of the 4 corners */
  float xFirstRowFirstCol;
  float yFirstRowFirstCol;
  float zFirstRowFirstCol;
  float xFirstRowLastCol;
  float yFirstRowLastCol;
  float zFirstRowLastCol;
  float xLastRowFirstCol;
  float yLastRowFirstCol;
  float zLastRowFirstCol;
  float xLastRowLastCol;
  float yLastRowLastCol;
  float zLastRowLastCol;
} TRAYS;

typedef struct Testers {
	float x;
	float y;
	float z;
	float rot;
	float z1;
	float z2;
	float z3;
} TESTERS;

/* --------------- Constants ------------------------------------------------*/
/* The TCP port we use for communication with the main test controler */
#define TCP_PORT 2000

/* staus of commands */
#define STATUS_NONE     -1
#define STATUS_IDLE      0
#define STATUS_SERVO_ON  1
#define STATUS_ORIGIN    2
#define STATUS_ERROR     3
#define STATUS_SERVO_OFF 4
#define STATUS_MOVE1     5
#define STATUS_MOVE2     7
#define STATUS_MOVE3     8
#define STATUS_MOVE4     9
#define STATUS_MOVE5    10
#define STATUS_MOVE6    11
#define STATUS_MOVE7    12
#define STATUS_FETCH1   13
#define STATUS_FETCH2   14
#define STATUS_FETCH3   15
#define STATUS_FETCH4   16
#define STATUS_FETCH5   17
#define STATUS_FETCH6   18
#define STATUS_FETCH7   19
#define STATUS_STORE1   20
#define STATUS_STORE2   21
#define STATUS_STORE3   22
#define STATUS_STORE4   23
#define STATUS_STORE5   24
#define STATUS_STORE6   25
#define STATUS_START    26
#define STATUS_FETCH8   27
#define STATUS_STORE7   28
#define STATUS_ROBOT1   29
#define STATUS_ROBOT2   30
#define STATUS_FETCH9   31 /* Go to tester position + 1 */
#define STATUS_FETCH10  32 /* Sence the sensors = 0 */
#define STATUS_FETCH11  33 /* Decode the reply */
#define STATUS_FETCH12  34 /* Go to tester position + 2 */
#define STATUS_FETCH13  35 /* Sence the sensors = B1111 */
#define STATUS_FETCH14  36 /* Decode the reply */
#define STATUS_FETCH15  37 /* Go to sensor position + 3 */
#define STATUS_STORE8   38 /* Sense the sensors */
#define STATUS_STORE9   39 /* Check if sensors in or out */
#define STATUS_STORE10  40 /* Go to catch position */
#define STATUS_FETCH4a	41
#define STATUS_FETCH6a	42
#define STATUS_REINSERT1 43
#define STATUS_REINSERT2 44
#define STATUS_REINSERT3 45
#define STATUS_REINSERT4 46
#define STATUS_REINSERT5 47
#define STATUS_REINSERT6 48
#define STATUS_REINSERT7 49
#define STATUS_REINSERT8 50
#define STATUS_REINSERT9 51
#define STATUS_REINSERT10 52
#define STATUS_REINSERT11 53
#define STATUS_REINSERT12 54
#define STATUS_REINSERT13 55
#define STATUS_REINSERT14 56
#define STATUS_REINSERT15 57
#define STATUS_REINSERT16 58
#define STATUS_STORE10a   59
#define STATUS_TMOVE1     60
#define STATUS_TMOVE2     62
#define STATUS_TMOVE3     63

/* Trays and testers that we have */
#define TRAY_FIRST       1
#define TRAY_LAST        8
#define ROW_FIRST        1
#define ROW_LAST        18
#define COL_FIRST        1
#define COL_LAST         7
#define TESTER_FIRST     1
#define TESTER_LAST      4

/* Vacuum suck bits */
#define SUCK_BIT         0
#define RELEASE_CHIP     0
#define CATCH_CHIP       1
#define SUCK_DELAY     100

/* The old single test socket */
#define VALVE_BIT        2
#define RELEASE_VALVE    0
#define ACTIVATE_VALVE   1

/* The new four chip pushers */
#define PUSH_BIT1   3
#define PUSH_BIT2   4
#define PUSH_BIT3   5
#define PUSH_BIT4   6

/* Arguments */
#define FETCH_STAY       4
#define ZPOS_OFFSET      -3.00

#define Z0              27.0 /* Z during normal movements */

#define ROBOT_BUFFER_SIZE 512

/* --------------- Global variables  --------------------------------------- */
int  ComPort = 1;
int  receiveRobotPtr = 0;
char receiveRobot[ROBOT_BUFFER_SIZE];
char transmitRobot[ROBOT_BUFFER_SIZE];
int  statusRobot = STATUS_IDLE;
int  commandRemote = 0;
int  moveArguments[7];
float moveCoords[3];
int  fetchArguments[5];
int  AllowedHost(char *ip);

TRAYS tray[8];
TESTERS tester[4];
char statusBuf[256];

/* --------------- Main function ------------------------------------------- */
int main (int argc, char *argv[])
{
int status;

	if (InitCVIRTE (0, argv, 0) == 0)	/* Needed if linking in external compiler; harmless otherwise */
		return -1;	/* out of memory */
	if ((panelHandle = LoadPanel (0, "robotserver.uir", PANEL)) < 0)
		return -1;
	if ((CalPanel = LoadPanel (0, "robotserver.uir", CALPANEL)) < 0)
		return -1;
	InitPositions();
	OpenComPort();
	tcpHandle = 0;
	printf("Program version %s: %s %s\n", VERSION, __DATE__, __TIME__);
	if ((status = RegisterTCPServer (TCP_PORT, ClbTcpRobot, 0)) != -kTCP_NoError) {
		printf("TCP server error %d\n", status);
	}
	else printf("TCP server ready %d\n", status);

	DisplayPanel (panelHandle);
	InitPanel ();
	RunUserInterface ();
	return 0;
}	/* End of main */

void ShutDown (void)
{
	if (ConfirmPopup ("EXIT", "Are you sure you want to exit ?")) {
		QuitUserInterface (0);
		UnregisterTCPServer (TCP_PORT);
		CloseComPort();
	}
}	/* End of ShutDown */

int AllowedHost(char *ip)
{
FILE *fin;
char buf[256];
char ipnum[256], name[256];
	if (fin = fopen("allowed.txt", "r")) {
		while(fgets (buf, 250, fin)) {
			if (buf[0] != '#') {
				sscanf(buf, "%s %s", ipnum, name);
				if (!strcmp(ip, ipnum)) {
					fclose(fin);
					return(1);
				}
			}
		}
		fclose(fin);
		return(0);
	}
	return(1);
}	/* End of AllowedHost */

/* --------------- Callback functions from Tcp Port -------------------------*/
int CVICALLBACK ClbTcpRobot (unsigned handle, int event, int error, void *callbackData)
{
int  dataSize = 256;
char receiveBuf[256] = {0};
char addrBuf[31];

	switch (event) {
		case TCP_CONNECT:
			/* ignore extra connections or hosts that are not allowed */
			GetTCPPeerAddr (handle, addrBuf, 31);
			if (tcpHandle || !AllowedHost(addrBuf)) {
				/* We already have one client, don't accept another one */
				printf ("-- Refusing conection request from %s\n", addrBuf); 
				DisconnectTCPClient (handle);
			}
			else {
				/* Handle this new connection */
				DisablePanel();
				tcpHandle = handle;
				SetCtrlVal (panelHandle, PANEL_CLIENT, 1);
				SetCtrlVal (panelHandle, PANEL_CLIENT_IP, addrBuf);
				GetTCPPeerName (tcpHandle, receiveBuf, 256);
				SetCtrlVal (panelHandle, PANEL_CLIENT_NAME, receiveBuf);
				printf ("-- New connection from %s\n", addrBuf);
				
				/* Set the disconect mode so we do not need to terminate */
				/* connections ourselves. */
				SetTCPDisconnectMode (tcpHandle, TCP_DISCONNECT_AUTO);
			}
		break;
		case TCP_DATAREADY:
			if (handle == tcpHandle) {
				if ((dataSize = ServerTCPRead (tcpHandle, receiveBuf, dataSize, 1000)) < 0) {
					printf("Tcp: Receive Error\n");
					TcpSendMessage("ERROR", "Receive Error");
				}
				else {
					printf("Tcp -> %s\n", receiveBuf);
					DecodeMessage (receiveBuf);
//					TcpSendMessage("OK", "0");
				}
			}
		break;
		case TCP_DISCONNECT:
			if (handle == tcpHandle) {
				/* The client we were talking to has disconnected... */
				SetCtrlVal (panelHandle, PANEL_CLIENT, 0);
				tcpHandle = 0;
				SetCtrlVal (panelHandle, PANEL_CLIENT_IP, "");
				SetCtrlVal (panelHandle, PANEL_CLIENT_NAME, "");
				printf("-- Client disconnected\n");
				EnablePanel();

				/* Note that we do not need to do any more because we set the*/
				/* disconnect mode to AUTO. */
			}
		break;
	}
	return 0;
}	/* End of ClbTcpRobot */

void TcpSendMessage(char *type, char *message)
{
//int  dataSize         = 256;
char transmitBuf[256] = {0};

	sprintf(transmitBuf, "$%s %s", type, message);
	printf("Tcp <- %s\n\n", transmitBuf);
	if (ServerTCPWrite (tcpHandle, transmitBuf, strlen (transmitBuf) + 1, 1000) < 0)
		printf("Transmit Error\n");
}	/* End of TcpSendMessage */

/* --------------- Interrupts from the Com port ---------------------------- */
void CVICALLBACK ComInt (int portNo, int eventMask, void *callbackData)
{
unsigned char Chr;
	if (eventMask & LWRS_RECEIVE) {
		while (GetInQLen (ComPort) > 0) {
			Chr = ComRdByte (ComPort);
//			putchar(Chr);
			if (Chr != 10) {
				receiveRobot[receiveRobotPtr++] = Chr;
//				if ((Chr == '\r') && commandRemote) {
				if (Chr == '\r') {
//					putchar('\n');
//					printf("(%d)\n",receiveRobotPtr);
					receiveRobot[--receiveRobotPtr] = '\0';
					receiveRobotPtr = 0;
					printf ("Com -> %s\n", receiveRobot);
					if (commandRemote)
						DecodeRobotReply();
				}
			}
		}
	}
}	/* End of ComInt */

/* Open the com-port to the robot */
int OpenComPort(void)
{
	if (OpenComConfig (ComPort, "", 9600, 1, 8, 1, 512, 512) != 0)
		return -1;
	if (InstallComCallback (ComPort, LWRS_RECEIVE, 1, 0, ComInt, 0) != 0)
		return -2;
return 0;
}	/* End of OpenComPort */

/* Close the com-port to the robot */
void CloseComPort(void)
{
	CloseCom (ComPort);
}	/* End of ClosePort */

/* Callbacks from the panel itself */
int CVICALLBACK ClbRobot (int panel, int event, void *callbackData, int eventData1, int eventData2)
{
	switch (event) {
		case EVENT_GOT_FOCUS:
		break;

		case EVENT_LOST_FOCUS:
		break;

		case EVENT_CLOSE:
			ShutDown();
		break;
	}
	return 0;
}	/* End of ClbRobot */

/* If manualy clicking a control on the panel */
int CVICALLBACK ClbSendCommand (int panel, int control, int event, void *callbackData, int eventData1, int eventData2)
{
char buf[512] = {0};
float move_px, move_py, move_pz, move_rot, move_a, move_b;
int speed_value;
int val;
	commandRemote = 0;
	switch (event) {
		case EVENT_COMMIT:
			switch(control) {
				case PANEL_SERVO_ON:
					strcpy(buf, "@SERVO ON\r");
				break;
				case PANEL_SERVO_OFF:
					strcpy(buf, "@SERVO OFF\r");
				break;
				case PANEL_SPEED_GET:
					strcpy(buf, "@?SPEED\r");
//					SetComTime (1, 5.0);
				break;
				case PANEL_SPEED_SET:
					GetCtrlVal (panel, PANEL_SPEED_VALUE, &speed_value);
					sprintf(buf, "@MSPEED %d\r", speed_value);
				break;
				case PANEL_ASPEED_SET:
					GetCtrlVal (panel, PANEL_ASPEED_VALUE, &speed_value);
					sprintf(buf, "@ASPEED %d\r", speed_value);
				break;
				case PANEL_ORIGIN:
					strcpy(buf, "@ORIGIN\r");
				break;
				case PANEL_ORIGIN_GET:
					strcpy(buf, "@?ORIGIN\r");
				break;
				case PANEL_WHERE:
					strcpy(buf, "@?WHRXY\r");
				break;
				case PANEL_MOVEP:
					GetCtrlVal (panel,PANEL_MOVE_PX, &move_px);
					GetCtrlVal (panel,PANEL_MOVE_PY, &move_py);
					GetCtrlVal (panel,PANEL_MOVE_PZ, &move_pz);
					GetCtrlVal (panel,PANEL_MOVE_ROTATE, &move_rot);
					GetCtrlVal (panel,PANEL_MOVE_A, &move_a);
					GetCtrlVal (panel,PANEL_MOVE_B, &move_b);
					sprintf(buf, "%s , %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f, Z=0.00\r",
						"@MOVE P", move_px, move_py, move_pz, move_rot, move_a, move_b);
				break;
				case PANEL_LEFTY:
					strcpy(buf, "@LEFTY\r");
				break;
				case PANEL_RIGHTY:
					strcpy(buf, "@RIGHTY\r");
				break;
				case PANEL_COMMAND_SEND:
					GetCtrlVal(panel, PANEL_COMMAND, buf);
					strcat(buf, "\r");
				break;
				case PANEL_AIR:
					GetCtrlVal(panel, PANEL_AIR, &val);
					sprintf(buf, "@DO2(%d)=%d\r", SUCK_BIT, val);
				break;
				case PANEL_SOCKET:
					GetCtrlVal(panel, PANEL_SOCKET, &val);
					sprintf(buf, "@DO2(%d)=%d\r", VALVE_BIT, val);
				break;
				case PANEL_SOCKET1:
					GetCtrlVal(panel, PANEL_SOCKET1, &val);
					sprintf(buf, "@DO2(%d)=%d\r", PUSH_BIT1, val);
				break;
				case PANEL_SOCKET2:
					GetCtrlVal(panel, PANEL_SOCKET2, &val);
					sprintf(buf, "@DO2(%d)=%d\r", PUSH_BIT2, val);
				break;
				case PANEL_SOCKET3:
					GetCtrlVal(panel, PANEL_SOCKET3, &val);
					sprintf(buf, "@DO2(%d)=%d\r", PUSH_BIT3, val);
				break;
				case PANEL_SOCKET4:
					GetCtrlVal(panel, PANEL_SOCKET4, &val);
					sprintf(buf, "@DO2(%d)=%d\r", PUSH_BIT4, val);
				break;
				case PANEL_RELOAD_POS:
					InitPositions();
				return 0;
				break;
				case PANEL_ENABLE:
					EnablePanel();
				return 0;
				case PANEL_DISABLE:
					DisablePanel();
				return 0;
				break;
			} /* end switch */
			printf("Com <- %s\n", buf);
			ComWrt (ComPort, buf, strlen(buf));
		break;
	}
	return 0;
}	/* End of ClbSendCommand */

/*
int MoveChip(int fromBrick, int fromCol, int fromRow, int toBrick, int toCol, int toRow)
{
	return 0;
}
*/

/* --------------- Function to decode commands received from tester over TCP */
void DecodeMessage(char *inbuf)
{
char buf[256];
char *command;
	buf[0] = '\0';
	if (command = strtok(inbuf, "$ ")) {
		if (!strcmp(command, "STATUS")) {
			switch (statusRobot) {
				case STATUS_IDLE: strcpy(buf, "IDLE"); break;
				case STATUS_SERVO_OFF:
				case STATUS_SERVO_ON: strcpy(buf, "SERVO"); break;
				case STATUS_ORIGIN: strcpy(buf, "ORIGIN"); break;
				case STATUS_ERROR: strcpy(buf, "ERROR"); break;
				case STATUS_MOVE1: strcpy(buf, "MOVE_ONE"); break;
				default: strcpy(buf, "?"); break;
			}
			TcpSendMessage(command, buf);
		}
		else if (!strcmp(command, "INIT")) {
			InitRobot();
		}
		else if (!strcmp(command, "START")) {
			StartRobot();
		}
		else if (!strcmp(command, "STOP")) {
			StopRobot();
		}
		else if (!strcmp(command, "MOVE")) {
			MoveRobot();
		}
		else if (!strcmp(command, "TMOVE")) {
			TesterMoveChip();
		}
		else if (!strcmp(command, "FETCH")) {
			FetchChip();
		}
		else if (!strcmp(command, "STORE")) {
			StoreChip();
		}
		else if (!strcmp(command, "ROBOTSTATUS")) {
			SendStatus();
		}
		else if (!strcmp(command, "REINSERT")) {
			ReInsert();
		}
		else if (!strcmp(command, "SOCKET")) {
			Socket();
		}
		else if (!strcmp(command, "PUSH")) {
			Push();
		}
		else TcpSendMessage ("NONE", "0");
	} else TcpSendMessage ("WHAT", "0");
}	/* End of DecodeMessage */

/* Ask the robot if origin is completed */
void SendStatus(void)
{
char buf[64];
	commandRemote = 1;
	receiveRobotPtr = 0;
	statusRobot = STATUS_ROBOT1;
	strcpy(transmitRobot,"@?ORIGIN\r");
	printf("Com <- %s\n", transmitRobot);
	ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
}

/* Ask the robot if the servo is on */
void InitRobot(void)
{
char buf[64];
	commandRemote = 1;
	receiveRobotPtr = 0;
	statusRobot = STATUS_SERVO_ON;
	strcpy(transmitRobot, "@SERVO ON\r");
	printf("Com <- %s\n",transmitRobot);
	ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
}

/* Tell the robot to turn on the servo */
void StartRobot(void)
{
char buf[64];
	commandRemote = 1;
	receiveRobotPtr = 0;
	statusRobot = STATUS_START;
	strcpy(transmitRobot, "@SERVO ON\r");
	printf("Com <- %s\n", transmitRobot);
	ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
}

/* Tell the robot to turn off the servo */
void StopRobot(void)
{
char buf[64];
	commandRemote = 1;
	receiveRobotPtr = 0;
	statusRobot = STATUS_SERVO_OFF;
	strcpy(transmitRobot, "@SERVO OFF\r");
	printf("Com <- %s\n", transmitRobot);
	ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
}

int InterpolatePosition(int nb, int *args)
{
  int   traynb, row, col;
  float mult;
  float x, y, z;

  traynb = args[nb];
  col = args[nb+1];
  row = args[nb+2];

  if ((traynb >= TRAY_FIRST) && (traynb <= TRAY_LAST)) {
    traynb--;
    if ((row >= ROW_FIRST) && (row <= ROW_LAST) &&
        (col >= COL_FIRST) && (col <= COL_LAST)) {
      mult = 1.0 / ( (COL_LAST - COL_FIRST) * (ROW_LAST - ROW_FIRST) );
      
      x = ( tray[traynb].xFirstRowFirstCol * (COL_LAST - col)*(ROW_LAST - row)  +
            tray[traynb].xFirstRowLastCol * (col - COL_FIRST)*(ROW_LAST - row)  +
            tray[traynb].xLastRowFirstCol * (COL_LAST - col)*(row - ROW_FIRST)  +
            tray[traynb].xLastRowLastCol * (col - COL_FIRST)*(row - ROW_FIRST)  ) * mult;

      y = ( tray[traynb].yFirstRowFirstCol * (COL_LAST - col)*(ROW_LAST - row)  +
            tray[traynb].yFirstRowLastCol * (col - COL_FIRST)*(ROW_LAST - row)  +
            tray[traynb].yLastRowFirstCol * (COL_LAST - col)*(row - ROW_FIRST)  +
            tray[traynb].yLastRowLastCol * (col - COL_FIRST)*(row - ROW_FIRST)  ) * mult;

      z = ( tray[traynb].zFirstRowFirstCol * (COL_LAST - col)*(ROW_LAST - row)  +
            tray[traynb].zFirstRowLastCol * (col - COL_FIRST)*(ROW_LAST - row)  +
            tray[traynb].zLastRowFirstCol * (COL_LAST - col)*(row - ROW_FIRST)  +
            tray[traynb].zLastRowLastCol * (col - COL_FIRST)*(row - ROW_FIRST)  ) * mult;

      moveCoords[0] = x;
      moveCoords[1] = y;
      moveCoords[2] = z;

      return(1); // OK                                                                                   
    }
  }

  return(0); // not OK                                                                                   
}

char *NextPosition(int nb, char *buf, int *args, float zoff)
{
  int result;
  float x, y, z, rot;
  
  result = InterpolatePosition(nb, args);
  if (result) {
    x = moveCoords[0];
    y = moveCoords[1];
    z = moveCoords[2];
    
    rot = 0.0;
    sprintf(buf, "%s , %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f, Z=%5.2f\r",
	    "@MOVE P", x, y, z, rot, 0.0, 0.0, Z0);
    return(buf);
  }
  return(NULL);
}

char *ParkPosition(int nb, char *buf, int *args)
{
  int result;
  float x, y, z, rot;
  
  result = InterpolatePosition(nb, args);
  if (result) {
    x = moveCoords[0];
    y = moveCoords[1];
    
    z = Z0;
    rot = 0.0;
    
    sprintf(buf, "%s , %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f, Z=%5.2f\r",
	    "@MOVE P", x, y, z, rot, 0.0, 0.0, Z0);
    return(buf);
  }
  
  return(NULL);
}

/* Set the hand (lefty or righty) for a position on a tray */
char *SetHanded(int nb, char *buf, int *args)
{
int   traynb, row, col;
float x, y, z, rot;
	traynb = args[nb];
	col = args[nb+1];
	row = args[nb+2];
	if ((traynb >= TRAY_FIRST) && (traynb <= TRAY_LAST)) {
		traynb--;
		if ((row >= 1) && (row <= tray[traynb].rows) && (col >= 1) && (col <= tray[traynb].columns)) {
			y = tray[traynb].y - tray[traynb].yoff - tray[traynb].dy*(row-1);
			if (y < 0.0) strcpy(buf, "@LEFTY\r");
			else strcpy(buf, "@RIGHTY\r");
			return(buf);
		}
	}
	return(NULL);
}	/* End of SetHanded */

/* Set the hand (lefty or righty) for a tester */
char *SetTesterHanded(int nb, char *buf, int *args)
{
int   testernb;
float x, y, z, rot;
	testernb = args[nb];
	if ((testernb >= TESTER_FIRST) && (testernb <= TESTER_LAST)) {
		testernb--;
		y = tester[testernb].y;
		if (y < 0.0) strcpy(buf, "@LEFTY\r");
		else strcpy(buf,"@RIGHTY\r");
		return(buf);
	}
	return(NULL);
}	/* End of SetTesterHanded */
/*
void MoveRobotOld(void)
{
int nbarg = 0;
char *ptr;
	receiveRobotPtr = 0;
	commandRemote = 1;
	statusRobot = STATUS_ERROR;
	while((ptr = strtok(NULL, " ")) && (nbarg < 8)) {
		moveArguments[nbarg++] = atoi(ptr);
		printf("ARG %d %d\n", nbarg, moveArguments[nbarg-1]);
	}
	if (nbarg == 7) {
		statusRobot = STATUS_MOVE1;
		if (ptr = NextPosition(0, transmitRobot, moveArguments, 0.0)) {
			ComWrt (ComPort, transmitRobot, strlen(transmitRobot) );
			printf("Com -> %s\n", transmitRobot);
		}
		else statusRobot = STATUS_ERROR;
	}
}
*/
void MoveRobot(void)
{
int  nbarg = 0;
char *ptr;
	receiveRobotPtr = 0;
	commandRemote = 1;
	statusRobot = STATUS_ERROR;
	while((ptr = strtok(NULL, " ")) && (nbarg < 8)) {
		moveArguments[nbarg++] = atoi(ptr);
//		printf("ARG %d %d\n", nbarg, moveArguments[nbarg-1]);
	}
	if (nbarg == 7) {
		if (SetHanded (0, transmitRobot, moveArguments)) {
			statusRobot = STATUS_MOVE1;
			printf ("Com -> %s\n", transmitRobot);
			ComWrt (ComPort, transmitRobot, strlen(transmitRobot) );
		}
	}
	if (statusRobot == STATUS_ERROR) {
		TcpSendMessage("STATUS", "ERROR MOVE");
	}
}	/* End of MoveRobot */

void TesterMoveChip(void)
{
int  nbarg = 0;
char *ptr;
	receiveRobotPtr = 0;
	commandRemote = 1;
	statusRobot = STATUS_ERROR;
	while((ptr = strtok(NULL, " ")) && (nbarg < 8)) {
		fetchArguments[nbarg++] = atoi(ptr);
//		printf("ARG %d %d\n", nbarg, moveArguments[nbarg-1]);
	}
	if (nbarg == 5) {
		if (SetTesterHanded(0, transmitRobot, fetchArguments)) {
			statusRobot = STATUS_TMOVE1;
			printf ("Com -> %s\n", transmitRobot);
			ComWrt (ComPort, transmitRobot, strlen(transmitRobot) );
		}
	}
	if (statusRobot == STATUS_ERROR) {
		TcpSendMessage("STATUS", "ERROR MOVE");
	}
}	/* End of TesterMoveChip */

char *TesterPosition(int nb, char *buf, int stay)
{
float x, y, z, rot;
	if ((nb >= TESTER_FIRST) && (nb <= TESTER_LAST)) {
		nb--;
		x = tester[nb].x;
		y = tester[nb].y;
		switch(stay) {
			case 0: z = 0.0; break;
			case 1: z = tester[nb].z; break;
			case 2: z = tester[nb].z1 + tester[nb].z; break;
			case 3: z = tester[nb].z2 + tester[nb].z; break;
			case 4: z = tester[nb].z3 + tester[nb].z; break;
			default: return NULL;
		}
//		rot = -90.0;	// 90.0 for PASA, 0.0 for ALTRO and -90.0 for PCA16
		rot = tester[nb].rot;
		if (stay <= 1)
			sprintf(buf, "%s , %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f, Z=%5.2f\r",
			"@MOVE P", x, y, z, rot, 0.0, 0.0, Z0);
		else
			sprintf(buf, "%s , %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f\r",
			"@MOVE P", x, y, z, rot, 0.0, 0.0);

		return(buf);
	}
	return(NULL);
}

void FetchChip(void)
{
int  nbarg = 0;
int  testernb;
char *ptr;
	receiveRobotPtr = 0;
	commandRemote = 1;
	statusRobot = STATUS_ERROR;
	while((ptr = strtok(NULL, " ")) && (nbarg < 8)) {
		fetchArguments[nbarg++] = atoi(ptr);
//		printf("ARG %d %d\n", nbarg, fetchArguments[nbarg-1]);
	}
	if (nbarg == 5) {
		testernb = fetchArguments[3];
		if ((testernb >= TESTER_FIRST) && (testernb <= TESTER_LAST)) {
			if (SetHanded(0, transmitRobot, fetchArguments)) {
				statusRobot = STATUS_FETCH1;
				printf("Com -> %s\n", transmitRobot);
				ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
			}
		}
	}
	if (statusRobot == STATUS_ERROR) {
		TcpSendMessage("STATUS", "ERROR FETCH");
	}
}  /* End of FetchChip */

void StoreChip(void)
{
int  nbarg = 0;
char *ptr;
	receiveRobotPtr = 0;
	commandRemote = 1;
	statusRobot = STATUS_ERROR;
	while((ptr = strtok(NULL, " ")) && (nbarg < 8)) {
		fetchArguments[nbarg++] = atoi(ptr);
//		printf("ARG %d %d\n", nbarg, fetchArguments[nbarg-1]);
	}
	if (nbarg == 5) {
		if (SetTesterHanded(0, transmitRobot, fetchArguments)) {
			statusRobot = STATUS_STORE1;
			printf("Com -> %s\n", transmitRobot);
			ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
		}
	}
	if (statusRobot == STATUS_ERROR) {
		TcpSendMessage("STATUS", "ERROR STORE");
	}
}  /* End of StoreChip */

void ReInsert(void)
{
int nbarg = 0;
int TesterNb = 2;
char *ptr;
	receiveRobotPtr = 0;
	commandRemote = 1;
	statusRobot = STATUS_ERROR;
	while((ptr = strtok(NULL, " ")) && (nbarg < 8)) {
		fetchArguments[nbarg++] = atoi(ptr);
	}
	if (nbarg == 1) {
		TesterNb = fetchArguments[0];
		if ((TesterNb >= TESTER_FIRST) && (TesterNb <= TESTER_LAST)) {
			if (SetTesterHanded(0, transmitRobot, fetchArguments)) {
				statusRobot = STATUS_REINSERT1;
				printf("Com -> %s\n", transmitRobot);
				ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
			}
		}
	}
	if (statusRobot == STATUS_ERROR) {
		TcpSendMessage("STATUS", "ERROR RE_INSERT");
	}
}	/* End of ReInsert */

void Socket(void)
{
int  nbarg = 0;
int  UpDown = 1;
char *ptr;
char Buf[20];

	receiveRobotPtr = 0;
	commandRemote = 1;
	statusRobot = STATUS_ERROR;
	while((ptr = strtok(NULL, " ")) && (nbarg < 8)) {
		fetchArguments[nbarg++] = atoi(ptr);
	}
	if (nbarg == 1) {
		UpDown = fetchArguments[0];	/* 1 -> Down/open. 0 -> Up/Closed */
		if ((UpDown >= 0) && (UpDown <= 1)) {
			sprintf(Buf, "@DO2(%d)=%d\r", VALVE_BIT, UpDown);
			printf("Com <- %s\n", Buf);
			ComWrt (ComPort, Buf, strlen(Buf));
		}
	}
	TcpSendMessage("STATUS", "OK");
}	/* End of Socket */

/* Move the pusher in position and push down the chip */
/*  Det har ar bara en rakopia av Socket */
void Push(void)
{
int  nbarg = 0;
int  UpDown;
char *ptr;
char Buf[20];
int  TesterNb;
int  PushBits[] = {0, PUSH_BIT1, PUSH_BIT2, PUSH_BIT3, PUSH_BIT4};

	receiveRobotPtr = 0;
	commandRemote = 1;
	statusRobot = STATUS_ERROR;
	while((ptr = strtok(NULL, " ")) && (nbarg < 8)) {
		fetchArguments[nbarg++] = atoi(ptr);
	}
	if (nbarg == 2) {
		TesterNb = fetchArguments[0];
		UpDown = fetchArguments[1];	/* 1 -> In and down. 0 -> Up and out */
		if ((UpDown >= 0) && (UpDown <= 1)) {
			sprintf(Buf, "@DO2(%d)=%d\r", PushBits[TesterNb], UpDown);
			printf("Com <- %s\n", Buf);
			ComWrt (ComPort, Buf, strlen(Buf));
		}
	}
	TcpSendMessage("STATUS", "OK");
}	/* End of Push */

/*
  Decode bit pattern as received by the
  SEND DI command, i.e. DI2()= Bbbbbbbbb
  into an integer
*/
int DecodeBitPattern(char *head, char *buf)
{
char *token;
int  j, val;
	if (!(token=strtok(buf, "="))) return -1;
	if (strcmp(head,token)) return -2;
	if (!(token=strtok(NULL, " \n"))) return -3;
	if (*token++ != '&') return -4;	
	if (*token++ != 'B') return -5;	
	if (strlen(token) != 8) return -6;
	for (j = 0, val = 0; j <  8; j++, token++) {
		val <<= 1;
		switch (*token) {
		case '0': break;
		case '1': val |= 1; break;
		default: return(-7);
		}
	}
	return(val);
}

void DecodeRobotReply(void)
{
int  newstatusRobot = STATUS_NONE;
char buf[32];
int  len, pattern;
//	printf("ROBOT REPLY %s\n",receiveRobot);

/* Special for socklarna */
	if (!strcmp(receiveRobot, "DI3()")) {
		pattern = DecodeBitPattern("DI3()", receiveRobot);
		printf("Com <-> %d\n", pattern);
	}

	switch(statusRobot) {
	case STATUS_ROBOT1:
		if (!strcmp(receiveRobot, "COMPLETE") || !strcmp(receiveRobot, "INCOMPLETE")) {
			strcpy(statusBuf, "ORIGIN ");
			strcat(statusBuf, receiveRobot);
			newstatusRobot = STATUS_ROBOT2;
			strcpy(transmitRobot, "@?SERVO\r");
			printf("Com <- %s\n", transmitRobot);
			receiveRobotPtr = 0;
			ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
		}
	break;
	case STATUS_ROBOT2:
		strcat(statusBuf, ";SERVO ");
		len = sizeof(statusBuf) - strlen(statusBuf);
		strncat(statusBuf, receiveRobot, len);
		newstatusRobot = STATUS_IDLE;
		TcpSendMessage("STATUS", statusBuf);
	break;
	case STATUS_SERVO_ON:
		if (!strcmp(receiveRobot, "OK")) {
			newstatusRobot = STATUS_ORIGIN;
			strcpy(transmitRobot, "@ORIGIN\r");
			printf("Com <- %s\n", transmitRobot);
			receiveRobotPtr = 0;
			ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
		}
	break;
	case STATUS_ORIGIN:
		if (!strcmp(receiveRobot, "OK")) {
			newstatusRobot = STATUS_IDLE;
			TcpSendMessage("STATUS", "OK");
		}
	break;
	case STATUS_START:
	case STATUS_SERVO_OFF:
		if (!strcmp(receiveRobot, "OK")) {
			newstatusRobot = STATUS_IDLE;
			TcpSendMessage("STATUS", "OK");
		}
	break;

	case STATUS_MOVE1:
		if (!strcmp(receiveRobot, "OK")) {
			if (NextPosition(0, transmitRobot, moveArguments, 0.0)) {
				receiveRobotPtr = 0;
				ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
				printf("Com <- %s\n", transmitRobot);
				newstatusRobot = STATUS_MOVE2;
			}
			else newstatusRobot = STATUS_ERROR;
		}
	break;
	case STATUS_MOVE2:
		if (!strcmp(receiveRobot, "OK")) {
			newstatusRobot = STATUS_MOVE3;
			receiveRobotPtr = 0;
			sprintf(transmitRobot, "@DO2(%d)=%d\r", SUCK_BIT, CATCH_CHIP);
			printf("Com <- %s\n", transmitRobot);
			ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
		}
	break;
	case STATUS_MOVE3:
		if (!strcmp(receiveRobot, "OK")) {
			if (SetHanded(3, transmitRobot, moveArguments)) {
				newstatusRobot = STATUS_MOVE4;
				printf("Com <- %s\n", transmitRobot);
				ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
			}
			else newstatusRobot = STATUS_ERROR;
		}
	break;
	case STATUS_MOVE4:
		if (!strcmp(receiveRobot, "OK")) {
			if (NextPosition(3, transmitRobot, moveArguments, ZPOS_OFFSET)) {
				receiveRobotPtr = 0;
				ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
				printf("Com <- %s\n", transmitRobot);
				newstatusRobot = STATUS_MOVE5;
			}
			else newstatusRobot = STATUS_ERROR;
		}
	break;
	case STATUS_MOVE5:
		if (!strcmp(receiveRobot, "OK")) {
			newstatusRobot = STATUS_MOVE6;
			receiveRobotPtr = 0;
			sprintf(transmitRobot, "@DO2(%d)=%d\r", SUCK_BIT, RELEASE_CHIP);
			printf("Com <- %s\n", transmitRobot);
			ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
		}
	break;
	case STATUS_MOVE6:
		if (!strcmp(receiveRobot, "OK")) {
			if (ParkPosition(3, transmitRobot, moveArguments)) {
				receiveRobotPtr = 0;
				ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
				printf("Com <- %s\n", transmitRobot);
				newstatusRobot = STATUS_MOVE7;
			}
			else newstatusRobot = STATUS_ERROR;
		}
	break;
	case STATUS_MOVE7:
		if (!strcmp(receiveRobot, "OK")) {
			newstatusRobot = STATUS_IDLE;
			TcpSendMessage("STATUS", "OK");
		}
	break;

	// Open the socket but do not wail for it

	case STATUS_FETCH1:	/* Go to the fetch-tray */
		if (!strcmp(receiveRobot, "OK")) {
			if (NextPosition(0, transmitRobot, fetchArguments, 0.0)) {
				receiveRobotPtr = 0;
				ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
				printf("Com <- %s\n", transmitRobot);
				newstatusRobot = STATUS_FETCH2;
			}
			else newstatusRobot = STATUS_ERROR;
		}
	break;
	case STATUS_FETCH2:	// Pick up the chip
		if (!strcmp(receiveRobot, "OK")) {
			newstatusRobot = STATUS_FETCH8;
			receiveRobotPtr = 0;
			sprintf(transmitRobot,"@DO2(%d)=%d\r", SUCK_BIT, CATCH_CHIP);
			printf("Com <- %s\n", transmitRobot);
			ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
			Delay(0.5);
		}
	break;
	case STATUS_FETCH3:	// Delay
		if (!strcmp(receiveRobot, "OK")) {
			newstatusRobot = STATUS_FETCH8;
			receiveRobotPtr = 0;
			sprintf(transmitRobot, "@DELAY %d\r", SUCK_DELAY);
			printf("Com <- %s\n", transmitRobot);
			ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
		}
	case STATUS_FETCH8: // Change handedness to tester
		if (!strcmp(receiveRobot, "OK")) {
			if (SetTesterHanded(3, transmitRobot, fetchArguments)) {
				newstatusRobot = STATUS_FETCH4;
				printf("Com <- %s\n", transmitRobot);
				ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
			}
			else newstatusRobot = STATUS_ERROR;
		}
	break;
	
	// Check if the socket is open and wait until it is
	
	case STATUS_FETCH4:	// Activate the valve
		if (!strcmp(receiveRobot, "OK")) {
			newstatusRobot = STATUS_FETCH4a;
			receiveRobotPtr = 0;
			sprintf(transmitRobot, "@DO2(%d)=%d\r", VALVE_BIT, ACTIVATE_VALVE);
			printf("Com <- %s\n", transmitRobot);
			ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
			Delay(0.5);
		}
	break;
	case STATUS_FETCH4a:	// Goto the tester
		if (!strcmp(receiveRobot, "OK")) {
			if (TesterPosition(fetchArguments[3], transmitRobot, 1)) {
				receiveRobotPtr = 0;
				ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
				printf("Com <- %s\n", transmitRobot);
				newstatusRobot = STATUS_FETCH5;
			}
			else newstatusRobot = STATUS_ERROR;
		}
	break;
	case STATUS_FETCH5:	// Release the chip
		if (!strcmp(receiveRobot, "OK")) {
			if (fetchArguments[FETCH_STAY]) newstatusRobot = STATUS_FETCH9;
			else newstatusRobot = STATUS_FETCH9;
			receiveRobotPtr = 0;
			sprintf(transmitRobot, "@DO2(%d)=%d\r", SUCK_BIT, RELEASE_CHIP);
			printf("Com <- %s\n", transmitRobot);
			ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
			Delay(0.5);
		}
	break;
	case STATUS_FETCH6:	// Go up
		if (!strcmp(receiveRobot, "OK")) {
			newstatusRobot = STATUS_FETCH6a;
			if (TesterPosition(fetchArguments[3], transmitRobot, 0)) {
				receiveRobotPtr = 0;
				ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
				printf("Com <- %s\n", transmitRobot);
			}
			else newstatusRobot = STATUS_ERROR;
		}
	break;
	case STATUS_FETCH6a:	// Release the valve
		if (!strcmp(receiveRobot, "OK")) {
			newstatusRobot = STATUS_FETCH7;
			receiveRobotPtr = 0;
			sprintf(transmitRobot, "@DO2(%d)=%d\r", VALVE_BIT, RELEASE_VALVE);
			printf("Com <- %s\n", transmitRobot);
			ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
			Delay(0.5);
		}
	break;
	case STATUS_FETCH7:	// Stay in up position
		if (!strcmp(receiveRobot, "OK")) {
			newstatusRobot = STATUS_IDLE;
			TcpSendMessage("STATUS", "OK");
		}
	break;
	case STATUS_FETCH9:	// Go to tester position + 1
		if (!strcmp(receiveRobot, "OK")) {
			if (TesterPosition(fetchArguments[3], transmitRobot, 2)) {
				receiveRobotPtr = 0;
				ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
				printf("Com <- %s\n", transmitRobot);
				newstatusRobot = STATUS_FETCH10;
			}
			else newstatusRobot = STATUS_ERROR;
		}
	break;
	case STATUS_FETCH10:	// Sence the sensors
		if (!strcmp(receiveRobot, "OK")) {
			newstatusRobot = STATUS_FETCH11;
			receiveRobotPtr = 0;
			sprintf(transmitRobot, "@SEND DI2() TO CMU\r");
			printf("Com <- %s\n", transmitRobot);
			ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
		}
	break;
	case STATUS_FETCH11:	// Decode sensor reply = 0
		pattern = DecodeBitPattern("DI2()", receiveRobot);
		printf("Com <-> %d\n", pattern);
		if (pattern == 0) {
			receiveRobotPtr = 0;
			newstatusRobot = STATUS_FETCH12;
		}
		else newstatusRobot = STATUS_ERROR;
	break;
	case STATUS_FETCH12:	// Go to tester position + 2
		if (!strcmp(receiveRobot, "OK")) {
			if (TesterPosition(fetchArguments[3], transmitRobot, 3)) {
				receiveRobotPtr = 0;
				ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
				printf("Com <- %s\n", transmitRobot);
				newstatusRobot = STATUS_FETCH13;
			}
			else newstatusRobot = STATUS_ERROR;
		}
	break;
	case STATUS_FETCH13:	// Sence the sensors = B0001
		if (!strcmp(receiveRobot, "OK")) {
			newstatusRobot = STATUS_FETCH14;
			receiveRobotPtr = 0;
			sprintf(transmitRobot, "@SEND DI2() TO CMU\r");
			printf("Com <- %s\n", transmitRobot);
			ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
		}
	break;
	case STATUS_FETCH14:	// Decode sensor reply = 0x1
		pattern = DecodeBitPattern("DI2()", receiveRobot);
		printf("Com <-> %d\n", pattern);
		if (pattern == 0x1) {
			receiveRobotPtr = 0;
			newstatusRobot = STATUS_FETCH6;
		}
		else newstatusRobot = STATUS_ERROR;
	break;
	case STATUS_FETCH15:	// Go to sensor position + 3
		if (!strcmp(receiveRobot, "OK")) {
			if (TesterPosition(fetchArguments[3], transmitRobot, 4)) {
				receiveRobotPtr = 0;
				ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
				printf("Com <- %s\n", transmitRobot);
				newstatusRobot = STATUS_FETCH7;
//				Delay (15);
			}
			else newstatusRobot = STATUS_ERROR;
		}
	break;

	case STATUS_STORE8:  // Sence the sensors = B1111
		if (!strcmp(receiveRobot, "OK")) {
			newstatusRobot = STATUS_STORE9;
			receiveRobotPtr = 0;
			sprintf(transmitRobot, "@SEND DI2() TO CMU\r");
			printf("Com <- %s\n", transmitRobot);
			ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
		}
	break;
	case STATUS_STORE9:	// Decode sensor reply = 0x1
		pattern = DecodeBitPattern("DI2()", receiveRobot);
		printf("Com <-> %d\n", pattern);
		if (pattern == 0x1) {
			receiveRobotPtr = 0;
			newstatusRobot = STATUS_STORE10;
		}
		else if (pattern == 0) {
			receiveRobotPtr = 0;
			newstatusRobot = STATUS_STORE1;
		}
		else newstatusRobot = STATUS_ERROR;
	break;
	case STATUS_STORE1:	// Activate the valve
		if (!strcmp(receiveRobot, "OK")) {
			receiveRobotPtr = 0;
			sprintf(transmitRobot, "@DO2(%d)=%d\r", VALVE_BIT, ACTIVATE_VALVE);
			printf("Com <- %s\n", transmitRobot);
			ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
			newstatusRobot = STATUS_STORE10;
			Delay(0.5);
		}
	break;
	case STATUS_STORE10:  // Move to tester
		if (!strcmp(receiveRobot, "OK")) {
			if (TesterPosition(fetchArguments[0], transmitRobot, 1)) {
				receiveRobotPtr = 0;
				printf("Com <- %s\n", transmitRobot);
				ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
				newstatusRobot = STATUS_STORE10a;
			}
			else newstatusRobot = STATUS_ERROR;
		}
	break;

	case STATUS_TMOVE1:  // Move to tester
		if (!strcmp(receiveRobot, "OK")) {
			if (TesterPosition(fetchArguments[0], transmitRobot, 1)) {
				receiveRobotPtr = 0;
				printf("Com <- %s\n", transmitRobot);
				ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
				newstatusRobot = STATUS_TMOVE2;
			}
			else newstatusRobot = STATUS_ERROR;
		}
	break;

	case STATUS_STORE10a:  // Go down
		if (!strcmp(receiveRobot,"OK")) {
			if (TesterPosition(fetchArguments[0], transmitRobot, 2)) {
				receiveRobotPtr = 0;
				printf("Com <- %s\n", transmitRobot);
				ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
				newstatusRobot = STATUS_STORE2;
			}
			else newstatusRobot = STATUS_ERROR;
		}
	break;

	case STATUS_TMOVE2:  // Go down
		if (!strcmp(receiveRobot,"OK")) {
			if (TesterPosition(fetchArguments[0], transmitRobot, 2)) {
				receiveRobotPtr = 0;
				printf("Com <- %s\n", transmitRobot);
				ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
				newstatusRobot = STATUS_TMOVE3;
			}
			else newstatusRobot = STATUS_ERROR;
		}
	break;

	case STATUS_STORE2:  // Catch the chip
		if (!strcmp(receiveRobot, "OK")) {
			newstatusRobot = STATUS_STORE7;
			receiveRobotPtr = 0;
			sprintf(transmitRobot, "@DO2(%d)=%d\r", SUCK_BIT, CATCH_CHIP);
			printf("Com <- %s\n", transmitRobot);
			ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
			Delay(0.5);
		}
	break;

	case STATUS_TMOVE3:  // Catch the chip
		if (!strcmp(receiveRobot, "OK")) {
			newstatusRobot = STATUS_FETCH8;
			receiveRobotPtr = 0;
			sprintf(transmitRobot, "@DO2(%d)=%d\r", SUCK_BIT, CATCH_CHIP);
			printf("Com <- %s\n", transmitRobot);
			ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
			Delay(0.5);
		}
	break;

	case STATUS_STORE7:  // Change to storage tray handed
		if (!strcmp(receiveRobot, "OK")) {
			if (SetHanded(1, transmitRobot, fetchArguments)) {
				newstatusRobot = STATUS_STORE3;
				printf("Com <- %s\n", transmitRobot);
				ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
			}
			else newstatusRobot = STATUS_ERROR;
		}
	break;
	case STATUS_STORE3:  // Goto storage tray
		if (!strcmp(receiveRobot, "OK")) {
			if (NextPosition(1, transmitRobot, fetchArguments, ZPOS_OFFSET)) {
				receiveRobotPtr = 0;
				printf("Com <- %s\n", transmitRobot);
				ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
				newstatusRobot = STATUS_STORE4;
			}
			else newstatusRobot = STATUS_ERROR;
		}
	break;
	case STATUS_STORE4:  // Release the chip
		if (!strcmp(receiveRobot, "OK")) {
			receiveRobotPtr = 0;
			sprintf(transmitRobot, "@DO2(%d)=%d\r", SUCK_BIT, RELEASE_CHIP);
			printf("Com <- %s\n", transmitRobot);
			ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
			newstatusRobot = STATUS_STORE5;
			Delay(0.5);
		}
	break;
	case STATUS_STORE5:  // Park the robot
		if (!strcmp(receiveRobot, "OK")) {
			if (ParkPosition(1, transmitRobot, fetchArguments)) {
				receiveRobotPtr = 0;
				printf("Com <- %s\n", transmitRobot);
				ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
				newstatusRobot = STATUS_STORE6;
			}
			else newstatusRobot = STATUS_ERROR;
		}
	break;
	case STATUS_STORE6:
		if (!strcmp(receiveRobot, "OK")) {
			newstatusRobot = STATUS_IDLE;
			TcpSendMessage("STATUS", "OK");
		}
	break;

	case STATUS_REINSERT1:	// Activate the valve
		if (!strcmp(receiveRobot, "OK")) {
			receiveRobotPtr = 0;
			sprintf(transmitRobot, "@DO2(%d)=%d\r", VALVE_BIT, ACTIVATE_VALVE);
			printf("Com <- %s\n", transmitRobot);
			ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
			newstatusRobot = STATUS_REINSERT2;
			Delay(0.5);
		}
	break;
	case STATUS_REINSERT2:  // Move to tester
		if (!strcmp(receiveRobot, "OK")) {
			if (TesterPosition(fetchArguments[0], transmitRobot, 1)) {
				receiveRobotPtr = 0;
				printf("Com <- %s\n", transmitRobot);
				ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
				newstatusRobot = STATUS_REINSERT3;
			}
			else newstatusRobot = STATUS_ERROR;
		}
	break;
	case STATUS_REINSERT3:	// Go to tester position + 1
		if (!strcmp(receiveRobot, "OK")) {
			if (TesterPosition(fetchArguments[0], transmitRobot, 2)) {
				receiveRobotPtr = 0;
				printf("Com <- %s\n", transmitRobot);
				ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
				newstatusRobot = STATUS_REINSERT4;
			}
			else newstatusRobot = STATUS_ERROR;
		}
	break;
	case STATUS_REINSERT4:  // Catch the chip
		if (!strcmp(receiveRobot, "OK")) {
			receiveRobotPtr = 0;
			sprintf(transmitRobot, "@DO2(%d)=%d\r", SUCK_BIT, CATCH_CHIP);
			printf("Com <- %s\n", transmitRobot);
			ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
			newstatusRobot = STATUS_REINSERT5;
			Delay(0.5);
		}
	break;
	
	case STATUS_REINSERT5:	// Go up
		if (!strcmp(receiveRobot, "OK")) {
			if (TesterPosition(fetchArguments[0], transmitRobot, 0)) {
				receiveRobotPtr = 0;
				printf("Com <- %s\n", transmitRobot);
				ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
				newstatusRobot = STATUS_REINSERT6;
			}
			else newstatusRobot = STATUS_ERROR;
		}
	break;
	case STATUS_REINSERT6:  // Go down
		if (!strcmp(receiveRobot, "OK")) {
			if (TesterPosition(fetchArguments[0], transmitRobot, 1)) {
				receiveRobotPtr = 0;
				printf("Com <- %s\n", transmitRobot);
				ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
				newstatusRobot = STATUS_REINSERT7;
			}
			else newstatusRobot = STATUS_ERROR;
		}
	break;
	case STATUS_REINSERT7:	// Release the chip
		if (!strcmp(receiveRobot, "OK")) {
			if (fetchArguments[FETCH_STAY]) newstatusRobot = STATUS_FETCH9;
			else newstatusRobot = STATUS_REINSERT8;
			receiveRobotPtr = 0;
			sprintf(transmitRobot, "@DO2(%d)=%d\r", SUCK_BIT, RELEASE_CHIP);
			printf("Com <- %s\n", transmitRobot);
			ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
			Delay(0.5);
		}
	break;

	case STATUS_REINSERT8:	// Go to tester position + 1
		if (!strcmp(receiveRobot, "OK")) {
			if (TesterPosition(fetchArguments[3], transmitRobot, 2)) {
				receiveRobotPtr = 0;
				printf("Com <- %s\n", transmitRobot);
				ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
				newstatusRobot = STATUS_REINSERT9;
			}
			else newstatusRobot = STATUS_ERROR;
		}
	break;
	case STATUS_REINSERT9:	// Sence the sensors
		if (!strcmp(receiveRobot,"OK")) {
			receiveRobotPtr = 0;
			sprintf(transmitRobot, "@SEND DI2() TO CMU\r");
			printf("Com <- %s\n", transmitRobot);
			ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
			newstatusRobot = STATUS_REINSERT10;
		}
	break;
	case STATUS_REINSERT10:	// Decode sensor reply = 0
		pattern = DecodeBitPattern("DI2()", receiveRobot);
		printf("Com <-> %d\n", pattern);
		if (pattern == 0) {
			receiveRobotPtr = 0;
			newstatusRobot = STATUS_REINSERT11;
		}
		else newstatusRobot = STATUS_ERROR;
	break;
	case STATUS_REINSERT11:	// Go to tester position + 2
		if (!strcmp(receiveRobot, "OK")) {
			if (TesterPosition(fetchArguments[3], transmitRobot, 3)) {
				receiveRobotPtr = 0;
				printf("Com <- %s\n", transmitRobot);
				ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
				newstatusRobot = STATUS_REINSERT12;
			}
			else newstatusRobot = STATUS_ERROR;
		}
	break;
	case STATUS_REINSERT12:	// Sence the sensors = B0001
		if (!strcmp(receiveRobot, "OK")) {
			newstatusRobot = STATUS_REINSERT13;	
			receiveRobotPtr = 0;
			sprintf(transmitRobot, "@SEND DI2() TO CMU\r");
			printf("Com <- %s\n", transmitRobot);
			ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
		}
	break;
	case STATUS_REINSERT13:	// Decode sensor reply = 0x1
		pattern = DecodeBitPattern("DI2()", receiveRobot);
		printf("Com <-> %d\n", pattern);
		if (pattern == 0x1) {
			receiveRobotPtr = 0;
			newstatusRobot = STATUS_REINSERT14;
		}
		else newstatusRobot = STATUS_ERROR;
	break;
	case STATUS_REINSERT14:	// Go up
		if (!strcmp(receiveRobot, "OK")) {
			newstatusRobot = STATUS_REINSERT15;
			if (TesterPosition(fetchArguments[0], transmitRobot, 0)) {
				receiveRobotPtr = 0;
				printf("Com <- %s\n", transmitRobot);
				ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
			}
			else newstatusRobot = STATUS_ERROR;
		}
	break;
	case STATUS_REINSERT15:	// Release the valve
		if (!strcmp(receiveRobot, "OK")) {
			receiveRobotPtr = 0;
			sprintf(transmitRobot, "@DO2(%d)=%d\r", VALVE_BIT, RELEASE_VALVE);
			printf("Com <- %s\n", transmitRobot);
			ComWrt (ComPort, transmitRobot, strlen(transmitRobot));
			newstatusRobot = STATUS_REINSERT16;
			Delay(0.5);
		}
	break;
	case STATUS_REINSERT16:	// End of Reinsert
		if (!strcmp(receiveRobot, "OK")) {
			newstatusRobot = STATUS_IDLE;
			TcpSendMessage("STATUS", "OK");
		}
	break;

	}	/* End switch */

	if (newstatusRobot == STATUS_NONE) {
		newstatusRobot = STATUS_IDLE;
//		TcpSendMessage("STATUS", "NONE");
	}
	else if (newstatusRobot == STATUS_ERROR) {
		newstatusRobot = STATUS_IDLE;
		sprintf(buf, "ERROR %d", statusRobot);
		printf("STATUS ERROR: %s\n", buf);
		TcpSendMessage("STATUS", buf);
	}
	statusRobot = newstatusRobot;
}	/* End of DecodeRobotReply */

/* Read tray and tester position information from file */
void InitPositions(void)
{
  FILE *fpos;
  char buf[512];
  int traynb,cols,rows;
  float x, y, z;
  int testernb;
  float rot, z1, z2, z3;

  fpos = fopen("positions_trays.txt", "r");
  if (fpos) {
    while(fgets (buf, 510, fpos)) {
      if (buf[0] != '#') {
        if (sscanf(buf,"%d %d %d %f %f %f",
                   &traynb, &rows, &cols, &x, &y, &z) == 6) {

          if (traynb < 10) {
            traynb--;

            if ( (cols == COL_FIRST) && (rows == ROW_FIRST) ) {
              tray[traynb].xFirstRowFirstCol = x;
              tray[traynb].yFirstRowFirstCol = y;
              tray[traynb].zFirstRowFirstCol = z;
            }
            else if ( (cols == COL_LAST) && (rows == ROW_FIRST) ) {
              tray[traynb].xFirstRowLastCol = x;
              tray[traynb].yFirstRowLastCol = y;
              tray[traynb].zFirstRowLastCol = z;
            }
            else if ( (cols == COL_FIRST) && (rows == ROW_LAST) ) {
              tray[traynb].xLastRowFirstCol = x;
              tray[traynb].yLastRowFirstCol = y;
              tray[traynb].zLastRowFirstCol = z;
            }
            else if ( (cols == COL_LAST) && (rows == ROW_LAST) ) {
              tray[traynb].xLastRowLastCol = x;
              tray[traynb].yLastRowLastCol = y;
              tray[traynb].zLastRowLastCol = z;
            }

          }
        }
      }
    }
    fclose(fpos);
  }

  fpos = fopen("positions_tester.txt", "r");
  if (fpos) {
    while(fgets (buf, 510, fpos)) {
      if (buf[0] != '#') {
        if (sscanf(buf,"%d %f %f %f %f %f %f %f",
                   &testernb, &x, &y, &z, &rot, &z1, &z2, &z3) == 8) {

          if ((testernb >= 10) &&  (testernb <= 13)) {
            testernb -= 10;
            tester[testernb].x = x;
            tester[testernb].y = y;
            tester[testernb].z = z;
            tester[testernb].rot = rot;
            tester[testernb].z1 = z1;
            tester[testernb].z2 = z2;
            tester[testernb].z3 = z3;
          }
        }
      }
    }
    fclose(fpos);
  }
  return;
}	/* End of InitPositions */

void InitPanel(void)
{
	SetCtrlAttribute (panelHandle, PANEL_ENABLE,  ATTR_DIMMED, 1);
	SetCtrlAttribute (panelHandle, PANEL_DISABLE, ATTR_DIMMED, 0);
}

void EnablePanel(void)
{
	SetCtrlAttribute (panelHandle,PANEL_ENABLE, 	ATTR_DIMMED, 1);
	SetCtrlAttribute (panelHandle,PANEL_DISABLE,	ATTR_DIMMED, 0);
	SetCtrlAttribute (panelHandle,PANEL_SERVO_ON,	ATTR_DIMMED, 0);
	SetCtrlAttribute (panelHandle,PANEL_SERVO_OFF,	ATTR_DIMMED, 0);
	SetCtrlAttribute (panelHandle,PANEL_SPEED_GET,	ATTR_DIMMED, 0);
	SetCtrlAttribute (panelHandle,PANEL_SPEED_SET,	ATTR_DIMMED, 0);
	SetCtrlAttribute (panelHandle,PANEL_ASPEED_SET,	ATTR_DIMMED, 0);
	SetCtrlAttribute (panelHandle,PANEL_ORIGIN, 	ATTR_DIMMED, 0);
	SetCtrlAttribute (panelHandle,PANEL_ORIGIN_GET, ATTR_DIMMED, 0);
	SetCtrlAttribute (panelHandle,PANEL_WHERE,		ATTR_DIMMED, 0);
	SetCtrlAttribute (panelHandle,PANEL_MOVEP,		ATTR_DIMMED, 0);
	SetCtrlAttribute (panelHandle,PANEL_LEFTY,		ATTR_DIMMED, 0);
	SetCtrlAttribute (panelHandle,PANEL_RIGHTY,		ATTR_DIMMED, 0);
	SetCtrlAttribute (panelHandle,PANEL_COMMAND_SEND,ATTR_DIMMED, 0);
	SetCtrlAttribute (panelHandle,PANEL_AIR,		ATTR_DIMMED, 0);
//	SetCtrlAttribute (panelHandle,PANEL_SOCKET,		ATTR_DIMMED, 0);
	SetCtrlAttribute (panelHandle,PANEL_SOCKET1,		ATTR_DIMMED, 0);
	SetCtrlAttribute (panelHandle,PANEL_SOCKET2,		ATTR_DIMMED, 0);
	SetCtrlAttribute (panelHandle,PANEL_SOCKET3,		ATTR_DIMMED, 0);
	SetCtrlAttribute (panelHandle,PANEL_SOCKET4,		ATTR_DIMMED, 0);
}

void DisablePanel(void)
{
	SetCtrlAttribute (panelHandle,PANEL_ENABLE,		ATTR_DIMMED, 0);
	SetCtrlAttribute (panelHandle,PANEL_DISABLE,	ATTR_DIMMED, 1);
	SetCtrlAttribute (panelHandle,PANEL_SERVO_ON,	ATTR_DIMMED, 1);
	SetCtrlAttribute (panelHandle,PANEL_SERVO_OFF,	ATTR_DIMMED, 1);
	SetCtrlAttribute (panelHandle,PANEL_SPEED_GET,	ATTR_DIMMED, 1);
	SetCtrlAttribute (panelHandle,PANEL_SPEED_SET,	ATTR_DIMMED, 1);
	SetCtrlAttribute (panelHandle,PANEL_ASPEED_SET,	ATTR_DIMMED, 1);
	SetCtrlAttribute (panelHandle,PANEL_ORIGIN,		ATTR_DIMMED, 1);
	SetCtrlAttribute (panelHandle,PANEL_ORIGIN_GET, ATTR_DIMMED, 1);
	SetCtrlAttribute (panelHandle,PANEL_WHERE,		ATTR_DIMMED, 1);
	SetCtrlAttribute (panelHandle,PANEL_MOVEP,		ATTR_DIMMED, 1);
	SetCtrlAttribute (panelHandle,PANEL_LEFTY,		ATTR_DIMMED, 1);
	SetCtrlAttribute (panelHandle,PANEL_RIGHTY,		ATTR_DIMMED, 1);
	SetCtrlAttribute (panelHandle,PANEL_COMMAND_SEND, ATTR_DIMMED, 1);
	SetCtrlAttribute (panelHandle,PANEL_AIR,		ATTR_DIMMED, 1);
	SetCtrlAttribute (panelHandle,PANEL_SOCKET,		ATTR_DIMMED, 1);
	SetCtrlAttribute (panelHandle,PANEL_SOCKET1,		ATTR_DIMMED, 1);
	SetCtrlAttribute (panelHandle,PANEL_SOCKET2,		ATTR_DIMMED, 1);
	SetCtrlAttribute (panelHandle,PANEL_SOCKET3,		ATTR_DIMMED, 1);
	SetCtrlAttribute (panelHandle,PANEL_SOCKET4,		ATTR_DIMMED, 1);
}

/* Callbacks from the calibration panel itself */
int CVICALLBACK CalPanelClb (int panel, int event, void *callbackData, int eventData1, int eventData2)
{
	switch (event)
		{
		case EVENT_GOT_FOCUS:
		break;

		case EVENT_LOST_FOCUS:
		break;

		case EVENT_CLOSE:
			HidePanel (CalPanel);
		break;
		}
	return 0;
}

/* If something is changed on the calibration panel */
int CVICALLBACK TrayCalClb (int panel, int control, int event, void *callbackData, int eventData1, int eventData2)
{
int    TrayNr, Corner;
double Z, Rot=0, Xpos, Ypos;
double YOffset[] ={0,-20, -295,  -319.2,-295,   -20,     3.5};
double XOffset[] ={0, -3.5, -3.5, 115,   140.1, 140.1, 115};
char   Buf [100];

	switch (event) {
		case EVENT_COMMIT:
			GetCtrlVal (CalPanel, CALPANEL_TRAY, &TrayNr);
			GetCtrlVal (CalPanel, CALPANEL_CORNER, &Corner);
			GetCtrlVal (CalPanel, CALPANEL_Z, &Z);

			Xpos = tray[TrayNr-1].x + XOffset[Corner];
			Ypos = tray[TrayNr-1].y + YOffset[Corner];

			if (TrayNr >= 5) 
				ComWrt (ComPort, "@LEFTY\r", 7);
			else
				ComWrt (ComPort, "@RIGHTY\r", 8);
			
			switch (control) {
				case CALPANEL_TRAY:
					sprintf(Buf, "@MOVE P, %5.2f %5.2f %5.2f %5.2f 0 0, Z=0.00\r", Xpos, Ypos, Z, Rot);
					printf ("Com <- %s", Buf);
					ComWrt (ComPort, Buf, strlen(Buf));
				break;

				case CALPANEL_Z:
					sprintf(Buf, "@MOVE P, %5.2f %5.2f %5.2f %5.2f 0 0\r", Xpos, Ypos, Z, Rot);
					printf ("Com <- %s", Buf);
					ComWrt (ComPort, Buf, strlen(Buf));
				break;

				case CALPANEL_CORNER:
					sprintf(Buf, "@MOVE P, %5.2f %5.2f %5.2f %5.2f 0 0, Z=100.00\r", Xpos, Ypos, Z, Rot);
					printf ("Com <- %s", Buf);
					ComWrt (ComPort, Buf, strlen(Buf));
				break;
			}
		break;
	}
	return 0;
}	/* End of CVICALLBACK TrayCalClb */

void CVICALLBACK MenuClb (int menuBar, int menuItem, void *callbackData, int panel)
{
	switch (menuItem) {
		case MENU_FILE_EXIT:
			ShutDown();
		break;

		case MENU_TOOLS_CALIBRATE:
			DisplayPanel(CalPanel);
		break;
	}
}