#include "global.h"
#include "i2c.h"
#include "ina226.h"

void ina226_init(struct ina226 *dev, I2C_TypeDef *i2c_periph, uint8_t i2c_addr, uint16_t rs_uOhm, uint16_t conv_config) {
    dev->i2c_periph = i2c_periph;
    dev->i2c_addr = i2c_addr;

    dev->conv_config = conv_config;
    dev->rs_uOhm = rs_uOhm;
    dev->i_lsb_uA = INA226_VS_LSB_nV * 1000 / rs_uOhm;

    uint16_t cal = 5120000 / dev->i_lsb_uA * 1000 / dev->rs_uOhm;
    ina226_write_reg(dev, INA226_REG_CAL, cal);
    dev->conv_ready = false;

    /* Do not trigger a conversion yet since depending on settings a conversion might take a long time, and we don't
     * want to immediately block the device after initialization. */
}

void ina226_trigger(struct ina226 *dev, int mode) {
    ina226_write_reg(dev, INA226_REG_CONFIG,
            dev->conv_config | mode | INA226_CONFIG_MODE_TRIG);
    dev->conv_ready = false;
}

void ina226_write_reg(struct ina226 *dev, uint8_t reg, uint16_t val) {
    uint8_t buf[3] = { reg, val>>8, val&0xff };
    i2c_transmit(dev->i2c_periph, buf, sizeof(buf), dev->i2c_addr, I2C_GENSTOP_YES);
}

uint16_t ina226_read_reg(struct ina226 *dev, uint8_t reg) {
    uint8_t buf2[1] = { reg };
    i2c_transmit(dev->i2c_periph, buf2, sizeof(buf2), dev->i2c_addr, I2C_GENSTOP_NO);
    uint8_t rx[2];
    i2c_receive(dev->i2c_periph, rx, sizeof(rx), dev->i2c_addr);
    return (rx[0]<<8) | rx[1];
}

int ina226_read_raw(struct ina226 *dev, int16_t *I_raw, int16_t *V_raw, int16_t *Vs_raw) {
    if (!ina226_conv_ready(dev))
        return -1;

    if (I_raw)
        *I_raw = (int16_t)ina226_read_reg(dev, INA226_REG_I);
    if (V_raw)
        *V_raw = (int16_t)ina226_read_reg(dev, INA226_REG_VB);
    if (Vs_raw)
        *Vs_raw = (int16_t)ina226_read_reg(dev, INA226_REG_VS);

    return 0;
}

int ina226_read_scaled(struct ina226 *dev, int16_t *I_mA, int16_t *V_mV, int16_t *Vs_uV) {
    if (!ina226_conv_ready(dev))
        return -1;

    if (I_mA)
        *I_mA = ((int16_t)ina226_read_reg(dev, INA226_REG_I))*dev->i_lsb_uA/1000;
    if (V_mV)
        *V_mV = ((int16_t)ina226_read_reg(dev, INA226_REG_VB))*INA226_VB_LSB_uV/1000;
    if (Vs_uV)
        *Vs_uV = ((int16_t)ina226_read_reg(dev, INA226_REG_VS))*INA226_VS_LSB_nV/1000;

    return 0;
}

bool ina226_conv_ready(struct ina226 *dev) {
    /* We need a bit of logic here to track state since this flag auto-resets not only on conversion trigger but also on
     * flag read. */
    if (!dev->conv_ready && (ina226_read_reg(dev, INA226_REG_MASK_EN) & INA226_MASK_EN_CVRF))
        dev->conv_ready = true;
    return dev->conv_ready;
}