const tMotor right = (tMotor) motorA;
const tMotor left = (tMotor) motorC;
const TSynchedMotors rightLeft = synchAC;
const TSynchedMotors leftRight = synchCA;
void Move(int motorSteps, int steering, int speed, bool doLog);
void BackTrace(int steps);

/*
 * Semaphore (Lock) mechanism for task management
 */
#define Lock(sem) int sem
#define Acquire(sem) while(sem){}; sem = 1
#define Release(sem) sem = 0
Lock(motorLock);

/*
 * logs for storing motor movements
 * implemented as a stack of size LOG_SIZE
 */
#define LOG_SIZE 10
int speedLog[LOG_SIZE];
int steeringLog[LOG_SIZE];
int stepLog[LOG_SIZE];
int currentLogIndex;
int numberOfLogEntries;
void LogData(int speed, int steering, int motorSteps);
void ClearLog();

typedef enum
{
   stateInitializing,
   stateSearching,      // exploring floor to find a black line
     stateIdentifying,      // is it really a line?
     stateFollowing,      // following a blackline
     stateShuttingDown
} TStates;

TStates currentState = stateInitializing;

task main()
{
   currentState = stateInitializing;
     // Continuous loop for state machine
     while (true)
     {
       switch (currentState)
       {
       case stateInitializing:
          currentLogIndex = 0;
         numberOfLogEntries = 0;
         ClearLog();
         Release(motorLock);
         currentState = stateSearching;
         break;

       case stateSearching:
         Move(360,0,50,true);
         Move(360,0,-50,true);
         BackTrace(2);
         currentState = stateShuttingDown;
         break;

       case stateIdentifying:
         /* Insert program specific code here */
         break;

       case stateFollowing:
         /* Insert program specific code here */
         break;

       case stateShuttingDown:
         StopAllTasks();
         break;
       }
     }
}

void LogData(int speed, int steering, int motorSteps)
{
   if (++currentLogIndex == LOG_SIZE) {currentLogIndex = 0;}
   speedLog[currentLogIndex] = speed;
   steeringLog[currentLogIndex] = steering;
   stepLog[currentLogIndex] = motorSteps;
   if (numberOfLogEntries < LOG_SIZE) {numberOfLogEntries++;}
}

void ClearLog()
{
   for (int i = 0; i < LOG_SIZE; i++)
   {
      speedLog[i] = 0;
      steeringLog[i] = 0;
      stepLog[i] =0;
   }
}

void Move(int motorSteps, int steering, int speed, bool doLog)
{
   if(steering >= 0)       // right turn
   {
      Acquire(motorLock);
      nSyncedMotors = leftRight;
      nSyncedTurnRatio = (100 - (2*abs(steering)));
      nMotorEncoderTarget[left]  = motorSteps;
      motor[left] = speed;
      while(nMotorRunState[left] != runStateIdle) {}
      Release(motorLock);
   }
   else      // left turn
   {
      Acquire(motorLock);
      nSyncedMotors = rightLeft;
      nSyncedTurnRatio = (100 - (2*abs(steering)));
      nMotorEncoderTarget[right]  = motorSteps;
      motor[right] = speed;
      while(nMotorRunState[right] != runStateIdle) {}
      Release(motorLock);
   }
   if (doLog) {LogData(speed, steering, motorSteps);}
   return;
}

void BackTrace(int steps)
{
   if (steps > numberOfLogEntries) {steps = numberOfLogEntries;}
   for (int i = 1; i <= steps; i++)
   {
      Move(stepLog[currentLogIndex],-steeringLog[currentLogIndex],-speedLog[currentLogIndex],false);
      if (--currentLogIndex < 0) {currentLogIndex = LOG_SIZE -1;}
      numberOfLogEntries--;
   }
}