/***********************************************************************/
/** @name SensorsOmni.c
    This program makes an Eyebot omnidirectional robot vehicle react to
		input given through sensors at the analog ports
    
    @author Birgit Graf, UWA, 2000
    @version 1
*/
/***********************************************************************/

/*@{*/


#include "eyebot.h"
#include "libomni/omni.h"

#include <stdio.h>
#include <math.h>

#define FALSE   0
#define TRUE    1

#define RIGHT 0
#define LEFT 1

/** Handle for motors */
VWHandle m_vw;

/* \D to terminate transmission */
char term    =  4;

/** control input? */
int m_bUseControl = 0;

/** array to save last sensor values */
#define MAX_ARRAY 100
int m_lastValues[MAX_ARRAY][2];

/* currently used maximum number of elements */
int m_arrayMaxElements = 10;	
/* pointer to current element (elements 0 to m_currentValue are used) */
int m_currentValue = 0;	
/* values currently in array:0 to m_currentMaxElementNo */
int m_currentMaxElementNo = 0;

int m_actValsPerSec = 16;
int m_nomValsPerSec = 20;
 
double MaxVelLin = 0.8;		    /* m/s   */
double MaxVelRot = M_PI/4.0;	/* rad/s */


/***********************************************************************/
/** Init VW/OMNI interface.

    Called by main().
    @see main()
*/
/***********************************************************************/

int InitDrive()
{
	/** values for RoBIOS PI controller (used in vw interface) */
	#define v_lin 7.0
	#define t_lin 0.3
	#define v_rot 10.0
	#define t_rot 0.1

  LCDPutString("InitDrive\n");

  m_vw = OMNI_Init(VW_DRIVE, 1);
  if (!m_vw)
    LCDPutString("Error: OMNI_Init!\n");

/*	if (OMNI_StartControl(m_vw, 0.09 , 0.3,  1.0, 0.1) != 0)
    LCDPutString("Error: VWStart!\n");
*/
  OMNI_SetPosition(m_vw, 0.0, 0.0, 0.0);
  return 0;
}



void set_ctrl_params()
{
	int boc[] = {0, 0, 0, 0};
  int ind = 0;

  int end_proc = FALSE;

  LCDClear();
  LCDMenu(" + "," - ","Nxt","END");
  LCDPrintf("Set parameters:\n");

	boc[0] = m_bUseControl;
	boc[1] = m_arrayMaxElements;

  while (!end_proc)
  {
    LCDSetPos(2, 0);
    LCDPrintf("Filter:    ");
    if (boc[0] == 0) LCDPrintf("OFF\n");
    else LCDPrintf(" ON \n");

		if (boc[0])	/* control activated */
	    LCDPrintf("No. elem. :%3.d\n", boc[1]);
		else
	    LCDPrintf("                    \n");	/* clear */

    LCDSetChar(2 + ind, 15, '*');

    switch (KEYRead())
    {
    case KEY1:      boc[ind] ++;
      if (ind == 0)
				if (boc[ind] > 1) boc[ind] = 0;
			if (ind == 1)
				if (boc[0])	/* control activated */
					if (boc[1] > MAX_ARRAY)
						boc[1] = MAX_ARRAY;
      break;

    case KEY2:      boc[ind] --;
      if (ind == 0)
				if (boc[ind] < 0) boc[ind] = 1;
			if (ind == 1)
				if (boc[0])	/* control activated */
					if (boc[1] < 1)
						boc[1] = 1;
     break;

    case KEY3:
 			if (boc[0])	/* control activated */
			{
				LCDSetChar(2 + ind, 15, ' ');
				ind ++;
				if (ind > 1) ind = 0;
			}
      break;

    case KEY4:
			m_bUseControl = boc[0];
		 	m_arrayMaxElements = boc[1];
			LCDClear(); 
			end_proc = TRUE; 
			break;

    default: break;
    }
  }
}


/***********************************************************************/
/** Set Parameter over keyboard (float).

    @param text[] name of parameter
    @param minp,maxp minimum and maximum value of parameter 
    @param start start value for parameter
    @param inc step to increment parameter
*/
/***********************************************************************/

float set_fparam(char text[], float minp, float start, float maxp, float inc)
{
  float val;
  int   done = FALSE;

  LCDClear();
  LCDPrintf("Set Parameter\n%s\n", text);

  LCDMenu("+", "-", " ","OK");
  val = start;
  do
  {
    LCDSetPos(5, 0);
    LCDPrintf("%f\n", val);
    switch(KEYGet())
    {
    case KEY1:
      if (val < maxp)
	val += inc;
      break;
    case KEY2:
      if (val > minp)
	val -= inc;
      break;
    case KEY4:
      done = TRUE;
    }
  } while (!done);

  LCDClear();
  return val;
}



/***********************************************************************/
/** Change driving parameters.
    Changes robot speed.
*/
/***********************************************************************/

void set_drv_params()
{
  int ind = 0;
  int end_proc = FALSE;

  LCDClear();
  LCDMenu("CHG", "NXT", " ", "END");

  while (!end_proc)
  {
    LCDSetPos(0, 0);
    LCDPrintf("Driving param.\n");

    LCDSetPos(2, 0);
    LCDPrintf("Lin speed\n");
    LCDSetPos(3, 0);
    LCDPrintf("Rot speed\n");

    LCDSetChar(2 + ind, 15, '*');

    switch (KEYGet())
    {
    case KEY1:
      switch(ind)
      {
      case 0:
				MaxVelLin = set_fparam("Lin speed", 0.0, MaxVelLin, 1.0, 0.1);
				break;
			case 1:
				MaxVelRot = set_fparam("Rot. speed", 0.0, MaxVelRot, 1.5, 0.1);
				break;
      default: break;
      }
      
      LCDMenu("CHG", "NXT", " ", "END");
      break;

    case KEY2:
      LCDSetChar(2 + ind, 15, ' ');
      ind ++;
			if (ind > 1) ind = 0;
      break;

    case KEY4:
      LCDClear(); end_proc = TRUE; break;

    default: break;
    }
  }
}


void SensorValFrom0(int* Sensor)
{
	int m_SensorOffset = 200;

	if (*Sensor < m_SensorOffset)
		*Sensor = 0;
	else
		*Sensor -= m_SensorOffset;
}



void speedFromSensor(int leftSensor, int rightSensor, double* linSpeed, double* rotSpeed)
{
	int SensorRange = 824; 
	int sumRight = 0, sumLeft = 0,i;

	SensorValFrom0(&leftSensor);
	SensorValFrom0(&rightSensor);

	if (m_bUseControl)
	{
		m_lastValues[m_currentValue][RIGHT] = rightSensor;
		m_lastValues[m_currentValue][LEFT] = leftSensor;
		
		/* set pointer to next element in list (will be overwritten in next cycle */
		if (m_currentValue<m_arrayMaxElements)	/* reached end of array? */
			m_currentValue++;
		else
			m_currentValue = 0;

		/* set number of elements currently in list */
		if (m_currentMaxElementNo<m_arrayMaxElements)	/* reached end of array? */
			m_currentMaxElementNo++;

		for (i=0;i<m_currentMaxElementNo;i++)
		{
			sumRight += m_lastValues[i][RIGHT];
			sumLeft += m_lastValues[i][LEFT];
		}


		*linSpeed = MaxVelLin * (double)(sumLeft + sumRight)/SensorRange/2/m_currentMaxElementNo;
		*rotSpeed = MaxVelRot * (double)(sumRight - sumLeft)/SensorRange/m_currentMaxElementNo;
	}
	else
	{
		*linSpeed = MaxVelLin * (double)(leftSensor + rightSensor)/SensorRange/2;
		*rotSpeed = MaxVelRot * (double)(rightSensor - leftSensor)/SensorRange;
	}

	LCDPrintf("lin: %f\n", *linSpeed);
	LCDPrintf("rot: %f\n", *rotSpeed);
}


/***********************************************************************/
/** Send string.
    Send string to computer. 
*/
/***********************************************************************/

void sendstr(char s[], int size)
{
	int i=0, lines=0;

  OSInitRS232(SER115200, NONE, SERIAL1);	
	for (i=0; i<size; i++)
  { 
		OSSendCharRS232(*&s[i], SERIAL1);
    LCDPrintf("%c",s[i]);
		if (s[i] =='\n')
			lines ++;
	}
  OSSendRS232(&term, SERIAL1);
  LCDPrintf("\ndone (%d lines)\n", lines);
}


void clear_buffer(char s[], int size)
{
	int i;
	for (i=0;i<size; i++)
		s[i] = ' ';
}

/***********************************************************************/
/** Start program.
    The main-funktion controlls the several states of the
    program. 
*/
/***********************************************************************/

int main()
{
  int exit_program = FALSE;
  int key;
	int rightSensor = 0;
	int leftSensor = 0;

	/* buffer to save sensor values - can be uploaded */
#define BUFFER_SIZE 10000
	char sensorValues[BUFFER_SIZE];
	int buffer_pos = BUFFER_SIZE;
	int last100 = 100;
	int duplicateCounter = 0;

	/* to measure duration of sensor reading */
	int start_time, dur_time = 0;
	int measurements = 0;

	double linSpeed, rotSpeed;
	
  InitDrive();			/* init motors */

  do
  {
    /* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
    LCDMenu("GO", "CTR", "DRV", "END");
    /* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */

   key = KEYRead();
     
    switch(key)
    {
    /* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
    case KEY1:			/* - - - - - GO - - - - - */
		 	clear_buffer(sensorValues, buffer_pos);
			buffer_pos = 0;
			last100 = 100;
			duplicateCounter = 0;

			measurements = 0;
			start_time = OSGetCount();
		 
			do {
	    LCDClear();
	    LCDMenu("", "", "STOP", "");

			/* read sensors */
	    leftSensor = OSGetAD(3);
	    LCDPrintf("lV: %d\n", leftSensor);
	    OSWait(1);
	    rightSensor = OSGetAD(2);
	    LCDPrintf("rV: %d\n", rightSensor);

			/* save sensor values to buffer */
			if (buffer_pos + 10 < BUFFER_SIZE)
			{
				measurements ++;
/*			buffer_pos += sprintf(sensorValues + buffer_pos, 
				"%4d %4d\n", 1234, 5678);*/
				buffer_pos += sprintf(sensorValues + buffer_pos, 
					"%4d %4d\n", leftSensor, rightSensor); 

				if (m_nomValsPerSec != m_actValsPerSec)
				{
					if (m_nomValsPerSec < m_actValsPerSec)
						LCDPrintf("Error: nom < act!");
					else
					{
						if (duplicateCounter < m_actValsPerSec/(m_nomValsPerSec-m_actValsPerSec))
							duplicateCounter ++;
						else
						{
							/* dublicate sensor value in order to get <m_nomValsPerSec>
							values per second */
							buffer_pos += sprintf(sensorValues + buffer_pos, 
								"%4d %4d\n", leftSensor, rightSensor); 
							duplicateCounter =0;
						}
					}
				} /* m_nomValsPerSec == m_actValsPerSec: do nothing */
			}

			/* beep every 100 sensor readings */
			if (measurements > last100)
			{
				AUBeep();
				if (buffer_pos +10 >= BUFFER_SIZE)
					AUBeep();

				last100 += 100;
			}

			speedFromSensor(leftSensor, rightSensor, &linSpeed, &rotSpeed);
		  OMNI_SetSpeed (m_vw, linSpeed, 0, rotSpeed);

	    key = KEYRead();
	  }while (key != KEY3);

		OMNI_SetSpeed(m_vw, 0, 0, 0);

		dur_time = OSGetCount() - start_time;
		m_actValsPerSec = (100 * measurements)/dur_time;

		OSWait(100);
		LCDClear();

		LCDPrintf("Speed: %d\nmeasurements\nper second\n", m_actValsPerSec);
		LCDPrintf("\nUpload sensor \nvalues?");
		LCDMenu("Yes", "", "", "No");
		if (KEYGet() == KEY1)
			sendstr(sensorValues, buffer_pos);
		else
			LCDClear();
    break;
    
    /* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
    case KEY2:			/* - - - - - CTR - - - - - */
			set_ctrl_params();
		  break;
   
    /* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
    case KEY3:			/* - - - - - DRV - - - - - */
			set_drv_params();
		  break;

		/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */   
    case KEY4:       /* - - - - END - - - - */
			LCDClear();
			exit_program = TRUE; 
			break;
    default: break;
    }
  } while (!exit_program);
  
  OMNI_Release(m_vw);
  return 0;
}

/*@}*/