#include <Wire.h>
///#include <Adafruit_MCP4725.h>

///Adafruit_MCP4725 dac;

#define POT_PIN A1
#define RPM_PIN 2

volatile unsigned int pulseCount = 0;
unsigned long lastTime = 0;
float rpm = 0;
float kmh = 0;
float lastRPM = 0;

const unsigned int pulsesPerRotation = 28;  // Actual confirmed value
const float wheelCircumference = 1.43;      // meters

// --- Tunable Parameters ---
float RAMP_RATE = 51.19;
float LOOP_DELAY = 100;
float SMOOTHING_ALPHA = 0.2;
float DECAY_THRESHOLD = 0.35;
bool debugMode = true;
bool autoCalibrate = false;

int calibratedMin = 1023;
int calibratedMax = 0;
bool calibrationDone = false;
unsigned long calibrationStartTime;

int targetPWM = 0;
float PrecisePWM = 0;
int currentPWM = 0;
float smoothedReading = 0;

unsigned long prevLoopTime = 0;

void countPulse() {
  pulseCount++;
}

void setup() {
  Serial.begin(9600);
  ///dac.begin(0x60);
  pinMode(RPM_PIN, INPUT);
  attachInterrupt(digitalPinToInterrupt(RPM_PIN), countPulse, RISING);

  calibrationStartTime = millis();
  int initial = analogRead(POT_PIN);
  calibratedMin = initial;
  calibratedMax = initial;
  smoothedReading = initial;

  Serial.println("Starting calibration...");
}

void handleSerial() {
  if (!Serial.available()) return;
  char cmd = Serial.read();
  float val;

  switch (cmd) {
    case 'a': val = Serial.parseFloat(); if (val > 0 && val <= 1.0) SMOOTHING_ALPHA = val; break;
    case 'r': val = Serial.parseFloat(); if (val > 0 && val <= 4095) RAMP_RATE = val; break;
    case 'l': val = Serial.parseFloat(); if (val > 0 && val <= 1000) LOOP_DELAY = val; break;
    case 'd': val = Serial.parseFloat(); if (val > 0 && val <= 1.0) DECAY_THRESHOLD = val; break;
    case 't': val = Serial.parseFloat(); if (val > 0 && val <= 120000) RAMP_RATE = (4095 * LOOP_DELAY) / val; break;
    case 'b':
      while (Serial.available() && isspace(Serial.peek())) Serial.read();
      debugMode = Serial.available() ? (Serial.parseInt() > 0) : !debugMode;
      break;
    case 'x':
      SMOOTHING_ALPHA = 0.2; RAMP_RATE = 51.19; LOOP_DELAY = 100.0; DECAY_THRESHOLD = 0.35; debugMode = true;
      break;
    case 'c':
      calibratedMin = 1023; calibratedMax = 0;
      calibrationStartTime = millis(); calibrationDone = false;
      Serial.println("Re-calibration started.");
      break;
    case 'p':
      Serial.println("=== Current Settings ===");
      Serial.print("Alpha: "); Serial.println(SMOOTHING_ALPHA);
      Serial.print("Ramp Rate: "); Serial.println(RAMP_RATE);
      Serial.print("Loop Delay: "); Serial.println(LOOP_DELAY);
      Serial.print("Decay Threshold: "); Serial.println(DECAY_THRESHOLD);
      Serial.print("AutoCalibrate: "); Serial.println(autoCalibrate ? "ON" : "OFF");
      Serial.print("0->MAX time: "); Serial.println((4095 / RAMP_RATE) * LOOP_DELAY);
      Serial.print("Min: "); Serial.print(calibratedMin); Serial.print(" Max: "); Serial.println(calibratedMax);
      break;
    case 'h':
      debugMode = false;
      Serial.println("=== Commands ===");
      Serial.println("a[float] - Alpha");
      Serial.println("r[float] - Ramp Rate");
      Serial.println("l[float] - Loop Delay");
      Serial.println("d[float] - Decay Threshold");
      Serial.println("t[ms] - Set 0->MAX time");
      Serial.println("b / b1 / b0 - Toggle Debug");
      Serial.println("p - Print Settings");
      Serial.println("c - Recalibrate");
      Serial.println("x - Reset to Defaults");
      Serial.println("h - Help");
      break;
  }
  Serial.readString(); // Flush
}

void loop() {
  unsigned long now = millis();
  if (!calibrationDone) {
    int reading = analogRead(POT_PIN);
    if (reading < calibratedMin) calibratedMin = reading;
    if (reading > calibratedMax) calibratedMax = reading;

    if (now - calibrationStartTime >= 5000) {
      calibrationDone = true;
      Serial.println("Calibration complete.");
    }
    delay(10);
    return;
  }

  if (now - prevLoopTime >= LOOP_DELAY) {
    handleSerial();

    // --- RPM Calculation ---
    noInterrupts();
    unsigned int count = pulseCount;
    pulseCount = 0;
    interrupts();

    float revsPerSec = (count / float(pulsesPerRotation)) / (LOOP_DELAY / 1000.0);
    rpm = revsPerSec * 60.0;
    kmh = (rpm * wheelCircumference * 60.0) / 1000.0;

    // --- Pot Reading and Filtering ---
    int potReading = analogRead(POT_PIN);
    if (potReading < smoothedReading * DECAY_THRESHOLD) {
      smoothedReading = potReading;
    } else {
      smoothedReading = SMOOTHING_ALPHA * potReading + (1 - SMOOTHING_ALPHA) * smoothedReading;
    }

    if (autoCalibrate && fabs(rpm - lastRPM) < 10.0) {
      if (potReading < calibratedMin) calibratedMin = potReading;
      if (potReading > calibratedMax) calibratedMax = potReading;
    }
    lastRPM = rpm;

    targetPWM = map((int)smoothedReading, calibratedMin + 50, calibratedMax - 50, 0, 4095);
    targetPWM = constrain(targetPWM, 0, 4095);

    if (PrecisePWM < targetPWM) {
      PrecisePWM += (float)(now - prevLoopTime) * RAMP_RATE / LOOP_DELAY;
    }
    if (PrecisePWM > targetPWM) {
      PrecisePWM = targetPWM;
    }
    currentPWM = (int)PrecisePWM;

    ///dac.setVoltage(currentPWM, false);

    if (debugMode) {
      Serial.print("RPM: "); Serial.print(rpm, 1);
      Serial.print(" | KMH: "); Serial.print(kmh, 2);
      Serial.print(" | Pot: "); Serial.print(potReading);
      Serial.print(" | Smooth: "); Serial.print(smoothedReading);
      Serial.print(" | Target PWM: "); Serial.print(map(targetPWM, 0, 4095, 0, 500));
      Serial.print(" | Output PWM: "); Serial.println(map(currentPWM, 0, 4095, 0, 500));
    }

    prevLoopTime = now;
  }
}