#define LIGHT_SENSOR_PORT IN_3
#define LIGHT_SENSOR2_PORT IN_4
 
#define SOUND_SENSOR_PORT  IN_1
#define MIC SENSOR_1
#define THRESHOLD          65

#define RIGHT_MOTOR                      OUT_A
#define LEFT_MOTOR                       OUT_C
#define SPEED_PERCENT                    40

#define SLOW_SPEED                       8

#define  LINE_THRESHOLD                  300

#define LINE_LIMIT1                      200
#define LINE_LIMIT2                      450


#define LINE_LIMIT21                      250
#define LINE_LIMIT22                      550


#define INRANGE(value,limit1,limit2)   ((value>limit1) && (value <limit2))
#define COLOR_SENSOR_PORT                IN_2


#define LINE_FOLLOW                      0
#define HOME_MODE                        1

#define STOP                             0
#define SPINING                          1
#define STRAIGHT                         2


#define MOTOR_SLOW_SPEED                 10
#define MOTOR_FAST_SPEED                 85

#define MOTOR_STRAIGHT_SPEED             25


int sensor_left = 0 ;
int sensor_right = 0 ;

int motor_power_left = 60 ;
int motor_power_right =  60;

float _timer;

int robot_mode = 0 ;
mutex mode_mutex;

/*
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
subroutine name : reset_timer
description     : function to reset the timer
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
sub reset_timer()
{
    _timer = CurrentTick();
}
/*
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
subroutine name : timer
description     : function to get the current value of timer
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/

float timer()
{
    return CurrentTick() - _timer;
}


/*

sub waddle_left()
{

    OnFwd(RIGHT_MOTOR, SPEED_PERCENT);
    OnFwd(LEFT_MOTOR , SLOW_SPEED );
}
sub waddle_right()
{
    OnFwd(RIGHT_MOTOR , SLOW_SPEED );
    OnFwd(LEFT_MOTOR, SPEED_PERCENT);

}
*/
int line_sensor()
{

   int value = Sensor( LIGHT_SENSOR_PORT);
   NumOut(0,LCD_LINE1,value);
   return value;

}

int helper_sensor()
{
   int value = Sensor( LIGHT_SENSOR2_PORT);
   NumOut(0,LCD_LINE2,value);
   return value;
}

sub spin ()
{

    OnFwd(RIGHT_MOTOR, 90);
    OnFwd(LEFT_MOTOR, 0);
}

sub  straight()
{
    OnFwd(RIGHT_MOTOR,75);
    OnFwd(LEFT_MOTOR, 75);
    Wait(500);
}


sub sound_follow()
{
     int state = STOP ;
    SetSensorSound(SOUND_SENSOR_PORT);
    while(true)
    {
        if ( MIC  > THRESHOLD)
        {
          while(MIC > THRESHOLD)
          {
        
             straight();
          }
        }
        else
        {
            spin();
        }

       /*
       if(state == STOP)
       {
          state = SPINING;
          spin();
       }
       else if ( state == SPINING )
       {
          state = STRAIGHT;
          straight();
       
       }
       else if ( state == STRAIGHT)
       {
           state = SPINING;
           spin();
       }
       */
    }

}


task color_sensor()
{
    // Initialize Color Sensors
    // unsigned int rgb[4];
   //SetSensorLowspeed(COLOR_SENSOR_PORT);
   int color_value = 0 ;
   byte  color_num ,red , blue , green ;
   unsigned int red_value = 0 , blue_value = 0 , green_value = 0 ;
   SensorHTColorNum(COLOR_SENSOR_PORT);
   SetSensorLowspeed(COLOR_SENSOR_PORT);
   SetSensorMode(COLOR_SENSOR_PORT,SENSOR_MODE_RAW );
   ResetSensor( COLOR_SENSOR_PORT);
   while(1 )
   {
        //ReadSensorColorRaw (COLOR_SENSOR_PORT, rgb);
        while(!ReadSensorHTColor(COLOR_SENSOR_PORT,color_num,red,green,blue));
        red_value = red;
        blue_value = blue;
        green_value = green;
        //color_value =  SensorHTColorNum(COLOR_SENSOR_PORT);
        ClearScreen();

        if ( INRANGE(red,249,256)&& INRANGE (green,249,256) && INRANGE(blue,55,75))
        {

             Acquire(mode_mutex);
             robot_mode = HOME_MODE;
             Release(mode_mutex);
             Off(LEFT_MOTOR);
             Off(RIGHT_MOTOR);
             PlayToneEx(1200,200,100,FALSE);
             Wait(230);
             sound_follow();
        }

        Wait(5);
        NumOut(0,LCD_LINE3,red);
        NumOut(0,LCD_LINE4,green);
        NumOut(0,LCD_LINE5,blue);

   }

}
/*

sub waituntil_on_the_line()
{
      while(1)
      {
          if( INRANGE(line_sensor(),LINE_LIMIT1,LINE_LIMIT2) )
          {
               sensor_left = 1 ;
               sensor_right = 0 ;
               break;
          }
          if( INRANGE(helper_sensor(),LINE_LIMIT1,LINE_LIMIT2) )
          {
               sensor_right = 1 ;
               sensor_left = 0 ;
               break;
          }

      }

}

sub waituntil_off_the_line()
{
    reset_timer();
  //  while(INRANGE(line_sensor(),LINE_LIMIT1,LINE_LIMIT2) && !INRANGE(helper_sensor(),LINE_LIMIT1,LINE_LIMIT2));
    while(INRANGE(line_sensor(),LINE_LIMIT1,LINE_LIMIT2));

}

int isOtherSensorOnLine()
{

    if (INRANGE(helper_sensor(),LINE_LIMIT1,LINE_LIMIT2))
    {
           return 1 ;
    }
    return 0;
}
*/
task line_follow()
{


    int flag = 0 ;
    int left_sensor_value;
    int right_sensor_value;
    int left_hit =  0 ;
    int right_hit = 0 ;
     // Set up light sensor 1
    SetSensorLight(LIGHT_SENSOR_PORT);
    SetSensorMode(LIGHT_SENSOR_PORT,SENSOR_MODE_RAW );
    ResetSensor(LIGHT_SENSOR_PORT);

    // Set up light sensor 2
    SetSensorLight( LIGHT_SENSOR2_PORT);
    SetSensorMode(LIGHT_SENSOR2_PORT,SENSOR_MODE_RAW );
    ResetSensor(LIGHT_SENSOR2_PORT);

    while(true && robot_mode == LINE_FOLLOW)
    {
        left_sensor_value = line_sensor();
        right_sensor_value = helper_sensor();
        if (INRANGE(line_sensor(),LINE_LIMIT1,LINE_LIMIT2) && !INRANGE(helper_sensor(),LINE_LIMIT21,LINE_LIMIT22 ))
        {
              motor_power_left = MOTOR_SLOW_SPEED ;
              motor_power_right = MOTOR_FAST_SPEED ;
              left_hit =  1 ;

              OnFwd(LEFT_MOTOR,motor_power_left);
              OnFwd(RIGHT_MOTOR,motor_power_right);
              while(INRANGE(line_sensor(),LINE_LIMIT1,LINE_LIMIT2));
              while(!INRANGE(helper_sensor(),LINE_LIMIT21,LINE_LIMIT22) );
        }
        if (INRANGE(helper_sensor(),LINE_LIMIT21,LINE_LIMIT22) && !INRANGE(line_sensor(),LINE_LIMIT1,LINE_LIMIT2) )
        {
              motor_power_left = MOTOR_FAST_SPEED;
              motor_power_right = MOTOR_SLOW_SPEED;
              PlayToneEx(1200,10,100,FALSE);
              OnFwd(LEFT_MOTOR,motor_power_left);
              OnFwd(RIGHT_MOTOR,motor_power_right);
              while(INRANGE(helper_sensor(),LINE_LIMIT21,LINE_LIMIT22));
              while(!INRANGE(line_sensor(),LINE_LIMIT1,LINE_LIMIT2));
              
        }
        if (INRANGE(line_sensor(),LINE_LIMIT1,LINE_LIMIT2) &&  INRANGE(helper_sensor(),LINE_LIMIT21,LINE_LIMIT22))
        
        {
        
              motor_power_left = MOTOR_FAST_SPEED;
              motor_power_right = MOTOR_SLOW_SPEED;
              PlayToneEx(1200,10,100,FALSE);
              OnFwd(LEFT_MOTOR,motor_power_left);
              OnFwd(RIGHT_MOTOR,motor_power_right);
              while(INRANGE(helper_sensor(),LINE_LIMIT21,LINE_LIMIT22));
              while(!INRANGE(line_sensor(),LINE_LIMIT1,LINE_LIMIT2));
              

        }
        
        motor_power_left  = MOTOR_STRAIGHT_SPEED  ;
        motor_power_right = MOTOR_STRAIGHT_SPEED  ;
        OnFwd(LEFT_MOTOR,motor_power_left);
        OnFwd(RIGHT_MOTOR,motor_power_right);

    }

}


task main()
{

    Precedes(line_follow,color_sensor);
}