/******************************************************
 *  Nic Grounds (with Moshe Gutman and Zachariah Ross)
 *  Project 1 - Team 7
 *  CS 4023 - Intro to Robotics - Spring 2004
 *****************************************************/

#use "cmucamlib.ic"

/*  GLOBALS  */
int TURN_MOTOR = 3;
int DRIVE_MOTOR = 1;
int TURN_ENCODER = 3;
int DRIVE_ENCODER = 2;
int TURN_TICKS = 17;                  // 90 degrees
float DRIVE_OFFSET_TURN_TIME = 0.11;  // time to sleep when driving to offset the turn
int TURN_SPEED = 70;
int DRIVE_SPEED = 55;

/*  GLOBALS - COLOR RECOGNITION  */
int COLOR_PINK_V_MIN = 210;
int COLOR_PINK_V_MAX = 240;
int COLOR_PINK_Y_MIN = 5;
int COLOR_PINK_Y_MAX = 245;
int COLOR_PINK_U_MIN = 50;
int COLOR_PINK_U_MAX = 80;
int COLOR_PINK_CONFIDENCE = 100;

int COLOR_YELLOW_V_MIN = 105;
int COLOR_YELLOW_V_MAX = 135;
int COLOR_YELLOW_Y_MIN = 5;
int COLOR_YELLOW_Y_MAX = 245;
int COLOR_YELLOW_U_MIN = 13; 
int COLOR_YELLOW_U_MAX = 60; 
int COLOR_YELLOW_CONFIDENCE = 100;

int COLOR_BLUE_V_MIN = 40;
int COLOR_BLUE_V_MAX = 80;
int COLOR_BLUE_Y_MIN = 5;
int COLOR_BLUE_Y_MAX = 245;
int COLOR_BLUE_U_MIN = 130;
int COLOR_BLUE_U_MAX = 190;
int COLOR_BLUE_CONFIDENCE = 100;

int COLOR_GREEN_V_MIN = 60;
int COLOR_GREEN_V_MAX = 100;
int COLOR_GREEN_Y_MIN = 5;
int COLOR_GREEN_Y_MAX = 245;
int COLOR_GREEN_U_MIN = 13;
int COLOR_GREEN_U_MAX = 60;
int COLOR_GREEN_CONFIDENCE = 100;

int COLOR_ORANGE_V_MIN = 160;
int COLOR_ORANGE_V_MAX = 230;
int COLOR_ORANGE_Y_MIN = 5;
int COLOR_ORANGE_Y_MAX = 245;
int COLOR_ORANGE_U_MIN = 0;
int COLOR_ORANGE_U_MAX = 30;
int COLOR_ORANGE_CONFIDENCE = 100;

int COLOR_BLACK_V_MIN = 100;
int COLOR_BLACK_V_MAX = 250;
int COLOR_BLACK_Y_MIN = 5;
int COLOR_BLACK_Y_MAX = 254;
int COLOR_BLACK_U_MIN = 100;
int COLOR_BLACK_U_MAX = 250;
int COLOR_BLACK_CONFIDENCE = 100;


/* function for recognizing pink - returns 1 if found, 0 otherwise */
int see_pink()
{
    if (trackRaw(COLOR_PINK_V_MIN, COLOR_PINK_V_MAX,
                 COLOR_PINK_Y_MIN, COLOR_PINK_Y_MAX,
                 COLOR_PINK_U_MIN, COLOR_PINK_U_MAX) > COLOR_PINK_CONFIDENCE)
      return 1;
    else return 0;
}

/* function for recognizing yellow - returns 1 if found, 0 otherwise */
int see_yellow()
{
    if (trackRaw(COLOR_YELLOW_V_MIN, COLOR_YELLOW_V_MAX,
                 COLOR_YELLOW_Y_MIN, COLOR_YELLOW_Y_MAX,
                 COLOR_YELLOW_U_MIN, COLOR_YELLOW_U_MAX) > COLOR_YELLOW_CONFIDENCE)
      return 1;
    else return 0;
}

/* function for recognizing blue - returns 1 if found, 0 otherwise */
int see_blue()
{
    if (trackRaw(COLOR_BLUE_V_MIN, COLOR_BLUE_V_MAX,
                 COLOR_BLUE_Y_MIN, COLOR_BLUE_Y_MAX,
                 COLOR_BLUE_U_MIN, COLOR_BLUE_U_MAX) > COLOR_BLUE_CONFIDENCE)
      return 1;
    else return 0;
}

/* function for recognizing green - returns 1 if found, 0 otherwise */
int see_green()
{
    if (trackRaw(COLOR_GREEN_V_MIN, COLOR_GREEN_V_MAX,
                 COLOR_GREEN_Y_MIN, COLOR_GREEN_Y_MAX,
                 COLOR_GREEN_U_MIN, COLOR_GREEN_U_MAX) > COLOR_GREEN_CONFIDENCE)
      return 1;
    else return 0;
}

/* function for recognizing orange - returns 1 if found, 0 otherwise */
int see_orange()
{
    if (trackRaw(COLOR_ORANGE_V_MIN, COLOR_ORANGE_V_MAX,
                 COLOR_ORANGE_Y_MIN, COLOR_ORANGE_Y_MAX,
                 COLOR_ORANGE_U_MIN, COLOR_ORANGE_U_MAX) > COLOR_ORANGE_CONFIDENCE)
      return 1;
    else return 0;
}

/* function for recognizing black - returns 1 if found, 0 otherwise */
int see_black()
{
    if (trackRaw(COLOR_BLACK_V_MIN, COLOR_BLACK_V_MAX,
                 COLOR_BLACK_Y_MIN, COLOR_BLACK_Y_MAX,
                 COLOR_BLACK_U_MIN, COLOR_BLACK_U_MAX) > COLOR_BLACK_CONFIDENCE)
      return 1;
    else return 0;
}

/* function that runs the motor, m, associated with encoder, e, for 
   t ticks (of that encoder) at speed, s */
void run_motor(int m, int e, int t, int s)
{
    int enc = 0, old_enc = 0;
    
    enable_encoder(e);
    reset_encoder(e);
    motor(m, s);
    
    while ((enc = read_encoder(e)) < t)
      {
        if (enc == old_enc) motor(m, s+15);
    }
    
    motor(m, 0);
    disable_encoder(e);
}

/* main program for project
   breaks the project into 4 steps:
   1) Find end of the first black square
   2) Find the next black square
   3) Find colored square
   4) Make appropriate turn  */
void run_program()
{
    int color = 0;
    
    printf("Starting Project 1\n");
    
    /* used to make sure camera is ready to go
       we had problems with camera not initializing
       and failing on its first attempt to see black */
    sleep(2.0);
    see_black();
    sleep(10.0);
    
    /* main loop */
    while(1)
      {
        /* GOAL: drive to end of square */
        motor(DRIVE_MOTOR, DRIVE_SPEED);
        while (!see_black()) ;
        sleep(DRIVE_OFFSET_TURN_TIME);
        motor(DRIVE_MOTOR, 0);
        
        /* GOAL: drive to next square */
        motor(DRIVE_MOTOR, DRIVE_SPEED);
        while (!see_black()) ;
        sleep(DRIVE_OFFSET_TURN_TIME);
        motor(DRIVE_MOTOR, 0);
        if (!see_black())
          {
            printf("Didn't find square?\n");
            beep();
            sleep(5.0);
            /* missed next square, go back and try again 
               we never inserted any code to correctly handle
               this because we felt is was not likely and that
               if this did happen we would restart the course */
        }
        
        /* GOAL: find colored square */
        while (1)
          {
            motor(DRIVE_MOTOR, DRIVE_SPEED);
            sleep(DRIVE_OFFSET_TURN_TIME);
            motor(DRIVE_MOTOR, 0);
            if (see_black()) { color = 1;  break; }
            if (see_yellow()) { color = 2; break; }
            if (see_green()) { color = 3; break; }
            if (see_blue()) { color = 4; break; }
            if (see_orange()) { color = 5; break; }
            if (see_pink()) { color = 6; break; }
        }
        
        /* GOAL: make the appropriate turn for the color,
           or break out of loop if yellow */
        if (color == 1)
          {
            printf("Missed the colored square!\n");
            beep();
            sleep(5.0);
            /* missed colored square 
               again we didn't write proper code to handle this
               we would restart the robot on the course */
        }
        else if (color == 2)  // YELLOW
            {
              /* done with course */
              break;
          }
          else if (color == 3)  // GREEN
              {
                /* do nothing */
            }
            else if (color == 4)  // BLUE
                {
                  motor(DRIVE_MOTOR, DRIVE_SPEED);
                  sleep(DRIVE_OFFSET_TURN_TIME);
                  motor(DRIVE_MOTOR, 0);
                  run_motor(TURN_MOTOR, TURN_ENCODER, TURN_TICKS, TURN_SPEED);
              }
              else if (color == 5)  // ORANGE
                  {
                    motor(DRIVE_MOTOR, DRIVE_SPEED);
                    sleep(DRIVE_OFFSET_TURN_TIME);
                    motor(DRIVE_MOTOR, 0);
                    run_motor(TURN_MOTOR, TURN_ENCODER, TURN_TICKS, -TURN_SPEED);
                }
                else if (color == 6)  // PINK
                    {
                      motor(DRIVE_MOTOR, DRIVE_SPEED);
                      sleep(DRIVE_OFFSET_TURN_TIME);
                      motor(DRIVE_MOTOR, 0);
                      run_motor(TURN_MOTOR, TURN_ENCODER, TURN_TICKS * 2, TURN_SPEED);
                  }
                  
    }
    
    /* Found yellow, broke out of loop! */
    printf("Done\n");
}

void main()
{
    /* initialize camera to balance white */
    init_camera();
    clamp_camera_yuv();
    
    /* wait for start button to be pushed */
    while (!start_button()) ;
    run_program();
}