#include "ina233.h"

#include <app/zedmon/usb.h>
#include <assert.h>
#include <err.h>
#include <dev/i2c.h>
#include <lib/cbuf.h>
#include <lib/console.h>
#include <platform.h>
#include <platform/timer_capture.h>
#include <stdint.h>
#include <stdio.h>
#include <target/gpioconfig.h>

enum {
    INA233_RER_CLEAR_FAULTS        = 0x03,
    INA233_REG_RESTORE_DEFAULT_ALL = 0x12,
    INA233_REG_CAPABILITY          = 0x19,
    INA233_REG_IOUT_OC_WARN_LIMIT  = 0x4a,
    INA233_REG_VIN_OV_WARN_LIMIT   = 0x57,
    INA233_REG_VIN_UV_WARN_LIMIT   = 0x58,
    INA233_REG_PIN_OP_WANR_LIMIT   = 0x6B,
    INA233_REG_STATUS_BYTE         = 0x78,
    INA233_REG_STATUS_WORD         = 0x79,
    INA233_REF_STATUS_IOUT         = 0x7b,
    INA233_REG_STATUS_INPUT        = 0x7c,
    INA233_REG_STATUS_CML          = 0x7e,
    INA233_REG_STATUS_MFR_SPECIFIC = 0x80,
    INA233_REG_READ_EIN            = 0x86,
    INA233_REG_READ_VIN            = 0x88,
    INA233_REG_READ_IN             = 0x89,
    INA233_REG_READ_VOUT           = 0x8b,
    INA233_REG_READ_IOUT           = 0x8c,
    INA233_REG_READ_POUT           = 0x96,
    INA233_REG_READ_PIN            = 0x97,
    INA233_REG_MFR_ID              = 0x99,
    INA233_REG_MFR_MODEL           = 0x9A,
    INA233_REG_MFR_REVISION        = 0x9b,
    INA233_REG_MFR_ADC_CONFIG      = 0xd0,
    INA233_REG_MFR_READ_VSHUNT     = 0xd1,
    INA233_REG_MFR_ALERT_MASK      = 0xd2,
    INA233_REG_MFR_CALIBRATION     = 0xd4,
    INA233_REG_MFR_DEVICE_CONFIG   = 0xd5,
    INA233_REG_CLEAR_EIN           = 0xd6,
    INA233_REG_TI_MFR_ID           = 0xe0,
    INA233_REG_TI_MFR_MODEL        = 0xe1,
    INA233_REF_TI_MFR_REVISION     = 0xe2,
};

enum {
    INA233_ADC_CONFIG_MODE_POWER_DOWN     = 0 << 0,
    INA233_ADC_CONFIG_MODE_TRIG_SHUNT     = 1 << 0,
    INA233_ADC_CONFIG_MODE_TRIG_BUS       = 2 << 0,
    INA233_ADC_CONFIG_MODE_TRIG_SHUNT_BUS = 3 << 0,
    INA233_ADC_CONFIG_MODE_POWER_DOWN2    = 4 << 0,
    INA233_ADC_CONFIG_MODE_CONT_SHUNT     = 5 << 0,
    INA233_ADC_CONFIG_MODE_CONT_BUS       = 6 << 0,
    INA233_ADC_CONFIG_MODE_CONT_SHUNT_BUS = 7 << 0,

    INA233_ADC_CONFIG_V_SHUNT_CT_140_US  = 0 << 3,
    INA233_ADC_CONFIG_V_SHUNT_CT_204_US  = 1 << 3,
    INA233_ADC_CONFIG_V_SHUNT_CT_332_US  = 2 << 3,
    INA233_ADC_CONFIG_V_SHUNT_CT_588_US  = 3 << 3,
    INA233_ADC_CONFIG_V_SHUNT_CT_1100_US = 4 << 3,
    INA233_ADC_CONFIG_V_SHUNT_CT_2116_US = 5 << 3,
    INA233_ADC_CONFIG_V_SHUNT_CT_4156_US = 6 << 3,
    INA233_ADC_CONFIG_V_SHUNT_CT_8244_US = 7 << 3,

    INA233_ADC_CONFIG_V_BUS_CT_140_US  = 0 << 6,
    INA233_ADC_CONFIG_V_BUS_CT_204_US  = 1 << 6,
    INA233_ADC_CONFIG_V_BUS_CT_332_US  = 2 << 6,
    INA233_ADC_CONFIG_V_BUS_CT_588_US  = 3 << 6,
    INA233_ADC_CONFIG_V_BUS_CT_1100_US = 4 << 6,
    INA233_ADC_CONFIG_V_BUS_CT_2116_US = 5 << 6,
    INA233_ADC_CONFIG_V_BUS_CT_4156_US = 6 << 6,
    INA233_ADC_CONFIG_V_BUS_CT_8244_US = 7 << 6,

    INA233_ADC_CONFIG_AVG_1    = 0 << 9,
    INA233_ADC_CONFIG_AVG_4    = 1 << 9,
    INA233_ADC_CONFIG_AVG_16   = 2 << 9,
    INA233_ADC_CONFIG_AVG_64   = 3 << 9,
    INA233_ADC_CONFIG_AVG_128  = 4 << 9,
    INA233_ADC_CONFIG_AVG_256  = 5 << 9,
    INA233_ADC_CONFIG_AVG_512  = 6 << 9,
    INA233_ADC_CONFIG_AVG_1024 = 7 << 9,

    INA233_ADC_CONFIG_RESERVED = 1 << 14,
};

enum {
    INA233_STATUS_MFR_IN_UV_WARNING    = 1 << 0,
    INA233_STATUS_MFR_IN_OV_WARNING    = 1 << 1,
    INA233_STATUS_MFR_IN_OC_WARNING    = 1 << 2,
    INA233_STATUS_MFR_IN_OP_WARNING    = 1 << 3,
    INA233_STATUS_MFR_COMM_FAILURE     = 1 << 4,
    INA233_STATUS_MFR_POR              = 1 << 5,
    INA233_STATUS_MFR_ADC_OVERFLOW     = 1 << 6,
    INA233_STATUS_MFR_CONVERSION_READY = 1 << 7,
};

enum {
    INA233_ALERT_MASK_IN_UV_WARNING    = 1 << 0,
    INA233_ALERT_MASK_IN_OV_WARNING    = 1 << 1,
    INA233_ALERT_MASK_IN_OC_WARNING    = 1 << 2,
    INA233_ALERT_MASK_IN_OP_WARNING    = 1 << 3,
    INA233_ALERT_MASK_COMM_FAILURE     = 1 << 4,
    INA233_ALERT_MASK_POR              = 1 << 5,
    INA233_ALERT_MASK_ADC_OVERFLOW     = 1 << 6,
    INA233_ALERT_MASK_CONVERSION_READY = 1 << 7,
};

static const uint16_t INA233_MFR_ID = 0x5449;
static const uint16_t INA233_MFR_MODEL = 0x3333;
static const uint8_t INA233_ADDR = 0x40;

static uint32_t ina233_errors;

static status_t ina233_write16(uint8_t reg, uint16_t val) {
    uint8_t data[2];
    data[1] = val >> 8;
    data[0] = val & 0xff;

    status_t ret = i2c_write_reg_bytes(INA_BUS, INA233_ADDR, reg, data, 2);
    if (ret != NO_ERROR) {
        return ret;
    }

    return NO_ERROR;
}

static status_t ina233_write8(uint8_t reg, uint8_t val) {
    status_t ret = i2c_write_reg_bytes(INA_BUS, INA233_ADDR, reg, &val, 1);
    if (ret != NO_ERROR) {
        return ret;
    }

    return NO_ERROR;
}

static status_t ina233_read8(uint8_t reg, uint8_t *val) {
    status_t ret = i2c_read_reg_bytes(INA_BUS, INA233_ADDR, reg, val, 1);
    if (ret != NO_ERROR) {
        return ret;
    }
    return NO_ERROR;
}

static status_t ina233_read16(uint8_t reg, uint16_t *val) {
    uint8_t data[2];

    status_t ret = i2c_read_reg_bytes(INA_BUS, INA233_ADDR, reg, data, 2);
    if (ret != NO_ERROR) {
        return ret;
    }

    *val = data[1] << 8 | data[0];
    return NO_ERROR;
}

bool ina233_init(void) {
    status_t ret;

    // Try to read TI_MFR_ID and TI_MFR_MODEL to detect device
    uint16_t mfr_id;
    ret = ina233_read16(INA233_REG_TI_MFR_ID, &mfr_id);
    if (ret != NO_ERROR || mfr_id != INA233_MFR_ID) {
        return false;
    }

    uint16_t mfr_model;
    ret = ina233_read16(INA233_REG_TI_MFR_MODEL, &mfr_model);
    if (ret != NO_ERROR || mfr_model != INA233_MFR_MODEL) {
        return false;
    }

    // At this point we know we're speaking to an ina233.
    ret = ina233_write16(INA233_REG_MFR_ADC_CONFIG,
                       INA233_ADC_CONFIG_MODE_CONT_SHUNT_BUS
                       | INA233_ADC_CONFIG_V_SHUNT_CT_332_US
                       | INA233_ADC_CONFIG_V_BUS_CT_332_US
                       | INA233_ADC_CONFIG_AVG_1
                       | INA233_ADC_CONFIG_RESERVED);
    if (ret != NO_ERROR) {
        return false;
    }

    ret = ina233_write8(INA233_REG_MFR_ALERT_MASK,
                                 ~INA233_ALERT_MASK_CONVERSION_READY);
    if (ret != NO_ERROR) {
        return false;
    }

    ret = ina233_write8(INA233_REG_STATUS_MFR_SPECIFIC, 0xff);
    if (ret != NO_ERROR) {
        return false;
    }

    return true;
}


bool ina233_sample(uint16_t *v_shunt, uint16_t *v_bus) {
    status_t ret;

    uint8_t status;
    ret = ina233_read8(INA233_REG_STATUS_MFR_SPECIFIC, &status);
    if (ret != NO_ERROR) {
        ina233_errors++;
        return false;
    }

    // Ack the status.
    ret = ina233_write8(INA233_REG_STATUS_MFR_SPECIFIC, status);
    if (ret != NO_ERROR) {
        ina233_errors++;
        return false;
    }

    ret = ina233_read16(INA233_REG_MFR_READ_VSHUNT, v_shunt);
    if (ret != NO_ERROR) {
        ina233_errors++;
        return false;
    }

    ret = ina233_read16(INA233_REG_READ_VIN, v_bus);
    if (ret != NO_ERROR) {
        ina233_errors++;
        return false;
    }

    return true;
}

static int cmd_ina_read16(int argc, const cmd_args *argv) {
    if (argc != 2) {
        printf("usage: ina_233_read <reg>\n");
        return 1;
    }

    uint8_t reg = argv[1].u;
    uint16_t data;

    status_t ret = ina233_read16(reg, &data);
    if (ret != NO_ERROR) {
        printf("error: %d\n", ret);
        return 1;
    }

    printf("%02x: %04x\n", reg, data);
    return 0;
}

static int cmd_ina_read8(int argc, const cmd_args *argv) {
    if (argc != 2) {
        printf("usage: ina_233_read <reg>\n");
        return 1;
    }

    uint8_t reg = argv[1].u;
    uint8_t data;

    status_t ret = ina233_read8(reg, &data);
    if (ret != NO_ERROR) {
        printf("error: %d\n", ret);
        return 1;
    }

    printf("%02x: %02x\n", reg, data);
    return 0;
}

static int cmd_ina_write8(int argc, const cmd_args *argv) {
    if (argc != 3) {
        printf("usage: ina_233_write8 <reg> <val>\n");
        return 1;
    }

    uint8_t reg = argv[1].u;
    uint8_t data = argv[2].u;

    status_t ret = ina233_write8(reg, data);
    if (ret != NO_ERROR) {
        printf("error: %d\n", ret);
        return 1;
    }

    printf("%02x: %02x\n", reg, data);

    return 0;
}

static int cmd_ina_write16(int argc, const cmd_args *argv) {
    if (argc != 3) {
        printf("usage: ina_233_write16 <reg> <val>\n");
        return 1;
    }

    uint8_t reg = argv[1].u;
    uint16_t data = argv[2].u;

    status_t ret = ina233_write16(reg, data);
    if (ret != NO_ERROR) {
        printf("error: %d\n", ret);
        return 1;
    }

    printf("%02x: %04x\n", reg, data);

    return 0;
}

STATIC_COMMAND_START
STATIC_COMMAND("ina_read8", "read an ina233 register", &cmd_ina_read8)
STATIC_COMMAND("ina_read16", "read an ina233 register", &cmd_ina_read16)
STATIC_COMMAND("ina_write8", "write an ina233 register", &cmd_ina_write8)
STATIC_COMMAND("ina_write16", "write an ina233 register", &cmd_ina_write16)
STATIC_COMMAND_END(ina233);