#pragma noinit
#define RAW_LIGHT SENSOR_2;
#define SAMPLE_COUNT 30
#define proximity 50
#define delay 200
#define forward OnRev(OUT_A+OUT_C);SetPower(OUT_A+OUT_C,8); //set to OnRev
#define reverse OnFwd(OUT_A+OUT_C);SetPower(OUT_A+OUT_C,8); //set to OnFwd
#define left OnFwd(OUT_A);OnRev(OUT_C);SetPower(OUT_A+OUT_C,8);
#define right OnRev(OUT_A);OnFwd(OUT_C);SetPower(OUT_A+OUT_C,8);
#define dancing OnFwd(OUT_B);SetPower(OUT_B,8);
//the master robot sends out IR flashes with a message of 255
//the other robot sends out IR with a message of 254

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

task pingaway()			//sends out IR flashes on demand
{			       	//Run while other tasks continue
  while (true)
{
if (Timer(1)<=5)
{
    SendMessage(255);
}
if (Timer(1) > 10) ClearTimer(1);  
}
}

task ProximityDriver() // converted from code by Mark R. David        (markdavid@mediaone.net)
{
  SetSensorType(SENSOR_2,SENSOR_LIGHT);			//DEFCON Middle Sensor
  SetSensorMode(SENSOR_2,SENSOR_MODE_RAW);
  SetTxPower(TX_POWER_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)
    }
    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()
{
  ClearMessage();
  ClearTimer(1);
  ClearTimer(2);
  start ProximityDriver;	               		//Start the sensor readings
  start pingaway;
  start readdefcon;
  forward;
}

task readdefcon()
{
  while (true)
  {
    if ((Timer(2) >= delay) && (Message() == 254))
    {
    stop ProximityDriver;
    stop pingaway;
    start dance;
    stop readdefcon;
    }
    
    if (DEFCON == 1) PlayTone(200,1);			//Just let's me know what DEFCON
    if (DEFCON == 2) PlayTone(300,1);
    if (DEFCON == 3) PlayTone(400,1);
    if (DEFCON == 4) PlayTone(500,1);
    if (DEFCON == 5) PlayTone(600,1);
    if (DEFCON == 6) PlayTone(700,1);
    if (DEFCON >= 2)		                     	//If too close take extreme action-back up
    {
      if (Random(100) > 50)
      {
        reverse;
        Wait(50);
        left;
        Wait(75);
        forward;
      }
      else
      {
        reverse;
        Wait(50);
        right;
        Wait(75);
        forward;
}}}}                                              //Total laziness ;)

task dance()
{
left;
dancing;
repeat (4)
{
PlayTone(440,4);Wait(4);
PlayTone(894,4);Wait(4);
PlayTone(442,2);Wait(2);
PlayTone(892,4);Wait(4);
PlayTone(468,4);Wait(4);
PlayTone(866,2);Wait(2);
PlayTone(470,2);Wait(2);
PlayTone(864,2);Wait(2);
PlayTone(472,2);Wait(2);
PlayTone(862,2);Wait(2);
PlayTone(474,2);Wait(2);
PlayTone(860,1);Wait(1);
PlayTone(476,1);Wait(1);
PlayTone(858,4);Wait(4);
PlayTone(478,4);Wait(4);
PlayTone(856,1);Wait(1);
PlayTone(480,1);Wait(1);
PlayTone(854,2);Wait(2);
PlayTone(482,2);Wait(2);
PlayTone(852,2);Wait(2);
PlayTone(484,2);Wait(2);
PlayTone(850,1);Wait(1);
PlayTone(486,1);Wait(1);
PlayTone(848,1);Wait(1);
PlayTone(488,1);Wait(1);
PlayTone(846,1);Wait(1);
PlayTone(490,4);Wait(4);
PlayTone(842,4);Wait(4);
PlayTone(492,1);Wait(1);
PlayTone(840,1);Wait(1);
PlayTone(494,1);Wait(1);
PlayTone(838,1);Wait(1);
PlayTone(640,1);Wait(1);
PlayTone(240,2);Wait(2);
PlayTone(642,2);Wait(2);
PlayTone(242,2);Wait(2);
PlayTone(644,2);Wait(2); 
PlayTone(290,4);Wait(4); 
PlayTone(646,4);Wait(4);
PlayTone(288,4);Wait(4);
PlayTone(647,4);Wait(4);
PlayTone(286,4);Wait(4);
PlayTone(650,4);Wait(4);
PlayTone(284,2);Wait(2);
PlayTone(652,2);Wait(2);
PlayTone(282,2);Wait(2);
PlayTone(654,2);Wait(2);
PlayTone(280,2);Wait(2);
PlayTone(656,2);Wait(2);
PlayTone(278,2);Wait(2);
PlayTone(658,2);Wait(2);
PlayTone(276,1);Wait(1);
PlayTone(660,1);Wait(1);
PlayTone(274,1);Wait(1);
PlayTone(362,1);Wait(1);
PlayTone(272,1);Wait(1);
PlayTone(364,2);Wait(2);
PlayTone(270,2);Wait(2);
PlayTone(366,2);Wait(2);
PlayTone(268,2);Wait(2);
PlayTone(368,2);Wait(2);
PlayTone(266,2);Wait(2);
PlayTone(370,2);Wait(2);
PlayTone(264,2);Wait(2);
PlayTone(372,2);Wait(2);
PlayTone(262,2);Wait(2);
PlayTone(374,2);Wait(2);
PlayTone(260,2);Wait(2);
PlayTone(376,2);Wait(2);
PlayTone(258,2);Wait(2);
PlayTone(378,2);Wait(2);
PlayTone(256,2);Wait(2);
PlayTone(380,2);Wait(2);
PlayTone(254,2);Wait(2);
PlayTone(444,2);Wait(2); 
PlayTone(890,2);Wait(2); 
PlayTone(446,2);Wait(2);
PlayTone(888,2);Wait(2);
PlayTone(448,2);Wait(2);
PlayTone(886,2);Wait(2);
PlayTone(450,2);Wait(2);
PlayTone(884,2);Wait(2);
PlayTone(452,2);Wait(2);
PlayTone(882,2);Wait(2);
PlayTone(454,2);Wait(2);
PlayTone(880,2);Wait(2);
PlayTone(456,2);Wait(2);
PlayTone(878,2);Wait(2);
PlayTone(458,2);Wait(2);
PlayTone(876,2);Wait(2);
PlayTone(460,2);Wait(2);
PlayTone(874,2);Wait(2);
PlayTone(462,2);Wait(2);
PlayTone(872,1);Wait(1);
PlayTone(464,1);Wait(1);
PlayTone(870,2);Wait(2);
PlayTone(466,1);Wait(1);
PlayTone(868,1);Wait(1);
PlayTone(382,2);Wait(2);
PlayTone(252,2);Wait(2);
PlayTone(384,2);Wait(2);
PlayTone(250,1);Wait(1);
PlayTone(386,1);Wait(1);
PlayTone(248,2);Wait(2);
PlayTone(388,2);Wait(2);
PlayTone(246,2);Wait(2);
PlayTone(390,2);Wait(2);
PlayTone(244,1);Wait(1);
PlayTone(392,1);Wait(1);
PlayTone(242,2);Wait(2);
PlayTone(394,2);Wait(2);
PlayTone(240,2);Wait(2);
}
}