// Pinout diagram:
// https://www.upesy.com/cdn/shop/files/doc-esp32-pinout-reference-wroom-devkit.png

// Once we pass a certain threshold, we'll need to trigger an EVENT! Then we can
// measure the sound and turn the servos.

// Direction: 0 = CCW, 1 = CW
#define stepNum 5
#define dirNum 17
#define buttNum 0

// We're gonnna try switching between the two!
#define angle0 52
#define angle1 86

// The initial target is angle0 since button starts LOW
int target = angle0;

volatile byte buttState = HIGH;
#define dbDelay 250
int lastDebounce = 0;
//float time = 0;
//float freq = 30;

// We have two angle positions... When we press a button,
// switch to the other button! 

void setup() {
  // Defining pins:
  pinMode(buttNum,INPUT);
  pinMode(stepNum,OUTPUT);
  pinMode(dirNum,OUTPUT);
  attachInterrupt(digitalPinToInterrupt(buttNum),isPressed,CHANGE);

  Serial.begin(115200);
  // Setting defaults:
  digitalWrite(stepNum,LOW);
  digitalWrite(dirNum,LOW);
}

/*

We'd like to work with a PD controller!
 - Simpler than PID
 - Small overshoot is acceptible
 - We want to respond fast since noises might be sudden
 - Don't want to overshoot very far 

Discrete PD...
  u_k = Kp*e_k + Kd*(1/Ts)*(e_k = e_k-1)

We'll need to return an array of e_k and e_k-1

 */

void PD_loop(float targ_angle) {

}

void isPressed() {
  noInterrupts();
  time = millis();
  if (time > lastDebounce + dbDelay) {
    buttState = !buttState;
    lastDebounce = time;
    interrupts();
  }
}

void loop() {

}