#define DANGER_THRESH     10
#define SPACE_THRESH      50

#define MIN_CHANGE        2
#define BRAKE_TIME        60
#define REV_TIME          200

// the fine adjustment times:
#define LFT_FN_ADJ_TIME   200
#define RGT_FN_ADJ_TIME   200

#define NUM_PTS           4

int lastMeasurement;

task main() {

  //long lastMoment = CurrentTick();
  int currentSum = 0, lastSum = 0;
  int sum1, sum2, sum3;
  bool justAdjusted;
  int counter = 1,errorCount=0;
  
  int sums[];
  
  //ArrayInit(sums,0,SUM_HIST_SIZE);

  SetSensorLowspeed(S1);

  OnFwd(OUT_BC,70);
  
  while (true) {

    lastMeasurement = SensorUS(S1);
    currentSum += lastMeasurement;

    if (counter == NUM_PTS) {
    
      if (errorCount>4) {
        // TOO close
        Off(OUT_BC);
        Wait(BRAKE_TIME);
        OnRev(OUT_BC,50);
        Wait(2*REV_TIME);
        OnFwd(OUT_C,50);
        Wait(REV_TIME);
        OnFwd(OUT_BC,50);
        justAdjusted = true;
        lastSum = currentSum;
        TextOut(0,0,"1");
        errorCount=0;
      }
      if (currentSum < DANGER_THRESH*NUM_PTS) {
        // too close
        Off(OUT_BC);
        Wait(BRAKE_TIME);
        if (errorCount == 0) {  // neww
          OnRev(OUT_BC,70);
          Wait(100);
        }
        OnRev(OUT_B,70);
        Off(OUT_C);   // /new
        Wait(REV_TIME);
        justAdjusted = true;
        lastSum = currentSum;
        TextOut(0,0,"1");
        errorCount++;
      }
      else if (currentSum > SPACE_THRESH*NUM_PTS) {
        // gap!
        OnFwd(OUT_BC,70);
        Wait(100);    // NEWWWWW
        OnFwd(OUT_C,40);
        OnFwd(OUT_B,80);
        justAdjusted = true;
        lastSum = currentSum;
        TextOut(0,0,"3");
      }
      else if (currentSum-lastSum < -MIN_CHANGE*NUM_PTS) {
        // we're getting closer..
        Coast(OUT_B);
        Wait(RGT_FN_ADJ_TIME);
        OnFwd(OUT_BC,70);
        lastSum = currentSum;
        TextOut(0,0,"2");
      }
      else if (currentSum-lastSum > MIN_CHANGE*NUM_PTS) {
        // we're getting farther..
        Coast(OUT_C);
        Wait(LFT_FN_ADJ_TIME);
        OnFwd(OUT_BC,70);
        lastSum = currentSum;
        TextOut(0,0,"4");
      }
      else if (justAdjusted) {
        justAdjusted = false;
        errorCount=0;
        OnFwd(OUT_BC,70);
        TextOut(0,0,"5");
      }
      else
        TextOut(0,0," ");
      NumOut(0,LCD_LINE4,currentSum);
    
      currentSum = 0;
      counter = 1;
    } else {
      counter++;
    }
    
  }
}
    
