#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;   // Hall pulses per revolution (rising only)
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;    // 35 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;

// RPM‐cap smoothing (0→1).  
// Higher → more smoothing (slower follow). Lower → faster follow.
const float RPM_SMOOTH_ALPHA = 0.15f;

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

// Hall/RPM (single‐edge original—set PULSES_PER_REV=56 if counting both edges):
volatile unsigned int pulseCount = 0; // increments on RISING edge
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;  // Initialized in setup()

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

// Speed coefficient (for wheel→kmh scaling):
float  speedCoef    = DEFAULT_SPEED_COEF;

// Speed limiter toggle:
bool   speedLimiterOn = DEFAULT_SPEED_LIM;

// “Instant” cap from speed (before smoothing), and a smoothed‐cap:
float  instantCap    = float(DEFAULT_DAC_MIN);
float  smoothedCap   = float(DEFAULT_DAC_MIN);

// Flags:
bool   potCalibrating = false;
unsigned long potCalStart = 0;
bool   debugEnabled   = true;

// 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 on some boards

  // Initialize DAC (commented for now)
  /////dac.begin(DAC_I2C_ADDR);

  // Hall/encoder input: RISING‐edge only
  pinMode(PULSE_PIN, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(PULSE_PIN), countPulseISR, RISING);

  // Load calibration from EEPROM
  loadFromEEPROM();

  // Initialize smoothedPot from actual raw reading so no phantom dead‐zone
  smoothedPot = float(analogRead(POT_PIN));

  // Initialize smoothedCap to the minimum
  smoothedCap = float(dacMin);

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

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

// ─────────────────────────────────────────────────────────────────────────────
// MAIN LOOP (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);

    // Stop if 6 s elapsed
    if (now - potCalStart >= POT_CAL_DURATION_MS) {
      stopPotCal();
      saveToEEPROM();
      Serial.println(F("[POT CAL] Done (timeout 6 s)."));
      helpPauseEndTime = now + HELP_PAUSE_MS;
    }

    handleSerial();
    return;
  }

  // 2) DAC CALIBRATION MODE
  if (calibratingDac) {
    static int testVal = 0;
    testVal = min(testVal + 5, 4095);
    /////dac.setVoltage(testVal, false);
    Serial.print(F("[DAC CAL] Testing DAC=")); Serial.println(testVal);

    if (pulseCount > 0) {
      dacMin = testVal;
      pulseCount = 0;
      calibratingDac = false;
      EEPROM.put(ADDR_DAC_MIN, dacMin);
      Serial.print(F("[DAC CAL] Found minDAC=")); Serial.println(dacMin);
      helpPauseEndTime = now + HELP_PAUSE_MS;
      testVal = 0;
      // Reset smoothedCap to new minimum
      smoothedCap = float(dacMin);
    }

    handleSerial();
    return;
  }

  // 3) NORMAL OPERATION

  // 3a) Update speed via hall pulses (every 100 ms)
  updateSpeed();

  // 3b) Read & smooth pot
  readAndSmoothPot();

  // 3c) Compute rawMapped from pot → [dacMin..4095], apply dead‐band
  int rawMapped;
  {
    if (smoothedPot <= float(potMin + POT_DEADBAND)) {
      rawMapped = 0;
    } else {
      float shifted = smoothedPot - float(potMin + POT_DEADBAND);
      float fullRange = float(potMax - (potMin + POT_DEADBAND));
      if (fullRange <= 0.0f) {
        rawMapped = 0;
      } else {
        rawMapped = int((shifted / fullRange) * float(4095 - dacMin) + float(dacMin) + 0.5f);
        rawMapped = constrain(rawMapped, dacMin, 4095);
      }
    }
  }

  // 3d) Compute instantCap based on speed
  {
    if (speedLimiterOn && currentKmh >= SPEED_LIMIT_KMH) {
      instantCap = 0.0f;
    } else {
      float ratio = currentKmh / SPEED_LIMIT_KMH;
      if (ratio > 1.0f) ratio = 1.0f;
      float capF = float(dacMin) + ratio * float(4095 - dacMin);
      instantCap = capF;
    }
  }

  // 3e) Smooth that cap upward but snap downward instantly
  if (instantCap < smoothedCap) {
    smoothedCap = instantCap; 
  } else {
    smoothedCap = RPM_SMOOTH_ALPHA * instantCap + (1.0f - RPM_SMOOTH_ALPHA) * smoothedCap;
  }

  // 3f) Final DAC = min(rawMapped, int(smoothedCap + 0.5))
  int finalDAC = rawMapped;
  {
    int capInt = int(smoothedCap + 0.5f);
    if (finalDAC > capInt) finalDAC = capInt;
  }

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

  // 3g) Debug print (unless 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("  | rawMapped=")); Serial.print(rawMapped);
    Serial.print(F("  | instantCap=")); Serial.print(int(instantCap));
    Serial.print(F("  | smoothedCap=")); Serial.print(int(smoothedCap));
    Serial.print(F("  | finalDAC=")); Serial.print(finalDAC);
    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"));
  }

  // 3h) Serial commands
  handleSerial();
}

// ─────────────────────────────────────────────────────────────────────────────
// INTERRUPT: count rising‐edge hall pulses
// ─────────────────────────────────────────────────────────────────────────────
void countPulseISR() {
  pulseCount++;
}

// ─────────────────────────────────────────────────────────────────────────────
// readAndSmoothPot():
//  - Snap down immediately if raw < smoothedPot
//  - Smooth upward by POT_SMOOTH_ALPHA if raw > smoothedPot
// ─────────────────────────────────────────────────────────────────────────────
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():
//  - Count hall pulses in 100 ms, compute currentRpm and currentKmh
//  - Each revolution = PULSES_PER_REV rising edges
// ─────────────────────────────────────────────────────────────────────────────
void updateSpeed() {
  noInterrupts();
  unsigned int count = pulseCount;
  pulseCount = 0;
  interrupts();

  float revs = float(count) / PULSES_PER_REV;       // revs in 0.1 s
  currentRpm = revs * 600.0f;                       // → RPM
  float mps = (currentRpm / 60.0f) * WHEEL_CIRC_M;   // → m/s
  currentKmh = mps * 3.6f * speedCoef;               // → km/h, then apply coef
}

// ─────────────────────────────────────────────────────────────────────────────
// handleSerial():
//   cp    → toggle POT calibration (6 s)
//   cd    → start DAC calibration (ramp until 1 edge seen)
//   sl    → toggle 35 km/h limiter
//   scX.Y → set speedCoef to X.Y
//   sd    → print current status (pauses debug for 5 s)
//   db    → toggle debug prints
//   hp    → print help menu (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. 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 edge is detected
// ─────────────────────────────────────────────────────────────────────────────
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 edge)"));
  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 potMin, potMax, dacMin, speedCoef, limiter state
// ─────────────────────────────────────────────────────────────────────────────
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 fallback:
  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;
}