#include "GyrAdis1650XHandler.h"

#include "fsfw/action/HasActionsIF.h"
#include "fsfw/datapool/PoolReadGuard.h"
#include "mission/acs/acsBoardPolling.h"

GyrAdis1650XHandler::GyrAdis1650XHandler(object_id_t objectId, object_id_t deviceCommunication,
                                         CookieIF *comCookie, adis1650x::Type type)
    : DeviceHandlerBase(objectId, deviceCommunication, comCookie),
      adisType(type),
      primaryDataset(this),
      configDataset(this),
      breakCountdown() {
  adisRequest.type = adisType;
}

void GyrAdis1650XHandler::doStartUp() {
  if (internalState == InternalState::NONE) {
    internalState = InternalState::STARTUP;
    commandExecuted = false;
  }
  // Initial 310 ms start up time after power-up
  if (internalState == InternalState::STARTUP) {
    if (not commandExecuted) {
      warningSwitch = true;
      breakCountdown.setTimeout(adis1650x::START_UP_TIME);
      updatePeriodicReply(true, adis1650x::REPLY);
      commandExecuted = true;
    }
  }
  if (internalState == InternalState::STARTUP_DONE) {
    setMode(MODE_ON);
    commandExecuted = false;
    internalState = InternalState::NONE;
  }
}

void GyrAdis1650XHandler::doShutDown() {
  if (internalState != InternalState::SHUTDOWN) {
    commandExecuted = false;
    PoolReadGuard pg(&primaryDataset);
    primaryDataset.setValidity(false, true);
    internalState = InternalState::SHUTDOWN;
  }
  if (internalState == InternalState::SHUTDOWN and commandExecuted) {
    updatePeriodicReply(false, adis1650x::REPLY);
    internalState = InternalState::NONE;
    commandExecuted = false;
    setMode(MODE_OFF);
  }
}

ReturnValue_t GyrAdis1650XHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
  *id = adis1650x::REQUEST;
  return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL);
}

ReturnValue_t GyrAdis1650XHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
  switch (internalState) {
    case (InternalState::STARTUP): {
      if (breakCountdown.isBusy()) {
        return NOTHING_TO_SEND;
      }
      *id = adis1650x::REQUEST;
      adisRequest.cfg.decRateReg = adis1650x::DEC_RATE;
      return preparePeriodicRequest(acs::SimpleSensorMode::NORMAL);
    }
    case (InternalState::SHUTDOWN): {
      *id = adis1650x::REQUEST;
      return preparePeriodicRequest(acs::SimpleSensorMode::OFF);
    }
    default: {
      return NOTHING_TO_SEND;
    }
  }
  return returnvalue::OK;
}

ReturnValue_t GyrAdis1650XHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
                                                           const uint8_t *commandData,
                                                           size_t commandDataLen) {
  return NOTHING_TO_SEND;
}

void GyrAdis1650XHandler::fillCommandAndReplyMap() {
  insertInCommandMap(adis1650x::REQUEST);
  insertInReplyMap(adis1650x::REPLY, 5, nullptr, 0, true);
}

ReturnValue_t GyrAdis1650XHandler::scanForReply(const uint8_t *start, size_t remainingSize,
                                                DeviceCommandId_t *foundId, size_t *foundLen) {
  if (breakCountdown.isBusy() or getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON or
      getMode() == _MODE_POWER_DOWN) {
    return IGNORE_FULL_PACKET;
  }
  if (internalState == InternalState::STARTUP) {
    internalState = InternalState::STARTUP_DONE;
  }
  *foundLen = remainingSize;
  if (remainingSize != sizeof(acs::Adis1650XReply)) {
    return returnvalue::FAILED;
  }
  *foundId = adis1650x::REPLY;
  if (internalState == InternalState::SHUTDOWN) {
    commandExecuted = true;
  }
  return returnvalue::OK;
}

ReturnValue_t GyrAdis1650XHandler::interpretDeviceReply(DeviceCommandId_t id,
                                                        const uint8_t *packet) {
  using namespace adis1650x;
  const acs::Adis1650XReply *reply = reinterpret_cast<const acs::Adis1650XReply *>(packet);
  if (internalState == InternalState::STARTUP and reply->cfgWasSet) {
    switch (adisType) {
      case (adis1650x::Type::ADIS16507): {
        if (reply->cfg.prodId != adis1650x::PROD_ID_16507) {
          sif::error << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID "
                     << reply->cfg.prodId << "for ADIS type 16507" << std::endl;
          return returnvalue::FAILED;
        }
        break;
      }
      case (adis1650x::Type::ADIS16505): {
        if (reply->cfg.prodId != adis1650x::PROD_ID_16505) {
          sif::error << "GyroADIS1650XHandler::interpretDeviceReply: Invalid product ID "
                     << reply->cfg.prodId << "for ADIS type 16505" << std::endl;
          return returnvalue::FAILED;
        }
        break;
      }
    }
    PoolReadGuard rg(&configDataset);
    configDataset.setValidity(true, true);
    configDataset.mscCtrlReg = reply->cfg.mscCtrlReg;
    configDataset.rangMdl = reply->cfg.rangMdl;
    configDataset.diagStatReg = reply->cfg.diagStat;
    configDataset.filterSetting = reply->cfg.filterSetting;
    configDataset.decRateReg = reply->cfg.decRateReg;
    commandExecuted = true;
  }
  if (reply->dataWasSet) {
    {
      PoolReadGuard rg(&configDataset);
      configDataset.diagStatReg = reply->cfg.diagStat;
    }
    auto sensitivity = reply->data.sensitivity;
    auto accelSensitivity = reply->data.accelScaling;
    PoolReadGuard pg(&primaryDataset);
    primaryDataset.setValidity(true, true);
    primaryDataset.angVelocX = static_cast<double>(reply->data.angVelocities[0]) * sensitivity;
    primaryDataset.angVelocY = static_cast<double>(reply->data.angVelocities[1]) * sensitivity;
    primaryDataset.angVelocZ = static_cast<double>(reply->data.angVelocities[2]) * sensitivity;
    // TODO: Check whether we need to divide by INT16_MAX
    primaryDataset.accelX = static_cast<double>(reply->data.accelerations[0]) * accelSensitivity;
    primaryDataset.accelY = static_cast<double>(reply->data.accelerations[1]) * accelSensitivity;
    primaryDataset.accelZ = static_cast<double>(reply->data.accelerations[2]) * accelSensitivity;
    primaryDataset.temperature.value = static_cast<float>(reply->data.temperatureRaw) * 0.1;
  }
  return returnvalue::OK;
}

LocalPoolDataSetBase *GyrAdis1650XHandler::getDataSetHandle(sid_t sid) {
  if (sid.ownerSetId == adis1650x::ADIS_DATASET_ID) {
    return &primaryDataset;
  } else if (sid.ownerSetId == adis1650x::ADIS_CFG_DATASET_ID) {
    return &configDataset;
  }
  return nullptr;
}

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

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

ReturnValue_t GyrAdis1650XHandler::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::RANG_MDL, &rangMdl);
  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;
}

adis1650x::BurstModes GyrAdis1650XHandler::getBurstMode() {
  using namespace adis1650x;
  configDataset.mscCtrlReg.read();
  uint16_t currentCtrlReg = configDataset.mscCtrlReg.value;
  configDataset.mscCtrlReg.commit();
  return adis1650x::burstModeFromMscCtrl(currentCtrlReg);
}

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

ReturnValue_t GyrAdis1650XHandler::preparePeriodicRequest(acs::SimpleSensorMode mode) {
  adisRequest.mode = mode;
  rawPacket = reinterpret_cast<uint8_t *>(&adisRequest);
  rawPacketLen = sizeof(acs::Adis1650XRequest);
  return returnvalue::OK;
}