#include "AcsController.h"

#include <fsfw/datapool/PoolReadGuard.h>

#include "mission/devices/torquer.h"

AcsController::AcsController(object_id_t objectId)
    : ExtendedControllerBase(objectId, objects::NO_OBJECT), mgmData(this) {}

ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
  return returnvalue::OK;
}

void AcsController::performControlOperation() {
  switch (internalState) {
    case InternalState::STARTUP: {
      initialCountdown.resetTimer();
      internalState = InternalState::INITIAL_DELAY;
      return;
    }
    case InternalState::INITIAL_DELAY: {
      if (initialCountdown.hasTimedOut()) {
        internalState = InternalState::READY;
      }
      return;
    }
    case InternalState::READY: {
      break;
    }
    default:
      break;
  }

  {
    // TODO: Calculate actuator output
    PoolReadGuard pg(&dipoleSet);
    MutexGuard mg(torquer::lazyLock());
    torquer::NEW_ACTUATION_FLAG = true;
    // TODO: Insert correct values here
    dipoleSet.setDipoles(500, 500, 500, 150);
  }

  {
    PoolReadGuard pg(&mgmData);
    if (pg.getReadResult() == returnvalue::OK) {
      copyMgmData();
    }
  }
}

ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                     LocalDataPoolManager &poolManager) {
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0PoolVec);
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_RM3100_UT, &mgm1PoolVec);
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_LIS3_UT, &mgm2PoolVec);
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_RM3100_UT, &mgm3PoolVec);
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmPoolVec);
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_ACT_STATUS, &imtqCalActStatus);
  poolManager.subscribeForRegularPeriodicPacket({mgmData.getSid(), false, 5.0});
  return returnvalue::OK;
}

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

ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
                                              uint32_t *msToReachTheMode) {
  return returnvalue::OK;
}

void AcsController::copyMgmData() {
  {
    PoolReadGuard pg(&mgm0Lis3Set);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(mgmData.mgm0Lis3.value, mgm0Lis3Set.fieldStrengths.value, 3 * sizeof(float));
    }
  }
  {
    PoolReadGuard pg(&mgm1Rm3100Set);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(mgmData.mgm1Rm3100.value, mgm1Rm3100Set.fieldStrengths.value, 3 * sizeof(float));
    }
  }
  {
    PoolReadGuard pg(&mgm2Lis3Set);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(mgmData.mgm2Lis3.value, mgm2Lis3Set.fieldStrengths.value, 3 * sizeof(float));
    }
  }
  {
    PoolReadGuard pg(&mgm3Rm3100Set);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(mgmData.mgm3Rm3100.value, mgm3Rm3100Set.fieldStrengths.value, 3 * sizeof(float));
    }
  }
  {
    PoolReadGuard pg(&imtqMgmSet);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(mgmData.imtqRaw.value, imtqMgmSet.mtmRawNt.value, 3 * sizeof(float));
      mgmData.actuationCalStatus.value = imtqMgmSet.coilActuationStatus.value;
    }
  }
}