#include "SpiTestClass.h"

#include <fcntl.h>
#include <fsfw/globalfunctions/arrayprinter.h>
#include <fsfw/serviceinterface/ServiceInterface.h>
#include <fsfw/tasks/TaskFactory.h>
#include <fsfw/timemanager/Stopwatch.h>
#include <fsfw_hal/common/gpio/GpioCookie.h>
#include <fsfw_hal/common/gpio/gpioDefinitions.h>
#include <fsfw_hal/linux/UnixFileGuard.h>
#include <fsfw_hal/linux/utility.h>
#include <linux/spi/spidev.h>
#include <sys/ioctl.h>
#include <unistd.h>

#include <bitset>

#include "busConf.h"
#include "devices/gpioIds.h"
#include "mission/devices/max1227.h"

SpiTestClass::SpiTestClass(object_id_t objectId, GpioIF *gpioIF)
    : TestTask(objectId), gpioIF(gpioIF) {
  if (gpioIF == nullptr) {
    sif::error << "SpiTestClass::SpiTestClass: Invalid GPIO ComIF!" << std::endl;
  }
  testMode = TestModes::SUS_0;
  spiTransferStruct[0].rx_buf = reinterpret_cast<__u64>(recvBuffer.data());
  spiTransferStruct[0].tx_buf = reinterpret_cast<__u64>(sendBuffer.data());
}

ReturnValue_t SpiTestClass::performOneShotAction() {
  switch (testMode) {
    case (TestModes::NONE): {
      break;
    }
    case (TestModes::MGM_LIS3MDL): {
      performLis3MdlTest(mgm0Lis3mdlChipSelect);
      break;
    }
    case (TestModes::MGM_RM3100): {
      performRm3100Test(mgm1Rm3100ChipSelect);
      break;
    }
    case (TestModes::GYRO_L3GD20H): {
      performL3gTest(gyro1L3gd20ChipSelect);
      break;
    }
    case (TestModes::SUS_0): {
      performMax1227Test();
      break;
    }
  }
  return HasReturnvaluesIF::RETURN_OK;
}

ReturnValue_t SpiTestClass::performPeriodicAction() { return HasReturnvaluesIF::RETURN_OK; }

void SpiTestClass::performRm3100Test(uint8_t mgmId) {
  /* Configure all SPI chip selects and pull them high */
  acsInit();

  /* Adapt accordingly */
  if (mgmId != mgm1Rm3100ChipSelect and mgmId != mgm3Rm3100ChipSelect) {
    sif::warning << "SpiTestClass::performRm3100Test: Invalid MGM ID!" << std::endl;
  }
  gpioId_t currentGpioId = 0;
  uint8_t chipSelectPin = mgmId;
  if (chipSelectPin == mgm1Rm3100ChipSelect) {
    currentGpioId = gpioIds::MGM_1_RM3100_CS;
  } else {
    currentGpioId = gpioIds::MGM_3_RM3100_CS;
  }
  uint32_t rm3100speed = 976'000;
  uint8_t rm3100revidReg = 0x36;
  spi::SpiModes rm3100mode = spi::SpiModes::MODE_3;

#ifdef RASPBERRY_PI
  std::string deviceName = "/dev/spidev0.0";
#else
  std::string deviceName = "/dev/spidev2.0";
#endif
  int fileDescriptor = 0;

  UnixFileGuard fileHelper(deviceName, &fileDescriptor, O_RDWR, "SpiComIF::initializeInterface");
  if (fileHelper.getOpenResult()) {
    sif::error << "SpiTestClass::performRm3100Test: File descriptor could not be opened!"
               << std::endl;
    return;
  }
  setSpiSpeedAndMode(fileDescriptor, rm3100mode, rm3100speed);

  uint8_t revId = readRegister(fileDescriptor, currentGpioId, rm3100revidReg);
  sif::info << "SpiTestClass::performRm3100Test: Revision ID 0b" << std::bitset<8>(revId)
            << std::endl;

  /* Write configuration to CMM register */
  writeRegister(fileDescriptor, currentGpioId, 0x01, 0x75);
  uint8_t cmmRegister = readRm3100Register(fileDescriptor, currentGpioId, 0x01);
  sif::info << "SpiTestClass::performRm3100Test: CMM register value: " << std::hex << "0x"
            << static_cast<int>(cmmRegister) << std::dec << std::endl;

  /* Read the cycle count registers */
  uint8_t cycleCountsRaw[6];
  readMultipleRegisters(fileDescriptor, currentGpioId, 0x04, cycleCountsRaw, 6);

  uint16_t cycleCountX = cycleCountsRaw[0] << 8 | cycleCountsRaw[1];
  uint16_t cycleCountY = cycleCountsRaw[2] << 8 | cycleCountsRaw[3];
  uint16_t cycleCountZ = cycleCountsRaw[4] << 8 | cycleCountsRaw[5];

  sif::info << "Cycle count X: " << cycleCountX << std::endl;
  sif::info << "Cycle count Y: " << cycleCountY << std::endl;
  sif::info << "Cycle count z: " << cycleCountZ << std::endl;

  writeRegister(fileDescriptor, currentGpioId, 0x0B, 0x96);
  uint8_t tmrcReg = readRm3100Register(fileDescriptor, currentGpioId, 0x0B);
  sif::info << "SpiTestClass::performRm3100Test: TMRC register value: " << std::hex << "0x"
            << static_cast<int>(tmrcReg) << std::dec << std::endl;

  TaskFactory::delayTask(10);
  uint8_t statusReg = readRm3100Register(fileDescriptor, currentGpioId, 0x34);
  sif::info << "SpiTestClass::performRm3100Test: Status Register 0b" << std::bitset<8>(statusReg)
            << std::endl;
  /* This means that data is not ready */
  if ((statusReg & 0b1000'0000) == 0) {
    sif::warning << "SpiTestClass::performRm3100Test: Data not ready!" << std::endl;
    TaskFactory::delayTask(10);
    uint8_t statusReg = readRm3100Register(fileDescriptor, currentGpioId, 0x34);
    if ((statusReg & 0b1000'0000) == 0) {
      return;
    }
  }

  uint32_t rm3100DefaultCycleCout = 0xC8;
  /* Gain scales lineary with cycle count and is 38 for cycle count 100 */
  float rm3100Gain = rm3100DefaultCycleCout / 100.0 * 38.0;
  float scaleFactor = 1 / rm3100Gain;
  uint8_t rawValues[9];
  readMultipleRegisters(fileDescriptor, currentGpioId, 0x24, rawValues, 9);

  /* The sensor generates 24 bit signed values */
  int32_t rawX = ((rawValues[0] << 24) | (rawValues[1] << 16) | (rawValues[2] << 8)) >> 8;
  int32_t rawY = ((rawValues[3] << 24) | (rawValues[4] << 16) | (rawValues[5] << 8)) >> 8;
  int32_t rawZ = ((rawValues[6] << 24) | (rawValues[7] << 16) | (rawValues[8] << 8)) >> 8;

  float fieldStrengthX = rawX * scaleFactor;
  float fieldStrengthY = rawY * scaleFactor;
  float fieldStrengthZ = rawZ * scaleFactor;

  sif::info << "RM3100 measured field strengths in microtesla:" << std::endl;
  sif::info << "Field Strength X: " << fieldStrengthX << " uT" << std::endl;
  sif::info << "Field Strength Y: " << fieldStrengthY << " uT" << std::endl;
  sif::info << "Field Strength Z: " << fieldStrengthZ << " uT" << std::endl;
}

void SpiTestClass::performLis3MdlTest(uint8_t lis3Id) {
  /* Configure all SPI chip selects and pull them high */
  acsInit();

  /* Adapt accordingly */
  if (lis3Id != mgm0Lis3mdlChipSelect and lis3Id != mgm2Lis3mdlChipSelect) {
    sif::warning << "SpiTestClass::performLis3MdlTest: Invalid MGM ID!" << std::endl;
  }
  gpioId_t currentGpioId = 0;
  uint8_t chipSelectPin = lis3Id;
  uint8_t whoAmIReg = 0b0000'1111;
  uint8_t whoAmIRegExpectedVal = 0b0011'1101;
  if (chipSelectPin == mgm0Lis3mdlChipSelect) {
    currentGpioId = gpioIds::MGM_0_LIS3_CS;
  } else {
    currentGpioId = gpioIds::MGM_2_LIS3_CS;
  }
  uint32_t spiSpeed = 10'000'000;
  spi::SpiModes spiMode = spi::SpiModes::MODE_0;
#ifdef RASPBERRY_PI
  std::string deviceName = "/dev/spidev0.0";
#else
  std::string deviceName = "/dev/spidev2.0";
#endif
  int fileDescriptor = 0;

  UnixFileGuard fileHelper(deviceName, &fileDescriptor, O_RDWR, "SpiComIF::initializeInterface");
  if (fileHelper.getOpenResult()) {
    sif::error << "SpiTestClass::performLis3Mdl3100Test: File descriptor could not be opened!"
               << std::endl;
    return;
  }
  setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
  spiTransferStruct[0].delay_usecs = 0;

  uint8_t whoAmIRegVal = readStmRegister(fileDescriptor, currentGpioId, whoAmIReg, false);
  sif::info << "SpiTestClass::performLis3MdlTest: WHO AM I register 0b"
            << std::bitset<8>(whoAmIRegVal) << std::endl;
  if (whoAmIRegVal != whoAmIRegExpectedVal) {
    sif::warning << "SpiTestClass::performLis3MdlTest: WHO AM I register invalid!" << std::endl;
  }
}

void SpiTestClass::performL3gTest(uint8_t l3gId) {
  /* Configure all SPI chip selects and pull them high */
  acsInit();

  gpioId_t currentGpioId = 0;
  uint8_t chipSelectPin = l3gId;
  uint8_t whoAmIReg = 0b0000'1111;
  uint8_t whoAmIRegExpectedVal = 0b1101'0111;

  if (chipSelectPin == gyro1L3gd20ChipSelect) {
    currentGpioId = gpioIds::GYRO_1_L3G_CS;
  } else {
    currentGpioId = gpioIds::GYRO_3_L3G_CS;
  }
  uint32_t spiSpeed = 3'900'000;
  spi::SpiModes spiMode = spi::SpiModes::MODE_3;
#ifdef RASPBERRY_PI
  std::string deviceName = "/dev/spidev0.0";
#else
  std::string deviceName = "/dev/spidev2.0";
#endif
  int fileDescriptor = 0;

  UnixFileGuard fileHelper(deviceName, &fileDescriptor, O_RDWR, "SpiComIF::initializeInterface");
  if (fileHelper.getOpenResult()) {
    sif::error << "SpiTestClass::performLis3Mdl3100Test: File descriptor could not be opened!"
               << std::endl;
    return;
  }
  setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
  uint8_t whoAmIRegVal = readStmRegister(fileDescriptor, currentGpioId, whoAmIReg, false);
  sif::info << "SpiTestClass::performLis3MdlTest: WHO AM I register 0b"
            << std::bitset<8>(whoAmIRegVal) << std::endl;
  if (whoAmIRegVal != whoAmIRegExpectedVal) {
    sif::warning << "SpiTestClass::performL3gTest: Read WHO AM I register invalid!" << std::endl;
  }

  uint8_t ctrlReg1Addr = 0b0010'0000;
  {
    uint8_t commandRegs[5];
    commandRegs[0] = 0b0000'1111;
    commandRegs[1] = 0x0;
    commandRegs[2] = 0x0;
    /* Configure big endian data format */
    commandRegs[3] = 0b0100'0000;
    commandRegs[4] = 0x0;
    writeMultipleStmRegisters(fileDescriptor, currentGpioId, ctrlReg1Addr, commandRegs,
                              sizeof(commandRegs));
    uint8_t readRegs[5];
    readMultipleRegisters(fileDescriptor, currentGpioId, ctrlReg1Addr, readRegs, sizeof(readRegs));
    for (uint8_t idx = 0; idx < sizeof(readRegs); idx++) {
      if (readRegs[idx] != commandRegs[0]) {
        sif::warning << "SpiTestClass::performL3gTest: Read control register "
                     << static_cast<int>(idx + 1) << " not equal to configured value" << std::endl;
      }
    }
  }

  uint8_t readOutBuffer[14];
  readMultipleStmRegisters(fileDescriptor, currentGpioId, ctrlReg1Addr, readOutBuffer,
                           sizeof(readOutBuffer));

  uint8_t statusReg = readOutBuffer[7];
  sif::info << "SpiTestClass::performL3gTest: Status Register 0b" << std::bitset<8>(statusReg)
            << std::endl;

  uint16_t l3gRange = 245;
  float scaleFactor = static_cast<float>(l3gRange) / INT16_MAX;
  /* The sensor spits out little endian */
  int16_t angVelocRawX = (readOutBuffer[8] << 8) | readOutBuffer[9];
  int16_t angVelocRawY = (readOutBuffer[10] << 8) | readOutBuffer[11];
  int16_t angVelocRawZ = (readOutBuffer[12] << 8) | readOutBuffer[13];

  float angVelocX = scaleFactor * angVelocRawX;
  float angVelocY = scaleFactor * angVelocRawY;
  float angVelocZ = scaleFactor * angVelocRawZ;

  sif::info << "Angular velocities for the L3GD20H in degrees per second:" << std::endl;
  sif::info << "X: " << angVelocX << std::endl;
  sif::info << "Y: " << angVelocY << std::endl;
  sif::info << "Z: " << angVelocZ << std::endl;
}

void SpiTestClass::performMax1227Test() {
  using namespace max1227;
  bool testRadSensorExtConv = false;
  bool testRadSensorIntConv = false;
  bool extConversion = false;
  bool intConversion = true;
#ifdef XIPHOS_Q7S
  std::string deviceName = q7s::SPI_DEFAULT_DEV;
#elif defined(RASPBERRY_PI)
  std::string deviceName = "";
#endif
  int fd = 0;
  int retval = 0;
  UnixFileGuard fileHelper(deviceName, &fd, O_RDWR, "SpiComIF::initializeInterface");
  if (fileHelper.getOpenResult()) {
    sif::error << "SpiTestClass::performLis3Mdl3100Test: File descriptor could not be opened!"
               << std::endl;
    return;
  }
  uint32_t spiSpeed = 1'000'000;
  spi::SpiModes spiMode = spi::SpiModes::MODE_3;
  setSpiSpeedAndMode(fd, spiMode, spiSpeed);

  if (testRadSensorExtConv) {
    sendBuffer[0] = max1227::buildResetByte(true);
    sendBuffer[1] = max1227::buildSetupByte(ClkSel::EXT_CONV_EXT_TIMED, RefSel::INT_REF_NO_WAKEUP,
                                            DiffSel::NONE_0);
    spiTransferStruct[0].len = 2;
    ReturnValue_t result = gpioIF->pullLow(gpioIds::CS_RAD_SENSOR);
    if (result != HasReturnvaluesIF::RETURN_OK) {
    }
    retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct);
    if (retval < 0) {
      utility::handleIoctlError("SpiTestClass::writeRegister: Write failed");
    }

    max1227::prepareExternallyClockedRead0ToN(sendBuffer.data(), 7, spiTransferStruct[0].len);
    result = gpioIF->pullLow(gpioIds::CS_RAD_SENSOR);
    if (result != HasReturnvaluesIF::RETURN_OK) {
    }
    retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct);
    if (retval < 0) {
      utility::handleIoctlError("SpiTestClass::writeRegister: Write failed");
    }
    result = gpioIF->pullHigh(gpioIds::CS_RAD_SENSOR);
    if (result != HasReturnvaluesIF::RETURN_OK) {
    }
    arrayprinter::print(recvBuffer.data(), 13, OutputType::HEX);
    uint16_t adcRaw[8] = {};
    adcRaw[0] = (recvBuffer[1] << 8) | recvBuffer[2];
    adcRaw[1] = (recvBuffer[3] << 8) | recvBuffer[4];
    adcRaw[2] = (recvBuffer[5] << 8) | recvBuffer[6];
    adcRaw[3] = (recvBuffer[7] << 8) | recvBuffer[8];
    adcRaw[4] = (recvBuffer[9] << 8) | recvBuffer[10];
    adcRaw[5] = (recvBuffer[11] << 8) | recvBuffer[12];
    adcRaw[6] = (recvBuffer[13] << 8) | recvBuffer[14];
    adcRaw[7] = (recvBuffer[15] << 8) | recvBuffer[16];
    arrayprinter::print(recvBuffer.data(), 17, OutputType::HEX);
    for (int idx = 0; idx < 8; idx++) {
      sif::info << "ADC raw " << idx << ": " << adcRaw[idx] << std::endl;
    }
  }
  if (testRadSensorIntConv) {
    sendBuffer[0] = max1227::buildResetByte(false);
    spiTransferStruct[0].len = 1;
    ReturnValue_t result = gpioIF->pullLow(gpioIds::CS_RAD_SENSOR);
    if (result != HasReturnvaluesIF::RETURN_OK) {
    }
    retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct);
    if (retval < 0) {
      utility::handleIoctlError("SpiTestClass::writeRegister: Write failed");
    }
    result = gpioIF->pullHigh(gpioIds::CS_RAD_SENSOR);
    if (result != HasReturnvaluesIF::RETURN_OK) {
    }
    usleep(65);

    // Now use internal conversion
    sendBuffer[0] = max1227::buildSetupByte(ClkSel::INT_CONV_INT_TIMED_CNVST_AS_AIN,
                                            RefSel::INT_REF_NO_WAKEUP, DiffSel::NONE_0);
    spiTransferStruct[0].len = 1;
    result = gpioIF->pullLow(gpioIds::CS_RAD_SENSOR);
    if (result != HasReturnvaluesIF::RETURN_OK) {
    }
    retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct);
    if (retval < 0) {
      utility::handleIoctlError("SpiTestClass::writeRegister: Write failed");
    }
    result = gpioIF->pullHigh(gpioIds::CS_RAD_SENSOR);
    if (result != HasReturnvaluesIF::RETURN_OK) {
    }
    usleep(10);
    sendBuffer[0] = buildConvByte(ScanModes::CHANNELS_0_TO_N, 7, true);
    spiTransferStruct[0].len = 1;
    result = gpioIF->pullLow(gpioIds::CS_RAD_SENSOR);
    if (result != HasReturnvaluesIF::RETURN_OK) {
    }
    retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct);
    if (retval < 0) {
      utility::handleIoctlError("SpiTestClass::writeRegister: Write failed");
    }
    result = gpioIF->pullHigh(gpioIds::CS_RAD_SENSOR);
    if (result != HasReturnvaluesIF::RETURN_OK) {
    }

    usleep(65);
    spiTransferStruct[0].len = 18;
    // Shift out zeros
    spiTransferStruct[0].tx_buf = 0;
    result = gpioIF->pullLow(gpioIds::CS_RAD_SENSOR);
    if (result != HasReturnvaluesIF::RETURN_OK) {
    }
    retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct);
    if (retval < 0) {
      utility::handleIoctlError("SpiTestClass::writeRegister: Write failed");
    }
    result = gpioIF->pullHigh(gpioIds::CS_RAD_SENSOR);
    if (result != HasReturnvaluesIF::RETURN_OK) {
    }

    arrayprinter::print(recvBuffer.data(), 14);
    uint16_t adcRaw[8] = {};
    int16_t tempRaw = ((recvBuffer[0] & 0x0f) << 8) | recvBuffer[1];

    sif::info << "Temperature: " << tempRaw * 0.125 << " C" << std::endl;
    adcRaw[0] = (recvBuffer[2] << 8) | recvBuffer[3];
    adcRaw[1] = (recvBuffer[4] << 8) | recvBuffer[5];
    adcRaw[2] = (recvBuffer[6] << 8) | recvBuffer[7];
    adcRaw[3] = (recvBuffer[8] << 8) | recvBuffer[9];
    adcRaw[4] = (recvBuffer[10] << 8) | recvBuffer[11];
    adcRaw[5] = (recvBuffer[12] << 8) | recvBuffer[13];
    adcRaw[6] = (recvBuffer[14] << 8) | recvBuffer[15];
    adcRaw[7] = (recvBuffer[16] << 8) | recvBuffer[17];
    for (int idx = 0; idx < 8; idx++) {
      sif::info << "ADC raw " << idx << ": " << adcRaw[idx] << std::endl;
    }
  }
  if (extConversion) {
    sendBuffer[0] = max1227::buildResetByte(false);
    spiTransferStruct[0].len = 1;
    retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct);
    if (retval < 0) {
      utility::handleIoctlError("SpiTestClass::writeRegister: Write failed");
    }

    usleep(65);
    sendBuffer[0] = max1227::buildSetupByte(ClkSel::EXT_CONV_EXT_TIMED, RefSel::INT_REF_NO_WAKEUP,
                                            DiffSel::NONE_0);
    spiTransferStruct[0].len = 1;
    retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct);
    if (retval < 0) {
      utility::handleIoctlError("SpiTestClass::writeRegister: Write failed");
    }

    max1227::prepareExternallyClockedRead0ToN(sendBuffer.data(), 5, spiTransferStruct[0].len);
    retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct);
    if (retval < 0) {
      utility::handleIoctlError("SpiTestClass::writeRegister: Write failed");
    }
    uint16_t adcRaw[6] = {};
    adcRaw[0] = (recvBuffer[1] << 8) | recvBuffer[2];
    adcRaw[1] = (recvBuffer[3] << 8) | recvBuffer[4];
    adcRaw[2] = (recvBuffer[5] << 8) | recvBuffer[6];
    adcRaw[3] = (recvBuffer[7] << 8) | recvBuffer[8];
    adcRaw[4] = (recvBuffer[9] << 8) | recvBuffer[10];
    adcRaw[5] = (recvBuffer[11] << 8) | recvBuffer[12];
    arrayprinter::print(recvBuffer.data(), 13, OutputType::HEX);
    for (int idx = 0; idx < 6; idx++) {
      sif::info << "ADC raw " << idx << ": " << adcRaw[idx] << std::endl;
    }
  }
  if (intConversion) {
    sendBuffer[0] = max1227::buildResetByte(false);
    spiTransferStruct[0].len = 1;
    retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct);
    if (retval < 0) {
      utility::handleIoctlError("SpiTestClass::writeRegister: Write failed");
    }

    usleep(65);
    // Now use internal conversion
    sendBuffer[0] = max1227::buildSetupByte(ClkSel::INT_CONV_INT_TIMED_CNVST_AS_AIN,
                                            RefSel::INT_REF_NO_WAKEUP, DiffSel::NONE_0);
    spiTransferStruct[0].len = 1;
    retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct);
    if (retval < 0) {
      utility::handleIoctlError("SpiTestClass::writeRegister: Write failed");
    }
    usleep(10);
    sendBuffer[0] = buildConvByte(ScanModes::CHANNELS_0_TO_N, 5, true);
    spiTransferStruct[0].len = 1;
    retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct);
    if (retval < 0) {
      utility::handleIoctlError("SpiTestClass::writeRegister: Write failed");
    }

    usleep(65);
    spiTransferStruct[0].len = 14;
    // Shift out zeros
    spiTransferStruct[0].tx_buf = 0;
    retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct);
    if (retval < 0) {
      utility::handleIoctlError("SpiTestClass::writeRegister: Write failed");
    }
    arrayprinter::print(recvBuffer.data(), 14);
    float temp = static_cast<int16_t>(((recvBuffer[0] & 0x0f) << 8) | recvBuffer[1]) * 0.125;
    sif::info << "Temperature: " << temp << " C" << std::endl;
    uint16_t adcRaw[6] = {};
    adcRaw[0] = (recvBuffer[2] << 8) | recvBuffer[3];
    adcRaw[1] = (recvBuffer[4] << 8) | recvBuffer[5];
    adcRaw[2] = (recvBuffer[6] << 8) | recvBuffer[7];
    adcRaw[3] = (recvBuffer[8] << 8) | recvBuffer[9];
    adcRaw[4] = (recvBuffer[10] << 8) | recvBuffer[11];
    adcRaw[5] = (recvBuffer[12] << 8) | recvBuffer[13];
    for (int idx = 0; idx < 6; idx++) {
      sif::info << "ADC raw " << idx << ": " << adcRaw[idx] << std::endl;
    }
  }
}

/*
 *   sendBuffer[0] = buildConvByte(ScanModes::N_ONCE, 0, false);
  spiTransferStruct[0].len = 1;
  uint8_t reply0 = 0x00;
  uint8_t reply1 = 0x00;
  spiTransferStruct[1].tx_buf = 0;
  spiTransferStruct[1].rx_buf = reinterpret_cast<__u64>(&reply0);
  spiTransferStruct[1].len = 1;
  // Shift out zeros
  spiTransferStruct[2].tx_buf = 0;
  spiTransferStruct[2].rx_buf = reinterpret_cast<__u64>(&reply1);
  spiTransferStruct[2].len = 1;
  retval = ioctl(fd, SPI_IOC_MESSAGE(3), spiTransferStruct);
  if (retval < 0) {
    utility::handleIoctlError("SpiTestClass::writeRegister: Write failed");
  }
  recvBuffer[1] = reply0;
  recvBuffer[2] = reply1;
  arrayprinter::print(recvBuffer.data(), 3);
 */

void SpiTestClass::acsInit() {
  GpioCookie *gpioCookie = new GpioCookie();

#ifdef RASPBERRY_PI
  GpiodRegularByChip *gpio = nullptr;
  std::string rpiGpioName = "gpiochip0";
  gpio = new GpiodRegularByChip(rpiGpioName, mgm0Lis3mdlChipSelect, "MGM_0_LIS3", gpio::DIR_OUT,
                                gpio::HIGH);
  gpioCookie->addGpio(gpioIds::MGM_0_LIS3_CS, gpio);

  gpio = new GpiodRegularByChip(rpiGpioName, mgm1Rm3100ChipSelect, "MGM_1_RM3100", gpio::DIR_OUT,
                                gpio::HIGH);
  gpioCookie->addGpio(gpioIds::MGM_1_RM3100_CS, gpio);

  gpio = new GpiodRegularByChip(rpiGpioName, gyro0AdisChipSelect, "GYRO_0_ADIS", gpio::DIR_OUT,
                                gpio::HIGH);
  gpioCookie->addGpio(gpioIds::GYRO_0_ADIS_CS, gpio);

  gpio = new GpiodRegularByChip(rpiGpioName, gyro1L3gd20ChipSelect, "GYRO_1_L3G", gpio::DIR_OUT,
                                gpio::HIGH);
  gpioCookie->addGpio(gpioIds::GYRO_1_L3G_CS, gpio);

  gpio = new GpiodRegularByChip(rpiGpioName, gyro3L3gd20ChipSelect, "GYRO_2_L3G", gpio::DIR_OUT,
                                gpio::HIGH);
  gpioCookie->addGpio(gpioIds::GYRO_3_L3G_CS, gpio);

  gpio = new GpiodRegularByChip(rpiGpioName, mgm2Lis3mdlChipSelect, "MGM_2_LIS3", gpio::DIR_OUT,
                                gpio::HIGH);
  gpioCookie->addGpio(gpioIds::MGM_2_LIS3_CS, gpio);

  gpio = new GpiodRegularByChip(rpiGpioName, mgm3Rm3100ChipSelect, "MGM_3_RM3100", gpio::DIR_OUT,
                                gpio::HIGH);
  gpioCookie->addGpio(gpioIds::MGM_3_RM3100_CS, gpio);
#elif defined(XIPHOS_Q7S)
  GpiodRegularByLineName *gpio = nullptr;
  gpio =
      new GpiodRegularByLineName(q7s::gpioNames::MGM_0_CS, "MGM_0_LIS3", gpio::DIR_OUT, gpio::HIGH);
  gpioCookie->addGpio(gpioIds::MGM_0_LIS3_CS, gpio);
  gpio = new GpiodRegularByLineName(q7s::gpioNames::MGM_1_CS, "MGM_1_RM3100", gpio::DIR_OUT,
                                    gpio::HIGH);
  gpioCookie->addGpio(gpioIds::MGM_1_RM3100_CS, gpio);
  gpio =
      new GpiodRegularByLineName(q7s::gpioNames::MGM_2_CS, "MGM_2_LIS3", gpio::DIR_OUT, gpio::HIGH);
  gpioCookie->addGpio(gpioIds::MGM_2_LIS3_CS, gpio);
  gpio = new GpiodRegularByLineName(q7s::gpioNames::MGM_1_CS, "MGM_3_RM3100", gpio::DIR_OUT,
                                    gpio::HIGH);
  gpioCookie->addGpio(gpioIds::MGM_3_RM3100_CS, gpio);

  gpio = new GpiodRegularByLineName(q7s::gpioNames::GYRO_0_ADIS_CS, "GYRO_0_ADIS", gpio::DIR_OUT,
                                    gpio::HIGH);
  gpioCookie->addGpio(gpioIds::GYRO_0_ADIS_CS, gpio);
  gpio = new GpiodRegularByLineName(q7s::gpioNames::GYRO_1_L3G_CS, "GYRO_1_L3G", gpio::DIR_OUT,
                                    gpio::HIGH);
  gpioCookie->addGpio(gpioIds::GYRO_1_L3G_CS, gpio);
  gpio = new GpiodRegularByLineName(q7s::gpioNames::GYRO_2_ADIS_CS, "GYRO_2_ADIS", gpio::DIR_OUT,
                                    gpio::HIGH);
  gpioCookie->addGpio(gpioIds::GYRO_2_ADIS_CS, gpio);
  gpio = new GpiodRegularByLineName(q7s::gpioNames::GYRO_3_L3G_CS, "GYRO_3_L3G", gpio::DIR_OUT,
                                    gpio::HIGH);
  gpioCookie->addGpio(gpioIds::GYRO_3_L3G_CS, gpio);

  // Enable pins must be pulled low for regular operations
  gpio = new GpiodRegularByLineName(q7s::gpioNames::GYRO_0_ENABLE, "GYRO_0_ENABLE", gpio::DIR_OUT,
                                    gpio::LOW);
  gpioCookie->addGpio(gpioIds::GYRO_0_ENABLE, gpio);
  gpio = new GpiodRegularByLineName(q7s::gpioNames::GYRO_0_ENABLE, "GYRO_2_ENABLE", gpio::DIR_OUT,
                                    gpio::LOW);
  gpioCookie->addGpio(gpioIds::GYRO_2_ENABLE, gpio);
#endif
  if (gpioIF != nullptr) {
    gpioIF->addGpios(gpioCookie);
  }
}

void SpiTestClass::setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed) {
  int modeUnix = 0;
  switch (mode) {
    case (spi::SpiModes::MODE_0): {
      modeUnix = SPI_MODE_0;
      break;
    }
    case (spi::SpiModes::MODE_1): {
      modeUnix = SPI_MODE_1;
      break;
    }
    case (spi::SpiModes::MODE_2): {
      modeUnix = SPI_MODE_2;
      break;
    }
    case (spi::SpiModes::MODE_3): {
      modeUnix = SPI_MODE_3;
      break;
    }
  }

  int retval = ioctl(spiFd, SPI_IOC_WR_MODE, &modeUnix);  // reinterpret_cast<uint8_t*>(&mode));
  if (retval != 0) {
    utility::handleIoctlError("SpiTestClass::performRm3100Test: Setting SPI mode failed!");
  }

  retval = ioctl(spiFd, SPI_IOC_WR_MAX_SPEED_HZ, &speed);
  if (retval != 0) {
    utility::handleIoctlError("SpiTestClass::performRm3100Test: Setting SPI speed failed!");
  }
}

void SpiTestClass::writeRegister(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t value) {
  spiTransferStruct[0].len = 2;
  sendBuffer[0] = reg;
  sendBuffer[1] = value;

  if (gpioIF != nullptr and chipSelect != gpio::NO_GPIO) {
    gpioIF->pullLow(chipSelect);
  }
  int retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct);
  if (retval < 0) {
    utility::handleIoctlError("SpiTestClass::writeRegister: Write failed");
  }
  if (gpioIF != nullptr and chipSelect != gpio::NO_GPIO) {
    gpioIF->pullHigh(chipSelect);
  }
}

void SpiTestClass::writeStmRegister(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t value,
                                    bool autoIncrement) {
  if (autoIncrement) {
    reg |= STM_AUTO_INCR_MASK;
  }
  writeRegister(fd, chipSelect, reg, value);
}

void SpiTestClass::writeMultipleStmRegisters(int fd, gpioId_t chipSelect, uint8_t reg,
                                             uint8_t *values, size_t len) {
  if (values == nullptr) {
    return;
  }

  reg |= STM_AUTO_INCR_MASK;
  /* Clear read mask */
  reg &= ~STM_READ_MASK;
  writeMultipleRegisters(fd, chipSelect, reg, values, len);
}

void SpiTestClass::writeMultipleRegisters(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t *values,
                                          size_t len) {
  if (values == nullptr) {
    return;
  }

  sendBuffer[0] = reg;
  std::memcpy(sendBuffer.data() + 1, values, len);
  spiTransferStruct[0].len = len + 1;

  if (gpioIF != nullptr and chipSelect != gpio::NO_GPIO) {
    gpioIF->pullLow(chipSelect);
  }
  int retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct);
  if (retval < 0) {
    utility::handleIoctlError("SpiTestClass::readRegister: Read failed");
  }
  if (gpioIF != nullptr and chipSelect != gpio::NO_GPIO) {
    gpioIF->pullHigh(chipSelect);
  }
}

uint8_t SpiTestClass::readRm3100Register(int fd, gpioId_t chipSelect, uint8_t reg) {
  return readStmRegister(fd, chipSelect, reg, false);
}

void SpiTestClass::readMultipleStmRegisters(int fd, gpioId_t chipSelect, uint8_t reg,
                                            uint8_t *reply, size_t len) {
  reg |= STM_AUTO_INCR_MASK;
  readMultipleRegisters(fd, chipSelect, reg, reply, len);
}

void SpiTestClass::readMultipleRegisters(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t *reply,
                                         size_t len) {
  if (reply == nullptr) {
    return;
  }

  spiTransferStruct[0].len = len + 1;
  sendBuffer[0] = reg | STM_READ_MASK;

  for (uint8_t idx = 0; idx < len; idx++) {
    sendBuffer[idx + 1] = 0;
  }

  if (gpioIF != nullptr and chipSelect != gpio::NO_GPIO) {
    gpioIF->pullLow(chipSelect);
  }
  int retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct);
  if (retval < 0) {
    utility::handleIoctlError("SpiTestClass::readRegister: Read failed");
  }
  if (gpioIF != nullptr and chipSelect != gpio::NO_GPIO) {
    gpioIF->pullHigh(chipSelect);
  }
  std::memcpy(reply, recvBuffer.data() + 1, len);
}

uint8_t SpiTestClass::readStmRegister(int fd, gpioId_t chipSelect, uint8_t reg,
                                      bool autoIncrement) {
  reg |= STM_READ_MASK;
  if (autoIncrement) {
    reg |= STM_AUTO_INCR_MASK;
  }
  return readRegister(fd, chipSelect, reg);
}

uint8_t SpiTestClass::readRegister(int fd, gpioId_t chipSelect, uint8_t reg) {
  spiTransferStruct[0].len = 2;
  sendBuffer[0] = reg;
  sendBuffer[1] = 0;

  if (gpioIF != nullptr and chipSelect != gpio::NO_GPIO) {
    gpioIF->pullLow(chipSelect);
  }
  int retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct);
  if (retval < 0) {
    utility::handleIoctlError("SpiTestClass::readRegister: Read failed");
  }
  if (gpioIF != nullptr and chipSelect != gpio::NO_GPIO) {
    gpioIF->pullHigh(chipSelect);
  }
  return recvBuffer[1];
}