#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
double RAMP_RATE = 3.1875;           // Maximum PWM change per loop iteration (affects acceleration smoothness)
double LOOP_DELAY = 100;         // Loop delay in milliseconds (affects effective ramp rate)
float SMOOTHING_ALPHA = 0.2;  // Exponential smoothing factor (0 < alpha <= 1)
float DECAY_THRESHOLD = 0.7;   // Threshold for fast decay when pot value drops

int calibratedMin = 1023;     // Minimum pot reading during calibration
int calibratedMax = 0;        // Maximum pot reading during calibration
bool calibrationDone = false; // Flag indicating calibration is complete
unsigned long calibrationStartTime;  // Timestamp when calibration starts

int targetPWM = 0;            // Desired PWM value derived from pot reading
double PrecisePWM = 0;
int currentPWM = 0;           // Actual PWM value output (gradually updated toward targetPWM)
float smoothedReading = 0;    // Exponential moving average of the potentiometer reading

void setup() {
  Serial.begin(9600);
  pinMode(PWM_PIN, OUTPUT);
  
  // Start calibration and initialize values
  calibrationStartTime = millis();
  int initial = analogRead(POT_PIN);
  calibratedMin = initial;
  calibratedMax = initial;
  smoothedReading = initial;
  
  Serial.println("Starting calibration...");
}


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

  if (!calibrationDone) {
    // Update calibration bounds during the first CALIBRATION_TIME milliseconds
    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); // Short delay during calibration for stability
    return;    // Skip normal operation until calibration is complete
  }

  if (Serial.available()) {
    char cmd = Serial.read();
    
    if (cmd == 'a') {  // Handle Exponential smoothing factor (0 < alpha <= 1)
      float newAlpha = Serial.parseFloat();
      if (newAlpha > 0 && newAlpha <= 1.0) {
        SMOOTHING_ALPHA = newAlpha;
        Serial.print("Alpha set to: ");
        Serial.println(SMOOTHING_ALPHA);
        delay(1000);
      }
    }

    else if (cmd == 'r') {  // Handle Maximum PWM change per loop iteration (affects acceleration smoothness)
      int newRate = Serial.parseInt();
      if (newRate > 0 && newRate <= 100) {
        RAMP_RATE = newRate;
        Serial.print("Ramp rate set to: ");
        Serial.println(RAMP_RATE);
        delay(1000);
      }
    }

    else if (cmd == 'l') {  // Handle Loop delay in milliseconds (affects effective ramp rate)
      int newLoop = Serial.parseInt();
      if (newLoop > 0 && newLoop <= 1000) {
        LOOP_DELAY = newLoop;
        Serial.print("Loop delay set to: ");
        Serial.println(LOOP_DELAY);
        delay(1000);
      }
    }

    else if (cmd == 'd') {  // Handle Threshold for fast decay when pot value drops
      int newDec= Serial.parseFloat();
      if (newDec > 0 && newDec <= 1.0) {
        DECAY_THRESHOLD = newDec;
        Serial.print("Decey set to: ");
        Serial.println(DECAY_THRESHOLD);
        delay(1000);
      }
    }
    
    else if (cmd == 't') {  // set how fast currentPWM should reach targetPWM
      int newTime= Serial.parseFloat();
      if (newTime > 0 && newTime <= 60000) {
        RAMP_RATE = ((255*LOOP_DELAY)/newTime);
        Serial.print("0 -> MAX time set to: ");
        Serial.println((255/RAMP_RATE)*LOOP_DELAY);
        delay(1000);
      }
    }

    else if (cmd == 'p') {  // Print all current settings
      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("========================");
      delay(5000);
    }

    else if (cmd == 'x') {
      // Reset to default values
      SMOOTHING_ALPHA = 0.2;
      RAMP_RATE = 5;
      LOOP_DELAY = 50;
      DECAY_THRESHOLD = 0.9;
      Serial.println("Settings reset to defaults.");
      delay(1000);
    }

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

    else if (cmd == 'h') {  // Help - print available commands
      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("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("==========================");
    }

    // Clear any remaining input using non-loop method
    Serial.readString();
  }
  
  // Fast decay: if the current pot reading is significantly lower than the smoothed value,
  // immediately drop the smoothedReading to the pot value.
  if (potReading < smoothedReading * DECAY_THRESHOLD) {
    smoothedReading = potReading;
  } else {
    // Otherwise, use exponential smoothing to update the reading.
    smoothedReading = SMOOTHING_ALPHA * potReading + (1 - SMOOTHING_ALPHA) * smoothedReading;
  }

  // Map the smoothed value from the calibrated range to a PWM value (0–255)
  targetPWM = map((int)smoothedReading, calibratedMin+50, calibratedMax-50, 0, 255);
  targetPWM = constrain(targetPWM, 0, 255);

  // Gradually update currentPWM toward targetPWM using a constant rate (RAMP_RATE)
  if (PrecisePWM < targetPWM) {
    PrecisePWM += (double)(c_time - prev_time) * RAMP_RATE / LOOP_DELAY;
  } 
  if (PrecisePWM > targetPWM) {
    PrecisePWM = targetPWM;
  }
  currentPWM = (int)PrecisePWM;

  // Output the ramped PWM signal to the motor controller (only one PWM output here)
  analogWrite(PWM_PIN, currentPWM);

  // Debug output to Serial
  Serial.print("Pot: ");
  Serial.print(potReading);
  Serial.print(" | Smoothed: ");
  Serial.print(smoothedReading, 1);
  Serial.print(" | Target PWM: ");
  Serial.print(targetPWM);
  Serial.print(" | Current PWM: ");
  Serial.println(currentPWM);

  prev_time = c_time;
}