int gyro_type = 300;        /* 150 for ADXRS150, 300 for ADXRS300 */
float lsb_ms_per_deg = 256.0;   /* nominal scale factor depending on gyro type */
float scale_factor = 1.017; /* adjust this for your particular gyro! */
float theta;                /* current angle, units explained in update_angle() */
float offset;               /* gyro offset */
int gyro_port = 6;          /* gyro analog input port number */
int num_avgs = 800;         /* number of averages for calibration */
long time_ms;               /* system time the last gyro reading was taken */


int posdetermine;
int orientation;  

int ypos0, ypos1, xpos0, xpos1;

int frontpos= 1;

int state = 0;

int led_threshold;

void update_position(){
                if(posdetermine == 0){
                        ypos0 = rf_y0;
                        xpos0 = rf_x0;
                        ypos1 = rf_y1;
                        xpos1 = rf_x1;
                }
                else if (posdetermine == 1){
                        ypos0 = rf_y1;
                        xpos0 = rf_x1;
                        ypos1 = rf_y0;
                        xpos1 = rf_x0;
                }
}

int max3(int first, int second, int third){
        if(first >= second){
                if(first >= third){
                        return first;
                }
                else return third;
        }
        else if( second >= third) return second;
                else return third;
}

int min3(int first, int second, int third){
if(first <= second){
                if(first <= third){
                        return first;
                }
                else return third;
        }
        else if(second <= third) return second;
        else return third;

}



void calibrate_led(){
long current = mseconds();
printf("0 for black, 1 for white\n");
while(mseconds() < current + 5000L){
if(start_button()){
/* start button is the black */
led_threshold = min3(analog(22), analog(20), analog(16)) -5;
printf("black");
printf("%d", led_threshold);
}
else if (stop_button()){
/* stop is white */
led_threshold = max3(analog(22), analog(20), analog(16)) + 5;
printf("white");
printf("%d", led_threshold);
}
}
sleep(2.0);
}


void orient_syncro(){

        int led1= analog(22);
        int led2 = analog(20);
        int led3 = analog(16);
        msleep(70L);

        led1 = analog(22);
        led2 = analog(20);
        led3 = analog(16);

        /* first the four cases of the black area */

        if(led1 >= led_threshold && led2 <led_threshold && led3 >= led_threshold){ /*robot facing north, just move forwards, led's black, white, black*/
                if(rf_y0 > 0)
                {posdetermine = 0;
                }
                else {posdetermine = 1;}  /*assigning the position of my robot and opponent */
                orientation = 0;
                frontpos = 1;
                state = 0;
                printf("Forward\n");
        }
         else if(led1 < led_threshold && led2 >= led_threshold && led3 >= led_threshold){ /*robot facing 90 degress cw from north, led's white, black, black*/
                if(rf_y0 > 0)
                {posdetermine = 0;
                }
                else {posdetermine = 1;}  
                orientation = 1;
                frontpos = 1;
                state = 1;
                printf("90DegF\n");
        }

        else if (led1 >= led_threshold && led2 >= led_threshold && led3 >= led_threshold){ /* robot facing south, led's black, black, black */
                if(rf_y0 > 0)
                {posdetermine = 0;
                }
                else {posdetermine = 1;}  
                printf("Move Backwards!\n"); /* assigning the position of my robot and opponent */

                frontpos = -1;
                orientation = 2;                
                state = 0;
                }

        else if (led1 >= led_threshold && led2 >=led_threshold && led3 < led_threshold){ /* robot facing 270 degrees anticlockwise from north, led's black, black, white */
                
                /* assigning the position of my robot and opponent */
                if(rf_y0 > 0)
                {posdetermine = 0;
                }
                else {posdetermine = 1;}  
                frontpos = -1;
                orientation = 3;
                state = 1;
                printf("90DegB!\n");
        }

        /*the white side with four cases */

        else if(led1 < led_threshold && led2 < led_threshold && led3 >= led_threshold){ /* the robot facing north, led's white, white, black */
                if(rf_y0 <0)
                {posdetermine = 0;
                }
                else {posdetermine = 1;}          /* assigning the position of my robot and opponent */
                orientation = 0;
                frontpos = -1; /* ********** double check this later ************** */
                state = 0;
                printf("Move Backward!\n");
                }

         else if(led1 < led_threshold && led2 >= led_threshold && led3 < led_threshold){/*the robot facing 90 degrees cw from north, led's white, black, white*/
                
                if(rf_y0 <0)
                {posdetermine = 0;
                }
                else {posdetermine = 1;}  /* assigning the position of my robot and opponent */

                orientation = 1;
                frontpos= -1; /* ********** double check later *************** */
                state = 1;
                printf("90DegB\n");
                }
        else if (led1 >= led_threshold && led2 < led_threshold && led3 < led_threshold){ /*the robot facing south, led's black, white, white */
                if(rf_y0 <0)
                {posdetermine = 0;
                }
                else {posdetermine = 1;}  /* assigning the position of my robot and opponent */
                state = 0;
                orientation = 2;
                printf("Forwards!\n");
                frontpos = -1; /* *******************double check later ***************** */
                }

        else if (led1 < led_threshold && led2 < led_threshold && led3 < led_threshold){ /*the robot facing 90 ccw from north, led's white, white, white */
                
                if(rf_y0 <0)
                {posdetermine = 0;
                }
                else {posdetermine = 1;}          /* assigning the position of my robot and opponent */
                state = 1;
                orientation = 3;
                frontpos = 1; /* ********************* double check later ****************** */
                printf("90Forwards!\n");
        }

}