/*
  XIAO ESP32-C3 Dual FSR + MOSFET LED + atomizer MVP-style link test

  This version restores the feel of the earlier MVP:
    - A0/A1 pressure drives the LED.
    - Light fades in after pressure starts.
    - Light fades back after pressure releases.
    - If pressure stays stable, the light begins a slow breathing motion.

  Pins:
    Left FSR signal  -> A0
    Right FSR signal -> A1
    MOSFET signal    -> D2 / GPIO4
    Atomizer signal  -> A3 / other_SIG

  Wiring:
    XIAO D2  -> MOSFET TRIG / PWM / IN / SIG
    XIAO GND -> MOSFET GND
    External power - -> MOSFET GND
    External power + -> LED strip +
    LED strip -      -> MOSFET LOAD / OUT / LED-

    XIAO A3 / other_SIG -> atomizer driver IN / SIG / EN
    XIAO GND            -> atomizer driver GND
    Atomizer power -    -> atomizer driver GND

  Serial Monitor:
    Baud: 115200

  Commands:
    C        recalibrate FSR baselines while unloaded
    I        invert FSR direction if pressure reads backwards
    R        reverse MOS logic if light is inverted
    L        LED enable / disable
    T        visible LED output test
    AUTO     return to FSR-driven mode
    P:120    manual PWM test, 0-255
    G:12     idle glow PWM
    N:85     first visible active PWM
    M:180    max LED brightness
    S:45     pressure scale, larger = less sensitive
    AT:3000  fade-in time in ms
    FT:4000  fade-out time in ms
    W:2500   wait before breathing starts in ms
    B:5600   breathing period in ms
    D:45     breathing depth percent
*/

#if defined(A0)
const int FSR_LEFT_PIN = A0;
#else
const int FSR_LEFT_PIN = 2;
#endif

#if defined(A1)
const int FSR_RIGHT_PIN = A1;
#else
const int FSR_RIGHT_PIN = 3;
#endif

#if defined(D2)
const int LED_PIN = D2;
#else
const int LED_PIN = 4;  // XIAO ESP32-C3 D2 = GPIO4
#endif

#if defined(A3)
const int ATOMIZER_PIN = A3;
#else
const int ATOMIZER_PIN = 5;  // XIAO ESP32-C3 A3 / other_SIG fallback
#endif

const int ACTIVE_PRESSURE_THRESHOLD = 3;

int rawLeft = 0;
int rawRight = 0;

float filteredLeft = 0;
float filteredRight = 0;
float baselineLeft = 0;
float baselineRight = 0;
float sensorAlpha = 0.12;

// Earlier MVP behavior: pressing the FSR module made ADC raw move lower.
bool pressureDropsWhenPressed = true;
bool reverseMos = false;
bool ledEnabled = true;
bool autoMode = true;

float pressureScale = 45.0;

int pressureLeft = 0;
int pressureRight = 0;
int pressureTotal = 0;

const bool ATOMIZER_ACTIVE_HIGH = true;
const int ATOMIZER_ON_THRESHOLD = 3;
const int ATOMIZER_PWM_MIN = 0;
const int ATOMIZER_PWM_MAX = 255;
const unsigned long ATOMIZER_ON_DELAY_MS = 0;
const unsigned long ATOMIZER_OFF_DELAY_MS = 1200;
const unsigned long ATOMIZER_RAMP_UP_MS = 2500;
const unsigned long ATOMIZER_RAMP_DOWN_MS = 2500;
const unsigned long ATOMIZER_MAX_ON_MS = 30000;

enum AtomizerState {
  ATOMIZER_OFF,
  ATOMIZER_RAMP_UP,
  ATOMIZER_ON,
  ATOMIZER_RAMP_DOWN
};

AtomizerState atomizerState = ATOMIZER_OFF;
bool atomizerOn = false;
bool atomizerMaxLatched = false;
int atomizerPwm = 0;
int atomizerRampStartPwm = 0;
unsigned long atomizerWantOnSince = 0;
unsigned long atomizerWantOffSince = 0;
unsigned long atomizerRampStartedMs = 0;
unsigned long atomizerTurnedOnAt = 0;

int idleGlowPwm = 12;
int minActivePwm = 85;
int maxPwm = 180;

unsigned long attackTime = 3000;
unsigned long fadeTime = 4000;
unsigned long waitBeforeBreath = 2500;
unsigned long breathPeriod = 5600;
int breathDepthPercent = 45;

float currentPwm = 0;
bool pressureActive = false;
unsigned long pressureStartedMs = 0;
unsigned long lastPrintMs = 0;
unsigned long lastLoopMs = 0;

int clampInt(int value, int low, int high) {
  if (value < low) return low;
  if (value > high) return high;
  return value;
}

float clampFloat(float value, float low, float high) {
  if (value < low) return low;
  if (value > high) return high;
  return value;
}

void writeLed(int pwm) {
  pwm = clampInt(pwm, 0, 255);
  if (!ledEnabled) pwm = 0;

  int out = reverseMos ? 255 - pwm : pwm;
  analogWrite(LED_PIN, out);
}

void writeAtomizerPwm(int pwm) {
  atomizerPwm = clampInt(pwm, ATOMIZER_PWM_MIN, ATOMIZER_PWM_MAX);
  atomizerOn = atomizerPwm > 0;

  int out = ATOMIZER_ACTIVE_HIGH ? atomizerPwm : ATOMIZER_PWM_MAX - atomizerPwm;
  analogWrite(ATOMIZER_PIN, out);
}

void startAtomizerRampUp(unsigned long now) {
  if (atomizerState == ATOMIZER_RAMP_UP || atomizerState == ATOMIZER_ON) return;

  atomizerState = ATOMIZER_RAMP_UP;
  atomizerRampStartPwm = atomizerPwm;
  atomizerRampStartedMs = now;
  if (atomizerPwm == 0) atomizerTurnedOnAt = now;
}

void startAtomizerRampDown(unsigned long now) {
  if (atomizerState == ATOMIZER_RAMP_DOWN || atomizerState == ATOMIZER_OFF) return;

  atomizerState = ATOMIZER_RAMP_DOWN;
  atomizerRampStartPwm = atomizerPwm;
  atomizerRampStartedMs = now;
}

void stopAtomizerNow() {
  atomizerState = ATOMIZER_OFF;
  atomizerOn = false;
  atomizerMaxLatched = false;
  atomizerPwm = 0;
  atomizerRampStartPwm = 0;
  atomizerWantOnSince = 0;
  atomizerWantOffSince = 0;
  atomizerRampStartedMs = 0;
  atomizerTurnedOnAt = 0;
  writeAtomizerPwm(0);
}

void updateAtomizerRamp(unsigned long now) {
  if (atomizerState == ATOMIZER_RAMP_UP) {
    unsigned long elapsed = now - atomizerRampStartedMs;
    if (elapsed >= ATOMIZER_RAMP_UP_MS) {
      writeAtomizerPwm(ATOMIZER_PWM_MAX);
      atomizerState = ATOMIZER_ON;
      return;
    }

    float t = (float)elapsed / (float)ATOMIZER_RAMP_UP_MS;
    float eased = t * t * (3.0 - 2.0 * t);
    int pwm = atomizerRampStartPwm + (int)((ATOMIZER_PWM_MAX - atomizerRampStartPwm) * eased);
    writeAtomizerPwm(pwm);
  } else if (atomizerState == ATOMIZER_RAMP_DOWN) {
    unsigned long elapsed = now - atomizerRampStartedMs;
    if (elapsed >= ATOMIZER_RAMP_DOWN_MS) {
      writeAtomizerPwm(0);
      atomizerState = ATOMIZER_OFF;
      atomizerTurnedOnAt = 0;
      return;
    }

    float t = (float)elapsed / (float)ATOMIZER_RAMP_DOWN_MS;
    float eased = t * t * (3.0 - 2.0 * t);
    int pwm = atomizerRampStartPwm - (int)(atomizerRampStartPwm * eased);
    writeAtomizerPwm(pwm);
  }
}

float readAverage(int pin, int count) {
  long sum = 0;
  for (int i = 0; i < count; i++) {
    sum += analogRead(pin);
    delay(3);
  }
  return (float)sum / (float)count;
}

void calibrateBaselines() {
  Serial.println("CALIBRATING: keep both FSRs unloaded");
  writeLed(0);
  stopAtomizerNow();
  delay(600);

  baselineLeft = readAverage(FSR_LEFT_PIN, 80);
  baselineRight = readAverage(FSR_RIGHT_PIN, 80);
  filteredLeft = baselineLeft;
  filteredRight = baselineRight;

  pressureActive = false;
  pressureStartedMs = 0;
  currentPwm = idleGlowPwm;
  writeLed((int)currentPwm);

  Serial.print("BASELINE left=");
  Serial.print(baselineLeft, 1);
  Serial.print(" right=");
  Serial.println(baselineRight, 1);
}

float pressureFromReading(float filtered, float baseline) {
  float diff = pressureDropsWhenPressed ? baseline - filtered : filtered - baseline;
  if (diff < 0) diff = 0;
  return clampFloat(diff / pressureScale, 0.0, 100.0);
}

void updateSensors() {
  rawLeft = analogRead(FSR_LEFT_PIN);
  rawRight = analogRead(FSR_RIGHT_PIN);

  filteredLeft += sensorAlpha * ((float)rawLeft - filteredLeft);
  filteredRight += sensorAlpha * ((float)rawRight - filteredRight);

  pressureLeft = (int)pressureFromReading(filteredLeft, baselineLeft);
  pressureRight = (int)pressureFromReading(filteredRight, baselineRight);
  pressureTotal = clampInt(pressureLeft + pressureRight, 0, 100);
}

void updateAtomizer() {
  unsigned long now = millis();
  int seatPressure = max(pressureLeft, pressureRight);
  bool seatPresent = seatPressure >= ATOMIZER_ON_THRESHOLD;

  if (!seatPresent) {
    atomizerMaxLatched = false;
  }

  bool activeOrRising = atomizerState == ATOMIZER_RAMP_UP || atomizerState == ATOMIZER_ON;
  if (activeOrRising && atomizerTurnedOnAt > 0 && now - atomizerTurnedOnAt >= ATOMIZER_MAX_ON_MS) {
    atomizerMaxLatched = true;
  }

  bool wantsOn = autoMode && seatPresent && !atomizerMaxLatched;

  if (wantsOn) {
    atomizerWantOffSince = 0;
    if (atomizerWantOnSince == 0) atomizerWantOnSince = now;
    if (now - atomizerWantOnSince >= ATOMIZER_ON_DELAY_MS) {
      startAtomizerRampUp(now);
    }
  } else {
    atomizerWantOnSince = 0;
    if (atomizerWantOffSince == 0) atomizerWantOffSince = now;
    if (now - atomizerWantOffSince >= ATOMIZER_OFF_DELAY_MS) {
      startAtomizerRampDown(now);
    }
  }

  updateAtomizerRamp(now);
}

int targetPwmFromPressure() {
  if (pressureTotal < ACTIVE_PRESSURE_THRESHOLD) {
    pressureActive = false;
    pressureStartedMs = 0;
    return idleGlowPwm;
  }

  if (!pressureActive) {
    pressureActive = true;
    pressureStartedMs = millis();
  }

  float amount = clampFloat((float)pressureTotal / 100.0, 0.0, 1.0);
  int basePwm = minActivePwm + (int)((maxPwm - minActivePwm) * amount);
  basePwm = clampInt(basePwm, minActivePwm, maxPwm);

  if (millis() - pressureStartedMs < waitBeforeBreath) {
    return basePwm;
  }

  float phase = (millis() % breathPeriod) / (float)breathPeriod;
  float wave = 0.5 + 0.5 * sin(phase * 6.2831853);
  float depth = clampFloat(breathDepthPercent / 100.0, 0.0, 0.9);
  int lowPwm = (int)(basePwm * (1.0 - depth));
  int breathingPwm = lowPwm + (int)((basePwm - lowPwm) * wave);

  return clampInt(breathingPwm, idleGlowPwm, maxPwm);
}

void updateLight() {
  unsigned long now = millis();
  unsigned long dt = now - lastLoopMs;
  lastLoopMs = now;
  if (dt == 0 || dt > 250) dt = 20;

  int targetPwm = targetPwmFromPressure();
  float duration = targetPwm > currentPwm ? (float)attackTime : (float)fadeTime;
  duration = max(duration, 1.0f);

  float maxStep = 255.0 * ((float)dt / duration);
  float diff = targetPwm - currentPwm;

  if (abs(diff) <= maxStep) {
    currentPwm = targetPwm;
  } else {
    currentPwm += diff > 0 ? maxStep : -maxStep;
  }

  writeLed((int)currentPwm);
}

void runLedTest() {
  bool wasAuto = autoMode;
  autoMode = false;
  ledEnabled = true;

  Serial.println("LED_TEST full -> off -> medium -> off");
  writeLed(255);
  delay(1000);
  writeLed(0);
  delay(500);
  writeLed(120);
  delay(1000);
  writeLed(0);
  delay(500);

  autoMode = wasAuto;
}

void printStatus() {
  if (millis() - lastPrintMs < 250) return;
  lastPrintMs = millis();

  Serial.print("A0_raw:");
  Serial.print(rawLeft);
  Serial.print(" A1_raw:");
  Serial.print(rawRight);
  Serial.print(" A0_p:");
  Serial.print(pressureLeft);
  Serial.print(" A1_p:");
  Serial.print(pressureRight);
  Serial.print(" total:");
  Serial.print(pressureTotal);
  Serial.print(" seat:");
  Serial.print(max(pressureLeft, pressureRight));
  Serial.print(" pwm:");
  Serial.print((int)currentPwm);
  Serial.print(" atom:");
  Serial.print(atomizerOn ? 1 : 0);
  Serial.print(" atomPwm:");
  Serial.print(atomizerPwm);
  Serial.print(" active:");
  Serial.print(pressureActive ? 1 : 0);
  Serial.print(" auto:");
  Serial.print(autoMode ? 1 : 0);
  Serial.print(" inv:");
  Serial.print(pressureDropsWhenPressed ? 1 : 0);
  Serial.print(" mosR:");
  Serial.println(reverseMos ? 1 : 0);
}

void handleCommand(String cmd) {
  cmd.trim();
  if (cmd.length() == 0) return;

  if (cmd == "C" || cmd == "c") {
    calibrateBaselines();
  } else if (cmd == "I" || cmd == "i") {
    pressureDropsWhenPressed = !pressureDropsWhenPressed;
    Serial.print("pressureDropsWhenPressed=");
    Serial.println(pressureDropsWhenPressed ? "true" : "false");
  } else if (cmd == "R" || cmd == "r") {
    reverseMos = !reverseMos;
    Serial.print("reverseMos=");
    Serial.println(reverseMos ? "true" : "false");
  } else if (cmd == "L" || cmd == "l") {
    ledEnabled = !ledEnabled;
    Serial.print("ledEnabled=");
    Serial.println(ledEnabled ? "true" : "false");
  } else if (cmd == "T" || cmd == "t") {
    runLedTest();
  } else if (cmd == "AUTO" || cmd == "auto") {
    autoMode = true;
    Serial.println("autoMode=true");
  } else if (cmd.startsWith("P:") || cmd.startsWith("p:")) {
    autoMode = false;
    currentPwm = clampInt(cmd.substring(2).toInt(), 0, 255);
    writeLed((int)currentPwm);
    Serial.print("manualPwm=");
    Serial.println((int)currentPwm);
  } else if (cmd.startsWith("G:") || cmd.startsWith("g:")) {
    idleGlowPwm = clampInt(cmd.substring(2).toInt(), 0, 255);
    Serial.print("idleGlowPwm=");
    Serial.println(idleGlowPwm);
  } else if (cmd.startsWith("N:") || cmd.startsWith("n:")) {
    minActivePwm = clampInt(cmd.substring(2).toInt(), 0, 255);
    if (maxPwm < minActivePwm) maxPwm = minActivePwm;
    Serial.print("minActivePwm=");
    Serial.println(minActivePwm);
  } else if (cmd.startsWith("M:") || cmd.startsWith("m:")) {
    maxPwm = clampInt(cmd.substring(2).toInt(), 0, 255);
    if (maxPwm < minActivePwm) minActivePwm = maxPwm;
    Serial.print("maxPwm=");
    Serial.println(maxPwm);
  } else if (cmd.startsWith("S:") || cmd.startsWith("s:")) {
    pressureScale = clampFloat(cmd.substring(2).toFloat(), 1.0, 200.0);
    Serial.print("pressureScale=");
    Serial.println(pressureScale, 1);
  } else if (cmd.startsWith("AT:") || cmd.startsWith("at:")) {
    attackTime = clampInt(cmd.substring(3).toInt(), 100, 15000);
    Serial.print("attackTime=");
    Serial.println(attackTime);
  } else if (cmd.startsWith("FT:") || cmd.startsWith("ft:")) {
    fadeTime = clampInt(cmd.substring(3).toInt(), 100, 15000);
    Serial.print("fadeTime=");
    Serial.println(fadeTime);
  } else if (cmd.startsWith("W:") || cmd.startsWith("w:")) {
    waitBeforeBreath = clampInt(cmd.substring(2).toInt(), 0, 15000);
    Serial.print("waitBeforeBreath=");
    Serial.println(waitBeforeBreath);
  } else if (cmd.startsWith("B:") || cmd.startsWith("b:")) {
    breathPeriod = clampInt(cmd.substring(2).toInt(), 800, 15000);
    Serial.print("breathPeriod=");
    Serial.println(breathPeriod);
  } else if (cmd.startsWith("D:") || cmd.startsWith("d:")) {
    breathDepthPercent = clampInt(cmd.substring(2).toInt(), 0, 90);
    Serial.print("breathDepthPercent=");
    Serial.println(breathDepthPercent);
  }
}

void readSerialCommands() {
  static String input = "";
  static unsigned long lastCharMs = 0;

  while (Serial.available()) {
    char c = Serial.read();
    if (c == '\n' || c == '\r') {
      handleCommand(input);
      input = "";
    } else {
      input += c;
      lastCharMs = millis();
      if (input.length() > 40) input = "";
    }
  }

  if (input.length() > 0 && millis() - lastCharMs > 250) {
    handleCommand(input);
    input = "";
  }
}

void setup() {
  Serial.begin(115200);
  delay(700);

  pinMode(LED_PIN, OUTPUT);
  pinMode(ATOMIZER_PIN, OUTPUT);
  writeLed(0);
  stopAtomizerNow();

  Serial.println();
  Serial.println("Dual FSR + MOS LED + atomizer MVP-style link test");
  Serial.print("Pins: A0=");
  Serial.print(FSR_LEFT_PIN);
  Serial.print(" A1=");
  Serial.print(FSR_RIGHT_PIN);
  Serial.print(" D2/GPIO=");
  Serial.print(LED_PIN);
  Serial.print(" A3/ATOM=");
  Serial.println(ATOMIZER_PIN);
  Serial.println("Atomizer: light pressure -> soft ON 2.5s, release 1.2s -> soft OFF 2.5s, max 30s -> soft OFF until release");
  Serial.println("Commands: C I R L T AUTO P:120 G:12 N:85 M:180 S:45 AT:3000 FT:4000 W:2500 B:5600 D:45");

  calibrateBaselines();
  lastLoopMs = millis();
}

void loop() {
  readSerialCommands();
  updateSensors();
  updateAtomizer();

  if (autoMode) {
    updateLight();
  }

  printStatus();
  delay(20);
}