#include "screen.h"

esp_err_t write_reg_bit_16(i2c_master_dev_handle_t mcp, uint8_t reg, bool val, uint8_t bit)
{
    uint8_t buf[2];

    // Read the current value from the register
    buf[0] = reg; // register address
    ESP_ERROR_CHECK(i2c_master_transmit(mcp, &buf[0], sizeof(uint8_t), -1));
    ESP_ERROR_CHECK(i2c_master_receive(mcp, &buf[1], sizeof(uint8_t), -1));
    // Update the value in the buffer

    buf[1] = (buf[1] & ~BV(bit)) | (val ? BV(bit) : 0);
    
    // Write the updated value back to the register
    buf[0] = reg; // Send register address
    ESP_ERROR_CHECK(i2c_master_transmit(mcp, &buf[0], sizeof(uint16_t), -1));

    return ESP_OK;
}

esp_err_t mcp_set_mode(i2c_master_dev_handle_t mcp, uint8_t reg, uint8_t pin, bool mode)
{
    return write_reg_bit_16(mcp, reg, mode, pin);
}

esp_err_t mcp_set_level(i2c_master_dev_handle_t mcp, uint8_t reg, uint8_t pin, bool val)
{
    return write_reg_bit_16(mcp, reg, val, pin);
}

void configure_screen(struct Screen* screen)
{
    i2c_master_bus_config_t i2c_bus_config = {
        .clk_source = I2C_CLK_SRC_DEFAULT,
        .i2c_port = 0,
        .sda_io_num = SDA_PIN,
        .scl_io_num = SCL_PIN,
        .glitch_ignore_cnt = 7,
        .flags.enable_internal_pullup = true,
    };
    ESP_ERROR_CHECK(i2c_new_master_bus(&i2c_bus_config, &(screen->i2c_bus)));
    
    i2c_device_config_t mcp_cfg = {
        .dev_addr_length = I2C_ADDR_BIT_LEN_7,
        .device_address = MCP_I2C_ADDR,
        .scl_speed_hz = 1000000,
    };
    ESP_ERROR_CHECK(i2c_master_bus_add_device(screen->i2c_bus, &mcp_cfg, &(screen->mcp)));

    printf("I2C MCP Configured!\n");
    
    screen->nixies[0].digit = 9;
    screen->nixies[0].reg = REG_GPIOA;
    screen->nixies[0].C = N0_P0;
    screen->nixies[0].B = N0_P1;
    screen->nixies[0].D = N0_P2;
    screen->nixies[0].A = N0_P3;
    screen->nixies[1].digit = 9;
    screen->nixies[1].reg = REG_GPIOA;
    screen->nixies[1].C = N1_P0;
    screen->nixies[1].B = N1_P1;
    screen->nixies[1].D = N1_P2;
    screen->nixies[1].A = N1_P3;
    screen->nixies[2].digit = 9;
    screen->nixies[2].reg = REG_GPIOB;
    screen->nixies[2].C = N2_P0;
    screen->nixies[2].B = N2_P1;
    screen->nixies[2].D = N2_P2;
    screen->nixies[2].A = N2_P3;
    screen->nixies[3].digit = 9;
    screen->nixies[3].reg = REG_GPIOB;
    screen->nixies[3].C = N3_P0;
    screen->nixies[3].B = N3_P1;
    screen->nixies[3].D = N3_P2;
    screen->nixies[3].A = N3_P3;

    for (uint8_t i = 0; i <= 3; ++i)
    {
        uint8_t reg = REG_IODIRA;
        if (screen->nixies[i].reg != REG_GPIOA) reg = REG_IODIRB;
        mcp_set_mode(screen->mcp, reg, screen->nixies[i].D, MCP_GPIO_OUTPUT);
        mcp_set_mode(screen->mcp, reg, screen->nixies[i].C, MCP_GPIO_OUTPUT);
        mcp_set_mode(screen->mcp, reg, screen->nixies[i].B, MCP_GPIO_OUTPUT);
        mcp_set_mode(screen->mcp, reg, screen->nixies[i].A, MCP_GPIO_OUTPUT);
    }


    printf("MCP GPIO Configured!\n");
    return;
}

void set_digit(i2c_master_dev_handle_t mcp, struct Nixie* n)
{
    switch (n->digit) {
        case 0:
            mcp_set_level(mcp, n->reg, n->D, 0);
            mcp_set_level(mcp, n->reg, n->C, 0);
            mcp_set_level(mcp, n->reg, n->B, 0);
            mcp_set_level(mcp, n->reg, n->A, 0);
            break;
        case 1:
            mcp_set_level(mcp, n->reg, n->D, 0);
            mcp_set_level(mcp, n->reg, n->C, 0);
            mcp_set_level(mcp, n->reg, n->B, 0);
            mcp_set_level(mcp, n->reg, n->A, 1);
            break;
        case 2:
            mcp_set_level(mcp, n->reg, n->D, 0);
            mcp_set_level(mcp, n->reg, n->C, 0);
            mcp_set_level(mcp, n->reg, n->B, 1);
            mcp_set_level(mcp, n->reg, n->A, 0);
            break;
        case 3:
            mcp_set_level(mcp, n->reg, n->D, 0);
            mcp_set_level(mcp, n->reg, n->C, 0);
            mcp_set_level(mcp, n->reg, n->B, 1);
            mcp_set_level(mcp, n->reg, n->A, 1);
            break;
        case 4:
            mcp_set_level(mcp, n->reg, n->D, 0);
            mcp_set_level(mcp, n->reg, n->C, 1);
            mcp_set_level(mcp, n->reg, n->B, 0);
            mcp_set_level(mcp, n->reg, n->A, 0);
            break;       
        case 5:          
            mcp_set_level(mcp, n->reg, n->D, 0);
            mcp_set_level(mcp, n->reg, n->C, 1);
            mcp_set_level(mcp, n->reg, n->B, 0);
            mcp_set_level(mcp, n->reg, n->A, 1);
            break;      
        case 6:         
            mcp_set_level(mcp, n->reg, n->D, 0);
            mcp_set_level(mcp, n->reg, n->C, 1);
            mcp_set_level(mcp, n->reg, n->B, 1);
            mcp_set_level(mcp, n->reg, n->A, 0);
            break;
        case 7:
            mcp_set_level(mcp, n->reg, n->D, 0);
            mcp_set_level(mcp, n->reg, n->C, 1);
            mcp_set_level(mcp, n->reg, n->B, 1);
            mcp_set_level(mcp, n->reg, n->A, 1);
            break;
        case 8:
            mcp_set_level(mcp, n->reg, n->D, 1);
            mcp_set_level(mcp, n->reg, n->C, 0);
            mcp_set_level(mcp, n->reg, n->B, 0);
            mcp_set_level(mcp, n->reg, n->A, 0);
            break;
        case 9:
            mcp_set_level(mcp, n->reg, n->D, 1);
            mcp_set_level(mcp, n->reg, n->C, 0);
            mcp_set_level(mcp, n->reg, n->B, 0);
            mcp_set_level(mcp, n->reg, n->A, 1);
            break;
        default:
            mcp_set_level(mcp, n->reg, n->D, 1);
            mcp_set_level(mcp, n->reg, n->C, 1);
            mcp_set_level(mcp, n->reg, n->B, 1);
            mcp_set_level(mcp, n->reg, n->A, 1);
            break;
        return;
    }
}

void test_screen(struct Screen* screen)
{
    while(1){
        for (uint8_t i = 0; i <= 9; ++i) {
            for (uint8_t j = 0; j < 4; ++j) {
                screen->nixies[j].digit = (i + j) % 10; // Update the digit cyclically
                set_digit(screen->mcp, &(screen->nixies[j])); // Call set_digit function
            }
            sleep(1);
        }
    }
}

void display(struct Screen* screen, char digits[4])
{
    for (uint8_t i = 0; i < 4; ++i) {
        uint8_t digit = digits[i] - '0';
        //printf("%i", digit);
        if (screen->nixies[i].digit != digit){
            screen->nixies[i].digit = digit;
            set_digit(screen->mcp, &(screen->nixies[i])); // Call set_digit function
        }
    }
}