/*
This program is used to collect trainingdata for maze driving. Therefore the robot should
be pushed through the maze in a way he should act on his own later. The training data is
send to the PC via radio, therefore the recaive_train program should run on a PC where a radio
transiever is connencted. The robot starts sending data when the "SEN" button is pressed (KEY2). It
stops sending when "STP" is pressed (KEY3). The files on PC are closed when "Q" is pressed (KEY1). The
program is ended pressing "END" (KEY4).
*/
#include "eyebot.h"
#include "messaging.h"
#define CALL_F 10/*sample rate for training data collection*/

PSDHandle f_psd,l_psd,r_psd;
TimerHandle t1;
VWHandle vw;
RadioIOParameters radioParams;
BYTE myId, nextId, fromId;
BYTE mes[40]; /* message buffer */

int key;

/*
gets training data and sends it
message structure:
                1 BYTE - number of input  data (3)
                1 BYTE - number of output data (2)
3 * sizeof(int)   BYTE - input  data
2 * sizeof(float) BYTE - output data
*/
void callback(){
	int front,left,right,err,offset,count;


	SpeedType speed;
	PositionType pos;
        /*read OSDs*/
	left  = PSDGet(l_psd);
	front = PSDGet(f_psd);
	right = PSDGet(r_psd);
	/*read V-Omega*/
	err = VWGetSpeed(vw,&speed);
	err = VWGetPosition(vw, &pos);

	LCDSetPos (0, 0);
	LCDPrintf("%2.2f  %2.2f\n%4d  %4d  %4d\n%2.2f %2.2f %2.2f\n", speed.v,speed.w,left,front,right,pos.x,pos.y,pos.phi);

        mes[0]=0x03;/*NIN*/
	mes[1]=0x02;/*NOUT*/
	offset=2;
	int2mes(left,mes + offset);
	offset+=sizeof(int);
	int2mes(front,mes + offset);
	offset+=sizeof(int);
	int2mes(right,mes + offset);
	offset+=sizeof(int);
	flt2mes(speed.v,mes + offset);
	offset+=sizeof(float);
	flt2mes(speed.w,mes + offset);
	offset+=sizeof(float);
	/* sends the data to the pc*/
	if (key==KEY2){
	  err = RADIOSend(nextId, offset, mes);
	  if (err) {count++;LCDPrintf("%3d Error Send\n",count);}
	}
	/* stops the pc programm sending the quit message*/
	if (key==KEY1){
	  mes[0]=0;
	  err = RADIOSend(nextId, 1, mes);
	  if (err) {count++;LCDPrintf("%3d Error Send\n",count);}
	}
}

int main(void)
{

	int check,err;

	/* initialisation*/
	l_psd = PSDInit(PSD_LEFT);
	f_psd = PSDInit(PSD_FRONT);
	r_psd = PSDInit(PSD_RIGHT);
	OSWait(50);
	printf("Drive");
	vw = VWInit(VW_DRIVE ,1);
	if (vw==0) { printf("Error Drive Init\n"); return 1; }
	else printf("Init\n");
	err = VWStartControl(vw,7,0.3,7,0.1);
	if (err ==-1) { printf("Error starting control\n"); return 1; }
	else printf("Control started\n");
	err = PSDStart(l_psd|f_psd|r_psd, TRUE);
	if (err != 0) { printf("Error starting PSDs\n"); return 1; }
	else printf("PSDs started\n");
	/* start radio*/
	printf("Radio");
	err = RADIOInit();
	if (err) { printf("Error Radio Init\n"); return 1; }
	else printf("Init\n");
	LCDPrintf("sizeof(float) %d\n",sizeof(float));
	OSWait(50);



	LCDClear();
	LCDMenu ("Q","SEN","STP","END");
	t1 = OSAttachTimer(CALL_F, callback);
	key = 0;
	while (key!=KEY4){
		check=KEYRead();
		if (check!=0) key = check;
		OSWait(1);
	}
	/* release*/
	VWRelease(vw);
	OSDetachTimer(t1);
        return 1;
}