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

// ─────────────────────────────────────────────────────────────────────────────
// HARDWARE PINS
// ─────────────────────────────────────────────────────────────────────────────
#define POT_PIN        A0      // Throttle potentiometer
#define PULSE_PIN      2       // Hall/encoder input (interrupt)
#define DAC_I2C_ADDR   0x60    // MCP4725 I²C address

// ─────────────────────────────────────────────────────────────────────────────
// EEPROM ADDRESSES (2 bytes each for int/float)
// ─────────────────────────────────────────────────────────────────────────────
#define ADDR_POT_MIN     0    // int (2 bytes)
#define ADDR_POT_MAX     2    // int (2 bytes)
#define ADDR_DAC_MIN     4    // int (2 bytes)
#define ADDR_SPEED_COEF  6    // float (4 bytes)
#define ADDR_SPEED_LIM   10   // bool  (1 byte)

// ─────────────────────────────────────────────────────────────────────────────
// DEFAULTS (used only if EEPROM is “blank” or invalid)
// ─────────────────────────────────────────────────────────────────────────────
const int   DEFAULT_POT_MIN    = 270;
const int   DEFAULT_POT_MAX    = 870;
const int   DEFAULT_DAC_MIN    = 1000;
const float DEFAULT_SPEED_COEF = 1.00f;
const bool  DEFAULT_SPEED_LIM  = true;    // limiter ON by default

// ─────────────────────────────────────────────────────────────────────────────
// CALCULATION CONSTANTS
// ─────────────────────────────────────────────────────────────────────────────
const float PULSES_PER_REV      = 28.0f;    
const float WHEEL_CIRC_M        = 1.43f;    // meters

const unsigned long LOOP_INTERVAL_MS    = 100;     // 100 ms main loop
const unsigned long POT_CAL_DURATION_MS = 6000;    // 6 s for pot calibration
const float SPEED_LIMIT_KMH           = 35.0f;    // km/h hard limiter

// Time to pause regular debug output after HELP/STATUS
const unsigned long HELP_PAUSE_MS       = 5000;    // 5 seconds

// Pot smoothing: if pot rises, apply α smoothing; if pot falls, snap immediately
const float POT_SMOOTH_ALPHA = 0.10f;  

// Dead‐band width (counts above potMin that still count as zero)
const int POT_DEADBAND = 50;

// ─────────────────────────────────────────────────────────────────────────────
// GLOBALS
// ─────────────────────────────────────────────────────────────────────────────
/////Adafruit_MCP4725 dac;

// Hall/RPM:
volatile unsigned int pulseCount = 0;
unsigned long        lastRpmCalc  = 0;
float                currentRpm   = 0.0f;
float                currentKmh   = 0.0f;

// Pot range + smoothing:
int    potMin       = DEFAULT_POT_MIN;
int    potMax       = DEFAULT_POT_MAX;
float  smoothedPot  = 0.0f;  // Will be initialized in setup()

// DAC-min:
int    dacMin       = DEFAULT_DAC_MIN;
bool   calibratingDac = false;

// Speed coefficient (for GPS-vs-wheel scaling):
float  speedCoef    = DEFAULT_SPEED_COEF;

// Speed limiter toggle:
bool   speedLimiterOn = DEFAULT_SPEED_LIM;

// Flags:
bool   potCalibrating = false;      // currently in pot calibration
unsigned long potCalStart = 0;      // when pot calibration began
bool   debugEnabled   = true;       // toggle with “db”

// Pause help/status timing:
unsigned long helpPauseEndTime = 0;

// Timing:
unsigned long lastLoopTime = 0;

// ─────────────────────────────────────────────────────────────────────────────
// FORWARD DECLARATIONS
// ─────────────────────────────────────────────────────────────────────────────
void countPulseISR();
void readAndSmoothPot();
void updateSpeed();
int  computeThrottledDac();
void handleSerial();
void saveToEEPROM();
void loadFromEEPROM();
void startPotCal();
void stopPotCal();
void startDacCal();
void printHelp();
void printStatus();

// ─────────────────────────────────────────────────────────────────────────────
// SETUP
// ─────────────────────────────────────────────────────────────────────────────
void setup() {
  Serial.begin(9600);
  while (!Serial) { ; } // Wait for Serial if needed

  // Initialize DAC
  /////dac.begin(DAC_I2C_ADDR);

  // Hall/encoder input
  pinMode(PULSE_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(PULSE_PIN), countPulseISR, RISING);

  // Load previously saved calibration from EEPROM
  loadFromEEPROM();

  // Immediately read the pot once so smoothedPot starts exactly at raw
  smoothedPot = float(analogRead(POT_PIN));

  // Timestamps
  lastRpmCalc = millis();
  lastLoopTime = millis();

  Serial.println(F("=== READY ==="));
  Serial.println(F("Type 'hp' for help."));
}

// ─────────────────────────────────────────────────────────────────────────────
// MAIN LOOP (runs every 100 ms)
// ─────────────────────────────────────────────────────────────────────────────
void loop() {
  unsigned long now = millis();

  // Only run logic every 100 ms
  if (now - lastLoopTime < LOOP_INTERVAL_MS) return;
  lastLoopTime = now;

  // ─── 1) POT CALIBRATION MODE ───────────────────────────────────────────────
  if (potCalibrating) {
    int raw = analogRead(POT_PIN);
    potMin = min(potMin, raw);
    potMax = max(potMax, raw);

    Serial.print(F("[POT CAL] raw=")); Serial.print(raw);
    Serial.print(F("  min=")); Serial.print(potMin);
    Serial.print(F("  max=")); Serial.println(potMax);

    // If 6 s have passed, stop automatically
    if (now - potCalStart >= POT_CAL_DURATION_MS) {
      stopPotCal();
      saveToEEPROM();
      Serial.println(F("[POT CAL] Done (timeout 6 s)."));
      helpPauseEndTime = now + HELP_PAUSE_MS;
    }

    handleSerial(); // allow “cp” to cancel early
    return;
  }

  // ─── 2) DAC CALIBRATION MODE ───────────────────────────────────────────────
  if (calibratingDac) {
    static int testValue = 0;
    // Ramp in increments of 5 units every 100 ms
    testValue = min(testValue + 5, 4095);
    /////dac.setVoltage(testValue, false);
    Serial.print(F("[DAC CAL] Testing DAC=")); Serial.println(testValue);

    // If the first hall-pulse arrives, record & stop immediately
    if (pulseCount > 0) {
      dacMin = testValue;
      pulseCount = 0;
      calibratingDac = false;
      testValue = 0;
      EEPROM.put(ADDR_DAC_MIN, dacMin);
      Serial.print(F("[DAC CAL] Found minDAC=")); Serial.println(dacMin);
      helpPauseEndTime = now + HELP_PAUSE_MS;
      testValue = 0; // reset for next time
    }

    handleSerial(); // allow “cd” to cancel if needed
    return;
  }

  // ─── 3) NORMAL OPERATION STARTS HERE ────────────────────────────────────

  // 3a) UPDATE SPEED (every 100 ms)
  updateSpeed();

  // 3b) POT READ & SMOOTH
  readAndSmoothPot();

  // 3c) COMPUTE DAC OUTPUT (throttle + speed curve + limiter)
  int dacValue = computeThrottledDac();

  // 3d) SEND TO DAC
  /////dac.setVoltage(dacValue, false);

  // 3e) DEBUG PRINT (only if not currently “paused”)
  if (debugEnabled && now >= helpPauseEndTime) {
    int rawNow = analogRead(POT_PIN);
    Serial.print(F("rawPot=")); Serial.print(rawNow);
    Serial.print(F("  smoothed=")); Serial.print(smoothedPot, 1);
    Serial.print(F("  | DAC=")); Serial.print(dacValue);
    Serial.print(F("  | minDAC=")); Serial.print(dacMin);
    Serial.print(F("  | RPM=")); Serial.print(int(currentRpm));
    Serial.print(F("  | km/h=")); Serial.print(currentKmh, 1);
    Serial.print(F("  | coef=")); Serial.print(speedCoef, 2);
    Serial.print(F("  | lim=")); Serial.println(speedLimiterOn ? F("ON") : F("OFF"));
  }

  // 3f) SERIAL HANDLING (commands)
  handleSerial();
}

// ─────────────────────────────────────────────────────────────────────────────
// INTERRUPT: Count each Hall pulse
// ─────────────────────────────────────────────────────────────────────────────
void countPulseISR() {
  pulseCount++;
}

// ─────────────────────────────────────────────────────────────────────────────
// readAndSmoothPot():
//   - If raw < smoothedPot, snap smoothed down immediately.
//   - Else apply α-smoothing upward.
// ─────────────────────────────────────────────────────────────────────────────
void readAndSmoothPot() {
  int raw = analogRead(POT_PIN);

  if (raw < smoothedPot) {
    smoothedPot = float(raw);
  } else {
    smoothedPot = POT_SMOOTH_ALPHA * float(raw)
               + (1.0f - POT_SMOOTH_ALPHA) * smoothedPot;
  }
}

// ─────────────────────────────────────────────────────────────────────────────
// updateSpeed():
//   - Reads pulseCount over 100 ms to compute RPM & km/h (then applies coef)
// ─────────────────────────────────────────────────────────────────────────────
void updateSpeed() {
  noInterrupts();
  unsigned int count = pulseCount;
  pulseCount = 0;
  interrupts();

  float revs = float(count) / PULSES_PER_REV;
  currentRpm = revs * 600.0f;       // (0.1 s → revs×600)

  // Convert to km/h: (RPM/60)*circ * 3.6
  float mps = (currentRpm / 60.0f) * WHEEL_CIRC_M;
  currentKmh = mps * 3.6f;
  currentKmh *= speedCoef;
}

// ─────────────────────────────────────────────────────────────────────────────
// computeThrottledDac():
//   - If smoothedPot ≤ (potMin + POT_DEADBAND), return 0.
//   - Otherwise, map [potMin+POT_DEADBAND..potMax] → [dacMin..4095].
//   - Compute a speed-dependent maxAllowed = dacMin + ratio*(4095-dacMin).
//   - If limiter ON and currentKmh ≥ limit → 0.
//   - Else constrain mapped to ≤ maxAllowed.
// ─────────────────────────────────────────────────────────────────────────────
int computeThrottledDac() {
  // 1) Dead‐band check
  if (smoothedPot <= float(potMin + POT_DEADBAND)) {
    return 0;
  }

  // 2) Shifted pot value (offset by potMin+deadband)
  float shifted = smoothedPot - float(potMin + POT_DEADBAND);
  float fullRange = float(potMax - (potMin + POT_DEADBAND));
  if (fullRange <= 0.0f) {
    return 0;  // fallback if calibration gave weird values
  }

  // 3) Map shifted [0..fullRange] → [dacMin..4095]
  int rawMapped = int((shifted / fullRange) * float(4095 - dacMin) + float(dacMin) + 0.5f);
  rawMapped = constrain(rawMapped, dacMin, 4095);

  // 4) Speed limiter override if at or above limit
  if (speedLimiterOn && currentKmh >= SPEED_LIMIT_KMH) {
    return 0;
  }

  // 5) Compute allowed max based on speed
  float ratio = currentKmh / SPEED_LIMIT_KMH;
  if (ratio > 1.0f) ratio = 1.0f;
  float maxAllowedF = float(dacMin) + ratio * float(4095 - dacMin);
  int maxAllowed = int(maxAllowedF + 0.5f);

  // 6) Finally, constrain rawMapped to ≤ maxAllowed
  return constrain(rawMapped, 0, maxAllowed);
}

// ─────────────────────────────────────────────────────────────────────────────
// handleSerial():
//   cp   → toggle POT calibration (6 s)  
//   cd   → start DAC calibration  
//   sl   → toggle 35 km/h limiter  
//   scX.Y→ set speedCoef = X.Y  
//   sd   → print current status (pauses debug for 5 s)  
//   db   → toggle debug prints  
//   hp   → print help (pauses debug for 5 s)  
// ─────────────────────────────────────────────────────────────────────────────
void handleSerial() {
  if (!Serial.available()) return;
  String cmd = Serial.readStringUntil('\n');
  cmd.trim();

  if (cmd.equalsIgnoreCase("hp")) {
    printHelp();
    helpPauseEndTime = millis() + HELP_PAUSE_MS;
  }
  else if (cmd.equalsIgnoreCase("cp")) {
    if (!potCalibrating) {
      startPotCal();
    } else {
      stopPotCal();
      saveToEEPROM();
      Serial.println(F("[POT CAL] Saved."));
      helpPauseEndTime = millis() + HELP_PAUSE_MS;
    }
  }
  else if (cmd.equalsIgnoreCase("cd")) {
    if (!calibratingDac) {
      startDacCal();
    }
  }
  else if (cmd.equalsIgnoreCase("sl")) {
    speedLimiterOn = !speedLimiterOn;
    EEPROM.put(ADDR_SPEED_LIM, speedLimiterOn);
    Serial.print(F("[CMD] Speed limiter = "));
    Serial.println(speedLimiterOn ? F("ON") : F("OFF"));
  }
  else if (cmd.startsWith("sc")) {
    String valStr = cmd.substring(2);
    float v = valStr.toFloat();
    if (v > 0.0f) {
      speedCoef = v;
      EEPROM.put(ADDR_SPEED_COEF, speedCoef);
      Serial.print(F("[CMD] speedCoef = "));
      Serial.println(speedCoef, 3);
    } else {
      Serial.println(F("[ERR] Invalid speedCoef format. Use scX.Y (e.g. sc1.05)"));
      helpPauseEndTime = millis() + HELP_PAUSE_MS;
    }
  }
  else if (cmd.equalsIgnoreCase("sd")) {
    printStatus();
    helpPauseEndTime = millis() + HELP_PAUSE_MS;
  }
  else if (cmd.equalsIgnoreCase("db")) {
    debugEnabled = !debugEnabled;
    Serial.print(F("[CMD] Debug = "));
    Serial.println(debugEnabled ? F("ON") : F("OFF"));
  }
  else {
    Serial.print(F("[ERR] Unknown command: "));
    Serial.println(cmd);
    Serial.println(F("       Type 'hp' for help."));
    helpPauseEndTime = millis() + HELP_PAUSE_MS;
  }
}

// ─────────────────────────────────────────────────────────────────────────────
// startPotCal(): begin capturing raw pot min/max
// ─────────────────────────────────────────────────────────────────────────────
void startPotCal() {
  potCalibrating = true;
  potCalStart = millis();
  potMin = 1023;
  potMax = 0;
  Serial.println(F("[CMD] POT calibration started (6 s)..."));
}

// ─────────────────────────────────────────────────────────────────────────────
// stopPotCal(): end pot calibration and save to EEPROM
// ─────────────────────────────────────────────────────────────────────────────
void stopPotCal() {
  potCalibrating = false;
  if (potMin >= potMax) {
    potMin = DEFAULT_POT_MIN;
    potMax = DEFAULT_POT_MAX;
    Serial.println(F("[WARN] POT calibration failed or no movement. Using defaults."));
  } else {
    EEPROM.put(ADDR_POT_MIN, potMin);
    EEPROM.put(ADDR_POT_MAX, potMax);
    Serial.print(F("[POT CAL] Saved min=")); Serial.print(potMin);
    Serial.print(F("  max=")); Serial.println(potMax);
  }
}

// ─────────────────────────────────────────────────────────────────────────────
// startDacCal(): ramp the DAC until a hall pulse is seen
// ─────────────────────────────────────────────────────────────────────────────
void startDacCal() {
  calibratingDac = true;
  pulseCount = 0;   // reset any old pulses
  Serial.println(F("[CMD] Starting DAC calibration..."));
  Serial.println(F("       Motor must be stationary. Ramping DAC..."));
}

// ─────────────────────────────────────────────────────────────────────────────
// printHelp(): show available serial commands
// ─────────────────────────────────────────────────────────────────────────────
void printHelp() {
  Serial.println(F("===== HELP ====="));
  Serial.println(F("cp        → toggle POT calibration (6 s)"));
  Serial.println(F("cd        → start DAC calibration (ramp until 1 pulse)"));
  Serial.println(F("sl        → toggle 35 km/h limiter"));
  Serial.println(F("scX.Y     → set speedCoef to X.Y (e.g. sc1.05)"));
  Serial.println(F("sd        → show current settings"));
  Serial.println(F("db        → toggle debug on/off"));
  Serial.println(F("hp        → show help menu"));
  Serial.println(F("================"));
}

// ─────────────────────────────────────────────────────────────────────────────
// printStatus(): display current potMin, potMax, dacMin, speedCoef, limiter
// ─────────────────────────────────────────────────────────────────────────────
void printStatus() {
  Serial.println(F("=== CURRENT SETTINGS ==="));
  Serial.print(F("potMin    = ")); Serial.println(potMin);
  Serial.print(F("potMax    = ")); Serial.println(potMax);
  Serial.print(F("dacMin    = ")); Serial.println(dacMin);
  Serial.print(F("speedCoef = ")); Serial.println(speedCoef, 3);
  Serial.print(F("limiter   = ")); Serial.println(speedLimiterOn ? F("ON") : F("OFF"));
  Serial.println(F("========================="));
}

// ─────────────────────────────────────────────────────────────────────────────
// saveToEEPROM(): write potMin, potMax, dacMin to EEPROM
// ─────────────────────────────────────────────────────────────────────────────
void saveToEEPROM() {
  EEPROM.put(ADDR_POT_MIN, potMin);
  EEPROM.put(ADDR_POT_MAX, potMax);
  EEPROM.put(ADDR_DAC_MIN, dacMin);
  // speedCoef and speedLimiter saved in their own handlers
}

// ─────────────────────────────────────────────────────────────────────────────
// loadFromEEPROM(): read potMin, potMax, dacMin, speedCoef, limiter
// ─────────────────────────────────────────────────────────────────────────────
void loadFromEEPROM() {
  int storedMin = 0, storedMax = 0, storedDac = 0;
  float storedCoef = 0.0f;
  bool storedLim   = false;

  EEPROM.get(ADDR_POT_MIN, storedMin);
  EEPROM.get(ADDR_POT_MAX, storedMax);
  EEPROM.get(ADDR_DAC_MIN, storedDac);
  EEPROM.get(ADDR_SPEED_COEF, storedCoef);
  EEPROM.get(ADDR_SPEED_LIM, storedLim);

  // Validate or fall back:
  if (storedMin >= 0 && storedMin < 1023 &&
      storedMax > storedMin && storedMax <= 1023) {
    potMin = storedMin;
    potMax = storedMax;
  } else {
    potMin = DEFAULT_POT_MIN;
    potMax = DEFAULT_POT_MAX;
  }

  if (storedDac > 0 && storedDac < 4095) {
    dacMin = storedDac;
  } else {
    dacMin = DEFAULT_DAC_MIN;
  }

  if (storedCoef > 0.0f && storedCoef < 10.0f) {
    speedCoef = storedCoef;
  } else {
    speedCoef = DEFAULT_SPEED_COEF;
  }

  speedLimiterOn = storedLim;
}