/*
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Program Name : metsensing_multi.nxc
description  : Program developed for the escape contest
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/


/* PROGRAM CONSTANTS */

#define LEFT_TOUCH IN_2
#define RIGHT_TOUCH IN_1
#define RIGHT_MOTOR OUT_C
#define LEFT_MOTOR OUT_A

#define YES           1
#define NO            0

#define SPEED_PERCENT 50
#define TIMER_WAIT    1200
#define BUMP_COUNT    4
#define WAIT_TIME     300

#define TONE_FREQ     1000
#define TONE_VOL      100

#define MAX_WAIT       300
#define MAX_RANDOMWAIT 1000

float _timer;
int recent_bumps = 1;
int rotate_bumps = 0 ;
mutex moveMutex;
/*
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
subroutine name : forward
description   : function to move the robot forward
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
sub forward()
{
    OnFwd(LEFT_MOTOR, SPEED_PERCENT);
    OnFwd(RIGHT_MOTOR, SPEED_PERCENT);
}
/*
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
subroutine name : backward
description   : function to move the robot backward
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
sub backward()
{
    OnRev(LEFT_MOTOR, SPEED_PERCENT);
    OnRev(RIGHT_MOTOR, SPEED_PERCENT);
}

/*
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
subroutine name : right
description   : function to move the robot right
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
sub right()
{
    OnFwd(LEFT_MOTOR, SPEED_PERCENT);
    OnRev(RIGHT_MOTOR, SPEED_PERCENT);
}
/*
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
subroutine name : left
description     : function to move the robot left
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
sub left()
{
    OnFwd(RIGHT_MOTOR, SPEED_PERCENT);
    OnRev(LEFT_MOTOR, SPEED_PERCENT);
}
/*
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
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;
}


/*
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
subroutine name :  Random_avoid
description     :  after hitting an obstacle , turns left or right in a
                   random fashion
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/

sub Random_avoid()
{
    float wait;
    backward();
    Wait(WAIT_TIME);


    rotate_bumps++;
    if ( rotate_bumps >= 3 )
    {

       Wait(2*WAIT_TIME);
       rotate_bumps = 0 ;
    }
    if(Random(2) == 0)
      left();
    else
      right();
    wait = Random(MAX_RANDOMWAIT) + MAX_WAIT;
    PlayToneEx(TONE_FREQ,wait,TONE_VOL,FALSE);
    Wait(wait);


}
/*
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
subroutine name : right_avoid
description     : turns right after hitting the obstacle
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/

sub right_avoid()
{
    backward();
    Wait(WAIT_TIME);
    right();
    Wait(WAIT_TIME);
}
/*
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
subroutine name : left_avoid
description     : turns left after hitting the obstacle
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
sub left_avoid()
{
    backward();
    Wait(WAIT_TIME);
    left();
    Wait(WAIT_TIME);
}

/*
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
task name       : moveforward
description     : task to move the robot forward
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
task move_forward()
{
   while(YES)
   {
       Acquire(moveMutex);
       forward();
       Release(moveMutex);
   }
}
/*
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
task name       :  SENSORLEFT
description     : task to handle if left sensor has been touched
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
task SENSORLEFT()
{
  while(YES)
  {

    Acquire(moveMutex);
    if( SENSOR_1  == YES)
    {
                if(timer() < TIMER_WAIT)
                {
                    if(recent_bumps == BUMP_COUNT)
                    {
                        Random_avoid();
                        reset_timer();
                        recent_bumps = 1;

                    }
                    else
                    {
                        right_avoid();
                        reset_timer();
                        recent_bumps++;
                    }
                }
                else
                {
                    right_avoid();
                    reset_timer();
                    recent_bumps = 1;
                }
     }
     Release(moveMutex);
  }
}
/*
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
task name       :  SENSORIGHT
description     : task to handle if right sensor has been touched
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/

task SENSORRIGHT()
{
   while(TRUE)
   {

      Acquire(moveMutex);
      if(SENSOR_2 == YES)
      {
            if(timer() < TIMER_WAIT)
            {

                if(recent_bumps ==BUMP_COUNT)
                {
                    Random_avoid();
                    reset_timer();
                    recent_bumps = 1;
                }
                else
                {
                    left_avoid();
                    reset_timer();
                    recent_bumps++;
                }
            }
            else
            {
                left_avoid();
                reset_timer();
                recent_bumps = 1;
            }

      }
      Release(moveMutex);
    }

}
/*
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
task name       :  main
description     :  the main task
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
*/
task main()
{
    SetSensorTouch(LEFT_TOUCH);
    SetSensorTouch(RIGHT_TOUCH);
    reset_timer();
    Precedes(move_forward,SENSORLEFT,SENSORRIGHT);
}