/******************************************************************************
* Copyright (C) 2023 Advanced Micro Devices, Inc. All Rights Reserved.
* SPDX-License-Identifier: MIT
******************************************************************************/
/*
 * Application to read temperature and power using INA700 Sensor
 *
 */
#include <stdint.h>
#include "xparameters.h"
#include "xiic.h"
#include "xstatus.h"
#include "xil_printf.h"
#include "sleep.h"

/* INA700 I2C configuration */
#define INA700_IIC_BASEADDR 0x40820000
#define INA700_VCCINT_DEVADDR 0x45
#define INA700_VCC5V_DEVADDR  0x44

static XIic INA700_IicInstance;

/* I2C Transfer Function */
static s8 ina700_i2cTransfer(u8 i2cAddr, u8 *dataToWrite, u8 writeLength, u8 *dataToRead, u8 readLength) {
    if (XIic_WaitBusFree(INA700_IIC_BASEADDR) == XST_FAILURE) return XST_FAILURE;
    if (writeLength && XIic_Send(INA700_IIC_BASEADDR, i2cAddr, dataToWrite, writeLength, XIIC_STOP) != writeLength) return XST_FAILURE;
    if (readLength) {
        if (XIic_WaitBusFree(INA700_IIC_BASEADDR) == XST_FAILURE) return XST_FAILURE;
        if (XIic_Recv(INA700_IIC_BASEADDR, i2cAddr, dataToRead, readLength, XIIC_STOP) != readLength) return XST_FAILURE;
    }
    return XST_SUCCESS;
}

/* INA700 Initialization */
static s8 ina700_init(void) {
    XIic_Config *cfg = XIic_LookupConfig(INA700_IIC_BASEADDR);
    if (!cfg) return XST_FAILURE;
    if (XIic_CfgInitialize(&INA700_IicInstance, cfg, cfg->BaseAddress) != XST_SUCCESS) return XST_FAILURE;
    return XST_SUCCESS;
}

/* INA700 Read Functions */
static float INA700_getTemp(u8 devAddr) {
    u8 txBuf[1] = {0x06}, rxBuf[2];
    if (ina700_i2cTransfer(devAddr, txBuf, 1, rxBuf, 2) != XST_SUCCESS) return 0;
    int raw = ((rxBuf[0] << 8) | rxBuf[1]) >> 4;
    if (raw > 0x7FF) return ((float)raw - 0x1000) * 0.125;
    return (float)raw * 0.125;
}

static float INA700_getVoltage(u8 devAddr) {
    u8 txBuf[1] = {0x05}, rxBuf[2];
    if (ina700_i2cTransfer(devAddr, txBuf, 1, rxBuf, 2) != XST_SUCCESS) return 0;
    int raw = (rxBuf[0] << 8) | rxBuf[1];
    if (raw > 0x7FFF) return ((float)raw - 0x10000) * 0.003125;
    return (float)raw * 0.003125;
}

static float INA700_getCurrent(u8 devAddr) {
    u8 txBuf[1] = {0x07}, rxBuf[2];
    if (ina700_i2cTransfer(devAddr, txBuf, 1, rxBuf, 2) != XST_SUCCESS) return 0;
    int raw = (rxBuf[0] << 8) | rxBuf[1];
    if (raw > 0x7FFF) return ((float)raw - 0x10000) * 0.00048;
    return (float)raw * 0.00048;
}

/* Main Application */
int main() {
    xil_printf("INA700 Baremetal Test Started\r\n");

    if (ina700_init() != XST_SUCCESS) {
        xil_printf("INA700 init failed!\r\n");
        return -1;
    }

    xil_printf("\r\n%-10s | %-10s | %-10s | %-10s\r\n", "Iteration", "Rail", "Temp(°C)", "Power(W)");
    xil_printf("---------------------------------------------------------------\r\n");

    for (int i = 0; i < 10; i++) {
        float temp_vccint = INA700_getTemp(INA700_VCCINT_DEVADDR);
        float volt_vccint = INA700_getVoltage(INA700_VCCINT_DEVADDR);
        float curr_vccint = INA700_getCurrent(INA700_VCCINT_DEVADDR);
        float power_vccint = volt_vccint * curr_vccint;

        float temp_5v = INA700_getTemp(INA700_VCC5V_DEVADDR);
        float volt_5v = INA700_getVoltage(INA700_VCC5V_DEVADDR);
        float curr_5v = INA700_getCurrent(INA700_VCC5V_DEVADDR);
        float power_5v = volt_5v * curr_5v;

        xil_printf("%-10d | %-10s | %d.%02d      | %d.%03d\r\n",
            i+1, "VCCINT",
            (int)temp_vccint, (int)((temp_vccint - (int)temp_vccint)*100),
            (int)power_vccint, (int)((power_vccint - (int)power_vccint)*1000));

        xil_printf("%-10s | %-10s | %d.%02d      | %d.%03d\r\n",
            "", "5V",
            (int)temp_5v, (int)((temp_5v - (int)temp_5v)*100),
            (int)power_5v, (int)((power_5v - (int)power_5v)*1000));

        xil_printf("---------------------------------------------------------------\r\n");

        usleep(5000000); // 500ms delay
    }

    xil_printf("INA700 Baremetal test Completed.\r\n");
    return 0;
}
