#pragma once

#include "stepperDrv.hpp"
#include "stepperMotor.hpp"
#include <cmath>
#include "syslog.hpp"

class WatchMotor {
    static constexpr uint8_t Positions = 6;

  public:
    using AlmostFinishedCb = void (*)(uint8_t newIdx);
    WatchMotor(IStepperMotor &stepper, AlmostFinishedCb onDone)
        : _stepper(stepper),
          _onFinCb(onDone) {
    }

    void moveTo(uint8_t position, uint8_t profile, int minJumps, float accel, float speed) {
        auto sign = position % 6;

        int minTurns = minJumps / 6;

        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;
        }

        int steps = 0;
        if (_currentIndex < sign) {
            steps = (sign - _currentIndex);
        } else if (_currentIndex >= sign) {
            steps = (sign + 6 - _currentIndex);
        }
        steps += minTurns * 6;
        if (steps < minJumps) {
            steps += 6;
        }

        float delta = 2.0F * std::numbers::pi / 6 * steps;

        ESP_LOGD("MV", "Moving to %d by %.2f + %.2f", sign, delta, corr);

        _stepper.moveBy(delta + corr, profile, accel, speed, [&](int diff) {
            Syslog::logv(Syslog::Severity::Debug,"Almost done with head=%i",diff);
            if (diff < 6) {
                Syslog::logv(Syslog::Severity::Warning,"Low headroom in Profile pipeline=%i",diff);
            }
            
            if (_onFinCb) {
                _onFinCb(position);
            }
        });
        _currentIndex = sign;
    }

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

  private:
    IStepperMotor &_stepper;
    uint8_t _currentIndex{};
    AlmostFinishedCb _onFinCb{};
};