/*
 * Standing desk leg controller - Wokwi simulation.
 *
 * Position simulation: measuredPos is updated internally by
 * counting STEP pulses. Each step changes the position by one
 * unit in the current direction. On real hardware, measuredPos
 * would come from the VL53L0X ToF sensor.
 *
 * Controls:
 *   SIT     -> target = 200
 *   STAND   -> target = 700
 *   STOP    -> halt motion, disable driver
 *   ENDSTOP -> reset position to 0 (simulates homing)
 *   POT     -> fine manual adjust: if the potentiometer moves
 *              more than POT_DEADBAND units from the last
 *              accepted reading, it takes over as the target
 *              (overrides any preset). This mimics the manual
 *              height adjust knob on a real standing desk,
 *              which needs a deadband to avoid ADC noise
 *              constantly retargeting the motion.
 */

#include <Wire.h>
#include <LiquidCrystal_I2C.h>

// ---------- Pin mapping ----------
const uint8_t PIN_STEP    = 2;
const uint8_t PIN_DIR     = 3;
const uint8_t PIN_ENABLE  = 8;

const uint8_t PIN_SDA     = 4;   // I2C0 SDA
const uint8_t PIN_SCL     = 5;   // I2C0 SCL

const uint8_t PIN_BTN_SIT    = 10;
const uint8_t PIN_BTN_STAND  = 11;
const uint8_t PIN_BTN_STOP   = 12;
const uint8_t PIN_ENDSTOP    = 13;

const uint8_t PIN_POT        = 28;   // ADC2

// ---------- Motion / control ----------
const int POS_MIN       = 0;
const int POS_MAX       = 1000;
const int PRESET_SIT    = 200;
const int PRESET_STAND  = 700;
const int TOLERANCE     = 2;

const unsigned long STEP_INTERVAL_US = 2000;  // ~500 steps/s
const uint8_t DIR_UP    = HIGH;
const uint8_t DIR_DOWN  = LOW;

// Manual override (potentiometer) — deadband avoids ADC noise
// constantly retargeting the motion. The pot only takes control
// when its reading differs from the last accepted value by more
// than POT_DEADBAND.
const int POT_DEADBAND  = 15;

// ---------- State ----------
int  target      = 0;
int  measuredPos = 0;
bool moving      = false;
int  lastDir     = DIR_UP;

int  lastPotReading  = -1000;  // force first read to register
bool manualMode      = false;

unsigned long lastStepMicros = 0;
unsigned long lastLcdMillis  = 0;
unsigned long lastSerialMillis = 0;

LiquidCrystal_I2C lcd(0x27, 16, 2);

// ---------- Helpers ----------
int readPotScaled() {
  int raw = analogRead(PIN_POT);
  // Wokwi's potentiometer model returns 0-1023 at full range.
  return map(raw, 0, 1023, POS_MIN, POS_MAX);
}

void enableDriver(bool on) {
  digitalWrite(PIN_ENABLE, on ? LOW : HIGH);
}

void pulseStep() {
  digitalWrite(PIN_STEP, HIGH);
  delayMicroseconds(5);
  digitalWrite(PIN_STEP, LOW);

  if (lastDir == DIR_UP)   measuredPos++;
  else                     measuredPos--;

  measuredPos = constrain(measuredPos, POS_MIN, POS_MAX);
}

void updateLcd() {
  if (millis() - lastLcdMillis < 100) return;
  lastLcdMillis = millis();

  lcd.setCursor(0, 0);
  lcd.print("Target: ");
  lcd.print(target);
  if (manualMode) lcd.print(" M ");
  else            lcd.print("   ");
  lcd.print("   ");

  lcd.setCursor(0, 1);
  lcd.print("Actual: ");
  lcd.print(measuredPos);
  lcd.print("    ");
}

void setTarget(int newTarget, bool manual) {
  target = constrain(newTarget, POS_MIN, POS_MAX);
  moving = true;
  manualMode = manual;
  enableDriver(true);
  Serial.print("New target: ");
  Serial.print(target);
  Serial.println(manual ? " (manual)" : " (preset)");
}

void stopMotion() {
  moving = false;
  enableDriver(false);
  Serial.println("Stopped.");
}

void setup() {
  Serial.begin(115200);
  delay(500);
  Serial.println("=== Standing desk leg controller ===");
  Serial.println("Booting...");

  pinMode(PIN_STEP, OUTPUT);
  pinMode(PIN_DIR, OUTPUT);
  pinMode(PIN_ENABLE, OUTPUT);
  enableDriver(false);

  pinMode(PIN_BTN_SIT,   INPUT_PULLUP);
  pinMode(PIN_BTN_STAND, INPUT_PULLUP);
  pinMode(PIN_BTN_STOP,  INPUT_PULLUP);
  pinMode(PIN_ENDSTOP,   INPUT_PULLUP);

  Serial.println("Setting up I2C on GP4/GP5...");
  Wire.setSDA(PIN_SDA);
  Wire.setSCL(PIN_SCL);
  Wire.begin();

  Serial.println("Initializing LCD...");
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Ready.");
  lcd.setCursor(0, 1);
  lcd.print("SIT / STAND / POT");
  delay(1200);
  lcd.clear();

  // Seed the pot tracker so the startup position doesn't fire
  // the manual override immediately.
  lastPotReading = readPotScaled();

  Serial.println("Ready. Press SIT, STAND, STOP, ENDSTOP or move the pot.");
}

void loop() {
  if (digitalRead(PIN_BTN_SIT)   == LOW) setTarget(PRESET_SIT,   false);
  if (digitalRead(PIN_BTN_STAND) == LOW) setTarget(PRESET_STAND, false);
  if (digitalRead(PIN_BTN_STOP)  == LOW) stopMotion();

  if (digitalRead(PIN_ENDSTOP) == LOW) {
    Serial.println("Endstop hit. Position reset to 0.");
    measuredPos = POS_MIN;
    target = POS_MIN;
    stopMotion();
  }

  // Manual override: check pot movement against deadband.
  int potNow = readPotScaled();
  if (abs(potNow - lastPotReading) > POT_DEADBAND) {
    lastPotReading = potNow;
    setTarget(potNow, true);
  }

  if (moving) {
    int error = target - measuredPos;

    if (abs(error) <= TOLERANCE) {
      stopMotion();
    } else {
      lastDir = (error > 0) ? DIR_UP : DIR_DOWN;
      digitalWrite(PIN_DIR, lastDir);

      unsigned long now = micros();
      if (now - lastStepMicros >= STEP_INTERVAL_US) {
        pulseStep();
        lastStepMicros = now;
      }
    }
  }

  updateLcd();

  if (millis() - lastSerialMillis > 500) {
    lastSerialMillis = millis();
    Serial.print("target=");
    Serial.print(target);
    Serial.print(" measured=");
    Serial.print(measuredPos);
    Serial.print(" moving=");
    Serial.print(moving);
    Serial.print(" manual=");
    Serial.println(manualMode);
  }
}