    I = integra;  change based on recent sum of errors
    D = derivative: change based on how error is changing.
    The controller needs to avoid problems of being too sluggish or too skittery.
-   A sluggish control (overdamped) takes too long to reach the set-point. 
-   A skitterish control (underdamped) can overshoot, then undershoot, producing 
+   A sluggish control (overdamped) takes too long to reach the set-point.
+   A skitterish control (underdamped) can overshoot, then undershoot, producing
    oscillations and jerky motion.
    Getting the right control is a matter of tuning right parts of P, I, and D,
    which is something of a black art.
    For more information, search for:
    VanDoren Proportional Integral Derivative Control
 */
-{ 
+{
   static int  throttle_control = MIN_ACC_OUT;
   static int  brake_control = MAX_BRAKE_OUT;
   static int error_index = 0;

  // setting the max_error affacts control: anything bigger gets maximum response
  const long max_error_mmPs = 2500; // about 5.6 mph

//lets look through this
  error_sum -= speed_errors[error_index];
  speed_errors[error_index] = error_speed_mmPs;
  error_sum += error_speed_mmPs;
  mean_speed_error = error_sum / ERROR_HISTORY;
  i = (error_index -1) % ERROR_HISTORY;
  if (++error_index >= ERROR_HISTORY)
      error_index = 0;
  extrapolated_error = 2 * error_speed_mmPs - speed_errors[i];
    PID_error = P_tune * error_speed_mmPs
            + I_tune * mean_speed_error
            + D_tune * extrapolated_error;
  if (PID_error > speed_tolerance_mmPs)
  {  // too fast
    long throttle_decrease = (MAX_ACC_OUT - MIN_ACC_OUT) * PID_error / max_error_mmPs;
    throttle_control -= throttle_decrease;
    if (throttle_control < MIN_ACC_OUT)
        throttle_control = MIN_ACC_OUT;
    moveVehicle(throttle_control);
    
    long brake_increase = (MAX_BRAKE_OUT - MIN_BRAKE_OUT) * PID_error / max_error_mmPs;
    // MIN_BRAKE_OUT = 180; MAX_BRAKE_OUT = 250; 
    brake_control -= brake_increase;
    if (brake_control > MAX_BRAKE_OUT)
        brake_control = MAX_BRAKE_OUT;
    brake(brake_control);
  }
  else if (PID_error < speed_tolerance_mmPs)
  {  // too slow
    long throttle_increase = (MAX_ACC_OUT - MIN_ACC_OUT) * PID_error / max_error_mmPs;
    throttle_control += throttle_increase;
    if (throttle_control > MAX_ACC_OUT)
        throttle_control = MAX_ACC_OUT;
    moveVehicle(throttle_control);
    
    // release brakes
    long brake_decrease = (MAX_BRAKE_OUT - MIN_BRAKE_OUT) * PID_error / max_error_mmPs;
    // MIN_BRAKE_OUT = 180; MAX_BRAKE_OUT = 250; 
    brake_control += brake_decrease;
    if (brake_control < MIN_BRAKE_OUT)
        brake_control = MIN_BRAKE_OUT;
    brake(brake_control);
  }
  // else maintain current speed
//  Serial.print("\tThrottle Brake \t");  // csv for spreadsheet
//  Serial.print(throttle_control);
//  Serial.print("\t");
//  Serial.print(brake_control);
//  Serial.print("\t");
//  Serial.print(drive_speed_mmPs);  Serial.print("\t");
//  Serial.println(sensor_speed_mmPs); 
}

/*---------------------------------------------------------------------------------------*/

volatile unsigned long TickTime = 0;  // Time from one wheel rotation to the next gives speed.
volatile unsigned long OldTick = 0;

static struct hist {
  long olderSpeed_mmPs;  // older data
  unsigned long olderTime_ms;   // time stamp of older speed

  long oldSpeed_mmPs;  // last data from the interrupt
  byte oldClickNumber;
  unsigned long oldTime_ms;  // time stamp of old speed

  byte nowClickNumber;  // situation when we want to display the speed
  byte InterruptState;
  unsigned long nowTime_ms;
  unsigned long TickTime_ms;  // Tick times are used to compute speeds
  unsigned long OldTick_ms;   // Tick times may not match time stamps if we don't process
  // results of every interrupt
} history;

/*---------------------------------------------------------------------------------------*/
// WheelRev is called by an interrupt.
void WheelRev()
{
  static int flip = 0;
  unsigned long tick;
  noInterrupts();
  tick = millis();
  if (InterruptState != IRQ_RUNNING)
    // Need to process 1st two interrupts before results are meaningful.
    InterruptState++;

  if (tick - TickTime > MinTickTime_ms)
  {
    OldTick = TickTime;
    TickTime = tick;
    ++ClickNumber;
  }
  if (flip)
    digitalWrite(13, LOW);
  else
    digitalWrite(13, HIGH);
  flip = !flip;

  interrupts();
}
/*---------------------------------------------------------------------------------------*/

void setupWheelRev()
{

  //    SerialOdoOut.begin(115200); // C6 to C4
  //    pinMode(13, OUTPUT); //led
  //    digitalWrite(13, LOW);//turn LED off
  //
  pinMode(3, INPUT);//pulls input HIGH
  float MinTick = WHEEL_DIAMETER_MM * PI;
  //    SerialMonitor.print (" MinTick = ");
  //    SerialMonitor.println (MinTick);
  MinTick *= 1000.0;
  MinTick /= MAX_SPEED_mmPs;
  //    SerialMonitor.print (MinTick);
  MinTickTime_ms = MinTick;
  //    SerialMonitor.print (" MinTickTime_ms = ");
  //    SerialMonitor.println (MinTickTime_ms);

  //    SerialMonitor.print (" MIN_SPEED_mPh = ");
  //    SerialMonitor.print (MIN_SPEED_mPh);
  float MIN_SPEED_mmPs =  ((MIN_SPEED_mPh * 1000.0) / 3600.0);
  // MIN_SPEED_mmPs = 135 mm/s
  //    SerialMonitor.print (" MIN_SPEED_mmPs = ");
  //    SerialMonitor.print (MIN_SPEED_mmPs);
  float MaxTick = (WHEEL_DIAMETER_MM * PI * 1000.0) / MIN_SPEED_mmPs;
  //    SerialMonitor.print (" MaxTick = ");
  //    SerialMonitor.print (MaxTick);
  MaxTickTime_ms = MaxTick;
  //    SerialMonitor.print (" MaxTickTime = ");
  //    SerialMonitor.println (MaxTickTime_ms);
  TickTime = millis();
  // OldTick will normally be less than TickTime.
  // When it is greater, TickTime - OldTick is a large positive number,
  // indicating that we have not moved.
  // TickTime would overflow after days of continuous operation, causing a glitch of
  // a display of zero speed.  It is unlikely that we have enough battery power to ever see this.
  OldTick = TickTime;
  InterruptState = IRQ_NONE;
  ClickNumber = 0;
  history.oldSpeed_mmPs = history.olderSpeed_mmPs = NO_DATA;

  attachInterrupt (1, WheelRev, RISING);//pin 3 on Mega
  //    SerialMonitor.print("TickTime: ");
  //    SerialMonitor.print(TickTime);
  //    SerialMonitor.print(" OldTick: ");
  //    SerialMonitor.println(OldTick);

  //    SerialMonitor.println("WheelRev setup complete");

}
/*---------------------------------------------------------------------------------------*/
void ComputeSpeed( struct hist *data)
{
  if (data->InterruptState == IRQ_NONE || data->InterruptState == IRQ_FIRST)
  { // No data
    SpeedCyclometer_mmPs = 0;
    SpeedCyclometer_revPs = 0;
    return;
  }
  float Circum_mm = (WHEEL_DIAMETER_MM * PI);
  long WheelRev_ms = data->TickTime_ms - data->OldTick_ms;
  if (data->InterruptState >= IRQ_SECOND && data->oldSpeed_mmPs == NO_DATA && WheelRev_ms > 0)
  { //  first computed speed
    SpeedCyclometer_revPs = 1000.0 / WheelRev_ms;
    SpeedCyclometer_mmPs  =
      data->oldSpeed_mmPs = Circum_mm * SpeedCyclometer_revPs;
    data->oldTime_ms = data->TickTime_ms;  // time stamp for oldSpeed_mmPs
    data->oldClickNumber = data->nowClickNumber;
    return;
  }
  if (data->InterruptState == IRQ_RUNNING && data->olderSpeed_mmPs == NO_DATA && WheelRev_ms > 0
      && data->nowClickNumber != data->oldClickNumber)
  { //  new data for second computed speed
    data->olderSpeed_mmPs = data->oldSpeed_mmPs;
    data->olderTime_ms = data->oldTime_ms;

    SpeedCyclometer_revPs = 1000.0 / WheelRev_ms;
    SpeedCyclometer_mmPs  =
      data->oldSpeed_mmPs = Circum_mm * SpeedCyclometer_revPs;
    data->oldTime_ms = data->TickTime_ms;  // time stamp for oldSpeed_mmPs
    data->oldClickNumber = data->nowClickNumber;
    return;
  }
  if (data->InterruptState == IRQ_RUNNING && data->olderSpeed_mmPs != NO_DATA && WheelRev_ms > 0)
  { // Normal situation after initialization
    if (data->nowClickNumber == data->oldClickNumber)
    { // No new information; extrapolate the speed if decelerating; else keep old speed
      if (data->olderSpeed_mmPs > data->oldSpeed_mmPs)
      { // decelerrating
        float deceleration = (float) (data->olderSpeed_mmPs - data->oldSpeed_mmPs) /
                             (data->oldTime_ms - data->olderTime_ms);
        SpeedCyclometer_mmPs = data->oldSpeed_mmPs - deceleration *
                               (data->nowTime_ms - data->oldTime_ms);
        if (SpeedCyclometer_mmPs < 0)
          SpeedCyclometer_mmPs = 0;
        SpeedCyclometer_revPs = SpeedCyclometer_mmPs / Circum_mm;
      }
      else
      { // accelerating; should get new data soon

      }
      if (data->nowTime_ms - data->oldTime_ms > MaxTickTime_ms)
      { // too long without getting a tick
        SpeedCyclometer_mmPs = 0;
        SpeedCyclometer_revPs = 0;
        if (data->nowTime_ms - data->oldTime_ms > 2 * MaxTickTime_ms)
        {
          InterruptState = IRQ_FIRST;  //  Invalidate old data
          data->oldSpeed_mmPs = NO_DATA;
          data->olderSpeed_mmPs = NO_DATA;
        }
      }
    }
    else
    { // moving; use new data to compute speed
      data->olderSpeed_mmPs = data->oldSpeed_mmPs;
      data->olderTime_ms = data->oldTime_ms;

      SpeedCyclometer_revPs = 1000.0 / WheelRev_ms;
      SpeedCyclometer_mmPs  =
        data->oldSpeed_mmPs = Circum_mm * SpeedCyclometer_revPs;
      data->oldTime_ms = data->TickTime_ms;  // time stamp for oldSpeed_mmPs
      data->oldClickNumber = data->nowClickNumber;
    }
  }
  if (data->nowTime_ms - data->TickTime_ms > WheelRev_ms)
  { // at this speed, should have already gotten a tick?; If so, we are slowing.
    float SpeedSlowing_revPs = 1000.0 / (data->nowTime_ms - data->TickTime_ms);
    long SpeedSlowing_mmPs  = Circum_mm * SpeedCyclometer_revPs;
    SpeedCyclometer_revPs = min(SpeedCyclometer_revPs, SpeedSlowing_revPs);
    SpeedCyclometer_mmPs  = min(SpeedCyclometer_mmPs, SpeedSlowing_mmPs);
  }
  return;
}
/*---------------------------------------------------------------------------------------*/
void PrintSpeed( struct hist *data)
{
  Serial.print(SpeedCyclometer_mmPs); Serial.print("\t");
  Serial.print(data->InterruptState); Serial.print("\t");
  Serial.print(data->oldSpeed_mmPs); Serial.print("\t");
  Serial.print(data->olderSpeed_mmPs); Serial.print("\t");
  Serial.print(data->oldClickNumber); Serial.print("\t");
  Serial.print(data->nowClickNumber); Serial.print("\t");
  Serial.print(data->olderTime_ms); Serial.print("\t");
  Serial.print(data->oldTime_ms); Serial.print("\t");
  Serial.print(data->nowTime_ms); Serial.print("\t");
  Serial.print(data->TickTime_ms); Serial.print("\t");
  Serial.println(data->OldTick_ms);
}
/*---------------------------------------------------------------------------------------*/
void show_speed(SerialData *Results)
{
  history.nowTime_ms = millis();
  noInterrupts();
  history.TickTime_ms = TickTime;
  history.OldTick_ms = OldTick;
  history.nowClickNumber = ClickNumber;
  history.InterruptState = InterruptState;
  interrupts();

  ComputeSpeed (&history);
  //   PrintSpeed(&history);

  Odometer_m += (float)(LOOP_TIME_MS * SpeedCyclometer_mmPs) / MEG;
  // Since Results have not been cleared, angle information will also be sent.
  Results->speed_cmPs = SpeedCyclometer_mmPs / 10;
  writeSerial(&Serial3, Results);  // Send speed to C6

  show7seg( SpeedCyclometer_mmPs);   // Show speed on 7 segment LEDs

}
/*---------------------------------------------------------------------------------------*/
