#include "AcsController.h"

#include <fsfw/datapool/PoolReadGuard.h>

AcsController::AcsController(object_id_t objectId)
    : ExtendedControllerBase(objectId, objects::NO_OBJECT),
      sensorProcessing(&acsParameters),
      navigation(&acsParameters),
      actuatorCmd(&acsParameters),
      guidance(&acsParameters),
      detumble(&acsParameters),
      ptgCtrl(&acsParameters),
      detumbleCounter{0},
      mgmData(this),
      susData(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: {
      if (mode != MODE_OFF) {
        switch (submode) {
          case SUBMODE_SAFE:
            // performSafe();
            break;

          case SUBMODE_DETUMBLE:
            performDetumble();
            break;

          case SUBMODE_PTG_GS:
            performPointingCtrl();
            break;
          case SUBMODE_PTG_SUN:
             performPointingCtrlSun();
             break;
        }
      }
      break;
    }
    default:
      break;
  }

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

  // DEBUG : REMOVE AFTER COMPLETION
  mode = MODE_ON;
  submode = SUBMODE_DETUMBLE;
  // DEBUG END
}

void AcsController::performSafe() {}

void AcsController::performDetumble() {
  ACS::SensorValues sensorValues;
  ACS::OutputValues outputValues;

  timeval now;
  Clock::getClock_timeval(&now);

  sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
  ReturnValue_t validMekf;
  navigation.useMekf(&sensorValues, &outputValues, &validMekf);
  double magMomMtq[3] = {0, 0, 0};
  detumble.bDotLaw(outputValues.magneticFieldVectorDerivative,
                   &outputValues.magneticFieldVectorDerivativeValid, outputValues.magFieldEst,
                   &outputValues.magFieldEstValid, magMomMtq);
  double dipolCmdUnits[3] = {0, 0, 0};
  actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits);

  if (outputValues.satRateMekfValid && VectorOperations<double>::norm(outputValues.satRateMekf, 3) <
                                           acsParameters.detumbleParameter.omegaDetumbleEnd) {
    detumbleCounter++;
  }

  else if (outputValues.satRateEstValid &&
           VectorOperations<double>::norm(outputValues.satRateEst, 3) <
               acsParameters.detumbleParameter.omegaDetumbleEnd) {
    detumbleCounter++;
  }

  if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) {
    submode = SUBMODE_SAFE;
    detumbleCounter = 0;
  }
}

void AcsController::performPointingCtrl() {
  ACS::SensorValues sensorValues;
  ACS::OutputValues outputValues;

  timeval now;  // Übergabe ?

  sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
  ReturnValue_t validMekf;
  navigation.useMekf(&sensorValues, &outputValues, &validMekf);
  double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
  guidance.targetQuatPtg(&sensorValues, &outputValues, now, targetQuat, refSatRate);
  double quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0};
  guidance.comparePtg(targetQuat, &outputValues, refSatRate, quatError, deltaRate);
  double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
  double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0;
  ptgCtrl.ptgLaw(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws);
  double rwTrqNs[4] = {0, 0, 0, 0};
  ptgCtrl.ptgNullspace(
      &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
      &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
  double cmdSpeedRws[4] = {0, 0, 0, 0};  // Should be given to the actuator reaction wheel as input
  actuatorCmd.cmdSpeedToRws(
      &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
      &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torquePtgRws,
      rwTrqNs, cmdSpeedRws);
  double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0};  // Desaturation Dipol
  ptgCtrl.ptgDesaturation(
      outputValues.magFieldEst, &outputValues.magFieldEstValid, outputValues.satRateMekf,
      &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
      &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
  actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits);
}

void AcsController::performPointingCtrlSun() {
  ACS::SensorValues sensorValues;
  ACS::OutputValues outputValues;

  timeval now;  // Übergabe ?

  sensorProcessing.process(now, &sensorValues, &outputValues, &acsParameters);
  ReturnValue_t validMekf;
  navigation.useMekf(&sensorValues, &outputValues, &validMekf);
  double targetQuat[4] = {0, 0, 0, 0}, refSatRate[3] = {0, 0, 0};
  guidance.sunQuatPtg(&sensorValues, &outputValues, targetQuat, refSatRate);
  double quatError[3] = {0, 0, 0}, deltaRate[3] = {0, 0, 0};
  guidance.comparePtg(targetQuat, &outputValues, refSatRate, quatError, deltaRate);
  double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv);
  double torquePtgRws[4] = {0, 0, 0, 0}, mode = 0;
  ptgCtrl.ptgLaw(mode, quatError, deltaRate, *rwPseudoInv, torquePtgRws);
  double rwTrqNs[4] = {0, 0, 0, 0};
  ptgCtrl.ptgNullspace(
      &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
      &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), rwTrqNs);
  double cmdSpeedRws[4] = {0, 0, 0, 0};  // Should be given to the actuator reaction wheel as input
  actuatorCmd.cmdSpeedToRws(
      &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
      &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), torquePtgRws,
      rwTrqNs, cmdSpeedRws);
  double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0};  // Desaturation Dipol
  ptgCtrl.ptgDesaturation(
      outputValues.magFieldEst, &outputValues.magFieldEstValid, outputValues.satRateMekf,
      &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value),
      &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes);
  actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits);
}

ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                     LocalDataPoolManager &poolManager) {
  // MGM
  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});
  // SUS
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_0_N_LOC_XFYFZM_PT_XF, &sus0PoolVec);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_1_N_LOC_XBYFZM_PT_XB, &sus1PoolVec);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_2_N_LOC_XFYBZB_PT_YB, &sus2PoolVec);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_3_N_LOC_XFYBZF_PT_YF, &sus3PoolVec);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_4_N_LOC_XMYFZF_PT_ZF, &sus4PoolVec);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_5_N_LOC_XFYMZB_PT_ZB, &sus5PoolVec);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_6_R_LOC_XFYBZM_PT_XF, &sus6PoolVec);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_7_R_LOC_XBYBZM_PT_XB, &sus7PoolVec);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_8_R_LOC_XBYBZB_PT_YB, &sus8PoolVec);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_9_R_LOC_XBYBZB_PT_YF, &sus9PoolVec);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_10_N_LOC_XMYBZF_PT_ZF, &sus10PoolVec);
  localDataPoolMap.emplace(acsctrl::PoolIds::SUS_11_R_LOC_XBYMZB_PT_ZB, &sus11PoolVec);
  poolManager.subscribeForRegularPeriodicPacket({susData.getSid(), false, 5.0});
  return returnvalue::OK;
}

LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) {
  if (sid == mgmData.getSid()) {
    return &mgmData;
  } else if (sid == susData.getSid()) {
    return &susData;
  }
  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 ((mode == MODE_ON) || (mode == MODE_NORMAL)) {
    if ((submode > 5) || (submode < 2)) {
      return INVALID_SUBMODE;
    } else {
      return returnvalue::OK;
    }
  }
  return INVALID_MODE;
}

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

void AcsController::announceMode(bool recursive) {}

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;
    }
  }
}

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