//Tortoise NXT code
//Definitions in plain English
#define DriveMotor OUT_C
#define TurnMotor OUT_A
#define EyePort S1
#define Eye SENSOR_1
#define ShellPort S2
#define Shell SENSOR_2
#define PilotLight OUT_B

//Level 0: Turn the drive motor on.
int DriveSpeed;

task level_0(){
  while(true){
    OnFwd(DriveMotor, DriveSpeed);
  }
}

//Level 1: Turn the steering motor on.
int TurnSpeed;

task level_1(){
  while(true){
    OnRevReg(TurnMotor, TurnSpeed, OUT_REGMODE_SPEED);
  }
}

//Level 2: Alter motor speeds with light changes.
int Zero, HalfDrive, HalfTurn, FullDrive, FullTurn;//Motor speed presets.
int dark, bright;//Light thresholds
int Vision, See;//Light sensor controls
int lightSwitch(int x);//Pilot light brightness.

task level_2(){
  while(true){
    OnFwd(PilotLight, lightSwitch(TurnSpeed));//Pilot lights on if turning.
    if (Vision == 1) {See = Eye;}
    //Sensor in dark threshold
    if (See <= dark){
      DriveSpeed = HalfDrive;
      TurnSpeed = FullTurn;
    }
    else {
      //Sensor in moderate threshold
      if (See < bright){
        DriveSpeed = FullDrive;
        TurnSpeed = Zero;
      }
      else {
      //Sensor in bright threshold
        DriveSpeed = FullDrive;
        TurnSpeed = HalfTurn;
      }
    }
  }
}

//This just toggles the lights.
int lightSwitch(x){
  if (x == 0) return 0;
  else return 100;
}

//Level 3: The shell can temporarily override the light sensors.

int TimeConstant, Bumped; //Reaction time constant and threshold for contact.

task level_3(){
  while(true){
    until (Shell < Bumped);
    Vision = 0;
    while (Shell < Bumped){//Flicker between dark and bright.
      See = dark ;
      Wait(TimeConstant * Eye);//It'll flicker differently if it sees light.
      See = bright;
      Wait(TimeConstant * Eye * 2);
    }
    Vision = 1;
  }
}

task main(){
  //Set up hardware.
  SetSensorType(EyePort, SENSOR_TYPE_LIGHT_INACTIVE);
  SetSensorMode(EyePort, SENSOR_MODE_RAW);
  SetSensorType(ShellPort, SENSOR_TYPE_LIGHT_ACTIVE);
  SetSensorMode(ShellPort, SENSOR_MODE_RAW);

  //Init level 0.
  DriveSpeed = 70;
  start level_0;

  //Init level 1.
  TurnSpeed = 40;
  start level_1;

  //Init level 2.
  Zero = 0;
  HalfDrive = 40; FullDrive = 60;
  HalfTurn = 7; FullTurn = 20;
  dark = 450; bright = 700;
  Vision = 1;
  start level_2;

  //Init level 3.
  TimeConstant = 5;
  Bumped = 620;
  start level_3;
}