#define RAW_LIGHT IN_3;
#define SAMPLE_COUNT 30
#define proximity 50

int AVG_AMBIENT,DiffRaw,ThisRaw,DEFCON,MinRaw,RisingMins,LastRaw,DescendingRaw;
int TRUE,FALSE,Temp,lastping,left,right;

task pingaway			//sends out IR flashes on demand
{				//Run while other tasks continue
  while (true)
  {
    SendMessage(255);    
  }
  
}



task ProximityDriver
// converted from code by Mark R. David        (markdavid@mediaone.net)
{
  SensorType(IN_1,STYPE_LIGHT);			//Left Light Sensor
  SensorMode(IN_1,SMODE_RAW);
  SensorType(IN_2,STYPE_LIGHT);			//Right Light Sensor
  SensorMode(IN_2,SMODE_RAW);
  SensorType(IN_3,IN_LIGHT);			//DEFCON Middle Sensor
  SensorMode(IN_3,SMODE_RAW);
  IRMode(IR_HI);

  Temp = 0;
  repeat (SAMPLE_COUNT) {
    Temp += RAW_LIGHT;
  }
  Temp /= SAMPLE_COUNT;                        
  AVG_AMBIENT = Temp;			// average starting ambient light value

  DEFCON = 0;			 //DEFCON - number of diffs the last min is below ambient
  MinRaw = AVG_AMBIENT;			// The current minimum flash read - init to ambient
  RisingMins = FALSE;			//Flag that the last min was higher than the one before
  LastRaw = AVG_AMBIENT;			//Previous raw reading
  DescendingRaw = FALSE;			//Flag that raw readings are descending

  while (true)
  {
    ThisRaw = RAW_LIGHT;			 //Take ambient reading ...
    Temp = (AVG_AMBIENT*4 + ThisRaw) / 5;                      
    AVG_AMBIENT = Temp;			// ... and keep a moving avg of it

    DiffRaw = (615-AVG_AMBIENT)/6;			//DiffRaw = The diff - a difference in light reading
    if (DiffRaw > -6) {			 //Is negative and no greater than -5
      DiffRaw = -6;			//DiffRaw = Min(-6, (615 - AVG_AMBIENT) / 6)
    }
    
    start pingaway;			//Start the Pinging Task
    left = 0;			//Check the Left Sensor
    lastping = IN_1;                     
    if (IN_1 > lastping + proximity)
    {
      left = 1;
    }
    
    right = 0;			//Check the Right Sensor                                                                           
    lastping = IN_2;
    if (IN_2 > lastping + proximity)
    {
      right = 1;
    }

   
    Sleep(3);
    stop pingaway;			//Stop the Pinging Task
    ThisRaw = RAW_LIGHT;
    if (ThisRaw < MinRaw) {			//New minimum!
      MinRaw = ThisRaw;			//Reset the effective min
      RisingMins = FALSE;			 //Clear the rising mins flag
      DescendingRaw = TRUE;			//Set the descending flag
      Temp = (MinRaw-AVG_AMBIENT) / DiffRaw;
      DEFCON = Temp;			// DEFCON = (MinRaw - AVG_AMBIENT) / DiffRaw
    } else {
      if (ThisRaw > LastRaw) {			//Ascending
        if (DescendingRaw==TRUE) {			//Was descending must be a higher min
          if (RisingMins==TRUE) {			//It's the second one! Reset the effective min
            MinRaw = LastRaw;			// Reset the effective min
            Temp = (MinRaw-AVG_AMBIENT) / DiffRaw;
            DEFCON = Temp;			// Calculate DEFCON again
          } else {
            RisingMins = TRUE;
          }
        }
        DescendingRaw = FALSE;
      } else {
        DescendingRaw = TRUE;			//Descending
      }
    }

    LastRaw = ThisRaw;
  }			//loop forever
}

task main
{
  start ProximityDriver;			//Start the sensor readings
  
  while (true)
  {
    if (DEFCON == 1) PlayNote(200,1);			//Just let's me know what DEFCON
    if (DEFCON == 2) PlayNote(300,1);
    if (DEFCON == 3) PlayNote(400,1);
    if (DEFCON == 4) PlayNote(500,1);
    if (DEFCON == 5) PlayNote(600,1);
    if (DEFCON == 6) PlayNote(700,1);
    if (DEFCON > 3)			//If too close take extreme action-back up
    {
      Rev(OUT_A + OUT_C,8);
      Sleep(50);
    }
    
    if (right == 1)			//If right is activated turn left
    {
      Rev(OUT_A,8);Fwd(OUT_C,8);Sleep(50);
    }
    
    if (left == 1)			//If left is activated turn right
    {
      Rev(OUT_C,8);Fwd(OUT_A,8);Sleep(50);
    }
    left = 0; right = 0;
    
    Fwd(OUT_A + OUT_C ,8);
   }
  
}