/**
 * This is a firmware for the Lego Mindstorm robot partially based on
*/

#include "constantes.h"


#define CMD_IGNORE 0x81
/**
 * ======================================================================
 * GLOBAL VARIABLES
* ======================================================================
 */
// gdata = global message buffer which is the message received by the robot (8 bytes) 
// there is one for each robot, id=0..7
// so the robot's id is set (global constant below) and then the robot only needs to
//  look up it's byte in gdata[id] to know what it should be doing
static volatile signed char gdata[8];		
static volatile unsigned char id;		//the ID of the robot
static unsigned char rv;			//flag that we received a msg

int leftWheel[2][3];
int rightWheel[2][3];
int motadir;
int motcdir;
int motavel;
int motcvel;

/**
 * ======================================================================
 * Prototypes
 * ======================================================================
 */
// wakeup_t is a brickos data type -- a wakeup task descriptor
wakeup_t prgmKeyCheck(wakeup_t data);
wakeup_t cmdReceived(wakeup_t data);
wakeup_t cmdOrKey(wakeup_t dummy);
void rx_msg( const unsigned char * data, unsigned char len );
void motor_drive();


/**
 * ======================================================================
 * The MAIN function
 * ======================================================================
 */
int main(int argc,char *argv[]) 
{

 
  rightWheel[L_ARC][XXS] = XL_SPEED;
  rightWheel[L_ARC][XS] = XL_SPEED;
  rightWheel[L_ARC][S] = XL_SPEED;
  leftWheel[R_ARC][XXS] = XL_SPEED;
  leftWheel[R_ARC][XS] = XL_SPEED;
  leftWheel[R_ARC][S] = XL_SPEED;
  
  leftWheel[L_ARC][XXS] = 10;
  leftWheel[L_ARC][XS] = 70;
  leftWheel[L_ARC][S] = 90;
  
  rightWheel[R_ARC][XXS] = 10;
  rightWheel[R_ARC][XS] = 95;
  rightWheel[R_ARC][S] = 145;
  
  
  //Local variables
  tid_t process_id = -1;			//the process ID
  
  //Initializing code
  lnp_logical_init();				//May not have to call
  lnp_fast_block_set_handler(rx_msg);		//Set the handle to incoming msg function
  
  memset((void *)gdata, 0, sizeof(gdata) );	//initialize the global message buffer
  
  id = 0;					//Set the id default 0
  cputw(id);					//prints a hex value on the RCX display

  //Normal execution code
  while(1) 
  {
    //cputs("wait");				//displays "wait" to the RCX display (or any short 4-char string you specify)
    wait_event(cmdOrKey, 0);			//suspend program until either key is pressed or a message is received
						//see cmdOrKey() function declared below

    //If the "Prgm" key (on the RCX) was pressed
    if(prgmKeyCheck(0)) 
    {
      //Ensure proper termination of process
      if (process_id != -1) 
      {			// only kill tasks if the robot isn't idle
	kill(process_id);
      }//end if
      
      //Try to find a robot ID
      id = (id + 1) % ROBOTS_NUM;		//Try next ID
   
      cputw(id);				//Display ID
      msleep(500);				//Keep display for 1/2 second
    } 
    else 
    {
      //It wasn't the program key, it was the received command
      if ((unsigned char)gdata[id] != CMD_IGNORE)
       {
	if (process_id != -1) 
	{	/* kill what robot is currently doing...*/
//	  motor_a_speed(-1);
//	  motor_c_speed(-1);
//	  motor_a_dir(brake);
//	  motor_c_dir(brake);
	  kill(process_id);
	}//end if
	/* and then create a process to execute the robot's command! */
	process_id = execi((void *)&motor_drive,0,0,PRIO_NORMAL,DEFAULT_STACK_SIZE);
	}
    }//end if
  }//end while
  
  return 0;		//Normal exit of program
}//end main


/**
 * ======================================================================
 * The SUPPORTING function
 * ======================================================================
 */

/**
 * Handles incoming messages
 */
void rx_msg( const unsigned char * data, unsigned char len ) 
{
	int i;					//loop counter
	//	static const note_t click[] = { { PITCH_D4,  2 } , { PITCH_C4,  1 } , { PITCH_END,  0 } };
	//	dsound_play(click);
	for( i=0; i< len-2; i++) 
	{
		gdata[i] = data[i+1]; //copy the passed data to global data
	}//end for

	rv = 1;					//set the rcv flag
}//end rx_msg


/**
 * Checks if the program key is pressed
 */
wakeup_t prgmKeyCheck(wakeup_t data) 
{
	wakeup_t value = 0;		//return value

	//Only if the program key was pressed.
	if (dkey == KEY_PRGM) 
	{
		value = 1;
	}
	return value;
}//end prgmKeyCheck


/**
 * Checks if a receive command was received
 */
wakeup_t cmdReceived(wakeup_t data) 
{
	wakeup_t value = rv;			//Assign the return value to be whatever was the rv flag
	rv = 0;					//reset the rcv flag
	return value;
}//end cmdReceived


/**
 * Whether the command received or key was pressed
 */
wakeup_t cmdOrKey(wakeup_t dummy) 
{
	return (cmdReceived(0) || prgmKeyCheck(0));
}//end cmdOrKey


/**
 * The function that handles the motor functions
 */
void motor_drive() {
	unsigned char msg;		//El Mensaje Recibido
	msg = gdata[id];		//Extrae el Mensaje al Robot
	int cmd=(int)msg / 10;		//Comando a Ejecutar
	int sleept=msg % 10;		//Tiempo de sleep
	
	int sleeptime = sleept*20 + 20;  //Define un rango de 20 a 120 ms con pasos de a 20.

	motadir=0;
	motcdir=0;
	motavel=0;
	motcvel=0;
	switch (cmd)
	{
	  case ROT_CW:
	    motadir = DIR_REV;
	    motcdir = DIR_FWD;
	    motavel = XL_SPEED;
	    motcvel = XL_SPEED;
	    break;
	  case ROT_CCW:
	    motadir = DIR_FWD;
	    motcdir = DIR_REV;
	    motavel = XL_SPEED;
	    motcvel = XL_SPEED;
	    break;
	  case MOV_FWD:
	    motadir = DIR_FWD;
	    motcdir = DIR_FWD;
	    motavel = XL_SPEED;
	    motcvel = L_SPEED;		//Cambiado para que ande derecho
	    break;
	  case MOV_BWD:
	    motadir = DIR_REV;
	    motcdir = DIR_REV;
	    motavel = XL_SPEED;
	    motcvel = L_SPEED;		//Cambiado para que ande derecho
	    break;	  
	  case RARCS_FWD:
	    motadir = DIR_REV;
	    motcdir = DIR_FWD;
	    motavel = rightWheel[R_ARC][XXS];
	    motcvel = leftWheel[R_ARC][XXS];
	    break;
	  case RARCS_BWD:
	    motadir = DIR_FWD;
	    motcdir = DIR_REV;
	    motavel = rightWheel[R_ARC][XXS];
	    motcvel = leftWheel[R_ARC][XXS];
	    break;
	  case LARCS_FWD:
	    motadir = DIR_FWD;
	    motcdir = DIR_REV;
	    motavel = rightWheel[L_ARC][XXS];
	    motcvel = leftWheel[L_ARC][XXS];
	    break;
	  case LARCS_BWD:
	    motadir = DIR_REV;
	    motcdir = DIR_FWD;
	    motavel = rightWheel[L_ARC][XXS];
	    motcvel = leftWheel[L_ARC][XXS];
	    break;
	  case RARCM_FWD:
	    motadir = DIR_FWD;
	    motcdir = DIR_FWD;
	    motavel = rightWheel[R_ARC][XS];
	    motcvel = leftWheel[R_ARC][XS];
	    break;
	  case RARCM_BWD:
	    motadir = DIR_REV;
	    motcdir = DIR_REV;
	    motavel = rightWheel[R_ARC][XS];
	    motcvel = leftWheel[R_ARC][XS];
	    break;
	  case LARCM_FWD:
	    motadir = DIR_FWD;
	    motcdir = DIR_FWD;
	    motavel = rightWheel[L_ARC][XS];
	    motcvel = leftWheel[L_ARC][XS];
	    break;
	  case LARCM_BWD:
	    motadir = DIR_REV;
	    motcdir = DIR_REV;
	    motavel = rightWheel[L_ARC][XS];
	    motcvel = leftWheel[L_ARC][XS];
	    break;
	  case RARCL_FWD:
	    motadir = DIR_FWD;
	    motcdir = DIR_FWD;
	    motavel = rightWheel[R_ARC][S];
	    motcvel = leftWheel[R_ARC][S];
	    break;
	  case RARCL_BWD:
	    motadir = DIR_REV;
	    motcdir = DIR_REV;
	    motavel = rightWheel[R_ARC][S];
	    motcvel = leftWheel[R_ARC][S];
	    break;
	  case LARCL_FWD:
	    motadir = DIR_FWD;
	    motcdir = DIR_FWD;
	    motavel = rightWheel[L_ARC][S];
	    motcvel = leftWheel[L_ARC][S];
	    break;
	  case LARCL_BWD:
	    motadir = DIR_REV;
	    motcdir = DIR_REV;
	    motavel = rightWheel[L_ARC][S];
	    motcvel = leftWheel[L_ARC][S];
	    break;
	  case FRENAR:
	    motadir = brake;
	    motcdir = brake;
	    motavel = -1;
	    motcvel = -1;
	    break;	    
	  case MOVSLOW_FWD:
	    motadir = DIR_FWD;
	    motcdir = DIR_FWD;
	    motavel = M_SPEED;
	    motcvel = M_SPEED;
	    break;	    
	  case MOVSLOW_BWD:
	    motadir = DIR_REV;
	    motcdir = DIR_REV;
	    motavel = M_SPEED;
	    motcvel = M_SPEED;
	    break;	    
	}	
	motor_a_dir(motadir);
	motor_c_dir(motcdir);
	motor_a_speed(motavel);
	motor_c_speed(motcvel);
	msleep(sleeptime);
	motor_a_dir(brake);
	motor_c_dir(brake);
}//end motor_drive
