#pragma once

#include "StepperDrv8833.hpp"
#include "stepperMotor.hpp"
#include <cmath>

class WatchMotor {
    static constexpr uint8_t Positions = 6;

  public:
    WatchMotor(StepperMotor &stepper, OutputPort &sleep)
        : _stepper(stepper),
          _motorSleep(sleep) {
        _motorSleep.write(false);
    }

    void moveTo(uint8_t sign, uint8_t profile = 0) {
        sign = sign % 6;

        float delta = 2.0F * std::numbers::pi;
        float corr = 0.0F;
        float currentPos = _stepper.getScaledPos() * 6;
        if ((_currentIndex > 4) && (currentPos < 2)) {
            corr = 6 - _currentIndex - currentPos;
        } else if ((_currentIndex < 2) && (currentPos > 2)) {
            corr = _currentIndex + 6 - currentPos;
        } else {
            corr = _currentIndex - currentPos;
        }

        // stepper.getScaledPos() * 6
        if (_currentIndex < sign) {
            delta = delta / 6 * (sign - _currentIndex);
        } else if (_currentIndex > sign) {
            delta = delta / 6 * (sign + 6 - _currentIndex);
        }
        ESP_LOGD("MV", "Moving to %d by %.2f + %.2f", sign, delta, corr);

        _stepper.moveBy(delta + corr, profile);
        _currentIndex = sign;
    }

    float getScaled() { return _stepper.getScaledPos() * 6; }
    uint8_t getIndex() { return _currentIndex; }

  private:
    StepperMotor &_stepper;
    OutputPort &_motorSleep;
    uint8_t _currentIndex{};
};