#include <PinChangeInterrupt.h>

// --- Hardware Pins ---
const int pinPA = 10; 
const int pinPB = 11;
const int encoderA = A14;
const int encoderB = A15;
const int potPin = A0;

// --- PID Constants (Tuning for Position) ---
// Note: These usually differ significantly from Speed constants.
double Kp = 4.0;  // Aggressiveness of movement
double Ki = 2.0; // Keep very low to prevent hunting/oscillation
double Kd = 1.5;  // Dampens the approach to prevent overshoot

// --- Global Variables ---
volatile long current_position = 0; 
long target_position = 0;
long command = 0;

// --- PID State ---
double integral = 0;
double last_error = 0;

void pin_change_isr() {
  int pinA = digitalRead(encoderA);
  int pinB = digitalRead(encoderB);
  static int oldA = 0;
  if (pinA != oldA) {
    oldA = pinA;
    current_position += (pinA ^ pinB) ? (-1) : 1;
  }
}

void setup() {
  Serial.begin(115200);
  pinMode(pinPA, OUTPUT);
  pinMode(pinPB, OUTPUT);
  pinMode(encoderA, INPUT_PULLUP);
  pinMode(encoderB, INPUT_PULLUP);
  attachPCINT(digitalPinToPCINT(encoderA), pin_change_isr, CHANGE);
}

void setMotorPWM(int pwm) {
  // deadband
  if (abs(pwm) < 15) pwm = 0; 

  // Note: if using IBT-2 as motor driver, one should ensure that both PWM pins are never high simultaneously
  pwm = constrain(pwm, -255, 255);
  if (pwm > 0) {
    digitalWrite(pinPB, LOW);
    analogWrite(pinPA, pwm);
  } else if (pwm < 0) {
    digitalWrite(pinPA, LOW);
    analogWrite(pinPB, -pwm);
  } else {
    digitalWrite(pinPA, LOW);
    digitalWrite(pinPB, LOW);
  }
}

void task_position_control() {
  static unsigned long last_time = 0;
  const int period_ms = 10; // Fast loop (100Hz) is better for position
  
  unsigned long now = millis();
  if (now - last_time >= period_ms) {
    double dt = (now - last_time) / 1000.0;
    last_time = now;

    // 1. Get Target from Potentiometer (mapping to pulses)
    // Example: Map pot to 0-10 full rotations (36 ppr * 5 turns)
    target_position = map(analogRead(potPin), 0, 1023, 0, 36*2);

    // 2. Calculate Error
    long pos_snapshot = current_position; // Snapshot of volatile
    double error = target_position - pos_snapshot;

    // 3. PID Math
    integral += error * dt;
    integral = constrain(integral, -50, 50); // Anti-windup
    double derivative = (error - last_error) / dt;
    
    command = (int)(Kp * error + Ki * integral + Kd * derivative);
    command = constrain(command, -255, 255);
    setMotorPWM(command);
    last_error = error;
  }
}

void loop() {
  task_position_control();

  static unsigned long report_time = 0;
  if (millis() - report_time > 100) {
    Serial.print("Target:"); Serial.print(target_position);
    Serial.print(" Actual:"); Serial.print(current_position);
    Serial.print(" Command:"); Serial.print(command);
    Serial.println();
    report_time = millis();
  }
}