#pragma once

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

class ProfileBuffer {
  public:
    static constexpr int SliceSize = 128;
    static constexpr int BufferSize = 16;
    using Segment = std::array<uint8_t, SliceSize>;
    using Buffer = std::array<Segment, BufferSize>;
    ProfileBuffer() : _semaphore(BufferSize) {}

    void reset() {
        _idxReader = 0;
        _idxWriter = 0;
        _written = 0;
        _read = 0;
        _minDiff = BufferSize;
        _idxProcess = _scratchpad.size() - 1;
        _writeFinished = false;
    }

    void append(Segment set, bool isFinal = false) {
        // _semaphore.give();
        _writeFinished = isFinal;
        _scratchpad.at(_idxWriter) = set;
        _idxWriter++;
        if (_idxWriter >= _scratchpad.size()) {
            _idxWriter = 0;
        }
        _written++;
        _minDiff = std::min(_minDiff, _written - _read);
    }

    const Segment *get() {
        // _semaphore.take();
        _idxProcess = _idxReader;
        _idxReader++;
        if (_idxReader >= _scratchpad.size()) {
            _idxReader = 0;
        }
        _read++;
        _minDiff = std::min(_minDiff, _written - _read);
        return &_scratchpad[_idxProcess];
    }

    bool reachedFinal() {
        return _writeFinished && (_read == _written);
    }

    bool canWrite() {
        return _idxWriter != _idxProcess;
    }

    bool canStart() {
        return (_written > (BufferSize / 2));
    }

    uint32_t getStats() {
        return _minDiff;
    }
    uint32_t getDiff() {
        return _written - _read;
    }
    uint32_t getRead() {
        return _read;
    }

  private:
    Buffer _scratchpad{};
    fos::Semaphore _semaphore;
    uint32_t _idxReader{};
    uint32_t _idxWriter{};
    uint32_t _written{};
    uint32_t _read{};
    uint32_t _minDiff{BufferSize};
    uint32_t _idxProcess = _scratchpad.size() - 1;
    bool _writeFinished{};
};

class ProfileGenerator {
  public:
    enum Profile {
        Smooth = 0,
        BuildUp,
        RapidStart,
        JumpIn,
    };


    ProfileGenerator(StepperConfig &config, ProfileBuffer &buffer, float dT)
        : _config(config), _transformer(config, buffer), _dT(dT), _aMax(1), _wMax(1) {
    }

    void setDynamic(float a, float w) {
        _aMax = a;
        _wMax = w;
    }

    void createProfile(Profile profile, float delta) {
        init();
        switch (profile) {
        default:
        case Profile::Smooth:
            createSmoothStep(delta);
            break;
        case Profile::BuildUp:
            createBuildUpStep(delta);
            break;
        case Profile::RapidStart:
            createRapidStartSwingInEnd(delta);
            break;

        case Profile::JumpIn:
            createJumpInTarget(delta);
            break;
        }
        _transformer.finish();
    }

  private:
    struct Point {
        float p;
        float w;
        float t;
    };
    class Transformer {
      public:
        Transformer(StepperConfig &config, ProfileBuffer &profile)
            : _config(config), _profile(profile) {
            add(0);
        }

        void add(float p) {
            auto f = p / stp;
            f = f - std::floor(f);
            _last = (uint8_t)(f * _config.resolution);
            _temp[_idx] = _last;
            _idx++;
            if (_idx >= _temp.size()) {
                awaitCanWrite();
                _profile.append(_temp);
                ESP_LOGD("Profile", "add slice, canStart %d", _profile.canStart());
                _idx = 0;
            }
        }
        void finish() {
            while (_idx < _temp.size()) {
                _temp[_idx] = _last;
                _idx++;
            }
            awaitCanWrite();
            _profile.append(_temp, true);
            ESP_LOGD("Profile", "add final slice");
            _idx = 0;
        }

        void awaitCanWrite() {
            while (!_profile.canWrite()) {
                fos::Task::delayMs(200);
                ESP_LOGD("Profile", "waiting... diff=%lu read=%lu canStart %d",
                         _profile.getDiff(), _profile.getRead(), _profile.canStart());
            }
        }

      private:
        StepperConfig &_config;
        ProfileBuffer &_profile;
        const float stp = _config.stepRad * 4;
        const float res = stp / _config.resolution;
        ProfileBuffer::Segment _temp{};
        uint32_t _idx{};
        uint8_t _last;
    };

    StepperConfig &_config;
    Transformer _transformer;
    float _dT;
    float _aMax;
    float _wMax;

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

    void createSmoothStep(float delta) {
        // already solved in simple step
        smoothSubStep(delta, _aMax, _wMax);
    }

    void createBuildUpStep(float delta) {
        // 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; });
    }

    void createRapidStartSwingInEnd(float delta) {
        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 + (_pBack - delta) / 2;
        accelUntil(-_aMax, 0, [pCen](Point pnt) { return pnt.p < pCen; });
        accelUntil(_aMax, _wBack, [](Point pnt) { return pnt.w > 0; });
    }

    void createJumpInTarget(float delta) {
        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);
        }
    }

    void init() {
        _pBack = 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);
        _pBack = pnt.p;
        _wBack = pnt.w;
        _transformer.add(pnt.p);
    }

    void finish() {
    }

    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 = _pBack;
        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 = _pBack;
        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 = _pBack;

        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 - _pBack) / _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 + (_pBack - 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; });
        }
    }
};