////////////////////////////////////
//Directions
////////////////////////////////////
#define UP 1
#define DOWN 0
#define CLOCKWISE 1
#define ANTICLOCKWISE 0
////////////////////////////////////
#define SESITYVITY_DIFF 65
#define MOTOR_SPEED 3 //0 to 9
#define TOUCH_SENSOR SENSOR_1
#define LIGHT_SENSOR SENSOR_3
#define ARM_MOTOR OUT_A
#define TURN_MOTOR OUT_C
#define RANGE_START 37
#define RANGE_END 45

int lastlevel;           // To store the previous level
int arm_direction;
int turn_direction;
int flag=0;

task main()
{
  SetSensor(TOUCH_SENSOR, SENSOR_TOUCH);
  SetSensorMode(TOUCH_SENSOR, SENSOR_MODE_BOOL);

  SetSensor(LIGHT_SENSOR, SENSOR_LIGHT);
  SetSensorMode(LIGHT_SENSOR, SENSOR_MODE_PERCENT);
  ClearSensor(TOUCH_SENSOR);
  SetPower(TURN_MOTOR,8);
  //TurnClockWise(100);
  //TurnAntiClockWise(100);
  SetPower(TURN_MOTOR,1);
  SelectDisplay(DISPLAY_SENSOR_3); // view sensor 1
  GoUptoScanLimit();

  start CheckForObject;
  
  //MaxArmDown();
  //MaxArmLift();
  //start CheckMaxArmLift;

  //LeftRight();
}


task CheckForObject()
{
    ClearTimer(0);
    while(true)
    {

        if (Timer(0)>20)
        {
         ClearTimer(0);
         SetDirection(TURN_MOTOR,OUT_TOGGLE);
        }

        if((LIGHT_SENSOR >= RANGE_START) &&  (LIGHT_SENSOR <= RANGE_END))
        {

          SetDirection(TURN_MOTOR,OUT_TOGGLE);
          Off(TURN_MOTOR);
          SetDirection(TURN_MOTOR,OUT_TOGGLE);
          PlaySound(SOUND_CLICK);
          PlaySound(SOUND_CLICK);
          PlaySound(SOUND_CLICK);
          PlaySound(SOUND_CLICK);
          PickupAndDrop();
        }
        On(TURN_MOTOR);
     }

}

void PickupAndDrop()
{
   PickUpObject();
   DropOffObject();
}

void PickUpObject()
{
 SetDirection(TURN_MOTOR,OUT_TOGGLE);
 Off(TURN_MOTOR);
 MaxArmDown();
 GoUptoScanLimit();
}

void DropOffObject()
{
 On(TURN_MOTOR);
 Wait(200);
 //SetDirection(TURN_MOTOR,OUT_TOGGLE);
 Off(TURN_MOTOR);
 MaxArmDown();
 SetDirection(TURN_MOTOR,OUT_TOGGLE);
 On(TURN_MOTOR);
 Wait(80);
 Off(TURN_MOTOR);
 GoUptoScanLimit();
 SetDirection(TURN_MOTOR,OUT_TOGGLE);
}

void GoUptoScanLimit()
{
  MaxArmLift();
  ArmMoveDown(270);
  
}

task CheckMaxArmLift()
{
   //TurnAntiClockWise(200);
     while(true)
     {
                if(TOUCH_SENSOR==1)
                {
                    //Now check that which sensor is pressed
                    switch(arm_direction)
                    {
                     case UP: //Upper sensor pressed (Which is pressed in the Middle of arm)
                          //SetDirection(ARM_MOTOR,OUT_TOGGLE);
                          Off(ARM_MOTOR);
                          //ClearSensor(TOUCH_SENSOR);
                          ArmMoveDown(50);

                          PlaySound(SOUND_CLICK);
                          PlaySound(SOUND_CLICK);
                          PlaySound(SOUND_CLICK);
                          PlaySound(SOUND_CLICK);
                          break;
                     case DOWN: //Upper sensor pressed (Which is pressed in the Middle of arm)
                          Off(ARM_MOTOR);
                          //ClearSensor(TOUCH_SENSOR);
                          ArmMoveUp(50);

                          PlaySound(SOUND_CLICK);
                          PlaySound(SOUND_CLICK);
                          PlaySound(SOUND_CLICK);
                          PlaySound(SOUND_CLICK);
                          break;
                    }
                }
     }
}


void LeftRight()
{
  ClearTimer(0);
  while(true)
  {
     if (true)
     {
       ClearTimer(0);
       if(flag==0)
       {
        flag=1;
        TurnClockWise(400);
       }
       else
       {
        flag=0;
        TurnAntiClockWise(400);
       }
     }
  }
}


void ArmMoveUp(int nTime)
{
  arm_direction=UP;

  SetDirection(ARM_MOTOR, OUT_REV);
  On(ARM_MOTOR);
  Wait(nTime);
  Off(ARM_MOTOR);
}

void ArmMoveDown(int nTime)
{
  arm_direction=DOWN;

  SetDirection(ARM_MOTOR, OUT_FWD);
  On(ARM_MOTOR);
  Wait(nTime);
  Off(ARM_MOTOR);
}

void TurnClockWise(int nTime)
{
  SetDirection(TURN_MOTOR, OUT_FWD);
  On(TURN_MOTOR);
  Wait(nTime);
  Off(TURN_MOTOR);
}

void TurnAntiClockWise(int nTime)
{
  SetDirection(TURN_MOTOR, OUT_REV);
  On(TURN_MOTOR);
  Wait(nTime);
  Off(TURN_MOTOR);
}

void MaxArmLift()
{
  arm_direction=UP;

  SetDirection(ARM_MOTOR, OUT_REV);
  On(ARM_MOTOR);
  while(TOUCH_SENSOR==0)
  {
  }
  Off(ARM_MOTOR);
  ArmMoveDown(50);
}

void MaxArmDown()
{
  arm_direction=DOWN;

  SetDirection(ARM_MOTOR, OUT_FWD);
  On(ARM_MOTOR);
  while(TOUCH_SENSOR==0)
  {
  }
  Off(ARM_MOTOR);
  ArmMoveUp(50);
}