// Motor speed and direction

#define MFWD 3
#define MFLT 2
#define MOFF 1
#define MREV 0


// Distance thresholds
#define XL3 527 
#define XL2 480
#define XL1 444

#define XR1 430
#define XR2 420
#define XR3 410

int value, x, dx;
int motl, motr;

task main()
{
  SetSensor(SENSOR_1,SENSOR_LIGHT);
  Wait(100); // Charge GP2D12 sensor
  SetUserDisplay(value,0); // Display distance (remove for RCX firmware 1.5)

  SetSensor(SENSOR_1,SENSOR_TOUCH); // Activate sensor
  Wait(5);
  SetSensorMode(SENSOR_1,SENSOR_MODE_RAW);
  value=SENSOR_1; // Get distance
  SetSensor(SENSOR_1,SENSOR_LIGHT); // Re-cahrge sensor for next time
  Wait(25);
  x=value;
  dx=0;

  until (false)
  {
    
    SetSensor(SENSOR_1,SENSOR_TOUCH);
    Wait(5); 
    SetSensorMode(SENSOR_1,SENSOR_MODE_RAW);
    value=SENSOR_1; // Get distance
    SetSensor(SENSOR_1,SENSOR_LIGHT); // and recharge
    Wait(15); // shorter delay because calculations take time!
  
    dx=x-value; //Distance variarion
    x=value;
    
    motl=MFWD;
    motr=MFWD;
    if (x>=XL1) // Close to the wall
    {
      if (x>=XL3)
      {
          motr=MREV; // Very close, turn in place
      }
      else if (x>=XL2) 
      {
          motr=MOFF; // close enough, turn 
      }
      else if (x>=XL1)
      {
          motr=MFLT; // a bit too close, shallow turn
      }
      
    }
    else // far from the wall
    {
      if (x<=XR2) // far from wall,
      {
        if (dx<2) // if small variation turn (avoids turning too short on salient angles)
            motl=MOFF;
        }
      else if (x<=XR1)
      {
          motl=MFLT; // a bit too far, shallow turn

      }
      
    }
    
    switch(motr)
    {
      case MFWD:
        OnFwd(OUT_C);
        break;
      case MFLT:
        Float(OUT_C);
        break;
      case MOFF:
        Off(OUT_C);
        break;
      case MREV:
        OnRev(OUT_C);
        break;
    }
    switch(motl)
    {
      case MFWD:
        OnFwd(OUT_A);
        break;
      case MFLT:
        Float(OUT_A);
        break;
      case MOFF:
        Off(OUT_A);
        break;
      case MREV:
        OnRev(OUT_A);
        break;
    }
        
  }
}