#include "AcsController.h"

#include <fsfw/datapool/PoolReadGuard.h>
#include <mission/acs/defs.h>
#include <mission/config/torquer.h>

AcsController::AcsController(object_id_t objectId, bool enableHkSets)
    : ExtendedControllerBase(objectId),
      enableHkSets(enableHkSets),
      fusedRotationEstimation(&acsParameters),
      guidance(&acsParameters),
      safeCtrl(&acsParameters),
      ptgCtrl(&acsParameters),
      parameterHelper(this),
      mgmDataRaw(this),
      mgmDataProcessed(this),
      susDataRaw(this),
      susDataProcessed(this),
      gyrDataRaw(this),
      gyrDataProcessed(this),
      gpsDataProcessed(this),
      mekfData(this),
      ctrlValData(this),
      actuatorCmdData(this),
      fusedRotRateData(this),
      tleData(this) {}

ReturnValue_t AcsController::initialize() {
  ReturnValue_t result = parameterHelper.initialize();
  if (result != returnvalue::OK) {
    return result;
  }
  return ExtendedControllerBase::initialize();
}

ReturnValue_t AcsController::handleCommandMessage(CommandMessage *message) {
  ReturnValue_t result = actionHelper.handleActionMessage(message);
  if (result == returnvalue::OK) {
    return result;
  }
  result = parameterHelper.handleParameterMessage(message);
  if (result == returnvalue::OK) {
    return result;
  }
  return result;
}

ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
                                           const uint8_t *data, size_t size) {
  switch (actionId) {
    case SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL: {
      ReturnValue_t result = guidance.solarArrayDeploymentComplete();
      if (result == returnvalue::FAILED) {
        return FILE_DELETION_FAILED;
      }
      return HasActionsIF::EXECUTION_FINISHED;
    }
    case RESET_MEKF: {
      navigation.resetMekf(&mekfData);
      return HasActionsIF::EXECUTION_FINISHED;
    }
    case RESTORE_MEKF_NONFINITE_RECOVERY: {
      mekfLost = false;
      return HasActionsIF::EXECUTION_FINISHED;
    }
    case UPDATE_TLE: {
      if (size != 69 * 2) {
        return INVALID_PARAMETERS;
      }
      ReturnValue_t result = navigation.updateTle(data, data + 69);
      if (result != returnvalue::OK) {
        PoolReadGuard pg(&tleData);
        navigation.updateTle(tleData.line1.value, tleData.line2.value);
        return result;
      }
      {
        PoolReadGuard pg(&tleData);
        if (pg.getReadResult() == returnvalue::OK) {
          std::memcpy(tleData.line1.value, data, 69);
          std::memcpy(tleData.line2.value, data + 69, 69);
          tleData.setValidity(true, true);
        }
      }
      return HasActionsIF::EXECUTION_FINISHED;
    }
    default: {
      return HasActionsIF::INVALID_ACTION_ID;
    }
  }
}

MessageQueueId_t AcsController::getCommandQueue() const { return commandQueue->getId(); }

ReturnValue_t AcsController::getParameter(uint8_t domainId, uint8_t parameterId,
                                          ParameterWrapper *parameterWrapper,
                                          const ParameterWrapper *newValues,
                                          uint16_t startAtIndex) {
  return acsParameters.getParameter(domainId, parameterId, parameterWrapper, newValues,
                                    startAtIndex);
}

void AcsController::performControlOperation() {
#if OBSW_THREAD_TRACING == 1
  trace::threadTrace(opCounter, "ACS & TCS PST");
#endif
  {
    PoolReadGuard pg(&mgmDataRaw);
    if (pg.getReadResult() == returnvalue::OK) {
      copyMgmData();
    }
  }
  {
    PoolReadGuard pg(&susDataRaw);
    if (pg.getReadResult() == returnvalue::OK) {
      copySusData();
    }
  }
  {
    PoolReadGuard pg(&gyrDataRaw);
    if (pg.getReadResult() == returnvalue::OK) {
      copyGyrData();
    }
  }

  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: {
      if (mode != MODE_OFF) {
        performAttitudeControl();
      }
      break;
    }
    default:
      break;
  }
}

void AcsController::performAttitudeControl() {
  timeval timeAbsolute;
  Clock::getClock_timeval(&timeAbsolute);
  timeval timeRelative;
  Clock::getClockMonotonic(&timeRelative);

  ReturnValue_t result = navigation.useSpg4(timeAbsolute, &gpsDataProcessed);
  if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) {
    triggerEvent(acs::TLE_TOO_OLD);
    tleTooOldFlag = true;
  } else if (result != Sgp4Propagator::TLE_TOO_OLD) {
    tleTooOldFlag = false;
  }

  sensorProcessing.process(timeAbsolute, timeRelative, &sensorValues, &mgmDataProcessed,
                           &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
  fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
                                                        &gyrDataProcessed, &fusedRotRateData);
  result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
                              &susDataProcessed, &mekfData, &acsParameters);

  switch (mode) {
    case acs::SAFE:
      if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
          result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
        if (not mekfInvalidFlag) {
          triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value);
          mekfInvalidFlag = true;
        }
        if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
          triggerEvent(acs::MEKF_AUTOMATIC_RESET);
          navigation.resetMekf(&mekfData);
          mekfLost = true;
        }
      } else if (mekfInvalidFlag) {
        triggerEvent(acs::MEKF_RECOVERY);
        mekfInvalidFlag = false;
      }
      switch (submode) {
        case SUBMODE_NONE:
          performSafe();
          break;
        case acs::DETUMBLE:
          performDetumble();
          break;
      }
      break;
    case acs::PTG_IDLE:
    case acs::PTG_TARGET:
    case acs::PTG_TARGET_GS:
    case acs::PTG_NADIR:
    case acs::PTG_INERTIAL:
      if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
          result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
        mekfInvalidCounter++;
        if (not mekfInvalidFlag) {
          triggerEvent(acs::MEKF_INVALID_INFO, (uint32_t)mekfData.mekfStatus.value);
          mekfInvalidFlag = true;
        }
        if (result == MultiplicativeKalmanFilter::MEKF_NOT_FINITE && !mekfLost) {
          triggerEvent(acs::MEKF_AUTOMATIC_RESET);
          navigation.resetMekf(&mekfData);
          mekfLost = true;
        }
        if (mekfInvalidCounter > acsParameters.onBoardParams.mekfViolationTimer) {
          // Trigger this so STR FDIR can set the device faulty.
          EventManagerIF::triggerEvent(objects::STAR_TRACKER, acs::MEKF_INVALID_MODE_VIOLATION, 0,
                                       0);
          mekfInvalidCounter = 0;
        }
        commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration,
                         cmdSpeedRws[0], cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
                         acsParameters.rwHandlingParameters.rampTime);
        return;
      } else {
        if (mekfInvalidFlag) {
          triggerEvent(acs::MEKF_RECOVERY);
          mekfInvalidFlag = false;
        }
        mekfInvalidCounter = 0;
      }
      performPointingCtrl();
      break;
  }
}

void AcsController::performSafe() {
  // get desired satellite rate, sun direction to align to and inertia
  double sunTargetDir[3] = {0, 0, 0};
  guidance.getTargetParamsSafe(sunTargetDir);

  double magMomMtq[3] = {0, 0, 0}, errAng = 0.0;
  acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy(
      mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag,
      gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(),
      fusedRotRateData.rotRateTotal.isValid(), acsParameters.safeModeControllerParameters.useMekf,
      acsParameters.safeModeControllerParameters.useGyr,
      acsParameters.safeModeControllerParameters.dampingDuringEclipse);
  switch (safeCtrlStrat) {
    case (acs::SafeModeStrategy::SAFECTRL_MEKF):
      safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value,
                        susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir,
                        magMomMtq, errAng);
      safeCtrlFailureFlag = false;
      safeCtrlFailureCounter = 0;
      break;
    case (acs::SafeModeStrategy::SAFECTRL_GYR):
      safeCtrl.safeGyr(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value,
                       susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
      safeCtrlFailureFlag = false;
      safeCtrlFailureCounter = 0;
      break;
    case (acs::SafeModeStrategy::SAFECTRL_SUSMGM):
      safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateTotal.value,
                          fusedRotRateData.rotRateParallel.value,
                          fusedRotRateData.rotRateOrthogonal.value,
                          susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng);
      safeCtrlFailureFlag = false;
      safeCtrlFailureCounter = 0;
      break;
    case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR):
      safeCtrl.safeRateDampingGyr(mgmDataProcessed.mgmVecTot.value,
                                  gyrDataProcessed.gyrVecTot.value, sunTargetDir, magMomMtq,
                                  errAng);
      safeCtrlFailureFlag = false;
      safeCtrlFailureCounter = 0;
      break;
    case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM):
      safeCtrl.safeRateDampingSusMgm(mgmDataProcessed.mgmVecTot.value,
                                     fusedRotRateData.rotRateTotal.value, sunTargetDir, magMomMtq,
                                     errAng);
      safeCtrlFailureFlag = false;
      safeCtrlFailureCounter = 0;
      break;
    case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING):
      errAng = NAN;
      safeCtrlFailureFlag = false;
      safeCtrlFailureCounter = 0;
      break;
    case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
      safeCtrlFailure(1, 0);
      break;
    case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
      safeCtrlFailure(0, 1);
      break;
    default:
      sif::error << "AcsController: Invalid safe mode strategy for performSafe" << std::endl;
      break;
  }

  actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
                           acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);

  // detumble check and switch
  if (acsParameters.safeModeControllerParameters.useMekf) {
    if (mekfData.satRotRateMekf.isValid() and
        VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) >
            acsParameters.detumbleParameter.omegaDetumbleStart) {
      detumbleCounter++;
    }
  } else if (acsParameters.safeModeControllerParameters.useGyr) {
    if (gyrDataProcessed.gyrVecTot.isValid() and
        VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) >
            acsParameters.detumbleParameter.omegaDetumbleStart) {
      detumbleCounter++;
    }
  } else if (fusedRotRateData.rotRateTotal.isValid() and
             VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) >
                 acsParameters.detumbleParameter.omegaDetumbleStart) {
    detumbleCounter++;
  } else if (detumbleCounter > 0) {
    detumbleCounter -= 1;
  }
  if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
    detumbleCounter = 0;
    // Triggers detumble mode transition in subsystem
    triggerEvent(acs::SAFE_RATE_VIOLATION);
    startTransition(mode, acs::SafeSubmode::DETUMBLE);
  }

  updateCtrlValData(errAng, safeCtrlStrat);
  updateActuatorCmdData(cmdDipoleMtqs);
  commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
                   acsParameters.magnetorquerParameter.torqueDuration);
}

void AcsController::performDetumble() {
  acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy(
      mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(),
      mgmDataProcessed.mgmVecTotDerivative.isValid(),
      acsParameters.detumbleParameter.useFullDetumbleLaw);
  double magMomMtq[3] = {0, 0, 0};
  switch (safeCtrlStrat) {
    case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL):
      detumble.bDotLawFull(gyrDataProcessed.gyrVecTot.value, mgmDataProcessed.mgmVecTot.value,
                           magMomMtq, acsParameters.detumbleParameter.gainFull);
      break;
    case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED):
      detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTot.value,
                       magMomMtq, acsParameters.detumbleParameter.gainBdot);
      break;
    case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL):
      safeCtrlFailure(1, 0);
      break;
    case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL):
      safeCtrlFailure(0, 1);
      break;
    default:
      sif::error << "AcsController: Invalid safe mode strategy for performDetumble" << std::endl;
      break;
  }

  actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
                           acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs);

  if (acsParameters.safeModeControllerParameters.useMekf) {
    if (mekfData.satRotRateMekf.isValid() and
        VectorOperations<double>::norm(mekfData.satRotRateMekf.value, 3) <
            acsParameters.detumbleParameter.omegaDetumbleEnd) {
      detumbleCounter++;
    }
  } else if (acsParameters.safeModeControllerParameters.useGyr) {
    if (gyrDataProcessed.gyrVecTot.isValid() and
        VectorOperations<double>::norm(gyrDataProcessed.gyrVecTot.value, 3) <
            acsParameters.detumbleParameter.omegaDetumbleEnd) {
      detumbleCounter++;
    }
  } else if (fusedRotRateData.rotRateTotal.isValid() and
             VectorOperations<double>::norm(fusedRotRateData.rotRateTotal.value, 3) <
                 acsParameters.detumbleParameter.omegaDetumbleEnd) {
    detumbleCounter++;
  } else if (detumbleCounter > 0) {
    detumbleCounter -= 1;
  }

  if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
    detumbleCounter = 0;
    // Triggers safe mode transition in subsystem
    triggerEvent(acs::SAFE_RATE_RECOVERY);
    startTransition(mode, acs::SafeSubmode::DEFAULT);
  }

  updateCtrlValData(safeCtrlStrat);
  updateActuatorCmdData(cmdDipoleMtqs);
  commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
                   acsParameters.magnetorquerParameter.torqueDuration);
}

void AcsController::performPointingCtrl() {
  uint8_t enableAntiStiction = true;
  double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
  if (result == returnvalue::FAILED) {
    if (multipleRwUnavailableCounter >=
        acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) {
      triggerEvent(acs::MULTIPLE_RW_INVALID);
      multipleRwUnavailableCounter = 0;
    }
    multipleRwUnavailableCounter++;
    return;
  } else {
    multipleRwUnavailableCounter = 0;
  }

  // Variables required for guidance
  double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 1},
         errorAngle = 0, errorSatRotRate[3] = {0, 0, 0};
  // Variables required for setting actuators
  double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}, torqueRws[4] = {0, 0, 0, 0},
         mgtDpDes[3] = {0, 0, 0};

  switch (mode) {
    case acs::PTG_IDLE:
      guidance.targetQuatPtgSun(now, susDataProcessed.sunIjkModel.value, targetQuat,
                                targetSatRotRate);
      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
                          targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
      ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate,
                     *rwPseudoInv, torquePtgRws);
      ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters,
                           sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
                           sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
                           rwTrqNs);
      VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
      actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
      ptgCtrl.ptgDesaturation(
          &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
          mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
      enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction;
      break;

    case acs::PTG_TARGET:
      guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value,
                                      gpsDataProcessed.gpsVelocity.value, targetQuat,
                                      targetSatRotRate);
      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
                          targetSatRotRate, acsParameters.targetModeControllerParameters.quatRef,
                          acsParameters.targetModeControllerParameters.refRotRate, errorQuat,
                          errorSatRotRate, errorAngle);
      ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate,
                     *rwPseudoInv, torquePtgRws);
      ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters,
                           sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
                           sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
                           rwTrqNs);
      VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
      actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
      ptgCtrl.ptgDesaturation(
          &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
          mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
      enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
      break;

    case acs::PTG_TARGET_GS:
      guidance.targetQuatPtgGs(now, gpsDataProcessed.gpsPosition.value,
                               susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate);
      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
                          targetSatRotRate, errorQuat, errorSatRotRate, errorAngle);
      ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate,
                     *rwPseudoInv, torquePtgRws);
      ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters,
                           sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
                           sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
                           rwTrqNs);
      VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
      actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
      ptgCtrl.ptgDesaturation(
          &acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
          mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
      enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction;
      break;

    case acs::PTG_NADIR:
      guidance.targetQuatPtgNadirThreeAxes(now, gpsDataProcessed.gpsPosition.value,
                                           gpsDataProcessed.gpsVelocity.value, targetQuat,
                                           targetSatRotRate);
      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
                          targetSatRotRate, acsParameters.nadirModeControllerParameters.quatRef,
                          acsParameters.nadirModeControllerParameters.refRotRate, errorQuat,
                          errorSatRotRate, errorAngle);
      ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate,
                     *rwPseudoInv, torquePtgRws);
      ptgCtrl.ptgNullspace(&acsParameters.nadirModeControllerParameters,
                           sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
                           sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
                           rwTrqNs);
      VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
      actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
      ptgCtrl.ptgDesaturation(
          &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
          mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
      enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
      break;

    case acs::PTG_INERTIAL:
      std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat,
                  sizeof(targetQuat));
      guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat,
                          targetSatRotRate, acsParameters.inertialModeControllerParameters.quatRef,
                          acsParameters.inertialModeControllerParameters.refRotRate, errorQuat,
                          errorSatRotRate, errorAngle);
      ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate,
                     *rwPseudoInv, torquePtgRws);
      ptgCtrl.ptgNullspace(&acsParameters.inertialModeControllerParameters,
                           sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
                           sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
                           rwTrqNs);
      VectorOperations<double>::add(torquePtgRws, rwTrqNs, torqueRws, 4);
      actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq);
      ptgCtrl.ptgDesaturation(
          &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value,
          mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value,
          sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
          sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes);
      enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
      break;
    default:
      sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl;
      break;
  }

  actuatorCmd.cmdSpeedToRws(
      sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value,
      sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
      acsParameters.onBoardParams.sampleTime, acsParameters.rwHandlingParameters.inertiaWheel,
      acsParameters.rwHandlingParameters.maxRwSpeed, torqueRws, cmdSpeedRws);
  if (enableAntiStiction) {
    ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws);
  }
  actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
                           acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes, cmdDipoleMtqs);

  updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate);
  updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipoleMtqs);
  commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
                   acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
                   cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
                   acsParameters.rwHandlingParameters.rampTime);
}

void AcsController::safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure) {
  if (not safeCtrlFailureFlag) {
    triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE, mgmFailure, sensorFailure);
    safeCtrlFailureFlag = true;
  }
  safeCtrlFailureCounter++;
  if (safeCtrlFailureCounter > 150) {
    safeCtrlFailureFlag = false;
    safeCtrlFailureCounter = 0;
  }
}

ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
                                              uint16_t dipoleTorqueDuration) {
  {
    PoolReadGuard pg(&dipoleSet);
    MutexGuard mg(torquer::lazyLock(), torquer::LOCK_TYPE, torquer::LOCK_TIMEOUT,
                  torquer::LOCK_CTX);
    torquer::NEW_ACTUATION_FLAG = true;
    dipoleSet.setDipoles(xDipole, yDipole, zDipole, dipoleTorqueDuration);
  }
  return returnvalue::OK;
}

ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole,
                                              uint16_t dipoleTorqueDuration, int32_t rw1Speed,
                                              int32_t rw2Speed, int32_t rw3Speed, int32_t rw4Speed,
                                              uint16_t rampTime) {
  {
    PoolReadGuard pg(&dipoleSet);
    MutexGuard mg(torquer::lazyLock(), torquer::LOCK_TYPE, torquer::LOCK_TIMEOUT,
                  torquer::LOCK_CTX);
    torquer::NEW_ACTUATION_FLAG = true;
    dipoleSet.setDipoles(xDipole, yDipole, zDipole, dipoleTorqueDuration);
  }
  {
    PoolReadGuard pg(&rw1SpeedSet);
    rw1SpeedSet.setRwSpeed(rw1Speed, rampTime);
  }
  {
    PoolReadGuard pg(&rw2SpeedSet);
    rw2SpeedSet.setRwSpeed(rw2Speed, rampTime);
  }
  {
    PoolReadGuard pg(&rw3SpeedSet);
    rw3SpeedSet.setRwSpeed(rw3Speed, rampTime);
  }
  {
    PoolReadGuard pg(&rw4SpeedSet);
    rw4SpeedSet.setRwSpeed(rw4Speed, rampTime);
  }
  return returnvalue::OK;
}

void AcsController::updateActuatorCmdData(const int16_t *mtqTargetDipole) {
  updateActuatorCmdData(RW_OFF_TORQUE, RW_OFF_SPEED, mtqTargetDipole);
}

void AcsController::updateActuatorCmdData(const double *rwTargetTorque,
                                          const int32_t *rwTargetSpeed,
                                          const int16_t *mtqTargetDipole) {
  PoolReadGuard pg(&actuatorCmdData);
  if (pg.getReadResult() == returnvalue::OK) {
    std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTargetTorque, 4 * sizeof(double));
    std::memcpy(actuatorCmdData.rwTargetSpeed.value, rwTargetSpeed, 4 * sizeof(int32_t));
    std::memcpy(actuatorCmdData.mtqTargetDipole.value, mtqTargetDipole, 3 * sizeof(int16_t));
    actuatorCmdData.setValidity(true, true);
  }
}

void AcsController::updateCtrlValData(uint8_t safeModeStrat) {
  PoolReadGuard pg(&ctrlValData);
  if (pg.getReadResult() == returnvalue::OK) {
    std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
    ctrlValData.tgtQuat.setValid(false);
    std::memcpy(ctrlValData.errQuat.value, ZERO_VEC4, 4 * sizeof(double));
    ctrlValData.errQuat.setValid(false);
    ctrlValData.errAng.value = 0;
    ctrlValData.errAng.setValid(false);
    std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
    ctrlValData.tgtRotRate.setValid(false);
    ctrlValData.safeStrat.value = safeModeStrat;
    ctrlValData.safeStrat.setValid(true);
    ctrlValData.setValidity(true, false);
  }
}

void AcsController::updateCtrlValData(double errAng, uint8_t safeModeStrat) {
  PoolReadGuard pg(&ctrlValData);
  if (pg.getReadResult() == returnvalue::OK) {
    std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
    ctrlValData.tgtQuat.setValid(false);
    std::memcpy(ctrlValData.errQuat.value, ZERO_VEC4, 4 * sizeof(double));
    ctrlValData.errQuat.setValid(false);
    ctrlValData.errAng.value = errAng;
    ctrlValData.errAng.setValid(true);
    std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
    ctrlValData.tgtRotRate.setValid(false);
    ctrlValData.safeStrat.value = safeModeStrat;
    ctrlValData.safeStrat.setValid(true);
    ctrlValData.setValidity(true, false);
  }
}

void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQuat, double errAng,
                                      const double *tgtRotRate) {
  PoolReadGuard pg(&ctrlValData);
  if (pg.getReadResult() == returnvalue::OK) {
    std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double));
    std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double));
    ctrlValData.errAng.value = errAng;
    std::memcpy(ctrlValData.tgtRotRate.value, tgtRotRate, 3 * sizeof(double));
    ctrlValData.safeStrat.value = acs::SafeModeStrategy::SAFECTRL_OFF;
    ctrlValData.setValidity(true, true);
  }
}

ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                     LocalDataPoolManager &poolManager) {
  // MGM Raw
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_LIS3_UT, &mgm0VecRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_RM3100_UT, &mgm1VecRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_LIS3_UT, &mgm2VecRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_RM3100_UT, &mgm3VecRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_NT, &imtqMgmVecRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_IMTQ_CAL_ACT_STATUS, &imtqCalActStatus);
  poolManager.subscribeForRegularPeriodicPacket({mgmDataRaw.getSid(), false, 10.0});
  // MGM Processed
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_0_VEC, &mgm0VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_1_VEC, &mgm1VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_2_VEC, &mgm2VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_3_VEC, &mgm3VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_4_VEC, &mgm4VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT, &mgmVecTot);
  localDataPoolMap.emplace(acsctrl::PoolIds::MGM_VEC_TOT_DERIVATIVE, &mgmVecTotDer);
  localDataPoolMap.emplace(acsctrl::PoolIds::MAG_IGRF_MODEL, &magIgrf);
  poolManager.subscribeForRegularPeriodicPacket({mgmDataProcessed.getSid(), enableHkSets, 10.0});
  // SUS Raw
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_N_LOC_XFYFZM_PT_XF, &sus0ValRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_N_LOC_XBYFZM_PT_XB, &sus1ValRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_N_LOC_XFYBZB_PT_YB, &sus2ValRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_N_LOC_XFYBZF_PT_YF, &sus3ValRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_N_LOC_XMYFZF_PT_ZF, &sus4ValRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_N_LOC_XFYMZB_PT_ZB, &sus5ValRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_R_LOC_XFYBZM_PT_XF, &sus6ValRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_R_LOC_XBYBZM_PT_XB, &sus7ValRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_R_LOC_XBYBZB_PT_YB, &sus8ValRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_R_LOC_XBYBZB_PT_YF, &sus9ValRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_N_LOC_XMYBZF_PT_ZF, &sus10ValRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, &sus11ValRaw);
  poolManager.subscribeForRegularPeriodicPacket({susDataRaw.getSid(), false, 10.0});
  // SUS Processed
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_VEC, &sus0VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_VEC, &sus1VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_VEC, &sus2VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_VEC, &sus3VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_VEC, &sus4VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_VEC, &sus5VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_VEC, &sus6VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_VEC, &sus7VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_VEC, &sus8VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_VEC, &sus9VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_VEC, &sus10VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_VEC, &sus11VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT, &susVecTot);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_VEC_TOT_DERIVATIVE, &susVecTotDer);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUN_IJK_MODEL, &sunIjk);
  poolManager.subscribeForRegularPeriodicPacket({susDataProcessed.getSid(), enableHkSets, 10.0});
  // GYR Raw
  localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_ADIS, &gyr0VecRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_L3, &gyr1VecRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_ADIS, &gyr2VecRaw);
  localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_L3, &gyr3VecRaw);
  poolManager.subscribeForDiagPeriodicPacket({gyrDataRaw.getSid(), false, 10.0});
  // GYR Processed
  localDataPoolMap.emplace(acsctrl::PoolIds::GYR_0_VEC, &gyr0VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::GYR_1_VEC, &gyr1VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::GYR_2_VEC, &gyr2VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::GYR_3_VEC, &gyr3VecProc);
  localDataPoolMap.emplace(acsctrl::PoolIds::GYR_VEC_TOT, &gyrVecTot);
  poolManager.subscribeForDiagPeriodicPacket({gyrDataProcessed.getSid(), enableHkSets, 10.0});
  // GPS Processed
  localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude);
  localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude);
  localDataPoolMap.emplace(acsctrl::PoolIds::ALTITUDE, &altitude);
  localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition);
  localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity);
  localDataPoolMap.emplace(acsctrl::PoolIds::SOURCE, &gpsSource);
  poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), enableHkSets, 30.0});
  // MEKF
  localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
  localDataPoolMap.emplace(acsctrl::PoolIds::SAT_ROT_RATE_MEKF, &satRotRateMekf);
  localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus);
  poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), enableHkSets, 10.0});
  // Ctrl Values
  localDataPoolMap.emplace(acsctrl::PoolIds::SAFE_STRAT, &safeStrat);
  localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat);
  localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat);
  localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng);
  localDataPoolMap.emplace(acsctrl::PoolIds::TGT_ROT_RATE, &tgtRotRate);
  poolManager.subscribeForRegularPeriodicPacket({ctrlValData.getSid(), enableHkSets, 10.0});
  // Actuator CMD
  localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_TORQUE, &rwTargetTorque);
  localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_SPEED, &rwTargetSpeed);
  localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole);
  poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), enableHkSets, 10.0});
  // Fused Rot Rate
  localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL, &rotRateOrthogonal);
  localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL, &rotRateParallel);
  localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL, &rotRateTotal);
  poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 10.0});
  // TLE Data
  localDataPoolMap.emplace(acsctrl::PoolIds::TLE_LINE_1, &line1);
  localDataPoolMap.emplace(acsctrl::PoolIds::TLE_LINE_2, &line2);
  return returnvalue::OK;
}

LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) {
  switch (sid.ownerSetId) {
    case acsctrl::MGM_SENSOR_DATA:
      return &mgmDataRaw;
    case acsctrl::MGM_PROCESSED_DATA:
      return &mgmDataProcessed;
    case acsctrl::SUS_SENSOR_DATA:
      return &susDataRaw;
    case acsctrl::SUS_PROCESSED_DATA:
      return &susDataProcessed;
    case acsctrl::GYR_SENSOR_DATA:
      return &gyrDataRaw;
    case acsctrl::GYR_PROCESSED_DATA:
      return &gyrDataProcessed;
    case acsctrl::GPS_PROCESSED_DATA:
      return &gpsDataProcessed;
    case acsctrl::MEKF_DATA:
      return &mekfData;
    case acsctrl::CTRL_VAL_DATA:
      return &ctrlValData;
    case acsctrl::ACTUATOR_CMD_DATA:
      return &actuatorCmdData;
    case acsctrl::FUSED_ROTATION_RATE_DATA:
      return &fusedRotRateData;
    default:
      return nullptr;
  }
  return nullptr;
}

ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
                                              uint32_t *msToReachTheMode) {
  if (mode == MODE_OFF) {
    if (submode == SUBMODE_NONE) {
      return returnvalue::OK;
    } else {
      return INVALID_SUBMODE;
    }
  } else if (not((mode < acs::AcsMode::SAFE) or (mode > acs::AcsMode::PTG_INERTIAL))) {
    if (mode == acs::AcsMode::SAFE) {
      if (not((submode == SUBMODE_NONE) or (submode == acs::SafeSubmode::DETUMBLE))) {
        return INVALID_SUBMODE;
      } else {
        return returnvalue::OK;
      }
    } else if (not(submode == SUBMODE_NONE)) {
      return INVALID_SUBMODE;
    } else {
      return returnvalue::OK;
    }
  }
  return INVALID_MODE;
}

void AcsController::modeChanged(Mode_t mode, Submode_t submode) {
  return ExtendedControllerBase::modeChanged(mode, submode);
}

void AcsController::announceMode(bool recursive) {
  const char *modeStr = "UNKNOWN";
  if (mode == HasModesIF::MODE_OFF) {
    modeStr = "OFF";
  } else {
    modeStr = acs::getModeStr(static_cast<acs::AcsMode>(mode));
  }
  const char *submodeStr = "UNKNOWN";
  if (submode == HasModesIF::SUBMODE_NONE) {
    submodeStr = "NONE";
  }
  if (mode == acs::AcsMode::SAFE) {
    acs::SafeSubmode safeSubmode = static_cast<acs::SafeSubmode>(this->submode);
    if (safeSubmode == acs::SafeSubmode::DETUMBLE) {
      submodeStr = "DETUMBLE";
    }
  }
  sif::info << "ACS controller is now in " << modeStr << " mode with " << submodeStr << " submode"
            << std::endl;
  return ExtendedControllerBase::announceMode(recursive);
}

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

void AcsController::copySusData() {
  {
    PoolReadGuard pg(&sensorValues.susSets[0]);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(susDataRaw.sus0.value, sensorValues.susSets[0].channels.value,
                  6 * sizeof(uint16_t));
      susDataRaw.sus0.setValid(sensorValues.susSets[0].channels.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.susSets[1]);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(susDataRaw.sus1.value, sensorValues.susSets[1].channels.value,
                  6 * sizeof(uint16_t));
      susDataRaw.sus1.setValid(sensorValues.susSets[1].channels.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.susSets[2]);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(susDataRaw.sus2.value, sensorValues.susSets[2].channels.value,
                  6 * sizeof(uint16_t));
      susDataRaw.sus2.setValid(sensorValues.susSets[2].channels.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.susSets[3]);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(susDataRaw.sus3.value, sensorValues.susSets[3].channels.value,
                  6 * sizeof(uint16_t));
      susDataRaw.sus3.setValid(sensorValues.susSets[3].channels.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.susSets[4]);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(susDataRaw.sus4.value, sensorValues.susSets[4].channels.value,
                  6 * sizeof(uint16_t));
      susDataRaw.sus4.setValid(sensorValues.susSets[4].channels.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.susSets[5]);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(susDataRaw.sus5.value, sensorValues.susSets[5].channels.value,
                  6 * sizeof(uint16_t));
      susDataRaw.sus5.setValid(sensorValues.susSets[5].channels.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.susSets[6]);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(susDataRaw.sus6.value, sensorValues.susSets[6].channels.value,
                  6 * sizeof(uint16_t));
      susDataRaw.sus6.setValid(sensorValues.susSets[6].channels.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.susSets[7]);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(susDataRaw.sus7.value, sensorValues.susSets[7].channels.value,
                  6 * sizeof(uint16_t));
      susDataRaw.sus7.setValid(sensorValues.susSets[7].channels.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.susSets[8]);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(susDataRaw.sus8.value, sensorValues.susSets[8].channels.value,
                  6 * sizeof(uint16_t));
      susDataRaw.sus8.setValid(sensorValues.susSets[8].channels.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.susSets[9]);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(susDataRaw.sus9.value, sensorValues.susSets[9].channels.value,
                  6 * sizeof(uint16_t));
      susDataRaw.sus9.setValid(sensorValues.susSets[9].channels.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.susSets[10]);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(susDataRaw.sus10.value, sensorValues.susSets[10].channels.value,
                  6 * sizeof(uint16_t));
      susDataRaw.sus10.setValid(sensorValues.susSets[10].channels.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.susSets[11]);
    if (pg.getReadResult() == returnvalue::OK) {
      std::memcpy(susDataRaw.sus11.value, sensorValues.susSets[11].channels.value,
                  6 * sizeof(uint16_t));
      susDataRaw.sus11.setValid(sensorValues.susSets[11].channels.isValid());
    }
  }
}

void AcsController::copyGyrData() {
  {
    PoolReadGuard pg(&sensorValues.gyr0AdisSet);
    if (pg.getReadResult() == returnvalue::OK) {
      gyrDataRaw.gyr0Adis.value[0] = sensorValues.gyr0AdisSet.angVelocX.value;
      gyrDataRaw.gyr0Adis.value[1] = sensorValues.gyr0AdisSet.angVelocY.value;
      gyrDataRaw.gyr0Adis.value[2] = sensorValues.gyr0AdisSet.angVelocZ.value;
      gyrDataRaw.gyr0Adis.setValid(sensorValues.gyr0AdisSet.angVelocX.isValid() &&
                                   sensorValues.gyr0AdisSet.angVelocY.isValid() &&
                                   sensorValues.gyr0AdisSet.angVelocZ.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.gyr1L3gSet);
    if (pg.getReadResult() == returnvalue::OK) {
      gyrDataRaw.gyr1L3.value[0] = sensorValues.gyr1L3gSet.angVelocX.value;
      gyrDataRaw.gyr1L3.value[1] = sensorValues.gyr1L3gSet.angVelocY.value;
      gyrDataRaw.gyr1L3.value[2] = sensorValues.gyr1L3gSet.angVelocZ.value;
      gyrDataRaw.gyr1L3.setValid(sensorValues.gyr1L3gSet.angVelocX.isValid() &&
                                 sensorValues.gyr1L3gSet.angVelocY.isValid() &&
                                 sensorValues.gyr1L3gSet.angVelocZ.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.gyr2AdisSet);
    if (pg.getReadResult() == returnvalue::OK) {
      gyrDataRaw.gyr2Adis.value[0] = sensorValues.gyr2AdisSet.angVelocX.value;
      gyrDataRaw.gyr2Adis.value[1] = sensorValues.gyr2AdisSet.angVelocY.value;
      gyrDataRaw.gyr2Adis.value[2] = sensorValues.gyr2AdisSet.angVelocZ.value;
      gyrDataRaw.gyr2Adis.setValid(sensorValues.gyr2AdisSet.angVelocX.isValid() &&
                                   sensorValues.gyr2AdisSet.angVelocY.isValid() &&
                                   sensorValues.gyr2AdisSet.angVelocZ.isValid());
    }
  }
  {
    PoolReadGuard pg(&sensorValues.gyr3L3gSet);
    if (pg.getReadResult() == returnvalue::OK) {
      gyrDataRaw.gyr3L3.value[0] = sensorValues.gyr3L3gSet.angVelocX.value;
      gyrDataRaw.gyr3L3.value[1] = sensorValues.gyr3L3gSet.angVelocY.value;
      gyrDataRaw.gyr3L3.value[2] = sensorValues.gyr3L3gSet.angVelocZ.value;
      gyrDataRaw.gyr3L3.setValid(sensorValues.gyr3L3gSet.angVelocX.isValid() &&
                                 sensorValues.gyr3L3gSet.angVelocY.isValid() &&
                                 sensorValues.gyr3L3gSet.angVelocZ.isValid());
    }
  }
}