#define RM 0 // right motor
#define LM 1 // left motor
#define MAX_POWER 60 // max power setting of motor
#define LOW_POWER 40 // low power setting of motor
#define RS 2 // right light sensor
#define LS 3 // lest light sensor
#define RENC 0 // right encoder
#define LENC 1 // left encoder
#define threshold 130 // threshold for light sensor reading
int enc0=0,enc1=0,j;


void main ()
{
    ready(); // call reay fucntion
    
    for(j=1;j<13;j++) // repeat calling below three fucntions 12 time to make 3 circuit.
      {
        departure(); //call departure function
        sleep(1.0);
        cruise();    // call cruise function
        sleep(1.0);
        turn();     // call turn fuction
        sleep(1.0);
    }
    
    printf("I am done!!!!!!!!!!\n"); // printing message to show that job has done
}

/****************************SUBROUTINES*********************************/

void ready()
{
    printf("Ready. Punch the start button!\n");
    while(!start_button()); //wait until start button is pressed
    sleep(1.0); // wait one second
}

void cruise() /* this is the subroutine to adjust
                           rotational defference between wheels */
{
    
    motor_full_power(); // call fucntion to set both motor to full power
    
    enable_encoders(); // call fuction to enable both encoders   
    
    while (1)
      {
        enc0=read_encoder(RENC); //read encoder vlaues
        enc1=read_encoder(LENC);
        sleep(0.05);
        
        printf("Keep going\n");
        
        if(enc0>enc1) {motor(RM,(LOW_POWER*9)/10); motor(LM,MAX_POWER);} //if one encoder value is bigger than the other
        if(enc1>enc0) {motor(LM,(LOW_POWER*5)/10); motor(RM,MAX_POWER);} //power on the motor with high encorder value
                                                                         // is reduce.
        if(analog(RS) > threshold || analog(LS) > threshold) //when one of either light sensor senses value higher than
          if (enc0>40) break;                          // threshold and encoder value is bigger than 40, the loop stops
    }
    
    motor_break(); //motor stops
}


/*************This section is about departure and turn************/

void departure () /* controls departuring from the square */
{    
    int i;
    
    printf("I am free as wind blow!!\n");
    
    for (i=0;i<4;i++) 
      {
        motor_back(); // stop both motors
        sleep(0.15);
        adjust_to_line(); // align the robot to black tape
    }
    
    motor_break(); //motor stops
    
}

void turn () /* controls the turning */
{    
    
    motor_slow();
    sleep(0.7);
    
    printf("I am spining!!\n");
    
    reset_encoders(); // reset both encorders
    
    motor(LM,30); // set power of motors so that one wheel turns forwar
    motor(RM,-30); // and the other moves backward
    
    while (enc0<7 && enc1<7)
      {
        enc0=read_encoder(RENC);
        enc1=read_encoder(LENC);
        sleep(0.1);
        if (enc0 == 6) motor(RM,0); // to make 90 degree turn when encoder vlaue reachs 6
        if (enc1 == 6) motor(LM,0); // it stops motor
    }
    
    motor_break();
}

/*******Miscellaneous Subroutines***********/

void adjust_to_line()
{   
    while (analog(RS) < threshold || analog(LS) < threshold) // keep looping ultin readings from both light sensors
      {                                                      // are higher than threshold
        
        if (analog(RS) >= threshold) // if right light sensor reading is higher than threshol
          { 
            motor(RM,-5); // set right motor power at 5 backward
            motor(LM,20);sleep(0.05); // set left motor power at 20 forwar then wait 0.05 second
        }
        else if (analog(LS) >= threshold) // if left light sensor reading is higher than threshold
            { 
              motor(LM,-5); // set left motor power at 5 backward
              motor(RM,20);sleep(0.05); // set right motor power at 20 forward then wait 0.05 second
          }
          else
            {
              motor_slow(); // set motor power low forward
          }
    }
    
    motor_break();
}

void motor_full_power() /* set motor to predefined full power*/
{
    motor (RM,MAX_POWER);
    motor (LM,MAX_POWER);
}
void motor_slow() /* set motor power to move robot slowly*/
{
    motor(RM,15);
    motor(LM,13);
}
void motor_back() /* set motor power to move robot backward slowly*/
{
    motor(RM,-20);
    motor(LM,-20);
}
void motor_break() /* stops robot*/
{
    motor(RM,-10);
    motor(LM,-10);
    sleep(0.05);
    ao();
}
void enable_encoders() /* enable both encoders*/
{
    enable_encoder(RENC);
    enable_encoder(LENC);
}
void reset_encoders() /* reset both encoders*/
{
    enc0=enc1=0;
    reset_encoder(RENC);
    reset_encoder(LENC);
}