#include "INA233.h"


uint16_t get_word(uint8_t address, uint8_t reg) {
    uint8_t data[2];
    int16_t rawVoltage;

    // Request data from the PMBus device
    Wire.beginTransmission(address);
    Wire.write(reg);
    Wire.endTransmission();
    Wire.requestFrom(address, 2);

    if (Wire.available() == 2) {
        data[0] = Wire.read();
        data[1] = Wire.read();
    } else {
        // return 0 on error
        return 0.0;
    }

    // Combine the two bytes into a single 16-bit value
    return (data[1] << 8) | data[0];
}


void sendWord(uint8_t deviceAddress, uint8_t registerAddress, uint16_t value) {
  Wire.beginTransmission(deviceAddress);
  Wire.write(registerAddress); // Send the register address
  Wire.write((uint8_t)value); // Send the low byte first
  Wire.write((uint8_t)(value >> 8)); // Send the high byte next
  Wire.endTransmission();
}

INA233::INA233(const uint8_t address, TwoWire *wire)
{
  _address     = address;
  _wire        = wire;
  //  not calibrated values by default.
  _current_LSB = 0;
  _maxCurrent  = 0;
  _shunt       = 0;
}

bool INA233::begin(const uint8_t sda, const uint8_t scl)
{
  _wire->begin(sda, scl);
  if (! isConnected()) return false;
  return true;
}

bool INA233::isConnected()
{
  _wire->beginTransmission(_address);
  return ( _wire->endTransmission() == 0);
}

float INA233::getBusVoltage() {
    uint8_t data[2];
    int16_t rawVoltage;

    // Request data from the PMBus device
    Wire.beginTransmission(INA233::_address);
    Wire.write(REGISTER_READ_VIN);
    Wire.endTransmission();
    Wire.requestFrom(INA233::_address, 2);

    if (Wire.available() == 2) {
        data[0] = Wire.read();
        data[1] = Wire.read();
    } else {
        // Handle error (e.g., return 0.0 or a special error value)
        return 0.0;
    }

    // Combine the two bytes into a single 16-bit value
    rawVoltage = (data[1] << 8) | data[0];

    // Convert raw value to actual voltage using the appropriate scaling
    // For example, assuming a linear scaling with a coefficient of 1.25 mV per LSB:
    float voltage = rawVoltage * 1.25 / 1000.0;

    return voltage;
}

float INA233::getShuntVoltage() {
    uint16_t rawVoltage = get_word(INA233::_address, REGISTER_READ_VSHUNT);
    float voltage = rawVoltage * pow(10,-4);
    return voltage;
}

float INA233::getShuntVoltage_mV() {
    return getShuntVoltage() * 1000;
}

void INA233::setAveragingMode(AveragingMode avg_mode) {
    uint8_t avg_bits = avg_mode;

    uint16_t old_conf = get_word(INA233::_address, REGISTER_CONFIGURATION);

    // Mask to clear bits 9, 10, and 11 (positions 9 to 11) in the original register
    uint16_t clear_mask = ~(0x7 << 9);
    old_conf &= clear_mask;

    // Prepare new bits: extract 3 bits from new_bits and shift them to position 9
    // 0x7 ensures only the last 3 bits are considered
    uint16_t set_bits = (avg_bits & 0x7) << 9;  
    
    // Set the new bits in the register
    old_conf |= set_bits;
    sendWord(INA233::_address, REGISTER_CONFIGURATION, old_conf);
};

void INA233::setBusVoltageConversionTime(ConversionTime conversion_time) {
    uint8_t conv_bits = conversion_time;

    uint16_t old_conf = get_word(INA233::_address, REGISTER_CONFIGURATION);

    // Mask to clear bits 9, 10, and 11 (positions 6 to 8) in the original register
    uint16_t clear_mask = ~(0x7 << 6);
    old_conf &= clear_mask;

    // Prepare new bits: extract 3 bits from new_bits and shift them to position 6
    // 0x7 ensures only the last 3 bits are considered
    uint16_t set_bits = (conv_bits & 0x7) << 6;  
    
    // Set the new bits in the register
    old_conf |= set_bits;
    sendWord(INA233::_address, REGISTER_CONFIGURATION, old_conf);
};

void INA233::setShuntVoltageConversionTime(ConversionTime conversion_time) {
    uint8_t conv_bits = conversion_time;

    uint16_t old_conf = get_word(INA233::_address, REGISTER_CONFIGURATION);

    // Mask to clear bits 9, 10, and 11 (positions 3 to 5) in the original register
    uint16_t clear_mask = ~(0x7 << 3);
    old_conf &= clear_mask;

    // Prepare new bits: extract 3 bits from new_bits and shift them to position 3
    // 0x7 ensures only the last 3 bits are considered
    uint16_t set_bits = (conv_bits & 0x7) << 3;  
    
    // Set the new bits in the register
    old_conf |= set_bits;
    sendWord(INA233::_address, REGISTER_CONFIGURATION, old_conf);
};

uint16_t INA233::getConfigRegister() {
    return get_word(INA233::_address, REGISTER_CONFIGURATION);
}