#include <Servo.h>
#define SPEAKER_PIN 8

// ---------------- Pins ----------------
const int sensorPower = 2;  // Sensor power pin (we duty-cycle it per read)
const int sensorPin = A0;   // Sensor signal pin
const int servoPin = 9;     // Servo control pin

const int redLED = 3;
const int whiteLED = 4;
const int blueLED = 5;

Servo waterServo;

// ---------------- Requested zone ranges (for servo mapping) ----------------
// LOW:  0–500    → angles 5..60
// MID:  500–520  → angles 60..90
// HIGH: 520–530  → angles 90..180
const int Z_LOW_MIN = 0; //0
const int Z_LOW_MAX = 199; //400
const int Z_MID_MIN = 200; //451
const int Z_MID_MAX = 412; //545
const int Z_HIGH_MIN = 413; //546
const int Z_HIGH_MAX = 440; //580

// LEDs aligned to your ranges
const int LOW_THRESHOLD = Z_LOW_MAX;  // 500
const int MID_THRESHOLD = Z_MID_MAX;  // 520

// ---- Hysteresis (to prevent premature HIGH due to spikes) ----
// Tune these if it still flips too early/late.
const int HYST_LOW_TO_MID_ENTER = 199;   // must reach ≥505 to leave LOW
const int HYST_MID_TO_LOW_EXIT = 200;    // must fall ≤495 to leave MID to LOW
const int HYST_MID_TO_HIGH_ENTER = 412;  // must reach ≥522 to leave MID to HIGH
const int HYST_HIGH_TO_MID_EXIT = 413;   // must fall ≤518 to leave HIGH to MID

// Require the value to stay beyond a threshold for a bit before committing
const unsigned long ZONE_DWELL_MS = 300;  // 300ms dwell

// ---------------- Servo motion tuning (your base) ----------------
float filteredValue = 0.0f;
float currentAngle = 0.0f;
const float alpha = 0.12f;   // smoothing factor (slightly higher to tame noise)
const float easing = 0.05f;  // servo easing speed

// Overall servo limits
const int MIN_ANGLE = 5;
const int MAX_ANGLE = 270;

// Per-zone angle bands
const int LOW_MIN_DEG = 5;
const int LOW_MAX_DEG = 60;
const int MID_MIN_DEG = 60;
const int MID_MAX_DEG = 90;
const int HIGH_MIN_DEG = 90;
const int HIGH_MAX_DEG = 180;

// ---------------- Sound (non-blocking via millis) ----------------
// LOW: continuous low tone
const int LOW_TONE_FREQ = 300;
// MID: slow beeps (no delay)
const int MID_BEEP_FREQ = 700;
const unsigned long MID_ON_MS = 180;
const unsigned long MID_OFF_MS = 820;
// HIGH: silent

enum Level { LEVEL_LOW,
             LEVEL_MID,
             LEVEL_HIGH };
Level currentLevel = LEVEL_LOW;

bool midBeepOn = false;
unsigned long midTimestamp = 0;

// dwell state
Level pendingLevel = LEVEL_LOW;
unsigned long dwellStartMs = 0;
bool dwellActive = false;

static inline float fmap(float x, float in_min, float in_max, float out_min, float out_max) {
  if (in_max == in_min) return out_min;
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

int median7() {
  // Read sensor 7 times and return median (robust to spikes)
  int v[7];
  for (int i = 0; i < 7; i++) {
    v[i] = analogRead(sensorPin);
    delayMicroseconds(500);  // tiny spacing
  }
  // simple insertion sort for 7 elements
  for (int i = 1; i < 7; i++) {
    int key = v[i], j = i - 1;
    while (j >= 0 && v[j] > key) {
      v[j + 1] = v[j];
      j--;
    }
    v[j + 1] = key;
  }
  return v[3];  // median
}

void enterLevel(Level lvl) {
  currentLevel = lvl;
  if (lvl == LEVEL_LOW) {
    tone(SPEAKER_PIN, LOW_TONE_FREQ);  // steady low tone
    midBeepOn = false;
  } else if (lvl == LEVEL_MID) {
    tone(SPEAKER_PIN, MID_BEEP_FREQ);  // start MID beep
    midBeepOn = true;
    midTimestamp = millis();
  } else {                // LEVEL_HIGH
    noTone(SPEAKER_PIN);  // HIGH is silent
    midBeepOn = false;
  }
}

void startDwell(Level target) {
  pendingLevel = target;
  dwellStartMs = millis();
  dwellActive = true;
}

void tryCommitDwell(Level candidate) {
  if (!dwellActive) {
    startDwell(candidate);
    return;
  }
  if (candidate != pendingLevel) {
    startDwell(candidate);
    return;
  }
  if (millis() - dwellStartMs >= ZONE_DWELL_MS) {
    dwellActive = false;
    if (pendingLevel != currentLevel) enterLevel(pendingLevel);
  }
}

void updateMidBeep() {
  unsigned long now = millis();
  if (midBeepOn) {
    if (now - midTimestamp >= MID_ON_MS) {
      noTone(SPEAKER_PIN);
      midBeepOn = false;
      midTimestamp = now;
    }
  } else {
    if (now - midTimestamp >= MID_OFF_MS) {
      tone(SPEAKER_PIN, MID_BEEP_FREQ);
      midBeepOn = true;
      midTimestamp = now;
    }
  }
}

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

  pinMode(sensorPower, OUTPUT);
  digitalWrite(sensorPower, LOW);  // we’ll pulse it during reads to reduce galvanic corrosion

  pinMode(redLED, OUTPUT);
  pinMode(whiteLED, OUTPUT);
  pinMode(blueLED, OUTPUT);

#if defined(ARDUINO_ARCH_SAMD) || defined(ARDUINO_ARCH_NRF5) || defined(ARDUINO_ARCH_MEGAAVR) || defined(ARDUINO_ARDUINO_NANO33BLE) || defined(ARDUINO_ARCH_ESP32)
  analogReadResolution(10);
#endif

  waterServo.attach(servoPin, 500, 2400);
  delay(200);
  waterServo.write(MIN_ANGLE);
  currentAngle = MIN_ANGLE;
  delay(150);

  enterLevel(LEVEL_LOW);
}

void loop() {
  // --- Power the sensor briefly (settle longer to reduce spikes) ---
  digitalWrite(sensorPower, HIGH);
  delay(20);            // increased settle time
  int raw = median7();  // robust sample
  digitalWrite(sensorPower, LOW);

  // --- Smooth the reading ---
  filteredValue = (alpha * raw) + ((1.0f - alpha) * filteredValue);

  // --- Decide candidate zone with hysteresis on the FILTERED value ---
  Level candidate = currentLevel;

  if (currentLevel == LEVEL_LOW) {
    if (filteredValue >= HYST_LOW_TO_MID_ENTER) candidate = LEVEL_MID;
  } else if (currentLevel == LEVEL_MID) {
    if (filteredValue <= HYST_MID_TO_LOW_EXIT) candidate = LEVEL_LOW;
    else if (filteredValue >= HYST_MID_TO_HIGH_ENTER) candidate = LEVEL_HIGH;
  } else {  // LEVEL_HIGH
    if (filteredValue <= HYST_HIGH_TO_MID_EXIT) candidate = LEVEL_MID;
  }

  // --- Dwell logic: require stability before switching zones ---
  tryCommitDwell(candidate);

  // --- Map to per-zone servo sub-range using your hard ranges ---
  float targetAngle;
  if (currentLevel == LEVEL_LOW) {
    // Map 0..500  -> 5..60
    float fv = filteredValue;
    if (fv < Z_LOW_MIN) fv = Z_LOW_MIN;
    if (fv > Z_LOW_MAX) fv = Z_LOW_MAX;
    targetAngle = fmap(fv, Z_LOW_MIN, Z_LOW_MAX, LOW_MIN_DEG, LOW_MAX_DEG);
  } else if (currentLevel == LEVEL_MID) {
    // Map 500..520 -> 60..90
    float fv = filteredValue;
    if (fv < Z_MID_MIN) fv = Z_MID_MIN;
    if (fv > Z_MID_MAX) fv = Z_MID_MAX;
    targetAngle = fmap(fv, Z_MID_MIN, Z_MID_MAX, MID_MIN_DEG, MID_MAX_DEG);
  } else {  // HIGH
    // Map 520..530 -> 90..180
    float fv = filteredValue;
    if (fv < Z_HIGH_MIN) fv = Z_HIGH_MIN;
    if (fv > Z_HIGH_MAX) fv = Z_HIGH_MAX;
    targetAngle = fmap(fv, Z_HIGH_MIN, Z_HIGH_MAX, HIGH_MIN_DEG, HIGH_MAX_DEG);
  }

  // Constrain and ease
  if (targetAngle < MIN_ANGLE) targetAngle = MIN_ANGLE;
  if (targetAngle > MAX_ANGLE) targetAngle = MAX_ANGLE;

  currentAngle += (targetAngle - currentAngle) * easing;
  waterServo.write((int)currentAngle);

  // --- LEDs from RAW (snappier) but you can switch to filtered if you want less flicker ---
  if (raw <= LOW_THRESHOLD) {
    digitalWrite(redLED, HIGH);
    digitalWrite(whiteLED, LOW);
    digitalWrite(blueLED, LOW);
  } else if (raw <= MID_THRESHOLD) {
    digitalWrite(redLED, LOW);
    digitalWrite(whiteLED, HIGH);
    digitalWrite(blueLED, LOW);
  } else {
    digitalWrite(redLED, LOW);
    digitalWrite(whiteLED, LOW);
    digitalWrite(blueLED, HIGH);
  }

  // --- Sound updates (non-blocking) ---
  if (currentLevel == LEVEL_MID) {
    updateMidBeep();
  } else if (currentLevel == LEVEL_HIGH) {
    noTone(SPEAKER_PIN);  // ensure silence
  }
  // LOW uses continuous tone started in enterLevel()

  // --- Debug (rate-limited) ---
  static unsigned long lastPrint = 0;
  unsigned long now = millis();
  if (now - lastPrint > 200) {
    lastPrint = now;
    Serial.print("Raw: ");
    Serial.print(raw);
    Serial.print(" | Filt: ");
    Serial.print(filteredValue, 1);
    Serial.print(" | Level: ");
    Serial.print(currentLevel == LEVEL_LOW ? "LOW" : (currentLevel == LEVEL_MID ? "MID" : "HIGH"));
    Serial.print(" | Target: ");
    Serial.print(targetAngle, 1);
    Serial.print(" | Servo: ");
    Serial.println(currentAngle, 1);
  }
}

Credit: ChatGPT, Professor Danny Rozin