// Beyond Botball 2009 Bot A
//   Moves out to Botguy, grabs him in the claw,
//   releases the golf balls, and pushes them all
//   to the top of our hill.
//
// 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 Arm = servo 2
// Grabber Right Arm = servo 3
// Botguy Grabber = servo 1
// Pom Remover = servo 0
//
// Need about 500 pixels to start camera tracking of house
// 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/orange, 1=yellow, 2=green
#define house 2
#define pom 0
#define botguy 0

#define startinglight 2

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

#define left_arm 2
#define right_arm 3
#define botguy_arm 1
#define botguy_motor 3
#define pom_arm 0

int phase1 = 1; // Drive to center, get balls and Botguy, then bottom of hill
int phase2 = 1; // Drive up the hill and remove poms from gated house

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

int navigatepid = 0; // Navigate process ID

int linegray = 60; // Greater is black
int leftlinecount = 0;  // Triggers for crossing lines
int rightlinecount = 0;

int house_min = 400;    // Minimum pixel count for a house
int pom_min = 900;      // Minimum pixel count for an orange pom
int slice_top = 100;    // Highest y value
int slice_bottom = 240; // Lowest y value
int x_min = 0;          // Left most x value
int x_max = 356;        // Right most x value
int x_center = 200;     // Center x value (normally 178)
int x_delta = 16;       // Plus/Minus value for location value
int target = 0;         // For driving control

int left_arm_open = 0;  // Arm positions
int left_arm_close = 220;
int right_arm_open = 233;
int right_arm_close = 13;

int botguy_front = 0;
int botguy_up = 120;
int botguy_grab = 28;
int botguy_back = 230;

int pom_in = 70;
int pom_out = 110;

int position = 0;  // Servo position
int rate = 0;  // Servo position rate

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

void main() {
    display_clear();
    printf("Golf Ball v7\n");
    msleep(200L);
    beep(); 
    msleep(200L);
    beep();
    msleep(200L);
    beep();
    msleep(200L);
    
    create = create_connect(); // Connect to the Create
    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);
    
    create_battery_charge();
    printf(" Create voltage=%d\n",gc_batt_voltage);
    printf(" XBC voltage=%f\n\n",power_level());
    
    // Test/set servos
    printf("Press A button to run servos\n");
    printf("  Press B to Bypass\n");
    while(!b_button()) {
        if (a_button()) { 
            set_servo_position(left_arm,left_arm_close);
            set_servo_position(right_arm,right_arm_close);
            set_servo_position(botguy_arm,botguy_front);
            set_servo_position(pom_arm,pom_out);
            enable_servos();
            set_servo_position(left_arm,left_arm_close);
            msleep(500L);
            set_servo_position(right_arm,right_arm_close);
            msleep(500L);
            set_servo_position(botguy_arm,botguy_front);
            msleep(500L);
            set_servo_position(pom_arm,pom_out);
            msleep(500L);
            disable_servos();
            break;
        }
    }
    display_clear();
    beep();
    msleep(200L);
    beep();
    msleep(200L);
    beep();
    msleep(200L);
    
    // Calibrate camera
    init_camera();
    track_set_minarea(house_min); 
    while(!b_button()) {
        track_update();
        display_clear();
        printf("Press A to Set Camera\n"); 
        printf("  Press B to Bypass\n");
        printf("  Default cent=%d, min=%d\n", x_center,house_min);
        printf("  House=%d x=%d y=%d s=%d\n",
               track_count(house),track_x(house,0),track_y(house,0),track_size(house,0));
        if (a_button()) { 
            if ((track_count(house)==0) || (track_size(house,0)<100) ||
                (track_x(house,0)<100) || (track_x(house,0)>250)) {
                printf("  Error with house\n");
                msleep(1000L); // Wait to see message
            } else {
                x_center = track_x(house,0);
                house_min = (track_size(house,0)-40);
                track_set_minarea(house_min); 
                break;
            }
        }
        msleep(200L); // Slow the refresh rate to read screen
    }
    display_clear();
    printf("House cent=%d, min=%d\n", x_center,house_min);
    msleep(200L);
    beep();
    msleep(200L);
    beep();
    msleep(200L);
    beep();
    
    /*
    // Select navigation phases
    printf("Press A for Phase 1\n");
    printf("  Press B to Bypass\n");
    while(phase1==0) {
        if (a_button()==1) {  // Do Phase 1
            phase1 = 1; 
            printf("    Do Phase 1\n");
        }
        if (b_button()==1) { // Bypass Phase 1
            phase1 = -1; 
            printf("    Bypass Phase 1\n");
            beep();
            msleep(100L);
        }
    }
    beep();
    msleep(500L);
    
    printf("Press A for Phase 2\n");
    printf("  Press B to Bypass\n");
    while(phase2==0) {
        if (a_button()==1) {  // Do Phase 2
            phase2 = 1; 
            printf("    Do Phase 2\n");
        }
        if (b_button()==1) { // Bypass Phase 2
            phase2 = -1; 
            printf("    Bypass Phase 2\n");
            beep();
            msleep(100L);
        }
    }
    beep();
    msleep(500L);
    */
    
    // Calibrate starting light
    printf("EVERYTHING OK?\n");
    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(160.0); // Max 180 seconds
    
    // Start navigate process
    navigatepid = start_process(navigate());
    msleep(200L);    
}

void navigate() {
    
    // Set Servos ----------------------------------
    set_servo_position(left_arm,left_arm_close);
    set_servo_position(right_arm,right_arm_close);
    set_servo_position(botguy_arm,botguy_front);
    set_servo_position(pom_arm,pom_out);
    enable_servos();
    set_servo_position(left_arm,left_arm_close);
    set_servo_position(right_arm,right_arm_close);
    set_servo_position(botguy_arm,botguy_front);
    set_servo_position(pom_arm,pom_out);
    
    // Phase 1 -- Get the golf balls and Botguy =========================================
    if (phase1==1) {
        
        printf("Phase 1\n");
        
        // Step 1 --------------------------------
        forward();
        msleep(500L); // Drop blocker bot
        stop();
        msleep(1500L); // Wait for blocker to leave   
        
        forward();
        endtime = mseconds() + 3200L;  // Set end time
        while (mseconds()<endtime) { 
            // Look for black line at the back
            if (analog(left_back_line)>linegray) break;
            if (analog(right_back_line)>linegray) break;
        }
        stop();
        left(39); // Rotate left (critical alignment)
        
        // Step 2 ---------------------------------
        set_servo_position(botguy_arm,botguy_up);
        msleep(500L);
        set_servo_position(pom_arm,pom_out);
        msleep(500L);
        
        // Dead reconning to Botguy
        // Measure Botguy hole to be sure he is in the middle.
        // Need to be able to run from either starting box
        forward();
        msleep(2000L); // Critical number
        stop();
        
        // Grab Botguy
        set_servo_position(botguy_arm,botguy_grab);
        msleep(500L); 
        mav(botguy_motor,999);
        msleep(4400L);
        freeze(botguy_motor);
        msleep(200L);
        set_servo_position(botguy_arm,botguy_up);
        msleep(4000L); // Let balls drop
        // Slowly put Botguy back
        rate = 2; // Movement rate
        for (position=botguy_up; position<botguy_back; position=position+rate) {
            set_servo_position(botguy_arm,position);
            msleep(20L);  // Wait for servo movement
        }
        set_servo_position(botguy_arm,botguy_back);
        msleep(1000L); // Continue to let balls drop
        mav(botguy_motor,0);
    }
    
    // Phase 2 -- Climb the hill with the golf balls =======================
    if (phase2==1) {
        
        printf("Phase 2\n");
        
        // Step 3 ---------------------------------
        right(51); // Rotate right
        forward();
        msleep(4300L); // Dead reconning
        stop();
        right(74); // Rotate right  
        
        // Step 4 --------------------------------
        house_locate(); // Locate house
        forward();
        msleep(2000L); // Over bump
        // Check house location as we climb hill
        track_update();
        if (track_count(house)>0) {
            if ((track_x(house,0)>(x_center+x_delta)) ||
                (track_x(house,0)<(x_center-x_delta))) {
                stop();
                house_locate(); // Locate house
                forward();
            }
        }
        endtime = mseconds() + 4000L;  // Set end time
        while (mseconds()<endtime) { 
            // Look for black line at the front
            if ((analog(left_line)>linegray)&&(analog(right_line)>linegray)) break;
        }
        stop();
        // Open arms 160 degrees
        set_servo_position(right_arm,right_arm_open);
        set_servo_position(left_arm,left_arm_open);
        msleep(500L);
        
        // Step 5 --------------------------------
        house_locate(); // Locate house
        forward();
        msleep(800L);  // Go past first line
        endtime = mseconds() + 1200L;  // Set end time
        while (mseconds()<endtime) { 
            // Look for black line at the front
            if ((analog(left_line)>linegray)&&(analog(right_line)>linegray)) break;
        }
        msleep(350L);  // Go past second line
        stop();
        house_locate(); // Locate house again
        
        // Pick pom from house 
        track_set_minarea(pom_min); 
        for (count=0;count<5;count++) {
            set_servo_position(pom_arm,pom_in);
            msleep(1000L);        
            set_servo_position(pom_arm,pom_out);
            msleep(500L);
            // If pom not on hook, move forward and try again
            track_update();
            if (track_count(pom)>0) {
                if ((track_x(pom,0)<(x_center+x_delta)) ||
                    (track_x(pom,0)>(x_center-x_delta))) {
                    printf("Pom c=%d x=%d y=%d s=%d\n",
                           track_count(pom),track_x(pom,0),track_y(pom,0),track_size(pom,0));
                    break;
                }
            }
            msleep(1500L);
        }
        
        // Push forward, move poms out and put Botguy in front
        forward();
        msleep(420L);  // Push some balls into gate area
        stop();
        set_servo_position(botguy_arm,botguy_front);
        msleep(3000L); 
        set_servo_position(pom_arm,pom_in);
        msleep(1000L);  
    }
    
    // Done ===========================================================
    // Drive motors off
    stop();
    // Release servos
    disable_servos();
    // Done with create
    create_disconnect();
    
    beep(); 
    msleep(200L);
    beep();
    msleep(200L);
    beep();
    msleep(200L);
    printf("Done!\n");
    
}   

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

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

// Forward Fast Function
void forward_fast() {
    create_drive_straight(400);
}

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

// Forward Left Angle Function
void forward_left() {
    create_drive_direct(150,135); 
}

// Forward Right Angle Function
void forward_right() {
    create_drive_direct(135,150); 
}

// Backward Left Angle Function
void backward_left() {
    create_drive_direct(-150,-130); 
}

// Backward Right Angle Function
void backward_right() {
    create_drive_direct(-130,-150);
}

// Right Spin Function
void right(int angle) {
    create_spin_block(150,-(angle)); 
    msleep(500L);
}

// Left Spin Function
void left(int angle) {
    create_spin_block(150,angle); 
    msleep(500L); 
}

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

// Locate House Function
void house_locate() {
    track_update();
    printf("House c=%d x=%d y=%d s=%d\n",
           track_count(house),track_x(house,0),track_y(house,0),track_size(house,0));
    // Rotate to center house
    if (track_count(house)>0) {
        if (track_x(house,0)>(x_center+x_delta)) {
            printf("  Rotate right=%d\n", ((track_x(house,0)-x_center)/16));
            right((track_x(house,0)-x_center)/16);  // (track_x(house,0)-x_center/16)
        } else {
            if (track_x(house,0)<(x_center-x_delta)) {
                printf("  Rotate left=%d\n", ((x_center-track_x(house,0))/16));
                left((x_center-track_x(house,0))/16); // (x_center-track_x(house,0)/16)
            } else {
                msleep(500L);   
            }
        }
    }
    //printf("  House c=%d x=%d y=%d s=%d\n",
    //       track_count(house),track_x(house,0),track_y(house,0),track_size(house,0));
}