/* Team 5
Project #1
Intell. Robotics

This code allows a robot to travel around a circut of four boxes three times.
The adjust heading algorithm and the wiggle algorithm, were both taken from 
Dr. Miller's code in the \tests directory from the dmiller-super-demo.ic file. 
Changes to these algorithms are noted in the function descriptions */

#define R_MOTOR 0 /* motor port 0 */
#define L_MOTOR 3 /* motor port 3 */
#define R_SENSOR 4 //port where right sensor is
#define L_SENSOR 5 //port where left sensor is
#define RIGHT_ENC 0 //port where right encoder is
#define LEFT_ENC 1 //port where left encoder is
#define GUIDE_MOTOR 1 //guide wheel motor port
#define BLACK_TAPE_VALUE 160 //light reading reading for black tape
#define MIN_TICKS 75 //min ticks before start looking for black tape
#define MAX_TICKS 90 //when you get this far, you have probably missed a box
#define TURN_TICKS 11  /* encoder tics for right angle turn */
int leftLightReading, rightLightReading, numLaps;


//returns TRUE when the robot needs to stop
//uses encoder clicks to make sure the robot does not miss a box
int goneTooFar(int leftCount, int rightCount)
{
    return ((leftCount >= MAX_TICKS) || (rightCount >= MAX_TICKS));
    //beep();
}

//reads the light sensors and stores the values in global vars
void getLightReading()
{
    leftLightReading = analog(L_SENSOR);
    rightLightReading = analog(R_SENSOR);   
}

//returns TRUE when robot has hit the tape
int hitTape(int count)
{
    int leftSensorVal, rightSensorVal;
    
    leftSensorVal = analog(L_SENSOR);
    rightSensorVal = analog(R_SENSOR);
    
    //unless the encoders have 75 clicks, ignore the tape
    if (count > 75)
      {        
        return ((leftSensorVal >= BLACK_TAPE_VALUE) || (rightSensorVal >= BLACK_TAPE_VALUE)); 
    }
    else
      //if you see black tape before 50 clicks, reset the encoders
      if (count < 50)
        {
          if ((leftSensorVal >= BLACK_TAPE_VALUE) || (rightSensorVal >= BLACK_TAPE_VALUE))
            {   
              reset_encoder(LEFT_ENC);
              reset_encoder(RIGHT_ENC);
              return 0;
          }
      }
      else 
        return 0;
}

//set the back guidewheels to be straight
void guideWheelsStraight()
{
    motor(R_MOTOR, -10);
    motor(L_MOTOR, 4);
    motor(GUIDE_MOTOR, -200);
    sleep(0.3);
    motor(GUIDE_MOTOR, -20);
    off(R_MOTOR);
    off(L_MOTOR);
}

//set the guidewheels to be sideways
void guideWheelsSideways()
{
    motor(R_MOTOR, -5);
    motor(L_MOTOR, -5);
    
    motor(GUIDE_MOTOR, 150);
    sleep(0.5);
    motor(GUIDE_MOTOR, 20);
}

//make sure the robot goes straight
//this code was taken from Dr. Miller's in the \tests directory from the dmiller-super-demo.ic file
//local variable names were changed, as well as the function name (from go_straight)
void adjustHeading(int leftEncCount, int rightEncCount, int fullSpeed)
{
    int leftSpeed, rightSpeed;
    
    leftSpeed = rightSpeed = fullSpeed;
    
    if (leftEncCount > rightEncCount)
      leftSpeed = (8 * fullSpeed) / 10;
    else
      if (rightEncCount > leftEncCount)
        rightSpeed = (8 * fullSpeed) / 10;
      
      motor(R_MOTOR, rightSpeed);
    motor(L_MOTOR, leftSpeed);
}

/* reverses motors briefly for a quick stop */
//code was taken from Dr. Miller's code in the \tests directory from the dmiller-super-demo.ic file
//the function name was changed (from stop_wheels to stopWheelsStraight)
void stopWheelsStraight()
{
    motor(L_MOTOR,-20);
    motor(R_MOTOR,-20);
    sleep(0.1);
    ao();
}

//drives the robot straight and stops in the next box
void goStraight(int speed)
{
    int leftEncCount, rightEncCount;
    
    reset_encoder(RIGHT_ENC);
    reset_encoder(LEFT_ENC);
    
    rightEncCount = read_encoder(RIGHT_ENC);
    leftEncCount = read_encoder(LEFT_ENC);
    
    //get the back wheels straight
    guideWheelsStraight();
    
    //get the motors going
    motor(R_MOTOR, speed);
    motor(L_MOTOR, speed);
    
    //if we havent gone too far or hit tape then....
    while (!goneTooFar(leftEncCount, rightEncCount) && (!hitTape(leftEncCount)))
      {  
        sleep(0.06);
        leftEncCount = read_encoder(LEFT_ENC);
        rightEncCount = read_encoder(RIGHT_ENC);
        adjustHeading(leftEncCount, rightEncCount, speed);  
    }
    //after we stop, beep
    beep();
    
    //here is where we make sure it stops in the same spot every time
    reset_encoder(RIGHT_ENC);
    reset_encoder(LEFT_ENC);
    
    rightEncCount = read_encoder(RIGHT_ENC);
    leftEncCount = read_encoder(LEFT_ENC);
    //go ahead 3 clicks after we need to stop
    while (rightEncCount <= 3)
      {
        rightEncCount = read_encoder(RIGHT_ENC);
        sleep(0.06);
    }
    //stop the robot
    stopWheelsStraight();
}

//stops the wheels when we are turning
void stopWheelsTurning()
{
    motor(R_MOTOR, -10);
    motor(L_MOTOR, 5);
    sleep(0.1);
    ao();
}

//this is the code to align with the black tape
void getLinedUp()
{
    int leftWiggleCount, rightWiggleCount;
    
    guideWheelsStraight();
    
    //backup for a bit to give us room....and make sure we dont miss
    motor(R_MOTOR, -15);
    motor(L_MOTOR, -15);
    sleep(1.5);
    getLightReading();
    leftWiggleCount = rightWiggleCount = 0;
    //while both the sensors do not see black....do this
    while ((leftLightReading < BLACK_TAPE_VALUE) || (rightLightReading < BLACK_TAPE_VALUE))
      {
        printf("left %d right%d\n",leftLightReading, rightLightReading);
        //if the right sensor sees black and the left doesnt...wiggle right
        if (rightLightReading >= BLACK_TAPE_VALUE)
          {  
            stopWheelsStraight();
            motor(R_MOTOR, -10);
            motor(L_MOTOR, 20);
            sleep(0.1);
            rightWiggleCount++;
            leftWiggleCount = 0;
            printf("right sees tape\n");
        }
        else
          //if the left sees black and the right one doesnt....wiggle left
          if (leftLightReading >= BLACK_TAPE_VALUE)
            {
              stopWheelsStraight();
              motor(L_MOTOR, -10);
              motor(R_MOTOR, 10);
              leftWiggleCount++;
              rightWiggleCount = 0;
              sleep(0.1);
              printf("left sees tape\n");
          }
          else
            //if neither one sees black, go forward slowly
            {
              motor(R_MOTOR, 8);
              motor(L_MOTOR, 8);
          }
        //if you wiggle right or left 10 consecutive times, then give up and go straight
        if ((rightWiggleCount >= 10) || (leftWiggleCount >= 10))
          return;
        
        sleep(0.06);
        getLightReading();
    }   
    //    stopWheelsStraight();    
}

//do a 90 degree left turn
void leftTurn()
{
    int leftEncCount, rightEncCount;
    
    sleep(1.0);
    reset_encoder(RIGHT_ENC); 
    reset_encoder(LEFT_ENC);
    
    rightEncCount = read_encoder(RIGHT_ENC);
    leftEncCount = read_encoder(LEFT_ENC);
    
    //set the back wheels sideways to allow for easy turning
    guideWheelsSideways();
    
    //get the motors going
    motor(R_MOTOR, 25);
    motor(L_MOTOR, -10);
    
    //while the right wheel has not gone far enough.....
    while(rightEncCount < TURN_TICKS)
      {
        rightEncCount = read_encoder(RIGHT_ENC);
        //L_Enc = read_encoder(1);
        
        printf("Right=%d Left=%d\n", rightEncCount, leftEncCount);
    }
    //stop the wheels
    stopWheelsTurning();
}

//going around the circuit 3 times
void doCircuit()
{
    int i, j;
    numLaps = 0;
    for (i = 0; i < 3; i++)
      {
        numLaps++; 
        for (j = 0; j < 4; j++)
          {                       
            goStraight(25);
            
            //if we are in the last square, we dont care about aligning ourselves
            if ((j == 3) && (i == 2))
              {}else
              {
                leftTurn();  
                getLinedUp();
            }
            
        }      
    }
    ao(); //turn the motors off so we can stop
}


void main()
{
    //wait for the start button
    while(!start_button())
      {    
    }
    
    //enable the encoders
    enable_encoder(RIGHT_ENC);
    enable_encoder(LEFT_ENC);
    
    //do the circuit
    doCircuit();
    
    //turn off the encoders
    disable_encoder(RIGHT_ENC);
    disable_encoder(LEFT_ENC);
}