/*
 * Standing desk leg controller — manual UP/DOWN via capacitive touch
 *
 * Touch sensors (TTP223, output HIGH when touched):
 *   - UP sensor   on J6 pin 4 (UART_RX -> D7)
 *   - DOWN sensor on J5 pin 3 (SDA     -> D4)
 *   - VCC and GND shared between both sensors
 *
 * Stepper driver TMC2209 on PCB:
 *   - STEP -> D2
 *   - DIR  -> D3
 *   - EN   -> D1 (active LOW, auto-managed)
 *
 * Soft position limits (no endstop or ToF yet):
 *   - Position is set to 0 at boot — that's "the floor" for this session.
 *   - MAX_POSITION_STEPS is the upper soft limit.
 *   - Holding UP near MAX or DOWN near MIN does nothing (motor stops).
 *
 * Adjust MAX_POSITION_STEPS and STEPS_PER_MM for your mechanics.
 */

#include <FastAccelStepper.h>

const uint8_t PIN_TOUCH_UP   = D7;   // J6 pin 4 (UART_RX)
const uint8_t PIN_TOUCH_DWN  = D4;   // J5 pin 3 (SDA)

const uint8_t PIN_STEP = D2;
const uint8_t PIN_DIR  = D3;
const uint8_t PIN_EN   = D1;

// ----- Mechanics -----
// Open-loop calibration: pulses per mm of linear travel.
// Motor without gearbox: 200 steps/rev x 8 microsteps / 8 mm/rev (T8x8) = 200 pulses/mm
// Motor 17HS15-1684S-PG5 (5.18:1) + TR10x3: 200 x 8 x 5.18 / 3 = 2763 pulses/mm
const int32_t STEPS_PER_MM = 200;        // change to 2763 once geared motor + TR10x3 is in place

// ----- Soft travel limits -----
const int32_t MIN_POSITION_STEPS = 0;
const int32_t MAX_POSITION_STEPS = 200L * STEPS_PER_MM;   // 200 mm of travel

// ----- Motion profile -----
const uint32_t MAX_SPEED_HZ  = 6400;
const uint32_t ACCELERATION  = 3200;

// ----- Touch debounce -----
const unsigned long DEBOUNCE_MS = 30;

FastAccelStepperEngine engine = FastAccelStepperEngine();
FastAccelStepper *stepper = NULL;

bool stateUp        = LOW;
bool stateDwn       = LOW;
bool lastReadingUp  = LOW;
bool lastReadingDwn = LOW;
unsigned long lastChangeUp  = 0;
unsigned long lastChangeDwn = 0;

void setup() {
  pinMode(PIN_TOUCH_UP,  INPUT);
  pinMode(PIN_TOUCH_DWN, INPUT);

  Serial.begin(115200);
  while (!Serial && millis() < 3000) {}

  engine.init();
  stepper = engine.stepperConnectToPin(PIN_STEP);

  if (!stepper) {
    Serial.println("ERROR: stepper init failed");
    while (true) { delay(1000); }
  }

  stepper->setDirectionPin(PIN_DIR);
  stepper->setEnablePin(PIN_EN);
  stepper->setAutoEnable(true);

  stepper->setSpeedInHz(MAX_SPEED_HZ);
  stepper->setAcceleration(ACCELERATION);
  stepper->setCurrentPosition(0);

  Serial.println("=== Touch UP/DOWN control with soft limits ===");
  Serial.print("Limits: MIN=0   MAX="); Serial.print(MAX_POSITION_STEPS);
  Serial.print(" steps (");
  Serial.print(MAX_POSITION_STEPS / STEPS_PER_MM); Serial.println(" mm)");
  Serial.println("Touch UP (D7) or DOWN (D4). Release to stop.");
}

void loop() {
  bool upTouched  = pollTouch(PIN_TOUCH_UP,  "UP",   stateUp,  lastReadingUp,  lastChangeUp);
  bool dwnTouched = pollTouch(PIN_TOUCH_DWN, "DOWN", stateDwn, lastReadingDwn, lastChangeDwn);

  int32_t pos = stepper->getCurrentPosition();
  bool canMoveUp   = (pos < MAX_POSITION_STEPS);
  bool canMoveDown = (pos > MIN_POSITION_STEPS);

  if (upTouched && !dwnTouched && canMoveUp) {
    stepper->moveTo(MAX_POSITION_STEPS);
  } else if (dwnTouched && !upTouched && canMoveDown) {
    stepper->moveTo(MIN_POSITION_STEPS);
  } else {
    stepper->stopMove();
  }
}

bool pollTouch(uint8_t pin, const char* label,
               bool& stableState, bool& lastReading, unsigned long& lastChange) {
  bool reading = digitalRead(pin);
  unsigned long now = millis();

  if (reading != lastReading) {
    lastChange = now;
    lastReading = reading;
  }

  if ((now - lastChange) > DEBOUNCE_MS && reading != stableState) {
    stableState = reading;
    Serial.print("[");
    Serial.print(now);
    Serial.print(" ms] ");
    Serial.print(label);
    Serial.print(stableState == HIGH ? " TOUCHED" : " released");
    Serial.print(" — pos=");
    Serial.print(stepper->getCurrentPosition());
    Serial.print(" (");
    Serial.print(stepper->getCurrentPosition() / STEPS_PER_MM);
    Serial.println(" mm)");
  }

  return (stableState == HIGH);
}