// FILE: motors.cpp
// programmer: R. Murphy
// purpose:    provide an interface to Clem's drive and steering motors
//
// 8/15	changed	include files
//		initall to incorporate preconfigure
//             	EnableMotors -> MotorsEncoders_EnableMotors()
//		DisableMotors -> MotorsEncoders_DisableMotors()
//		demos.h to allow access to SteerToHome
//		joystick to just use demos.h, DriveWithJoyStick_Text
//		DRIVE to DRIVE_MOTOR, STEER to STEER_MOTOR
//		IdleMotors -> MotorsEncoders_IdleMotors
//		move to use trapezoid functions and ICONV routines
//		turn to use trapezoid functions
// 1/20/96 removed all sonar & lasernav code, changed class name to Motors

#include "comp_dep.h"

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

#include "motors.h"
#include "hardware.h"
#include "mrv4ms.h"
#include "config.h"
#include "general.h"
#include "pi.h"
#include "misc.h"

#define ICONV_UNITS(x) 	(CONFIG_user_io_imperial ? INCHES_TO_METRES((x)) : (x))
#define OCONV_UNITS(x) 	(CONFIG_user_io_imperial ? METRES_TO_INCHES((x)) : (x))
#define NAME_UNITS	  	(CONFIG_user_io_imperial ? "inches" : "m")

int Motors::robot_initialized = 0;
MotorMode Motors::drive_mode = Unknown;
MotorMode Motors::steer_mode = Unknown;

// the constructor
Motors::Motors()
{
  xCoord = 0.0;
  yCoord = 0.0;
  thetaCoord = 0.0;
  TRANSLATING = 0;		//false
  ROTATING = 0;			//false
  
} // end Motors()


// PreconfigureRobot():
//
// This routine reads the config file then uses the information to
// "preconfigure" the robot.  Note that the "config" module is completely
// separate from the hardware library.  Once the config variables have
// been read, the appropriate "Preconfigure" routines must be called
// explicitly.  Note also that some preconfigurations are "fixed" - ie:
// they are made without reference to the config file.  The user is free
// to modify this so that more (or less) of the configuration depends on
// the config file.  Note also that if some variables are not mentioned
// in the config file, the appropriate CONFIG... variables are given
// default values, which for convenience are set up in the "config.cpp"
// file to be the same as the defaults in the hardware library (but this
// need not always be the case). Note also that the user i/o may also work in
// "inches" if the CONFIG_user_io_imperial variable is TRUE.  (But, all
// internal calculations are still done in metres).

char* PreconfigureRobot()
{
  // Now reading the config file and setting up the CONFIG... variables
  char *errmsg = "";
  errmsg = ReadConfigFile(CONFIG_FILE);	// a routine from the CONFIG module
  if (*errmsg) return errmsg;
  
  
  // Now preconfiguring the LaserNav
  enum LaserNavs LaserNav_Kind = NO_LASERNAV;
  switch (CONFIG_com_port_for_lasernav) {
  case 1:
    LaserNav_Kind = SERIAL_LASERNAV_COM1;
    break;
  case 2:
    LaserNav_Kind = SERIAL_LASERNAV_COM2;
    break;
  default:
    LaserNav_Kind = NO_LASERNAV;
    break;
  }
  enum CW_ACW DirectionOfRotation = (CONFIG_lasernav_ACW ? ACW : CW);
  
  LaserNav_Preconfigure(LaserNav_Kind,
			DirectionOfRotation,
			DIRECTION_OF_ANGLES_ACW,
			RADIANS );
  
  // Now preconfiguring the MotorsEncoders
  
  MotorsEncoders_Preconfigure_RobotBaseType(CONFIG_robotbase_MRV4 ?
					    ROBOTBASE_MRV4 : ROBOTBASE_UTV200);
  
  MotorsEncoders_Preconfigure_CountsPerUnit(STEER_MOTOR, CONFIG_steer_encoder_counts_per_radian);
  MotorsEncoders_Preconfigure_CountsPerUnit(DRIVE_MOTOR, CONFIG_drive_encoder_counts_per_metre);
  
  if (CONFIG_steer_swap_direction) {
    MotorsEncoders_Preconfigure_SwapMotorDirection(STEER_MOTOR);
    MotorsEncoders_Preconfigure_SwapEncoderDirection(STEER_MOTOR);
  }
  if (CONFIG_drive_swap_direction) {
    MotorsEncoders_Preconfigure_SwapMotorDirection(DRIVE_MOTOR);
    MotorsEncoders_Preconfigure_SwapEncoderDirection(DRIVE_MOTOR);
  }
  
  MotorsEncoders_Preconfigure_Filter(STEER_MOTOR, CONFIG_steer_samp,
				     CONFIG_steer_gain,
				     CONFIG_steer_pole,
				     CONFIG_steer_zero);
  MotorsEncoders_Preconfigure_Filter(DRIVE_MOTOR, CONFIG_drive_samp,
				     CONFIG_drive_gain,
				     CONFIG_drive_pole,
				     CONFIG_drive_zero);
  
  MotorsEncoders_Preconfigure_ExternalClockRate(STEER_MOTOR,
						CONFIG_sample_clockrate);
  MotorsEncoders_Preconfigure_ExternalClockRate(DRIVE_MOTOR,
						CONFIG_sample_clockrate);
  
  return errmsg;
}


// init()
// purpose: initialize all the Denning interfaces:
//		touch, sonar, laser nav, drive and steer motors
//
// this also includes the preconfigure routine, which isn't
// public.

int Motors::init()
{
  printf("In Motors::init()\n");
  
  // *** 1. preconfigure the robot  (MANDATORY)
  // Now preconfiguring the robot before initializing
  char* errmsg = "";
  errmsg = PreconfigureRobot();	// a local routine to interpret the
  // config file and call the appropriate
  // "Preconfigure" routines
  if (*errmsg) {
    printf("\n\nInit Error - %s\n\n",errmsg);
    printf("(Hit any key to abort)\n");
    getch();
    exit(0);
  }
  
  // *** 2. display the configuration file (OPTIONAL)
  // Now displaying the configuration (according to the file)
  printf("\n\nConfiguration     (To modify edit file: %s)\n",CONFIG_FILE);
  printf("-------------\n");
  DisplayCurrentConfig();	// a routine from the CONFIG module
  printf("\n\n");
  printf("(Hit any key to continue. ESC to exit.)\n");
  if (getch()==27) exit(0);
  
  // ***3. Initialize the robot (MANDATORY)
  // Now initializing the robot (according to the preconfiguation)
  printf("\n\nInit the robot - (please wait)...\n");
  InitialiseRobot();
  robot_initialized = 1;
  
  printf("Denning interfaces are initialized. Soft enabling...\n");
  softEnable();
  printf("soft enabled.\n");

  return 0;
}

// move(float dist)
// 
// purpose: use a trapezoidal velocity profile to move a certain
//          distance (dist) in feet, then return to joystick control
//          (i.e., await further directions)
//
// dist is the distance to move in feet.
//
// move() returns when the entire trapezoidal move is complete.
// Therefore there is really no way to use it with obstacle avoidance.
//
// Note: The trapezoidal acceleration and max velocity are assumed to
//       have been set up already by calling setMoveVel() and
//       setMoveAccel().
//
// WARNING: The trapezoidal move function of the hardware (HCTL chips)
// is kind of dangerous. The trapezoid profile gets mapped into a
// profile of LOCATION over TIME, and if it gets behind for any reason
// (someone pushed the joystick while it was moving, it hit something,
// or it's going uphill and can't meet the profile schedule, for
// example) it will turn the motor on FULL until it catches up with
// it's preset profile. If it gets to it's destination it will try to
// just stop, but the momentum will be to high, so it will go full
// speed back. This can make it oscillate, which is very bad.

int Motors::move(float dist)
{
  printf("WARNING: ROBOT ABOUT TO TRANSLATE %lg FEET\n", dist/12.0);
  
  // ICONV_UNITS() wants inches, so multiply by 12 to convert
  // feet to inches. 
  
  if(drive_mode != Trapezoidal){
    MotorsEncoders_TrapezMode_Setup(DRIVE_MOTOR,
				    ICONV_UNITS(dist * 12.0),
				    ICONV_UNITS(trap.move_accel * 12.0),
				    ICONV_UNITS(trap.move_vel * 12.0));
    drive_mode = Trapezoidal;
  }
  
  MotorsEncoders_TrapezMode_Go(DRIVE_MOTOR);
  
  while (1) {
    if (MotorsEncoders_TrapezMode_IsFinished(DRIVE_MOTOR)) break;
    if (kbhit()) {
      printf("Aborting!\n");
      MotorsEncoders_IdleMotors(DRIVE_MOTOR);
      MotorsEncoders_DisableMotors();
      getch();
      break;
    }
  }
  // The robot is now in "homeostatic" mode where it will "actively"
  // try to maintain its position at the end of the move.
  
  MotorsEncoders_IdleMotors(DRIVE_MOTOR);
  // Disable "homeostatic" behaviour and return to joystick
  // control
  
  return 0;
}				//end of move(float dist)

// returns true if the latest trapezoidal turn is done.
int Motors::steer_done()
{
  return MotorsEncoders_TrapezMode_IsFinished(STEER_MOTOR);
}

//
// turn
// purpose: turn through relative angle (in radians)
//          turning is not accurate: problem with controllers
//
// NOTE: The following warning is not true right now. I commented
//       that section of the code out.
// WARNING: turn() doesn't return until it's done with the whole turn.
// Therefore, if you tell it to turn a large angle while the robot is
// driving, you won't have any feedback of any kind until the turn is
// done, even though the robot is still driving.
//
// Units: angle is in radians.

int Motors::turn(float angle)
{
  double acceleration = 50;
  double maxvelocity = 60;
//  double acceleration = 24;
//  double maxvelocity = 12;
  
  // mainly for debugging...
  if(fabs(angle) > 10.0){
    fprintf(stderr,
	    "WARNING: very large angle given to turn(): %g radians.\n",
	    angle);
    fprintf(stderr, "hit a key to continue.\n");
    getch();
  }

  maxvelocity = DEGS_TO_RADS(maxvelocity);
  acceleration = DEGS_TO_RADS(acceleration);
  if (DEBUG_MSG==DPRINT)
    {
      printf("current turn position is %f radians.\n", thetaCoord);
      printf("WARNING: fixing to turn %f radians\n", angle);
    }
  
  MotorsEncoders_TrapezMode_Setup(STEER_MOTOR,angle,acceleration,
				  maxvelocity);
  steer_mode = Trapezoidal;

  MotorsEncoders_TrapezMode_Go(STEER_MOTOR);
  
// Dave here - Experimentally removing this and adding a steer_done()
//             function, so we don't have to busy-wait for steer to
//             finish.
  while (1)
    {
      if (MotorsEncoders_TrapezMode_IsFinished(STEER_MOTOR)) break;
      if (kbhit()) {
	printf("Aborting!\n");
	MotorsEncoders_IdleMotors(STEER_MOTOR);

	// Disabling them seems a bit excessive, doesn't it?
	//	MotorsEncoders_DisableMotors();

	// Removing this getch() allows other functions to see
	// the character also.
	//	getch();
	break;
      }
    }
  
//  MotorsEncoders_IdleMotors(STEER_MOTOR); //return to ready state
  
  if (DEBUG_MSG==DPRINT)
    printf("current turn position is %f radians.\n", thetaCoord);
  
  return 0;
}				// end turn(float angle)

//
// turnAbs
// purpose: turn to the absolute angle (relative to home)
//
// WARNING: turnAbs() doesn't return until it's done with the whole turn.
// Therefore, if you tell it to turn a large angle while the robot is
// driving, you won't have any feedback of any kind until the turn is
// done, even though the robot is still driving.
//
// Units: angle is in radians.

int Motors::turnAbs(float angle)
{
  if (DEBUG_MSG==DPRINT)
    printf("angle to turnAbs: %f\n", angle);
  
  //compute the relative angle in radians
  angle -= thetaCoord;
  
  //put angle between -pi and pi so that it choses the shortest turn
  angle = FixAngle(angle);
  
  // just do it-- turn will update thetaCoord BTW
  return (turn(angle));
}				//end turnAbs(float angle)

//
// softEnable
// purpose: software enable of the steer and drive motors.  Equivalent
//				of hitting the "motor reset" switch
//
void Motors::softEnable()
{
  int status = 0;
  
  printf("Enabling drive and steer motors; this will take about 10 sec\n");
  status = MotorsEncoders_EnableMotors();
  
  if (status)
    {
      printf("Could not enable motors, please retry.   (Hit return)\n");
      getch();
      MotorsEncoders_DisableMotors();
      return;
    }

  drive_mode = Unknown;
  steer_mode = Unknown;
}				//end of softEnable

//
// drive
// purpose: to drive the robot at a constant velocity
//
// speed - speed in feet per second the robot should move.
//
int Motors::drive(float speed)
{
  speed *= 12.0; /* convert from feet/sec to inches/sec, which
		    is what ICONV_UNITS() wants. */
  speed = ICONV_UNITS(speed);
    
  if(drive_mode != Proportional){
    MotorsEncoders_PropMode_Setup(DRIVE_MOTOR);
    drive_mode = Proportional;
  }

  MotorsEncoders_PropMode_NewSpeed(DRIVE_MOTOR, speed);

  return 0;
}				//end drive(float vel, float accel)

// stopdrive(): added for compatibility with sun driver code.
//              Seems like it may be kind of abrupt.

int Motors::stopdrive()
{
  if(drive_mode != Proportional){
    MotorsEncoders_PropMode_Setup(DRIVE_MOTOR);
    drive_mode = Proportional;
  }
  MotorsEncoders_PropMode_NewSpeed(DRIVE_MOTOR, 0);
  readXY();
  return 0;
}

// stopsteer(): added for compatibility with sun driver code.
//              Seems like it may be kind of abrupt.

int Motors::stopsteer()
{
  if(steer_mode != Proportional){
    MotorsEncoders_PropMode_Setup(STEER_MOTOR);
    steer_mode = Proportional;
  }
  MotorsEncoders_PropMode_NewSpeed(STEER_MOTOR, 0);
  readXY();
  return 0;
}

int Motors::stop()
{
  stopdrive();
  stopsteer();
  
  return 0;
}

// stopdriveSlow() and stopsteerSlow() may be hacks. They just let the
//          motor coast to a stop, which may not be exactly the
//          intent. OK for now?

int Motors::stopdriveSlow(void)
{
  MotorsEncoders_IdleMotors(DRIVE_MOTOR);
  
  return 0;
}

int Motors::stopsteerSlow(void)
{
  MotorsEncoders_IdleMotors(STEER_MOTOR);
  
  return 0;
}

int Motors::stopSlow(void)
{
  MotorsEncoders_IdleMotors(DRIVE_MOTOR);
  MotorsEncoders_IdleMotors(STEER_MOTOR);
  
  return 0;
  
}

int Motors::halt(void)
{
  if(robot_initialized){
    FinaliseRobot();
    robot_initialized = 0;
  }
  return 0;
}

int Motors::wait_for_drive_to_stop(void)
{
  while (!MotorsEncoders_TrapezMode_IsFinished(DRIVE_MOTOR))
    shdel(100);			//otherwise it signals done on the decel side
  
  //so we need to force it to stop. Used to just delay a second in
  //case of coasting, but I don't like delays. Possible oscillations?
  stopdrive();
  
  return 0;
}				// wait_for_drive_to_stop(void)

int Motors::wait_for_steer_to_stop(void)
{
  while (!MotorsEncoders_TrapezMode_IsFinished(STEER_MOTOR))
    shdel(100);			//otherwise it signals done on the decel side
  
  //so we need to force it to stop. Used to just delay a second in
  //case of coasting, but I don't like delays. Possible oscillations?
  stopsteer();
  
  return 0;
}				// wait_for_steer_to_stop(void)


// Steers the wheels clockwise until they detect "home".
// Returns 0 on success, returns -1 if home sensor was not detected
// We will also stop the steer-motor from turning after it has done about 400
// degrees, because, in this case, the "home" sensor would not have been found.
// This is achieved by setting a trapezoidal-profile move which will stop at
// 400 degrees- unless it is aborted by a call to MotorsEncoders_IdleMotors();

int Motors::SteerToHome()
{
  int bad=0;
  MotorsEncoders_IdleMotors(STEER_MOTOR);
  MotorsEncoders_IdleMotors(DRIVE_MOTOR);
  
  unsigned char initial_home_counter = SteeringHomePosition_ReadCounter();
  
  // First a small rotation in the negative direction just in case the wheels
  // are already at the home position.
  MotorsEncoders_TrapezMode_Setup(STEER_MOTOR, DEGS_TO_RADS(-5),
				    DEGS_TO_RADS(50),
				    DEGS_TO_RADS(100));
  steer_mode = Trapezoidal;

  MotorsEncoders_TrapezMode_Go(STEER_MOTOR);
  while (!MotorsEncoders_TrapezMode_IsFinished(STEER_MOTOR));
  
  // Now we rotate the wheels whilst looking for the home sensor.
  MotorsEncoders_TrapezMode_Setup(STEER_MOTOR, DEGS_TO_RADS(500),
				  DEGS_TO_RADS(50),
				  DEGS_TO_RADS(100));
  MotorsEncoders_TrapezMode_Go(STEER_MOTOR);
  
  while (SteeringHomePosition_ReadCounter() == initial_home_counter) {
    if (MotorsEncoders_TrapezMode_IsFinished(STEER_MOTOR)) {bad=-1; break;}
  }
  
  MotorsEncoders_IdleMotors(STEER_MOTOR);
  
  return bad;
}

//
// home
// purpose: have the robot turn to home
// 		   0 means that it found the home switch
//		  -1 means that is was unsuccessful
//          It also calls sethome() for sun-code compatibility
int Motors::home()
{
  printf("WARNING: ROBOT ABOUT TO ROTATE TO HOME POSITION\n");
  if (SteerToHome()==0)
    {
      sethome();			//this is the (0,0,0) location!
      if (DEBUG_MSG==DPRINT)
	printf("Home is set at:x=%f, y=%f, theta=%f radians\n",
	       xCoord, yCoord, thetaCoord);
      sethome();
      return 0;
    }
  else
    return -1;
}

int Motors::sethome()
{
  setxy(0, 0, 0);
  return 0;
}


//
// turn180
//
int Motors::turn180(void)
{
  double acceleration = 50;
  double maxvelocity = 60;
  
  double radians;
  
  
  radians = DEGS_TO_RADS(200.00); //fudge for undershooting
  maxvelocity = DEGS_TO_RADS(maxvelocity);
  acceleration = DEGS_TO_RADS(acceleration);
  
  //turn the robot
  if(steer_mode != Trapezoidal){
    MotorsEncoders_TrapezMode_Setup(STEER_MOTOR,radians,acceleration,
				    maxvelocity);
    steer_mode = Trapezoidal;
  }
  MotorsEncoders_TrapezMode_Go(STEER_MOTOR);
  
  while (1)
    {
      if (MotorsEncoders_TrapezMode_IsFinished(STEER_MOTOR)) break;
      if (kbhit()) {
	printf("Aborting!\n");
	MotorsEncoders_IdleMotors(STEER_MOTOR);
	MotorsEncoders_DisableMotors();
	getch();
	break;
      }
    }
  
  MotorsEncoders_IdleMotors(STEER_MOTOR); //return to ready state
  
  /*	//update the value of thetaCoord-- it's always positive
	thetaCoord += angle;
	while (thetaCoord >= 360)
	thetaCoord -=360;
	while (thetaCoord <0)
	thetaCoord +=360;
	
	if (DEBUG_MSG==DPRINT)
	printf("current turn position is %f radians.\n", thetaCoord);
	
	thetaCoord = thetaCoord;	//is this correct?
	*/
  return 0;
  
}				//end of turn180


//
// readXY
// purpose: to provide the current x,y,theta values as the
//          robot moves in the world.
// Units: stores x and y values in feet and theta in radians.

int Motors::readXY(void)
{
  // Make these static to save stack space since this may
  // be called from a timer interrupt.
  static double Direction_Radians;
  static double Distance_Feet;
  
  MotorsEncoders_ReadEncodersRelative(&Distance_Feet, &Direction_Radians);
  
  // Since ...ReadEncodersRelative() above returns radians and meters,
  // we must convert meters to feet.
  Distance_Feet = METRES_TO_INCHES(Distance_Feet) / 12.0;
  
  // Now we update the robots (x,y,theta) position using the values of
  // change in direction and distance.
  
  thetaCoord = FixAngle2(thetaCoord + Direction_Radians);
  
  xCoord += Distance_Feet * cos(thetaCoord);
  yCoord += Distance_Feet * sin(thetaCoord);
  
  return 0;
}

//
//getxy(float *x, float *y, float *theta)
//
// purpose: get the current coordinates of the robot from memory
// units: x and y are in feet, theta is in radians.

int Motors::getxy(float *x, float *y, float *theta)
{
  static int status;
  
  status = readXY();
  *x = xCoord;
  *y = yCoord;
  *theta = thetaCoord;
  
  return(status);
}

//
// setxy
// purpose:  set the current position & angle to be (x, y, t). All new
//           getxy() position reads will assume that the physical
//           pos'n of the robot at *this* time was (x, y, t).
// units: x and y in feet, t (theta) in radians.

int Motors::setxy(float x, float y, float t)
     //
{
  readXY(); // initialize the shaft encoders
  
  thetaCoord = t;
  xCoord = x;
  yCoord = y;
  
  if (DEBUG_MSG==DPRINT)
    printf("Soft home is=> x: %f y: %f theta: %f radians\n",
	   xCoord, yCoord, thetaCoord);
  
  return 0;
}