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

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

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

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


    StepperMotor(StepperConfig &config, StepperDriver &driver, InputPort &initSensor)
        : _config(config), _driver(driver), _initSensor(initSensor),
          _timer(TimerCallParam, *this),
          _buffer{},
          _generator(_config, _buffer, 0.001F) {
        _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() {
        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 newProfile() {
        _subStep = 0;
        _mode = StartNew;
    }

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

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

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

  private:
    StepperConfig &_config;
    StepperDriver &_driver;
    InputPort &_initSensor;
    HwTimerT<StepperMotor &> _timer;
    Mode _mode{};
    ProfileBuffer _buffer;
    ProfileGenerator _generator;
    int32_t _subStep;
    const ProfileBuffer::Segment *_pSlice{};

    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._pSlice->at(stepper._subStep);
            stepper._subStep++;
            if (stepper._subStep >= stepper._pSlice->size()) {
                stepper._subStep = 0;
                if (stepper._buffer.reachedFinal()) {
                    stepper._mode = Idle;
                } else {
                    stepper._pSlice = stepper._buffer.get();
                }
            }
            auto delta = stepper._driver.setPos(stp);
            stepper._indexPos = stepper._indexPos + delta;
        } break;

        case Mode::StartNew: {
            if (stepper._buffer.canStart()) {
                stepper._mode = Run;
                stepper._pSlice = stepper._buffer.get();
                stepper._subStep = 0;
            }
        }

        case Mode::Idle:

        default:
            break;
        }

        return true;
    }
};