int retrievelog;  // set to 1 to tell computer to get DataLog from RCX - we list this first
                  // and hope that NQC assigns it to variable 0 (???)
                  // 'nqc -raw 120000' is used by computer to read it
                  // 'nqc -raw 1400020000 is used by computer to reset to 0
                  
int motor1speed;
int motor2speed;
int newmotor1speed;
int newmotor2speed;
int touch1;
int touch2;
int lastlightlevel;
int forwardtime;
int backtime;
int turntime;
int tolog;
int logvalues;
int numspins;
int offtime;


#define LOGSIZE      1024
#define LOGTHRESHOLD 16   /* send log when this many bytes from end */
#define LOGSTART     32000
#define LOGMOTORS    32001
#define LOGSENSORS   32002


#define MOTOR1 OUT_A  // left  motor
#define MOTOR2 OUT_C  // right motor
#define TOUCH1 IN_1   // left touch sensor
#define TOUCH2 IN_3   // right touch sensor
#define LIGHT1 IN_2   // light sensor

#define BACKBASE 50
#define TURNBASE 75
#define BACKTIME 75
#define TURNTIME 75
#define FORWARDBASE 300
#define FORWARDTIME 300


/* timers
   1 = how long the present motor power has been applied
*/

inline logValue
{
  Datalog(tolog);
  logvalues += 1;
} // logvalue()


inline startLog
{
  logvalues = 0;
  SetDatalog(LOGSIZE);
  tolog = LOGSTART;
  logValue();
} // startLog()


inline spinSome
{
  ClearTimer(1);
  Fwd(MOTOR1, OUT_LOW);
  Rev(MOTOR2, OUT_LOW);
  Sleep(10);
  Off(MOTOR1 + MOTOR2);
  numspins += 1;
}  // spinSome()


inline sendDataLog 
{
  stop ping;
  PlaySound(1);
  Sleep(250);  // wait 2.5 seconds in case computer is setting retrievelog on a ping
  retrievelog = 4;  // tell computer the log is ready
  Sleep(350); // wait 3.5 seconds for an ack
  numspins = 0; 
  while( retrievelog != 5)   // wait till it says it is starting download
  {
    spinSome();
    PlayNote(300,10);
    Sleep(100);  // wait for computer to respond
  }
//  if(numspins != 0) spinSome(); // if spin was necessary, add a little more
  
  // TODO - we should log the spins here 

  while( retrievelog != 6)   // wait till it says it has finished download
  {
    PlayNote(400,10);
    Sleep(100);  // wait for computer to retrieve
  }
  
  retrievelog = 0; // prepare for ping()
  
  startLog();
  logvalues = 0;
  SetDatalog(LOGSIZE);
  Datalog(LOGSTART);
  PlaySound(5);
  start ping;
} // sendDataLog()


inline checkLog
{
  if( logvalues + LOGTHRESHOLD > LOGSIZE) {
    sendDataLog();
  }
} //checkLog()


inline changeMotors
{
  Off( MOTOR1 + MOTOR2);  // stop the motors and the associated timer
  offtime = Timer(1);
  
  checkLog();  // make sure there's room to log the next three values
  tolog = LOGMOTORS;
  logValue();
  tolog = offtime;
  logValue();
  tolog = motor1speed;
  logValue();
  tolog = motor2speed;
  logValue();
  
  motor1speed = newmotor1speed;
  motor2speed = newmotor2speed;

  /* set new speed/direction */
  if( motor1speed >= 0)
  {
    OutputDir(MOTOR1, OUT_FWD);
    OutputPower(MOTOR1, motor1speed);
  }
  else
  {
    OutputDir(MOTOR1, OUT_REV);
    OutputPower(MOTOR1, -motor1speed);
  }
  
  if( motor2speed >= 0)
  {
    OutputDir(MOTOR2, OUT_FWD);
    OutputPower(MOTOR2, motor2speed);
  }
  else
  {
    OutputDir(MOTOR2, OUT_REV);
    OutputPower(MOTOR2, -motor2speed);
  }
  
  /* speed/directions set - now start timer and motors */
  ClearTimer(1);
  if( motor1speed != 0 && motor2speed != 0) 
  {
    OutputMode(MOTOR1 + MOTOR2, OUT_ON);
  }
  else if( motor1speed != 0) 
  {
    OutputMode(MOTOR1, OUT_ON);
  }
  else if( motor2speed != 0) 
  {
    OutputMode(MOTOR2, OUT_ON);
  }
  
} // changeMotors()


inline stopMotors
{
  newmotor1speed = 0;
  newmotor2speed = 0;
  changeMotors();
} // stopMotors()


inline logSensors
{
  checkLog();
  tolog = LOGSENSORS;
  logValue();
  tolog = TOUCH1;
  logValue();
  tolog = TOUCH2;
  logValue();
} // stopMotors()


inline logTouch
{
  checkLog();
  tolog = LOGSENSORS;
  logValue();
  tolog = touch1;
  logValue();
  tolog = touch2;
  logValue();
}


inline checkRadar
{
   if (LIGHT1 > lastlightlevel + 50) {
       PlayNote(500,50);
       
       // need to log here, but we need to stop motors or something to make sure
       // we don't overflow the datalog or lose information
   }
   lastlightlevel = LIGHT1;
} /* checkRadar() */


sub explore
{
  if( TOUCH1 == 0 && TOUCH2 == 0)   // if not touching anything
  {
    // move forward
    forwardtime = FORWARDBASE + Random(FORWARDTIME);
    newmotor1speed = OUT_FULL;
    newmotor2speed = OUT_FULL;
    changeMotors();
    while( TOUCH1 == 0 && TOUCH2 == 0  && (forwardtime > Timer(1)))  // until we hit something or timeout
    {
      checkRadar();
    }
    touch1 = TOUCH1;
    touch2 = TOUCH2;
    stopMotors();
  } else {
    touch1 = TOUCH1;
    touch2 = TOUCH2;
  }
  logTouch();

  do
  {
    newmotor1speed = -OUT_HALF;
    newmotor2speed = -OUT_HALF;
    backtime = BACKBASE + Random(BACKTIME);
    changeMotors();
    Sleep(backtime);
    stopMotors();
    logSensors();
  } while( TOUCH1 != 0 || TOUCH2 != 0);


  if( touch1 == touch2 && touch1 == 1)
  {
    // move backwards until one of them goes off
    newmotor1speed = -OUT_HALF;
    newmotor2speed = -OUT_HALF;
    // put some randomness in it 
    if( Random(1) == 0) 
    {
      newmotor1speed = -OUT_LOW;
    }
    else
    {
      newmotor2speed = -OUT_LOW;
    }
  }
  else if (touch1 == 1)
  {
    // turn right
    newmotor1speed = 0;
    newmotor2speed = -OUT_HALF;
  }
  else if(touch2 == 1)
  {
    // turn left
    newmotor1speed = -OUT_HALF;
    newmotor2speed = 0;
  }
  else 
  {
    if( Random(1) == 1)
    {
      // random turn
      newmotor1speed = -OUT_LOW;
      newmotor2speed = OUT_LOW;
    }
    else
    {
      newmotor1speed = OUT_LOW;
      newmotor2speed = -OUT_LOW;
    }
  }
  
  /* turn for a bit */
  turntime = TURNBASE + Random(TURNTIME);
  changeMotors();
  Sleep(turntime);
  stopMotors();  /* stop */
//  logSensors();  /* might as well record sensors */
  
} // explore()


task ping  // keep IR tower alive and make sure RCX stays in range
{
  while(true) 
  {
    if( retrievelog == 1) 
    {
      // computer sends 1, RCX acks 2
      PlaySound(2);
      retrievelog = 2;
    }
    else if( retrievelog == 3) 
    {
      // computer sends 3, RCX acks 0
      PlaySound(3);
      retrievelog = 0;
    }
    
    // need to add stop/spin here if RCX hasn't seen a ping for "a while"
  }
}

task main
{
  retrievelog = 0;
  newmotor1speed = 0;
  newmotor2speed = 0;
  motor1speed = 0;
  motor2speed = 0;
  tolog = 0;
  logvalues = 0;

  // initialize touch sensors  
  Sensor( TOUCH1, IN_SWITCH);
  Sensor( TOUCH2, IN_SWITCH);

  // initalize radar
  IRMode(IR_HI);  // for radar and long-range communications to computer
  SensorType(LIGHT1, STYPE_LIGHT);
  SensorMode(LIGHT1, SMODE_RAW);
  lastlightlevel = LIGHT1;
  
  // begin ping routine
  start ping;

  // init Datalog
  startLog();
  
  // blank motor timer (haven't started them yet)
  ClearTimer(1);

  // roam around and report back what is found
  while(true) {
    explore();
  }
  
} // main()