#include "pac1952_app.h"

#include "mcc_generated_files/i2c_host/i2c1.h"
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>

#define PAC1952_I2C_ADDRESS_DEFAULT      0x10U
#define PAC1952_CMD_REFRESH              0x00U
#define PAC1952_REG_VBUS1                0x07U
#define PAC1952_REG_VBUS2                0x08U
#define PAC1952_REG_VSENSE1_AVG          0x13U
#define PAC1952_REG_VSENSE2_AVG          0x14U
#define PAC1952_REG_VPOWER1              0x17U
#define PAC1952_REG_VPOWER2              0x18U

#define PAC1952_VBUS_FULL_SCALE_V        32.0f
#define PAC1952_VSENSE_FULL_SCALE_V      0.1f
#define PAC1952_SHUNT_RESISTOR_OHMS      0.010f
#define PAC1952_VBUS_CAL_GAIN            1.0f
#define PAC1952_CURRENT_OFFSET_A         0.0f
#define PAC1952_VPOWER_CODE_MAX          268435456.0f

#define APP_I2C_WAIT_LIMIT               100000U
#define APP_PAC_READ_RETRY_COUNT         3U
#define APP_VSENSE_FILTER_SAMPLES        8U
#define APP_I2C_SCAN_ADDRESS_MIN         0x08U
#define APP_I2C_SCAN_ADDRESS_MAX         0x77U

enum APP_I2C_RESULT
{
    APP_I2C_RESULT_OK = 0,
    APP_I2C_RESULT_REQUEST_DENIED,
    APP_I2C_RESULT_TIMEOUT,
    APP_I2C_RESULT_NACK,
    APP_I2C_RESULT_BUS_COLLISION,
    APP_I2C_RESULT_ERROR_UNKNOWN
};

static uint16_t pac1952Address = PAC1952_I2C_ADDRESS_DEFAULT;
static enum APP_I2C_RESULT appI2cLastResult = APP_I2C_RESULT_OK;
static enum I2C_HOST_ERROR appI2cLastHostError = I2C_HOST_ERROR_NONE;
static float pac1952LastVsense_mV[2] = {0.0f, 0.0f};
static int16_t vsenseFilterBuffer[2][APP_VSENSE_FILTER_SAMPLES] = {{0}};
static uint8_t vsenseFilterIndex[2] = {0U, 0U};
static uint8_t vsenseFilterCount[2] = {0U, 0U};
static int32_t vsenseFilterSum[2] = {0, 0};
static float appLastEnergyCh1 = 0.0f;
static float appLastEnergyCh2 = 0.0f;
static float pac1952LastCurrentCh1_A = 0.0f;
static float pac1952LastCurrentCh2_A = 0.0f;
static bool pac1952CurrentValid = false;

static const char *APP_I2CErrorStringGet(enum I2C_HOST_ERROR error)
{
    switch(error)
    {
        case I2C_HOST_ERROR_NONE:
            return "none";
        case I2C_HOST_ERROR_NACK:
            return "nack";
        case I2C_HOST_ERROR_BUS_COLLISION:
            return "bus_collision";
        default:
            return "unknown";
    }
}

static const char *APP_I2CResultStringGet(enum APP_I2C_RESULT result)
{
    switch(result)
    {
        case APP_I2C_RESULT_OK:
            return "ok";
        case APP_I2C_RESULT_REQUEST_DENIED:
            return "request_denied";
        case APP_I2C_RESULT_TIMEOUT:
            return "timeout";
        case APP_I2C_RESULT_NACK:
            return "nack";
        case APP_I2C_RESULT_BUS_COLLISION:
            return "bus_collision";
        default:
            return "error_unknown";
    }
}

static enum APP_I2C_RESULT APP_I2CResultFromHostError(enum I2C_HOST_ERROR error)
{
    switch(error)
    {
        case I2C_HOST_ERROR_NONE:
            return APP_I2C_RESULT_OK;
        case I2C_HOST_ERROR_NACK:
            return APP_I2C_RESULT_NACK;
        case I2C_HOST_ERROR_BUS_COLLISION:
            return APP_I2C_RESULT_BUS_COLLISION;
        default:
            return APP_I2C_RESULT_ERROR_UNKNOWN;
    }
}

static bool APP_I2C1_WaitBusIdle(void)
{
    uint32_t guardCount = 0U;

    while(I2C1_IsBusy())
    {
        guardCount++;
        if(guardCount > APP_I2C_WAIT_LIMIT)
        {
            appI2cLastResult = APP_I2C_RESULT_TIMEOUT;
            appI2cLastHostError = I2C_HOST_ERROR_NONE;
            return false;
        }
    }

    return true;
}

static bool APP_I2C1_WaitForCompletion(enum I2C_HOST_ERROR *error, bool *timedOut)
{
    uint32_t guardCount = 0U;

    if(error != NULL)
    {
        *error = I2C_HOST_ERROR_NONE;
    }

    if(timedOut != NULL)
    {
        *timedOut = false;
    }

    while(I2C1_IsBusy())
    {
        guardCount++;
        if(guardCount > APP_I2C_WAIT_LIMIT)
        {
            if(timedOut != NULL)
            {
                *timedOut = true;
            }
            appI2cLastResult = APP_I2C_RESULT_TIMEOUT;
            appI2cLastHostError = I2C_HOST_ERROR_NONE;
            return false;
        }
    }

    if(error != NULL)
    {
        *error = I2C1_Host_ErrorGet();
        appI2cLastHostError = *error;
        appI2cLastResult = APP_I2CResultFromHostError(*error);
    }

    return ((error == NULL) || (*error == I2C_HOST_ERROR_NONE));
}

static bool APP_I2C1_WriteReadBlocking(uint16_t address, uint8_t reg, uint8_t *readData, size_t readLen, enum I2C_HOST_ERROR *error, bool *timedOut)
{
    bool requestAccepted;

    if(!APP_I2C1_WaitBusIdle())
    {
        return false;
    }

    requestAccepted = I2C1_Host_WriteRead(address, &reg, 1U, readData, readLen);

    if(!requestAccepted)
    {
        if(timedOut != NULL)
        {
            *timedOut = false;
        }
        if(error != NULL)
        {
            *error = I2C_HOST_ERROR_NONE;
        }
        appI2cLastResult = APP_I2C_RESULT_REQUEST_DENIED;
        appI2cLastHostError = I2C_HOST_ERROR_NONE;
        return false;
    }

    return APP_I2C1_WaitForCompletion(error, timedOut);
}

static bool APP_I2C1_WriteCommandBlocking(uint16_t address, uint8_t command, enum I2C_HOST_ERROR *error, bool *timedOut)
{
    if(!APP_I2C1_WaitBusIdle())
    {
        return false;
    }

    if(!I2C1_Host_Write(address, &command, 1U))
    {
        if(timedOut != NULL)
        {
            *timedOut = false;
        }
        if(error != NULL)
        {
            *error = I2C_HOST_ERROR_NONE;
        }
        appI2cLastResult = APP_I2C_RESULT_REQUEST_DENIED;
        appI2cLastHostError = I2C_HOST_ERROR_NONE;
        return false;
    }

    return APP_I2C1_WaitForCompletion(error, timedOut);
}

static bool APP_I2C1_AddressAckProbe(uint16_t address)
{
    uint8_t command = PAC1952_CMD_REFRESH;
    enum I2C_HOST_ERROR i2cError;
    bool timedOut;

    if(!APP_I2C1_WaitBusIdle())
    {
        return false;
    }

    if(!I2C1_Host_Write(address, &command, 1U))
    {
        return false;
    }

    if(!APP_I2C1_WaitForCompletion(&i2cError, &timedOut))
    {
        return false;
    }

    return true;
}

static void APP_I2C1_BusScan(void)
{
    uint16_t address;

    for(address = APP_I2C_SCAN_ADDRESS_MIN; address <= APP_I2C_SCAN_ADDRESS_MAX; address++)
    {
        (void)APP_I2C1_AddressAckProbe(address);
    }
}

static bool APP_PAC1952_ReadReg16WithRetry(uint16_t address, uint8_t reg, uint16_t *value)
{
    uint8_t raw[2] = {0U};
    enum I2C_HOST_ERROR i2cError;
    bool timedOut;
    uint8_t retry;

    if(value == NULL)
    {
        return false;
    }

    for(retry = 0U; retry < APP_PAC_READ_RETRY_COUNT; retry++)
    {
        if(APP_I2C1_WriteReadBlocking(address, reg, raw, sizeof(raw), &i2cError, &timedOut))
        {
            *value = (uint16_t)(((uint16_t)raw[0] << 8) | raw[1]);
            return true;
        }
    }

    I2C1_Deinitialize();
    I2C1_Initialize();

    if(APP_I2C1_WriteReadBlocking(address, reg, raw, sizeof(raw), &i2cError, &timedOut))
    {
        *value = (uint16_t)(((uint16_t)raw[0] << 8) | raw[1]);
        return true;
    }

    return false;
}

static bool APP_PAC1952_RefreshWithRetry(uint16_t address)
{
    enum I2C_HOST_ERROR i2cError;
    bool timedOut;
    uint8_t retry;

    for(retry = 0U; retry < APP_PAC_READ_RETRY_COUNT; retry++)
    {
        if(APP_I2C1_WriteCommandBlocking(address, PAC1952_CMD_REFRESH, &i2cError, &timedOut))
        {
            return true;
        }
    }

    return false;
}

static bool APP_PAC1952_ReadPower(uint8_t reg, float *power_W)
{
    uint8_t raw[4] = {0U};
    uint8_t retry;
    uint32_t powerCode;
    float fullScalePower_W;
    enum I2C_HOST_ERROR i2cError;
    bool timedOut;

    if(power_W == NULL)
    {
        return false;
    }

    for(retry = 0U; retry < APP_PAC_READ_RETRY_COUNT; retry++)
    {
        if(APP_I2C1_WriteReadBlocking(pac1952Address, reg, raw, sizeof(raw), &i2cError, &timedOut))
        {
            powerCode = ((((uint32_t)raw[0]) << 24) |
                        (((uint32_t)raw[1]) << 16) |
                        (((uint32_t)raw[2]) << 8)  |
                        ((uint32_t)raw[3])) >> 4;

            fullScalePower_W = (PAC1952_VBUS_FULL_SCALE_V * PAC1952_VSENSE_FULL_SCALE_V) / PAC1952_SHUNT_RESISTOR_OHMS;
            *power_W = ((float)powerCode / PAC1952_VPOWER_CODE_MAX) * fullScalePower_W;
            return true;
        }
    }

    I2C1_Deinitialize();
    I2C1_Initialize();

    if(APP_I2C1_WriteReadBlocking(pac1952Address, reg, raw, sizeof(raw), &i2cError, &timedOut))
    {
        powerCode = ((((uint32_t)raw[0]) << 24) |
                    (((uint32_t)raw[1]) << 16) |
                    (((uint32_t)raw[2]) << 8)  |
                    ((uint32_t)raw[3])) >> 4;
        fullScalePower_W = (PAC1952_VBUS_FULL_SCALE_V * PAC1952_VSENSE_FULL_SCALE_V) / PAC1952_SHUNT_RESISTOR_OHMS;
        *power_W = ((float)powerCode / PAC1952_VPOWER_CODE_MAX) * fullScalePower_W;
        return true;
    }

    return false;
}

static bool APP_PAC1952_ReadChannel(uint8_t channelIndex, uint8_t regVbus, uint8_t regVsenseAvg, float *voltage_V, float *current_A)
{
    uint16_t vbusCode;
    int16_t vsenseCode;
    int16_t vsenseFilteredCode;
    float vsenseVoltage_V;

    if((voltage_V == NULL) || (current_A == NULL) || (channelIndex > 1U))
    {
        return false;
    }

    (void)APP_PAC1952_RefreshWithRetry(pac1952Address);

    if(!APP_PAC1952_ReadReg16WithRetry(pac1952Address, regVbus, &vbusCode))
    {
        return false;
    }

    if(!APP_PAC1952_ReadReg16WithRetry(pac1952Address, regVsenseAvg, (uint16_t *)&vsenseCode))
    {
        return false;
    }

    if(vsenseFilterCount[channelIndex] < APP_VSENSE_FILTER_SAMPLES)
    {
        vsenseFilterCount[channelIndex]++;
    }
    else
    {
        vsenseFilterSum[channelIndex] -= (int32_t)vsenseFilterBuffer[channelIndex][vsenseFilterIndex[channelIndex]];
    }

    vsenseFilterBuffer[channelIndex][vsenseFilterIndex[channelIndex]] = vsenseCode;
    vsenseFilterSum[channelIndex] += (int32_t)vsenseCode;

    vsenseFilterIndex[channelIndex]++;
    if(vsenseFilterIndex[channelIndex] >= APP_VSENSE_FILTER_SAMPLES)
    {
        vsenseFilterIndex[channelIndex] = 0U;
    }

    vsenseFilteredCode = (int16_t)(vsenseFilterSum[channelIndex] / (int32_t)vsenseFilterCount[channelIndex]);

    *voltage_V = (((float)vbusCode / 65535.0f) * PAC1952_VBUS_FULL_SCALE_V) * PAC1952_VBUS_CAL_GAIN;
    vsenseVoltage_V = ((float)vsenseFilteredCode / 32768.0f) * PAC1952_VSENSE_FULL_SCALE_V;
    pac1952LastVsense_mV[channelIndex] = vsenseVoltage_V * 1000.0f;
    *current_A = (vsenseVoltage_V / PAC1952_SHUNT_RESISTOR_OHMS) - PAC1952_CURRENT_OFFSET_A;

    return true;
}

void PAC1952_AppInitialize(void)
{
    struct I2C_TRANSFER_SETUP i2cSetup = {
        .clkSpeed = 100000UL
    };

    (void)I2C1_TransferSetup(&i2cSetup, I2C1_CLOCK_FREQUENCY);
    APP_I2C1_BusScan();
}

void PAC1952_AppReadAndPrint(float samplePeriodSeconds)
{
    float voltageCh1_V = 0.0f;
    float currentCh1_A = 0.0f;
    float powerCh1_W = 0.0f;
    float voltageCh2_V = 0.0f;
    float currentCh2_A = 0.0f;
    float powerCh2_W = 0.0f;
    enum I2C_HOST_ERROR i2cError;
    bool timedOut;

    if(!APP_PAC1952_ReadChannel(0U, PAC1952_REG_VBUS1, PAC1952_REG_VSENSE1_AVG, &voltageCh1_V, &currentCh1_A))
    {
        i2cError = appI2cLastHostError;
        timedOut = (appI2cLastResult == APP_I2C_RESULT_TIMEOUT);
        printf("PAC1952-1 CH1 read failed (addr=0x%02X, result=%s, err=%s%s)\r\n",
            (unsigned int)pac1952Address,
            APP_I2CResultStringGet(appI2cLastResult),
            APP_I2CErrorStringGet(i2cError),
            timedOut ? ", timeout" : "");
        return;
    }

    if(!APP_PAC1952_ReadPower(PAC1952_REG_VPOWER1, &powerCh1_W))
    {
        i2cError = appI2cLastHostError;
        timedOut = (appI2cLastResult == APP_I2C_RESULT_TIMEOUT);
        printf("PAC1952-1 CH1 read failed (addr=0x%02X, result=%s, err=%s%s)\r\n",
            (unsigned int)pac1952Address,
            APP_I2CResultStringGet(appI2cLastResult),
            APP_I2CErrorStringGet(i2cError),
            timedOut ? ", timeout" : "");
        return;
    }

    appLastEnergyCh1 += (powerCh1_W * samplePeriodSeconds) / 3600.0f;

    printf("PAC1952-1 CH1: VBUS=%.4f V, VSENSE=%.3f mV, I=%.4f A, P=%.4f W, E=%.6f Wh (%.3f mWh)\r\n",
        voltageCh1_V,
        pac1952LastVsense_mV[0],
        currentCh1_A,
        powerCh1_W,
        appLastEnergyCh1,
        appLastEnergyCh1 * 1000.0f);

    if(!APP_PAC1952_ReadChannel(1U, PAC1952_REG_VBUS2, PAC1952_REG_VSENSE2_AVG, &voltageCh2_V, &currentCh2_A))
    {
        i2cError = appI2cLastHostError;
        timedOut = (appI2cLastResult == APP_I2C_RESULT_TIMEOUT);
        printf("PAC1952-1 CH2 read failed (addr=0x%02X, result=%s, err=%s%s)\r\n",
            (unsigned int)pac1952Address,
            APP_I2CResultStringGet(appI2cLastResult),
            APP_I2CErrorStringGet(i2cError),
            timedOut ? ", timeout" : "");
        return;
    }

    if(!APP_PAC1952_ReadPower(PAC1952_REG_VPOWER2, &powerCh2_W))
    {
        i2cError = appI2cLastHostError;
        timedOut = (appI2cLastResult == APP_I2C_RESULT_TIMEOUT);
        printf("PAC1952-1 CH2 read failed (addr=0x%02X, result=%s, err=%s%s)\r\n",
            (unsigned int)pac1952Address,
            APP_I2CResultStringGet(appI2cLastResult),
            APP_I2CErrorStringGet(i2cError),
            timedOut ? ", timeout" : "");
        return;
    }

    appLastEnergyCh2 += (powerCh2_W * samplePeriodSeconds) / 3600.0f;

    printf("PAC1952-1 CH2: VBUS=%.4f V, VSENSE=%.3f mV, I=%.4f A, P=%.4f W, E=%.6f Wh (%.3f mWh)\r\n",
        voltageCh2_V,
        pac1952LastVsense_mV[1],
        currentCh2_A,
        powerCh2_W,
        appLastEnergyCh2,
        appLastEnergyCh2 * 1000.0f);

    pac1952LastCurrentCh1_A = currentCh1_A;
    pac1952LastCurrentCh2_A = currentCh2_A;
    pac1952CurrentValid = true;
}

bool PAC1952_AppGetLastCurrents(float *currentCh1_A, float *currentCh2_A)
{
    if((currentCh1_A == NULL) || (currentCh2_A == NULL) || (!pac1952CurrentValid))
    {
        return false;
    }

    *currentCh1_A = pac1952LastCurrentCh1_A;
    *currentCh2_A = pac1952LastCurrentCh2_A;
    return true;
}

bool PAC1952_AppGetLastEnergy(float *energyCh1Wh, float *energyCh2Wh)
{
    if((energyCh1Wh == NULL) || (energyCh2Wh == NULL) || (!pac1952CurrentValid))
    {
        return false;
    }

    (void)memcpy(energyCh1Wh, &appLastEnergyCh1, sizeof(float));
    (void)memcpy(energyCh2Wh, &appLastEnergyCh2, sizeof(float));
    return true;
}
