/*
 * Assignment #A, CS154 Spring 2001
 * Name         :   SooYoung Jung
 * Date         :   Feb, 5, 2001
 * File         :   wall.txt
 * Description  :   all functions related to wall following
 * 
 */


/*
 * Inside of main, 
 * Extract the only while part
 */


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


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