/*
 * survivor.cc
 *
 * Daniel Lowd
 * CS154: Robotics
 * Spring 2003
 *
 * Wall-following for the Nomad200.  Goes forward until it finds a wall,
 * then follows it, always keeping the wall a specified distance towards
 * its right.
 */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <stream.h>
#include "KbdInput.hh"

extern "C" {
#include "Nclient.h"
}

#define MAX_RV 450          // 45 deg/sec
#define MAX_TV 200          // 20 in/sec
#define WALL_SPACE 18      // 1.5 feet

/******************************************************************
 *
 * This function gets the shortest sonar reading in a range from
 *
 *   fromSonar to toSonar
 *
 * The index of that minimum reading is returned.
 *
 * Because I think of the sonars as being numbered starting from
 * zero, there is a conversion to the actual location of the 
 * sonar values in the State vector.
 *
 * Note: though input values may be anything bigger than -32, the
 *       index returned is the actual indes into the state vector,
 *       i.e., between 17 and 32 inclusive.
 *
 ******************************************************************/

int
getShortestSonarIndex(int fromSonar, int toSonar)
{
  int realSN;              /* real sonar number in the State vector */
  int minSonar = 1000;     /* higher than any real sonar value      */
  int minSonarIndex;    

    /* swap from and to, if they're not in order */

    /* I apologize for this especially esoteric code to swap two values:
     *    x ^= y ^= x ^= y;
     */

  if (fromSonar > toSonar) { fromSonar ^= toSonar ^= fromSonar ^= toSonar; }

  for (int i=fromSonar ; i<toSonar ; ++i) 
  {
    realSN = (i + 32)%16;  /* that way specifying negative sonar #s is OK */
    realSN = realSN+17;    /* add the offset into the State vector        */

    if (State[realSN] < minSonar)  /* ties go to the previous index */
    {
      minSonar = State[realSN];
      minSonarIndex = realSN;      
    }

  }
  return minSonarIndex;
}

/*
 * Code to keep track of past five errors and compute integrals
 * and derivatives.
 */

// Keep track of past errors...
#define INVALID 1000
static int past_errors[5] = {INVALID,INVALID,INVALID,INVALID,INVALID};
static int error_index = 0;

// Reset all errors to INVALID
void clear_past()
{
    for (int i = 0; i < 5; i++)
        past_errors[i] = INVALID;
}

// Enter a new error into the history
void add_error(int new_error)
{
    error_index = (error_index + 1) % 5;
    past_errors[error_index] = new_error;
}

// Compute the derivative of the errors
int get_deriv()
{
    int i;

    // Find the oldest valid error that's not the present one.
    for (i = 1; i < 5; i++)
    {
        if (past_errors[(i + error_index) % 5] != INVALID)
            break;
    }

    // If all other errors are invalid, assume the derivative is zero.
    if (i == 5)
        return 0;
    
    return (past_errors[error_index] - past_errors[(i + error_index) % 5]);
}

// We don't actually use the integral for anything, so why compute it?
#if 0
int get_integ()
{
    int total_error = 0;
    int valid_errors = 0;

    for (int i = 0; i < 5; i++)
    {
        if (past_errors[i] < INVALID)
        {
            total_error += past_errors[i];
            valid_errors++;
        }
    }

    return total_error/valid_errors;
}
#endif



int 
computeRotationalVelocity()
{
  double rv;

  // Keep track of whether or not we're in wall-seeking mode.
  static bool found_a_wall = false;

  // Start turning a bit before we're too close to the wall in front of us.
  int FRONT_THRESHOLD = 1.333*WALL_SPACE; 
                           
    
  int frontRange = State[getShortestSonarIndex(-2,2)];

  // Wall-avoidance behavior
  if (frontRange < FRONT_THRESHOLD)
  {
      rv = MAX_RV;
      found_a_wall = true;

      // Reset error histories, since this turning introduces inconsistencies.
      clear_past();
  }

  // Wall-seeking behavior: go straight until we find a wall
  else if (!found_a_wall)
  {
      rv = 0;
  }
  else
  {
      // Proportional and derivative gains.
      int P_GAIN = 30;
      int D_GAIN = 80;
      // int I_GAIN = 0.0;  // Not used!

      int rightIndex = getShortestSonarIndex(-2, -6);
      int rightDist = State[rightIndex];

      int error = WALL_SPACE - rightDist;

      /* If the wall is behind us, we're going away from it, so we should
       * turn to be closer.  If the wall is ahead of us, the reverse is true.
       */
      error *= 1.0 + (rightIndex+4.0)/5.0;

      // Provide some bounds on the error.
      if (error < -WALL_SPACE)
          error = -WALL_SPACE;

      add_error(error);
      rv = P_GAIN * error + D_GAIN * get_deriv(); // + I_GAIN * get_integ();

      if (rv > MAX_RV)
          rv = MAX_RV;
      else if (rv < -MAX_RV)
          rv = -MAX_RV;

      printf("rd: %d; err: %d; rv: %d\n\r", rightDist, error, (int)rv);
  }

  /* Enforce bounds on rotational velocity */
  if (rv > MAX_RV)
      rv = MAX_RV;
  else if (rv < -MAX_RV)
      rv = -MAX_RV;

  return rv;
}


/****************************
 *
 *  The usual main method
 *
 ****************************/

int
main(int argc, char** argv)
{

  strcpy(SERVER_MACHINE_NAME, "localhost");
  SERV_TCP_PORT = 7654;    /* this is Daniel's tcp port       */

  /*
   * tv is the constant translational velocity of the robot
   */

  int tv = MAX_TV; 

  if (argc > 1)            /* a different velocity can be sent in */
  {
	tv = atoi(argv[1]);
      cout << "Setting translational velocity to " << tv << endl;
  }

  /* 
   * connect to the simulator
   */

  if (connect_robot(1)) 
	printf("Connected to robot\n");
  else {
	printf("Failed to connect to robot\n");
	exit(0);
  }

  /* 
   * zero the robot
   */

  zr();

  /* 
   * turn on appropriate sensors: all sonar
   */

  for (int i=17 ; i<=32 ; ++i)  Smask[i] = 1;
  ct();

  gs();                     /* update the robot's state   */

  int rv = 0;               /* start going straight ahead */

  vm(tv,rv,rv);             /* set the initial velocities */

  /*
   * the KbdInput object allows nonblocking input
   */

  KbdInput KI; 
  char c; 

  /*
   * the sense/act loop implementing reactive control
   */

  KI.On();                  /* turn on raw input/output             */


  int totalTV = 0;
  int numSteps = 0;
  while (1) 
  {
    c = KI.nextChar();      /* c is 0 if there's no input ready     */
  
    if ( c == 'q' )         /* hit q in the terminal window to quit */
    {
       break;
    }

    rv = computeRotationalVelocity();        /* get velocity        */

    int frontRange = State[getShortestSonarIndex(-2,2)];

    // Don't go forward if we're too close already!
    if (frontRange < WALL_SPACE)
        tv = 0;
    else
        tv = MAX_TV;

    // Keep track of average speed, to satisfy curiosity.
    totalTV += tv;
    numSteps++;

    vm(tv,rv,rv);                            /* set velocity        */
    gs();                                    /* update state        */
  }

  KI.Off();                 /* reset terminal settings              */

  printf("Average tv: %d\n", totalTV/numSteps);

  disconnect_robot(1);      /* gracefully disconnect from the robot */
}