/*********************************************************
**********************************************************
            PROJECT1 : SENSING AND MOVEMENT
            GROUP 6  : Jianwei Zhuang
                       Joe Garfield
                       Manohar Darapureddi
                       Pedro Diaz
***********************************************************
**********************************************************/

/**********************************************************
                   NAMING THE CONSTANTS
**********************************************************/

#define CLICKS_L 16
#define CLICKS_R 12
#define DIAM_L 7.3
#define DIAM_R 7.3
#define B 15.5
#define SLEEP_TIME 0.2
#define CORR_POWER 50
#define LEFT_ENCODER 0
#define RIGHT_ENCODER 2
#define IR_SENSOR 2
#define FORWARD_MOTOR 0
#define TURN_MOTOR 2
#define PI 3.1415
#define THRESHOLD 0.1

/*********************************************************
                   GLOBAL VARIABLES
*********************************************************/

int EDGE;
int do_correction;
int PREV_CLICKS[2];
int CURR_CLICKS[2];
int FD,BK,TR,TL;
int TAPE_VAL=225;
long PREV_ST;
long CURR_ST;
float diff;
float ERROR=0.0;
float PREV_POS[2];
float CURR_POS[2];
float CURR_VEL[2];
float CURR_ANGLE;
float time_t;
float ANGLE_CHANGED;
float PREV_ANGLE;



/**********************************************************
           INITIALIZING ALL THE VARIABLES
**********************************************************/

void INIT()
{
    FD=0;
    BK=0;
    TR=0;
    TL=0;
    PREV_CLICKS[0] = 0;
    PREV_CLICKS[1] = 0;
    CURR_CLICKS[0] = 0;
    CURR_CLICKS[1] = 0;
    reset_encoder(0);
    reset_encoder(2);
    PREV_POS[0] = 0.0;
    PREV_POS[1] = 0.0;
    CURR_POS[0] = 0.0;
    CURR_POS[1] = 0.0;
    CURR_ANGLE = 0.0;
    PREV_ANGLE = 0.0;
    ANGLE_CHANGED = 0.0;
    PREV_ST = mseconds();
    CURR_ST = PREV_ST;
    do_correction = 1;
}

/************************************************************
       UPDATING THE POSITION OF ROBOT,NUMBER OF TICKS
        OF THE ENCODERS AND THE ANGLE IT HAD DRIFTED
************************************************************/

void UPDATE_STATUS()
{
    PREV_ST = CURR_ST;
    PREV_POS[0] = CURR_POS[0];
    PREV_POS[1] = CURR_POS[1];
    PREV_CLICKS[0] = CURR_CLICKS[0];
    PREV_CLICKS[1] = CURR_CLICKS[1];
    PREV_ANGLE = CURR_ANGLE;
    
}

/***********************************************************
      GETTING THE CURRENT STATUS OF THE ROBOT
***********************************************************/

void GET_STATUS()
{
    float sl,sr;
    float dl,dr,di;
    float min;
    
    if(FD||TR) sl =1.0;
    else if(BK||TL) sl=-1.0;
      else;
    
    if(FD||TL) sr=1.0;
    else if(BK||TR) sr = -1.0;
      else;
    
    CURR_ST = mseconds();
    CURR_CLICKS[0] = read_encoder(LEFT_ENCODER);
    CURR_CLICKS[1] = read_encoder(RIGHT_ENCODER);

    //distance travelled by left wheel
    dl = ((float)(CURR_CLICKS[0] - PREV_CLICKS[0])/(float)CLICKS_L)*PI*DIAM_L; 

    // distance travelled by right wheel
    dr = ((float)(CURR_CLICKS[1] - PREV_CLICKS[1])/(float)CLICKS_R)*PI*DIAM_R;    

    // distance travelled by the robot
    di = (dl+dr)/2.0;                                                                                      
    
    //  angle drifted by the robot from its previous updated position
    CURR_ANGLE = (((float)CURR_CLICKS[0]*sl/(float)CLICKS_L)-(float)CURR_CLICKS[1]*sr/(float)CLICKS_R))*PI*DIAM_L/B;

    //absolute position of the robot from (0,0) along X-coordinate
    CURR_POS[0] = CURR_POS[0] + di*sin(CURR_ANGLE);

    //absolute position of the robot from (0,0) along Y-coordinate          
    CURR_POS[1] = CURR_POS[1] + di*cos(CURR_ANGLE);    

    UPDATE_STATUS();
    printf("y = %f\n", CURR_POS[1]);
}

/****************************************************************
             ERROR CORRECTION FUNCTION
****************************************************************/

void ERROR_CORR()
{
     if(diff<0.0)
      {
        motor(2,10);
        sleep(0.1);
        off(2);
      }
    else
      {
        motor(2,-10);
        sleep(0.1);
        off(2);
      }
    beep();  
}

/*******************************************************
    CALIBRATE FUNCTION MAKES SURE THAT WHEN BOTH THE COLOR
     SENSORS SENSE THE TAPE THE ROBOT IS PERPENDICULAR TO
                     THE BLACK TAPE
********************************************************/

void CALIBRATE()
{
    while (1) {
        if(analog(2) < TAPE_VAL || analog(4) < TAPE_VAL) {
            motor(0,-50);
            if(analog(2) > TAPE_VAL) {  
                while (analog(4) < TAPE_VAL) {
                    motor(2,22);
                }
                off(2);
            }
            else if(analog(4) > TAPE_VAL) {
               while (analog(2) < TAPE_VAL) {
                    motor(2,-22);
               }
                  off(2);
            }
        }
        else
          break;
    }  
    ao();
}

/********************************************************
   THE CALIBRATE_2 FUNCTION MAKES SURE THAT WHEN ONLY LEFT
   SENSOR SENSES THE TAPE THE ROBOT MOVES BACK AND CHANGES
   ITS DIRECTION SO THAT BOTH THE SENSORS SEEK THE TAPE
**********************************************************/

void CALIBRATE_2()
{
    motor(FORWARD_MOTOR, -100);
    sleep(1.0);
    ao();
    motor(TURN_MOTOR, -50);
    sleep(0.4);
    ao();
}

/********************************************************
   THE CALIBRATE_3 FUNCTION MAKES SURE THAT WHEN ONLY RIGHT
   SENSOR SENSES THE TAPE THE ROBOT MOVES BACK AND CHANGES
   ITS DIRECTION SO THAT BOTH THE SENSORS SEEK THE TAPE
**********************************************************/

void CALIBRATE_3()
{
    motor(FORWARD_MOTOR, -100);
    sleep(1.0);
    ao();
    motor(TURN_MOTOR, 50);
    sleep(0.4);
    ao();
}

/*********************************************************
        THIS FUNCTION MAKES SURE THAT THE ROBOT TAKES 
       A 90 DEGREE TURN AFTER IT IS INSIDE THE SQUARE BOX
**********************************************************/

void TURN_90(float angle)
{
    int target_clicks, start_clicks;
    motor(TURN_MOTOR, 100);
    sleep(1.2);
    off(2);
    ao();
}

/*******************************************************
    THIS PROCESS KEEPS UPDATING THE STATUS OF THE ROBOT
********************************************************/

void update_status_process()
{
    while(1){
        GET_STATUS();
    }
}

/**********************************************************
    THIS FUNCTION MAKES SURE THAT THE ROBOT MOVES TILL IT
    SENSES THE TAPE AND STOPES A LITTLE BIT FURTHER AHEAD
    OF THE TAPE.THE FUNCTION RETURNS A VALUE OF "0" WHEN 
    THE BOTH THE SENSORS SENSE THE TAPE.THE FUNCTION RETURNS
    "-1"WHEN THE LEFT SENSOR ONLY SENSES THE TAPE.THE FUNCTION
    RETURNS "1" ONLY WHEN RIGHT SENSOR SENSES THE TAPE
***********************************************************/

int seek_tape(int power)
{
    int left_hit, right_hit, adjusted;
    float time_stamp;
    float y;
    
    left_hit = 0;
    right_hit = 0;
    adjusted = 0;
    
    motor(FORWARD_MOTOR, power);   
    while (analog(4) < TAPE_VAL && analog(2) < TAPE_VAL) {
        y = CURR_POS[1];
        if(!adjusted && y > 2.0 * 12.0 * 2.54) {
            adjusted = 1;
            motor(TURN_MOTOR, -10);
            sleep(0.3);
            motor(TURN_MOTOR, 0);
        }
        if(y > 8.0*( ((float)knob())/255.0) * 12.0 * 2.54) {
            return -1;
        }
     }
    time_stamp = -1.0;
    
    printf("%d %d\n",analog(2),analog(4));    
    while((analog(4) > TAPE_VAL || analog(2) > TAPE_VAL) ) {
        if(analog(4) >= TAPE_VAL){
            right_hit = 1;
        }
        else if(analog(2) >= TAPE_VAL) {
              left_hit = 1;
        }
        //If an IR has been activated and becomes 'inactive' 
        //we start the timer
          if((right_hit == 1 && analog(4) < TAPE_VAL) || left_hit == 1 && analog(2) < TAPE_VAL)){
              time_stamp = seconds();
          }
        
        if(time_stamp > 0.0 && seconds() - time_stamp >= 2.0) {
            if(analog(2) < TAPE_VAL && analog(4) >= TAPE_VAL) {
                ao();
                motor(FORWARD_MOTOR, -100);
                sleep(7.0);
                return 1;
            }
   }
        if(analog(4) < TAPE_VAL && analog(2) >= TAPE_VAL) {
                ao();
                motor(FORWARD_MOTOR, -100);
                sleep(7.0);
                return -1;
              }
        }
        
    }
    if(left_hit && !right_hit)
      return -1;
    else if(!left_hit && right_hit)
        return 1;
      return 0;
}

/********************************************************
    THE ROBOT MOVES TO APPROXIMATELY THE MIDDLE OF 
          THE SQUARE AFTER IT SENSES THE ROBOT
*********************************************************/

void recenter_robot()
{
    ao();
    motor(0, 30);
    while(CURR_POS[1] < 5.0 * 2.54);
    off(0); 
}

/********************************************************
                  MAIN  FUNCTION
********************************************************/

void main()
{
    int update_status_pid;
    int error_correction_pid;
    float angle,y;
    int i, direction;
    
    // This makes sure that the robot doesn'n start untill start button is //pressed
    while(!start_button());    
    sleep(0.5);
    enable_encoder(LEFT_ENCODER);
    enable_encoder(RIGHT_ENCODER);
    INIT();
    FD=1;    
    update_status_pid = start_process(update_status_process());
    y=CURR_POS[1];
    
    for(i=0;i<12;i++) {
        INIT();
        FD=1;seek_tape(100);
        beep();
        sleep(0.50);
        ao();
        beep();
        CALIBRATE();       
        motor(FORWARD_MOTOR, 100);
        sleep(0.5);
        ao();
        INIT();FD=1;       
        beep();
        direction = seek_tape(100);
        ao();       
        sleep(0.5);        
        while(direction != 0){
            sleep(0.5);
            ao();
            if(direction < 0)
              CALIBRATE_2();
            else 
              CALIBRATE_3();
            INIT();
            direction = seek_tape(40);
        }
        ao();
        sleep(0.5);
        CALIBRATE();
        beep();
        INIT();
        recenter_robot();
        ao();
        beep();
        sleep(1.5);
        INIT(); FD=0; TR = 1;
        TURN_90(angle);
        TR = 0;        
        
    }
    
    kill_process(update_status_pid);
    ao();
}

/*************************************************************
                   END OF THE CODE
*************************************************************/