// Beyond Botball 2009 Bot B
//   Pickes up five houses from the competition
//   and takes them to our hill.
//
// Motor Left Front = motor 1
// Motor Right Front = motor 3
//
// Touch Left Front = digital 8
// Touch Right Front = digital 9
// Touch Left Back = digital 10
// Touch Right Back = digital 11
//
// Grabber Arm = servo 3
// Grabber Left Claw = servo 2
// Grabber Right Claw = servo 1
//
// 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, 1=yellow, 2=green
#define house 2
#define spot 1 // Purple 
#define botguy 0

#define leftmotor 1
#define rightmotor 3

#define startinglight 2

#define leftfrontline 3
#define centerfrontline 4
#define rightfrontline 5

#define leftfronttouch 8
#define rightfronttouch 9
#define leftbacktouch 10
#define rightbacktouch 11

#define grab_arm 3    
#define grab_left 2
#define grab_right 1  

int phase1 = 1; // Pick up houses
int phase2 = 1; // Drive to our hill

int cal = 0; // Calibrate start light

int navigatepid = 0; // Navigate process ID

int linegray = 60; // Greater is black

// Don't forget, image is upsidedown
int house_min = 400;    // Minimum pixel count for a house
int spot_min = 400;     // Minimum pixel count for a spot
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 = 160;     // Center x value (normally 178)
int x_delta = 16;       // Plus/Minus value for location value
int target = 0;         // For driving control

int rate = 0; // Servo move rate (set before loop)
int position = 0;  // Servo position

int grab_left_center = 175;  // Grabber servo positions
int grab_left_open = 110; 
int grab_left_close_right = 220;
int grab_left_open_right = 200;
int grab_left_close_left = 110;
int grab_left_open_left = 90;
int grab_left_close_center = 175;

int grab_right_center = 65;
int grab_right_open = 130;
int grab_right_close_right = 130; 
int grab_right_open_right = 150;
int grab_right_close_left = 20; 
int grab_right_open_left = 45;
int grab_right_close_center = 65;

int grab_arm_down = 0;
int grab_arm_hold = 40;
int grab_arm_up = 230;
int grab_arm_half = 150;

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

// Main Controller ===================================================
void main() {
    display_clear();
    printf("House Grabber v1\n");
    printf(" XBC voltage=%f\n\n",power_level());
    beep(); 
    msleep(200L);
    beep();
    msleep(200L);
    beep();
    msleep(200L);
    
    // 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(grab_left,grab_left_center);
            set_servo_position(grab_right,grab_right_center);
            set_servo_position(grab_arm,grab_arm_half);
            enable_servos();
            set_servo_position(grab_left,grab_left_center);
            set_servo_position(grab_right,grab_right_center);
            msleep(500L);
            set_servo_position(grab_arm,grab_arm_half);
            msleep(500L);
            set_servo_position(grab_arm,grab_arm_up);
            msleep(1000L);
            set_servo_position(grab_arm,grab_arm_down);
            msleep(1000L);
            set_servo_position(grab_left,grab_left_open);
            set_servo_position(grab_right,grab_right_open);
            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);
    beep();
    msleep(200L);
    beep();
    msleep(200L);
    beep();
    msleep(200L);
    
    /*
    // Pick up houses
    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(200L);
    beep();
    msleep(200L);
    beep();
    msleep(200L);
    
    // Take houses to our hill
    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(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(200L);
            beep();
            msleep(200L);
            beep();
        }
    }
    
    // Start shut down process
    shut_down_in(170.0);  // Max 180 seconds
    
    // Start navigate process
    navigatepid = start_process(navigate());
    msleep(200L);    
}

void navigate() {
    
    // Phase 1 -- Pick up houses =========================================
    if (phase1==1) {
        
        // Wait for other bots to clear
        msleep(1000L); // A little delay
        
        // Push down ramp
        set_servo_position(grab_left,grab_left_center);
        set_servo_position(grab_left,grab_right_center);
        set_servo_position(grab_arm,grab_arm_half);
        enable_servos();
        set_servo_position(grab_left,grab_left_center);
        set_servo_position(grab_right,grab_right_center);
        msleep(1000L);
        set_servo_position(grab_arm,grab_arm_down);
        msleep(1500L);
        set_servo_position(grab_arm,grab_arm_half);
        msleep(1000L);
        
        // Drive to start white ramp
        forward();
        endtime = mseconds() + 2500L;  // Set end time
        while (mseconds()<endtime) {
            if ((analog(leftfrontline)<linegray) &&
                (analog(rightfrontline)<linegray)) break;
            if (analog(leftfrontline)<linegray) mav(leftmotor,0); 
            if (analog(rightfrontline)<linegray) mav(rightmotor,0);
        }
        stop();
        
        // Drive to competition hill 
        forward();
        msleep(5000L);
        endtime = mseconds() + 5000L;  // Set end time
        while (mseconds()<endtime) {
            // Stop if front touch
            if ((digital(leftfronttouch)==1)||(digital(rightfronttouch)==1)) break;
        }
        stop();
        
        // Rotate 90 degrees to right
        right(90L);
        // Backward align to pipe
        align_rear(900L);
        
        // Drive to middle black line
        forward();
        endtime = mseconds() + 4000L;  // Set end time
        while (mseconds()<endtime) {
            if (analog(leftfrontline)>linegray) break;
            if (analog(rightfrontline)>linegray) break;
        }
        msleep(900L);
        stop();
        
        // Rotate 90 degrees to left
        left(90L);
        
        // Get first house
        set_grabber();
        house_locate();
        forward();
        msleep(3500L);
        stop();
        house_locate();
        drive_house(3000L);
        grab_house1();
        
        // Position for second house
        right(59L);
        forward();
        endtime = mseconds() + 3800L;  // Set end time
        while (mseconds()<endtime) {
            if (digital(leftfronttouch)==1) break;
            if (digital(rightfronttouch)==1) break;
        }
        stop();
        right(42L);
        
        // Get second house
        set_grabber();
        house_locate();
        drive_house(1500L);
        grab_house2();
        
        // Position for third house
        right(68L);
        align_rear(1500L);
        forward();
        msleep(800L);
        stop();
        right(85L);
        forward();
        msleep(4950L);
        stop();
        right(37L);
        forward();
        endtime = mseconds() + 3000L;  // Set end time
        while (mseconds()<endtime) {
            if (digital(leftfronttouch)==1) break;
            if (digital(rightfronttouch)==1) break;
        }
        stop();         
        left(50L);
        
        // Get third house
        set_grabber();
        house_locate();
        drive_house(1000L);
        grab_house3();
        
        // Position for drive to other side
        left(70L);
        align_rear(1500L);
        forward();
        msleep(800L);
        stop();
        left(85L);
        forward();
        msleep(3000L);
        stop();
        right(85L);
    }
    
    // Phase 2 -- Take houses to our hill ==================================
    if (phase2==1) {
        // Drive to first spot
        track_set_minarea(spot_min); 
        spot_locate();
        forward();
        msleep(5000L);
        stop();
        spot_locate();
        forward();
        msleep(5000L);
        stop();
        spot_locate();
        //drive_spot(4000L);
        left(10L);
        forward();
        endtime = mseconds() + 4000L;  // Set end time
        while (mseconds()<endtime) {
            if (digital(leftfronttouch)==1) break;
            if (digital(rightfronttouch)==1) break;
        }
        stop();
        backward();
        msleep(500L);
        stop();
        
        // Drive to second spot
        left(80L);
        forward(); // Dead reconning to start of our hill
        msleep(11920L); 
        stop();
        right(75L);
        spot_locate();
        forward();
        msleep(3000L);
        stop();
        spot_locate();
        forward();
        msleep(3000L);
        stop();
        spot_locate();
        //drive_spot(4000L);
        forward();
        endtime = mseconds() + 3000L;  // Set end time
        while (mseconds()<endtime) {
            if ((analog(leftfrontline)>linegray) &&
                (analog(rightfrontline)>linegray)) break;
        }
        stop();
        left(36L);
        forward();
        msleep(2500L);
        endtime = mseconds() + 1500L;  // Set end time
        while (mseconds()<endtime) {
            if (digital(leftfronttouch)==1) break;
            if (digital(rightfronttouch)==1) break;
        }
        stop();
        backward();
        msleep(500L);
        stop();
        right(32L);
        forward();
        msleep(2000L);
        stop();
        deposit_houses();
    }
    
    // Done ===========================================================
    // Servos and motors off
    disable_servos(); 
    stop();
    beep(); 
    msleep(200L);
    beep();
    msleep(200L);
    beep();
    msleep(200L);
    printf("Done!\n");
}   

// Drive Functions ==========================================================

// Forward Function
void forward() {
    mav(leftmotor,915); mav(rightmotor,900);
}

// Forward Left Angle Function
void forward_left() {
    mav(leftmotor,820); mav(rightmotor,900);
}

// Forward Right Angle Function
void forward_right() {
    mav(leftmotor,900); mav(rightmotor,820);
}

// Backward Function
void backward() {
    mav(leftmotor,-915); mav(rightmotor,-900);
}

// Right Backward Sweep Function
void right(long angle) {
    mrp(rightmotor,600,-(angle*11L));
    bmd(rightmotor); 
    msleep(200L); 
}

// Left Backwards Sweep Function
void left(long angle) {
    mrp(leftmotor,600,-(angle*11L));
    bmd(leftmotor); 
    msleep(200L); 
}

// Stop Function
void stop() {
    mav(leftmotor,0); mav(rightmotor,0);
    msleep(400L); 
}

// Align to pipe in rear
void align_rear(long maxtime) {
    endtime = mseconds() + maxtime;  // Set end time
    backward();
    while (mseconds()<endtime) {
        if ((digital(leftbacktouch)==1)||(digital(rightbacktouch)==1)) break;
    }
    msleep(200L); // Go a little more to align with pipe
    stop();
}

// Drive to a house
void drive_house(long maxtime) {
    forward();
    endtime = mseconds() + maxtime;  // Set end time
    while (mseconds()<endtime) {
        // Go until house disappears
        track_update();
        if (track_count(house)>0) {
            if (track_x(house,0)>(x_center+x_delta)) {
                forward_left();
            } else {
                if (track_x(house,0)<(x_center-x_delta)) {
                    forward_right();
                }
            }
        } else break;
        msleep(50L);  
    }
    forward();
    msleep(190L);
    stop();
}

// Drive to a spot
void drive_spot(long maxtime) {
    forward();
    endtime = mseconds() + maxtime;  // Set end time
    while (mseconds()<endtime) {
        // Drive until spot disappears or front touch 
        track_update();
        if (track_count(spot)>0) {
            if (track_x(spot,0)>(x_center+x_delta)) {
                forward_left();
            } else {
                if (track_x(spot,0)<(x_center-x_delta)) {
                    forward_right();
                }
            }
        }
        // No disappear break
        if ((digital(leftfronttouch)==1)||(digital(rightfronttouch)==1)) break;
        msleep(100L); 
    }
    forward();
    msleep(10L);
    stop();
}


// Locate Function =========================================================
// Locate House
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 left=%d\n", ((track_x(house,0)-x_center)/15));
            left((long)((track_x(house,0)-x_center)/15));  // (track_x(house,0)-x_center/15)
        } else {
            if (track_x(house,0)<(x_center-x_delta)) {
                printf("  Rotate right=%d\n", ((x_center-track_x(house,0))/15));
                right((long)((x_center-track_x(house,0))/15)); // (x_center-track_x(house,0)/15)
            } else {
                msleep(500L);   
            }
        }
    }
}

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


// House Grabber Functions ========================================================

// Set grabber to get house
void set_grabber() {
    set_servo_position(grab_left,grab_left_open);
    set_servo_position(grab_right,grab_right_open);
    msleep(200L);
    set_servo_position(grab_arm,grab_arm_down);
    msleep(1000L);
}

// House arm up
void up_house(int up_position) {
    // Arm up slowly
    rate = 2; // Movement rate
    for (position=grab_arm_down; position<up_position; position=position+rate) {
        set_servo_position(grab_arm,position);
        msleep(10L);  // Wait for servo movement
    }
    set_servo_position(grab_arm,up_position);
    msleep(200L);
}

// House Grab 1
void grab_house1() {
    // Pick up house to right storage
    set_servo_position(grab_left,grab_left_close_right);
    set_servo_position(grab_right,grab_right_close_right);
    msleep(1000L);
    up_house(grab_arm_up);
    set_servo_position(grab_left,grab_left_open_right);
    set_servo_position(grab_right,grab_right_open_right);
    msleep(1000L);
    set_servo_position(grab_arm,grab_arm_half);
    msleep(1000L);
    set_servo_position(grab_left,grab_left_open);
    set_servo_position(grab_right,grab_right_open);
    msleep(500L);
}

// House Grab 2
void grab_house2() {
    // Pick up house to left storage
    set_servo_position(grab_left,grab_left_close_left);
    set_servo_position(grab_right,grab_right_close_left);
    msleep(1000L);
    backward();
    msleep(400L);
    stop();
    set_servo_position(grab_left,grab_left_open);
    set_servo_position(grab_right,grab_right_open_left);
    msleep(500L);
    set_servo_position(grab_arm,grab_arm_down);
    msleep(500L);
    // Pick up house to left storage
    set_servo_position(grab_left,grab_left_close_left);
    set_servo_position(grab_right,grab_right_close_left);
    msleep(1000L);
    up_house(grab_arm_up);
    set_servo_position(grab_left,grab_left_open_left);
    set_servo_position(grab_right,grab_right_open_left);
    msleep(1000L);
    set_servo_position(grab_arm,grab_arm_half);
    msleep(1000L);
    set_servo_position(grab_left,grab_left_open);
    set_servo_position(grab_right,grab_right_open);
    msleep(500L);
}

// House Grab 3
void grab_house3() {
    // Pick up house to center storage
    set_servo_position(grab_left,grab_left_close_center);
    set_servo_position(grab_right,grab_right_close_center);
    msleep(1000L);
    //up_house(grab_arm_hold);
    backward();
    msleep(400L);
    stop();
    set_servo_position(grab_left,grab_left_open);
    set_servo_position(grab_right,grab_right_open);
    msleep(500L);
    set_servo_position(grab_arm,grab_arm_down);
    msleep(500L);
    // Pick up house to center storage
    set_servo_position(grab_left,grab_left_close_center);
    set_servo_position(grab_right,grab_right_close_center);
    msleep(1000L);
    up_house(grab_arm_hold);
}


// House Deposit
void deposit_houses() {
    // Deposit third house
    set_servo_position(grab_arm,grab_arm_down);
    msleep(1000L);
    set_servo_position(grab_left,grab_left_open);
    set_servo_position(grab_right,grab_right_open);
    msleep(1000L);
    backward();
    msleep(500L);
    stop();
    
    // Deposit second house
    set_servo_position(grab_arm,grab_arm_half);
    msleep(1000L);
    left(10L);
    //forward();
    //msleep(200L);
    //stop();
    set_servo_position(grab_left,grab_left_open_left);
    set_servo_position(grab_right,grab_right_open_left);
    msleep(500L);
    set_servo_position(grab_arm,grab_arm_up);
    msleep(1000L);
    set_servo_position(grab_left,grab_left_close_left);
    set_servo_position(grab_right,grab_right_close_left);
    msleep(500L);
    set_servo_position(grab_arm,grab_arm_down);
    msleep(1000L);
    set_servo_position(grab_right,grab_right_open_left);
    msleep(1000L);
    
    // Deposit first house
    set_servo_position(grab_arm,grab_arm_half);
    msleep(1000L);
    right(20L);
    forward();
    msleep(400L);
    stop();
    set_servo_position(grab_left,grab_left_open_right);
    set_servo_position(grab_right,grab_right_open_right);
    msleep(500L);
    set_servo_position(grab_arm,grab_arm_up);
    msleep(1000L);
    set_servo_position(grab_left,grab_left_close_right);
    set_servo_position(grab_right,grab_right_close_right);
    msleep(500L);
    set_servo_position(grab_arm,grab_arm_down);
    msleep(1000L);
    set_servo_position(grab_left,grab_left_open_right);
    msleep(1000L);
}