#include <stdio.h>
#include <stdint.h>
#include <math.h>

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

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

#define TAG "INA226_TEST"

#define I2C_PORT            I2C_NUM_0
#define I2C_SDA_PIN         21
#define I2C_SCL_PIN         22
#define I2C_FREQ_HZ         100000

#define INA226_ADDR         0x40

#define INA226_REG_CONFIG       0x00
#define INA226_REG_SHUNT_V      0x01
#define INA226_REG_BUS_V        0x02
#define INA226_REG_POWER        0x03
#define INA226_REG_CURRENT      0x04
#define INA226_REG_CALIBRATION  0x05
#define INA226_REG_MASK_ENABLE  0x06
#define INA226_REG_ALERT_LIMIT  0x07
#define INA226_REG_MANUF_ID     0xFE
#define INA226_REG_DIE_ID       0xFF

#define INA226_SHUNT_LSB_UV     2.5f
#define INA226_BUS_LSB_MV       1.25f

/*
  Untuk simulasi:
  Rshunt = 0.1 ohm
  Max current sekitar 3.2 A
  Current_LSB = 100 uA / bit = 0.0001 A
  Calibration = 0.00512 / (Current_LSB * Rshunt)
              = 0.00512 / (0.0001 * 0.1)
              = 512
*/
#define CURRENT_LSB_A       0.0001f
#define POWER_LSB_W         (25.0f * CURRENT_LSB_A)
#define SHUNT_OHMS          0.1f
#define INA226_CAL_VALUE    512

static esp_err_t i2c_master_init(void)
{
    i2c_config_t conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = I2C_SDA_PIN,
        .scl_io_num = I2C_SCL_PIN,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = I2C_FREQ_HZ,
        .clk_flags = 0
    };

    ESP_ERROR_CHECK(i2c_param_config(I2C_PORT, &conf));
    return i2c_driver_install(I2C_PORT, conf.mode, 0, 0, 0);
}

static esp_err_t ina226_write_register(uint8_t reg, uint16_t value)
{
    uint8_t data[3];

    data[0] = reg;
    data[1] = (uint8_t)((value >> 8) & 0xFF);
    data[2] = (uint8_t)(value & 0xFF);

    return i2c_master_write_to_device(
        I2C_PORT,
        INA226_ADDR,
        data,
        sizeof(data),
        pdMS_TO_TICKS(1000)
    );
}

static esp_err_t ina226_read_register(uint8_t reg, uint16_t *value)
{
    uint8_t data[2] = {0};

    esp_err_t err = i2c_master_write_read_device(
        I2C_PORT,
        INA226_ADDR,
        &reg,
        1,
        data,
        2,
        pdMS_TO_TICKS(1000)
    );

    if (err != ESP_OK) {
        return err;
    }

    *value = ((uint16_t)data[0] << 8) | data[1];
    return ESP_OK;
}

static void i2c_scan(void)
{
    ESP_LOGI(TAG, "Mulai scan I2C di SDA=%d, SCL=%d", I2C_SDA_PIN, I2C_SCL_PIN);

    int found = 0;

    for (uint8_t addr = 1; addr < 127; addr++) {
        i2c_cmd_handle_t cmd = i2c_cmd_link_create();

        i2c_master_start(cmd);
        i2c_master_write_byte(cmd, (addr << 1) | I2C_MASTER_WRITE, true);
        i2c_master_stop(cmd);

        esp_err_t err = i2c_master_cmd_begin(I2C_PORT, cmd, pdMS_TO_TICKS(50));
        i2c_cmd_link_delete(cmd);

        if (err == ESP_OK) {
            ESP_LOGI(TAG, "Device ditemukan di address: 0x%02X", addr);
            found++;
        }
    }

    ESP_LOGI(TAG, "Total device ditemukan: %d", found);
}

static esp_err_t ina226_init(void)
{
    /*
      Config default-ish:
      AVG = 1
      VBUSCT = 1.1 ms
      VSHCT = 1.1 ms
      MODE = shunt + bus continuous
      Banyak library pakai 0x4127 atau 0x4527. Untuk simulasi ini aman.
    */
    ESP_RETURN_ON_ERROR(
        ina226_write_register(INA226_REG_CONFIG, 0x4127),
        TAG,
        "Gagal menulis config INA226"
    );

    ESP_RETURN_ON_ERROR(
        ina226_write_register(INA226_REG_CALIBRATION, INA226_CAL_VALUE),
        TAG,
        "Gagal menulis calibration INA226"
    );

    return ESP_OK;
}

static void ina226_print_data(void)
{
    uint16_t raw_bus = 0;
    uint16_t raw_shunt = 0;
    uint16_t raw_current = 0;
    uint16_t raw_power = 0;
    uint16_t raw_manuf = 0;
    uint16_t raw_die = 0;

    esp_err_t err;

    err = ina226_read_register(INA226_REG_MANUF_ID, &raw_manuf);
    if (err == ESP_OK) {
        ESP_LOGI(TAG, "Manufacturer ID: 0x%04X", raw_manuf);
    } else {
        ESP_LOGE(TAG, "Gagal baca Manufacturer ID: %s", esp_err_to_name(err));
    }

    err = ina226_read_register(INA226_REG_DIE_ID, &raw_die);
    if (err == ESP_OK) {
        ESP_LOGI(TAG, "Die ID: 0x%04X", raw_die);
    } else {
        ESP_LOGE(TAG, "Gagal baca Die ID: %s", esp_err_to_name(err));
    }

    err = ina226_read_register(INA226_REG_BUS_V, &raw_bus);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "Gagal baca bus voltage: %s", esp_err_to_name(err));
        return;
    }

    err = ina226_read_register(INA226_REG_SHUNT_V, &raw_shunt);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "Gagal baca shunt voltage: %s", esp_err_to_name(err));
        return;
    }

    err = ina226_read_register(INA226_REG_CURRENT, &raw_current);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "Gagal baca current: %s", esp_err_to_name(err));
        return;
    }

    err = ina226_read_register(INA226_REG_POWER, &raw_power);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "Gagal baca power: %s", esp_err_to_name(err));
        return;
    }

    int16_t shunt_signed = (int16_t)raw_shunt;
    int16_t current_signed = (int16_t)raw_current;

    float bus_voltage_v = raw_bus * INA226_BUS_LSB_MV / 1000.0f;
    float shunt_voltage_mv = shunt_signed * INA226_SHUNT_LSB_UV / 1000.0f;
    float current_a = current_signed * CURRENT_LSB_A;
    float power_w = raw_power * POWER_LSB_W;

    ESP_LOGI(TAG, "----------------------------------------");
    ESP_LOGI(TAG, "Bus Voltage   : %.4f V", bus_voltage_v);
    ESP_LOGI(TAG, "Shunt Voltage : %.4f mV", shunt_voltage_mv);
    ESP_LOGI(TAG, "Current       : %.4f A", current_a);
    ESP_LOGI(TAG, "Power         : %.4f W", power_w);
}

void app_main(void)
{
    ESP_LOGI(TAG, "ESP32 INA226 Wokwi Custom Chip Test");

    ESP_ERROR_CHECK(i2c_master_init());

    vTaskDelay(pdMS_TO_TICKS(500));

    i2c_scan();

    esp_err_t err = ina226_init();
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "INA226 init gagal: %s", esp_err_to_name(err));
        return;
    }

    ESP_LOGI(TAG, "INA226 init sukses");

    while (1) {
        ina226_print_data();
        vTaskDelay(pdMS_TO_TICKS(2000));
    }
}