#include "LeadLag.h"

#define sineInput A0
#define triangleInput A1
#define squareInput A2


#define buttonPin 9
#define modePin 2
#define timeInput A3
#define gainInput A4

#define LOWER 0.0
#define UPPER 100.0

#define DEBOUNCE_PERIOD 10UL  //  Debounce Delay 10 milliseconds
typedef struct {
  private:
    bool input;
    unsigned long startTime;

  public:
    bool state;

    bool debounce(bool bounce) {
      unsigned long currentTime = millis();
      bool prevState = state;
      if (bounce) {
        state = true;
      } else {
        if (state) {
          if (input) {
            startTime = currentTime;
          }
          unsigned long elapsedTime = currentTime - startTime;
          if (elapsedTime >= DEBOUNCE_PERIOD) {
            state = false;
          }
        }
      }
      input = bounce;
      return state != prevState & state == true;
    }
} debounce_t;


LeadLag leadlag;
debounce_t button;
debounce_t mode;
bool pressed;
bool firstScan;

uint8_t state;

void setup() {
  Serial.begin(152000);
  pinMode(buttonPin, INPUT_PULLUP);
  pinMode(modePin, INPUT_PULLUP);
  pinMode(LED_BUILTIN, OUTPUT);

  leadlag.parameters(0.0, 100.0);
  leadlag.gain(1.0);

}

void loop() {
  double input = 0;
  bool waveSelectionRead = !digitalRead(buttonPin);
  bool modeSelectionRead = !digitalRead(modePin);
  pressed = button.debounce(waveSelectionRead);
  if (pressed) {
    state = (state + 1) % 3;
  }

  int raw = analogRead(timeInput);
  double leadTimeConstant = R2E(raw, 0.0, 2.0);
  raw = analogRead(gainInput);
  double lagTimeConstant = R2E(raw, 0.0, 2.0);

  leadlag.leadTimeConstant(leadTimeConstant);
  leadlag.lagTimeConstant(lagTimeConstant);
  mode.debounce(!modeSelectionRead);
  Mode leadLagMode = mode.state ? MANUAL : AUTO;

  digitalWrite(LED_BUILTIN, mode.state);
  double sine = R2E(sineInput, 0, UPPER);
  double triangle = R2E(triangleInput, 0, UPPER);
  double square = R2E(squareInput, 0, UPPER);
  input = sine;
  leadlag.mode(leadLagMode);
  leadlag.compensator(input, false);
  SerialPlot(leadlag.input, leadlag.output, 0, LOWER, UPPER);
}

void SerialPlot(double input, double output, double trigger, double lower, double upper) {
  static unsigned long startTime;
  unsigned long currTime = millis();
  unsigned long elapsedTime = currTime - startTime;
  if (elapsedTime - startTime > 10UL) {
    startTime = currTime;
    Serial.println(
      "Input:" + String(input, 3)
      + ", Output:" + String(output, 3)
      //+ ", Trigger:" + String(trigger * 5, 3)
      + ", Lower:" + String(lower, 3)
      + ", Upper:" + String(upper, 3)
      // + ", FD:" + String(5 * (1 - FULLY_DISCHARGED), 3)
    );
  }
}

// RAW Value to Engineering Unit
double R2E(uint8_t pin, double min, double max) {
  long value = analogRead(pin);
  return value * max / 1023.0 + min;
}

double Noise() {
  static double noise;
  double prev = noise;
  while (prev == noise) {
    noise = random(-100, 00);
    noise = noise * 0.01F;
  }
  return noise;
}