#include "IM1281C.h"

IM1281C::IM1281C()
    : _serial(nullptr),
      _slaveAddress(1),
      _lastAResult(0xFF),
      _lastBResult(0xFF)
{
}

bool IM1281C::begin(HardwareSerial &serial,
                    int8_t rxPin,
                    int8_t txPin,
                    uint8_t slaveAddress,
                    uint32_t baudRate,
                    uint32_t serialConfig)
{
  _serial = &serial;
  _slaveAddress = slaveAddress;
  _a = IM1281CAData{};
  _b = IM1281CBData{};
  _lastAResult = 0xFF;
  _lastBResult = 0xFF;

  _serial->begin(baudRate, serialConfig, rxPin, txPin);
  _node.begin(_slaveAddress, *_serial);
  return true;
}

bool IM1281C::readA()
{
  return readAll();
}

bool IM1281C::readB()
{
  return readAll();
}

bool IM1281C::readAll()
{
  if (_serial == nullptr)
  {
    return false;
  }

  // IM1281C datasheet addresses are effectively 32-bit data items.
  // Requesting 0x0010 from 0x0048 yields 64 data bytes (0x40), which covers A/B block.
  const uint8_t result = _node.readHoldingRegisters(0x0048, 16);
  _lastAResult = result;
  _lastBResult = result;

  if (result != _node.ku8MBSuccess)
  {
    _a.valid = false;
    _b.valid = false;
    return false;
  }

  const uint32_t aVoltageRaw = combineWords(_node.getResponseBuffer(0), _node.getResponseBuffer(1));
  const uint32_t aCurrentRaw = combineWords(_node.getResponseBuffer(2), _node.getResponseBuffer(3));
  const uint32_t aPowerRaw = combineWords(_node.getResponseBuffer(4), _node.getResponseBuffer(5));
  const uint32_t aEnergyRaw = combineWords(_node.getResponseBuffer(6), _node.getResponseBuffer(7));
  const uint32_t aPfRaw = combineWords(_node.getResponseBuffer(8), _node.getResponseBuffer(9));
  const uint32_t aCo2Raw = combineWords(_node.getResponseBuffer(10), _node.getResponseBuffer(11));
  const uint32_t aTempRaw = combineWords(_node.getResponseBuffer(12), _node.getResponseBuffer(13));
  const uint32_t aFreqRaw = combineWords(_node.getResponseBuffer(14), _node.getResponseBuffer(15));

  _a.voltage = scaleValue(aVoltageRaw, 0.0001f);
  _a.current = scaleValue(aCurrentRaw, 0.0001f);
  _a.power = scaleValue(aPowerRaw, 0.0001f);
  _a.energy = scaleValue(aEnergyRaw, 0.0001f);
  _a.powerFactor = scaleValue(aPfRaw, 0.001f);
  _a.co2 = scaleValue(aCo2Raw, 0.0001f);
  _a.temperature = scaleValue(aTempRaw, 0.01f);
  _a.frequency = scaleValue(aFreqRaw, 0.01f);
  _a.valid = true;

  const uint32_t bVoltageRaw = combineWords(_node.getResponseBuffer(16), _node.getResponseBuffer(17));
  const uint32_t bCurrentRaw = combineWords(_node.getResponseBuffer(18), _node.getResponseBuffer(19));
  const uint32_t bPowerRaw = combineWords(_node.getResponseBuffer(20), _node.getResponseBuffer(21));
  const uint32_t bEnergyRaw = combineWords(_node.getResponseBuffer(22), _node.getResponseBuffer(23));
  const uint32_t bPfRaw = combineWords(_node.getResponseBuffer(24), _node.getResponseBuffer(25));
  const uint32_t bCo2Raw = combineWords(_node.getResponseBuffer(26), _node.getResponseBuffer(27));

  _b.voltage = scaleValue(bVoltageRaw, 0.0001f);
  _b.current = scaleValue(bCurrentRaw, 0.0001f);
  _b.power = scaleValue(bPowerRaw, 0.0001f);
  _b.energy = scaleValue(bEnergyRaw, 0.0001f);
  _b.powerFactor = scaleValue(bPfRaw, 0.001f);
  _b.co2 = scaleValue(bCo2Raw, 0.0001f);
  _b.valid = true;

  return true;
}

const IM1281CAData &IM1281C::a() const
{
  return _a;
}

const IM1281CBData &IM1281C::b() const
{
  return _b;
}

uint8_t IM1281C::lastAResult() const
{
  return _lastAResult;
}

uint8_t IM1281C::lastBResult() const
{
  return _lastBResult;
}

uint32_t IM1281C::combineWords(uint16_t highWord, uint16_t lowWord)
{
  return (static_cast<uint32_t>(highWord) << 16) | static_cast<uint32_t>(lowWord);
}

float IM1281C::scaleValue(uint32_t raw, float scale)
{
  return static_cast<float>(raw) * scale;
}
