#include "AcsBoardPolling.h"

#include <fcntl.h>
#include <fsfw/globalfunctions/arrayprinter.h>
#include <fsfw/tasks/SemaphoreFactory.h>
#include <fsfw/tasks/TaskFactory.h>
#include <fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h>
#include <fsfw_hal/linux/UnixFileGuard.h>
#include <fsfw_hal/linux/spi/SpiCookie.h>
#include <fsfw_hal/linux/utility.h>
#include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
#include <sys/ioctl.h>

#include "devices/gpioIds.h"

using namespace returnvalue;

AcsBoardPolling::AcsBoardPolling(object_id_t objectId, SpiComIF& lowLevelComIF, GpioIF& gpioIF)
    : SystemObject(objectId), spiComIF(lowLevelComIF), gpioIF(gpioIF) {
  semaphore = SemaphoreFactory::instance()->createBinarySemaphore();
  semaphore->acquire();
  ipcLock = MutexFactory::instance()->createMutex();
}

ReturnValue_t AcsBoardPolling::performOperation(uint8_t operationCode) {
  while (true) {
    ipcLock->lockMutex();
    state = InternalState::IDLE;
    ipcLock->unlockMutex();
    semaphore->acquire();
    // Give all tasks or the PST some time to submit all consecutive requests.
    TaskFactory::delayTask(2);
    gyroAdisHandler(gyro0Adis);
    gyroAdisHandler(gyro2Adis);
    gyroL3gHandler(gyro1L3g);
    gyroL3gHandler(gyro3L3g);
    // To prevent task being not reactivated by tardy tasks
    TaskFactory::delayTask(20);
  }
  return returnvalue::OK;
}

ReturnValue_t AcsBoardPolling::initialize() { return returnvalue::OK; }

ReturnValue_t AcsBoardPolling::initializeInterface(CookieIF* cookie) {
  SpiCookie* spiCookie = dynamic_cast<SpiCookie*>(cookie);
  if (spiCookie == nullptr) {
    return returnvalue::FAILED;
  }
  switch (spiCookie->getChipSelectPin()) {
    case (gpioIds::MGM_0_LIS3_CS): {
      mgm0L3Cookie = spiCookie;
      break;
    }
    case (gpioIds::MGM_1_RM3100_CS): {
      mgm1Rm3100Cookie = spiCookie;
      break;
    }
    case (gpioIds::MGM_2_LIS3_CS): {
      mgm2L3Cookie = spiCookie;
      break;
    }
    case (gpioIds::MGM_3_RM3100_CS): {
      mgm3Rm3100Cookie = spiCookie;
      break;
    }
    case (gpioIds::GYRO_0_ADIS_CS): {
      gyro0Adis.cookie = spiCookie;
      break;
    }
    case (gpioIds::GYRO_1_L3G_CS): {
      gyro1L3g.cookie = spiCookie;
      break;
    }
    case (gpioIds::GYRO_2_ADIS_CS): {
      gyro2Adis.cookie = spiCookie;
      break;
    }
    case (gpioIds::GYRO_3_L3G_CS): {
      gyro3L3g.cookie = spiCookie;
      break;
    }
    default: {
      sif::error << "AcsBoardPollingTask: invalid spi cookie" << std::endl;
    }
  }
  return spiComIF.initializeInterface(cookie);
}

ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* sendData,
                                           size_t sendLen) {
  SpiCookie* spiCookie = dynamic_cast<SpiCookie*>(cookie);
  if (spiCookie == nullptr) {
    return returnvalue::FAILED;
  }
  auto handleAdisRequest = [&](GyroAdis& adis) {
    if (sendLen != sizeof(acs::Adis1650XRequest)) {
      sif::error << "AcsBoardPolling: invalid adis request send length";
      adis.replyResult = returnvalue::FAILED;
      return returnvalue::FAILED;
    }
    auto* req = reinterpret_cast<const acs::Adis1650XRequest*>(sendData);
    MutexGuard mg(ipcLock);
    if (req->mode != adis.mode) {
      if (req->mode == acs::SimpleSensorMode::NORMAL) {
        adis.type = req->type;
        adis.countdown.setTimeout(adis1650x::START_UP_TIME);
        adis.countdown.resetTimer();
        if (adis.type == adis1650x::Type::ADIS16507) {
          adis.ownReply.data.accelScaling = adis1650x::ACCELEROMETER_RANGE_16507;
        } else if (adis.type == adis1650x::Type::ADIS16505) {
          adis.ownReply.data.accelScaling = adis1650x::ACCELEROMETER_RANGE_16505;
        } else {
          sif::warning << "AcsBoardPolling: Unknown ADIS type" << std::endl;
        }
        adis.performStartup = true;
      } else if (req->mode == acs::SimpleSensorMode::OFF) {
        adis.performStartup = false;
        adis.ownReply.cfgWasSet = false;
        adis.ownReply.dataWasSet = false;
      }
      adis.mode = req->mode;
    }
    return returnvalue::OK;
  };
  auto handleL3gRequest = [&](GyroL3g& gyro) {
    if (sendLen != sizeof(acs::GyroL3gRequest)) {
      sif::error << "AcsBoardPolling: invalid l3g request send length";
      gyro.replyResult = returnvalue::FAILED;
      return returnvalue::FAILED;
    }
    auto* req = reinterpret_cast<const acs::GyroL3gRequest*>(sendData);
    MutexGuard mg(ipcLock);
    if (req->mode != gyro.mode) {
      if (req->mode == acs::SimpleSensorMode::NORMAL) {
        gyro.performStartup = true;
      } else {
        gyro.ownReply.cfgWasSet = false;
      }
      gyro.mode = req->mode;
    }
    return returnvalue::OK;
  };
  switch (spiCookie->getChipSelectPin()) {
    case (gpioIds::GYRO_0_ADIS_CS): {
      handleAdisRequest(gyro0Adis);
      break;
    }
    case (gpioIds::GYRO_2_ADIS_CS): {
      handleAdisRequest(gyro2Adis);
      break;
    }
    case (gpioIds::GYRO_1_L3G_CS): {
      handleL3gRequest(gyro1L3g);
      break;
    }
    case (gpioIds::GYRO_3_L3G_CS): {
      handleL3gRequest(gyro3L3g);
      break;
    }
  }
  MutexGuard mg(ipcLock);
  if (state == InternalState::IDLE) {
    state = InternalState::BUSY;
    semaphore->release();
  }
  return returnvalue::OK;
}

ReturnValue_t AcsBoardPolling::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; }

ReturnValue_t AcsBoardPolling::requestReceiveMessage(CookieIF* cookie, size_t requestLen) {
  return returnvalue::OK;
}

ReturnValue_t AcsBoardPolling::readReceivedMessage(CookieIF* cookie, uint8_t** buffer,
                                                   size_t* size) {
  SpiCookie* spiCookie = dynamic_cast<SpiCookie*>(cookie);
  if (spiCookie == nullptr) {
    return returnvalue::FAILED;
  }
  MutexGuard mg(ipcLock);
  auto handleAdisReply = [&](GyroAdis& gyro) {
    std::memcpy(&gyro.readerReply, &gyro.ownReply, sizeof(acs::Adis1650XReply));
    *buffer = reinterpret_cast<uint8_t*>(&gyro.readerReply);
    *size = sizeof(acs::Adis1650XReply);
  };
  auto handleL3gReply = [&](GyroL3g& gyro) {
    std::memcpy(&gyro.readerReply, &gyro.ownReply, sizeof(acs::GyroL3gReply));
    *buffer = reinterpret_cast<uint8_t*>(&gyro.readerReply);
    *size = sizeof(acs::GyroL3gReply);
  };
  switch (spiCookie->getChipSelectPin()) {
    case (gpioIds::GYRO_0_ADIS_CS): {
      handleAdisReply(gyro0Adis);
      return gyro0Adis.replyResult;
    }
    case (gpioIds::GYRO_2_ADIS_CS): {
      handleAdisReply(gyro2Adis);
      return gyro2Adis.replyResult;
    }
    case (gpioIds::GYRO_1_L3G_CS): {
      handleL3gReply(gyro1L3g);
      return gyro1L3g.replyResult;
    }
    case (gpioIds::GYRO_3_L3G_CS): {
      handleL3gReply(gyro3L3g);
      return gyro3L3g.replyResult;
    }
  }
  return returnvalue::OK;
}

void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) {
  ReturnValue_t result;
  acs::SimpleSensorMode mode;
  bool gyroPerformStartup;
  {
    MutexGuard mg(ipcLock);
    mode = l3g.mode;
    gyroPerformStartup = l3g.performStartup;
  }
  if (mode == acs::SimpleSensorMode::NORMAL) {
    if (gyroPerformStartup) {
      cmdBuf[0] = l3gd20h::CTRL_REG_1 | l3gd20h::AUTO_INCREMENT_MASK;
      std::memcpy(cmdBuf.data() + 1, l3g.sensorCfg, 5);
      result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), 6);
      if (result != returnvalue::OK) {
        l3g.replyResult = returnvalue::OK;
      }
      // Ignore useless reply and red config
      cmdBuf[0] = l3gd20h::CTRL_REG_1 | l3gd20h::AUTO_INCREMENT_MASK | l3gd20h::READ_MASK;
      std::memset(cmdBuf.data() + 1, 0, 5);
      result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), 6);
      if (result != returnvalue::OK) {
        l3g.replyResult = returnvalue::OK;
      }
      result = spiComIF.readReceivedMessage(l3g.cookie, &rawReply, &dummy);
      if (result != returnvalue::OK) {
        l3g.replyResult = returnvalue::OK;
      }
      MutexGuard mg(ipcLock);
      // Cross check configuration as verification that communication is working
      for (uint8_t idx = 0; idx < 5; idx++) {
        if (rawReply[idx + 1] != l3g.sensorCfg[idx]) {
          sif::warning << "AcsBoardPolling: l3g config check missmatch" << std::endl;
          l3g.replyResult = returnvalue::FAILED;
          return;
        }
      }
      l3g.ownReply.cfgWasSet = true;
      l3g.ownReply.sensitivity = l3gd20h::ctrlReg4ToSensitivity(l3g.sensorCfg[3]);
    }
    cmdBuf[0] = l3gd20h::READ_START | l3gd20h::AUTO_INCREMENT_MASK | l3gd20h::READ_MASK;
    std::memset(cmdBuf.data() + 1, 0, l3gd20h::READ_LEN);
    result = spiComIF.sendMessage(l3g.cookie, cmdBuf.data(), 6);
    if (result != returnvalue::OK) {
      l3g.replyResult = returnvalue::FAILED;
      return;
    }
    // The regular read function always returns the full sensor config as well. Use that
    // to verify communications.
    for (uint8_t idx = 0; idx < 5; idx++) {
      if (rawReply[idx + 1] != l3g.sensorCfg[idx]) {
        sif::warning << "AcsBoardPolling: l3g config check missmatch" << std::endl;
        l3g.replyResult = returnvalue::FAILED;
        return;
      }
    }
    l3g.ownReply.statusReg = rawReply[l3gd20h::STATUS_IDX];
    l3g.ownReply.angVelocities[0] = (rawReply[l3gd20h::OUT_X_H] << 8) | rawReply[l3gd20h::OUT_X_L];
    l3g.ownReply.angVelocities[1] = (rawReply[l3gd20h::OUT_Y_H] << 8) | rawReply[l3gd20h::OUT_Y_L];
    l3g.ownReply.angVelocities[2] = (rawReply[l3gd20h::OUT_Z_H] << 8) | rawReply[l3gd20h::OUT_Z_L];
    l3g.ownReply.tempOffsetRaw = rawReply[l3gd20h::TEMPERATURE_IDX];
  }
}

ReturnValue_t AcsBoardPolling::readAdisCfg(SpiCookie& cookie, size_t transferLen) {
  ReturnValue_t result = returnvalue::OK;
  int retval = 0;
  // Prepare transfer
  int fileDescriptor = 0;
  std::string device = spiComIF.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);
  spiComIF.setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
  cookie.assignWriteBuffer(cmdBuf.data());
  cookie.setTransferSize(2);

  gpioId_t gpioId = cookie.getChipSelectPin();
  MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
  uint32_t timeoutMs = 0;
  MutexIF* mutex = spiComIF.getCsMutex();
  cookie.getMutexParams(timeoutType, timeoutMs);
  if (mutex == nullptr) {
    sif::warning << "GyroADIS16507Handler::spiSendCallback: "
                    "Mutex or GPIO interface invalid"
                 << std::endl;
    return returnvalue::FAILED;
  }

  size_t idx = 0;
  spi_ioc_transfer* transferStruct = cookie.getTransferStructHandle();
  uint64_t origTx = transferStruct->tx_buf;
  uint64_t origRx = transferStruct->rx_buf;
  while (idx < transferLen) {
    result = mutex->lockMutex(timeoutType, timeoutMs);
    if (result != returnvalue::OK) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
      sif::error << "AcsBoardPolling: Failed to lock mutex" << std::endl;
#endif
      return result;
    }
    // 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);
    }
    mutex->unlockMutex();

    idx += 2;
    transferStruct->tx_buf += 2;
    transferStruct->rx_buf += 2;
    if (idx < transferLen) {
      usleep(adis1650x::STALL_TIME_MICROSECONDS);
    }
  }
  transferStruct->tx_buf = origTx;
  transferStruct->rx_buf = origRx;
  cookie.setTransferSize(transferLen);
  return returnvalue::OK;
}

void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
  ReturnValue_t result;
  acs::SimpleSensorMode mode;
  bool cdHasTimedOut = false;
  bool mustPerformStartup = false;
  {
    MutexGuard mg(ipcLock);
    mode = gyro.mode;
    cdHasTimedOut = gyro.countdown.hasTimedOut();
    mustPerformStartup = gyro.performStartup;
  }
  if (mode == acs::SimpleSensorMode::NORMAL and cdHasTimedOut) {
    if (mustPerformStartup) {
      uint8_t regList[6];
      // Read configuration
      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;
      size_t transferLen =
          adis1650x::prepareReadCommand(regList, sizeof(regList), cmdBuf.data(), cmdBuf.size());
      result = readAdisCfg(*gyro.cookie, transferLen);
      if (result != returnvalue::OK) {
        gyro.replyResult = result;
        return;
      }
      result = spiComIF.readReceivedMessage(gyro.cookie, &rawReply, &dummy);
      if (result != returnvalue::OK or rawReply == nullptr) {
        gyro.replyResult = result;
        return;
      }
      uint16_t prodId = (rawReply[12] << 8) | rawReply[13];
      if (((gyro.type == adis1650x::Type::ADIS16505) and (prodId != adis1650x::PROD_ID_16505)) or
          ((gyro.type == adis1650x::Type::ADIS16507) and (prodId != adis1650x::PROD_ID_16507))) {
        sif::warning << "AcsPollingTask: Invalid  ADIS product ID " << prodId << std::endl;
        gyro.replyResult = returnvalue::FAILED;
        return;
      }
      MutexGuard mg(ipcLock);
      gyro.ownReply.cfgWasSet = true;
      gyro.ownReply.cfg.diagStat = (rawReply[2] << 8) | rawReply[3];
      gyro.ownReply.cfg.filterSetting = (rawReply[4] << 8) | rawReply[5];
      gyro.ownReply.cfg.rangMdl = (rawReply[6] << 8) | rawReply[7];
      gyro.ownReply.cfg.mscCtrlReg = (rawReply[8] << 8) | rawReply[9];
      gyro.ownReply.cfg.decRateReg = (rawReply[10] << 8) | rawReply[11];
      gyro.ownReply.cfg.prodId = prodId;
      gyro.ownReply.data.sensitivity = adis1650x::rangMdlToSensitivity(gyro.ownReply.cfg.rangMdl);
      gyro.performStartup = false;
    }
    // Read regular registers
    std::memcpy(cmdBuf.data(), adis1650x::BURST_READ_ENABLE.data(),
                adis1650x::BURST_READ_ENABLE.size());
    std::memset(cmdBuf.data() + 2, 0, 10 * 2);
    result = spiComIF.sendMessage(gyro.cookie, cmdBuf.data(), adis1650x::SENSOR_READOUT_SIZE);
    if (result != returnvalue::OK) {
      gyro.replyResult = returnvalue::FAILED;
      return;
    }
    result = spiComIF.readReceivedMessage(gyro.cookie, &rawReply, &dummy);
    if (result != returnvalue::OK or rawReply == nullptr) {
      gyro.replyResult = returnvalue::FAILED;
      return;
    }
    uint16_t checksum = (rawReply[20] << 8) | rawReply[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 += rawReply[idx];
    }
    if (checksum != calcChecksum) {
      sif::warning << "AcsPollingTask: Invalid ADIS reply checksum" << std::endl;
      gyro.replyResult = returnvalue::FAILED;
      return;
    }

    auto burstMode = adis1650x::burstModeFromMscCtrl(gyro.ownReply.cfg.mscCtrlReg);
    if (burstMode != adis1650x::BurstModes::BURST_16_BURST_SEL_0) {
      sif::error << "GyroADIS1650XHandler::interpretDeviceReply: Analysis for select burst mode"
                    " not implemented!"
                 << std::endl;
      gyro.replyResult = returnvalue::FAILED;
      return;
    }

    MutexGuard mg(ipcLock);
    gyro.ownReply.dataWasSet = true;
    gyro.ownReply.cfg.diagStat = rawReply[2] << 8 | rawReply[3];
    gyro.ownReply.data.angVelocities[0] = (rawReply[4] << 8) | rawReply[5];
    gyro.ownReply.data.angVelocities[1] = (rawReply[6] << 8) | rawReply[7];
    gyro.ownReply.data.angVelocities[2] = (rawReply[8] << 8) | rawReply[9];

    gyro.ownReply.data.accelerations[0] = (rawReply[10] << 8) | rawReply[11];
    gyro.ownReply.data.accelerations[1] = (rawReply[12] << 8) | rawReply[13];
    gyro.ownReply.data.accelerations[2] = (rawReply[14] << 8) | rawReply[15];

    gyro.ownReply.data.temperatureRaw = (rawReply[16] << 8) | rawReply[17];
  }
}