/*
 * survivor.cc
 *
 * Andrew McDonnell and Patrick Vinograd
 *
 */

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

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

/******************************************************************
 * 
 * These two functions return a scaled readout from the left and right
 * sides of the sonar ring on the robot.  They are used by 
 * computeRotationalVelocity().
 *
 ******************************************************************/


double loss = 5.0;
double expt = 6.0;

int gain = 2.7;
int threshold = 40;


int getRightVal() {
  int scales[] = {1,1,1,2};
  int accum = 0;
  int reading = 0;
  for (int i = 28; i < 32; i++) {
    reading = State[i];
    if (reading < threshold)
      reading *= -loss;
    accum += (255 - reading) * scales[i-28];
  }
  accum /= loss;
  return accum;
}

int getLeftVal() {
  int scales[] = {2,1,1,1};
  int accum = 0;
  int reading = 0;
  for (int i = 19; i < 23; i++) {
    reading = State[i];
    if (reading < threshold)
      reading *= -loss;
    accum += (255 - reading) * scales[i-19];
  }
  accum /= loss;
  return accum;
}

/****************************************************
 * 
 * These three functions process the readings from the front 
 * for cRV()
 *
 ***************************************************/

int getFrontVal() {
  int scales[] = {1,2,5,2,1};
  int accum = 0;
  for (int i = 17; i < 19; i++) {
    accum += (255 - State[i]) * scales[i-15];
  }
  for (int i = 32; i < 33; i++){
    accum += (255 - State[32]) * scales[i-31];
  }
  accum /= pow(loss,expt);
  return accum;
}

int getLFVal() {
  int reading = State[18];
  if (reading < threshold)
    reading *= -loss;
  return (255 - reading);
}

int getRFVal() {
  int reading = State[32];
  if (reading < threshold)
    reading *= -loss;
  return (255 - reading);
}

/******************************************************************
 *
 * This function computes a rotational velocity in a purely reactive
 * fashion, using just the information provided by the getRightVal()
 * and getLeftVal() functions.
 *
 ******************************************************************/

int computeRotationalVelocity() {
  const int D_RIGHT = 1;
  const int D_LEFT = 2;
  int direction = 0;
  int zeroBarrier = 200;
  int rv;
  int left = getLeftVal();
  int right = getRightVal();
  int front = getFrontVal();
  int leftFront = getLFVal();
  int rightFront = getRFVal();
  if (left > right && (direction == D_LEFT || 
			   abs(left - right) > threshold)) {
    rv = -1 * gain * (left + front);
    direction = D_LEFT;
  }
  else if(right > left && (direction == D_RIGHT || 
			   abs(left - right) > threshold)) {
    rv = 1 * gain * (right + front);
    direction = D_RIGHT;
  }
  if (front > zeroBarrier) {
    if ((rightFront) > (leftFront)) {  // rv > 0
      rv += 1000;
    }
    else {
      rv -= 1000;
    }
  }
  if (rv > 450)
    rv = 450;
  else if (rv < -450) 
    rv = -450;

  return rv;
}

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

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

  SERV_TCP_PORT = 20120;    /* this is the default tcp port       */

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

  int tv = 300;             /* five inches per second default      */

  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 
   */

  for (int i=17 ; i<=32 ; ++i)  Smask[i] = 1;
  Smask[39] = 1;            /* able to read current rotational velocity */
  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             */


  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        */
    
    vm(tv,rv,rv);                            /* set velocity        */

    gs();                                    /* update state        */
  }


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

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

}