#include "ina260.h"

i2c_device_config_t INA260_DEV_CFG = {
    .dev_addr_length = I2C_ADDR_BIT_LEN_7,
    .device_address = INA260_ADDRESS,
    .scl_speed_hz = 100000,
};

i2c_master_dev_handle_t INA260_DEV_HANDLE;


void ina260_reset() {
    i2c_write_register_16(INA260_DEV_HANDLE, 0x00, 0x0FFF); // set ina max averaging and max time
}

esp_err_t ina260_init()
{
    esp_err_t ret = i2c_master_bus_add_device(i2c0_bus_hdl, &INA260_DEV_CFG, &INA260_DEV_HANDLE);
    if (ret != ESP_OK) {return ret;}
    return i2c_write_register_16(INA260_DEV_HANDLE, INA260_CONFIG_REGISTER, 
        CONFIG_AVG_1024 | CONFIG_VBUSCT_8_244MS | CONFIG_ISHCT_8_244MS | CONFIG_MODE_CURRENT_VOLTAGE_CONTINOUS); // set ina max averaging and max time
}


esp_err_t i2c_master_read_register_transmit_receive(i2c_master_dev_handle_t device_handle, uint8_t reg_addr, uint8_t *data, size_t data_len) {
    esp_err_t ret;

    // The register address is what we want to send first (the "transmit" part)
    uint8_t write_buffer[1] = {reg_addr};
    size_t write_size = 1;

    // The data we read will be stored in the 'data' buffer (the "receive" part)
    uint8_t read_buffer[data_len];
    size_t read_size = data_len;

    // Perform the combined write (register address) and read (register value)
    ret = i2c_master_transmit_receive(INA260_DEV_HANDLE,
                                       write_buffer, write_size,
                                       read_buffer, read_size,
                                       I2C_TIMEOUT_MS_VALUE);

    if (ret == ESP_OK) {
        // Copy the data from the temporary read buffer to the output buffer
        memcpy(data, read_buffer, read_size);
    }

    return ret;
}

void ina260_readParams(uint16_t *volt, uint16_t *cur, uint16_t *pow)
{
    *volt = 0;
    *cur = 0;
    *pow = 0;
    for (uint8_t reg_addr = 1; reg_addr <= 3; reg_addr++)
    {
        uint8_t reg_value[2] = {0}; // Buffer for storing register data

        ESP_ERROR_CHECK(i2c_master_read_register_transmit_receive(INA260_DEV_HANDLE, reg_addr, reg_value, sizeof(reg_value)));
        // Perform the register read
        switch (reg_addr)
        {
        case 1:
            *cur = (((uint16_t)(reg_value[0] & 0xFF)) << 8) | (reg_value[1] & 0xFF);
            //*cur = *((uint16_t *)reg_value);
            break;
        case 2:
            *volt = (((uint16_t)(reg_value[0] & 0xFF)) << 8) | (reg_value[1] & 0xFF);
            break;
        case 3:
            *pow = (((uint16_t)(reg_value[0] & 0xFF)) << 8) | (reg_value[1] & 0xFF);
            break;
        default:
            break;
        }
    }
}

void ina260_printParams(uint16_t volt, uint16_t cur, uint16_t pow)
{
    float miliVolts = volt * 1.25;
    float miliAmps = cur * 1.25;
    float power = pow * 10;
    cur *= 125;
    ESP_LOGI(TAG_INA, "Current: %.3f mA (raw %d)", miliAmps, volt);
    cur *= 125;
    ESP_LOGI(TAG_INA, "Voltage: %.3f mV (raw %d)", miliVolts, cur);
    ESP_LOGI(TAG_INA, "Power: %.3f mW (raw %d)", power, pow);
}