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

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

const 	int LEFT 	=  1;
const 	int RIGHT 	= -1;

void 	turnTo(int);

bool 	turing();

int 	addUp(int from, int to);



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

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

    if (turing())
    {
		vm(tv, 0, 0);
    }

    int quater1 = addUp(13, 17);
    int quater2 = addUp( 1,  5);
    int quater3 = addUp( 5,  9);
    int quater4 = addUp( 9, 13);
    
    if ((quater1 + quater2 + quater3) == 225 && quater4 < 75)
    {
		while(addUp(9, 13) < 74)
		{
	    	vm(tv, 0, 0);
		}

		turnTo(RIGHT);
		vm (tv, 0, 0);
    }  

    gs();                                    /* update state        */
  }

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

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

}


/*
 * 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)
{    
    vm(0, 0, 0);
	
    pr(0, 900 * side, 900 * 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]  < 5) || (State[2]  < 5) || 
	 	 (State[15] < 5) || (State[16] < 5) ) 
		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 I_Add  = State[1] + State[2] + State[16];

    if (I_Add < 23 || isClose())
    {
		turnTo(LEFT);

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