#include "navigation3.nxc"
#include "lab6_forklift_functions.nxc"

#define ARM_LENGTH                    230 // mm
#define HALF_BOX_SIZE                 50  // mm
#define US_PORT                       S1
#define LT_PORT                       S3
#define SEEING_BOX                    (Sensor(LT_PORT) < initLight-5)
#define US_OFFSET_Y                   0
#define US_OFFSET_X                   40
#define FWD_SENSE_OFFSET_ANGLE   8000
#define REG_ROTATE_BOT(x)        OnFwdRegEx(OUT_B,x,OUT_REGMODE_SPEED,RESET_NONE);OnRevRegEx(OUT_C,x,OUT_REGMODE_SPEED,RESET_NONE);

int initLight;

long sweepFindLight() {
  long startAngle=0,endAngle,initAngle=angle;
  // start scanning for object:
  resetAngleCounter();
  while (true) {
    if (startAngle == 0) {
      if ( SEEING_BOX ) {
        PlayTone(880,200);
        startAngle = getAngleCounter();
      }
    } else if ( !SEEING_BOX ) {
      endAngle = getAngleCounter();
      Wait(20);
      if (!SEEING_BOX)  {
        PlayTone(880,200);
        break;
      }
    }
    Wait(20);
  }
  return initAngle+(startAngle+endAngle)/2;
}

task main () {

  long objectDirection;
  SetSensorType(LT_PORT,SENSOR_TYPE_LIGHT_INACTIVE);  // Configure light sensor
  SetSensorMode(LT_PORT,SENSOR_MODE_PERCENT);  // Configure light sensor
  SetSensorLowspeed(US_PORT);

  start odometry;

  initLight = Sensor(LT_PORT);
  calibrateForks();

  Wait(400);

  
  
  OnFwdRegEx(OUT_BC,15,OUT_REGMODE_SPEED,RESET_NONE);
  
  long avgDistCM,avgDistSum=0,minDistCM=255;

  int count=0;
  while (count < 5) {
    int sense = SensorUS(US_PORT);
    if (sense < 100) {
      avgDistSum += sense;
      if (sense<minDistCM)
        minDistCM = sense;
      count++;
    } else {
      count = 0;
    }
  }
  avgDistCM = avgDistSum / count;
  PlayTone(440,200);
  
  OffEx(OUT_BC,RESET_NONE);
  Wait(500);
  turnTo(-90000);

  if ( avgDistCM > 32 ) {
    long initTach = MotorTachoCount(OUT_B);
    OnFwdRegEx(OUT_BC,15,OUT_REGMODE_SPEED,RESET_NONE);
    until (uM_PER_DEGREE*(MotorTachoCount(OUT_B) - initTach) > (minDistCM-20)*10000);
  }
  
  // NOW USING LIGHT SENSOR ONLY

  while (true) {
    long initTach = MotorTachoCount(OUT_B);
    OnFwdRegEx(OUT_BC,15,OUT_REGMODE_SPEED,RESET_NONE);
    until (MotorTachoCount(OUT_B) - initTach > 100);
    OffEx(OUT_BC,RESET_NONE);
    Wait(500);
    turnTo(-90000);
    Wait(500);
    REG_ROTATE_BOT(5); // turn slowly, looking for box
    until (angle > 330000 || SEEING_BOX); // stop if we see the box or we hit limit
    if (SEEING_BOX)
      break; // we found the box!
    // otherwise we didn't see it, so we'll move forward and scan again
    OffEx(OUT_BC,RESET_NONE);
    Wait(500);
    turnTo(-90000);
  }
  
  // okay, if we get here, we just saw the box with the light sensor for the first time

  
  PlayTone(660,200);

  OffEx(OUT_BC,RESET_NONE);
  
  Wait(500);
  
  int angleDeg = angle/1000;
  long yTarget = yPosMM + ARM_LENGTH*Cos(angleDeg)/100 + HALF_BOX_SIZE - 20;

  // turn to 20 degrees so we can get behind the box
  turnTo(20000);

  
  Wait(500);
  
  OnFwdRegEx(OUT_BC,15,OUT_REGMODE_SPEED,RESET_NONE);
  
  until ( yPosMM > yTarget); // go until the robot is at the same (rough) y-pos as the box

  OffEx(OUT_BC,RESET_NONE);

  Wait(1500);

  
  turnTo(-90000);
  Wait(500);
  
  if ( !SEEING_BOX ) {
    // let's move forward just a bit
    OnFwdRegEx(OUT_BC,15,OUT_REGMODE_SPEED,RESET_NONE);
    until ( SEEING_BOX );
    Wait(300);
    OffEx(OUT_BC,RESET_NONE);
    Wait(500);
  }
  
  turnTo(-60000);
  Wait(500);
  REG_ROTATE_BOT(-5);
  objectDirection = sweepFindLight();
  OffEx(OUT_BC,RESET_NONE);
  Wait(500);
  turnTo(objectDirection);
  Wait(1000);

  OnFwdRegEx(OUT_BC,15,OUT_REGMODE_SPEED,RESET_NONE);
  until ( !SEEING_BOX );
  long xTarget = xPosMM+40;
  until ( xPosMM > xTarget )
    if ( SEEING_BOX )
      xTarget = xPosMM+40;
  Wait(400);
  OffEx(OUT_BC,RESET_NONE);
  
  Lift();


}

