#include "../../Library/DS_INA226/Inc/DS_INA226.h"

void DS_INA226_Init(DS_INA226 *DS_INA226, I2C_HandleTypeDef *hi2c, uint8_t DevAddr)
{
    DS_INA226->DevAddr = DevAddr;
    DS_INA226->hi2c = hi2c;
    DS_INA226->ShuntResistance = 100;       // 0.1 Ohm
                                            // Current_LSB = Maximum Expected Current / 32768
    DS_INA226->Current_LSB = (float)1/32768;       // for 1 Ampere



    // _current_lsb = _i_max / 32768

    DS_INA226->SensorData[0] = 0;
    DS_INA226->SensorData[1] = 0;
    DS_INA226->SensorData[2] = 0;
    DS_INA226->SensorData[3] = 0;
    DS_INA226->SensorData[4] = 0;
    DS_INA226->SensorData[5] = 0;
    DS_INA226->SensorData[6] = 0;
    DS_INA226->SensorData[7] = 0;
    //Die ID
    DS_INA226->SensorData[8] = 0;

    HAL_I2C_Mem_Read(DS_INA226->hi2c, DS_INA226->DevAddr << 1, 0, 1, (uint8_t*)(&DS_INA226->SensorData[0]), 2, 100);
    HAL_I2C_Mem_Read(DS_INA226->hi2c, DS_INA226->DevAddr << 1, 1, 1, (uint8_t*)(&DS_INA226->SensorData[1]), 2, 100);
    HAL_I2C_Mem_Read(DS_INA226->hi2c, DS_INA226->DevAddr << 1, 2, 1, (uint8_t*)(&DS_INA226->SensorData[2]), 2, 100);
    HAL_I2C_Mem_Read(DS_INA226->hi2c, DS_INA226->DevAddr << 1, 3, 1, (uint8_t*)(&DS_INA226->SensorData[3]), 2, 100);
    HAL_I2C_Mem_Read(DS_INA226->hi2c, DS_INA226->DevAddr << 1, 4, 1, (uint8_t*)(&DS_INA226->SensorData[4]), 2, 100);
    HAL_I2C_Mem_Read(DS_INA226->hi2c, DS_INA226->DevAddr << 1, 5, 1, (uint8_t*)(&DS_INA226->SensorData[5]), 2, 100);
    HAL_I2C_Mem_Read(DS_INA226->hi2c, DS_INA226->DevAddr << 1, 6, 1, (uint8_t*)(&DS_INA226->SensorData[6]), 2, 100);
    HAL_I2C_Mem_Read(DS_INA226->hi2c, DS_INA226->DevAddr << 1, 7, 1, (uint8_t*)(&DS_INA226->SensorData[7]), 2, 100);
    HAL_I2C_Mem_Read(DS_INA226->hi2c, DS_INA226->DevAddr << 1, 0xFF, 1, (uint8_t*)(&DS_INA226->SensorData[8]), 2, 100);

    //Set Calibration Register
    uint16_t CAL = 0.00512 / (DS_INA226->Current_LSB  * (DS_INA226->ShuntResistance/100));
    uint16_t temp=0;
    temp = temp|CAL<<8;
    temp = temp|CAL>>8;
    HAL_I2C_Mem_Write(DS_INA226->hi2c, DS_INA226->DevAddr<<1, 5, 1, (uint8_t*)(&temp), 2, 100);
}

void _DS_INA226_SetAveragingMode(DS_INA226 *DS_INA226, uint8_t AveragingMode)
{
    DS_INA226->SensorData[0] = DS_INA226->SensorData[0]|AveragingMode<<1;
    HAL_I2C_Mem_Write(DS_INA226->hi2c, DS_INA226->DevAddr<<1, 0, 1, (uint8_t*)(&DS_INA226->SensorData[0]), 2, 100);
}

void _DS_INA226_SetBusVoltageConversionTime(DS_INA226 *DS_INA226, uint8_t ConversionTime)
{
    //TODO need fix!

    DS_INA226->SensorData[0] = DS_INA226->SensorData[0]|ConversionTime;
    DS_INA226->SensorData[0] = DS_INA226->SensorData[0]|ConversionTime;

    HAL_I2C_Mem_Write(DS_INA226->hi2c, DS_INA226->DevAddr<<1, 0, 1, (uint8_t*)(&DS_INA226->SensorData[0]), 2, 100);
}

void _DS_INA226_SetShuntVoltageConversionTime(DS_INA226 *DS_INA226, uint8_t ConversionTime)
{
    //TODO need check!
    DS_INA226->SensorData[0] = DS_INA226->SensorData[0]|ConversionTime<<11;
    HAL_I2C_Mem_Write(DS_INA226->hi2c, DS_INA226->DevAddr<<1, 0, 1, (uint8_t*)(&DS_INA226->SensorData[0]), 2, 100);
}

void _DS_INA226_SetOperatingMode(DS_INA226 *DS_INA226, uint8_t OperatingMode)
{
    //TODO need check!
    DS_INA226->SensorData[0] = DS_INA226->SensorData[0]|OperatingMode<<8;
    HAL_I2C_Mem_Write(DS_INA226->hi2c, DS_INA226->DevAddr<<1, 0, 1, (uint8_t*)(&DS_INA226->SensorData[0]), 2, 100);
}

void _DS_INA226_GetAllMemory(DS_INA226 *DS_INA226)
{
    HAL_I2C_Mem_Read(DS_INA226->hi2c, DS_INA226->DevAddr << 1, 0, 1, (uint8_t*)(&DS_INA226->SensorData[0]), 2, 100);
    HAL_I2C_Mem_Read(DS_INA226->hi2c, DS_INA226->DevAddr << 1, 1, 1, (uint8_t*)(&DS_INA226->SensorData[1]), 2, 100);
    HAL_I2C_Mem_Read(DS_INA226->hi2c, DS_INA226->DevAddr << 1, 2, 1, (uint8_t*)(&DS_INA226->SensorData[2]), 2, 100);
    HAL_I2C_Mem_Read(DS_INA226->hi2c, DS_INA226->DevAddr << 1, 3, 1, (uint8_t*)(&DS_INA226->SensorData[3]), 2, 100);
    HAL_I2C_Mem_Read(DS_INA226->hi2c, DS_INA226->DevAddr << 1, 4, 1, (uint8_t*)(&DS_INA226->SensorData[4]), 2, 100);
    HAL_I2C_Mem_Read(DS_INA226->hi2c, DS_INA226->DevAddr << 1, 5, 1, (uint8_t*)(&DS_INA226->SensorData[5]), 2, 100);
    HAL_I2C_Mem_Read(DS_INA226->hi2c, DS_INA226->DevAddr << 1, 6, 1, (uint8_t*)(&DS_INA226->SensorData[6]), 2, 100);
    HAL_I2C_Mem_Read(DS_INA226->hi2c, DS_INA226->DevAddr << 1, 7, 1, (uint8_t*)(&DS_INA226->SensorData[7]), 2, 100);
    HAL_I2C_Mem_Read(DS_INA226->hi2c, DS_INA226->DevAddr << 1, 0xFF, 1, (uint8_t*)(&DS_INA226->SensorData[8]), 2, 100);
}


uint16_t DS_INA226_GetShuntVoltage(DS_INA226 *DS_INA226)
{
    //μV

    HAL_I2C_Mem_Read(DS_INA226->hi2c, DS_INA226->DevAddr << 1, 1, 1, (uint8_t*)(&DS_INA226->SensorData[1]), 2, 100);
    uint16_t temp=0;
    temp = temp|((uint8_t)DS_INA226->SensorData[1])<<8;
    temp = temp|DS_INA226->SensorData[1]>>8;
    temp = temp * 25 / 10;
    return temp;
}

uint16_t DS_INA226_GetBusVoltage(DS_INA226 *DS_INA226)
{
    //mV

    HAL_I2C_Mem_Read(DS_INA226->hi2c, DS_INA226->DevAddr << 1, 2, 1, (uint8_t*)(&DS_INA226->SensorData[2]), 2, 100);
    uint16_t temp=0;
    temp = temp|((uint8_t)DS_INA226->SensorData[2])<<8;
    temp = temp|DS_INA226->SensorData[2]>>8;
    temp = temp * 125 / 100;
    return temp;
}

float DS_INA226_GetPower(DS_INA226 *DS_INA226)
{
    HAL_I2C_Mem_Read(DS_INA226->hi2c, DS_INA226->DevAddr << 1, 3, 1, (uint8_t*)(&DS_INA226->SensorData[3]), 2, 100);
    uint16_t temp=0;
    temp = temp|((uint8_t)DS_INA226->SensorData[3])<<8;
    temp = temp|DS_INA226->SensorData[3]>>8;
//    temp = temp * DS_INA226->Current_LSB;
    return temp * DS_INA226->Current_LSB;
}

float DS_INA226_GetCurrent(DS_INA226 *DS_INA226)
{
    HAL_I2C_Mem_Read(DS_INA226->hi2c, DS_INA226->DevAddr << 1, 4, 1, (uint8_t*)(&DS_INA226->SensorData[4]), 2, 100);
    uint16_t temp=0;
    temp = temp|((uint8_t)DS_INA226->SensorData[4])<<8;
    temp = temp|DS_INA226->SensorData[4]>>8;
//    temp = temp * (DS_INA226->Current_LSB);
    return temp * (DS_INA226->Current_LSB);
}