#use "cmucamlib.ic"

//Robot Measurements in inches
//Robot wheel circumference = 10.21 to 10.25
//Wheel radius = 1.625
//Width of 2 wheels = 1.125
//Space between the centers of the two wheels = 4.875
//Space between insides of wheels = 3.75

float WHEEL_CIRC = 10.25;           // wheel circumference
float WHEEL_SPACING = 4.88;
int FORWARD = 0;
int BACK = 1;
int LEFT = 2;
int RIGHT = 3;
int DEG_PER_OD = 30;        // degrees per odometry change
float PI = 3.14;
float EPSILON = 0.01;
float SONAR_FACTOR = (32.768 / 0.29033);
#define NEST_DISTANCE 8.0
#define NEST_DISTANCE2 5.0
#define NEST_TOLERANCE 2.0
#define AIM_FUDGE_FACTOR 8.0
#define AIM_FUDGE_FACTOR2 5.0

//Direction motors are turning (fwd=1 bk=-1)
//Note: The left motor is backwards, thus when the wheel is turning
// forward, l_mtr_dir = -1.
int l_mtr_dir=0;
int r_mtr_dir=0;

int old_od_count[2] = {0, 0};
long old_time;
int od_count[2] = {0, 0};
float pos[3] = {0.0,0.0,0.0}; //x, y (inches) and theta (rad) according to odometry
int bump, frontbump, rearbump;  // bumped? in front? in rear?
int frontleft, frontright;

/* whether or not bump sensor code stops robot */
int self_preservation = 1;

int lines = 0;

void main(void)
{  
    //reset od_counters and position when robot restarts
    od_count[0] = 0;
    od_count[1] = 0;
    pos[0] = 0.0;
    pos[1] = 0.0;
    pos[2] = PI/2.0;
    init_expbd_servos(1);  // nonzero arg = servo controls enabled
    servo0 = 2400;
    sonar_init();
               //start_process(odometer(), 10, 50);
    //start_process(odometery());
    init_camera();
    clamp_camera_rgb();
    printf ("Done\n");
        
    while (!start_button());

    start_process(ir());
    start_process(bump_sensors(), 50, 50);
    
    dead_reckoning_sprint();
    move_to_nest();
    move_to_basket();
    
    // camera / sonar centering
    //printf("%d %d %f %f %d\n", od_count[0], od_count[1], pos[0], pos[1], (int)(180.0 / PI * pos[2]));
    //sleep(5.0);
    //printf("%f %f %f\n", pos[0], pos[1], 180.0 / PI * pos[2]);

    //  start_process(ir(), 1, 50);
    //  start_process(sonar_routine(), 1, 50);
}

void dead_reckoning_sprint()
{    
    //    old_time = mseconds();    
    //right_p = (int) (10.0 * 0.96);
    
    speed_arc(20.0, 60.0);
    /*
    int right_p = (int)(70.0 * 0.95);
    int left_p = (int)(20.0 * 1.05);
    motor(0, -right_p);
    r_mtr_dir = -1;
    motor(1, left_p);
    l_mtr_dir = 1;
    */
    
    sleep(1.2);
    //printf("%d %d %f %f %d\n", od_count[0], od_count[1], pos[0], pos[1], (int)(180.0 / PI * pos[2]));
          
    speed_forward(100.0);
    
    /* until we cross tape twice */
    while(lines < 4)
      ;
    sleep(0.25);
    
    stop();
}


void move_to_nest()
{
    int result;
    float d; 
    long time;
    //if (sonar_sweep(AIM_FUDGE_FACTOR))
    //
        //    do_distance(NEST_DISTANCE);
    
    int success = 0;
    
    speed_arc(15.0, -15.0);
    
    track_orange();
    while(track_confidence < 50)
      {
        track_orange();
    }
    do_distance(20.0);
    
    turn_left(45);
    speed_arc(12.0, -12.0);
    
    track_orange();
    while(track_confidence < 50) 
      {
        track_orange();
    }
    do_distance(NEST_DISTANCE2);    
    
    while (!success)
      {
        
        camera_sweep();
        turn_right(10);
        do_distance(NEST_DISTANCE2);    
        grab_shit();
        
        time = mseconds();
        turn_left(45);
        speed_arc(15.0, -15.0);
        
        track_orange();
        while(track_confidence < 50 && mseconds() < time + 4000L)
          {
            track_orange();
        }
        
        if (track_confidence >= 50)
          {
            
            printf("try again!");
            open_claw();
            sleep(1.0);
            off_claw();
        }
        else
          {
            stop();
            success = 1;
            printf("We're done, no orange");
        }
    }

   /*  while(1)
     {
        if (camera_sweep())
        {
   
            
            //while (1)
            
            //    track_orange();
            //    printf("%d\n", track_confidence);
            //}
            
            //grab_shit();
         }
            
         while (!start_button())
              ;   
     }
    //}
*/
    return;
}    

int sonar_sweep(float fudge_factor)
{
    int right_p, left_p, result;
    float d; 
    float best_reading = 100.0;
    long sweep_start_time;
    int sweep_done = 0;

    /* GET RIGHT ANGLE */

    turn_left(50);    // turn left 20-30 degrees
    
    printf("Starting sweep right\n");
    
     /* sweep right, looking for best value  */
    speed_arc(15.0, -15.0);
    
  /*
    right_p = (int)(15.0 * 0.95);
    left_p = (int)(15.0 * 1.05);
    motor(0, right_p);
    r_mtr_dir = 1;
    motor(1, left_p);
    l_mtr_dir = 1;
    */
    
    sweep_start_time = mseconds();
        
    while(mseconds() < sweep_start_time + 5000L)
    {
        result = sonar_sample();
        d = ((float)result / SONAR_FACTOR) - 12.0;
        if (d > 5.0 && d < 100.0 && d < best_reading) 
          best_reading = d;
    }
    
    stop();
    printf("Sweep done,     closest %f\n", best_reading); 
    
    /* sweep left again, re-finding best value */
    speed_arc(-15.0, 15.0);
    /*
    right_p = (int)(15.0 * 0.95);
    left_p = (int)(15.0 * 1.05);
    motor(0, -right_p);
    r_mtr_dir = -1;
    motor(1, -left_p);
    l_mtr_dir = -1;
    */
    
    sweep_start_time = mseconds();
    
    while(mseconds() < sweep_start_time + 5000L && !sweep_done)
    {
        result = sonar_sample();
        d = ((float)result / SONAR_FACTOR) - 12.0;
        if (d > 5.0 && d < best_reading + fudge_factor)
          sweep_done = 1;
    } 
    
    if (!sweep_done)
    {
        printf("Skunked!\n");
        stop();
        return 0;
    }  
    
    printf("Eureka! %f\n", d);
    stop();
    return 1;
}

int sonar_camera_sweep()
{
    int right_p, left_p, result;
    float d; 
    float cur_score;
    float best_score = 0.0;
    long sweep_start_time;
    int sweep_done = 0;

    /* GET RIGHT ANGLE */

    turn_left(50);    // turn left 20-30 degrees
    
    printf("Starting sweep right\n");
    
     /* sweep right, looking for best value  */
    speed_arc(15.0, -15.0);
    
  /*
    right_p = (int)(15.0 * 0.95);
    left_p = (int)(15.0 * 1.05);
    motor(0, right_p);
    r_mtr_dir = 1;
    motor(1, left_p);
    l_mtr_dir = 1;
    */
    
    sweep_start_time = mseconds();
        
    while(mseconds() < sweep_start_time + 5000L)
    {
        result = sonar_sample();
        track_orange();
        
        d = ((float)result / SONAR_FACTOR) - 12.0;
        
        cur_score = -d + (float) track_confidence / 10.0;
        
        if (d > 5.0 && d < 100.0 && cur_score < best_score) 
          best_score = cur_score;
    }
    
    stop();
    printf("Sweep done, bestscore %f\n", cur_score); 
    
    /* sweep left again, re-finding best value */
    speed_arc(-15.0, 15.0);
    /*
    right_p = (int)(15.0 * 0.95);
    left_p = (int)(15.0 * 1.05);
    motor(0, -right_p);
    r_mtr_dir = -1;
    motor(1, -left_p);
    l_mtr_dir = -1;
    */
    
    sweep_start_time = mseconds();
    
    while(mseconds() < sweep_start_time + 5000L && !sweep_done)
    {
        result = sonar_sample();
        track_orange();
        
        d = ((float)result / SONAR_FACTOR) - 12.0;
        cur_score = -d + (float) track_confidence / 10.0;
        
        if (d > 5.0 && cur_score < best_score + 5.0)
          sweep_done = 1;
    } 
    
    if (!sweep_done)
    {
        printf("Skunked!\n");
        stop();
        return 0;
    }  
    
    printf("Eureka! %f\n", cur_score);
    stop();
    return 1;
}

int camera_sweep()
{
    int cur_score;
    int best_score = 0;
    long sweep_start_time;
    int sweep_done = 0;

    /* GET RIGHT ANGLE */

    turn_left(50);    // turn left 20-30 degrees
    
    printf("Starting sweep right\n");
    
     /* sweep right, looking for best value  */
    speed_arc(15.0, -15.0);
    
  /*
    right_p = (int)(15.0 * 0.95);
    left_p = (int)(15.0 * 1.05);
    motor(0, right_p);
    r_mtr_dir = 1;
    motor(1, left_p);
    l_mtr_dir = 1;
    */
    
    sweep_start_time = mseconds();
        
    while(mseconds() < sweep_start_time + 4000L)
    {
        track_orange();
        
        cur_score = track_confidence;
        
        if (cur_score > best_score) 
          best_score = cur_score;
    }
    
    stop();
    printf("Sweep done, bestscore %d\n", cur_score); 
    
    /* sweep left again, re-finding best value */
    speed_arc(-15.0, 15.0);
    /*
    right_p = (int)(15.0 * 0.95);
    left_p = (int)(15.0 * 1.05);
    motor(0, -right_p);
    r_mtr_dir = -1;
    motor(1, -left_p);
    l_mtr_dir = -1;
    */
    
    sweep_start_time = mseconds();
    
    while(mseconds() < sweep_start_time + 5000L && !sweep_done)
    {
        track_orange();
        
        cur_score = track_confidence;
        
        if (cur_score > best_score - 40)
          sweep_done = 1;
    } 
    
    if (!sweep_done)
    {
        printf("Skunked!\n");
        stop();
        return 0;
    }  
    
    printf("Eureka! %d\n", cur_score);
    stop();
    return 1;
}

void move_to_basket()
{
    int result;
    float d; 
    long time;
    //if (sonar_sweep(AIM_FUDGE_FACTOR))
    //
        //    do_distance(NEST_DISTANCE);
    
    int success = 0;
    
    speed_arc(15.0, -15.0);
    
    track_green();
    while(track_confidence < 50)
      {
        track_green();
    }    
    do_distance(50.0);
    
    turn_left(45);
    speed_arc(12.0, -12.0);
    
    track_green();
    while(track_confidence < 50) 
      {
        track_green();
    }
    do_distance(NEST_DISTANCE);    
    
    while (!success)
      {
        
        camera_sweep_green();
        turn_right(10);
        do_distance(20.0);    
        dump_shit();
        
        time = mseconds();
        turn_left(45);
        speed_arc(15.0, -15.0);
        
        track_green();
        while(track_confidence < 50 && mseconds() < time + 4000L)
          {
            track_green();
        }
        
        if (track_confidence >= 50)
          {
            
            printf("try again!");
            //open_claw();
            //sleep(1.0);
            //off_claw();
            
            success = 1;
        }
        else
          {
            stop();
            success = 1;
            printf("We're done, no orange");
        }
    }

   /*  while(1)
     {
        if (camera_sweep())
        {
   
            
            //while (1)
            
            //    track_orange();
            //    printf("%d\n", track_confidence);
            //}
            
            //grab_shit();
         }
            
         while (!start_button())
              ;   
     }
    //}
*/
    return;    
}

int camera_sweep_green()
{
    int cur_score;
    int best_score = 0;
    long sweep_start_time;
    int sweep_done = 0;

    /* GET RIGHT ANGLE */

    turn_left(50);    // turn left 20-30 degrees
    
    printf("Starting sweep right\n");
    
     /* sweep right, looking for best value  */
    speed_arc(15.0, -15.0);
    
    sweep_start_time = mseconds();
        
    while(mseconds() < sweep_start_time + 4000L)
    {
        track_green();
        
        cur_score = track_confidence;
        
        if (cur_score > best_score) 
          best_score = cur_score;
    }
    
    stop();
    printf("Sweep done, bestscore %d\n", cur_score); 
    
    /* sweep left again, re-finding best value */
    speed_arc(-15.0, 15.0);
     
    sweep_start_time = mseconds();
    
    while(mseconds() < sweep_start_time + 5000L && !sweep_done)
    {
        track_green();
        
        cur_score = track_confidence;
        
        if (cur_score > best_score - 40)
          sweep_done = 1;
    } 
    
    if (!sweep_done)
    {
        printf("Skunked!\n");
        stop();
        return 0;
    }  
    
    printf("Eureka! %d\n", cur_score);
    stop();
    return 1;
}



void close_claw(int timetoclose)
{
    motor(2, -30);
}

void open_claw()
{
    motor(2, 20);
}

void off_claw()
{
    motor(2, 0);
}

/* GET RIGHT DISTANCE */
void do_distance (float new_distance)
{
    int result;
    long time;
    float d;
    
    self_preservation = 0;
    time = mseconds();
    while(mseconds() < time + 3000L)
    {
        result = sonar_sample();
        d = (float)(result)/SONAR_FACTOR - 12.0;
         
        if (d > 0.0 && d < 110.0)
        {
            printf ("Sonar: %f\n", d);
            
            if (d > new_distance + NEST_TOLERANCE)
            {  
                float compensation =  d - NEST_DISTANCE;
                printf("I'm too far awayDist=%f\n", d);
                if (compensation > 20.0) compensation = 20.0;
                if (compensation < 9.0) compensation = 9.0;
                
                if (frontleft)
                {
                    speed_arc(-60.0, -20.0);
                    sleep(0.3);
                }
                else if (frontright)
                {
                    speed_arc(-20.0, -60.0);
                    sleep(0.3);
                }                  
                else 
                {
                    speed_forward(compensation*1.5);
                }
                    //move_forward(compensation);
            }
            else if (d < new_distance - NEST_TOLERANCE && !rearbump)
            {
                //int compensation = (int) (NEST_DISTANCE - d);
                printf("I'm too close!  Dist=%f\n", d);               
                //compensation = 5;
           //     if (l_mtr_dir == 1)  // if moving forward stop first before jamming
                                     // motor into reverse
             //   {
               //     stop();                    
                 //   msleep(100L);
//                }                   
                speed_backward(15.0);
                msleep(50L);
                //move_backward(compensation);
              }
              else 
                {
                  printf("I'm done in distance\n");
                  stop();
                  self_preservation = 1;
                  return;
              }
        }
    }
    stop();
    printf("I ran out of time in distance\n");
    
}

void grab_shit()
{

    servo0=2400;
    sleep(2.5);
    servo0 = 1250;
    close_claw(5);
    sleep(3.0);        
    servo0 = 2400;
    sleep(2.0);
    //servo0 = 1400;
//    open_claw();
//    sleep(2.0);
    off_claw();
}

void dump_shit()
{
    servo0 = 1600;
    open_claw();
    sleep(2.0);        
    servo0 = 2400;
    sleep(2.0);
    //servo0 = 1400;
//    open_claw();
//    sleep(2.0);
    off_claw();
}


/*
void poop()
{
    
    while(1) 
      {
        //        if (track_orange() > 50) {
        // TC 180 240 20 150 16 18
        
        if (trackRaw (180, 240, 20, 150, 16, 18) > 0 && track_confidence > 30) {
            printf ("Found with size %d\n", track_size);
            if (track_size > 100 && (track_y > 5 || track_y < -5)) {
                stop();
                turn (track_y/2);
            } else if (track_size > 100)
                {
                  move_to_nest();
                  return();
              }
              else 
                {
                  speed (25);
              }
        }
        else 
          {
            printf ("you're stupid! %d\n", track_confidence);
            stop();            
        }
    }
}
*/


void sonar_routine()
{
    sonar_init();
    while(1) {
        int result = sonar_sample();
        float d = ((float)result / SONAR_FACTOR) - 12.0;
        if (d > 0.0 && d < 110.0) {
            //            printf ("Sonar: %f\n", d);
        }
        msleep (250L);
    }
}

void ir()
{
    int prev = 0;
    int reading = 0;
    
    while(1) {
        reading = analog(22);
        
        
        //printf("tophat: %d\n", reading);
        defer();
        
        
        if (prev == 0 && reading > 200)
        {
            ++lines;
            //printf ("Now black (%d), %d transitions\n", reading, lines);
            prev = 1;  
        }
        else if (prev == 1 && reading < 150)
        {
            ++lines;
            //printf ("Now white (%d), %d transitions\n", reading, lines);
            prev = 0;
        }
        defer();     
    }
}


void bump_sensors()
{
     while(1) {
        int rearleft = digital(13);
        int rearright = digital(14);
        frontleft = (analog(18) == 1);
        frontright = (analog(19) == 1);
        
        bump = rearleft | rearright | frontleft | frontright;
        rearbump = rearleft | rearright;
        frontbump = frontleft | frontright;
        
        if (bump)
          {
            if (self_preservation)
            {
                stop();
                if (frontleft)
                {
                    speed_arc(-60.0, -20.0);
                    hog_processor();               // oink!
                    sleep(0.2);
                    //hog_processor();
                    //sleep(0.2);
                }
                else if (frontright)
                {
                    speed_arc(-20.0, -60.0);
                    hog_processor();
                    sleep(0.2);
                    //hog_processor();
                    //sleep(0.2);
                }
            }
       
            /*
            printf("BUMP!           ");
            if (rearleft) printf("RL ");
            if (rearright) printf("RR ");
            if (frontleft) printf("FL ");
            if (frontright) printf("FR");
            printf("\n");
            */
        }
        
        defer();
    }
}

void odometer()
{
    int sensor[2] = { 11, 12 };
    int cur_od[2];
    cur_od[0] = digital(sensor[0]);
    cur_od[1] = digital(sensor[1]);
    while(1) {
        if(cur_od[0] != digital(sensor[0])) {
            cur_od[0] = digital(sensor[0]);
            
            // Instead of counting only ones count when the odometers change 
            // to double the resolution                       
            //There are 12 states on the odometry wheel thus 12 changes = 1 revolution
            od_count[0]++;            
        }
        if(cur_od[1] != digital(sensor[1])) {
            cur_od[1] = digital(sensor[1]);
            
            // Instead of counting only ones count when the odometers change 
            // to double the resolution                       
            //There are 12 states on the odometry wheel thus 12 changes = 1 revolution
            od_count[1]++;
        }
        defer();
    }
}

void odometery ()
{
    float dL, dR, diffLR, old_x, old_y, old_th, rICC, aICC;
    
    while (1) {
        while ((mseconds() - old_time) < (long)1000) {
            defer();
        }
        
        //Calculate the distance that each wheel traveled according to sensors
        dL = WHEEL_CIRC * (float)(-l_mtr_dir) * (float)(DEG_PER_OD * (od_count[0] - old_od_count[0]))/360.0;
        dR = WHEEL_CIRC * (float)r_mtr_dir * (float)(DEG_PER_OD * (od_count[1] - old_od_count[1]))/360.0;
        
        tone(400.0,0.1);
        
        //for calculation debugging    
        //printf("dL: %f, dR: %f\n", dL, dR);
        
        diffLR = dL - dR;
        
        //Get old robot position
        old_x = pos[0];
        old_y = pos[1];
        old_th = pos[2];
        
        //Calculate new position and orientation
        if (diffLR < EPSILON && diffLR > -EPSILON) {  //Traveling straight no change in th
            pos[0] = old_x + dL*cos(old_th);  //Update x
            pos[1] = old_y + dL*sin(old_th);  //Update y
        } else {  //Not straight path
            rICC = WHEEL_SPACING*(dR + dL)/diffLR;  //Radius from center of robot to ICC
            aICC = diffLR/WHEEL_SPACING;  //Angle that the robot rotated around the ICC in rad
            
            pos[0] = old_x + (rICC - rICC*cos(aICC));  //Update x
            pos[1] = old_y + (rICC * sin(aICC));  //Update y
            pos[2] = old_th + (PI/2.0 - aICC);  //Update th
        }
        
        //Store old od_count values for comparison
        old_od_count[0] = od_count[0];
        old_od_count[1] = od_count[1];
        
        old_time = mseconds();
        
        defer();
    }
}

void turn (int d)
{
    if (d > 0) {
        turn_left (d);
    } else {
        turn_right (-d);
    }
}

void turn_right (int d)
{
    long sleep_t = (long)(((float)d/90.0)*475.0);
    motor(0, 50);
    l_mtr_dir = -1;
    motor(1, 50);
    r_mtr_dir = -1;
    msleep(sleep_t);
    stop();
}

void turn_left (int d)
{
    long sleep_t = (long)(((float)d/90.0)*550.0);
    motor(0, -50);
    l_mtr_dir = 1;
    motor(1, -50);
    r_mtr_dir = 1;
    msleep(sleep_t);
    stop();
}

void move (int d)
{
    if (d > 0) {
        move_forward (d);
    } else {
        move_backward (-d);
    }
}

void move_forward (int d)
{
    long sleep_t = (long)(((float)d/10.0)*400.0);
    speed_forward (50.0);
    msleep(sleep_t);
    stop();
}

void move_backward (int d)
{
    long sleep_t = (long)(((float)d/10.0)*400.0);
    speed_backward (50.0);
    msleep(sleep_t);
    stop();
}

void speed_arc (float left, float right)
{
    int left_p = (int)(left * 1.00);
    int right_p = (int)(-right * 1.10);
    motor(1, left_p);
    l_mtr_dir = (left > 0.0);
    motor(0, right_p);
    r_mtr_dir = (right < 0.0);
    
}

void speed (float p)
{
    if (p > 0.0) {
        speed_forward (p);
    } else {
        speed_backward (-p);
    }
}

void speed_forward (float p)
{
    speed_arc(p, p);
    /*
    int right_p = (int)((float)p*1.10);
    int left_p = (int)((float)p*1.00);
    motor(0, -right_p);
    r_mtr_dir = -1;
    motor(1, left_p);
    l_mtr_dir = 1;
   */
}

void speed_backward (float p)
{
    speed_arc(-p, -p);
    /*    
    int left_p = (int)((float)p*0.8);
    int right_p = (int)((float)p*0.9);
    motor(0, right_p);
    r_mtr_dir = 1;
    motor(1, -left_p);
    l_mtr_dir = -1;
    */
}

void stop ()
{
    off(0);
    l_mtr_dir = 0;
    off(1);
    r_mtr_dir = 0;
}

void sonar_init() {
    bit_set(0x1026, 0x80);  /* set Digital Input #9 as output */
    bit_clear(0x1021, 1);   /* at TCTL2, */
    bit_set(0x1021, 2);     /* set TIC3 for falling edge */
}



int sonar_sample() {
    
    int start_time;
    int dist;
    
    poke(0x1023, 1);                /* clear tic3 flag */
    
    start_time= peekword(0x100e);   /* capture start time */
    
    bit_set(0x1000, 0x80);          /* send init pulse */
    bit_clear(0x1000, 0x80);
    
    while (peek(0x1000) & 0x1) {    /* wait until receive echo */
        if ((peekword(0x100e) - start_time) < 0) {
            /* if too much time has elapsed, abort */
            return -1;
        }
        defer();                     /* let others run while waiting */
    }
    
    msleep(10L);                    /* give unit time to reset */
    
    dist= peekword(0x1014)-start_time;
    
    return dist;                    /* tic3 has time of echo */
}


int track_green()
{
    if (trackRaw(0,80,100,255,0,80) > 0) {
        return track_confidence;
    } else return 0;
}