#include <sw7203.h>
#include "driver/i2c_master.h"
#include "esp_log.h"

uint8_t sw7203_I2C_address_find(i2c_master_bus_handle_t I2C_bus_handle)
{
    esp_err_t errcode;
    errcode = i2c_master_probe(I2C_bus_handle, sw7203_I2c_0x3C, -1);
    if (errcode == ESP_OK)
    {
        return 0x3C;
    }
    errcode = i2c_master_probe(I2C_bus_handle, sw7203_I2c_0x1C, -1);
    if (errcode == ESP_OK)
    {
        return 0x1C;
    }
    errcode = i2c_master_probe(I2C_bus_handle, sw7203_I2c_0x18, -1);
    if (errcode == ESP_OK)
    {
        return 0x18;
    }
    errcode = i2c_master_probe(I2C_bus_handle, sw7203_I2c_0x38, -1);
    if (errcode == ESP_OK)
    {
        return 0x38;
    }
    return -1;
}

void sw7203_register_change_bitmask(uint8_t reg_addr, uint8_t data, uint8_t bitmask)
{
    uint8_t original_reg;
    ESP_ERROR_CHECK(i2c_master_transmit_receive(SW7203_I2C_handle, &reg_addr, 1, &original_reg, 1, -1));
    uint8_t diff = (original_reg ^ data) & bitmask;
    if (diff)
    {
        uint16_t write_data = reg_addr << 8 | (diff ^ original_reg);
        ESP_ERROR_CHECK(i2c_master_transmit(SW7203_I2C_handle, &write_data, 2, -1));
    }
}

/*    {0x18, 0b00010011},
    {0x1A, 0b00000011},
    {0x20, 0b11100111},
    {0x21, 0b11111111},
    {0x22, 0b11110000},
    {0x30, 0b11001111},
    {0x31, 0b00001111},
    {0x32, 0b11011111},
    {0x33, 0b11111111},
    {0x34, 0b11111111},
    {0x35, 0b00000111},
    {0x36, 0b01111111},
    {0x37, 0b00001111},
    {0x38, 0b11111111},
    {0x39, 0b01111111},
    {0x3A, 0b01111111},
    {0x40, 0b01000011},
    {0x41, 0b00011111},
    {0x42, 0b01111111},
    {0x42, 0b11111111},
    {0x44, 0b00000011},*/

esp_err_t sw7203init(sw7203_config_t *sw7203_config, i2c_master_bus_handle_t I2C_bus_handle, uint8_t I2C_address)
{
    if (I2C_address == -1)
    {
        return ESP_ERR_NOT_ALLOWED;
    }
    esp_err_t errcode;
    i2c_device_config_t sw7203_i2c_dev_config = {
        .dev_addr_length = I2C_ADDR_BIT_LEN_7,
        .device_address = I2C_address,
        .scl_speed_hz = 100000};
    ESP_ERROR_CHECK(errcode = i2c_master_bus_add_device(I2C_bus_handle, &sw7203_i2c_dev_config, &SW7203_I2C_handle));
    sw7203_register_change_bitmask(SW7203RegAddress_bitmask[0][0], sw7203_config->connectDetect.hex, SW7203RegAddress_bitmask[0][1]);
    sw7203_register_change_bitmask(SW7203RegAddress_bitmask[1][0], sw7203_config->i2cAddress, SW7203RegAddress_bitmask[1][1]);
    sw7203_register_change_bitmask(SW7203RegAddress_bitmask[2][0], sw7203_config->funcConfig1.hex, SW7203RegAddress_bitmask[2][1]);
    sw7203_register_change_bitmask(SW7203RegAddress_bitmask[3][0], sw7203_config->funcConfig2.hex, SW7203RegAddress_bitmask[3][1]);
    sw7203_register_change_bitmask(SW7203RegAddress_bitmask[4][0], sw7203_config->funcConfig3.hex, SW7203RegAddress_bitmask[4][1]);
    sw7203_register_change_bitmask(SW7203RegAddress_bitmask[5][0], sw7203_config->funcConfig4.hex, SW7203RegAddress_bitmask[5][1]);
    return ESP_OK;
}