#ifndef ROBOT_CONTROL_HH
#define ROBOT_CONTROL_HH

/*

AngleConvFactor 0.0061359        // radians per encoder count diff (2PI/1024)
DistConvFactor  0.07979          // 5in*PI / 5000 (mm/count)
VelConvFactor 3.9898             // mm/sec per encoder count per 1/50 sec
RobotRadius 220.0                // radius in mm
RobotDiagonal 90.0               // half-height to diagonal of octagon
Holonomic 1                      // turns on its own radius
RangeConvFactor 0.1734           // sonar range mm per 2 usec tick

*/

#define VEL_CONV 0.01

#define GRIPPER_UP       1
#define GRIPPER_CLOSE    4
#define GRIPPER_DOWN     5

#define TOP_SWITCH	0
#define CARRY_SWITCH	1
#define OPEN_SWITCH	2
#define IN_BREAK_BEAM	3
#define OUT_BREAK_BEAM	4
#define BUMP_SWITCH	5
#define PUSH_SWITCH	6
#define SLIDE_SWITCH	7


#ifndef SUCCESS
#define SUCCESS 1
#endif

#ifndef COMMAND_DELAY
#define COMMAND_DELAY 100000	/* microseconds */
#endif

#ifndef MAX_PACKET_SIZE
#define MAX_PACKET_SIZE 201
#endif

#ifndef NUM_SONAR_POSITION
#define NUM_SONAR_POSITION 20
#endif


#include <stdio.h>
#include <unistd.h>
#include <iostream.h>

#include "Sonar.hh"
#include "Channel.hh"
#include "Semaphore.hh"

class RobotControl
{
public:
  // Default contructor:
  RobotControl( Channel<unsigned char*>* comm, bool init = true );
  ~RobotControl();

  /* Open and close the connection to the robot */
  int Initialize();
  int Close();
  
  /* Set the robot's current position to be (0,0,0) */
  int SetOrigin();

  /* Tell the robot that we're still alive */
  int Pulse();

  /* Translational velocity commands */
  int TranslationalVelocity( int mmSec );
  int TranslationalVelocity( int left_mmSec, 
			     int right_mmSec );

  int DigitalOut( unsigned char* sequence, int size );
  int SonarPollingSequence( unsigned char* sequence, int size );

  /* Rotational commands */
  int AbsoluteHeading (int degrees);
  int RelativeHeading (int degrees);
  int RotationalVelocity (int degreesSec);
  
  /* Gripper control commands */
  int GripperState (int state);

  /* Thread method that parses Server Information Packets from the robot
   * and updates the appropriate state variables
   */
  static void* UpdateState(void* arg);

  /* State variable retrieval methods */

  unsigned char MotorStatus ();
  int XPos ();
  int YPos ();
  float Theta ();
  int LeftWheelVelocity();
  int RightWheelVelocity();
  float BatteryCharge();
  bool LeftWheelStall();
  bool RightWheelStall();
  double SonarReading( int sonarNumber );
  bool DigitalValue( int request );

protected:
  // Default send command. Packages command packet with pioneer header
  // and tacks on default command delay
  unsigned char* SendCommand( unsigned char* command,
			      int size,
			      bool rval,
			      int delay = COMMAND_DELAY );

  // Calculate the packet checksum according to the pioneer 1 transfer
  // protocol. Code taken from the pioneer manual.
  int calculateChecksum( unsigned char* data );

  Channel<unsigned char*>* commlink;

  unsigned int motorStatus;		// Motor stop, stalled, running...
  unsigned int xPos;			// mm
  unsigned int yPos;			// mm
  int theta;				// degrees
  int leftWheelVel;			// mm/sec
  int rightWheelVel;			// mm/sec
  unsigned int batteryCharge;		// 10ths of a volt
  char leftWheelStall;			
  char rightWheelStall;			

  char left_bumber, right_bumper;	// 
  char digital_in, digital_out;		// Digital IO Bits
  char analog_in;			// 

  Sonar *sonar;				// Sonar information

  Semaphore* stateLock;			// State variable lock
};

#endif /* ROBOT_CONTROL_HH */