///////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                               Playback of On-Brick Programming Files
//
///////////////////////////////////////////////////////////////////////////////////////////////////////

#pragma platform(NXT)
//#pragma fileExtension("sys") // Store this as a "sys" file extension!!

typedef enum
{
  cmdType_EMPTY           = 0x21,        // Empty
  cmdTypeForwardUntil     = 0x22,        // Forward until
  cmdTypeForwardFive      = 0x23,        // Forward 5 rotations
  cmdTypeBackLeftTwo      = 0x24,        // Back left 2 rotations
  cmdTypeTurnLeftUntil    = 0x25,        // Turn left until
  cmdTypeTurnLeftTwo      = 0x26,        // Turn left 2 rotations
  cmdTypeBackRightUntil   = 0x27,        // Back right until
  cmdTypeTurnRightUntil   = 0x28,        // Turn right until
  cmdTypeTurnRightTwo     = 0x29,        // Turn right 2 rotations
  cmdTypeBackLeftUntil    = 0x2A,        // Back left until
  cmdTypePlayTone1        = 0x2B,        // Tone 1
  cmdTypePlayTone2        = 0x2C,        // Tone 2
  cmdTypeBackwardUntil    = 0x2D,        // Backward until
  cmdTypeBackwardFive     = 0x2E,        // Backward 5 rotations
  cmdTypeBackRightTwo     = 0x2F,        // Back right 2 rotations
  cmdTypeINVALID          = 0x30,        // Invalid
  cmdTypeStop             = 0xFB,        // Invalid
  cmdTypeLoop             = 0xFC,        // Invalid

  waitTypeEMPTY           = 0x41,        // Empty
  waitTypeLIGHT           = 0x42,        // Light
  waitTypeSeekObject      = 0x43,        // Seek obj.
  waitTypeSOUND           = 0x44,        // Sound
  waitTypeTOUCH           = 0x45,        // Touch
  waitTwoSeconds          = 0x46,        // Wait 2 seconds
  waitFiveSeconds         = 0x47,        // Wait 5 seconds
  waitTenSeconds          = 0x48,        // Wait 10 seconds
  waitTypeDARK            = 0x49,        // Dark
  waitTypeINVALID         = 0x4A,        // Invalid

} TCommands;

const tMotor motorRight = motorB;
const tMotor motorLeft  = motorC;
const tSensors touch    = S1;
const tSensors light    = S2;
const tSensors mike     = S3;
const tSensors sonar    = S4;

const string sFileName = "Program.tmp";
TFileIOResult ioResult;
uword hFile;
uword nFileSize;
uword nProgramStep = -1;

TCommands getNextCommand()
{
  TCommands nCommand;

  ++nProgramStep;
  nOnBrickProgrammingStep = nProgramStep;
  ReadByte(hFile, ioResult, nCommand);
  return (ioResult == ioRsltSuccess) ? nCommand : cmdTypeINVALID;
}

void stopMotors()
{
  motor[motorRight] = 0;
  motor[motorLeft]  = 0;
}

void motorControl(TSynchedMotors nSyncType, int nPower, int nTurn, int nTarget)
{
  tMotor nPrimeMotor;

  nSyncedMotors      = nSyncType;
  nPrimeMotor        = (nSyncType == synchBC) ? motorB : motorC;
  nMotorEncoderTarget[nPrimeMotor] = nTarget;
  nSyncedTurnRatio   = nTurn;
  motor[nPrimeMotor] = nPower;
  while (abs(nMotorEncoder[nPrimeMotor] - nTarget) > 3)
  {}
  return;
}

TFileIOResult openFileAndSkipHeader()
{
  OpenRead(hFile, ioResult, sFileName, nFileSize);
  if (ioResult == ioRsltSuccess)
  {
    int i;
    const int kFileHeaderSize = 8;

    for (i = 0; i < kFileHeaderSize; ++i)  // Skip file header portion of file
      getNextCommand();
    nProgramStep = -1;
  }
  return ioResult;
}


bool processNextCommand()
{
  switch (getNextCommand())
  {
  case cmdTypeForwardUntil:    motorControl(synchBC,  75,  100, 0);        break;
  case cmdTypeForwardFive:     motorControl(synchBC,  75,  100, 360 * 5);  break;
  case cmdTypeTurnLeftUntil:   motorControl(synchCB,  75,   50, 0);        break;
  case cmdTypeTurnLeftTwo:     motorControl(synchCB,  75,   50, 360 * 2);  break;
  case cmdTypeTurnRightUntil:  motorControl(synchBC,  75,   50, 0);        break;
  case cmdTypeTurnRightTwo:    motorControl(synchBC,  75,   50, 360 * 2);  break;
  case cmdTypeBackwardUntil:   motorControl(synchBC, -75,  100, 0);        break;
  case cmdTypeBackwardFive:    motorControl(synchBC, -75,  100, 360 * 5);  break;
  case cmdTypeBackRightUntil:  motorControl(synchBC, -75,   50, 0);        break;
  case cmdTypeBackRightTwo:    motorControl(synchBC, -75,   50, 360 * 2);  break;
  case cmdTypeBackLeftUntil:   motorControl(synchCB, -75,   50, 0);        break;
  case cmdTypeBackLeftTwo:     motorControl(synchCB, -75,   50, 360 * 2);  break;

  case cmdTypePlayTone1:       PlayTone(440, 2000); while (bSoundActive) {}; break;
  case cmdTypePlayTone2:       PlayTone(440, 5000); while (bSoundActive) {}; break;

  case cmdTypeStop:            return false;
  case cmdTypeLoop:            Close(hFile, ioResult); openFileAndSkipHeader();    break;

  case waitTypeSeekObject:     while (SensorValue[sonar] >  25) {};   stopMotors(); break;
  case waitTypeSOUND:          while (SensorValue[mike]  < 400) {};   stopMotors(); break;
  case waitTypeTOUCH:          while (SensorValue[touch] ==  0) {};   stopMotors(); break;
  case waitTypeLIGHT:          while (SensorValue[light] <  50) {};   stopMotors(); break;
  case waitTypeDARK:           while (SensorValue[light] >  30) {};   stopMotors(); break;

  case waitTwoSeconds:         wait1Msec( 2000);                      stopMotors(); break;
  case waitFiveSeconds:        wait1Msec( 5000);                      stopMotors(); break;
  case waitTenSeconds:         wait1Msec(10000);                      stopMotors(); break;

  case waitTypeINVALID:        break;
  case cmdType_EMPTY:          break;
  case cmdTypeINVALID:         return false;
  default:                     return false;
  }
  return true;
}

task main()
{
  nVolume = 3;
  SensorType[touch] = sensorTouch;
  SensorType[mike]  = sensorSoundDB;
  SensorType[light] = sensorLightActive;
  SensorType[sonar] = sensorSONAR9V;
  bFloatDuringInactiveMotorPWM = false;

  if (openFileAndSkipHeader()  == ioRsltSuccess)
  {
    while (processNextCommand())
    {}
    Close(hFile, ioResult);
  }
  return;
}