/*
 * Developed by TComLab
 * Version: v0.1
 * Date: 2025-12-15
 */

#include "ina226_monitor.h"

#include <math.h>

#include "driver/i2c.h"
#include "esp_check.h"
#include "esp_log.h"
#include "sdkconfig.h"

#ifndef CONFIG_WATCH_INA226_ENABLED
#define CONFIG_WATCH_INA226_ENABLED 0
#endif
#ifndef CONFIG_WATCH_INA226_I2C_PORT
#define CONFIG_WATCH_INA226_I2C_PORT 0
#endif
#ifndef CONFIG_WATCH_INA226_I2C_SDA
#define CONFIG_WATCH_INA226_I2C_SDA 6
#endif
#ifndef CONFIG_WATCH_INA226_I2C_SCL
#define CONFIG_WATCH_INA226_I2C_SCL 7
#endif
#ifndef CONFIG_WATCH_INA226_I2C_FREQ_HZ
#define CONFIG_WATCH_INA226_I2C_FREQ_HZ 400000
#endif
#ifndef CONFIG_WATCH_INA226_CURRENT_LSB_uA
#define CONFIG_WATCH_INA226_CURRENT_LSB_uA 100
#endif
#ifndef CONFIG_WATCH_INA226_SHUNT_MILLIOHM
#define CONFIG_WATCH_INA226_SHUNT_MILLIOHM 10
#endif
#ifndef CONFIG_WATCH_INA226_ADDR
#define CONFIG_WATCH_INA226_ADDR 0x40
#endif

#define INA226_REG_CONFIG 0x00
#define INA226_REG_BUS 0x02
#define INA226_REG_CURRENT 0x04
#define INA226_REG_CALIBRATION 0x05

#define INA226_CONFIG_AVG_16 (0x04 << 9)
#define INA226_CONFIG_VBUS_1100US (0x04 << 6)
#define INA226_CONFIG_VSH_1100US (0x04 << 3)
#define INA226_CONFIG_MODE_SHUNT_BUS_CONT 0x07

#if CONFIG_WATCH_INA226_ENABLED
static const char *TAG = "ina226";
static bool s_initialized;
static float s_current_lsb_ma;
static uint8_t s_address = CONFIG_WATCH_INA226_ADDR;
static ina226_reading_t s_last_reading;
#endif

// Ініціалізує контролер INA226: налаштовує I2C, записує конфігурацію та
// калібрувальні значення, а також зберігає початковий стан вимірювань.
esp_err_t ina226_monitor_init(void)
{
#if !CONFIG_WATCH_INA226_ENABLED
    return ESP_ERR_NOT_SUPPORTED;
#else
    if (s_initialized) {
        return ESP_OK;
    }

    i2c_config_t config = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = CONFIG_WATCH_INA226_I2C_SDA,
        .scl_io_num = CONFIG_WATCH_INA226_I2C_SCL,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = CONFIG_WATCH_INA226_I2C_FREQ_HZ,
    };
    ESP_RETURN_ON_ERROR(i2c_param_config(CONFIG_WATCH_INA226_I2C_PORT, &config), TAG, "i2c config failed");
    ESP_RETURN_ON_ERROR(i2c_driver_install(CONFIG_WATCH_INA226_I2C_PORT, config.mode, 0, 0, 0),
                        TAG, "i2c driver install failed");

    double current_lsb_a = ((double)CONFIG_WATCH_INA226_CURRENT_LSB_uA) / 1000000.0;
    double shunt_ohms = ((double)CONFIG_WATCH_INA226_SHUNT_MILLIOHM) / 1000.0;
    double calibration = 0.00512 / (current_lsb_a * shunt_ohms);
    if (calibration > 0xFFFF) calibration = 0xFFFF;
    uint16_t calibration_value = (uint16_t)calibration;
    s_current_lsb_ma = (float)current_lsb_a * 1000.0f;

    const uint16_t config_value = INA226_CONFIG_AVG_16 |
                                  INA226_CONFIG_VBUS_1100US |
                                  INA226_CONFIG_VSH_1100US |
                                  INA226_CONFIG_MODE_SHUNT_BUS_CONT;

    uint8_t payload_cfg[3] = { INA226_REG_CONFIG, (uint8_t)(config_value >> 8), (uint8_t)(config_value & 0xFF) };
    ESP_RETURN_ON_ERROR(i2c_master_write_to_device(CONFIG_WATCH_INA226_I2C_PORT,
                                                  s_address, payload_cfg, sizeof(payload_cfg),
                                                  pdMS_TO_TICKS(100)),
                        TAG, "config write failed");

    uint8_t payload_cal[3] = { INA226_REG_CALIBRATION, (uint8_t)(calibration_value >> 8),
                               (uint8_t)(calibration_value & 0xFF) };
    ESP_RETURN_ON_ERROR(i2c_master_write_to_device(CONFIG_WATCH_INA226_I2C_PORT,
                                                  s_address, payload_cal, sizeof(payload_cal),
                                                  pdMS_TO_TICKS(100)),
                        TAG, "calibration write failed");

    s_last_reading = (ina226_reading_t){0};
    s_initialized = true;
    ESP_LOGI(TAG, "INA226 ініціалізовано (адреса 0x%02X)", s_address);
    return ESP_OK;
#endif
}

// Дозволяє швидко перевірити, чи був модуль INA226 успішно ініціалізований.
bool ina226_monitor_ready(void)
{
#if CONFIG_WATCH_INA226_ENABLED
    return s_initialized;
#else
    return false;
#endif
}

// INA226 вимірює єдиний канал живлення, тому повертаємо 1.
size_t ina226_monitor_channel_count(void)
{
    return 1;
}

#if CONFIG_WATCH_INA226_ENABLED
// Зчитує 16-бітний регістр INA226, використовуючи транзакцію write-then-read.
static esp_err_t ina226_read_register(uint8_t reg, uint16_t *out_value)
{
    uint8_t value[2];
    esp_err_t err = i2c_master_write_read_device(CONFIG_WATCH_INA226_I2C_PORT,
                                                 s_address, &reg, 1, value, sizeof(value),
                                                 pdMS_TO_TICKS(100));
    if (err != ESP_OK) {
        return err;
    }
    *out_value = ((uint16_t)value[0] << 8) | value[1];
    return ESP_OK;
}
#endif

// Виконує одне вимірювання напруги/струму через INA226 та кешує результат.
esp_err_t ina226_monitor_sample(ina226_reading_t *out_reading)
{
#if !CONFIG_WATCH_INA226_ENABLED
    return ESP_ERR_NOT_SUPPORTED;
#else
    if (!s_initialized) {
        return ESP_ERR_INVALID_STATE;
    }
    uint16_t bus_raw = 0;
    uint16_t current_raw = 0;

    ESP_RETURN_ON_ERROR(ina226_read_register(INA226_REG_BUS, &bus_raw), TAG, "bus read failed");
    ESP_RETURN_ON_ERROR(ina226_read_register(INA226_REG_CURRENT, &current_raw), TAG, "current read failed");

    float voltage_v = (float)bus_raw * 1.25f / 1000.0f;
    float current_ma = (int16_t)current_raw * s_current_lsb_ma;
    float power_mw = voltage_v * current_ma;

    s_last_reading = (ina226_reading_t){
        .voltage_v = voltage_v,
        .current_ma = current_ma,
        .power_mw = power_mw,
    };
    if (out_reading) {
        *out_reading = s_last_reading;
    }
    return ESP_OK;
#endif
}

// Повертає останній виміряний набір даних без нового звернення до шини I2C.
const ina226_reading_t *ina226_monitor_get_last(void)
{
#if CONFIG_WATCH_INA226_ENABLED
    return &s_last_reading;
#else
    return NULL;
#endif
}