#include "tm1640.hpp"
#include <vector>
#include "driver/gpio.h"
#include "esp_rom_sys.h"

// Constants
static const uint8_t CMD_DATA_AUTO = 0x40;
static const uint8_t CMD_DATA_FIXED = 0x44;
static const uint8_t CMD_DISPLAY = 0x80;
static const uint8_t CMD_ADDRESS = 0xC0;

// TODO: we could use the RMT interface to do this more efficiently.

TM1640::TM1640(gpio_num_t clk_pin, gpio_num_t dio_pin) : clk_pin(clk_pin), dio_pin(dio_pin), intensity(0x0F) {
    // Configure pins as output
    gpio_config_t io_conf = {};
    io_conf.intr_type = GPIO_INTR_DISABLE;
    io_conf.mode = GPIO_MODE_OUTPUT;
    io_conf.pin_bit_mask = (1ULL << clk_pin) | (1ULL << dio_pin);
    io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
    io_conf.pull_up_en = GPIO_PULLUP_DISABLE;
    gpio_config(&io_conf);

    // Set pins high
    gpio_set_level(clk_pin, 1);
    gpio_set_level(dio_pin, 1);
}

void TM1640::bit_delay() {
    esp_rom_delay_us(1);
}

void TM1640::start() {
    gpio_set_level(dio_pin, 0);
    gpio_set_level(clk_pin, 0);
    bit_delay();
}

void TM1640::stop() {
    gpio_set_level(dio_pin, 0);
    bit_delay();
    gpio_set_level(clk_pin, 1);
    gpio_set_level(dio_pin, 1);
    bit_delay();
}

void TM1640::shift_out(uint8_t data) {
    for (int i = 0; i < 8; i++) {
        gpio_set_level(dio_pin, data & 1);
        data >>= 1;
        bit_delay();
        gpio_set_level(clk_pin, 1);
        bit_delay();
        gpio_set_level(clk_pin, 0);
        bit_delay();
    }
}

void TM1640::send(uint8_t* data, size_t len) {
    start();
    for (size_t i = 0; i < len; i++) {
        shift_out(data[i]);
    }
    stop();
}

void TM1640::init() {
    clear_display();
}

void TM1640::clear_display() {
    uint8_t data1[] = {CMD_DATA_AUTO};
    send(data1, 1);
    uint8_t data2[] = {CMD_ADDRESS, 0, 0, 0, 0, 0, 0, 0, 0};
    send(data2, 9);
    uint8_t data3 = CMD_DISPLAY | intensity;
    send(&data3, 1);
}

// TODO: can these become all one send??
// other functions too
void TM1640::set_digit(uint8_t digit, uint8_t segments) {
    uint8_t data1[] = {CMD_DATA_FIXED};
    send(data1, 1);
    uint8_t cmd = CMD_ADDRESS | digit;
    uint8_t data2[] = {cmd, segments};
    send(data2, 2);
    uint8_t data3 = CMD_DISPLAY | intensity;
    send(&data3, 1);
}

void TM1640::set_digits(uint8_t starting_pos, uint8_t* segments, size_t len) {
    uint8_t data1[] = {CMD_DATA_AUTO};
    send(data1, 1);
    std::vector<uint8_t> data;
    data.push_back(CMD_ADDRESS | starting_pos);
    for (size_t i = 0; i < len; i++) {
        data.push_back(segments[i]);
    }
    send(data.data(), data.size());
    uint8_t data3 = CMD_DISPLAY | intensity;
    send(&data3, 1);
}

void TM1640::set_intensity(uint8_t intensity) {
    uint8_t new_intensity = intensity & 0x07; // 0-7
    this->intensity = (this->intensity & 0xF8) | new_intensity;
    uint8_t cmd = CMD_DISPLAY | this->intensity;
    send(&cmd, 1);
}

void TM1640::set_display(bool on) {
    uint8_t display_bit = on ? 0x08 : 0x00;
    this->intensity = (this->intensity & 0xF7) | display_bit;
    uint8_t cmd = CMD_DISPLAY | this->intensity;
    send(&cmd, 1);
}