/*
 * survivor.cc
 *
 * Daniel Lowd
 * CS154: Robotics
 */

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

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

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


/******************************************************************
 *
 * This function computes a rotational velocity in a purely reactive
 * fashion, using just the information in the State vector.
 *
 ******************************************************************/


int 
computeRotationalVelocity()
{
  int SONAR_THRESHOLD = 24;  // 2.0 feet
    
  // Check sensors
  int frontRange = State[getShortestSonarIndex(-2,2)];
  int leftRange = State[getShortestSonarIndex(3,2)];
  int rightRange = State[getShortestSonarIndex(-3,-2)];

  // Turn left if we're about to hit a wall
  if (frontRange < SONAR_THRESHOLD)
  {
      return 150;
  }

  // Go wherever there's room.
  // The gain of 3.0 seems to work well enough in wallfollow_map_1
  return 3.0*(leftRange - rightRange);
}


/****************************
 *
 *  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 = 50;             /* 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: 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        */

    /* Slow down if we're in danger of hitting something.  Otherwise,
     * go fast.
     */
    tv = 1.5*State[getShortestSonarIndex(-2,2)];    

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