#define KICKER_MOTOR                    OUT_A
#define COLOR_SENSOR_PORT                IN_1
#define COMPASS_PORT                     IN_2
#define TOUCH_SENSOR_PORT                IN_3
#define LIGHT_SENSOR_PORT                 IN_4


#define LEFT_MOTOR                      OUT_C
#define RIGHT_MOTOR                     OUT_B

#define FWD_PWR_LEFT                         87
#define FWD_PWR_RIGHT                        95
#define LIFT_PWR                             42
#define LIFT_WAIT                            350

#define BCK_PWR_LEFT                         97
#define BCK_PWR_RIGHT                        100


#define COMPASS_THRESHOLD                    80

#define LINE_LIMIT1                         190
#define LINE_LIMIT2                         370


#define MOVE_DOWN_TIME                      450

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

#define TIMEOUTLIMT                         3000

#define TIMEOUTBALL                         4000

#define MOVE_UTIL_DARK                      6000

int red_count = 0;
int blue_count = 0 ;


int GetCompassRelativeHeading(int intialVaue)
{


     int tmp_heading = SensorHTCompass(COMPASS_PORT) - intialVaue + 180;
     return (tmp_heading >= 0 ? tmp_heading % 360 : 359 - (-1 - tmp_heading)%360) - 180;
}



float _timer=0;
sub init_sensors()
{

     // Set up Light Sensors
     SetSensorLight(LIGHT_SENSOR_PORT);
     SetSensorMode(LIGHT_SENSOR_PORT,SENSOR_MODE_RAW );
     ResetSensor(LIGHT_SENSOR_PORT);

     // Set up color sensors
     SensorHTColorNum(COLOR_SENSOR_PORT);
     SetSensorLowspeed(COLOR_SENSOR_PORT);
     SetSensorMode(COLOR_SENSOR_PORT,SENSOR_MODE_RAW );
     ResetSensor( COLOR_SENSOR_PORT);
     
     // Set up the compass sensor
     
     SetSensorLowspeed(COMPASS_PORT);
     ResetSensor( COMPASS_PORT);

     
    // Set up touch sensor
    
    SetSensorTouch(TOUCH_SENSOR_PORT);

}



// Read Light Sensor values
int line_sensor()
{

   int value = Sensor( LIGHT_SENSOR_PORT);

   return value;

}

// Read color sensor values
sub color_sensor( byte &color_num , byte &red , byte &blue , byte &green)
{

    while(!ReadSensorHTColor(COLOR_SENSOR_PORT,color_num,red,green,blue));
}

// Read compass sensor value
int compass_sensor_value()
{
    return  SensorHTCompass(COMPASS_PORT);
}

/*
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
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 mov_forward()
{
    OnFwd(LEFT_MOTOR, FWD_PWR_LEFT);
    OnFwd(RIGHT_MOTOR, FWD_PWR_RIGHT);
}

sub stop_motors()
{
   Off(LEFT_MOTOR);
   Off(RIGHT_MOTOR);
}

sub move_forward_till_dark_old()
{
     mov_forward();
     while(!INRANGE(line_sensor(),LINE_LIMIT1,LINE_LIMIT2));
     stop_motors();

}


sub mv_forward(int left_power , int right_power)
{

    OnFwd(LEFT_MOTOR,left_power);
    OnFwd(RIGHT_MOTOR,right_power);
}

sub mv_backward(int left_power , int right_power)
{

    OnRev(LEFT_MOTOR,left_power);
    OnRev(RIGHT_MOTOR,right_power);
    Wait(120);
}

sub lift_ball()
{
   OnFwd(KICKER_MOTOR,LIFT_PWR);
   Wait(LIFT_WAIT+70);
   Off(KICKER_MOTOR);
}
sub compass_wait(int limit1 , int limit2)
{

   while(1)
   {
          Wait(50);
          if(INRANGE(compass_sensor_value(),limit1,limit2) ||(timer() > TIMEOUTLIMT ) )
          {
              break;
          }

     }
     stop_motors();
}


sub turn_robot( int limit1 , int limit2, bool left )
{

     int counter = 0 ;
     if(left)
         mv_backward(55,0);
         //mv_backward(55,55);
     else
         mv_backward(0,55);
         //mv_backward(55,0);
     reset_timer();
     compass_wait(limit1,limit2);
     PlayToneEx(1200,30,100,FALSE);
     if ( timer() >= TIMEOUTLIMT )
     {

            reset_timer();
            mv_forward(0,55);
            compass_wait(limit1,limit2);
            PlayToneEx(1200,30,100,FALSE);
    }
}

sub move_forward_till_dark()
{


    int counter = 0 ;
    int motor_power_right =  100 ;
    int motor_power_left = 98;
    float relative_reading = 0.0 ;
    byte color_num = 0 ,  red = 0  , blue = 0  , green =0 ;
  //  int error_count = 0 ;
    Wait(10);

    int initialValue = SensorHTCompass(COMPASS_PORT) ;
    bool timed_out  = false;
 mov_fwd_again:
    reset_timer();
    while(!INRANGE(line_sensor(),LINE_LIMIT1,LINE_LIMIT2))
    {
             ++counter;
             Wait(30);
             relative_reading = GetCompassRelativeHeading(initialValue);
             if ( relative_reading < 0 )
             {
                  motor_power_right =  100.0 + relative_reading / 3.0;

             }
             if ( relative_reading >  0 )
             {

                 motor_power_left =98.0 - relative_reading / 3.0;
             }
             mv_forward(  motor_power_left  ,motor_power_right);
             if ( timer() >  MOVE_UTIL_DARK )
             {
                 timed_out  = true;
                 break;
             
             }
             if ( counter == 17 )
             {
                 Wait(20);
                initialValue = SensorHTCompass(COMPASS_PORT);
                counter = 0 ;
             }


    }
    stop_motors();
    if ( timed_out )
    {
        reset_timer();
        mv_backward(97,100);
        Wait(250);
        color_sensor(color_num,red,blue,green);
        if ( blue > 15 )
        {
           turn_robot(0,15,true);
        }
        else
        {
            turn_robot(140,190,true);
        }
        timed_out = false;
        goto mov_fwd_again;
    }


}

sub drop_ball()
{
     OnRev(KICKER_MOTOR,LIFT_PWR);
     Wait(MOVE_DOWN_TIME);
     Off( KICKER_MOTOR) ;
     mv_forward(80,80);
     Wait(200);
     stop_motors();
     mv_backward(50, 50);
}



sub turn_back(int  compass_limt1 , int compass_limit2,bool left)
{
     int counter = 0 ;


     counter++;
     mv_backward(57,0);
     turn_robot(compass_limt1,compass_limit2,left);
     stop_motors();
     Wait(50);
     mv_forward(98,100);
     Wait(800);
     //move_forward_till_dark();


     
     
}

sub throw_red_ball()
{
   byte color_num  = 0 , red = 0 , blue =0 , green = 0 ;
   while(1)
   {
      Wait(50);
      color_sensor(color_num,red,blue,green);
      OnFwd(KICKER_MOTOR,LIFT_PWR-9);
      if ( red < 15 )
      {
          break;
      }
   }
   Off(KICKER_MOTOR);
}

sub start_task(int finalWait)
{
   PlayToneEx(2000,100,100,FALSE);
   mv_forward(55,0);
   //mv_forward(0,55);
   Wait(1000);
   mv_forward(73,75);
   Wait(4000);
   mv_backward(80, 80);
   Wait(500);
   mv_forward(0,57);
   //mv_forward(57,0);
   Wait(1000+100);
   stop_motors();
   lift_ball();
   lift_ball();
   mv_forward(75, 75);
   Wait(finalWait);
}


sub  pick_up_ball()
{
    byte color_num = 0 ,  red = 0  , blue = 0  , green =0 ;
    bool  found_blue = false , found_red = false;
    int compass_reading;
    int time_to_move = 0 ;
    bool redstate = false;
    bool blue_state =false;
    int red_no_counter = 0 ;
do_again:
    mv_backward(37,40);
    
    Wait(400);
    found_blue = false ;
    found_red = false;

    OnRev(KICKER_MOTOR,LIFT_PWR);
    Wait(MOVE_DOWN_TIME);
    Off( KICKER_MOTOR) ;
    
    if(!redstate)
       mv_forward(37,40);
    else
       mv_forward(43,41);
    reset_timer();
    while(1)
    {
       color_sensor(color_num,red,blue,green);
       Wait(50);
       if ( blue > 15 )
       {
           found_blue = true;
           found_red  = false;
           break;
       }
       if ( red > 15 )
       {
           found_red = true;
           found_blue = false;
           break;
       }
       if ( timer() > TIMEOUTBALL )
       {
          break;
       }
       
    }
    stop_motors();
    mv_backward(37 , 40);
    Wait(450);
    stop_motors();
    lift_ball();
    Wait(200);
    if ( found_red )
    {
       mv_forward(39,41);
       Wait(550);
       throw_red_ball();
       mv_backward(37,40);
       while(INRANGE(line_sensor(),LINE_LIMIT1,LINE_LIMIT2));
       turn_robot(85,100,true);
       mv_forward(52,55);
       Wait(1100);
       turn_robot(140,190,false);
       redstate = true;

      // turn_robot(20, 0,false);
       goto do_again;
    }
    
    if ( found_blue)
    {
       mv_backward(37,40);
       while(INRANGE(line_sensor(),LINE_LIMIT1,LINE_LIMIT2));
       mv_backward(37,49);
       Wait(300);
       turn_back(0,15,false);
       mv_forward(37,40);
       Wait(250);
       move_forward_till_dark();
       drop_ball();
       redstate = false;
       red_no_counter +=2;
    }
    if ( ! ( found_blue || found_red ) )
    {
       mv_backward(37,40);
       while(INRANGE(line_sensor(),LINE_LIMIT1,LINE_LIMIT2));
       turn_robot(85,100,true);
       mv_forward(52,55);
       Wait(1100);
       turn_robot(140,190,false);
       redstate = true;
       ++red_no_counter;
       if ( red_no_counter > 5 )
       {

          red_no_counter  = 0 ;
          mv_backward(37,40);
          Wait(450);
          turn_back(0,15,true);
          mv_forward(98,100);
          Wait(750);
          turn_back(140,190,false);
          PlayToneEx(2000,100,100,FALSE);
          start_task(1000);
          goto do_again;
       }
       goto do_again;
    }
    
    found_red = false ;
    found_blue = false;


}

sub mov_backward()
{
   OnRev(LEFT_MOTOR, BCK_PWR_LEFT);
   OnRev(RIGHT_MOTOR, BCK_PWR_RIGHT);

}


task main()
{

   byte color_num = 0 , red = 0  , blue  = 0 , green = 0;
   init_sensors();

   start_task(3000);
   while(1)
    {
        lift_ball();
        lift_ball();
        move_forward_till_dark();
        pick_up_ball();
        lift_ball();
        lift_ball();
        turn_back(140,190,true);
     }

   while(1)
   {
      ClearScreen();

      color_sensor(color_num,red,blue,green);
      NumOut(0,LCD_LINE1,compass_sensor_value());
      NumOut(0,LCD_LINE2,red);
      NumOut(0,LCD_LINE3,blue);
      NumOut(0,LCD_LINE4,green);
      Wait(35);


     /*
      int value = compass_sensor_value();
      
      if(value == 325)
      {
        NumOut(0,LCD_LINE1,compass_sensor_value());
        NumOut(0,LCD_LINE3,0);
        mov_forward();
        stop_motors();
      }
      
      if(value < 10 || value = 10)
      {
         NumOut(0,LCD_LINE5, 10000);
         stop_motors();

      }
     */
   }

}