/**
 * @file pwmExpanderPca9685.cpp
 * @author Gustice
 * @brief PWM-Expander PCA9685
 *
 * @copyright Copyright (c) 2024
 */

#include "pwmExpanderPca9685.hpp"
#include "esp_log.h"
#include <cmath>
#include <unistd.h> // for usleepCommand

enum Pca9685Codes {
    PCA9685_MODE1 = 0x00,
    PCA9685_MODE2 = 0x01,
    Led0_ON_L = 0x06,
    Led0_ON_H = 0x07,
    Led0_OFF_L = 0x08,
    Led0_OFF_H = 0x09,
    // Led1_...Led15
    AllLedON_L = 0xFA,
    AllLedON_H = 0xFB,
    AllLedOFF_L = 0xFC,
    AllLedOFF_H = 0xFD,
    PCA9685_PRESCALE = 0xFE,
};

enum Pca9685_Mode1Bits {
    Mode1_Sleep = 0x10,
    Mode1_AutoIncrement = 0x20,
    Mode1_Restart = 0x80,
};

enum Pca9685_Mode2Bits {
    Mode2_Invert = 0x10,
    Mode2_OutDriver = 0x04,
};

constexpr int LedPortRegisterWidth = 2 * 2;
constexpr int BitResolution = 12;
constexpr int PwmMaxValue = 4096 - 1;

PmwExpanderPca9685::PmwExpanderPca9685(I2cMaster &port, bool inverted, uint8_t address)
    : _port(port, address, I2cMaster::Slave::_400k), _address(address & 0x7F) {
    _data.resize(PwmChannels * LedPortRegisterWidth + 1);
    reset();
    setPwmFreq(400, inverted);
}

void PmwExpanderPca9685::reset(void) {
    uint8_t mode = Mode1_Restart | Mode1_AutoIncrement;
    write(PCA9685_MODE1, mode);
    usleep(10000); // 10 ms
}

void PmwExpanderPca9685::setPwmFreq(float frequency, bool inverted) {
    frequency *= 0.9f; // Correct for overshoot in the frequency setting (see issue #11).
    float prescaleLevel = 25000000;
    prescaleLevel /= PwmMaxValue;
    prescaleLevel /= frequency;
    prescaleLevel -= 1;

    uint8_t prescale = (uint8_t)std::floor(prescaleLevel + 0.5f);
    uint8_t oldMode = read(PCA9685_MODE1).value_or(0);
    uint8_t newMode = (oldMode & 0x7F) | Mode1_Sleep; // sleep + Autoincrement
    write(PCA9685_MODE1, newMode);                    // go to sleep
    write(PCA9685_PRESCALE, prescale);                // set the prescaler
    write(PCA9685_MODE1, oldMode);
    if (inverted) {
        write(PCA9685_MODE2, Mode2_OutDriver | Mode2_Invert);
    } else {
        write(PCA9685_MODE2, Mode2_OutDriver);
    }

    usleep(5000); // 5 ms
    //  This sets the MODE1 register to turn on auto increment.
    write(PCA9685_MODE1, oldMode | Mode1_Restart | Mode1_AutoIncrement);
}

void PmwExpanderPca9685::sendImage(GrayValues &grayValues) {
    // Try to distribute load peaks over full duty cycle
    // OUT1 ..###______###____###______###____###__..
    // OUT2 ..__###______###____###______###____###..
    // OUT3 ..____###______###______###____###_____..
    // ...
    int idx = 0;
    int step = UINT16_MAX / 4 / PwmChannels; // max 90 deg phase shift
    int curStep = step;
    _data[idx++] = Led0_ON_L;
    for (size_t i = 0; i < _count; i++) {
        curStep = curStep + step;
        uint16_t dur = (grayValues[i] + curStep) >> 4;
        uint on = curStep >> 4;
        // On-Cnt
        _data[idx++] = (uint8_t)on;
        _data[idx++] = (uint8_t)(on >> 8);
        // Off-Cnt
        _data[idx++] = (uint8_t)dur;
        _data[idx++] = (uint8_t)(dur >> 8);
    }
    _port.writeTo(_data);
}

bool PmwExpanderPca9685::write(uint8_t reg, uint8_t word) {
    std::vector<uint8_t> data{reg, word};
    return _port.writeTo(data);
}

std::optional<uint8_t> PmwExpanderPca9685::read(uint8_t reg) {
    auto value = _port.readFrom({reg}, 1);
    if (value.size() == 0) {
        return std::nullopt;
    }

    return {value.front()};
}