#include "NXCDefs.h"

//#define DEBUG           //Create and write to the debug file
#define FULLCIRCLE 760  //degrees turned by one wheel to bring robot full circle
#define DIST 168        //cm robot travels when wheel turns 3600 degrees - going 100% on a lamintated map we are slipping a bit - see tenrev.nxc
#define NEAR 50
#define CLAWMOTOR OUT_A // motor port for claw
#define MAXREADING 50   // ignore readings over this range - they are more likely to be a wall rather than our small target
#define STOP 2          // stop this many cm from target
#define lost 490        // sensor completely off line - 480 OK except for grey lines
#define SpeedTurn -60   // speed to turn in place
#define TwiceAround 7550 // Degrees turned by C wheel going twice around the circle 7600 seems good for charged batteries and 7500 flat batteries

int lowSensor = 255;            // Lowest sensor reading found
int lowSensorTurns = 30000;  // High value stops us thinking we are lost before we have found target
int liDegrees, liDegreesB, liDegreesC;
int Estimate = 255;              // Best estimate to target
int originalEstimate;      // Estimate from first scan
int sensorEstimate;        // Estimate to target using sonar or light sensor
int liDistance;             //
int lowEstimate = 30000;
int lowTurns = 0;
int Accuracy;
int liRaw;
int SV, lastSV;
int SearchAngle = 120;
long currTime, lastTime, timeDiff;
int finalRun;
int bSpeed=-100, cSpeed=-10;
int lowSpeed=-55;        // Lowest speed - try -50 if battery is under 9V
int liTurn;

#ifdef DEBUG

byte fileHandle;
short fileSize;
short bytesWritten, bytesRead;

void writeNum (
  string asBefore,          // String to output before the number
  long aiNum)
{
  string lsOutput;

  lsOutput = NumToStr(aiNum);           // String form of number
  lsOutput = StrCat(asBefore,lsOutput);
  WriteString(fileHandle, lsOutput, bytesWritten);
}

void writeLnNum (
  string asBefore,          // String to output before the number
  long aiNum)
{
  string lsOutput;

  lsOutput = NumToStr(aiNum);           // String form of number
  lsOutput = StrCat(asBefore,lsOutput);
  WriteLnString(fileHandle, lsOutput, bytesWritten);
}
#endif

bool readUS (
  )
{
/*
  Returns TRUE if the reading from the sonar or light sensor are less than any previous reading (after adjusting
  for moving closer to the target)
*/
bool result;
string lsWrite;
string lsTmp;
int sonarEstimate;       // Estimated cm to target using sonar
int turnEstimate;        // Estimated cm to target using lowest found and wheel rotation
char resultRead;

result = FALSE;
liRaw = SensorUS(IN_4);
liDegreesB = MotorRotationCount(OUT_B);
liDegreesC = MotorRotationCount(OUT_C);

if (liRaw<=14)
  sonarEstimate = liRaw - 3
else
  if (liRaw<=20)
    sonarEstimate = liRaw - 4
  else
    if (liRaw<=22)
      sonarEstimate = liRaw - 3
    else
      if (liRaw==255)
        sonarEstimate = 255
      else
        sonarEstimate = liRaw - 1;                         // Estimate using sonar

liDegrees = liDegreesB + liDegreesC;
turnEstimate = lowEstimate + (lowTurns - liDegrees) * DIST / 7200;  // Estimate using lowest previous estimate and distance travelled since

sensorEstimate = sonarEstimate;
  
if (sensorEstimate <= MAXREADING)
  {
  if ((turnEstimate < sensorEstimate)||(sensorEstimate >= lowEstimate))  // Previous estimate was lower - when turning we might drift slightly away
    Estimate = turnEstimate            // Use previous reading to estimate distance to target
  else
    {                                  // We have a lower than previous estimate
    Estimate = sensorEstimate;         // Sensors have given the best estimate
    lowEstimate = sensorEstimate;      // Save this estimate
    lowTurns = liDegrees;              // Save the degrees where we found this low point
    #ifdef DEBUG
    writeLnNum("Lowest is now ", lowEstimate);
    #endif
    }
  if (sensorEstimate < lowSensor)
    {
    lowSensor = sensorEstimate;
    lowSensorTurns = liDegrees;
    result = TRUE;
    }
  }
#ifdef DEBUG
writeNum(" ",liDegreesB);            // Degrees turned by B
writeNum(",",liDegreesC);            // Degrees turned by C
writeNum(",",sonarEstimate);         // Estimate from US sensor
writeNum(",",turnEstimate);          // Estimate from lowest previous estimate and motor rotation
writeLnNum(",",Estimate);            // Estimate of cm to target
#endif
// Estimate the accuracy of the US sensor reading
if (Estimate > 18)
  Accuracy = 3
else
  switch (Estimate)
    {
    case 11:
      Accuracy = 26                  // It really is bad at this measurement
      break;
    case 12:
      Accuracy = 14
      break;
    case 9:
    case 10:
    case 13:
    case 14:
    case 15:
    case 16:
    case 17:
    case 18:
      Accuracy = 10;
      break;
    default:
      Accuracy = 2;
      break;
    }
return result;
}

void scan (
  int scandeg,                                         // degrees to scan left and right of centre
  bool scanCentre                                      // scanning from centre of path - otherwise turn right only
  )
{
/*
  Turn SCANDEG left then turn right through 2 * SCANDEG while reading the sensor.
  Turn the robot back to the lowest distance found.
*/
int liStop;   // Degrees motor B stops after right turn left
int liStart;  // Start position of motor B
string lsWrite;
string lsTmp;
int liMaxDegreesB, liMaxDegreesC;
int lowSensorTurnsB, lowSensorTurnsC;
int liHigh;

lowSensor = 255;         // Lowest US sensor reading found
lowSensorTurns = 30000;  // High value stops us thinking we are lost before we have found target
lowEstimate = 30000;

lowSensorTurnsB = MotorRotationCount(OUT_B)             //current angle - come back here if we don't find a target
lowSensorTurnsC = MotorRotationCount(OUT_C)             //current angle - come back here if we don't find a target

if (scanCentre)
  RotateMotorEx (OUT_BC, 80, scandeg, -100, true);       //turn SCANDEG deg left
else
  {
  lowSensorTurnsB = lowSensorTurnsB - scandeg;
  lowSensorTurnsC = lowSensorTurnsC + scandeg;
  }
liStart = lowSensorTurnsB;                              //save to work out what angle we turn overall
liStop = lowSensorTurnsB - scandeg;                     //this is where we will stop the scan

#ifdef DEBUG
lsWrite = NumToStr(liStop);
lsTmp = NumToStr(MotorRotationCount(OUT_B));
lsWrite = StrCat("Scanning from ", lsTmp, " to ", lsWrite);
WriteLnString(fileHandle, lsWrite, bytesWritten);
#endif

//Wait(300);                           // Stop movement
OnFwdSync(OUT_BC, 60, 100);          // Start turning right
liHigh = 0;
do {
  if (readUS())
    {
    liHigh = sensorEstimate + 2;    // Accuracy;  taking some more risks
    liMaxDegreesB = liDegreesB;
    liMaxDegreesC = liDegreesC;
    lowSensorTurnsB = liDegreesB;
    lowSensorTurnsC = liDegreesC;
    }
  else
    if (sensorEstimate < liHigh)
      {
      liMaxDegreesB = liDegreesB;
      liMaxDegreesC = liDegreesC;
      }
    else
      if ((lowSensor <= MAXREADING)&&(liDegreesB + 30 < lowSensorTurnsB))
        break;                          // No point going any further - Don't do this with multiple targets!
  }while(MotorRotationCount(OUT_B) > liStop);
Off(OUT_BC);
Wait(100);         // Make sure we have no more movement
// Always seems to be off in the calculation by about 10 - I don't know why - but only in first call!!!!!!!!!!!
//if (liHigh==0)
//  liStop = (MotorRotationCount(OUT_B) - lowSensorTurnsB - MotorRotationCount(OUT_C) + lowSensorTurnsC) / 2 + 10  // back to start position
//else
  liStop = (MotorRotationCount(OUT_B) - (lowSensorTurnsB + liMaxDegreesB) / 2 -
           MotorRotationCount(OUT_C) + (lowSensorTurnsC + liMaxDegreesC) / 2) / 2;

#ifdef DEBUG
writeNum("Finished scanning at ", MotorRotationCount(OUT_B));
writeNum(",", MotorRotationCount(OUT_C));
writeLnNum(" turning back by ", liStop);
#endif

RotateMotorEx (OUT_BC, 70, liStop, -100, true);       //turn back to the lowest measure
//Wait(300);         // Make sure we have no more movement
#ifdef DEBUG
writeNum("Turned back to ", MotorRotationCount(OUT_B));
writeLnNum(",", MotorRotationCount(OUT_C));
#endif
}

bool advance (
  )
{
/*
  Robot should advance towards a target which should be directly in front of the robot at about the distance
  indicated.  If we stop with 8 cm of the target the function will return TRUE.
  If the robot looses the target the function will return FALSE.
  
  Maximum range for the can is about 66cm (paper wrapped can). At this distance only 20 deg movement to either side will
  make the can disappear. At 50cm it takes 30 deg movement to make the can disappear. At 30cm it takes 40deg on the left
  but on 35 on the right. We register 10 to 20 cm as 24cm. At 15cm the angle is also 40deg left and 35 right.
*/
string read;
string lsWrite;
string lsTmp;
int liNoGood;          // Bad measurement
int startDegrees;      // Motor position when advance called

startDegrees = liDegrees;

while(Estimate > STOP)
  {
  readUS ();
  if ((lowSensor==255)&&(sensorEstimate>MAXREADING)&&((liDegrees - startDegrees)>FULLCIRCLE * 3))  // Have found nothing after three full rotations
    lowSensor = MAXREADING;                 // Force a scan
  if (Estimate > 8)           // Check if we should turn if more than 8 cm from target
    {
    if ((liDegrees > lowSensorTurns + Accuracy * FULLCIRCLE / 10)||  // Advance two motors one half turn - if we are not reading closer something is wrong
     (sensorEstimate > lowSensor + Accuracy))      // Getting significantly further away
      {
      readUS ();                               // This is a second chance - sometimes we get a bad reading
      if ((liDegrees > lowSensorTurns + Accuracy * FULLCIRCLE / 10)||  // Advance two motors one half turn - if we are not reading closer something is wrong
         (sensorEstimate > lowSensor + Accuracy))      // Getting significantly further away
        {
        Off(OUT_BC);
        Wait(100);                             // stop forward motion
        liNoGood = sensorEstimate + Accuracy;                      // This value is unacceptable
        lowSensor = sensorEstimate + Accuracy;                      // We will take anything less than this
        lowSensorTurns = liDegrees;
        scan (50, TRUE);
        if (lowSensor >= liNoGood)                // Did not get closer
          {
          Wait(100);                             // let shaking stop
          readUS ();
          if (lowSensor >= liNoGood)              // Did not get closer
            {
            #ifdef DEBUG
            writeNum("Advance returns FALSE with sensor reading ", sensorEstimate);
            writeNum(" low reading ", lowSensor);
            writeLnNum(" needed ", liNoGood);
            #endif
            PlayFile ("Woops.rso");
            return FALSE;
            }
          }
        lowSensor = liNoGood;                 // Reset this value - will recalculate lowest on this heading - may be slightly more than we found in scan
        OnFwdSync(OUT_BC, 50, 0);            // Ahead slow
        Wait (100);                          // Give it a chance to get going - otherwise at long distance we get 255 readings
        }
      }
    }
  if (Estimate > 10)
    OnFwdSync(OUT_BC, 100, 0)
  else
    OnFwdSync(OUT_BC, 50, 0);            // Ahead slow
  }
#ifdef DEBUG
Off(OUT_BC);
WriteLnString(fileHandle, "Advance returns TRUE", bytesWritten);
#endif
return TRUE;
}

bool FollowLine()
{
int Threshold1=520;          // Light - too far away from the line 500 works well but can pick up markings at the end of the mat
int Threshold2=620;          // Dark - too far over the line
int lastbSpeed=0, lastcSpeed=0;
int stopHere;
int rotateDiff, rotateStop, degreesB, degreesC;
long lastOnLine;             // Last time on the line

stopHere = MotorRotationCount(OUT_C) - TwiceAround;
if (liTurn > -300)
  stopHere = stopHere - 80;                    // Need to travel about one inch more when mount in right half - Otherwise we don't achive two revolutions
  
while (TRUE)
  {
  // read the light sensor value
  SV = SensorRaw(IN_3);
  currTime = CurrentTick();
  degreesB = MotorRotationCount(OUT_B);
  degreesC = MotorRotationCount(OUT_C);
  
  if (degreesC < stopHere)
    return TRUE;                               // we have gone around twice
    
  // lost the line altogether
  if (SV > lost)
    {
    lastOnLine = currTime
    }
  else
    {
    if ((lastOnLine+20<currTime)&&(cSpeed==-100))           // 20 seems a good delay
      {
      rotateDiff = MotorRotationCount(OUT_B) - MotorRotationCount(OUT_C);
      rotateStop = rotateDiff + SearchAngle;
      Off(OUT_B);
      OnFwd(OUT_C, SpeedTurn);
      lastSV = 0;
      do{
        SV = SensorRaw(IN_3);
        currTime = CurrentTick();
        degreesB = MotorRotationCount(OUT_B);
        degreesC = MotorRotationCount(OUT_C);
        rotateDiff = degreesB - degreesC;
        if (SV != lastSV)
          {
          timeDiff = currTime - lastTime;
          lastTime = currTime;
          }
        lastSV = SV;
        }while ((rotateDiff < rotateStop)&&(SV < Threshold1));
      if (SV < lost)
        return FALSE     // Completely lost
      }
    }
  lastbSpeed = bSpeed;
  lastcSpeed = cSpeed;
  if (SV > Threshold2)
    {
    bSpeed = -90;
    cSpeed = -100;
    }
  else
    if (SV > Threshold1)
      {
      bSpeed = -100;
      cSpeed = -100;
      }
    else
      if (SV > lost)
        {
        bSpeed = -100;
        if ((cSpeed == -99)||(cSpeed == lowSpeed))      // moving more over the line
          cSpeed = -99
        else
          cSpeed = -90;       // moving less over the line
        }
      else
        {
        bSpeed = -100;
//        if (cSpeed!=-10)
          cSpeed = lowSpeed;
        }
  if (lastbSpeed != bSpeed)
    OnFwd(OUT_B, bSpeed);
  if (lastcSpeed != cSpeed)
    OnFwd(OUT_C, cSpeed);

  timeDiff = currTime - lastTime;
/*
  Require a 50K log to do this
  #ifdef DEBUG
  writeNum(" ",timeDiff);
  writeNum(",",degreesB);
  writeNum(",",degreesC);
  writeNum(",",SV);
  writeNum(",",bSpeed);
  writeLnNum(",",cSpeed);
  #endif
*/
  lastTime = currTime;
  currTime = CurrentTick();
  }
}

task main()
{
string elapsedTimeString;
string digit;
long startTime, elapsedTime;
int lix;
int xPos, yPos;
int stopB;      // C wheel position to stop
string numFileName;
#ifdef DEBUG
// These lines are OK before the beep since they will be removed for timed runs
DeleteFile("alog.txt");                           // These lines must be commented out in final tests
CreateFile("alog.txt", 10000, fileHandle);
#endif

Wait (9750);    // 10 seconds total to set up camera
PlayTone(440, 250); // Play 'A' for one quarter second - Time starts at the end of this
startTime = CurrentTick();

SetSensorLowspeed(IN_4);
scan(100, FALSE);
originalEstimate = Estimate;        // Save the estimate from this scan
OnFwdSync(OUT_BC, 100, 0)
OnRev(OUT_A, 70);                      //close claws a little
// set sensor type and mode
SetSensorType(IN_3, IN_TYPE_LIGHT_ACTIVE);
SetSensorMode(IN_3, IN_MODE_RAW);

Wait (200);                  // Give it a chance to get going - otherwise at long distance we get 255 readings
Off(OUT_A);
Estimate = Estimate + 2;    // Sometimes the first estimate is too little - advance will fix it
if (advance())
  {
//  Wait(100);
  OnRev(OUT_A, 70);                      //close claws
  Wait(120);
  Off(OUT_A);
  liTurn = MotorRotationCount(OUT_C) - MotorRotationCount(OUT_B);   //Turn back to centre
  stopB = 480 - liTurn;
  #ifdef DEBUG
  writeLnNum("stopB ", stopB);
  #endif
  liTurn = liTurn - 560;                // Turn light sensor over the line
  OnRev(OUT_B, 100);
  if (MotorRotationCount(OUT_B) > stopB)                // Need to back up
    {
    OnRev(OUT_C, 100);
    while (MotorRotationCount(OUT_B) > stopB)
      {
      #ifdef DEBUG
      writeNum(" ",MotorRotationCount(OUT_B));          // Degrees turned by B
      writeLnNum(",",MotorRotationCount(OUT_C));        // Degrees turned by C
      #endif
      }
    stopB = FULLCIRCLE * 7 / 12;
    }
  else
    stopB = FULLCIRCLE / 2;
  OnFwd(OUT_C, 100);                                    // B motor keeps going, C reverses
  #ifdef DEBUG
  writeLnNum("stopB ", stopB);
  writeNum(" ",MotorRotationCount(OUT_B));              // Degrees turned by B
  writeLnNum(",",MotorRotationCount(OUT_C));            // Degrees turned by C
  #endif
  while ((MotorRotationCount(OUT_C) - MotorRotationCount(OUT_B)) < stopB)
    {
    #ifdef DEBUG
    writeNum(" ",MotorRotationCount(OUT_B));            // Degrees turned by B
    writeLnNum(",",MotorRotationCount(OUT_C));          // Degrees turned by C
    #endif
    }
  #ifdef DEBUG
  writeNum(" ",MotorRotationCount(OUT_B));              // Degrees turned by B
  writeLnNum(",",MotorRotationCount(OUT_C));            // Degrees turned by C
  #endif
  OnFwd(OUT_C,cSpeed);
  #ifdef DEBUG
  writeNum("Zero?",MotorRotationCount(OUT_B));          // Degrees turned by B
  writeLnNum(",",MotorRotationCount(OUT_C));            // Degrees turned by C
  #endif
  if (FollowLine())
    {
    #ifdef DEBUG
    writeNum("After FollowLine",MotorRotationCount(OUT_B));            // Degrees turned by B
    writeLnNum(",",MotorRotationCount(OUT_C));            // Degrees turned by C
    #endif
    Off (OUT_BC);
    Wait (100);
    SV = SensorRaw(IN_3);
    if (SV > 620)
      stopB = 0
    else
      if (SV > 520)
        stopB = 40
      else
        if (SV > lost)
          stopB = 60
        else
          stopB = 80;
    stopB = stopB + MotorRotationCount(OUT_C) - MotorRotationCount(OUT_B) + liTurn;  // The extra is because the robot keeps turning after Off
    #ifdef DEBUG
    writeNum("Turning ", liTurn);
    writeLnNum(", Adjust ", stopB);
    writeNum("Light ",SV);
    writeNum(", Before Rotation ",MotorRotationCount(OUT_B)); // Degrees turned by B
    writeLnNum(",",MotorRotationCount(OUT_C));            // Degrees turned by C
    #endif
    OnFwd(OUT_B, 100);
    OnRev(OUT_C, 100);
    #ifdef DEBUG
    writeLnNum("stopB ", stopB);
    writeNum(" ",MotorRotationCount(OUT_B));              // Degrees turned by B
    writeLnNum(",",MotorRotationCount(OUT_C));            // Degrees turned by C
    #endif
    while ((MotorRotationCount(OUT_C) - MotorRotationCount(OUT_B)) > stopB)
      {
      #ifdef DEBUG
      writeNum(" ",MotorRotationCount(OUT_B));            // Degrees turned by B
      writeLnNum(",",MotorRotationCount(OUT_C));          // Degrees turned by C
      #endif
      }
    #ifdef DEBUG
    writeNum(" ",MotorRotationCount(OUT_B));              // Degrees turned by B
    writeLnNum(",",MotorRotationCount(OUT_C));            // Degrees turned by C
    #endif
    Off(OUT_BC);
    #ifdef DEBUG
    writeNum("After Rotation",MotorRotationCount(OUT_B));            // Degrees turned by B
    writeLnNum(",",MotorRotationCount(OUT_C));            // Degrees turned by C
    #endif
    Wait (100);
    #ifdef DEBUG
    writeNum("After 100 ",MotorRotationCount(OUT_B));            // Degrees turned by B
    writeLnNum(",",MotorRotationCount(OUT_C));            // Degrees turned by C
    #endif
    lowSensor = 255;         // Lowest US sensor reading found
    lowSensorTurns = 30000;  // High value stops us thinking we are lost before we have found target
    lowEstimate = 30000;
    lowTurns = 0;
    Estimate = 255;
    stopB = MotorRotationCount(OUT_C) - 120;
    #ifdef DEBUG
    writeNum("Zero?",MotorRotationCount(OUT_B));            // Degrees turned by B
    writeLnNum(",",MotorRotationCount(OUT_C));            // Degrees turned by C
    #endif
    //  scan(50,TRUE);                 // We seem to be accurate enough to not need this
    Estimate = originalEstimate;       // Restore the original estimate - this seems to work better than a single new reading
    OnFwdSync(OUT_BC, 100, 0)
    Wait (200);                        // Give it a chance to get going - otherwise at long distance we get 255 readings
    if (advance())
      {
      OnRevSync(OUT_BC, 100, 0);
      OnFwd(OUT_A, 100);                  // open claws
      Wait(50);                           // allow less time for opening than closing
      OnFwd(OUT_A, 80);                   // open claws
      Wait(180);                          // allow less time for opening than closing
      Off(OUT_A);
      #ifdef DEBUG
      writeLnNum("Claws open",MotorRotationCount(OUT_C));            // Degrees turned by B
      #endif

      while (MotorRotationCount(OUT_C) > stopB)
        {
        }
      Off(OUT_BC);
      }
    #ifdef DEBUG
    writeLnNum("Rev",MotorRotationCount(OUT_C));            // Degrees turned by B
    #endif
    elapsedTime = CurrentTick() - startTime;
    #ifdef DEBUG
    writeLnNum("Elapsed ", elapsedTime);
    #endif
    // display elapsed time
    TextOut(0, LCD_LINE1, "NXTasy.org No. 2");
    TextOut(0, LCD_LINE2, "  Move the Ball!");
    GraphicOut(0, 35, "LrensRobot.ric");
    xPos = 20;
    yPos = 10;
    GraphicOut(0, yPos, "Timeglass.ric");
    elapsedTimeString = NumToStr(elapsedTime);
    lix = 0;
    repeat (StrLen(elapsedTimeString))
      {
      xPos = xPos + 12;
      digit = SubStr(elapsedTimeString, lix, 1);
      numFileName = StrCat("Large", digit, ".ric");
      GraphicOut(xPos, yPos, numFileName);
      lix++;
      }
//    NumOut(0, LCD_LINE6, MotorRotationCount(OUT_C));    This line was accidentally left in for some runs
    }
  }
PlayTone(440, 250);                       // Play 'A' for one quarter second
#ifdef DEBUG
CloseFile(fileHandle);
#endif
// let user see the last message
Wait (15000);                             // Film the screen
}