/*
   This is the source code for the project-1 carried out by group-8 for course CS5790

   The objective of the program is to make the robot go through the squares of 1-ft side 
   placed at the four corners of a 7 ft square.

   The module for left turn of the robot and the little of basic idea is taken from the program named 'dmiller-super-demo.c' 
   written by Divid Miller. This file is in the tests directory of the 'Interactive C 4' folder. 
*/ 

#define R_MOTOR 3 /* Right motor - port 3  */
#define L_MOTOR 1 /* Left motor - port 1 */
#define R_ENC 1 /* (Right)encoder 1 is digital port 8 */
#define L_ENC 0 /* (Left) encoder 0 is digital port 7 */       
#define R_SENSOR 6 /* Right sensor - analog port 6 */
#define L_SENSOR 5 /* Left Sensor - analog port 5 */
#define POWER 40 /* POWER used for forward movement */
#define REVERSEPOWER -40 /* REVERSE POWER used for back ward movement in mild turns */
#define STOP_POWER -20 /* POWER used in stop_wheels() */

// POWER used for slow back ward movement in aligning to the tape
#define STOP_POWER_FOR_ALIGN -10 

// difference in the sensors when they detected the black tape
#define SENSOR_DIFF 16

int L_Range, R_Range, L_Enc, R_Enc, L_Sensor,R_Sensor;

// distance to be travelled from one square to another, in encoder count
#define DISTANCE 169

// encoder count needed for left turn
#define TURN 52

// left sensor reading when it detected the black tape
#define SENSOR_VALUE 150

// distance to be travelled once it reaches the front tape of the square, in  encoder count
#define ADDITIONAL_DIST 34

// vairable used for storing the distance
int dist;

// any  process id
int pid;

// main program starts here
void main()
{   
    int i=1,ppid;
    
    // initializing the 'dist' to the actual distance to be travelled
    dist=DISTANCE;
    
    // this functions brings the robot to halt
    stop_wheels();
    
    // press start button to start
    while(!start_button());
    
    // start the process for monitoring the sensors
    ppid = start_process(monitor_sensors());
    
    // this function takes the robot back for some time
    go_back();
    
    // this function aligns the robot to the black tape
    align();
    
    // killing the process monitor_sensors()
    kill_process(ppid);
    
    // main loop starts here
    for(i=1;i<=12;i++)
      {
        ppid = start_process(monitor_sensors());
        
        // this function makes the robot to go straight
        go_straight();
        
        dist=DISTANCE;
        sleep(0.25);
        
        // this function turns the robot 90 degrees towards its left side
        left_turn();
        go_back();
        
        /*
         * this check is to handle a special case when the left sensor is on the
           black tape and right one is inside. 
         */ 
        if(L_SENSOR > SENSOR_VALUE)
          special_align();
        else align(); // normal align
        sleep(0.25); 
        kill_process(ppid);
    }
}

/*
  This procedure turns the robot  by 90 degrees basing on the encoder count
  This module is from the dmiller-super-demo.ic file. But modified little bit.
*/
void left_turn()
{
    reset_encoder(L_ENC); reset_encoder(R_ENC);
    defer();          //stopping the process 
    sleep(0.25);
    while((L_Enc+R_Enc) < TURN)
      {
        motor(L_MOTOR,-100);
        motor( R_MOTOR,100);
        defer();
    }
    stop_wheels();
}

/* reverses the wheels of the motors for short period to stop the robot */
void stop_wheels()
{
    motor( L_MOTOR,STOP_POWER);
    motor(R_MOTOR,STOP_POWER);
    sleep(0.05);
    ao();
}

/* this procedure monitors the sensors and encoders continuously */
void monitor_sensors()
{
    enable_encoder(R_ENC); /*enable the encoders */
    enable_encoder(L_ENC);
    
    while(1)
      {
        // getting the readings from the encoders and sensors
        L_Enc=read_encoder(L_ENC);
        R_Enc=read_encoder(R_ENC);
        R_Sensor  = analog(R_SENSOR);
        L_Sensor = analog(L_SENSOR);
        defer();
    }
}

// this procedure drives the robot straight for distance of 'dist' encoder count
void go_straight()
{
    int factor;
    int number,diff;
    int done=0;
    int front_line_detected=1;
    reset_encoder(0);
    reset_encoder(1);
    enable_encoder(0);
    enable_encoder(1);
    
    // this while loop executes as long as the encoder count for either of the encoders 
    // does not go beyond 'dist' 
    while (L_Enc <= dist || R_Enc <= dist)
      {
        /* if both sensors detect the black tape and the enoder count is greater than 100,
           it means that the robot detected the black tape of the fist side of the sqaure
           so, from there it has to go till the middle of the square so, make the distance
           to ADDITIONAL_DIST */
        
        if((R_Sensor > SENSOR_VALUE + SENSOR_DIFF|| L_Sensor > SENSOR_VALUE) && L_Enc >100)
          {
            reset_encoder(0);
            reset_encoder(1);
            dist = ADDITIONAL_DIST;
        }
        
        /* if both sensors detect the black tape and the encoder count is less than 50, 
           it means that the robot aligned to the back tape of a sqaure and going forward.
        */
        if((R_Sensor > SENSOR_VALUE + SENSOR_DIFF|| L_Sensor > SENSOR_VALUE) && L_Enc <50)
          {
            front_line_detected=0;
        }
        
        /* if the robot detected the front line, the dist is to be set to DISTANCE- the
           distance travelled till now
        */
        if(front_line_detected == 1 && L_Enc > 50 && done == 0)
          {
            dist = DISTANCE - (L_Enc+R_Enc)/2;
            reset_encoder(L_Enc);
            reset_encoder(R_Enc);
            
            done = 1;
            printf("%d",dist);
        }
        
        // Keep both axles turning at same rate:
        // run the left motor at full speed and the right motor either the full speed or 
        // full speed less the multiplier (50 currently) * the amount that axle is "ahead" of
        // the other.
        // 0 is the minimum since negative numbers run the motor backwards.
        motor (L_MOTOR, max(0,(100 - 0 * (max(0,L_Enc - R_Enc)))));
        motor (R_MOTOR, max(0,(100 - 50 * (max(0,R_Enc - L_Enc)))));
        defer();
    }
    stop_wheels();
    
}

// Returns max of 2 numbers

int max(int a, int b)
{
    if (a > b)
      return a;
    return b;
}

void align()
{
    int option=0, ppid;
    
    ppid = start_process(monitor_sensors());
    while(1)
      {
        // only left sensor detected the black tape, take left turn 
        if (L_Sensor > SENSOR_VALUE && R_Sensor < SENSOR_VALUE+SENSOR_DIFF)
          {
            if(option != 1) 
              {
                stop_wheels();
                kill_process(pid);
                option = 1;
                
                // starts a process to turn the motor slowly to left
                pid = start_process(mild_left_turn());
            }
        }
        
        // If only right sensor detected the black tape, take right turn         
        else if (L_Sensor < SENSOR_VALUE && R_Sensor > SENSOR_VALUE+SENSOR_DIFF)
            {
              if(option != 2) 
                {
                  stop_wheels();
                  pid = kill_process(pid);
                  option=2;
                  
                  // starts a process to turn the motor slowly to left                  
                  pid= start_process(mild_right_turn());
              }
          }
          
          // If none of the left and right sensors is on the black tape, go straight  
          else if (L_Sensor < SENSOR_VALUE && R_Sensor < SENSOR_VALUE+SENSOR_DIFF)
              {
                if(option != 3) 
                  {
                    stop_wheels();
                    kill_process(pid);
                    option=3;
                    
                    // starts a process to slowly move the motor straight
                    pid = start_process(mild_forward());
                }
            }
            
            // when both the sensors detect the black tape, stop the robot    
            else if (L_Sensor > SENSOR_VALUE && R_Sensor > SENSOR_VALUE+SENSOR_DIFF)
                {
                  /* if(R_Sensor - L_Sensor > SENSOR_DIFF)
              {
                  kill_process(pid);
                  option=2;
                  pid= start_process(mild_right_turn());
              }
              else if(L_Sensor - R_Sensor > SENSOR_DIFF)
              {
                  kill_process(pid);
                  option=1;
                  pid=start_process(mild_left_turn());
              }
              else {*/
                  
                  // same as stop_wheels() but uses different powers for the motors to stop
                  stop_wheels_for_align();
                  
                  kill_process(ppid);
                  kill_process(pid);
                  break;
              }     
    }
}

// moves the robot straight slowly
void mild_forward()
{
    motor(L_MOTOR,POWER);
    motor(R_MOTOR, POWER);
    defer();
}

// turns the robot left slowly
void mild_left_turn()
{
    motor(L_MOTOR,REVERSEPOWER*1);
    motor(R_MOTOR,POWER);
    defer();
}

// turns the robot right slowly
void mild_right_turn()
{
    motor(L_MOTOR,POWER);
    motor(R_MOTOR,REVERSEPOWER*1);
    defer();    
}

// stops the robot when both sensors on the black tape
void stop_wheels_for_align()
{
    motor(L_MOTOR,STOP_POWER_FOR_ALIGN);
    motor(R_MOTOR,STOP_POWER_FOR_ALIGN);
    sleep(0.05);
    ao();
}

// this makes the robot backtrack
void go_back()
{
    int ppid;
    motor(L_MOTOR, REVERSEPOWER*4);
    motor(R_MOTOR, REVERSEPOWER*4);
    sleep(2.0);
    stop_wheels();
}

/*
  This function turns the robot to the right until both sensors are on the
  black tape. This function will be called when the left sensor is on the black tape
  and right one is inside the square
*/ 
void special_align()
{
    while(!(R_SENSOR > SENSOR_VALUE && L_SENSOR > SENSOR_VALUE)) 
      {
        // turns the robot slowly to the right 
        mild_right_turn();
    }
    stop_wheels();
}