#include "ina226.h"

static uint8 is_iic_init = 0;
INA226_t ina226[INA226_NUM];

#if (1==INA226_USE_SOFT_IIC)

#include "iic_soft.h"
static IIC_Soft_Config ina226_soft_config;

void ina226_scl_set()
{
	INA226_SCL_LEVEL(1);
}

void ina226_sda_set()
{
	INA226_SDA_LEVEL(1);
}

void ina226_sda_clr()
{
	INA226_SDA_LEVEL(0);
}

void ina226_scl_clr()
{
	INA226_SCL_LEVEL(0);
}

uint8 ina226_sda_read()
{
	return INA226_SDA_READ();
}

#else

#include "iic.h"

#endif


void INA226_Init(INA226_t *ina226, uint8 addr, uint8 mode[2], float max_current_A, float rshunt_mOhm, float amp_fix_k, float vot_fix_k)
{
    float cal;
    if(!is_iic_init)
    {
        #if (1==INA226_USE_SOFT_IIC)
        ina226_soft_config.SCL_SET = ina226_scl_set;
        ina226_soft_config.SCL_CLR = ina226_scl_clr;
        ina226_soft_config.SDA_SET = ina226_sda_set;
        ina226_soft_config.SDA_CLR = ina226_sda_clr;
        ina226_soft_config.SDA_READ = ina226_sda_read;
        ina226_soft_config.wait_time = INA226_SOFT_WAIT_TIME;
        soft_iic_init(&ina226_soft_config);
        #else
        iic_init(INA226_IIC, INA226_HARD_WAIT_TIME);
        #endif
        is_iic_init = 1;
    }
    ina226->config.addr = addr;
    ina226->config.mode[0] = mode[0];
    ina226->config.mode[1] = mode[1];
    ina226->config.max_current_A = max_current_A;
    ina226->config.rshunt_mOhm = rshunt_mOhm;
    ina226->config.vot_fix_k = vot_fix_k;
    ina226->config.amp_fix_k = amp_fix_k;

    ina226->calibration.c_lsb_mA = max_current_A / 65.535f; // Current LSB
    cal = (5120.0f) / (ina226->calibration.c_lsb_mA * ina226->config.rshunt_mOhm) * ina226->config.amp_fix_k;
    ina226->calibration.cal[0] = (uint8)((uint16)cal >> 8);
    ina226->calibration.cal[1] = (uint8)((uint16)cal & 0xFF);

    memset(&ina226->result, 0, sizeof(ina226->result));
    #if (1==INA226_USE_SOFT_IIC)
    soft_iic_write_reg_bytes(&ina226_soft_config, ina226->config.addr, INA226_CONFIG_REG, ina226->config.mode, 2);
    soft_iic_write_reg_bytes(&ina226_soft_config, ina226->config.addr, INA226_CALIBRATION_REG, ina226->calibration.cal, 2);
    #else
    iic_write_reg_bytes(ina226->config.addr, INA226_CONFIG_REG, ina226->config.mode, 2);
    iic_write_reg_bytes(ina226->config.addr, INA226_CALIBRATION_REG, ina226->calibration.cal, 2);
    #endif

}

void INA226_Read(INA226_t *ina226)
{
    uint8 counter;
    if(!is_iic_init) return;

    // Read voltage, current, power, and shunt voltage registers
    #if (1==INA226_USE_SOFT_IIC)
    soft_iic_read_reg_bytes(&ina226_soft_config, ina226->config.addr, INA226_BUSVOT_REG, ina226->result.org_reg[1],2);
    soft_iic_read_reg_bytes(&ina226_soft_config, ina226->config.addr, INA226_CURRENT_REG, ina226->result.org_reg[3],2);
    soft_iic_read_reg_bytes(&ina226_soft_config, ina226->config.addr, INA226_POWER_REG, ina226->result.org_reg[2],2);
    soft_iic_read_reg_bytes(&ina226_soft_config, ina226->config.addr, INA226_SHUNTVOT_REG, ina226->result.org_reg[0],2);
    #else
    iic_read_reg_bytes(ina226->config.addr, INA226_BUSVOT_REG, ina226->result.org_reg[1],2);
    iic_read_reg_bytes(ina226->config.addr, INA226_CURRENT_REG, ina226->result.org_reg[3],2);
    iic_read_reg_bytes(ina226->config.addr, INA226_POWER_REG, ina226->result.org_reg[2],2);
    iic_read_reg_bytes(ina226->config.addr, INA226_SHUNTVOT_REG, ina226->result.org_reg[0],2);
    #endif

    ina226->result.voltage_V = (int16)((ina226->result.org_reg[1][0] << 8) | ina226->result.org_reg[1][1]) * 0.00125f;
    ina226->result.current_A = (int16)((ina226->result.org_reg[3][0] << 8) | ina226->result.org_reg[3][1]) * ina226->calibration.c_lsb_mA * 0.001f;
    ina226->result.power_W = (int16)((ina226->result.org_reg[2][0] << 8) | ina226->result.org_reg[2][1]) * ina226->calibration.c_lsb_mA * 0.025f;
    ina226->result.shunt_voltage_mV = (int16)((ina226->result.org_reg[0][0] << 8) | ina226->result.org_reg[0][1]) * 0.0025f;

    ina226->result.voltage_V = ina226->result.voltage_V > 0 ? ina226->result.voltage_V : 0;
    ina226->result.current_A = ina226->result.current_A > 0 ? ina226->result.current_A : 0;
    ina226->result.power_W = ina226->result.power_W > 0.016 && ina226->result.current_A > 0 && ina226->result.voltage_V > 0 ? ina226->result.power_W : 0;
    ina226->result.shunt_voltage_mV = ina226->result.shunt_voltage_mV > 0 ? ina226->result.shunt_voltage_mV : 0;

    for(counter = 0; counter < 9; counter++)
    {
        ina226->result.past_voltage_V[counter] = ina226->result.past_voltage_V[counter+1];
        ina226->result.past_current_A[counter] = ina226->result.past_current_A[counter+1];
    }
    ina226->result.past_voltage_V[9] = ina226->result.voltage_V;
    ina226->result.past_current_A[9] = ina226->result.current_A;
    
    ina226->result.energy_J += (ina226->result.past_voltage_V[9] * ina226->result.past_current_A[9] + ina226->result.past_voltage_V[8] * ina226->result.past_current_A[8]) * (INA226_READ_TIME_MS / 2000.0f);
    ina226->result.quantity_C += (ina226->result.past_current_A[9] + ina226->result.past_current_A[8]) * (INA226_READ_TIME_MS / 2000.0f);
}