/**
 * @brief stepperMotor.cpp
 * @author Gustice
 * @brief Stepper Motor device class
 *
 * @copyright Copyright (c) 2024
 */

#include "stepperMotor.hpp"
#include "esp_log.h"
#include <cmath>
#include <functional>
#include <vector>

class ProfileGenerator {
  public:
    ProfileGenerator(StepperMotor::Config &config, float dT, float a, float w)
        : _config(config), _dT(dT), _aMax(a), _wMax(w) {
    }

    Profile createSmoothStep(float delta) {
        init();
        // already solved in simple step
        smoothSubStep(delta, _aMax, _wMax);
        return transform();
    }

    Profile createBuildUpStep(float delta) {
        init();

        // t0..t1: increasing swing
        buildUpSwing(_aMax / 4, _wMax, 3);
        auto pnt = getStopDistance(_aMax, _wMax);

        // t1..t2: movement to until almost destination
        moveUntil(_wMax, delta - pnt.p);
        // t2..r3: deceleration until destination
        accelUntil(-_aMax, _wMax, [](Point pnt) { return pnt.w < 0; });

        return transform();
    }

    Profile createRapidStartSwingInEnd(float delta) {
        init();

        auto pnt = getStopDistance(_aMax, _wMax);
        auto wMax = _wMax;
        // t0..t1: acceleration to max velocity
        accelUntil(_aMax * 2, 0, [wMax](Point pnt) { return pnt.w > (wMax * 2); });
        // t1..r2: deceleration until moving speed
        accelUntil(-_aMax, _wMax * 2, [wMax](Point pnt) { return pnt.w < wMax; });
        // t2..t3: movement to until destination (override)
        moveUntil(_wMax, delta);
        // t3..t4: deceleration
        accelUntil(-_aMax, _wMax, [](Point pnt) { return pnt.w < 0; });

        float pCen = delta + (_pVec.back() - delta) / 2;
        accelUntil(-_aMax, 0, [pCen](Point pnt) { return pnt.p < pCen; });
        accelUntil(_aMax, _wBack, [](Point pnt) { return pnt.w > 0; });

        return transform();
    }

    Profile createJumpInTarget(float delta) {
        init();

        float jumpInRange = std::numbers::pi / 3;
        float deltaPre = delta - jumpInRange;

        smoothSubStep(deltaPre, _aMax, _wMax);
        std::vector<float> jumpInSteps{jumpInRange / 2, jumpInRange / 3, jumpInRange / 6};
        float nextPoint = deltaPre;
        for (auto &&step : jumpInSteps) {
            nextPoint = nextPoint + step;
            smoothSubStep(nextPoint, _aMax * 10, _wMax * 2);
        }

        return transform();
    }

  private:
    struct Point {
        float p;
        float w;
        float t;
    };

    StepperMotor::Config &_config;
    float _dT;
    float _aMax;
    float _wMax;

    float _t;
    std::vector<float> _pVec;
    // std::vector<float> _wVec;
    // std::vector<float> _tVec;
    float _wBack;
    float _tBack;

    void init() {
        _pVec = {0};
        // _wVec = {0};
        // _tVec = {0};
        _wBack = 0;
        _tBack = 0;
        _t = 0;
    }

    void append(Point pnt) {
        _pVec.push_back(pnt.p);
        // _wVec.push_back(pnt.w);
        // _tVec.push_back(pnt.t);
    }

    Point getStopDistance(float a, float w) {
        float tDec = w / a;
        float pDec = a * std::pow(tDec, 2) / 2;
        return {pDec, w, tDec};
    }

    void accelUntil(float a, float wInit, std::function<bool(Point)> checkCbk) {
        float t{};
        auto pX = _pVec.back();
        while (true) {
            float w = wInit + a * t;
            float p = pX + wInit * t + a * std::pow(t, 2) / 2;
            if (checkCbk(Point{p, w, t})) {
                break;
            }
            append(Point{p, w, _t + t});
            t = t + _dT;
        }
        _t = _t + t;
    }

    void moveUntil(float wM, float pEnd) {
        float t{};
        auto pX = _pVec.back();
        while (true) {
            float p = pX + wM * t;
            if (p > pEnd) {
                break;
            }
            append(Point{p, wM, _t + t});
            t = t + _dT;
        }
        _t = _t + t;
    }

    void buildUpSwing(float a, float vM, int swings) {
        float t{};
        auto pX = _pVec.back();

        float TwMax = vM / a;
        float tau = 2 * std::numbers::pi;
        float sTau = swings * tau;

        while (true) {
            t = t + _dT;
            float env = a * t;
            float p = std::sin(sTau * t / TwMax) * env * TwMax / sTau;
            float w = (p - _pVec.back()) / _dT;
            if (w > _wMax) {
                break;
            }
            append(Point{p, w, _t + t});
        }
        _t = _t + t;
    }

    void smoothSubStep(float delta, float aM, float wM) {
        auto pnt = getStopDistance(aM, wM);

        if (pnt.p < delta / 2) {
            // t0..t1: acceleration to max velocity
            accelUntil(aM, 0, [wM](Point pnt) { return pnt.w > wM; });
            // t1..t2: movement to until almost destination
            moveUntil(wM, delta - pnt.p);
            // t2..r3: deceleration until destination
            accelUntil(-aM, wM, [](Point pnt) { return pnt.w < 0; });
        } else {
            float pCen = delta + (_pVec.back() - delta) / 2;
            // t0..t1: acceleration until center is reached
            accelUntil(-aM, 0, [pCen](Point pnt) { return pnt.p < pCen; });
            // t2..r3: deceleration until destination
            accelUntil(-aM, wM, [](Point pnt) { return pnt.w < 0; });
        }
    }

    Profile transform() {
        std::vector<uint8_t> steps;
        steps.reserve(_pVec.size() + 1);
        float stp = _config.stepRad * 4;
        float res = stp / _config.resolution;

        steps.push_back(0);
        for (auto &&p : _pVec) {
            auto f = p / stp;
            f = f - std::floor(f);
            uint8_t v = (uint8_t)(f * _config.resolution);
            steps.push_back(v);
        }
        init(); // free memory for old values
        return {steps};
    }
};

void StepperMotor::createDemoProfile() {

    long start = xTaskGetTickCount();
    ProfileGenerator generator(_config, 0.001F, 1.0F, 1.0F);

    std::function<Profile(float)> call{};
    static int idx = 0;

    switch (idx) {
    case 0: {
        call = [&](float f) { return generator.createSmoothStep(f); };
    } break;
    case 1: {
        call = [&](float f) { return generator.createBuildUpStep(f); };
    } break;
    case 2: {
        call = [&](float f) { return generator.createRapidStartSwingInEnd(f); };
    } break;

    default:
    case 3: {
        call = [&](float f) { return generator.createJumpInTarget(f); };
    } break;
    }

    idx++;
    idx = idx % 4;

    auto profile = call(std::numbers::pi);
    long ticks = xTaskGetTickCount() - start;

    ESP_LOGD("Stepper", "TookTicks %lu ( %lu ms) for %d data points", ticks, ticks * portTICK_PERIOD_MS, profile.steps.size());
    ESP_LOG_BUFFER_HEXDUMP("Stepper", profile.steps.data(), profile.steps.size(), ESP_LOG_DEBUG);

    auto pp = std::make_unique<Profile>(std::move(profile));
    runProfile(std::move(pp));
}

void StepperMotor::moveBy(float angle) {
    ProfileGenerator generator(_config, 0.001F, 1.0F, 1.0F);
    auto profile = generator.createSmoothStep(angle);

    ESP_LOGD("Stepper", "TookTicks for %d data points", profile.steps.size());
    ESP_LOG_BUFFER_HEXDUMP("Stepper", profile.steps.data(), profile.steps.size(), ESP_LOG_DEBUG);

    auto pp = std::make_unique<Profile>(std::move(profile));
    runProfile(std::move(pp));
}

void StepperMotor::moveBy(float angle, uint8_t idx, float accel, float speed) {
    long start = xTaskGetTickCount();
    ESP_LOGD("Stepper", "Running %.1f with prof=%i a=%.2f w=%.2f", 
        angle *3 /std::numbers::pi, idx, accel, speed);

    ProfileGenerator generator(_config, 0.001F, accel, speed);

    std::function<Profile(float)> call{};
    switch (idx) {
    case 0: {
        call = [&](float f) { return generator.createSmoothStep(f); };
    } break;
    case 1: {
        call = [&](float f) { return generator.createBuildUpStep(f); };
    } break;
    case 2: {
        call = [&](float f) { return generator.createRapidStartSwingInEnd(f); };
    } break;

    default:
    case 3: {
        call = [&](float f) { return generator.createJumpInTarget(f); };
    } break;
    }
    auto profile = call(angle);
    // long ticks = xTaskGetTickCount() - start;
    // ESP_LOGD("Stepper", "TookTicks %lu ( %lu ms) for %d data points", ticks, ticks * portTICK_PERIOD_MS, profile.steps.size());
    // ESP_LOG_BUFFER_HEXDUMP("Stepper", profile.steps.data(), profile.steps.size(), ESP_LOG_DEBUG);

    auto pp = std::make_unique<Profile>(std::move(profile));
    runProfile(std::move(pp));
}