#include "GyroADIS1650XHandler.h"

#include <fsfw/action/HasActionsIF.h>
#include <fsfw/datapool/PoolReadGuard.h>

#include "fsfw/FSFW.h"

#ifdef FSFW_OSAL_LINUX
#include <sys/ioctl.h>
#include <unistd.h>

#include "fsfw_hal/linux/UnixFileGuard.h"
#include "fsfw_hal/linux/spi/SpiComIF.h"
#include "fsfw_hal/linux/spi/SpiCookie.h"
#include "fsfw_hal/linux/utility.h"
#endif

GyroADIS1650XHandler::GyroADIS1650XHandler(object_id_t objectId, object_id_t deviceCommunication,
                                           CookieIF *comCookie, ADIS1650X::Type type)
    : DeviceHandlerBase(objectId, deviceCommunication, comCookie),
      adisType(type),
      primaryDataset(this),
      configDataset(this),
      breakCountdown() {
#ifdef FSFW_OSAL_LINUX
  SpiCookie *cookie = dynamic_cast<SpiCookie *>(comCookie);
  if (cookie != nullptr) {
    cookie->setCallbackMode(&spiSendCallback, this);
  }
#endif
}

void GyroADIS1650XHandler::doStartUp() {
  // Initial 310 ms start up time after power-up
  if (internalState == InternalState::STARTUP) {
    if (not commandExecuted) {
      warningSwitch = true;
      breakCountdown.setTimeout(ADIS1650X::START_UP_TIME);
      commandExecuted = true;
    }
    if (breakCountdown.hasTimedOut()) {
      internalState = InternalState::CONFIG;
      commandExecuted = false;
    }
  }

  // Read all configuration registers first
  if (internalState == InternalState::CONFIG) {
    if (commandExecuted) {
      commandExecuted = false;
      internalState = InternalState::IDLE;
    }
  }

  if (internalState == InternalState::IDLE) {
    if (goToNormalMode) {
      setMode(MODE_NORMAL);
    } else {
      setMode(MODE_ON);
    }
  }
}

void GyroADIS1650XHandler::doShutDown() {
  commandExecuted = false;
  internalState = InternalState::STARTUP;
  setMode(_MODE_POWER_DOWN);
}

ReturnValue_t GyroADIS1650XHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
  *id = ADIS1650X::READ_SENSOR_DATA;
  return buildCommandFromCommand(*id, nullptr, 0);
}

ReturnValue_t GyroADIS1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
  switch (internalState) {
    case (InternalState::CONFIG): {
      *id = ADIS1650X::READ_OUT_CONFIG;
      buildCommandFromCommand(*id, nullptr, 0);
      break;
    }
    case (InternalState::STARTUP): {
      return NOTHING_TO_SEND;
      break;
    }
    default: {
      // Might be a configuration error
      sif::debug << "GyroADIS16507Handler::buildTransitionDeviceCommand: "
                    "Unknown internal state!"
                 << std::endl;
      return returnvalue::OK;
    }
  }
  return returnvalue::OK;
}

ReturnValue_t GyroADIS1650XHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
                                                            const uint8_t *commandData,
                                                            size_t commandDataLen) {
  switch (deviceCommand) {
    case (ADIS1650X::READ_OUT_CONFIG): {
      this->rawPacketLen = ADIS1650X::CONFIG_READOUT_SIZE;
      uint8_t regList[6] = {};
      regList[0] = ADIS1650X::DIAG_STAT_REG;
      regList[1] = ADIS1650X::FILTER_CTRL_REG;
      regList[2] = ADIS1650X::RANG_MDL_REG;
      regList[3] = ADIS1650X::MSC_CTRL_REG;
      regList[4] = ADIS1650X::DEC_RATE_REG;
      regList[5] = ADIS1650X::PROD_ID_REG;
      prepareReadCommand(regList, sizeof(regList));
      this->rawPacket = commandBuffer.data();
      break;
    }
    case (ADIS1650X::READ_SENSOR_DATA): {
      if (breakCountdown.isBusy()) {
        // A glob command is pending and sensor data can't be read currently
        return NO_REPLY_EXPECTED;
      }
      std::memcpy(commandBuffer.data(), ADIS1650X::BURST_READ_ENABLE.data(),
                  ADIS1650X::BURST_READ_ENABLE.size());
      std::memset(commandBuffer.data() + 2, 0, 10 * 2);
      this->rawPacketLen = ADIS1650X::SENSOR_READOUT_SIZE;
      this->rawPacket = commandBuffer.data();
      break;
    }
    case (ADIS1650X::SELF_TEST_SENSORS): {
      if (breakCountdown.isBusy()) {
        // Another glob command is pending
        return HasActionsIF::IS_BUSY;
      }
      prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::SENSOR_SELF_TEST, 0x00);
      breakCountdown.setTimeout(ADIS1650X::SELF_TEST_BREAK);
      break;
    }
    case (ADIS1650X::SELF_TEST_MEMORY): {
      if (breakCountdown.isBusy()) {
        // Another glob command is pending
        return HasActionsIF::IS_BUSY;
      }
      prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FLASH_MEMORY_TEST, 0x00);
      breakCountdown.setTimeout(ADIS1650X::FLASH_MEMORY_TEST_BREAK);
      break;
    }
    case (ADIS1650X::UPDATE_NV_CONFIGURATION): {
      if (breakCountdown.isBusy()) {
        // Another glob command is pending
        return HasActionsIF::IS_BUSY;
      }
      prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FLASH_MEMORY_UPDATE, 0x00);
      breakCountdown.setTimeout(ADIS1650X::FLASH_MEMORY_UPDATE_BREAK);
      break;
    }
    case (ADIS1650X::RESET_SENSOR_CONFIGURATION): {
      if (breakCountdown.isBusy()) {
        // Another glob command is pending
        return HasActionsIF::IS_BUSY;
      }
      prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::FACTORY_CALIBRATION, 0x00);
      breakCountdown.setTimeout(ADIS1650X::FACTORY_CALIBRATION_BREAK);
      break;
    }
    case (ADIS1650X::SW_RESET): {
      if (breakCountdown.isBusy()) {
        // Another glob command is pending
        return HasActionsIF::IS_BUSY;
      }
      prepareWriteCommand(ADIS1650X::GLOB_CMD, ADIS1650X::GlobCmds::SOFTWARE_RESET, 0x00);
      breakCountdown.setTimeout(ADIS1650X::SW_RESET_BREAK);
      break;
    }
    case (ADIS1650X::PRINT_CURRENT_CONFIGURATION): {
#if OBSW_VERBOSE_LEVEL >= 1
      PoolReadGuard pg(&configDataset);
      sif::info << "ADIS16507 Sensor configuration: DIAG_STAT: 0x" << std::hex << std::setw(4)
                << std::setfill('0') << "0x" << configDataset.diagStatReg.value << std::endl;
      sif::info << "MSC_CTRL: " << std::hex << std::setw(4) << "0x"
                << configDataset.mscCtrlReg.value << " | FILT_CTRL: 0x"
                << configDataset.filterSetting.value << " | DEC_RATE: 0x"
                << configDataset.decRateReg.value << std::setfill(' ') << std::endl;
#endif /* OBSW_VERBOSE_LEVEL >= 1 */
    }
  }
  return returnvalue::OK;
}

void GyroADIS1650XHandler::fillCommandAndReplyMap() {
  insertInCommandAndReplyMap(ADIS1650X::READ_SENSOR_DATA, 1, &primaryDataset);
  insertInCommandAndReplyMap(ADIS1650X::READ_OUT_CONFIG, 1, &configDataset);
  insertInCommandAndReplyMap(ADIS1650X::SELF_TEST_SENSORS, 1);
  insertInCommandAndReplyMap(ADIS1650X::SELF_TEST_MEMORY, 1);
  insertInCommandAndReplyMap(ADIS1650X::UPDATE_NV_CONFIGURATION, 1);
  insertInCommandAndReplyMap(ADIS1650X::RESET_SENSOR_CONFIGURATION, 1);
  insertInCommandAndReplyMap(ADIS1650X::SW_RESET, 1);
  insertInCommandAndReplyMap(ADIS1650X::PRINT_CURRENT_CONFIGURATION, 1);
}

ReturnValue_t GyroADIS1650XHandler::scanForReply(const uint8_t *start, size_t remainingSize,
                                                 DeviceCommandId_t *foundId, size_t *foundLen) {
  // For SPI, the ID will always be the one of the last sent command
  *foundId = this->getPendingCommand();
  *foundLen = this->rawPacketLen;

  return returnvalue::OK;
}

ReturnValue_t GyroADIS1650XHandler::interpretDeviceReply(DeviceCommandId_t id,
                                                         const uint8_t *packet) {
  using namespace ADIS1650X;
  switch (id) {
    case (ADIS1650X::READ_OUT_CONFIG): {
      uint16_t readProdId = packet[12] << 8 | packet[13];
      if (((adisType == ADIS1650X::Type::ADIS16507) and (readProdId != ADIS1650X::PROD_ID_16507)) or
          ((adisType == ADIS1650X::Type::ADIS16505) and (readProdId != ADIS1650X::PROD_ID_16505))) {
#if OBSW_VERBOSE_LEVEL >= 1
        if (warningSwitch) {
          sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID "
                       << readProdId << std::endl;
        }
        warningSwitch = false;
#endif
        return returnvalue::FAILED;
      }
      PoolReadGuard rg(&configDataset);
      configDataset.diagStatReg.value = packet[2] << 8 | packet[3];
      configDataset.filterSetting.value = packet[4] << 8 | packet[5];
      uint16_t rangMdlRaw = packet[6] << 8 | packet[7];
      ADIS1650X::RangMdlBitfield bitfield =
          static_cast<ADIS1650X::RangMdlBitfield>((rangMdlRaw >> 2) & 0b11);
      switch (bitfield) {
        case (ADIS1650X::RangMdlBitfield::RANGE_125_1BMLZ): {
          sensitivity = SENSITIVITY_1BMLZ;
          break;
        }
        case (ADIS1650X::RangMdlBitfield::RANGE_500_2BMLZ): {
          sensitivity = SENSITIVITY_2BMLZ;
          break;
        }
        case (ADIS1650X::RangMdlBitfield::RANGE_2000_3BMLZ): {
          sensitivity = SENSITIVITY_3BMLZ;
          break;
        }
        case (RangMdlBitfield::RESERVED): {
#if FSFW_CPP_OSTREAM_ENABLED == 1
          sif::error << "ADIS1650X: Unexpected value for RANG_MDL register" << std::endl;
#endif
          break;
        }
      }
      configDataset.rangMdl.value = rangMdlRaw;
      configDataset.mscCtrlReg.value = packet[8] << 8 | packet[9];
      configDataset.decRateReg.value = packet[10] << 8 | packet[11];
      configDataset.setValidity(true, true);
      if (internalState == InternalState::CONFIG) {
        commandExecuted = true;
      }
      break;
    }
    case (ADIS1650X::READ_SENSOR_DATA): {
      return handleSensorData(packet);
    }
  }
  return returnvalue::OK;
}

ReturnValue_t GyroADIS1650XHandler::handleSensorData(const uint8_t *packet) {
  BurstModes burstMode = getBurstMode();
  switch (burstMode) {
    case (BurstModes::BURST_16_BURST_SEL_1):
    case (BurstModes::BURST_32_BURST_SEL_1): {
      sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: Analysis with BURST_SEL1"
                      " not implemented!"
                   << std::endl;
      return returnvalue::OK;
    }
    case (BurstModes::BURST_16_BURST_SEL_0): {
      uint16_t checksum = packet[20] << 8 | packet[21];
      // Now verify the read checksum with the expected checksum according to datasheet p. 20
      uint16_t calcChecksum = 0;
      for (size_t idx = 2; idx < 20; idx++) {
        calcChecksum += packet[idx];
      }
      if (checksum != calcChecksum) {
#if OBSW_VERBOSE_LEVEL >= 1
        sif::warning << "GyroADIS1650XHandler::interpretDeviceReply: "
                        "Invalid checksum detected!"
                     << std::endl;
#endif
        return returnvalue::FAILED;
      }

      ReturnValue_t result = configDataset.diagStatReg.read();
      if (result == returnvalue::OK) {
        configDataset.diagStatReg.value = packet[2] << 8 | packet[3];
        configDataset.diagStatReg.setValid(true);
      }
      configDataset.diagStatReg.commit();

      {
        PoolReadGuard pg(&primaryDataset);
        int16_t angVelocXRaw = packet[4] << 8 | packet[5];
        primaryDataset.angVelocX.value = static_cast<float>(angVelocXRaw) * sensitivity;
        int16_t angVelocYRaw = packet[6] << 8 | packet[7];
        primaryDataset.angVelocY.value = static_cast<float>(angVelocYRaw) * sensitivity;
        int16_t angVelocZRaw = packet[8] << 8 | packet[9];
        primaryDataset.angVelocZ.value = static_cast<float>(angVelocZRaw) * sensitivity;

        float accelScaling = 0;
        if (adisType == ADIS1650X::Type::ADIS16507) {
          accelScaling = ADIS1650X::ACCELEROMETER_RANGE_16507;
        } else if (adisType == ADIS1650X::Type::ADIS16505) {
          accelScaling = ADIS1650X::ACCELEROMETER_RANGE_16505;
        } else {
          sif::warning << "GyroADIS1650XHandler::handleSensorData: "
                          "Unknown ADIS type"
                       << std::endl;
        }
        int16_t accelXRaw = packet[10] << 8 | packet[11];
        primaryDataset.accelX.value = static_cast<float>(accelXRaw) / INT16_MAX * accelScaling;
        int16_t accelYRaw = packet[12] << 8 | packet[13];
        primaryDataset.accelY.value = static_cast<float>(accelYRaw) / INT16_MAX * accelScaling;
        int16_t accelZRaw = packet[14] << 8 | packet[15];
        primaryDataset.accelZ.value = static_cast<float>(accelZRaw) / INT16_MAX * accelScaling;

        int16_t temperatureRaw = packet[16] << 8 | packet[17];
        primaryDataset.temperature.value = static_cast<float>(temperatureRaw) * 0.1;
        // Ignore data counter for now
        primaryDataset.setValidity(true, true);
      }

      if (periodicPrintout) {
        if (debugDivider.checkAndIncrement()) {
          sif::info << "GyroADIS1650XHandler: Angular velocities in deg / s" << std::endl;
          sif::info << "X: " << primaryDataset.angVelocX.value << std::endl;
          sif::info << "Y: " << primaryDataset.angVelocY.value << std::endl;
          sif::info << "Z: " << primaryDataset.angVelocZ.value << std::endl;
          sif::info << "GyroADIS1650XHandler: Accelerations in m / s^2: " << std::endl;
          sif::info << "X: " << primaryDataset.accelX.value << std::endl;
          sif::info << "Y: " << primaryDataset.accelY.value << std::endl;
          sif::info << "Z: " << primaryDataset.accelZ.value << std::endl;
        }
      }

      break;
    }
    case (BurstModes::BURST_32_BURST_SEL_0): {
      break;
    }
  }
  return returnvalue::OK;
}

uint32_t GyroADIS1650XHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { return 6000; }

void GyroADIS1650XHandler::prepareWriteCommand(uint8_t startReg, uint8_t valueOne,
                                               uint8_t valueTwo) {
  uint8_t secondReg = startReg + 1;
  startReg |= ADIS1650X::WRITE_MASK;
  secondReg |= ADIS1650X::WRITE_MASK;
  commandBuffer[0] = startReg;
  commandBuffer[1] = valueOne;
  commandBuffer[2] = secondReg;
  commandBuffer[3] = valueTwo;
  this->rawPacketLen = 4;
  this->rawPacket = commandBuffer.data();
}

void GyroADIS1650XHandler::prepareReadCommand(uint8_t *regList, size_t len) {
  for (size_t idx = 0; idx < len; idx++) {
    commandBuffer[idx * 2] = regList[idx];
    commandBuffer[idx * 2 + 1] = 0x00;
  }
  commandBuffer[len * 2] = 0x00;
  commandBuffer[len * 2 + 1] = 0x00;
}

ReturnValue_t GyroADIS1650XHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                            LocalDataPoolManager &poolManager) {
  localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_X, new PoolEntry<double>({0.0}));
  localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Y, new PoolEntry<double>({0.0}));
  localDataPoolMap.emplace(ADIS1650X::ANG_VELOC_Z, new PoolEntry<double>({0.0}));
  localDataPoolMap.emplace(ADIS1650X::ACCELERATION_X, new PoolEntry<double>({0.0}));
  localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Y, new PoolEntry<double>({0.0}));
  localDataPoolMap.emplace(ADIS1650X::ACCELERATION_Z, new PoolEntry<double>({0.0}));
  localDataPoolMap.emplace(ADIS1650X::TEMPERATURE, new PoolEntry<float>({0.0}));

  localDataPoolMap.emplace(ADIS1650X::DIAG_STAT_REGISTER, new PoolEntry<uint16_t>());
  localDataPoolMap.emplace(ADIS1650X::FILTER_SETTINGS, new PoolEntry<uint8_t>());
  localDataPoolMap.emplace(ADIS1650X::MSC_CTRL_REGISTER, new PoolEntry<uint16_t>());
  localDataPoolMap.emplace(ADIS1650X::DEC_RATE_REGISTER, new PoolEntry<uint16_t>());
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(primaryDataset.getSid(), false, 5.0));
  return returnvalue::OK;
}

GyroADIS1650XHandler::BurstModes GyroADIS1650XHandler::getBurstMode() {
  configDataset.mscCtrlReg.read();
  uint16_t currentCtrlReg = configDataset.mscCtrlReg.value;
  configDataset.mscCtrlReg.commit();
  if ((currentCtrlReg & ADIS1650X::BURST_32_BIT) == ADIS1650X::BURST_32_BIT) {
    if ((currentCtrlReg & ADIS1650X::BURST_SEL_BIT) == ADIS1650X::BURST_SEL_BIT) {
      return BurstModes::BURST_32_BURST_SEL_1;
    } else {
      return BurstModes::BURST_32_BURST_SEL_0;
    }
  } else {
    if ((currentCtrlReg & ADIS1650X::BURST_SEL_BIT) == ADIS1650X::BURST_SEL_BIT) {
      return BurstModes::BURST_16_BURST_SEL_1;
    } else {
      return BurstModes::BURST_16_BURST_SEL_0;
    }
  }
}

#ifdef FSFW_OSAL_LINUX

ReturnValue_t GyroADIS1650XHandler::spiSendCallback(SpiComIF *comIf, SpiCookie *cookie,
                                                    const uint8_t *sendData, size_t sendLen,
                                                    void *args) {
  GyroADIS1650XHandler *handler = reinterpret_cast<GyroADIS1650XHandler *>(args);
  if (handler == nullptr) {
    sif::error << "GyroADIS16507Handler::spiSendCallback: Passed handler pointer is invalid!"
               << std::endl;
    return returnvalue::FAILED;
  }
  DeviceCommandId_t currentCommand = handler->getPendingCommand();
  switch (currentCommand) {
    case (ADIS1650X::READ_SENSOR_DATA): {
      return comIf->performRegularSendOperation(cookie, sendData, sendLen);
    }
    case (ADIS1650X::READ_OUT_CONFIG):
    default: {
      ReturnValue_t result = returnvalue::OK;
      int retval = 0;
      // Prepare transfer
      int fileDescriptor = 0;
      std::string device = comIf->getSpiDev();
      UnixFileGuard fileHelper(device, &fileDescriptor, O_RDWR, "SpiComIF::sendMessage");
      if (fileHelper.getOpenResult() != returnvalue::OK) {
        return SpiComIF::OPENING_FILE_FAILED;
      }
      spi::SpiModes spiMode = spi::SpiModes::MODE_0;
      uint32_t spiSpeed = 0;
      cookie->getSpiParameters(spiMode, spiSpeed, nullptr);
      comIf->setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
      cookie->assignWriteBuffer(sendData);
      cookie->setTransferSize(2);

      gpioId_t gpioId = cookie->getChipSelectPin();
      GpioIF &gpioIF = comIf->getGpioInterface();
      MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
      uint32_t timeoutMs = 0;
      MutexIF *mutex = comIf->getCsMutex();
      cookie->getMutexParams(timeoutType, timeoutMs);
      if (mutex == nullptr) {
#if OBSW_VERBOSE_LEVEL >= 1
        sif::warning << "GyroADIS16507Handler::spiSendCallback: "
                        "Mutex or GPIO interface invalid"
                     << std::endl;
        return returnvalue::FAILED;
#endif
      }

      if (gpioId != gpio::NO_GPIO) {
        result = mutex->lockMutex(timeoutType, timeoutMs);
        if (result != returnvalue::OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
          sif::error << "SpiComIF::sendMessage: Failed to lock mutex" << std::endl;
#endif
          return result;
        }
      }

      size_t idx = 0;
      spi_ioc_transfer *transferStruct = cookie->getTransferStructHandle();
      uint64_t origTx = transferStruct->tx_buf;
      uint64_t origRx = transferStruct->rx_buf;
      while (idx < sendLen) {
        // Pull SPI CS low. For now, no support for active high given
        if (gpioId != gpio::NO_GPIO) {
          gpioIF.pullLow(gpioId);
        }

        // Execute transfer
        // Initiate a full duplex SPI transfer.
        retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), cookie->getTransferStructHandle());
        if (retval < 0) {
          utility::handleIoctlError("SpiComIF::sendMessage: ioctl error.");
          result = SpiComIF::FULL_DUPLEX_TRANSFER_FAILED;
        }
#if FSFW_HAL_SPI_WIRETAPPING == 1
        comIf->performSpiWiretapping(cookie);
#endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */

        if (gpioId != gpio::NO_GPIO) {
          gpioIF.pullHigh(gpioId);
        }

        idx += 2;
        if (idx < sendLen) {
          usleep(ADIS1650X::STALL_TIME_MICROSECONDS);
        }

        transferStruct->tx_buf += 2;
        transferStruct->rx_buf += 2;
      }
      transferStruct->tx_buf = origTx;
      transferStruct->rx_buf = origRx;
      if (gpioId != gpio::NO_GPIO) {
        mutex->unlockMutex();
      }
    }
  }
  return returnvalue::OK;
}

void GyroADIS1650XHandler::setToGoToNormalModeImmediately() { goToNormalMode = true; }

void GyroADIS1650XHandler::enablePeriodicPrintouts(bool enable, uint8_t divider) {
  periodicPrintout = enable;
  debugDivider.setDivider(divider);
}

#endif /* OBSW_ADIS1650X_LINUX_COM_IF == 1 */