/*
 * Assignment #A, CS154 Spring 2001
 * Name         :   SooYoung Jung
 * Date         :   Feb, 5, 2001
 * File         :   computeRotationVelocity.txt
 * Description  :   computeRotationalVelocity function
 * 
 */

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