#include <Arduino.h>

volatile int motorPos = 0;

#define ppr 100 // pulses per revolution

#define PWM_FREQ 1000
#define PIN_MOTOR_P 25
#define PIN_MOTOR_N 26

#define PWM_CHANNEL_0 0
#define PWM_CHANNEL_1 1

#define PIN_ENCODER_A 34
#define PIN_ENCODER_B 35

#define MIN(a,b) (((a)<(b)) ? (a) : (b))
#define MAX(a,b) (((a)>(b)) ? (a) : (b))

void IRAM_ATTR pinChangeInterrupt();


void setup() {
  Serial.begin(115200);
 
  /* motor outputs */
  pinMode(25, OUTPUT);
  pinMode(26, OUTPUT);

  /* 12 kHz PWM, 8-bit resolution */
  ledcSetup(PWM_CHANNEL_0, PWM_FREQ, 8);
  ledcSetup(PWM_CHANNEL_1, PWM_FREQ, 8);

  ledcAttachPin(PIN_MOTOR_P, PWM_CHANNEL_0);
  ledcAttachPin(PIN_MOTOR_N, PWM_CHANNEL_1);

  /* quadrature encoder inputs */
  pinMode(PIN_ENCODER_A, INPUT);
  pinMode(PIN_ENCODER_B, INPUT);

  attachInterrupt(PIN_ENCODER_A, pinChangeInterrupt, CHANGE);
  //attachInterrupt(PIN_ENCODER_B, pinChangeInterrupt, CHANGE);

  /* potentiometer input */
  analogReadResolution(10);

Serial.printf("setPos, motorPos, err, der, int, cmd\n");


}

void setSpeed(int speed) {
  speed = MAX(-255, MIN(255, speed));

  if (speed > 0) {
    ledcWrite(PWM_CHANNEL_0, 255);
    ledcWrite(PWM_CHANNEL_1, 255 - speed);
  } else {
    ledcWrite(PWM_CHANNEL_0, 255 + speed);
    ledcWrite(PWM_CHANNEL_1, 255);
  }
}

void loop() {
  static float integral = 0;
  static float prev_err = 0;

  int setpos = analogRead(2);

  
  float err = setpos - motorPos;
  
  float dt = 50.0/1000.0; // approx. 50ms
  integral = MAX(-1000, MIN(1000, integral + err * dt));
 
  float derivative = (err - prev_err) / dt;
  
  float kp = 1.0;
  float ti = 10.0;
  float td = 0.3;

  float cmd = kp * (err + integral / ti + derivative * td);
  setSpeed((int)cmd);
  Serial.printf("%d, %d, %3.3f, %3.3f, %3.3f, %3.3f\n", 
      setpos, motorPos, err, derivative, integral, cmd);

  delay(50);
}



void IRAM_ATTR pinChangeInterrupt() {
  int a = digitalRead(PIN_ENCODER_A);
  int b = digitalRead(PIN_ENCODER_B);

  motorPos += (a ^ b) ? 1 : -1;
}