#define POT_PIN A0            // Analog input for potentiometer
#define PWM_PIN 9             // PWM output pin for throttle control

#define CALIBRATION_TIME 5000 // Calibration period in milliseconds

float RAMP_RATE = 3.1875;
float LOOP_DELAY = 100;
float SMOOTHING_ALPHA = 0.2;
float DECAY_THRESHOLD = 0.7;
bool debugMode = true;

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 c_time, prev_time;

void setup() {
  Serial.begin(9600);
  pinMode(PWM_PIN, OUTPUT);

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

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

void loop() {
  unsigned long elapsed = millis() - calibrationStartTime;
  int potReading = analogRead(POT_PIN);
  c_time = millis();

  if (!calibrationDone) {
    if (potReading < calibratedMin) calibratedMin = potReading;
    if (potReading > calibratedMax) calibratedMax = potReading;

    Serial.print("Calibrating... Elapsed: ");
    Serial.print(elapsed);
    Serial.print(" ms, Min: ");
    Serial.print(calibratedMin);
    Serial.print(", Max: ");
    Serial.println(calibratedMax);

    if (elapsed >= CALIBRATION_TIME) {
      calibrationDone = true;
      Serial.println("Calibration complete!");
      Serial.print("Final Calibrated Min: ");
      Serial.print(calibratedMin);
      Serial.print(", Max: ");
      Serial.println(calibratedMax);
    }
    delay(10);
    return;
  }

  if (Serial.available()) {
    char cmd = Serial.read();

    if (cmd == 'a') {
      float newAlpha = Serial.parseFloat();
      if (newAlpha > 0 && newAlpha <= 1.0) {
        SMOOTHING_ALPHA = newAlpha;
        Serial.print("Alpha set to: ");
        Serial.println(SMOOTHING_ALPHA);
      }
    }

    else if (cmd == 'r') {
      float newRate = Serial.parseFloat();
      if (newRate > 0 && newRate <= 255) {
        RAMP_RATE = newRate;
        Serial.print("Ramp rate set to: ");
        Serial.println(RAMP_RATE);
      }
    }

    else if (cmd == 'l') {
      float newLoop = Serial.parseFloat();
      if (newLoop > 0 && newLoop <= 1000) {
        LOOP_DELAY = newLoop;
        Serial.print("Loop delay set to: ");
        Serial.println(LOOP_DELAY);
      }
    }

    else if (cmd == 'd') {
      float newDec = Serial.parseFloat();
      if (newDec > 0 && newDec <= 1.0) {
        DECAY_THRESHOLD = newDec;
        Serial.print("Decay threshold set to: ");
        Serial.println(DECAY_THRESHOLD);
      }
    }

    else if (cmd == 't') {
      float newTime = Serial.parseFloat();
      if (newTime > 0 && newTime <= 120000) {
        RAMP_RATE = (255 * LOOP_DELAY) / newTime;
        Serial.print("0 -> MAX time set to: ");
        Serial.println((255 / RAMP_RATE) * LOOP_DELAY);
      }
    }

    else if (cmd == 'p') {
      debugMode = false;
      Serial.println("=== Current Settings ===");
      Serial.print("Smoothing 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.println("========================");
      Serial.print("0 -> MAX: ");
      Serial.println((255 / RAMP_RATE) * LOOP_DELAY);
      Serial.println("========================");
      Serial.print("Calibrated Min: ");
      Serial.println(calibratedMin);
      Serial.print("Calibrated Max: ");
      Serial.println(calibratedMax);
      Serial.println("========================");
      Serial.print("Debug Mode: ");
      Serial.println(debugMode ? "ON" : "OFF");
      Serial.println("========================");
    }

    else if (cmd == 'b') {
      while (Serial.available() && isspace(Serial.peek())) Serial.read(); // skip whitespace
      if (Serial.available()) {
        int val = Serial.parseInt();
        debugMode = (val > 0);
        Serial.print("Debug mode set to: ");
      } else {
        debugMode = !debugMode;
        Serial.print("Debug mode toggled: ");
      }
      Serial.println(debugMode ? "ON" : "OFF");
      delay(500);
    }

    else if (cmd == 'x') {
      SMOOTHING_ALPHA = 0.2;
      RAMP_RATE = 5.10;
      LOOP_DELAY = 100.0;
      DECAY_THRESHOLD = 0.9;
      debugMode = true;
      Serial.println("Settings reset to defaults.");
      delay(1000);
    }

    else if (cmd == 'c') {
      calibratedMin = 1023;
      calibratedMax = 0;
      calibrationStartTime = millis();
      calibrationDone = false;
      Serial.println("Re-calibration started.");
      delay(200);
    }

    else if (cmd == 'h') {
      debugMode = false;
      Serial.println("=== Available Commands ===");
      Serial.println("a[value] - Set smoothing alpha (0 < alpha <= 1.0)");
      Serial.println("r[value] - Set ramp rate (1 - 100)");
      Serial.println("l[value] - Set loop delay in ms (1 - 1000)");
      Serial.println("d[value] - Set decay threshold (0 < value <= 1.0)");
      Serial.println("t[value] - Set 0->max time in ms (0 < value <= 60000)");
      Serial.println("b / b1 / b0 - Toggle or set debug mode ON/OFF");
      Serial.println("p - Print current settings");
      Serial.println("c - Re-calibrate potentiometer");
      Serial.println("x - Reset settings to default values");
      Serial.println("h - Show this help message");
      Serial.println("==========================");
      Serial.print("Debug Mode: ");
      Serial.println(debugMode ? "ON" : "OFF");
      Serial.println("========================");
    }

    Serial.readString(); // Clear serial buffer
  }

  if (potReading < smoothedReading * DECAY_THRESHOLD) {
    smoothedReading = potReading;
  } else {
    smoothedReading = SMOOTHING_ALPHA * potReading + (1 - SMOOTHING_ALPHA) * smoothedReading;
  }

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

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

  analogWrite(PWM_PIN, currentPWM);

  if (debugMode) {
    Serial.print("Pot: ");
    Serial.print(potReading);
    Serial.print(" | Smoothed: ");
    Serial.print(smoothedReading);
    Serial.print(" | Target PWM: ");
    Serial.print(targetPWM);
    Serial.print(" | Current PWM: ");
    Serial.println(currentPWM);
  }

  prev_time = c_time;
}