/************************************************************
 *  Project 1                                               *
 *  Team 1: Stephen Mckinney, Jeremy Branecky, Camilo Reyes *
 *                                                          *
 *  Program to make robot follow a course with colored      *
 *  tiles on the floor that tell it where to go.            *
 *                                                          *
 ************************************************************/

#use "cmucamlib.ic"
#define L_MOTOR 0 
#define R_MOTOR 3 
#define L_ENCODER 0 
#define R_ENCODER 1 

#define STRAIGHT_SPEED 40 
#define LEFT_TURN_TICS 365
#define RIGHT_TURN_TICS 370 

#define LEFT 0
#define RIGHT 1 

// global color values:
#define ORANGE 1
#define BLUE 2
#define YELLOW 3
#define PINK 4
#define GREEN 5
#define FLOOR 6



// global variable used for the confidence values in the colors.
int conf;


/***********************************************
 * void stop_wheels() - method used to break   *
 * the motors.                                 *
 * from dmiller-super-demo.ic                  *
 *                                             *
 **********************************************/
void stop_wheels()
{    
    motor(L_MOTOR,-20);
    motor(R_MOTOR,-20);
    sleep(0.05);    
    ao();    
}


/***********************************************
 * void straight(int tics) - method for driver *
 * robot straight.                             *
 *                                             *
 **********************************************/
// tics = total of left and right tics, 10000 ticks ~ 6ft.

void straight(int tics)
{
    int l_enc, r_enc;
    int totalTics = 0;
    reset_encoder(L_ENCODER);
    reset_encoder(R_ENCODER);
    while (totalTics < tics)
      {
        l_enc = read_encoder(L_ENCODER);
        r_enc = read_encoder(R_ENCODER);
        totalTics = l_enc + r_enc;
        if (l_enc > r_enc)
          {  
            motor(R_MOTOR, STRAIGHT_SPEED);
            motor(L_MOTOR, (6*STRAIGHT_SPEED)/10);
        }
        else 
          if (r_enc > l_enc)
            {
              motor(L_MOTOR, STRAIGHT_SPEED);
              motor(R_MOTOR, (7*STRAIGHT_SPEED)/10);  
          }
          else
            {
              motor(R_MOTOR, STRAIGHT_SPEED);
              motor(L_MOTOR, STRAIGHT_SPEED);
          }
    }
    stop_wheels();
    ao();
}

/******************************************
 * void turn_90(int dir) - method used to *
 * turn the robot to either right or left.*
 * Note: only makes 90 degree turns.      *
 *                                        *
 *****************************************/
void turn_90(int dir)
{
    int tics, speed, tot_tics;
    int r_enc, l_enc;
    
    if (dir == RIGHT){
        speed = -30;
        tot_tics = RIGHT_TURN_TICS;
    }
    else{
        speed = 30;
        tot_tics = LEFT_TURN_TICS;
    }
    
    // reset encoders
    reset_encoder(L_ENCODER);
    reset_encoder(R_ENCODER);
    
    // measure encoders for turn
    while (tics < tot_tics) {
        r_enc = read_encoder(R_ENCODER);
        l_enc = read_encoder(L_ENCODER);
        tics = r_enc + l_enc;
        motor(R_MOTOR, speed);
        motor(L_MOTOR, -speed);
    } 
    
    // stop the turn
    motor(R_MOTOR, -speed/2);
    motor(L_MOTOR, speed/2);
    sleep(.1);
    ao();
}

// make a right turn.
void right_turn()
{
    turn_90(RIGHT);
}

// make a left turn.
void left_turn()
{
    turn_90(LEFT);
}

// make a 180 degree turn.
void turn_180()
{
    left_turn();
    left_turn();
}

// track orange color.
int trackOrange()
{
    return trackRaw(150,200,40,100,10,35);
}

// track blue color.
int trackBlue()
{
    return trackRaw(65,90,40,80,80,150);
}

// tack yellow color.
int trackYellow()
{
    return trackRaw(120,150,16, 80,16,19);
}

// track pink color.
int trackPink()
{
    return trackRaw(179,230,61,120,30,80);
}

// track green color.
int trackGreen()
{
    return trackRaw(70,110,80,140,16,20);
}

// track floor.
int trackFloor()
{
    return trackRaw(120, 140, 60, 90, 20, 50);
}


/************************************************
 * int getColor() - method used in cdetect for  *
 * detecting colors; dependent on conf value.   *
 * returns the color read by checking the       *
 * confidence value.                            *
 *                                              *
 ***********************************************/
int getColor()
{
    int confidence;
    int confidence2;
    // checking for pink    
    confidence = trackPink();
    if (confidence > 10)
      {
        conf = confidence;
        return PINK;
    }    
    // checking for orange
    confidence = trackOrange();
    if (confidence > 30)
      {
        conf = confidence;
        return ORANGE;
    }
    // checking for blue
    confidence = trackBlue();
    if (confidence > 10)
      {
        conf = confidence;
        return BLUE;
    }
    // checking for green
    confidence = trackGreen();
    if (confidence > 30)
      {
        conf = confidence;
        return GREEN;
    }
    // check yellow make sure is the right color
    confidence2 = trackYellow();
    if (confidence2 > 30)
      {
        conf = confidence2;
        return YELLOW;
    }
    else return -1;    
}

// very simple method used to get on top of a color if it seems
// to find one.
void getOnTop()
{
    straight(20);
}

// compensate will make the robot move properly to make a turn
// once it finds a color.
void compensate()
{
    straight(500);
}


/**************************************************************
 * main part of the code - our basic implementation is to run *
 * a process in order to make the robot move forward, then it *
 * checks for a color, it stops the motors and performs the   *
 * appropriate actions once it finds one. In case it never    *
 * finds any color the robot stops.                           *
 *                                                            *
 *************************************************************/
void main()
{
    int ticks=10000;  // 10000 ticks ~ 6 ft.
    int straight_pid; // process id for straight
    int color; // variable for checking color
    // initilize camera
    init_camera();
    clamp_camera_yuv();
    // enable encoders
    enable_encoder(L_ENCODER);
    enable_encoder(R_ENCODER);    
    // wait for someone to press start button
    printf("Press start to let loose!\n");
    while(!start_button());
    // start the straight pid
    straight_pid = start_process(straight(ticks));  // start the robot!
    while(1)
    {
        // get the color
        color = getColor();
        // if color is not the floor then check the color
        if(color != -1)
        {
            kill_process(straight_pid);
            stop_wheels();
            sleep(1.0);
            color = getColor();
            if (color == GREEN)
            {
                // go straight
                printf("Green - %d\n", conf);
                straight_pid = start_process(straight(ticks));
            }
            else if (color == PINK)
            {
                // found pink make a 180 degree turn
                stop_wheels();
                printf("Pink - %d\n", conf);
                turn_180();
                straight_pid = start_process(straight(ticks));
            }
            else if (color == BLUE)
            {
                // found blue make a right turn
                stop_wheels();
                printf("Blue - %d\n", conf);
                compensate();
                right_turn();
                straight_pid = start_process(straight(ticks));
            }
            else if (color == ORANGE)
            {
                // found orange make a left turn
                printf("Orange - %d\n", conf);
                compensate();
                left_turn();
                straight_pid = start_process(straight(ticks));
            }
            else if ((color == YELLOW))
            {
                // found yellow stop the robot
                stop_wheels();
                getOnTop();
                sleep(0.5);
                color = getColor();
                if(color == YELLOW)
                {
                    printf("Course complete. - %d\n", conf);
                    beep();
                    beep();
                    beep();
                    beep();
                    break;
                }
                else
                {
                    straight_pid = start_process(straight(ticks));
                }
            }
            else if (color == FLOOR)
            {
                // found the floor
                straight_pid = start_process(straight(ticks));
                printf("FLOOR... %d\n", conf);
            }
            else if (color == -1)
            {
                straight_pid = start_process(straight(ticks));
                printf("ERROR!\n");
            }
            else
            {
                straight_pid = start_process(straight(ticks));
                printf("BIG ERROR!\n");
            }
        }
        else
        {
            printf("It's the floor...\n");
        }
    }
    disable_encoder(L_ENCODER);
    disable_encoder(R_ENCODER);
}