#use "xbccamlib.ic"
#define touchArm digital(8) // touch arm sensor
#define detectEgg analog(0) // Et sensor
#define arm digital(9)
//#define leftArm digital(10)
#define frontET analog(1) // front ET sensor

// biggest blob default
int biggest_blob = 0;

// counter
int i;

// default channel numbers
int blueEggChannel = 0;
int orangeGoalChannel = 1;
int blueGoalChannel = 2;
int tennisBallChannel = 3;
int goalColorChannel;

// color channel arrays
int blueEgg[4];
int tennisBall[4];
int blueGoal[4];
int orangeGoal[4];

// runs when the program is started
void main()
{
    printf("Pick your color goal (up for blue, down for orange), then hit \"B\":\n");    
    while(!escape_button())
      {        
        if(down_button())
          {
            goalColorChannel = orangeGoalChannel;
            display_clear();
            printf("The goal color is orange.\n");
        }
        
        if(up_button())
          {
            goalColorChannel = blueGoalChannel;
            display_clear();
            printf("The goal color is blue.\n");
        }        
    }
    sleep(1.0);
    display_clear();
    // initiates the camera and creates the color arrays
    // using the set_up_colors method
    init_camera();
    set_up_colors();
    
    // loads the various color arrays into the appropriate channel
    color_set_model(blueEggChannel, blueEgg);
    color_set_model(tennisBallChannel, tennisBall);
    color_set_model(blueGoalChannel, blueGoal);
    color_set_model(orangeGoalChannel, orangeGoal);
    
    while(1)
      {
        // identifies the biggest blob(the closest egg)   
        find_biggest_blob(blueEggChannel);
        
        // orients the robot to face towards the biggest blob(the closest egg)
        orient_to_biggest_blob(blueEggChannel);    
        
        // while the egg is in the cage
        while(detectEgg > 190 && detectEgg < 210)
          {     
            
            move_forward(); 
            track_update();
            sleep(.35);
            orient_to_biggest_blob(blueEggChannel); // orient to the biggest blob(the closest egg) to correct
            // any errors while trying to move towards the egg
        }
        move_arm_down(); // trap the eggs in the goal with the powered arm
        
        moveEggToGoal(goalColorChannel); // orient towards the goal and move into it
    }
}

void avoid_obstacle()
{
    ao();
    move_reverse();
    sleep(1.0);
    ao();
    motor(0,-100);
    motor(2,93);
    sleep(.50);
    ao();
    move_forward();
    sleep(1.75);
    motor(0,100);
    motor(2,-93);
    sleep(.50);
    ao();
}


// move foward (power is reduced to motor 2 to account for unequal motor performance)
void move_forward()
{
    if(analog(1) > 225)
      avoid_obstacle();
    motor(0, 100);
    motor(2, 93);
    if(arm == 1) 
      {
        move_reverse();
        sleep(1.5);
    }
    
}

void move_reverse()
{
    motor(0, -100);
    motor(2, -93);
}

// moves the egg trapping arm to the open position
void move_arm_up()
{
    ao();
    motor(1,-100);
    motor(3,-100);
    sleep(2.0);
    ao();
}

// moves the egg trapping arm to the closed position
void move_arm_down()
{   
    ao();
    motor(1,75);
    motor(3,75);
    sleep(1.0);
    ao();
}

// sets up the color's HSV values and stores them in arrays
void set_up_colors()
{    
    blueEgg[0] = 136; // HMIN
    blueEgg[1] = 201; // HMAX
    blueEgg[2] = 20;  // SMIN
    blueEgg[3] = 30;  // VMIN      
    
    tennisBall[0] = 50;  // HMIN
    tennisBall[1] = 79;  // HMAX
    tennisBall[2] = 133; // SMIN
    tennisBall[3] = 100; // VMIN         
    
    blueGoal[0] = 227; // HMIN
    blueGoal[1] = 269; // HMAX
    blueGoal[2] = 71;  // SMIN
    blueGoal[3] = 45;  // VMIN
    
    orangeGoal[0] = 355 ; // HMIN
    orangeGoal[1] = 14;   // HMAX
    orangeGoal[2] = 100;  // SMIN
    orangeGoal[3] = 75;   // VMIN 
}

// this method will identify the largest blob(closest egg) of the color on channel "currentChannel"
void find_biggest_blob(int currentChannel)
{
    // while there is no blobs
    while(track_count(currentChannel) < 1)
      {
        track_update(); //take a new snap shot    
    }     
    i = 0; // counter set to 0    
    // while all the blobs havnt been checked
    while(i < track_count(currentChannel))
      {        
        track_update(); //take a new snap shot 
        // if current blob being check is bigger than current biggest blob
        if(track_size(currentChannel,i) > track_size(currentChannel, biggest_blob))
          biggest_blob = i; // set the biggest blob to the current blob being checked
        i++; // increment i to check the next blob
    }        
}

// orients robot to the biggest blob (closest egg) on the channel "currentChannel"
void orient_to_biggest_blob(int currentChannel)
{       
    // if the center of the blob (x coord) is not in the center of the screen
    if(track_x(currentChannel,biggest_blob) > 185 || track_x(currentChannel,biggest_blob) < 170)
      { 
        // if the center of the blob is on the right side of the screen
        if(track_x(currentChannel,biggest_blob) > 185)
          {   
            // while the center of the blob is not yet in the center
            while(track_x(currentChannel,biggest_blob) > 185)
              {   
                // turn right 
                motor(2, 75);
                motor(0, 0);
                track_update();
                if(arm == 1) 
                {
                    move_reverse();
                    sleep(1.5);
                }
            }
        }
        // if the center of the blob is on the left side of the screen
        else if(track_x(currentChannel,biggest_blob) < 170)
          {  
            // while the center of the blob is not yet in the center
            while(track_x(currentChannel,biggest_blob) < 170)
              {  
                // turn left
                motor(0, 75);
                motor(2, 0);
                track_update(); 
                if(arm == 1) 
                {
                    move_reverse();
                    sleep(1.5);
                }
            }   
        }
    }
    ao(); // turn all motors off
    beep(); // beep to let us know that the robot is oriented
}

// first scans for the goal, then orients towards the goal, then moves towards the goal
void moveEggToGoal(int currentChannel)
{        
    // while there are no blobs
    while(track_count(currentChannel) == 0)
      {        
        // spin and scan for the goal (a blob)         
        motor(0,-100);
        motor(2, 100);
        track_update();        
    }
    ao(); //turn all motors off
    find_biggest_blob(currentChannel); // identify the biggest blob (should be the goal)
    orient_to_biggest_blob(currentChannel); //orient the robot to the goal
    while(track_size(currentChannel, biggest_blob) < 32500)
      {
        move_forward();
        sleep(.15);
        track_update();
        orient_to_biggest_blob(currentChannel);
        printf("I AM IN THE GOALLLLLLLL!");
        display_clear();
    }
    
    move_arm_up();
    move_forward();
    sleep(1.0);
    ao();
    
    move_reverse();
    sleep(2.0);
    ao();
    
    motor(0,100);
    motor(2,-100);
    sleep(1.0);
}