/**
 * @file stepperDrv8833.cpp
 * @author Gustice
 * @brief Software-driver for Stepper driver DRV8833
 *
 * @copyright Copyright (c) 2024
 */

#include "stepperDrv8833.hpp"

#include "esp_log.h"
#include <climits>
#include <cmath>

void populateLookUps(int resolution,
                     std::vector<uint8_t> &a1, std::vector<uint8_t> &a2,
                     std::vector<uint8_t> &b1, std::vector<uint8_t> &b2) {
    float step = 2.0F * std::numbers::pi / resolution;
    const uint16_t FullScale = 1 << LEDC_TIMER_7_BIT;
    
    for (size_t i = 0; i < resolution; i++) {
        
        auto cos = std::cos(step * i);
        if (cos > 0)
        {
            // 1 :   0 = Forward
            // 1 : PWM = Forward PWM, slow decay
            a1.push_back(FullScale);
            a2.push_back(FullScale - (cos * FullScale));
        } else {
            //   0 : 1 = Reverse
            // PWM : 1 = Reverse PWM, slow decay
            a1.push_back(FullScale - (-cos * FullScale));
            a2.push_back(FullScale);
        }

        auto sin = std::sin(step * i);
        if (sin > 0)
        {
            // 1 :   0 = Forward
            // 1 : PWM = Forward PWM, slow decay
            b1.push_back(FullScale);
            b2.push_back(FullScale - (sin * FullScale));
        } else {
            //   0 : 1 = Reverse
            // PWM : 1 = Reverse PWM, slow decay
            b1.push_back(FullScale - ((-sin * FullScale)));
            b2.push_back(FullScale);
        }
    };
}

StepperDriverDrv8833::StepperDriverDrv8833(Pins pins, int resolution)
    : StepperDriver(resolution), 
      _pwmA1(pins.timer, pins.channels[0], pins.pins[0], LEDC_TIMER_7_BIT, 20000),
      _pwmA2(pins.timer, pins.channels[1], pins.pins[1], LEDC_TIMER_7_BIT, 20000),
      _pwmB1(pins.timer, pins.channels[2], pins.pins[2], LEDC_TIMER_7_BIT, 20000),
      _pwmB2(pins.timer, pins.channels[3], pins.pins[3], LEDC_TIMER_7_BIT, 20000)
      {
    if ((Resolution % 4) != 0) {
        ESP_LOGE("Stepper", "Resolution must be multiple of 4");
    }

    populateLookUps(resolution, _outA1, _outA2, _outB1, _outB2);

    ESP_LOGD("Stepper", "Output Data:");
    ESP_LOG_BUFFER_HEXDUMP("Stepper", _outA1.data(), _outA1.size(), ESP_LOG_DEBUG);
    ESP_LOG_BUFFER_HEXDUMP("Stepper", _outA2.data(), _outA2.size(), ESP_LOG_DEBUG);
    ESP_LOG_BUFFER_HEXDUMP("Stepper", _outB1.data(), _outB1.size(), ESP_LOG_DEBUG);
    ESP_LOG_BUFFER_HEXDUMP("Stepper", _outB2.data(), _outB2.size(), ESP_LOG_DEBUG);

    step(0);
}

StepperDriverDrv8833::StepCplx StepperDriverDrv8833::fullStep(int8_t cnt) {
    if (cnt > 0) {
        cnt = 1;
    } else {
        cnt = -1;
    }

    int pos = (_position / Quad + cnt) % 4;
    pos = pos * Quad;
    return setPos(pos);
}

StepperDriverDrv8833::StepCplx StepperDriverDrv8833::step(int8_t cnt) {
    if (cnt == 0) {
        return {0, (uint8_t)_position};
    }
    if (cnt > Resolution) {
        // ESP_LOGE("Stepper", "microstep exceeds four steps");
    }
    if (cnt > Quad) {
        // ESP_LOGW("Stepper", "microstep exceeds quadrature step");
    }

    int8_t increment{};
    auto pos = _position + cnt;
    if (pos < 0) {
        increment--;
        pos += Resolution;
    } else if (pos >= Resolution) {
        increment++;
        pos -= Resolution;
    } else {
        // in that case ... nothing
    }
    setOutput(pos);
    return {increment, (uint8_t)_position};
}

StepperDriverDrv8833::StepCplx StepperDriverDrv8833::setPos(uint8_t pos) {
    pos = (pos) % Resolution;

    int8_t increment{};
    if (pos - _position < 0) {
        increment--;
    } else if (pos - _position > Resolution) {
        increment++;
    } else {
        // in that case ... nothing
    }

    setOutput(pos);
    return {increment, (uint8_t)_position};
}

void StepperDriverDrv8833::setOutput(uint8_t pos) {
    if (pos >= Resolution) {
        // ESP_LOGE("Stepper", "d exceeds micro-stepping size");
    }
    _position = pos;
    _pwmA1.writeRaw(_outA1[pos]);
    _pwmA2.writeRaw(_outA2[pos]);
    _pwmB1.writeRaw(_outB1[pos]);
    _pwmB2.writeRaw(_outB2[pos]);
}