#include "RobotControl.hh"

#define POS_INT 0x3B
#define NEG_INT 0x1B
#define STRING  0x2B

RobotControl::RobotControl( Channel<unsigned char*>* comm, bool init)
{
  commlink = comm;

  stateLock = NULL;

  sonar = new Sonar();

  stateLock = new Semaphore(1);

  if (!stateLock)
    {
      printf ("Error getting state lock semaphore.\n");
      exit (1);
    }

  if (init)
    {
      Initialize();
    }
}

RobotControl::~RobotControl ()
{
  Close ();

  delete sonar;
  delete stateLock;
}

int
RobotControl::Initialize()
{
  unsigned char sync0[] =      {0x00};
  unsigned char sync1[] =      {0x01};
  unsigned char sync2[] =      {0x02};
  unsigned char openString[] = {0x01};
  unsigned char echoString[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
  unsigned char* command;

  if (!commlink->Open())
    {
      cerr << commlink->Name() << ": Error opening communications channel!" 
	   << endl;
      return ERROR;
    }

  SendCommand( openString, 1, false );

  command = SendCommand( sync0, 1, true );

  int n = 10;
  bool valid = false;
  while ( n > 0 && !valid )
    {
      commlink->Read(echoString,6);
      n--;
      valid = true;

      for (int i = 0; i < 6; i++)
	if (command[i] != echoString[i])
	  valid = false;
    }

  delete command;

  if (!valid)
    {
      cerr << commlink->Name() << ": Error initiating communication with the "
 	   << "robot!" << endl;
      return ERROR;
    }
  
  command = SendCommand( sync1, 1, true );

  n = 10;
  valid = false;
  while (n > 0 && !valid)
    {
      commlink->Read(echoString,6);
      n--;
      valid = true;
      
      for (int i = 0; i < 6; i++)
	if (command[i] != echoString[i])
	  valid = false;
    }

  delete command;

  if (!valid)
    {
      cerr << commlink->Name() << ": Error initiating communication with the "
 	   << "robot!" << endl;
      return ERROR;
    }

  command = SendCommand( sync2, 1, true );

  SendCommand( openString, 1, false );

  cerr << commlink->Name() << ": Communication Initiated." << endl;

  return SUCCESS;
}

int 
RobotControl::Close ()
{
  unsigned char closeString[] = {0x02};
  SendCommand( closeString, 1, false );

  return 0;
}

int
RobotControl::SetOrigin ()
{
  unsigned char origin[] = { 0x07 };
  SendCommand( origin, 1, false );

  return 0;
}

int
RobotControl::Pulse ()
{
  unsigned char pulse[] = { 0x00 };
  SendCommand( pulse,
	       1,
	       false,
	       82000 );

  return 0;
}

int
RobotControl::TranslationalVelocity( int mmSec )
{
  unsigned char setv[] = { 11, POS_INT, 0x00, 0x00 };

  if (mmSec < 0)
    {
      mmSec = abs(mmSec);
      setv[1] = NEG_INT;
    }

  setv[2] = (unsigned char)(mmSec & 0xFF);
  setv[3] = (unsigned char)(mmSec >> 8);

  SendCommand( setv, 4, false );

  return 1;
}

int
RobotControl::TranslationalVelocity( int left_mmSec, 
				     int right_mmSec )
{
  int leftbit = left_mmSec / 4;
  int rightbit = right_mmSec / 4;

  unsigned char vel[] = { 32, POS_INT, leftbit, rightbit };

/*
  if ( left_mmSec < 0 )
    {
      left_mmSec = abs( left_mmSec );
      vel[1] = NEG_INT;
    }
  if ( right_mmSec < 0 )
    {
      right_mmSec = abs( right_mmSec );
      vel[1] = NEG_INT;
    }
*/
  SendCommand( vel, 4, false );

  return 0;
}

int
RobotControl::AbsoluteHeading (int degrees)
{
  unsigned char head[] = { 12, POS_INT, 0x00, 0x00 };

  if (degrees < 0)
    {
      degrees = -degrees;
      head[1] = NEG_INT;
    }

  head[2] = (unsigned char)(degrees & 0xFF);
  head[3] = (unsigned char)(degrees >> 8);

  SendCommand( head, 4, false );
  return 0;
}

int
RobotControl::RelativeHeading (int degrees)
{
  unsigned char dhead[] = { 13, POS_INT, 0x00, 0x00 };

  if (degrees < 0)
    {
      degrees = -degrees;
      dhead[1] = NEG_INT;
    }

  dhead[2] = (unsigned char)(degrees & 0xFF);
  dhead[3] = (unsigned char)(degrees >> 8);

  SendCommand(dhead, 4, false);
  return 0;
}

int
RobotControl::RotationalVelocity (int degreesSec)
{
  unsigned char rvel[] = { 21, POS_INT, 0x00, 0x00 };

  if (degreesSec < 0)
    {
      degreesSec = -degreesSec;
      rvel[1] = NEG_INT;
    }

  rvel[2] = (unsigned char)(degreesSec & 0xFF);
  rvel[3] = (unsigned char)(degreesSec >> 8);

  SendCommand( rvel, 4, false );
  return 0;
}

int
RobotControl::GripperState (int state)
{
  unsigned char gripper[] = { 33, POS_INT, 0x00, 0x00 };

  gripper[2] = (unsigned char)( state & 0xFF );
  gripper[3] = (unsigned char)( state >> 8 );

  SendCommand( gripper, 4, false );
  return 0;
}

int
RobotControl::DigitalOut( unsigned char* sequence, int size )
{
  // 0 - Gripper Motor Rotation Direction
  // 1 - Gripper Motor Enable
  // 2 - User LED
  // 3 - Right Paddle and Front LED's
  // 4 - Left Paddle and front LED's
  // 5 - Speaker
  // 6 - User Servo Motor Logic
  // 7 - User Servo Motor Logic

  unsigned char paddle[] = { 30, STRING, 0x03 };
  SendCommand( paddle, 3, false );

  /*
  unsigned char* sing = new char[ 3 + size ];

  sing[0] = 30;
  sing[1] = STRING;
  sing[2] = 5;

  for ( int i = 0 ; i < size ; i++ )
    sing[i+3] = sequence[i];

  SendCommand( sing, size + 3, false );

  delete sing;
  */
  return 1;
}

int
RobotControl::SonarPollingSequence( unsigned char* sequence, int size )
{
  unsigned char* polling = new unsigned char[ 4 + size ];

  polling[0] = 30;
  polling[1] = STRING;

  return 1;
}

int
RobotControl::XPos ()
{
  stateLock->bdown ();
  int x = xPos;
  stateLock->up ();
  
  return x;
}

int
RobotControl::YPos ()
{
  stateLock->bdown ();
  int y = yPos;
  stateLock->up ();

  return y;
}

float
RobotControl::Theta ()
{
  stateLock->bdown ();
  int t = theta % 3600;
  stateLock->up ();
  
  return (float)t/10.0;
}

unsigned char
RobotControl::MotorStatus ()
{
  stateLock->bdown ();
  unsigned char m = motorStatus;
  stateLock->up ();

  return m;
}

int
RobotControl::LeftWheelVelocity ()
{
  stateLock->bdown ();
  int lv = leftWheelVel;
  stateLock->up ();

  return lv;
}

int
RobotControl::RightWheelVelocity ()
{
  stateLock->bdown ();
  int rv = rightWheelVel;
  stateLock->up ();

  return rv;
}

float
RobotControl::BatteryCharge ()
{
  stateLock->bdown ();
  float bc = (float)batteryCharge/10.0;
  stateLock->up ();

  return bc;
}

bool
RobotControl::LeftWheelStall ()
{
  stateLock->bdown ();
  bool stall = (bool)leftWheelStall;
  stateLock->up ();
  
  return stall;
}

bool
RobotControl::RightWheelStall ()
{
  stateLock->bdown ();
  bool stall = (bool)rightWheelStall;
  stateLock->up ();

  return stall;
}

double
RobotControl::SonarReading (int sonarNumber )
{
  stateLock->bdown();
  double value = sonar->getSonar( sonarNumber );
  stateLock->up();

  return value;
}

void*
RobotControl::UpdateState(void* arg)
{
  RobotControl* robot = (RobotControl*)arg;

  
  /* Server Information Packet */
  /*
  unsigned char sip[] = 
  {
    0x00,	0		// motor status
    0x00,	1		// x position (2 bytes -> 15 LS bits)
    0x00,	2
    0x00,	3		// y position (2 bytes -> 15 LS bits)
    0x00,	4
    0x00,	5		// theta (2 bytes)
    0x00,	6
    0x00,	7		// left wheel velocity (2 bytes)
    0x00,	8
    0x00,	9		// right wheel velocity (2 bytes)
    0x00,	10
    0x00,	11		// battery charge
    0x00,	12		// left motor stall indicator
    0x00,	13		// right motor stall indicator
    0x00,	14		// bumpers ( unsigned int )
    0x00,	15
    0x00,	16		// Control
    0x00,	17
    0x00,	18		// PTU - pulsewidth
    0x00,	19
    0x00,	20		// verbal sound clue
    0x00,	21		// Sonar readings

    * Sonar readings go here *

    0x00,			// Input timer
    0x00,	
    0x00,			// analog input reading
    0x00,			// digital input pins
    0x00,			// digital output pins
    0x00			// digital input pins

    0x00,			// control (2 bytes)
    0x00,
    0x00,			// PTU (2 bytes -> 15 LS bits ??)
    0x00,
    0x00,			// Compass heading (2-degree units)
    0x00,			// Number of sonar readings
    0x00,			// Input timer (2 bytes)
    0x00,
    0x00,			// User analog input reading
    0x00,			// User digital input pins
    0x00,			// User digital output pins
    0x00,			// Checksum (2 bytes)
    0x00
  };
  */

  unsigned char sip[MAX_PACKET_SIZE] = {0};

  while ( true )
    {
      robot->Pulse();

      unsigned char header = 0x00;

      while (header != 0xFA)
	{
	  if ( header != 0x00 )
	    cout << "!";
	  robot->commlink->Read(&header, 1);
	}

      robot->commlink->Read( &header, 1 );

      if ( header == 0xFB )
	{
	  unsigned char size = 0x00;

	  robot->commlink->Read(&size, 1);

	  if ( size < MAX_PACKET_SIZE )
	    {
	      robot->commlink->Read(&sip[1], (int)size);

	      sip[0] = size;
	      int checksum = robot->calculateChecksum( sip );
	      int rec_check = (sip[size-1] << 8 ) + sip[size];

	      if ( checksum == rec_check )
		{
		  robot->stateLock->bdown();

		  robot->motorStatus     = sip[1];
		  robot->xPos            = (((int)sip[2] & 0x7F) << 8) + sip[3];
		  robot->yPos            = (((int)sip[4] & 0x7F) << 8) + sip[5];
		  robot->theta           = ((int)sip[6] << 8) + sip[7];
		  robot->leftWheelVel    = (((int)sip[8] << 8) + sip[9]) * VEL_CONV;
		  robot->rightWheelVel   = (((int)sip[10] << 8) + sip[11]) * VEL_CONV;
		  robot->batteryCharge   = abs(sip[12]);
		  robot->leftWheelStall  = sip[13];
		  robot->rightWheelStall = sip[14];

		  // number of sonar readings
		  if (sip[NUM_SONAR_POSITION] <= MAX_SONAR )
		    {
		      for (int i = 0; i < sip[NUM_SONAR_POSITION] && 
			     i < MAX_SONAR ; i++)
			{
			  int sonar_number = sip[NUM_SONAR_POSITION+1+3*i];
			  
			  int value = 
			    (sip[NUM_SONAR_POSITION+3+3*i] << 8 ) 
			    + sip[NUM_SONAR_POSITION+2+3*i];

			  robot->sonar->setSonar( sonar_number, value );
			}
		    }/* Too many sonar readings */

		  int pos = NUM_SONAR_POSITION + (int)(sip[NUM_SONAR_POSITION]*3) + 4;

		  if ( pos < size )
		    robot->digital_in = sip[pos];

		  robot->stateLock->up();
		}
	    }
	}
    }

  return arg;
}

bool
RobotControl::DigitalValue( int request )
{
  // 0 - Gripper Top Switch
  // 1 - Gripper Carry Switch
  // 2 - Gripper Open Switch
  // 3 - Paddle Inner Break Beam
  // 4 - Paddle Outer Break Beam
  // 5 - Paddle Bump Switches
  // 6 - User Push-Button Switch
  // 7 - User Slide Switch

  stateLock->bdown();

  bool returnVal = digital_in & ( 1 << request );

  stateLock->up();

  return returnVal;
}


unsigned char*
RobotControl::SendCommand( unsigned char* command,
			   int size,
			   bool rval,
			   int delay )
{
  unsigned char* packet = new unsigned char[size + 5];

  packet[0] = 0xFA;
  packet[1] = 0xFB;
  packet[2] = size + 2; // + 2 for checksum

  for ( int i = 0 ; i < size ; i++ )
    packet[ 3+i ] = command[i];

  int checksum = calculateChecksum( packet+2 );

  packet[ size + 3 ] = checksum >> 8;
  packet[ size + 4 ] = checksum & 0xFF;

  commlink->Write( packet, size + 5 );

  usleep( delay );

  if ( rval )
    return packet;

  delete packet;
  return NULL;
}


int 
RobotControl::calculateChecksum (unsigned char* data)
{
  int n;
  int c = 0;
  n = *(data++);
  n -= 2;

  while (n > 1)
    {
      c += (*(data) << 8) | *(data+1);
      c = c & 0xffff;
      n -= 2;
      data += 2;
    }

  if (n > 0)
    {
      c = c ^ (int)*(data++);
    }

  return c;
}