#define WHITE 0
#define BLACK 1
#define RED 0
#define GREEN 1
#define TL_PORT 4
#define TR_PORT 5
#define BR_PORT 6
#define M_180_PI 57.29577950
#define CM_PER_RF 0.3048
#define STALL_MOVING 0
#define STALL_TURNING 1
#define STALL_NONE 2
#define AXIS_X 0
#define AXIS_Y 1

int i_theta;
int ball_pid, update_pid, menu_pid, stall_pid;
int servo_sorter = 0;
persistent int equilibrium_period = 2180;
persistent int cutoff = 150;
persistent int orientation_cutoff = 190;
persistent int intake_speed = 100;

int sorter_sensor_port = 3;
int ball_motor_port = 4;
int green_gate_motor_port = 1;
int red_gate_motor_port = 3;
int ball_push_switch_port = 8;
int to_period;
int display_option = 0;
int dump_angle = 80;
int start_side;
int rf_num = 0;
long epoch;
int pos_x,pos_y,opp_x,opp_y,tar_x,tar_y;

#define PATH_SIZE 4

int path[PATH_SIZE][5] = {
    {0, 250, 100, 0, 0}, {250, 250, 200, 0, 0}, {-250, 250, 300, 0, 0}, {-250, -250, 400, 0, 0}
};
int step = 0;

#define digital_hi(x) !digital(x)
#define digital_lo(x) digital(x)


#define STALL_HISTORY 10
#define STALL_PERIOD 20

int stall_option; /* 0 = stall while moving, 1 = stall while turning, 2 = stall while stopping (off) */
int handling_stall = 0;
int stall_samples_x[STALL_HISTORY];
int stall_samples_y[STALL_HISTORY];
float stall_samples_theta[STALL_HISTORY];
int stall_marker = 0;
long last_stall_update;
int num_updates = 0;
int last_stall_index = 0;


#define LINE_ANY 0
#define LINE_LEFT 1
#define LINE_RIGHT 2

void main()
{
    float tmp;
    int i;
    int reflection_x, reflection_y;
    long mtime;

    calibration();
    calibrate_gyro();
    
    rf_team = 7;
    rf_enable();
    start_machine();
    get_side();

    update_pid = start_process(update());
    init_servos();
    update_ball_pickup();
    start_process(ball_sort());
    if(start_side==BLACK){
      go_angle(270.0,GLOBAL);
    }else{/*on white*/
      go_angle(90.0,GLOBAL);      
    }

    go_time(1000L);
    mtime = mseconds();
    while(mseconds()-mtime<500L) {};
    
    if(start_side==BLACK){
      go_angle(90.0,GLOBAL);
    }else{/*on white*/
      go_angle(270.0,GLOBAL);      
    }
    
    go_time(3500L);

    go_time(100L);
    /*wait_for_rf_update();*/
    while(mseconds()<30000L); /*wait until the last 10 seconds of the round*/
    get_pos();
    if(opp_x>=0) go_angle(0.0,GLOBAL);
    else         go_angle(180.0,GLOBAL);

    /*open the appropriate gate*/
    if(rf_vote_winner==RED)
      open_gate(RED);
    else
      open_gate(GREEN);

    go_time(3000L);
    
    if(start_side==BLACK){
      go_angle(90.0,GLOBAL);
    }else{/*on white*/
      go_angle(270.0,GLOBAL);      
    }

    go_time(1000L);
    ao();
    
    return;

    init_servos();
    update_ball_pickup();
    start_process(ball_sort());
    theta = 0.0;
    go_time(1000L);
    backup_time(250L);
    msleep(1000L);
    go_angle(-90.0, GLOBAL);
    go_time(1500L);
    go_to_line(LINE_ANY);
    msleep(1000L);
    backup_time(400L);
    stop_ball_pickup();
    go_time(1000L);
    theta = -90.0;
    backup_time(250L);
    update_ball_pickup();
    go_angle(180.0, GLOBAL);
    go_to_line(LINE_RIGHT);
    msleep(1000L);
    backup_time(400L);
    go_angle(90.0, LOCAL);
    go_to_line(LINE_RIGHT);
    msleep(1000L);
    backup_time(400L);
    while (1) {
        printf("%f\n", theta);
        msleep(250L);
    }
        /*rf_team = 7;
          rf_enable();*/
    calibrate_gyro();
    update_pid = start_process(update());
    init_servos();

        /*start_machine();
          printf("%d %d %d\n", analog(TL_PORT), analog(TR_PORT), analog(BR_PORT));
          get_side();
          get_rf_number();
          msleep(1000L);
          update_ball_pickup();
          start_process(ball_sort()); */
    epoch = mseconds();
    stall_option = STALL_NONE;
        /*****************Main Code...*******************/

    return;
}


void update()
{
    long delta_t_ms, new_time_ms;
    float delta;

    time_ms = mseconds();

    for(;;){
      /*hog_processor();*/
        new_time_ms = mseconds();
        delta_t_ms = (new_time_ms - time_ms);
        delta = ((float)analog(gyro_port) - offset) * (float)delta_t_ms;
        delta = delta / lsb_ms_per_deg;
        theta -= delta;
        time_ms = new_time_ms;
        if (theta > 360.0) {
            theta -= 360.0;
        } else if (theta < 0.0) {
            theta += 360.0;
        }
        i_theta = (int)(theta*10.0);
            /*get_pos();
              get_opp();*/
    }
}


void stall_detector()
{
    int dx, dy;
    float dt;
    for (;;) {
        if (mseconds() - last_stall_update > 200L && position_updates > last_stall_index && mseconds() - epoch > 5000L) {
            int check, i, stalled = 1;
            last_stall_index = position_updates;
            last_stall_update = mseconds();
            stall_samples_x[stall_marker] = pos_x;
            stall_samples_y[stall_marker] = pos_y;
            stall_samples_theta[stall_marker] = theta;
            if (num_updates >= STALL_PERIOD) {
                check = stall_marker;
                for (i = 0; i < STALL_PERIOD; i++) {
                    check--;
                    if (check < 0) check += STALL_HISTORY;
                    dx = stall_samples_x[check] - stall_samples_x[stall_marker];
                    dy = stall_samples_y[check] - stall_samples_y[stall_marker];
                    dt = stall_samples_theta[check] - stall_samples_theta[stall_marker];
                    if (dx < 0) dx = -dx;
                    if (dy < 0) dy = -dy;
                    if (dt < 0.0) dt = -dt;
                
                    if (stall_option == STALL_MOVING) { /* stalled moving */
                        if (dx > 5) {
                            stalled = 0;
                        }
                        if (dy > 5) {
                            stalled = 0;
                        }
                    } else if (stall_option == STALL_TURNING) { /* stalled turning */
                        if (dt > 5.0) {
                            stalled = 0;
                        }
                    } else if (stall_option == STALL_NONE) {
                        stalled = 0;
                    }
                }
                if (stalled == 1) {
                    if (stall_option == STALL_MOVING) { /* stalled moving */
                        printf("Handling stall! (moving)\n");
                        stop_ball_pickup();
                        handling_stall = 1;
                        motor(left, -100);
                        motor(right, -20);
                        msleep(1000L);
                        motor(left, -20);
                        motor(right, -100);
                        msleep(1000L);
                        update_ball_pickup();
                    } else if (stall_option == STALL_TURNING) { /* stalled turning */
                        printf("Handling stall! (turn)\n");
                        stop_ball_pickup();
                        handling_stall = 1;
                        motor(left, -100);
                        motor(right, -100);
                        msleep(1000L);
                        update_ball_pickup();
                    } else if (stall_option == STALL_NONE) { /* just sitting, no stall */
                            /* do nothing */
                    }
                    handling_stall = 0;
                }
            }
            num_updates++;
            stall_marker++;
            if (stall_marker >= STALL_HISTORY) stall_marker -= STALL_HISTORY;
        }
        defer();
    }
}

float reflect_wrt_y(float angle, int reflect)
{
    return ((float)reflect*(angle-90.0))+90.0;
}

float reflect_wrt_x(float angle, int reflect)
{
    return (float)reflect*angle;
}

#define DEBUG 1

void wait_for_rf_update()
{
    int current_update;

    current_update = position_updates;
    while(current_update == position_updates) {
        defer();
    }
}

void display_angle()
{
    while(1) {
        printf("%f\n", theta);
        msleep(150L);
    }
}

void calibration()
{
    int button, val0, val1, proceed = 0;
    printf("Proceed with calibration?\n");
    msleep(1000L);
    while (1) {
        button = (stop_button() << 1) | start_button();
        if (button) break;
    }
    if (button > 1) return;
    while (!proceed) {
      printf("Put red ball on.\n");
      msleep(1000L);
      while (1) {
	button = (stop_button() << 1) | start_button();
	if (button) break;
        }
      if (button & 0x10) {
            proceed = 1;
      }
        if (button & 0x01) {
            val0 = analog(sorter_sensor_port);
            printf("Put green ball on.\n");
            msleep(1000L);
            while (1) {
                button = (stop_button() << 1) | start_button();
                if (button) break;
            }
            if (button & 0x01) {
                val1 = analog(sorter_sensor_port);
                printf("Spread is %d, okay? (>100 good)\n", val1-val0);
                msleep(1000L);
                while (1) {
                    button = (stop_button() << 1) | start_button();
                    if (button) break;
                }
                if (button & 0x01) {
                    cutoff = (val1+val0)/2;
                    proceed = 1;
                }
            }
        }
    }
    proceed = 0;
    while (!proceed) {
        printf("Top right on black, bottom right on white.\n");
        msleep(1000L);
        while (1) {
            button = (stop_button() << 1) | start_button();
            if (button) break;
        }
        if (button & 0x10) {
            proceed = 1;
        }
        if (button & 0x01) {
            val0 = analog(BR_PORT);
            val1 = analog(TR_PORT);

            printf("Spread is %d, okay? (>100 good)\n", val1-val0);
            msleep(1000L);
            while (1) {
                button = (stop_button() << 1) | start_button();
                if (button) break;
            }
            if (button & 0x01) {
                cutoff = (val1+val0)/2;
                proceed = 1;
            }
        }
    }
}

void update_ball_pickup()
{
    motor(4,intake_speed);
    motor(5,intake_speed);
}

void stop_ball_pickup(){
    off(4);
    off(5);
}

void init_servos()
{
    long time;
    long timeout = 500L;

    time = mseconds();

    enable_servos();
    while(mseconds()-time < timeout){
        servo(servo_sorter, equilibrium_period);
        defer();
    }
    disable_servos();
}

void ball_sort()
{
    long time;
    long timeout = 250L; /*time to stay at angle*/

    enable_servos();
    for(;;){   
        if(digital_lo(ball_push_switch_port)){

            int light_val;

            msleep(500L); /*wait a bit for ball to settle*/      

            light_val = analog(sorter_sensor_port);
            printf("Lightval:%d\n",light_val);
            if (light_val < cutoff) {
                to_period = equilibrium_period + dump_angle*22;
                if (to_period > 4000) to_period = 4000;
            } else { 
                to_period = equilibrium_period - dump_angle*22;
                if (to_period < 0) to_period = 0;
            }

            time = mseconds();


            while(mseconds()-time < timeout){
                servo(servo_sorter, to_period);
                defer();
            }

            to_period = equilibrium_period;
            time = mseconds();

            while(mseconds()-time < timeout){
                servo(servo_sorter, to_period);
                defer();
            }
                /*disable_servos();*/
        }
        servo(servo_sorter, equilibrium_period);
    }
}

void open_gate(int color)
{
    if (color == RED) {
        motor(red_gate_motor_port, 20);
        msleep(250L);
        motor(red_gate_motor_port, 0);
    } else {
        motor(green_gate_motor_port, 20);
        msleep(250L);
        motor(green_gate_motor_port, 0);
    }
}

void close_gate(int color)
{
    if (color == RED) {
        motor(red_gate_motor_port, -20);
        msleep(250L);
        motor(red_gate_motor_port, 0);
    } else {
        motor(green_gate_motor_port, -20);
        msleep(250L);
        motor(red_gate_motor_port, 0);
    }
}

int sensor_color(int port)
{
    if (analog(port) > orientation_cutoff) return 1;
    return 0;
}

void get_side()
{
    if (sensor_color(TL_PORT) + sensor_color(TR_PORT) + sensor_color(BR_PORT) >= 2) {
        start_side = BLACK;
    }

    if (start_side == BLACK) {
        printf("black: ");
        if (sensor_color(TR_PORT) == WHITE) {
                /* facing forward */
            printf("Facing forward\n");
            theta = 270.0;
        } else if (sensor_color(TL_PORT) == WHITE) {
                /* facing right */
            printf("Facing right\n");
            theta = 180.0;
        } else if (sensor_color(BR_PORT) == WHITE) {
                /* facing left */
            printf("Facing left\n");
            theta = 0.0;
        } else {
                /* facing backward */
            printf("Facing backward\n");
            theta = 90.0;
        }
    } else {
        printf("On white: ");
        if (sensor_color(TR_PORT) == BLACK) {
                /* facing left */
            printf("Facing left\n");
            theta = 180.0;
        } else if (sensor_color(TL_PORT) == BLACK) {
                /* facing forward */
            printf("Facing forward\n");
            theta = 90.0;
        } else if (sensor_color(BR_PORT) == BLACK) {
                /* facing backward */
            printf("Facing backward\n");
            theta = 270.0;
        } else {
                /* facing right */
            printf("Facing right\n");
            theta = 0.0;
        }
    }
}

void get_rf_number()
{
    if(rf_y0 > 0 && start_side == BLACK || rf_y0 < 0 && start_side == WHITE){
        rf_num = 0;
    }
    else{
        rf_num = 1;
    }
}

int last_index = 0;
int recalculate = 0;

void get_pos()
{
    long current_time = mseconds();
    if (position_updates != last_index) {        
        if(rf_num==0){
            pos_x = rf_x0;
            pos_y = rf_y0;
        } else {
            pos_x = rf_x1;
            pos_y = rf_y1;
        }
        last_index = position_updates;
        recalculate = 1;
    }
}

void get_opp()
{
    if(rf_num==1){
        opp_x = rf_x0;
        opp_y = rf_y0;
    }
    else{
        opp_x = rf_x1;
        opp_y = rf_y1;
    }
}

#define FAST 40
#define SLOW 30

void go_to_line(int mode)
{
    int tweak = 15;
    int speed = FAST;
        /*float angle = theta;*/
    int i_angle;
    long last_jolt = 0L;
    int hi, lo;
    float tolerance = 0.5;

    i_angle = (int)(theta*10.0);
    hi = i_angle + 5;
    lo = i_angle - 5;
    last_jolt = mseconds();
    while (1) {
        if (mseconds() - last_jolt > 2000L) {
            motor(left, -100);
            motor(right, -100);
            msleep(100L);
            motor(left, 0);
            motor(right, 0);
            msleep(500L);
            last_jolt = mseconds();
        }
        if (mode == LINE_ANY) {
            if (analog(TR_PORT) > 200 || analog(TL_PORT) > 200) {
                break;
            }
        } else if (mode == LINE_LEFT) {
            if (analog(TL_PORT) > 200) {
                break;
            }
        } else if (mode == LINE_RIGHT) {            
            if (analog(TR_PORT) > 200) {
                break;
            }
        }
        if (i_theta < lo) {
            motor(left, speed - tweak);
            motor(right, speed + tweak);
        } else if (i_theta > hi) {
            motor(left, speed + tweak);
            motor(right, speed - tweak);
        } else {
            motor(left, speed);
            motor(right, speed);
        }
            /*if (theta < (angle - tolerance)) {
            motor(left, speed - tweak);
            motor(right, speed + tweak);
        } else if (theta > (angle + tolerance)){
            motor(left, speed + tweak);
            motor(right, speed - tweak);
        } else {
            motor(left, speed);
            motor(right, speed);
        }*/
        defer();
    }
            /* You can check a sensor here and terminate the while loop */
    stall_option = STALL_NONE;
    motor(left, -100);
    motor(right, -100);
    msleep(100L);
    motor(left, 0);
    motor(right, 0);

}

void go_time(long max_time)
{
    int tweak = 15;
    int speed = FAST;
    float angle = theta;
    float tolerance = 0.5;
    long start_time = mseconds();

    while (mseconds() - start_time < max_time) {
        if (theta < (angle - tolerance)) {
                /* Turn CCW a little */
            motor(left, speed - tweak);
            motor(right, speed + tweak);
        } else if (theta > (angle + tolerance)){
                /* Turn CW a little */
            motor(left, speed + tweak);
            motor(right, speed - tweak);
        } else {
            motor(left, speed);
            motor(right, speed);
        }
    }
            /* You can check a sensor here and terminate the while loop */
    stall_option = STALL_NONE;
    motor(right, -100);
    motor(left, -100);
    msleep(100L);
    motor(left, 0);
    motor(right, 0);
}

void backup_time(long max_time)
{
    int tweak = 15;
    int speed = FAST;
    float angle = theta;
    float tolerance = 0.5;
    long start_time = mseconds();
    long iteration_time = 0L;
    int iterations = 0;

    while (mseconds() - start_time < max_time) {
        iteration_time = mseconds();
        if (theta < (angle - tolerance)) {
                /* Turn CCW a little */
            motor(left, - speed + tweak);
            motor(right, - speed - tweak);
        } else if (theta > (angle + tolerance)){
                /* Turn CW a little */
            motor(left, - speed - tweak);
            motor(right, - speed + tweak);
        } else {
            motor(left, - speed);
            motor(right, - speed);
        }
        iterations++;
        defer();
    }
            /* You can check a sensor here and terminate the while loop */
    stall_option = STALL_NONE;
    motor(left, 0);
    motor(right, 0);
}

void go_angle(float angle,int mode)
{
        /* To convert degrees to units of theta, multiply by lsb_ms_per_deg.
         *    Demo robot turns at about 120 deg/s when speed = 100 */

    float theta0;
    float error, tolerance, error_abs;
    long start_time, d_time = 0L;
    int speed = 0;
    float factor = 1.0;
    long iteration_time;
    int sign_switches = 0;
    int old_sign = 0;
    
    if(mode == GLOBAL)
        theta0=0.0;
    else
        theta0=theta;

    if (angle < 0.0) angle += 360.0;
    else if (angle > 360.0) angle -= 360.0;
    
    tolerance = 2.0;
    error = ((theta-theta0) - angle);
    if (error < 0.0) error_abs = -error;
    else error_abs = error;
    if (error_abs > 360.0) error_abs -= 360.0;
    if (error_abs > 180.0) error_abs = 360.0 - error_abs;
    start_time = mseconds();

    stall_option = STALL_TURNING;

    while ( error_abs > tolerance)
    {
        int sign;
        speed = 45;
	
	printf("going to %f. at %f\n", angle,theta);

        if (sin(error / M_180_PI) < 0.0) {
            sign = -1;
        } else {
            sign = 1;
        }
        if (old_sign != 0 && old_sign != sign) {
            sign_switches++;
            if (sign_switches > 2) {
                break;
            }
        }
        spin(speed * -sign);

        /*update();*/

        error = ((theta-theta0) - angle);
        if (error < 0.0) error_abs = -error;
        else error_abs = error;
        if (error_abs > 360.0) error_abs -= 360.0;
        if (error_abs > 180.0) error_abs = 360.0 - error_abs;
        defer();
    }
    stall_option = STALL_NONE;
    spin(0);
} /* go_angle() */


void go_straight_time(long max_time)
{
    int speedleft = FAST;
    int speedright = FAST;
    int tweakleft = 15;
    int tweakright = 5;
    float angle = theta;
    float tolerance = 0.5;
    long start_time = mseconds();

    int diff=0;

    enable_encoder(10);
    enable_encoder(11);

    motor(left,speedleft);
    motor(right,speedright);

    diff = read_encoder(11)-read_encoder(10);

    while (mseconds() - start_time < max_time) {
      motor(left,speedleft);
      motor(right,speedright);

      if(diff>0){
	speedleft+=tweakleft;
	speedright-=tweakright;
	printf("Tweak left. ");
      }
      else if(diff<0){
	speedright+=tweakright;
	speedleft-=tweakleft;
	printf("Tweak right. ");
      }

      printf("%d\n",diff);
    }
            /* You can check a sensor here and terminate the while loop */
    stall_option = STALL_NONE;
    motor(left, 0);
    motor(right, 0);
}