/**
 * @file stepperMotor.hpp
 * @author Gustice
 * @brief Stepper Motor device class
 *
 * @copyright Copyright (c) 2024
 */
#pragma once

#include "gpTimer.hpp"
#include "gpioPort.hpp"
#include "stepperDrv.hpp"
#include <Task.hpp>
#include <cmath>
#include <memory>

struct Profile {
    std::vector<uint8_t> steps;
};

class StepperMotor {
    enum Mode {
        Idle = 0,
        HomingFromSensor, // if home sensor already active
        HomingToSensorBegin,
        HomingToSensorEnd,
        Homing,
        Run,
        CrawlToEnd,
    };

  public:
    template <int MAX, int PREC>
    struct Position {
        uint16_t position{};
        uint16_t subPos{};
        Position &operator+(Position &lhs) {
            auto v1 = getFixPoint();
            auto v2 = lhs.getFixPoint();
            auto v = v1 + v2;
            if (v > (MAX << 16)) {
                v -= (MAX << 16);
            }
            position = getPosition(v);
            subPos = getSubPos(v);
            return *this;
        }

        Position &operator-(Position &lhs) {
            auto v1 = getFixPoint();
            auto v2 = lhs.getFixPoint();
            auto v = v1 - v2;
            if (v < (MAX << 16)) {
                v += (MAX << 16);
            }
            position = getPosition(v);
            subPos = getSubPos(v);
            return *this;
        }

        Position &operator+(StepperDriver::StepCplx &pos) {
            position += pos.increment;
            if (position > MAX) {
                position -= MAX;
            } else if (position < 0) {
                position += MAX;
            } else {
                // nothing to do here
            }
            subPos += pos.subPos;
            return *this;
        }

        Position &halv() {
            auto v1 = getFixPoint();
            auto v = v1 / 2;
            position = getPosition(v);
            subPos = getSubPos(v);
            return *this;
        }

        float getScaled() {
            auto scaled = (float)position / MAX;
            // scaled += (float)subPos / (1 << PREC);
            return scaled;
        }

      private:
        int32_t getFixPoint() {
            int32_t value = (((int32_t)position) << 16) | (subPos << (16 - PREC));
            return value;
        }
        uint16_t getPosition(int32_t value) {
            auto pos = (value >> 16) & 0xFFFF;
            return pos;
        }
        uint16_t getSubPos(int32_t value) {
            auto pos = ((value & 0xFFFF) >> (16 - PREC));
            return pos;
        }
    };

    using Trajectory = std::unique_ptr<Profile>;

    struct Config {
        Config(float stepDeg, int microsteps)
            : step(stepDeg), resolution(microsteps), resolutionQuad(resolution/4) {}

        /// @brief Step-size in degree (e.g. 1.8 deg)
        float step;
        /// @brief Step-size in radiant (e.g. 1.8 deg)
        float stepRad{(float)(std::numbers::pi * step / 180)};
        /// @brief Number of microsteps for four steps
        int resolution;
        /// @brief Number of microsteps for four steps
        int resolutionQuad;
        /// @brief Number of Steps for full revolution
        int steps{std::lround(360.0F / step)};
    };

    StepperMotor(Config &config, StepperDriver &driver, InputPort &initSensor)
        : _config(config), _driver(driver), _initSensor(initSensor),
          _timer(TimerCallParam, *this) {
        _timer.setup(1'000, HwTimerBase::Mode::Periodic);
        _timer.start();
    }
    ~StepperMotor() {};

    void findRef() {
        ESP_LOGI("Stepper", "Homing ...");
        if (_initSensor.read()) {
            _mode = Mode::HomingFromSensor;
            ESP_LOGI("Stepper", "First leave HomePos");
        } else {
            _mode = Mode::HomingToSensorBegin;
            ESP_LOGI("Stepper", "Find start of HomePos");
        }

        while (_mode != Mode::Idle) {
            fos::Task::delayTicks(1);
        }

        int fullStep = _config.resolution / 4;
        _driver.setPos(0);

        if (_initSensor.read()) {
            ESP_LOGI("Stepper", "Need to move away from home");
            // MoveAway
            while (_initSensor.read()) {
                (void)_driver.step(-fullStep);
                fos::Task::delayTicks(1);
            }
            for (size_t i = 0; i < 8; i++) // Drive another 4 Steps away
            {
                (void)_driver.step(-fullStep);
                fos::Task::delayTicks(1);
            }
        }

        ESP_LOGI("Stepper", "Fast find right side of home sensor");
        // Find right Endpoint
        while (not _initSensor.read()) {
            (void)_driver.step(fullStep);
            fos::Task::delayTicks(1);
        }
        while (_initSensor.read()) { // move away
            (void)_driver.step(-fullStep);
            fos::Task::delayTicks(1);
        }
        ESP_LOGI("Stepper", "Find right side of home sensor");
        while (not _initSensor.read()) {
            (void)_driver.step(1);
            fos::Task::delayTicks(1);
        }
        int pos{};
        resetPosition();
        ESP_LOGI("Stepper", "Find end of sensor");
        while (_initSensor.read()) {
            (void)_driver.step(1);
            pos++;
            fos::Task::delayTicks(1);
        }
        for (size_t i = 0; i < 32; i++) // Drive another 4 Steps away
        {
            (void)_driver.step(1);
            pos++;
            fos::Task::delayTicks(1);
        }

        ESP_LOGI("Stepper", "Find left side of home sensor");
        while (not _initSensor.read()) {
            (void)_driver.step(-1);
            pos--;
            fos::Task::delayTicks(1);
        }

        ESP_LOGI("Stepper", "Move back to mid position %d", pos / 2);
        for (size_t i = 0; i < pos / 2; i++) {
            (void)_driver.step(-1);
            fos::Task::delayTicks(1);
        }
        ESP_LOGI("Stepper", "Reset position");
        resetPosition();
    }

    void demoFullTurn() {
        auto steps = _config.resolution * _config.steps;
        ESP_LOGI("Stepper", "Running %i * %i steps for full turn", _config.resolution, _config.steps);

        ESP_LOGI("Stepper", "Start is %f.2", getScaledPos());
        for (size_t i = 0; i < _config.resolutionQuad * _config.steps; i++) {
            auto delta = _driver.step(1);
            _indexPos = _indexPos + delta;
            fos::Task::delayTicks(1);
        }
        ESP_LOGI("Stepper", "End is %f.2", getScaledPos());
    }

    void createDemoProfile();
    void moveBy(float angle);
    void moveBy(float angle, uint8_t profile, float accel, float speed);

    void runProfile(Trajectory profile) {
        if (profile == nullptr) {
            return;
        }

        _trajectory = std::move(profile);
        _step = 0;
        _mode = Run;
    }

    bool isIdle() {
        return _mode == Mode::Idle;
    }

    float getScaledPos() {
        return _indexPos.getScaled();
    }

    void resetPosition() {
        _indexPos = Position<50, 5>();
    }

  private:
    Config &_config;
    StepperDriver &_driver;
    InputPort &_initSensor;
    HwTimerT<StepperMotor &> _timer;
    Mode _mode{};
    int _step{};
    Trajectory _trajectory{};
    Position<50, 5> _indexPos{};

    static bool TimerCallParam(HwTimerT<StepperMotor &> &, StepperMotor &stepper) {
        switch (stepper._mode) {
        case Mode::HomingFromSensor: {
            if (stepper._initSensor.read() == false) {
                stepper._mode = HomingToSensorBegin;
                break;
            }
            (void)stepper._driver.step(1);
        } break;

        case Mode::HomingToSensorBegin: {
            if (stepper._initSensor.read()) {
                stepper._mode = HomingToSensorEnd;
                stepper._indexPos = {0, stepper._driver.getSubPos()};
                break;
            }
            (void)stepper._driver.step(1);
        } break;

        case Mode::HomingToSensorEnd: {
            if (stepper._initSensor.read() == false) {
                stepper._mode = Homing;
                stepper._indexPos = stepper._indexPos.halv();
                break;
            }
            auto delta = stepper._driver.step(1);
            stepper._indexPos = stepper._indexPos + delta;
        } break;

        case Mode::Homing: {
            if (stepper._indexPos.position == 0) {
                stepper._mode = Idle;
                break;
            }
            auto delta = stepper._driver.step(-1);
            stepper._indexPos = stepper._indexPos + delta;
        } break;

        case Mode::Run: {
            auto stp = stepper._trajectory->steps[stepper._step];
            auto delta = stepper._driver.setPos(stp);
            stepper._indexPos = stepper._indexPos + delta;

            stepper._step++;
            if (stepper._step >= stepper._trajectory->steps.size()) {
                stepper._mode = Idle;
            }
        } break;

        case Mode::Idle:

        default:
            break;
        }

        return true;
    }
};