/*
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    program name : multiTask.ic
    description  : Obstacle Avoidance Program as a Multitasking Program 
    done by      : Team 9    
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/


/* PROGRAM CONSTANTS */

#define TRUE           1 
#define FALSE          0

#define MOTOR_LEFT     0
#define MOTOR_RIGHT    2


#define SENSOR_LEFT    10 
#define SENSOR_RIGHT   15

#define MAX_THREADS    2 

#define PID_MOVEFD     0 
#define PID_SENSORS    1  


#define LEFT           0
#define RIGHT          1


/*
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   function name : move_forward
   description   :  thread to move the robot forward 
   inputs        : None
   outputs       : moves the robot forward with 50% power on both the wheel  
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
void move_forward()
{
    int stop  = 0 ;
    int firstTime = TRUE; 
    /* Loop forever */
    while(TRUE)
      {
        if (firstTime)
          {
            fd(MOTOR_LEFT);
            fd(MOTOR_RIGHT);
            firstTime = 0 ; 
        }
        /* check if stop button has been pressed */    
        stop = stop_button();
        /* stop button pressed , stop it */
        if(stop)
          {
            off(MOTOR_LEFT);
            off(MOTOR_RIGHT);
           
            
        }
        
    }        
}

/*
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   function name : turn   
   description   : turns the robot in a left or right direction 
   inputs        : direction to turn 
   outputs       : truns the robot in the direction given  
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/

void turn(int direction)
{
    
    // Move backward  
    bk(MOTOR_LEFT);
    bk(MOTOR_RIGHT);
    sleep(.5);
    
    // Turn around in the given direction 
    if ( direction == RIGHT )
      {
        fd(MOTOR_RIGHT);
    }
    else
      {
        fd(MOTOR_LEFT);  
    }
    
    sleep(1.5);
    // Move forward 
    fd(MOTOR_LEFT);
    fd(MOTOR_RIGHT);
    
    
    
}


/*
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   function name : check_sensors
   description   : thread to check the sensors 
   inputs        : status of start button , process id of the move forward
                   process 

   outputs       : when one of the sensors have been touched , then 
                   then turn around and move forward  
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++


*/

void check_sensors(int start_button_pressed, int pid_movfd , int pid_chsensor)
{
    int stop_button_pressed = FALSE; 
    /* Loop forever */
    while(TRUE)
      {
        
        stop_button_pressed = stop_button(); 
        if ( stop_button_pressed)
        {
            start_button_pressed = FALSE;
            off(MOTOR_LEFT);
            off(MOTOR_RIGHT);
            kill_process(pid_movfd);
            kill_process(pid_chsensor);
            
        }
        // Check if left sensor has been pressed 
        if ( digital(SENSOR_LEFT) && start_button_pressed)
          {
            // Turn towards right  
            turn(RIGHT);  
            
        }
        // Check if right sensor has been pressed 
        if ( digital(SENSOR_RIGHT) && start_button_pressed)
          {
            // Turn towards right  
            turn(LEFT);  
        }    
        
    }
    
}

void main()
{
    int process_id[MAX_THREADS]; 
    int start_button_pressed = FALSE ; 
    // Wait until start button is pressed 
    while(TRUE)
      {
        if (start_button())
          {
            start_button_pressed = TRUE ;
            break;
        }
    }
    
    process_id[PID_MOVEFD] =  start_process(move_forward());
    process_id[PID_SENSORS] = start_process(check_sensors(start_button_pressed,process_id[PID_MOVEFD],process_id[PID_SENSORS]));
    // Let the main thread keep waiting
    while(TRUE); 
    
}