/*
 * Assignment #B, CS154 Spring 2001
 * Name         :   SooYoung Jung
 * Date         :   Feb, 25, 2001
 * File         :   bug2.cc
 * 
 */

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

/*
 * class Point to help to remember the position.
 */
class Point
{
public:
    Point():x(0), y(0){};
    Point(int x_, int y_): x(x_), y(y_) {};
    ~Point() {};
    int getX() const { return x; };
    int getY() const { return y; };
    
    void operator=(Point what)
	{
	    x = what.x;
	    y = what.y;
	}
    
    bool operator==(Point right)
	{
	    return (x == right.x && y == right.y);
	}
    
private:
    int x;
    int y;
};

const 	int   LEFT     	  =  1;
const 	int   RIGHT 	  = -1;
const   int   TENTH   	  = 10; 	// need to make the angle to 10th..
const   int   RIGHT_ANGLE = 900;
const   float PI      	  = 3.14159265;

const   int   LESS_CLOSE  = 100;	// Distance to obstacle
const   int   CLOSE       = 50;
const   int   VERY_CLOSE  = 25;
const   int   MINIMUM     = 5;

const   int   ADD_UP_MAX  = 75;		// Maximum value of one quater sensers 
                                    // addUp.

double  lengthIs(Point, Point);
double  angleIs(Point, Point, bool);

void 	turnTo(int, int);
bool 	turing(int);
int 	addUp(int, int);


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

/****************************
 *
 *  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 = 200;             /* 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;
  }
  
  /*
   * Getting destination point.
   */
  
  Point start(0, 0);
  Point end(2750, 4200);
  
  if (argc > 3)
  {
      end = Point(atoi(argv[2]), atoi(argv[3]));
      
      cout << "Setting end point to (" << argv[2] << ", " 
	   << argv[3] << ")" << 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();
  
  sp(200, 450, 450);
  
  /* 
   * 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             */
  
  int firstAngle = angleIs(start, end, true);
  
  int angle;						// angle to the goal point
  int currentAngle;					// temoprary storage to store current 
  									// angle.
  
  int tempAngle;					// This is the angle to make the robot 
                                    // right angle with obstacle.
  
  int turned = 0;                   // How many turn after wallfollowing 
                                    // start???
  
  bool followAngle = false;         // moving toward to the end.
  bool wallFollow = false;          // Whether the robot is following the wall
  
  bool foundPath = false;			// When the robot found s-line again, this 
  									// will set to true.
  
  Point current(State[34], State[35]);
  									// robot's current position.
  
  double shortLength = lengthIs(end, current);
                                    // Storage to store the shortest length to 
                                    // the end point.
  double currentLength;             // Temporary storage to store the current 
                                    // length to the end point.
  
  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;
	}
      
	current = Point(State[34], State[35]);
									// Getting current potision.

    /*
     * If the robot is not following the wall, then check following
     * 1. If the robot is at the end position, we are done.
     * 2. If either x or y position hit the end position and the other 
     *    coodinator is VERY_CLOSE (close to 25 distance) to the end point, 
     *    get new angle to the end position and slow down and try to get the 
     *    exact end position.
     * 3. If both x and y are CLOSE (close to 50 distance), slow down to get 
     *    the end position.
     *
     * If the test fail all cases, then get the angle to the end position and 
     * make the robot to foward to the end positon.
     */
	if (!wallFollow)
	{
        /*
         * This is case of No. 1
         */
	  	if (current == end)         // Yes, we are at the end position, end!			
		{	
			st();
			ws(TRUE, TRUE, TRUE, TRUE);
		      
			/* wait for turing */
			while (State[38] != 0)
			  	gs();	
		      
			exit(1);
		}

        /*
         * This is case of No. 2
         *
         * We are possibly close to the end position. get right angle to the 
         * end position and slow down speed to get precise position.
         */		
		else if (current.getX() == end.getX() || current.getY() == end.getY())
		{
            /*
             * Even if either x or y position is the end positon, 
             * the can be far away if the other coordinator is not VERY_CLOSE.
             */			
			if ( abs(current.getX() - end.getX()) < VERY_CLOSE &&
			     abs(current.getY() - end.getY()) < VERY_CLOSE)
			{
			  
				st();				// We are close to the end, let's get good 
                                    // angle to the end position.
				ws(TRUE, TRUE, TRUE, TRUE);
				  
				/* wait for turing */
				while (State[38] != 0)
					gs();	
				  
				angle = angleIs(current, end, true);
				  
				turnTo(LEFT, angle);
				  
				tv = State[38] / 2;
				if (tv < VERY_CLOSE)  
					tv = VERY_CLOSE / 2;
				  
				vm (tv, 0, 0);
			}
		} 

        /*
         * This is case of No. 3
         *
         * We are CLOSE to the end position. Let's slow down to get precise 
         * position.
         */		
		else if ( abs(current.getX() - end.getX()) < CLOSE &&
			      abs(current.getY() - end.getY()) < CLOSE)
		{
			tv = State[38] / 2;     // Slow down the speed to the half.
		    if (tv < CLOSE)
				tv = VERY_CLOSE;
		    vm (tv, 0, 0);
		}

        /*
         * The robot is not close to the end.
         * Get the angle to the end position, and make the robot to foward to 
         * the end positon.
         * or
         * If the robot found the s-line again, follow line!
         */
		if (!followAngle || foundPath)
		{
		    angle = angleIs(current, end, true);
		      
		    turnTo(LEFT, angle);
		    vm (tv, 0, 0);
		      
		    followAngle = true;
		    wallFollow = false;
		    foundPath = false;
		}
	}

    /*
     * this tempAngle is the angle to make the robot to be perpendicular to 
     * the obstacle. This angle will use only when the robot firstly hit the 
     * obstacle. Otherwise, set this to the RIGHT_ANGLE (90 degree).
     */
    if (followAngle)
    {
	  	tempAngle = RIGHT_ANGLE - angle;
    }
    else
    {
		tempAngle = RIGHT_ANGLE;
    }

    /*
     * If we found the wall, then, turn the robot to make the wall-follow.
     * The right angle is made previsely.
     */      
    if (turing(tempAngle))
    {
        /*
         * This wall following program is right-wall-following (wall is at the 
         * right side of the robot).  So, if the robot hit the wall at the 
         * left side, then the robot should make 180 degree turn so the 
         * program execute right-wall-following.
         * This is only happened when the robot is getting to wallFollow
         */
	  	if ((addUp(5, 9) < ADD_UP_MAX - 5) && followAngle)
	      	turnTo(RIGHT, RIGHT_ANGLE * 2);

        /*
         * remember the positon where the robot hit the obstacle.
         */
	  	if (followAngle)
	  	{
	      	shortLength = lengthIs(end, current);
	      	followAngle = false;	
	  	}
	  
	  	vm(tv, 0, 0);
	  	wallFollow = true;
	  
	  	turned++;
	  
	}
      
	int quater1 = addUp(13, 17);
    int quater2 = addUp( 1,  5);
    int quater3 = addUp( 5,  9);
    int quater4 = addUp( 9, 13);
      
    /*
     * If the robot is currenty wall-following, this will continuosly make 
     * wall-following.
     */
    if (wallFollow)
    {
        /*
         * If the robot is seeing only from the 4th quater(right bottom side), 
         * whish is first case, or not seeing anything from right side, this 
         * is the end of the wall, so, make the turn.
         * When obstacle is both side of the robot, the second case will 
         * holde the case.
         */    	
		if ( (quater1 + quater2 + quater3) == (ADD_UP_MAX * 3) && 
	   		 (quater4 < ADD_UP_MAX) ||
	   		 (quater1 + quater4) == (ADD_UP_MAX * 2) )
		{
            /*
             * same as before, remember the positon whether the robot hit the 
             * obstacle.
             */ 			
      		if (followAngle)
      		{
	  			shortLength = lengthIs(end, current);
	  			followAngle = false;
      		}

      		turned++;
      		wallFollow = true;

            /*
             * Wait until we get enough space to turn.
             */	      
      		while(addUp(9, 13) < (ADD_UP_MAX - 1))
	  			vm(tv, 0, 0);
	      
      		turnTo(RIGHT, tempAngle);

            /*
             * When second case arisen, obstacle is both side of the robot, 
             * then next time, the first case will arise, so the robot will 
             * make an unnecessary turn.  
             * This will prevent this unnecessary turn.
             */	      
      		while (addUp(13, 17) > (ADD_UP_MAX - MINIMUM))
		  		vm(tv, 0, 0);

  		}
	  
        /*
         * After 2 turns (first is from entering wall-following and second is 
         * from one turn after wall-following), check whether the robot has 
         * reached s-line or not. If it has reached and it is closer than the 
         * position whether the robot start the wall-following(current 
         * shortestLength), start follow s-line again.
         */
  		currentAngle = angleIs(current, end, false);
	  
  		if (abs(currentAngle - firstAngle) < MINIMUM && turned > 2)
  		{
      		currentLength = lengthIs(end, current);
	      
	      	/*
	      	 * Yes, got the s-line back and shorter than previse one.
	      	 */
      		if (currentLength < shortLength)
      		{						
	  			shortLength = currentLength;
				foundPath = true;
				wallFollow = false;
				turned = 0;
	  
	  			st();
	  			ws(TRUE, TRUE, TRUE, TRUE);
	  
	  			/* wait for turing */
	  			while (State[38] != 0)
	      			gs();
      		}
  		}
   	}
   	gs();                     		/* update state        */
  }

  KI.off();                 		/* reset terminal settings              */
  disconnect_robot(1);      		/* gracefully disconnect from the robot */
}

/*
 * lengthIs will get two points and return the length between two points
 */
double lengthIs(Point a, Point b)
{
    int tempX = (b.getX() - a.getX()) * (b.getX() - a.getX());
    int tempY = (b.getY() - a.getY()) * (b.getY() - a.getY());
    
    double result = sqrt(tempX + tempY);

    return result;
}

/*
 * angleIs will get two points and return the angle to the point.
 * In this case, the robot doesn't need steering angle.
 * Therefore, added flag for it.
 */
double angleIs(Point from, Point to, bool addSteer)
{
    int steeringAngle = State[36];

    int lengthB = to.getX() - from.getX();
    int lengthC = to.getY() - from.getY();

    /*
     * Result will be inverse tan of angle.
     */
    double result = (90 - (atan2(lengthB, lengthC) * 180 / PI) ) * TENTH;
    
    /*
     * The robot could facing different angle. This will make the angle be 
     * always start from 0 degree.
     * Only add when it is needed.
     */
    if (addSteer)
    {
		result += ((steeringAngle < 1800) ? 
		   		   -steeringAngle : (3600 - steeringAngle));
    }

    return result;
}


/*
 * Function will make a 90 degree turn with appropriate input
 *
 * Function will firstly stop the robot and wait until it really stops.
 * and make a 90 degree turn, then go again. 
 */

void turnTo(int side, int whatAngle)
{    
    vm(0, 0, 0);
    
    pr(0, whatAngle * side, whatAngle * side);
    
    ws(TRUE, TRUE, TRUE, TRUE);
    
    /* wait for turing */
    while (State[39] != 0)
        gs();
}


/*
 * Function will check whether it is too close to the block or not. 
 * sensors are 3 front sensors.
 * Why I put sensor 16 is, my robot will make left turn so the sensor 16 can 
 * detect fastest than others.
 */
bool isClose()
{
    if ( (State[1]  < MINIMUM) || (State[2]  < MINIMUM) || 
	 (State[15] < MINIMUM) || (State[16] < MINIMUM) ) 
	return true;
    
    return false;
}

/*
 * Function will add up three front sensors and if it is less than certain 
 * amount, 23, which is close to the wall, then it will make turn.
 * Even if this addup value is not less than 23, the robot can hit the wall: 
 * when it is turning the edge, the value will be bigger than 23, so, we gotta 
 * check whether there is a block or not.
 */
bool turing(int whatAngle)
{
    int I_Add  = State[1] + State[2] + State[16];
    
    if (I_Add < 23 || isClose())
    {
	turnTo(LEFT, whatAngle);
	return true;
    }
    
    return false;
}

/*
 * Function will add up sensors values
 * range will be determined by input.
 */
int addUp(int from, int to)    
{
    int result = 0;

    for (int i = from; i < (to + 1); i++)
    {
	result += State[(i == 17) ? 1 : i];
    }

    return result;
}