#include <mission/acs/GyrL3gCustomHandler.h>
#include <mission/acs/acsBoardPolling.h>

#include <cmath>

#include "fsfw/datapool/PoolReadGuard.h"

GyrL3gCustomHandler::GyrL3gCustomHandler(object_id_t objectId, object_id_t deviceCommunication,
                                         CookieIF *comCookie, uint32_t transitionDelayMs)
    : DeviceHandlerBase(objectId, deviceCommunication, comCookie),
      transitionDelayMs(transitionDelayMs),
      dataset(this) {}

GyrL3gCustomHandler::~GyrL3gCustomHandler() = default;

void GyrL3gCustomHandler::doStartUp() {
  if (internalState != InternalState::STARTUP) {
    internalState = InternalState::STARTUP;
    updatePeriodicReply(true, l3gd20h::REPLY);
    commandExecuted = false;
  }

  if (internalState == InternalState::STARTUP) {
    if (commandExecuted) {
      setMode(MODE_ON);
      internalState = InternalState::NONE;
      commandExecuted = false;
    }
  }
}

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

ReturnValue_t GyrL3gCustomHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
  switch (internalState) {
    case (InternalState::NONE):
    case (InternalState::NORMAL): {
      return NOTHING_TO_SEND;
    }
    case (InternalState::STARTUP): {
      *id = l3gd20h::REQUEST;
      gyrRequest.ctrlRegs[0] = l3gd20h::CTRL_REG_1_VAL;
      gyrRequest.ctrlRegs[1] = l3gd20h::CTRL_REG_2_VAL;
      gyrRequest.ctrlRegs[2] = l3gd20h::CTRL_REG_3_VAL;
      gyrRequest.ctrlRegs[3] = l3gd20h::CTRL_REG_4_VAL;
      gyrRequest.ctrlRegs[4] = l3gd20h::CTRL_REG_5_VAL;
      return doPeriodicRequest(acs::SimpleSensorMode::NORMAL);
    }
    case (InternalState::SHUTDOWN): {
      *id = l3gd20h::REQUEST;
      return doPeriodicRequest(acs::SimpleSensorMode::OFF);
    }
    default:
#if FSFW_CPP_OSTREAM_ENABLED == 1
      /* Might be a configuration error. */
      sif::warning << "GyroL3GD20Handler::buildTransitionDeviceCommand: "
                      "Unknown internal state!"
                   << std::endl;
#else
      sif::printDebug(
          "GyroL3GD20Handler::buildTransitionDeviceCommand: "
          "Unknown internal state!\n");
#endif
      return returnvalue::OK;
  }
  return returnvalue::OK;
}

ReturnValue_t GyrL3gCustomHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
  *id = l3gd20h::REQUEST;
  return doPeriodicRequest(acs::SimpleSensorMode::NORMAL);
}

ReturnValue_t GyrL3gCustomHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand,
                                                           const uint8_t *commandData,
                                                           size_t commandDataLen) {
  switch (deviceCommand) {
    default:
      return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
  }
  return returnvalue::OK;
}

ReturnValue_t GyrL3gCustomHandler::scanForReply(const uint8_t *start, size_t len,
                                                DeviceCommandId_t *foundId, size_t *foundLen) {
  if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON or getMode() == _MODE_POWER_DOWN) {
    return IGNORE_FULL_PACKET;
  }
  *foundLen = len;
  if (len != sizeof(acs::GyroL3gReply)) {
    return returnvalue::FAILED;
  }
  *foundId = adis1650x::REPLY;
  if (internalState == InternalState::SHUTDOWN) {
    commandExecuted = true;
  }
  return returnvalue::OK;
}

ReturnValue_t GyrL3gCustomHandler::interpretDeviceReply(DeviceCommandId_t id,
                                                        const uint8_t *packet) {
  const acs::GyroL3gReply *reply = reinterpret_cast<const acs::GyroL3gReply *>(packet);
  if (reply->cfgWasSet) {
    if (internalState == InternalState::STARTUP) {
      commandExecuted = true;
    }
    PoolReadGuard pg(&dataset);
    dataset.setValidity(true, true);
    dataset.angVelocX = static_cast<float>(reply->angVelocities[0]) * reply->sensitivity;
    dataset.angVelocY = static_cast<float>(reply->angVelocities[1]) * reply->sensitivity;
    dataset.angVelocZ = static_cast<float>(reply->angVelocities[2]) * reply->sensitivity;
    if (std::abs(dataset.angVelocX.value) > absLimitX) {
      dataset.angVelocX.setValid(false);
    }
    if (std::abs(dataset.angVelocY.value) > absLimitY) {
      dataset.angVelocY.setValid(false);
    }
    if (std::abs(dataset.angVelocZ.value) > absLimitZ) {
      dataset.angVelocZ.setValid(false);
    }
    dataset.temperature = 25.0 - reply->tempOffsetRaw;
  }
  return returnvalue::OK;
}

uint32_t GyrL3gCustomHandler::getTransitionDelayMs(Mode_t from, Mode_t to) {
  return this->transitionDelayMs;
}

ReturnValue_t GyrL3gCustomHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                           LocalDataPoolManager &poolManager) {
  localDataPoolMap.emplace(l3gd20h::ANG_VELOC_X, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Y, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Z, new PoolEntry<float>({0.0}));
  localDataPoolMap.emplace(l3gd20h::TEMPERATURE, new PoolEntry<float>({0.0}));
  poolManager.subscribeForRegularPeriodicPacket(
      subdp::RegularHkPeriodicParams(dataset.getSid(), false, 10.0));
  return returnvalue::OK;
}

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

void GyrL3gCustomHandler::modeChanged() { internalState = InternalState::NONE; }

void GyrL3gCustomHandler::setAbsoluteLimits(float limitX, float limitY, float limitZ) {
  this->absLimitX = limitX;
  this->absLimitY = limitY;
  this->absLimitZ = limitZ;
}

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

LocalPoolDataSetBase *GyrL3gCustomHandler::getDataSetHandle(sid_t sid) {
  if (sid == dataset.getSid()) {
    return &dataset;
  }
  return nullptr;
}

ReturnValue_t GyrL3gCustomHandler::doPeriodicRequest(acs::SimpleSensorMode mode) {
  gyrRequest.mode = mode;
  rawPacket = reinterpret_cast<uint8_t *>(&gyrRequest);
  rawPacketLen = sizeof(acs::GyroL3gRequest);
  return returnvalue::OK;
}