#include "StepperManager.h"
#include "diagnostics_manager.h" // FEAT-3: Stepper event logging
#include "mqtt_manager.h"    // To publish state on stop
#include "sinric_manager.h" // To report position to Google Home
#include "storage.h"        // To save position on stop

StepperManager stepperManager;

StepperManager::StepperManager()
    : _stepper(AccelStepper::DRIVER, PIN_STEPPER_STEP, PIN_STEPPER_DIR),
      _lastPublishTime(0), _lastSaveTime(0), _idleStartTime(0), _jogDirection(0),
      _lastSavedPosition(0), _isMotorEnabled(false), _lastMoveWasUp(false), _hasRelaxed(false), _isRelaxing(false), _stepperTimer(nullptr), _stepperMutex(nullptr) {}

void StepperManager::begin() {
  _stepperMutex = xSemaphoreCreateMutex();

  _stepper.setEnablePin(PIN_STEPPER_EN);
  _stepper.setPinsInverted(false, false,
                           true); // Dir, Step, Enable (Active LOW)

  // Ensure minimum step pulse width is large enough so the driver reliably
  // detects and reacts to every step at full torque, preventing missed steps.
  _stepper.setMinPulseWidth(20);

  _stepper.setMaxSpeed(config.stepper_open_speed > 0 ? config.stepper_open_speed : 4000);
  uint32_t accel = config.stepper_acceleration > 0 ? config.stepper_acceleration : 2000;
  _stepper.setAcceleration(accel);

  // Restore position from EEPROM so calibration survives reboots
  _stepper.setCurrentPosition(config.stepper_current_position);
  _lastSavedPosition = config.stepper_current_position;
  Serial.printf("[Stepper] Restored position: %ld\n",
                config.stepper_current_position);

  // Initialize motor state. 
  // We start enabled to hold position during boot, and let idle timeout handle disable.
  enableMotor();

  // Initialize highest-precision timer (50us = 20,000 Hz) to smoothly drive the
  // stepper bypassing any latency/blocking in the main loop() or WiFi task.
  esp_timer_create_args_t timer_args = {};
  timer_args.callback = &StepperManager::timerCallback;
  timer_args.arg = this;
  timer_args.name = "stepper_timer";

  if (esp_timer_create(&timer_args, &_stepperTimer) == ESP_OK) {
    esp_timer_start_periodic(_stepperTimer, 50);
    Serial.println("[Stepper] Initialized 20kHz precision hardware timer.");
  } else {
    Serial.println("[Stepper] ERROR initializing timer!");
  }
}

void StepperManager::timerCallback(void *arg) {
  StepperManager *sm = static_cast<StepperManager *>(arg);
  // esp_timer callbacks on ESP-IDF 5.x run in a dedicated high-priority RTOS
  // task (NOT a true ISR). Therefore we use xSemaphoreTake (task-safe API)
  // with zero timeout to avoid blocking. If the main loop holds the lock,
  // we gracefully skip this single 50us tick (imperceptible at 20kHz).
  if (xSemaphoreTake(sm->_stepperMutex, 0) == pdTRUE) {
    sm->_stepper.run();
    xSemaphoreGive(sm->_stepperMutex);
  }
}

void StepperManager::update() {
  bool isMotorMoving = false;
  long distanceToGo = 0;
  long currentPos = 0;

  if (xSemaphoreTake(_stepperMutex, portMAX_DELAY) == pdTRUE) {
    if (_jogDirection != 0 && _stepper.distanceToGo() == 0) {
      // Keep moving if we somehow reached the end of the large jog distance
      _stepper.move(_jogDirection * 1000000);
    }

    distanceToGo = _stepper.distanceToGo();
    currentPos = _stepper.currentPosition();

    // Tension relief: when target reached after upward move, perform a small reverse
    // to relieve cable/cord tension. Skipped entirely if relax steps is 0 (disabled).
    if (distanceToGo == 0 && _jogDirection == 0 && _lastMoveWasUp && !_hasRelaxed && isCalibrated()) {
        _hasRelaxed = true;
        uint32_t relaxSteps = config.stepper_relax_steps;
        
        if (relaxSteps > 0) {
            // Backlash/slack direction: move towards bottom position
            long stepDir = (config.stepper_bottom_position > config.stepper_top_position) ? 1 : -1;
            _stepper.setMaxSpeed(200); // 200 steps/sec relax speed (smoother transition)
            _stepper.move(stepDir * (long)relaxSteps);
            distanceToGo = _stepper.distanceToGo();
            _isRelaxing = true;
            Serial.printf("[Stepper] Target reached. Performing %lu-step tension relief...\n", relaxSteps);
        } else {
            Serial.println("[Stepper] Target reached. Tension relief disabled (0 steps).");
        }
    }

    // Detect when tension relief completes and restore full speed
    if (_isRelaxing && distanceToGo == 0 && _jogDirection == 0) {
        _isRelaxing = false;
        // CRITICAL: Restore max speed from config. Without this, the next
        // movement command inherits the 200 steps/sec relax speed, making
        // the blind appear extremely slow/stuck (Bug #6).
        uint32_t speed = config.stepper_open_speed > 0 ? config.stepper_open_speed : 4000;
        _stepper.setMaxSpeed(speed);
        Serial.println("[Stepper] Tension relief complete. Speed restored.");
    }

    isMotorMoving = (distanceToGo != 0 || _jogDirection != 0);

    xSemaphoreGive(_stepperMutex);
  }

  if (isMotorMoving) {
    // Periodically publish state while moving (every 0.25 seconds)
    if (millis() - _lastPublishTime >= 250) {
      if (mqttManager.isEnabled() && mqttManager.isConnected())
        mqttManager.publishState(true);
      _lastPublishTime = millis();
    }
    // Update base for idle save countdown
    _lastSaveTime = millis();
    _idleStartTime = millis();
  } else {
    // Idle state
    if (_isMotorEnabled && (millis() - _idleStartTime > (config.stepper_stop_delay_ms > 0 ? config.stepper_stop_delay_ms : 3000))) {
      disableMotor();
    }

    // Persists configuration if we've stopped for at least 500ms
    if (currentPos != _lastSavedPosition) {
      if (millis() - _lastSaveTime > 500) {
        // FEAT-3: Log completed move for diagnostics
        diagnosticsManager.logStepperEvent(
            "MOVE", _lastSavedPosition, currentPos, currentPos,
            millis() - _lastSaveTime, 0);
        diagnosticsManager.incrementStepperMoves();

        config.stepper_current_position = currentPos;
        _lastSavedPosition = currentPos;
        storage.save();

        if (mqttManager.isEnabled() && mqttManager.isConnected())
          mqttManager.publishState();

        // Report final position to Google Home / Alexa
        sinricManager.reportPosition(getCurrentPercentage());
      }
    }
  }
}

bool StepperManager::isCalibrated() const {
  return config.stepper_top_position != config.stepper_bottom_position;
}

bool StepperManager::isMoving() {
  bool moving = false;
  if (xSemaphoreTake(_stepperMutex, portMAX_DELAY) == pdTRUE) {
    moving = _stepper.distanceToGo() != 0 || _jogDirection != 0;
    xSemaphoreGive(_stepperMutex);
  }
  return moving;
}

void StepperManager::jog(int8_t direction) {
  enableMotor();
  if (xSemaphoreTake(_stepperMutex, portMAX_DELAY) == pdTRUE) {
    _jogDirection = direction;
    _hasRelaxed = true; // Disable relax during calibration jogs
    _isRelaxing = false; // Cancel any active relax
    if (direction == 0) {
      _stepper.stop();
    } else {
      uint32_t speed = (direction > 0) ? config.stepper_open_speed : config.stepper_close_speed;
      _stepper.setMaxSpeed(speed > 0 ? speed : 4000);
      _stepper.move(direction * 1000000);
    }
    xSemaphoreGive(_stepperMutex);
  }
}

void StepperManager::setTargetPercentage(int percentage, uint32_t overrideSpeed) {
  if (!isCalibrated())
    return;

  if (xSemaphoreTake(_stepperMutex, portMAX_DELAY) == pdTRUE) {
    _jogDirection = 0;
    // If a new position command arrives during tension relief, cancel the relax.
    // All command sources (MQTT, Sinric, automation, touch) converge here.
    _isRelaxing = false;
    xSemaphoreGive(_stepperMutex);
  }

  if (percentage < 0)
    percentage = 0;
  if (percentage > 100)
    percentage = 100;

  // Use int64_t to prevent overflow: with 16-microstep TMC2209 drivers,
  // a full blind travel can easily be 50,000+ steps. (50000 * 100) overflows
  // a 32-bit long. int64_t guarantees correctness up to billions of steps.
  int64_t range =
      (int64_t)config.stepper_top_position - config.stepper_bottom_position;
  long targetPos =
      config.stepper_bottom_position + (long)(range * percentage / 100);

  int currentPct = getCurrentPercentage();
  uint32_t speed = overrideSpeed > 0 ? overrideSpeed : ((percentage >= currentPct) ? config.stepper_open_speed : config.stepper_close_speed);

  enableMotor();

  if (xSemaphoreTake(_stepperMutex, portMAX_DELAY) == pdTRUE) {
    long currentPos = _stepper.currentPosition();
    long dist = targetPos - currentPos;
    if (dist != 0) {
       long stepToTopDir = (config.stepper_top_position >= config.stepper_bottom_position) ? 1 : -1;
       int8_t moveDir = (dist > 0) ? 1 : -1;
       _lastMoveWasUp = (moveDir == stepToTopDir);
       _hasRelaxed = false;
    }

    _stepper.setMaxSpeed(speed > 0 ? speed : 4000);
    _stepper.moveTo(targetPos);
    xSemaphoreGive(_stepperMutex);
  }

  // Publish state to indicate movement started
  if (mqttManager.isEnabled() && mqttManager.isConnected()) {
    mqttManager.publishState(true);
  }
}

int StepperManager::getCurrentPercentage() {
  if (!isCalibrated())
    return 0;

  int64_t range =
      (int64_t)config.stepper_top_position - config.stepper_bottom_position;
  if (range == 0)
    return 0;

  long currentPos = 0;
  if (xSemaphoreTake(_stepperMutex, portMAX_DELAY) == pdTRUE) {
    currentPos = _stepper.currentPosition();
    xSemaphoreGive(_stepperMutex);
  }

  int64_t current = (int64_t)currentPos - config.stepper_bottom_position;
  int pct = (int)(current * 100 / range);

  if (pct < 0)
    pct = 0;
  if (pct > 100)
    pct = 100;
  return pct;
}

int StepperManager::getTargetPercentage() {
  if (!isCalibrated())
    return 0;

  int64_t range =
      (int64_t)config.stepper_top_position - config.stepper_bottom_position;
  if (range == 0)
    return 0;

  long targetPos = 0;
  if (xSemaphoreTake(_stepperMutex, portMAX_DELAY) == pdTRUE) {
    targetPos = _stepper.targetPosition();
    xSemaphoreGive(_stepperMutex);
  }

  int64_t target = (int64_t)targetPos - config.stepper_bottom_position;
  int pct = (int)(target * 100 / range);

  if (pct < 0)
    pct = 0;
  if (pct > 100)
    pct = 100;
  return pct;
}

long StepperManager::getCurrentAbsolutePosition() {
  long pos = 0;
  if (xSemaphoreTake(_stepperMutex, portMAX_DELAY) == pdTRUE) {
    pos = _stepper.currentPosition();
    xSemaphoreGive(_stepperMutex);
  }
  return pos;
}

void StepperManager::open() { setTargetPercentage(100); }

void StepperManager::close() { setTargetPercentage(0); }

void StepperManager::stop() {
  if (xSemaphoreTake(_stepperMutex, portMAX_DELAY) == pdTRUE) {
    long dist = _stepper.distanceToGo();
    if (dist != 0) {
       long stepToTopDir = (config.stepper_top_position >= config.stepper_bottom_position) ? 1 : -1;
       int8_t moveDir = (dist > 0) ? 1 : -1;
       _lastMoveWasUp = (moveDir == stepToTopDir);
    }
    _jogDirection = 0;
    _hasRelaxed = false;
    _isRelaxing = false;
    _stepper.stop(); // Calculates new target position to decel cleanly
    xSemaphoreGive(_stepperMutex);
  }
}

// FEAT-7: Emergency Stop — immediate halt without deceleration.
// Unlike stop() which gracefully decelerates, this zeroes the target
// position instantly by setting currentPosition = current position.
// Use when the blind hits an obstruction or user needs immediate halt.
void StepperManager::emergencyStop() {
  if (xSemaphoreTake(_stepperMutex, portMAX_DELAY) == pdTRUE) {
    _jogDirection = 0;
    _hasRelaxed = true; // Skip tension relief after emergency stop
    _isRelaxing = false;
    // setCurrentPosition() sets both current and target to the same value,
    // immediately zeroing distanceToGo and halting motion.
    _stepper.setCurrentPosition(_stepper.currentPosition());
    xSemaphoreGive(_stepperMutex);
  }
  // Disable motor to release mechanical tension against obstruction
  disableMotor();
  long eStopPos = getCurrentAbsolutePosition();
  Serial.printf("[Stepper] EMERGENCY STOP at pos %ld — motor halted and disabled\n", eStopPos);

  // Persist position immediately — if device reboots after e-stop,
  // the blind must resume from the correct halted position.
  config.stepper_current_position = eStopPos;
  _lastSavedPosition = eStopPos;
  storage.save();

  // FEAT-3: Log emergency stop event with current position
  diagnosticsManager.logStepperEvent(
      "E_STOP", eStopPos, eStopPos, 0, 0, 0);
  diagnosticsManager.logGeneralEvent("MOT", "Emergency stop triggered");

  // Notify frontend of new position
  if (mqttManager.isEnabled() && mqttManager.isConnected())
    mqttManager.publishState(true);
}

// Lightweight instant halt — zeroes distanceToGo immediately.
// Unlike emergencyStop(), does NOT disable the motor, persist position,
// or publish state. Designed for touch interactions where a new movement
// command follows immediately (e.g., double-tap to reverse direction).
// The subsequent setTargetPercentage() call will handle speed/direction.
void StepperManager::instantStop() {
  if (xSemaphoreTake(_stepperMutex, portMAX_DELAY) == pdTRUE) {
    _jogDirection = 0;
    _hasRelaxed = true;
    _isRelaxing = false;
    _stepper.setCurrentPosition(_stepper.currentPosition());
    xSemaphoreGive(_stepperMutex);
  }
}

// Cancel tension relief micro-movement and restore full operating speed.
// Used by TouchManager when the user taps during the relax phase — the tap
// should start a new open/close movement, not waste itself stopping the
// barely-visible relax wiggle.
void StepperManager::stopRelax() {
  if (xSemaphoreTake(_stepperMutex, portMAX_DELAY) == pdTRUE) {
    _isRelaxing = false;
    _hasRelaxed = true;
    _stepper.setCurrentPosition(_stepper.currentPosition());
    // Restore full speed from config so the next move isn't capped at 200 steps/sec
    uint32_t speed = config.stepper_open_speed > 0 ? config.stepper_open_speed : 4000;
    _stepper.setMaxSpeed(speed);
    xSemaphoreGive(_stepperMutex);
  }
  Serial.println("[Stepper] Tension relief cancelled by user input.");
}

void StepperManager::setMaxSpeed(int speed) {
  // Clamp to safe range: minimum 100 steps/s to avoid stalls,
  // maximum 10000 steps/s (limited by 20kHz timer and TMC2209 capability)
  if (speed < 100)
    speed = 100;
  if (speed > 10000)
    speed = 10000;
  if (xSemaphoreTake(_stepperMutex, portMAX_DELAY) == pdTRUE) {
    _stepper.setMaxSpeed(speed);
    xSemaphoreGive(_stepperMutex);
  }
}

void StepperManager::setAcceleration(int accel) {
  // Clamp to safe range: minimum 200 steps/s² for usable motion,
  // maximum 8000 steps/s² to prevent missed steps
  if (accel < 200)
    accel = 200;
  if (accel > 8000)
    accel = 8000;
  if (xSemaphoreTake(_stepperMutex, portMAX_DELAY) == pdTRUE) {
    _stepper.setAcceleration(accel);
    xSemaphoreGive(_stepperMutex);
  }
}

void StepperManager::enableMotor() {
  if (xSemaphoreTake(_stepperMutex, portMAX_DELAY) == pdTRUE) {
    if (!_isMotorEnabled) {
      _stepper.enableOutputs();
      _isMotorEnabled = true;
      _idleStartTime = millis();
      diagnosticsManager.incrementMotorCycles(); // FEAT-3
      // TMC2209 stabilization: The driver needs a brief period after enable
      // to establish stable current regulation. Without this, the first
      // step pulses may be missed, causing perceived sluggishness.
      delayMicroseconds(500);
      Serial.println("[Stepper] Motor enabled.");
    }
    xSemaphoreGive(_stepperMutex);
  }
}

void StepperManager::disableMotor() {
  if (xSemaphoreTake(_stepperMutex, portMAX_DELAY) == pdTRUE) {
    if (_isMotorEnabled) {
      _stepper.disableOutputs();
      _isMotorEnabled = false;
      diagnosticsManager.incrementMotorCycles(); // FEAT-3
      Serial.println("[Stepper] Motor disabled (idle timeout).");
    }
    xSemaphoreGive(_stepperMutex);
  }
}
