// Create Botguy Grabber
//   Looks for Botguys, grabs them
//   and returns them to the green grass
//
// Starting Light = analog 2
//
// Line Left Front = analog 5
// Line Right Front = analog 6
// Line Left Back = analog 3
// Line Right Back = analog 4
//
// Grabber Left Claw = servo 2
// Grabber Right Claw = servo 3
//
// Need 300 pixels to start camera tracking
// Vision screen is 356x292 (103,952 pixels)
// Center of x dimmension is 178
//

#use botball.ic
#use xbccamlib.ic

// Define color model 
// default: 0=red, 1=yellow, 2=green
#define botguy 0
#define home 2

#define startinglight 2

#define left_line 5
#define right_line 6
#define left_back_line 3
#define right_back_line 4

#define left_claw 2
#define right_claw 3    

int create = 1;  // Create state (initially off)
int cal = 0; // Calibrate start light

int navigatepid = 0; // Navigate process ID

int linegray = 180; // Greater is black

int left_claw_open = 84;  // Claw positions
int left_claw_close = 216;
int right_claw_open = 132;
int right_claw_close = 0;
int position = 0;  // Claw Position
int rate = 2;  // Claw closing rate

long endtime = 0L; // General timer variable
int count = 0; // General count variable

// Actions
int botguy_found = 0;
int botguy_grabbed = 0;
int home_found = 0;
int home_done = 0;

// Navigation variables
int loc_x = 0;
int loc_center = 182; // Should be 178 but camera points slightly left
int loc_delta = 20; // For driving toward the object
int loc_edge = 110; // For disregarding the edge
int head_angle = 0;
int blob_min = 400;
int botguy_max = 20000;
int botguy_max_y = 260;
int home_max = 12000;
int home_max_y = 272;

// Main Controller ================================================
void main() {
    display_clear();
    printf("Grabber V1\n");
    msleep(200L);
    beep(); 
    
    // Connect to the Create
    create = create_connect(); 
    while (create!=0) {
        printf("  Turn on Create\n");
        create = create_connect(); 
    }
    // create_safe();  // Create in safe mode
    create_full();  // Create in non-safe mode
    printf("  Create connected non-safe\n");
    msleep(200L);
    beep(); 
    msleep(200L);
    beep();
    msleep(200L);
    beep();
    msleep(200L);
    
    // Initialize camera
    init_camera();
    track_set_minarea(blob_min); 
    while(!a_button()) {
        track_update();
        display_clear();
        printf("Check camera\n");
        printf("  Press A button to continue\n");
        printf("  Red   count=%d\n    x=%d, y=%d\n    size=%d\n",
               track_count(botguy),track_x(botguy,0),track_y(botguy,0),track_size(botguy,0));
        printf("  Green count=%d\n    x=%d, y=%d\n    size=%d\n",
               track_count(home),track_x(home,0),track_y(home,0),track_size(home,0));
        msleep(200L);
    }
    msleep(200L);
    beep(); 
    msleep(200L);
    beep();
    msleep(200L);
    beep();
    msleep(200L);
    
    printf("Press A to Calibrate\n");
    printf("  Press B to Bypass\n");
    while(cal==0) {  // Deal with starting light
        if (a_button()==1) {  // Calibrate
            cal = 1;
            beep();
            wait_for_light(startinglight);            
        }
        if (b_button()==1) {   // Bypass calibrate
            cal = -1;
            beep();
            msleep(100L);
            beep();
            msleep(500L);
        }
    }
    
    // Start shut down process
    shut_down_in(180.0); // Max 180 seconds (3 minutes)
    
    // Start navigate process
    navigatepid = start_process(navigate());
    msleep(200L);    
}

// Navigation ================================================
void navigate() {
    
    display_clear();
    printf("Get Botguy\n");
    printf("  Hold A button to quit\n");
    
    claw_tight();
    enable_servos(); 
    claw_tight();
    msleep(1000L);
    
    // Loop through actions
    while(!a_button()) {
        track_update();
        
        // Look for Botguy
        if (botguy_found==0) {
            right();
            if ((track_count(botguy)>0) &&
                (track_x(botguy,0)<(loc_center+loc_edge)) && 
                (track_x(botguy,0)>(loc_center-loc_edge))) { 
                stop();
                claw_open();
                printf("Botguy Found\n");
                botguy_found = 1;
            }    
        } else {
            // Grab Botguy
            if (botguy_grabbed==0) {
                if (track_count(botguy)==0) {  
                    botguy_found = 0;
                } else {
                    if (track_x(botguy,0)<(loc_center-loc_delta)) {
                        forward_left();
                    } else {
                        if (track_x(botguy,0)>(loc_center+loc_delta)) {
                            forward_right();
                        } else {
                            forward();
                        }
                    }
                    if ((track_size(botguy,0)>botguy_max)||
                        (track_y(botguy,0)>botguy_max_y)){
                        stop();
                        claw_close();
                        msleep(500L);
                        backward();
                        msleep(500L);
                        forward();
                        msleep(1000L);
                        stop();
                        printf("Botguy Grabbed\n");
                        botguy_grabbed = 1;
                    }
                }
            } else {
                // Look for Home
                if (home_found==0) {
                    right();
                    if ((track_count(home)>0) &&
                        (track_y(home,0)>(80)) && 
                        (track_x(home,0)<(loc_center+loc_edge)) && 
                        (track_x(home,0)>(loc_center-loc_edge))) { 
                        stop();
                        claw_open();
                        printf("Home Found\n");
                        home_found = 1;
                    }    
                } else {
                    // Go Home
                    if (home_done==0) { 
                        if (track_count(home)==0) {  
                            home_found = 0;
                        } else {
                            if (track_x(home,0)<(loc_center-loc_delta)) {
                                forward_left();
                            } else {
                                if (track_x(home,0)>(loc_center+loc_delta)) {
                                    forward_right();
                                } else {
                                    forward();
                                }
                            }
                            if ((track_size(home,0)>home_max)||
                                (track_y(home,0)>home_max_y)) {
                                stop();
                                forward();
                                msleep(1000L);
                                stop();
                                printf("Gone Home\n");
                                home_done = 1;
                                break;
                            }                 
                        }
                    }
                }
            }
        }
    }
    // Navigation over 
    stop(); // Drive motors off
    disable_servos(); // Release claws
    create_disconnect();  // Done with create
    beep(); 
    msleep(200L);
    beep();
    msleep(200L);
    beep();
    msleep(200L);
    printf("Done!\n");
}   

// Functions ==========================================================

// Forward Function
void forward() {
    create_drive_straight(200);
}

// Backward Function
void backward() {
    create_drive_straight(-200);
}

// Forward Left Angle Function
void forward_left() {
    create_drive_direct(200,160); 
}

// Forward Right Angle Function
void forward_right() {
    create_drive_direct(160,200); 
}

// Backward Left Angle Function
void backward_left() {
    create_drive_direct(-200,-160); 
}

// Backward Right Angle Function
void backward_right() {
    create_drive_direct(-160,-200);
}

// Right Spin Function
void right() {
    create_spin_CW(150);
}

// Left Spin Function
void left() {
    create_spin_CCW(150);
}

// Right Heading Function
void right_head(int angle) {
    create_spin_block(200,-(angle)); 
    msleep(200L); 
}

// Left Heading Function
void left_head(int angle) {
    create_spin_block(200,angle); 
    msleep(200L); 
}

// Stop Function
void stop() {
    create_stop();
    msleep(1000L); 
}

// Claw Open Function
void claw_open() { 
    set_servo_position(right_claw,right_claw_open);
    msleep(1000L);
    set_servo_position(left_claw,left_claw_open);
    msleep(500L);   
}

// Claw Close then Open Function
void claw_close() { 
    rate = 2; // Closing rate
    for (position=left_claw_open; position<left_claw_close; position=position+rate) {
        set_servo_position(left_claw,position);
        msleep(20L);  // Wait for servo movement
    }
    set_servo_position(left_claw,left_claw_close);
    msleep(500L);
    
    rate = 2; // Closing rate
    for (position=right_claw_open; position>right_claw_close; position=position-rate) {
        set_servo_position(right_claw,position);
        msleep(20L);  // Wait for servo movement
    }
    set_servo_position(right_claw,right_claw_close);
    msleep(500L);
}

// Claw Close Tight Function
void claw_tight() { 
    set_servo_position(left_claw,left_claw_close);
    msleep(500L);
    set_servo_position(right_claw,right_claw_close);
    msleep(500L); 
}