// motors.h
// programmer: R. Murphy
// purpose:    all the interfaces to the Denning code
//
// 		8/21/95 turn180 hack for IJCAI
// Modifier: Dave Hershberger
//    Took out sonar and lasernav code to make it just a motor interface.

#ifndef MOTORS_H
#define MOTORS_H

// DEBUG MESSAGES
#define DPRINT 1
#define DONTPRINT 0
#define DEBUG_MSG DONTPRINT

#include "pi.h"

// SIGNALS
#define STOPPED 0
#define STILL_GOING 1

// CONTROL PARAMETER DEFINITIONS

// these are our limits versus the actual physical limits
// in the Denning code
#define MOVE_VEL 	.3
#define MAX_MOVE_VEL    4.0		// max move velocity in feet per sec
#define MIN_MOVE_VEL    0.05		// min move velocity in feet per sec 

#define MOVE_ACCEL 	1               // accelerations in feet/sec/sec
#define MAX_MOVE_ACCEL  4.1		// maximum allowed accel for move
#define MIN_MOVE_ACCEL  1		// general purpose acceleration

#define TURN_VEL 	55
#define MAX_TURN_VEL    55		// max turn velocity in degrees per sec
#define MIN_TURN_VEL    10		// min turn velocity in degrees per sec

#define TURN_ACCEL 	500
#define MAX_TURN_ACCEL  500		// maximum allowed accel for turn 
#define MIN_TURN_ACCEL  4		// minimum allowed accel for turn 

struct trap_move{    // Units:
  float move_vel;    //   Feet/Sec
  float move_accel;  //   Feet/Sec/Sec
  float turn_vel;    //   Degrees/Sec
  float turn_accel;  //   Degrees/Sec/Sec
};

enum MotorMode {Proportional, Trapezoidal, Unknown};

class Motors
{
 private:
  static MotorMode drive_mode; // Proportional or Trapezoidal
  static MotorMode steer_mode;

  double xCoord;	//the current robot x (in feet) relative to
			//starting point
  double yCoord;	//the current robot y (in feet) relative to
			//starting point 
  double thetaCoord;	//the current robot theta (in radians)
			//relative to starting point
  double totalDist;	//holding value for shaft encoder in inches
  double totalDir;	//holding value for shaft encoder in radians
  int  TRANSLATING;   //boolean for whether the robot has moved since last time
  int  ROTATING;	//boolean for whether the robot has turned
  trap_move trap;       // parameters for a trapezoidal move
  static int robot_initialized; /* To keep from finalising the
				   robot before we initialise it. :) */
  int SteerToHome();
public:
  Motors();
  int init(); /* initialize the motors. This must be done before any
		 other "Motors" methods are called. */
  int halt(); /* finalize the motors. Should be done before program
		 exits. */

  void softEnable(); /* software equivalent of hitting the
			motor reset switch */
  int move(float dist);	 // do a trap move a fixed distance (in feet)
  int steer_done(); // returns true if the latest turn() is done.
  int turn(float angle); //turn relative to current position (in radians)
  int turnAbs(float angle); //turn to that absolute angle (in radians)
  int turn180();

  int drive(float speed); // drive at "speed" feet/second

  int stopdrive();
  int stopsteer();
  int stop();
  int stopdriveSlow();          // coast drive motor to a stop.
  int stopsteerSlow();          // coast steer motor to a stop.
  int stopSlow(void);		// coast to a stop for both motors
  int wait_for_drive_to_stop();
  int wait_for_steer_to_stop();
  int home(); //turn robot to hardware home position
  
  int getxy(float *x, float *y, float *theta); //read *Coord variables
  int readXY(void); //read the shaft encoders and update *Coord variables
  int setxy(float x=0.0, float y=0.0, float t=0.0); //set *Coord values
  int sethome(); // same as setxy(0, 0, 0);
};

#endif // MOTORS_H