/*
 * Corridor-following program for the Nomad 200
 *
 * Authors: Brie Finger and Jessica Fisher
 *          based off of code by Zach Dodds
 *
 * Date: February 14, 2002
 * File: avoid.cc
 *
 * This program implements corridor-following.  It is modeled after the 
 * three-layer architecture of Erann Gat and the subsumption architecture of
 * Rodney Brooks.  Currently, it has only two layers: controller and 
 * sequencer.  The controller contains two low-level behaviors: avoiding 
 * close-range obstacles, and centering the robot in a corridor.  The sequencer
 * decides which low-level behavior to use based off of sensor input.
 */

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

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

const int THRESHOLD = 8;        // Threshold for close avoidance
const int PORT = 7021;          // Port to contact server on
const int DEFAULT_TV = 80;      // Default translational velocity
const int DEFAULT_RV = 0;       // Default rotational velocity
const int MAX_SONAR = 10000;    // Greater than the maximum sonar value
const int SONAR_OFFSET = 17;    // Offset into the State vector
const int LOW_SONAR_INDEX = 0;  // Lowest sonar sensor to look at
const int HIGH_SONAR_INDEX = 15;// Highest sonar sensor to look at
const int MAX_INFRA = 1000;     // Higher than the maximum infrared value
const int INFRA_OFFSET = 1;     // Offset into the State vector
const int LOW_INFRA_INDEX = -3; // Lowest infrared sensor to look at
const int HIGH_INFRA_INDEX = 3; // Highest infrared sensor to look at
const int MAX_ROT = 375;        // Maximum rotational velocity
const int MIN_TRANS = 0;        // Minimum translational velocity
const int RADIUS = 2;           // Buffer zone around the robot
const int RIGHT = 12;           // Number of sensor directly to the right
const int LEFT = 4;             // Number of sensor directly to the left
const int KP = 80;              // Proportional constant for L/R centering
const int KD = 20;              // Proportional constant for angular centering

/*
 * Table of Contents:
 */

int getShortestSonarIndex(int fromSonar, int toSonar);
// Returns the number of the sonar sensor which has the lowest reading.
int getShortestInfraIndex(int fromInfra, int toInfra);
// Returns the number of the infrared sensor which has the lowest reading.
void closeAvoid();
// Implements close-range obstacle avoidance.
void farAvoid(); 
// Implements corridor centering.

/*
 * This function gets the shortest sonar reading in a range from
 *
 *   fromSonar to toSonar
 *
 * The index of that minimum reading is returned.
 *
 * We treat the sensors as being numbered from zero to fifteen.
 *
 * 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 index in the State vector 
    int minSonar = MAX_SONAR;   // Higher than any real sonar value
    int minSonarIndex;
    int temp;
    
    /* Here we swap from and to in case they are in the wrong order. */
        
    if (fromSonar > toSonar)
    { 
	temp = fromSonar;
	fromSonar = toSonar;
	toSonar = temp;
    }
    
    for (int i = fromSonar; i <= toSonar; ++i) 
    {
	realSN = (i + 32) % 16; // That way specifying negative sonar #s is OK
	realSN = realSN + SONAR_OFFSET;   
                                // Add the offset into the State vector

	if (State[realSN] < minSonar)
	{                       // Ties go to the previous index
	    minSonar = State[realSN];
	    minSonarIndex = realSN;      
	}
    }

    minSonarIndex = minSonarIndex - SONAR_OFFSET;
    return minSonarIndex;
}


/*
 * This function gets the shortest infrared reading in a range from
 *
 *   fromInfra to toInfra
 *
 * The index of that minimum reading is returned.
 *
 * We treat the sensors as being numbered from zero to fifteen.
 *
 * 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 getShortestInfraIndex(int fromInfra, int toInfra)
{
    int realIN;                 // Real infrared index in the State vector 
    int minInfra = MAX_INFRA;   // Higher than any real infrared value
    int minInfraIndex;
    int temp;
    
    /* Here we swap from and to in case they are in the wrong order. */
        
    if (fromInfra > toInfra)
    { 
	temp = fromInfra;
	fromInfra = toInfra;
	toInfra = temp;
    }
    
    for (int i = fromInfra; i <= toInfra; ++i) 
    {
	realIN = (i + 32) % 16; // That way specifying negative infra #s is OK
	realIN = realIN + 1;   
                                // Add the offset into the State vector

	if (State[realIN] < minInfra)
	{                       // Ties go to the previous index
	    minInfra = State[realIN];
	    minInfraIndex = realIN;      
	}
    }

    minInfraIndex = minInfraIndex - 1;
    return minInfraIndex;
}


/*
 * This function uses the infrared sensors to avoid close-range obstacles.
 * To implement this, we first take as an argument the index number of the
 * sensor which is closest to an obstacle.  We compute the distance to that
 * obstacle.  The next step is divided up into multiple cases:
 *    Case 1: The closest sensor is directly in front of the robot.  In this
 *            case, the robot begins to turn to the left and slow down.
 *    Case 2: The closest sensor is one of the left-side sensors.  
 *            In this case, the robot turns more slowly than in Case 1, and 
 *            turns to the right (i.e. clockwise) rather than the left.
 *            It also slows less dramatically as it gets closer to the 
 *            obstacle.
 *    Case 3: The closest sensor is one of the right-side sensors.
 *            This case mirrors Case 2, with the robot turning to the left
 *            rather than the right.
 */
void closeAvoid(int closestIndex)
{
    int dist = State[closestIndex + INFRA_OFFSET];
    int rotComp;
    int transComp;
    int prop;
    int rv;
    int tv;

    /*
     * Here we compute the proportion to be used in calculating the
     * compensation.
     */
    prop = ((THRESHOLD - (dist - RADIUS)) / THRESHOLD);

    if (closestIndex == 0)
    {
	/*
	 * We are assuming that when the threshold is crossed, the robot
	 * will begin to turn to the left, slowing and turning more as
	 * it gets closer to the object.
	 */
	
	rotComp = prop * (MAX_ROT - 100);
	rv = rotComp + 100;
	
	transComp = prop * (20 - MIN_TRANS);
	tv = 20 - transComp;
	
    }

    /*
     * We are assuming that when the threshold is crossed on the side, 
     * the robot will begin to turn to the left or right
     * depending on which sensor was closest, at a translational velocity
     * of 3 inches per second.  The robot will slow and turn faster if it
     * is closer to the object.
     */
    
    else if (closestIndex >= 1 && closestIndex < 4)
    {
	rotComp = prop * (MAX_ROT - 80);
	rv = -1 * (rotComp + 80);
	
	transComp = prop * (30 - MIN_TRANS);
	tv = 30 - transComp;
    }
    
    else
    {
	rotComp = prop * (MAX_ROT - 70);
	rv = rotComp + 70;
	
	transComp = prop * (30 - MIN_TRANS);
	tv = 30 - transComp;
    }
    
    vm(tv, rv, rv);
    gs();

}

/*
 * This function uses the sonar sensors to align the robot in the clearest
 * space available (i.e. center of a hallway).  It uses proportional control
 * based on (a) the difference in readings of the direct left and direct right
 * sensors ("error"), and (b) the difference in readings of the sensors 
 * directly in front of and behind the right sensor ("D"), to get an idea of 
 * the angle that the robot is facing.
 */
void farAvoid()
{
    int R = State[RIGHT + SONAR_OFFSET];
    int L = State[LEFT + SONAR_OFFSET];
    int error = L - R;
    int D = State[RIGHT + 1 + SONAR_OFFSET] - State[RIGHT - 1 + SONAR_OFFSET];
    int rv = DEFAULT_RV;
    int tv = DEFAULT_TV;


    /*
     * In this case, the robot is closer to the right wall than the left wall.
     */
    if (error > 0)
    {
	/*
	 * Here, the robot is much closer to the right than the left, but
	 * is not angled very sharply away from the wall.
	 */
	if (error > 10 && D < 10)
            rv = KP * error;
	
	/* 
	 * Here, the robot is much closer to the right than the left, and
	 * is angled sharply away from the wall.
	 */
        else if (error > 10 && D >= 10)
            rv = 0;             // already getting there

	/* 
	 * Here, the robot is close to being directly in between the
	 * two walls.
	 */
	else                    // error <= 10
        {
	    /* 
	     * In this case, the robot is angled too sharply, so we
	     * turn the other way to compensate.
	     */
            if (D > error)
 		rv = -1 * KD * D;
	    /*
	     * In this case, the robot is angled well, and continues
	     * on in the same direction.
	     */
            else if (D > 0)
		rv = 0;
	    /*
	     * In this case, the robot is either turned the wrong way or
	     * is close to the center of the corridor but heading away
	     * from it.
	     */
            else
		rv = KP * error;
	}
    }

    /*
     * In this case, the robot is closer to the left wall than the right 
     * wall.
     */
    else if (error < 0)
    {
	/*
	 * Here, the robot is much closer to the left than the right, but
	 * is not angled very sharply.
	 */
	if (error < -10 && D > -10)
	    rv = KP * error;
	/*
	 * Here, the robot is much closer to the left than the right, and is
	 * at a good angle, so we simply keep going.
	 */
	else if (error < -10 && D <= -10)
	    rv = 0;
	/*
	 * Here, the robot is close to being directly between the walls.
	 */
	else
	{
	    /*
	     * In this case, the robot is angled too sharply, and must turn
	     * the other way to compensate.
	     */
	    if (D < error)
		rv = -1 * KD * D;
	    /*
	     * Here, the robot is angled well and should continue on.
	     */
	    else if (D < 0)
		rv = 0;
	    /* 
	     * Here, the robot is either facing the wrong direction, or is
	     * near the center of the corridor but headed the wrong way.
	     */
	    else
		rv = KP * error;
	}
    }

    /*
     * In this case, the robot is directly between the walls, and simply
     * continues going straight.
     */
    else
    {
	rv = 0;
    }

    vm(tv, rv, rv);
}


/*
 * The main function implements the sequencer.  It determines whether to
 * call closeAvoid or farAvoid based on sensor input, and it monitors the
 * keyboard input to determine when to quit.
 */

int main(int argc, char** argv)
{
    SERV_TCP_PORT = 7021;       // This is the port we run on 

    char c;                     // Store keyboard input
    KbdInput KI;                // Allows nonblocking input
    bool done = false;          // True when q is pressed

    int closestInfra;           // The closest infrared sensor
    int dist;                   // Its reading

    int shortestIndex;          // The closest sonar sensor

    /* 
     * Here we connect to the simulator.
     */

    if (connect_robot(1)) 
    {
	cout << "\nConnected to robot\n";
    }
    else 
    {
	cout << "Failed to connect to robot\n";
	exit(0);
    }

    zr();                       // Zero the robot

    /* 
     * Here we turn on the appropriate sensors.
     */

    for (int i = 17; i <= 32; ++i)
	Smask[i] = 1;

    ct();                       // Register the new mask vector with the robot
    gs();                       // Update the robot's state
    
    /*
     * This is the sense/act loop implementing reactive control
     */

    KI.on();                    // Turn on raw input/output

    shortestIndex = getShortestSonarIndex(0, 15);

    /*
     * Here, we want to turn the robot so that the closest obstacle is on its
     * side (either right or left) rather than at an angle or in front 
     * of/behind the robot.  Also, we check to make sure that we are not
     * simply rotating in the center of a large open space.
     */
    while (shortestIndex != RIGHT && shortestIndex != LEFT 
	   && State[shortestIndex + SONAR_OFFSET] != 255)
    {
	shortestIndex = getShortestSonarIndex(0, 15);

	/*
	 * If the closest obstacle is in front, we rotate to the clearest
	 * side.
	 */
	if (shortestIndex == 0)
	{
	    if (State[RIGHT + SONAR_OFFSET] > State[LEFT + SONAR_OFFSET])
		vm(0, -100, -100);
	    else
		vm(0, 100, 100);
	}

	/* 
	 * If the closest obstacle is to the upper left, we rotate to 
	 * the right to center on the left sensor.
	 */
	else if (shortestIndex >0 && shortestIndex < LEFT)
	    vm(0, -100, -100);

	/*
	 * If the closest obstacle is to the lower left, we rotate to
	 * the left to center on the left sensor.
	 */
	else if (shortestIndex > LEFT && shortestIndex <= 8)
	    vm(0, 100, 100);
	
	/*
	 * If the closest obstacle is to the lower right, we rotate to
	 * the right to center on the right sensor.
	 */
	else if (shortestIndex > 8 && shortestIndex < RIGHT)
	    vm(0, -100, -100);

	/*
	 * If the closest obstacle is to the upper right, we rotate to
	 * the left to center on the right sensor.
	 */
	else
	    vm(0, 100, 100);
    }


    /*
     * Main action loop (AKA sequencer).
     */
    while (!done) 
    {
	closestInfra = getShortestInfraIndex(LOW_INFRA_INDEX,
					     HIGH_INFRA_INDEX);
	dist = State[closestInfra + INFRA_OFFSET];

	if (dist <= THRESHOLD)
	    closeAvoid(closestInfra);

	else
	    farAvoid();
	
	c = KI.nextChar();      // c is 0 if there's no input ready
	
	if ( c == 'q' )         // Press q in the terminal window to quit
	{
	    done = true;
	}
	
    }
    
    KI.off();                   // Reset terminal settings
    
    disconnect_robot(1);        // Gracefully disconnect from the robot
    
}