#include "MgmLis3CustomHandler.h"

#include <cmath>

#include "fsfw/datapool/PoolReadGuard.h"

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

MgmLis3CustomHandler::~MgmLis3CustomHandler() = default;

void MgmLis3CustomHandler::doStartUp() {
  if (internalState != InternalState::STARTUP) {
    commandExecuted = false;
    updatePeriodicReply(true, REPLY);
    internalState = InternalState::STARTUP;
  }
  if (internalState == InternalState::STARTUP) {
    if (commandExecuted) {
      setMode(MODE_ON);
      internalState = InternalState::NONE;
      commandExecuted = false;
    }
  }
}

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

ReturnValue_t MgmLis3CustomHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
  if (internalState == InternalState::STARTUP) {
    *id = REQUEST;
    return prepareRequest(acs::SimpleSensorMode::NORMAL);
  } else if (internalState == InternalState::SHUTDOWN) {
    *id = REQUEST;
    return prepareRequest(acs::SimpleSensorMode::OFF);
  }
  return NOTHING_TO_SEND;
}

ReturnValue_t MgmLis3CustomHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) {
  *id = REQUEST;
  return prepareRequest(acs::SimpleSensorMode::NORMAL);
}

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

ReturnValue_t MgmLis3CustomHandler::scanForReply(const uint8_t *start, size_t len,
                                                 DeviceCommandId_t *foundId, size_t *foundLen) {
  if (getMode() == _MODE_WAIT_OFF or getMode() == _MODE_WAIT_ON) {
    return IGNORE_FULL_PACKET;
  }
  if (len != sizeof(acs::MgmLis3Reply)) {
    *foundLen = len;
    return returnvalue::FAILED;
  }
  *foundId = REPLY;
  *foundLen = len;
  if (internalState == InternalState::SHUTDOWN) {
    commandExecuted = true;
  }
  return returnvalue::OK;
}
ReturnValue_t MgmLis3CustomHandler::interpretDeviceReply(DeviceCommandId_t id,
                                                         const uint8_t *packet) {
  const auto *reply = reinterpret_cast<const acs::MgmLis3Reply *>(packet);
  if (reply->dataWasSet) {
    if (internalState == InternalState::STARTUP) {
      commandExecuted = true;
    }
    PoolReadGuard pg(&dataset);
    for (uint8_t idx = 0; idx < 3; idx++) {
      dataset.fieldStrengths[idx] =
          reply->mgmValuesRaw[idx] * reply->sensitivity * mgmLis3::GAUSS_TO_MICROTESLA_FACTOR;
    }

    dataset.setValidity(true, true);
    if (std::abs(dataset.fieldStrengths[0]) > absLimitX or
        std::abs(dataset.fieldStrengths[1]) > absLimitY or
        std::abs(dataset.fieldStrengths[2]) > absLimitZ) {
      dataset.fieldStrengths.setValid(false);
    }
    dataset.temperature = 25.0 + ((static_cast<float>(reply->temperatureRaw)) / 8.0);
  }
  return returnvalue::OK;
}

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

void MgmLis3CustomHandler::setToGoToNormalMode(bool enable) { this->goToNormalMode = enable; }

uint32_t MgmLis3CustomHandler::getTransitionDelayMs(Mode_t from, Mode_t to) {
  return transitionDelay;
}

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

ReturnValue_t MgmLis3CustomHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                            LocalDataPoolManager &poolManager) {
  localDataPoolMap.emplace(mgmLis3::FIELD_STRENGTHS, &mgmXYZ);
  localDataPoolMap.emplace(mgmLis3::TEMPERATURE_CELCIUS, &temperature);
  poolManager.subscribeForRegularPeriodicPacket({dataset.getSid(), false, 10.0});
  return returnvalue::OK;
}

void MgmLis3CustomHandler::setAbsoluteLimits(float xLimit, float yLimit, float zLimit) {
  this->absLimitX = xLimit;
  this->absLimitY = yLimit;
  this->absLimitZ = zLimit;
}

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

ReturnValue_t MgmLis3CustomHandler::prepareRequest(acs::SimpleSensorMode mode) {
  request.mode = mode;
  rawPacket = reinterpret_cast<uint8_t *>(&request);
  rawPacketLen = sizeof(acs::MgmLis3Request);
  return returnvalue::OK;
}

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