/*
 * Assignment #A, CS154 Spring 2001
 * Name         :   SooYoung Jung
 * Date         :   Feb, 5, 2001
 * File         :   survivor.cc
 * 
 */

/*
 * survivor.cc
 *
 * This is a skeleton file for controlling the Nomad
 *   simulator in Assignment A on various reactive control strategies.
 *
 * Feel free to use the makefile in this directory also.
 *
 */

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

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

const int RIGHT     = 12;
const int SONAR     = 16;

int 
computeRotationalVelocity()
{

    double  rv      = 0.0;          // The value that I compute.

    int     lastRV  = State[39];    // Last rotational speed.

    /*
     * The distance from the wall made by speed.
     * The speed, lower than 100 will get 50 or 50% of its speed.
     */ 
    double  turingPoint = (State[38]  < 100) ? 50 : State[38] * 0.5; 

    /*
     * Let me assume that default rotational value 
     * and set it up as turingPoint.
     */
    double defaultRV = turingPoint;


    /*
     * Add up the value for each sides sensors.  These values are used to 
     * determine which side has more room for turning.
     */
    
    int I_leftAdd  = 0;             // Sum of left side infrared sensors, 
                                    // which is from 1 to 5
    int I_rightAdd = 0;             // Sum of right side infrared sensors, 
                                    // which is from 13 to 16, and 1
    
    int S_leftAdd  = 0;             // Sum of left side Sonar sensors, 
                                    // which is from 17 to 21
    int S_rightAdd = 0;             // Sum of right side Sonar sensors, 
                                    // which is from 29 to 32, and 17
    
    for (int i = 1; i < 6; i++)
    {
        int sonarI = i + SONAR;     // Getting Sonar index number.

        /* infrared add up */
        I_leftAdd  += State[i];     
        I_rightAdd += State[(i + RIGHT) == 17 ? 1 : (i + RIGHT)];
        
        /* Sonar add up */
        S_leftAdd  += State[sonarI];
        S_rightAdd += State[(sonarI + RIGHT)== 33 ? 17 : (sonarI + RIGHT)];
    }

    /*
     * Checking whether the robot is seeing the edge or not.
     * When the robot sees the edge, only one or two infrared sensor will 
     * dramatically go down compared to others.
     * In this case, addup values will not make the robot turn so make the 
     * turn when it sees the edge.
     */

    int edge = 0;                   // Whether the sensors is seeing edge
    int edgeIndex;                  // Which sensor is seeing edge?
    bool edgeFlag = false;          // Seeing edge now?

    for (int i = 1; i < 17; i++)
    {
        if (State[i] < 10)          // How many sensors is seeing edge?
        {
            edge++;
            edgeIndex = i;
            edgeFlag = true;
        }

        /*
         * More than two will not be the edge. not edge.. forget or
         * sensors addup value can detect this.
         */
        if (edge > 2)
        {
            edgeFlag = false;
            break;
        }
    }

    /*
     * If the robot is close enough to turn, then turn!
     * turingPoint here is just made up by me and is determined by speed.
     *
     * When it is time, compare one left and right sensors, and make it turn 
     * to the side whichever sees more.
     */
    if (State[17] < turingPoint)
    {
        rv = defaultRV;             // Let's set it up to turn left first.
        
        /*
         * If there more room on right side, let's turn the other side!
         */
        if (State[18] < turingPoint && State[32] > turingPoint)
        {
            rv *=  -1;              // Other direction.
        }
    }

    /*
     * Checking whether the robot is seeing the edge or not.
     */
    if (edgeFlag)
    {
        if (edgeIndex < 9)          // edge at left... avoid!!!
        {
            rv = lastRV - defaultRV;
        } else
        {
            rv = lastRV + defaultRV;
        }
    }
    /*
     * even though it passed the edge test, this might be the edge if 
     * sensor of 5(left) and 13(right) is 0.
     */
    else if (State[5] == 0)         // edge at left... turn right
    {
        rv = lastRV - defaultRV;
        edgeFlag = true;
    }
    else if (State[13] == 0)        // edge at right .. turn left
    {
        rv = lastRV + defaultRV;
        edgeFlag = true;
    }

    /*
     * Gotta keep turning if we found the edge.
     */
    if (edgeFlag)
    {
        return (int)rv;
    }

    /*
     * mmmmmmmm.. Too close to the block if we see something from the front 
     * infrared sensor.
     */
    if (State[1] < 10 )
    {
        if (I_leftAdd > I_rightAdd)
        {
            rv = lastRV + defaultRV;
        } else
        {
            rv = lastRV - defaultRV;
        }
        
        return (int)rv;
    }
    
    /*
     * If we see blocks too much, then make the robot turn to the side which 
     * has more room to turn.
     */
    else if (I_leftAdd < 55 || I_rightAdd < 55)
    {
        if (I_leftAdd > I_rightAdd)
        {
            rv = lastRV + defaultRV;
        } else
        {
            rv = lastRV - defaultRV;
        }
    }

    /*
     * Or if difference of two addup values is greater than 15, 
     * one side is getting a lot more blocks.
     * Be ready to avoid the block so turn.
     */
    else if ( abs(I_leftAdd - I_rightAdd) > 15 )
    {
        if (I_leftAdd >= I_rightAdd)
        {
            rv = lastRV + defaultRV;
        } else
        {
            rv = lastRV - defaultRV;
        }
    }


    /*
     * If the front sonar sees really a lot but not infrared sensor,
     * This means the robot is seeing the corner of the block and very close to 
     * the block at the right or left.
     */

    if (State[17] == 255 && State[1] != 15)
    {
        if ( I_leftAdd >= I_rightAdd && lastRV > 0)
        {
            rv = lastRV + defaultRV;
        } else
        {
            rv = lastRV - defaultRV;
        }
    }

    return (int)rv;
}


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

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

  SERV_TCP_PORT = 7098; // this is the default 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");
    printf("Hit q to exit.\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;
  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        */

    if (rv > 450)
    {
        rv = 450;
    } 
    else if (rv < -450)
    {
        rv = -450;
    }

    vm(tv,rv,rv);                            /* set velocity        */

    gs();                                    /* update state        */
  }


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

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

}