#include "MultiplicativeKalmanFilter.h"

MultiplicativeKalmanFilter::MultiplicativeKalmanFilter(AcsParameters *acsParameters) {
  updateStandardDeviations(acsParameters);
}

MultiplicativeKalmanFilter::~MultiplicativeKalmanFilter() {}

void MultiplicativeKalmanFilter::updateStandardDeviations(const AcsParameters *acsParameters) {
  sigmaSus = acsParameters->kalmanFilterParameters.sensorNoiseSus;
  sigmaMgm = acsParameters->kalmanFilterParameters.sensorNoiseMgm;
  sigmaStr = acsParameters->kalmanFilterParameters.sensorNoiseStr;
  sigmaGyr = acsParameters->kalmanFilterParameters.sensorNoiseGyr;
  sigmaGyrArw = acsParameters->kalmanFilterParameters.sensorNoiseGyrArw;
  sigmaGyrBs = acsParameters->kalmanFilterParameters.sensorNoiseGyrBs;

  MatrixOperations<double>::multiplyScalar(*EYE3, sigmaSus * sigmaSus, *covSus, 3, 3);
  MatrixOperations<double>::multiplyScalar(*EYE3, sigmaMgm * sigmaMgm, *covMgm, 3, 3);
  MatrixOperations<double>::multiplyScalar(*EYE3, sigmaStr * sigmaStr, *covStr, 3, 3);
}

ReturnValue_t MultiplicativeKalmanFilter::reset(
    acsctrl::AttitudeEstimationData *attitudeEstimationData) {
  std::memcpy(estimatedQuaternionBI, UNIT_QUAT, sizeof(estimatedQuaternionBI));
  std::memcpy(estimatedBiasGyr, ZERO_VEC3, sizeof(estimatedBiasGyr));
  std::memcpy(estimatedCovarianceMatrix, ZERO_MAT66, sizeof(estimatedCovarianceMatrix));
  mekfStatus = MekfStatus::UNINITIALIZED;
  updateDataSetWithoutData(attitudeEstimationData);
  return MEKF_UNINITIALIZED;
}

ReturnValue_t MultiplicativeKalmanFilter::init(
    const acsctrl::SusDataProcessed *susData, const acsctrl::MgmDataProcessed *mgmData,
    const acsctrl::GyrDataProcessed *gyrData,
    acsctrl::AttitudeEstimationData *attitudeEstimationData) {
  // check for valid QUEST quaternion
  if (attitudeEstimationData->quatQuest.isValid()) {
    // Initial Quaternion
    std::memcpy(estimatedQuaternionBI, attitudeEstimationData->quatQuest.value,
                sizeof(estimatedQuaternionBI));

    // Initial Covariance
    if (susData->susVecTot.isValid() and mgmData->mgmVecTot.isValid() and
        gyrData->gyrVecTot.isValid()) {
      // QUEST Covariance
      double normSunI[3] = {0, 0, 0}, normMagI[3] = {0, 0, 0}, normSunEstB[3] = {0, 0, 0},
             normMagEstB[3] = {0, 0, 0};
      VectorOperations<double>::normalize(susData->sunIjkModel.value, normSunI, 3);
      VectorOperations<double>::normalize(mgmData->magIgrfModel.value, normMagI, 3);
      QuaternionOperations::multiplyVector(estimatedQuaternionBI, normSunI, normSunEstB);
      QuaternionOperations::multiplyVector(estimatedQuaternionBI, normMagI, normMagEstB);

      double matrixSus[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
             matrixMgm[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
             matrixSusMgm[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
             matrixMgmSus[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
      MatrixOperations<double>::multiply(normSunEstB, normSunEstB, *matrixSus, 3, 1, 3);
      MatrixOperations<double>::multiply(normMagEstB, normMagEstB, *matrixMgm, 3, 1, 3);
      MatrixOperations<double>::multiply(normSunEstB, normMagEstB, *matrixSusMgm, 3, 1, 3);
      MatrixOperations<double>::multiply(normMagEstB, normSunEstB, *matrixMgmSus, 3, 1, 3);

      double factorSus = 1. / (sigmaSus * sigmaSus);
      double factorMgm = 1. / (sigmaMgm * sigmaMgm);

      double covQuest[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
      MatrixOperations<double>::multiplyScalar(*matrixSus, factorSus * factorSus, *matrixSus, 3, 3);
      MatrixOperations<double>::multiplyScalar(*matrixMgm, factorMgm * factorMgm, *matrixMgm, 3, 3);
      MatrixOperations<double>::add(*matrixSusMgm, *matrixMgmSus, *covQuest, 3, 3);
      MatrixOperations<double>::multiplyScalar(
          *covQuest,
          factorSus * factorMgm * VectorOperations<double>::dot(normSunEstB, normMagEstB),
          *covQuest, 3, 3);
      MatrixOperations<double>::add(*covQuest, *matrixSus, *covQuest, 3, 3);
      MatrixOperations<double>::add(*covQuest, *matrixMgm, *covQuest, 3, 3);

      double crossSunMag[3] = {0, 0, 0};
      VectorOperations<double>::cross(normSunEstB, normMagEstB, crossSunMag);
      double normCrossSunMag = VectorOperations<double>::norm(crossSunMag, 3);
      double factor = factorSus * factorMgm * normCrossSunMag * normCrossSunMag;

      MatrixOperations<double>::multiplyScalar(*covQuest, 1. / factor, *covQuest, 3, 3);
      MatrixOperations<double>::add(*EYE3, *covQuest, *covQuest, 3, 3);
      MatrixOperations<double>::multiplyScalar(*covQuest, 1. / (factorSus + factorMgm), *covQuest,
                                               3, 3);

      // GYR Covariance
      double covGyr[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
      MatrixOperations<double>::multiplyScalar(*EYE3, sigmaGyr, *covGyr, 3, 3);

      // Combine Covariances
      MatrixOperations<double>::writeSubmatrix(*estimatedCovarianceMatrix, *covQuest, 3, 3, 6, 6, 0,
                                               0);
      MatrixOperations<double>::writeSubmatrix(*estimatedCovarianceMatrix, *covGyr, 3, 3, 6, 6, 3,
                                               3);

      mekfStatus = MekfStatus::INITIALIZED;
      updateDataSetWithoutData(attitudeEstimationData);
      return MEKF_INITIALIZED;
    }
  }
  // no initialisation possible, no valid measurements
  mekfStatus = MekfStatus::UNINITIALIZED;
  updateDataSetWithoutData(attitudeEstimationData);
  return MEKF_UNINITIALIZED;
}

ReturnValue_t MultiplicativeKalmanFilter::mekfEst(
    const acsctrl::SusDataProcessed *susData, const acsctrl::MgmDataProcessed *mgmData,
    const acsctrl::GyrDataProcessed *gyrData, const double timeDelta,
    acsctrl::AttitudeEstimationData *attitudeEstimationData) {
  ReturnValue_t result = checkAvailableSensors(susData, mgmData, gyrData, attitudeEstimationData);
  if (result != returnvalue::OK) {
    reset(attitudeEstimationData);
    return result;
  }

  double measSensMatrix[matrixDimensionFactor][6] = {{0}},
         measCovMatrix[matrixDimensionFactor][matrixDimensionFactor] = {{0}},
         measVec[matrixDimensionFactor] = {0}, estVec[matrixDimensionFactor] = {0};
  kfUpdate(susData, mgmData, *measSensMatrix, *measCovMatrix, measVec, estVec);

  double kalmanGain[6][matrixDimensionFactor] = {{0}};
  result = kfGain(*measSensMatrix, *measCovMatrix, *kalmanGain, attitudeEstimationData);
  if (result != returnvalue::OK) {
    reset(attitudeEstimationData);
    return result;
  }

  kfCovAposteriori(*kalmanGain, *measSensMatrix);
  kfStateAposteriori(*kalmanGain, measVec, estVec);
  kfPropagate(gyrData, timeDelta);

  result = kfFiniteCheck(attitudeEstimationData);
  if (result != returnvalue::OK) {
    return result;
  }
  mekfStatus = MekfStatus::RUNNING;
  updateDataSet(attitudeEstimationData);
  return MEKF_RUNNING;
}

ReturnValue_t MultiplicativeKalmanFilter::checkAvailableSensors(
    const acsctrl::SusDataProcessed *susData, const acsctrl::MgmDataProcessed *mgmData,
    const acsctrl::GyrDataProcessed *gyrData,
    acsctrl::AttitudeEstimationData *attitudeEstimationData) {
  // Check for GYR Measurements
  if (not gyrData->gyrVecTot.isValid()) {
    mekfStatus = MekfStatus::NO_GYR_DATA;
    updateDataSetWithoutData(attitudeEstimationData);
    return MEKF_NO_GYR_DATA;
  }
  // Check for Model Calculations
  if (not susData->sunIjkModel.isValid() or not mgmData->magIgrfModel.isValid()) {
    mekfStatus = MekfStatus::NO_MODEL_VECTORS;
    updateDataSetWithoutData(attitudeEstimationData);
    return MEKF_NO_MODEL_VECTORS;
  }
  // Check Measurements available from SUS, MGM, STR
  if (not susData->susVecTot.isValid() and not mgmData->mgmVecTot.isValid() and
      not strData.strQuat.valid) {
    sensorsAvailable = SensorAvailability::NONE;
    mekfStatus = MekfStatus::NO_SUS_MGM_STR_DATA;
    updateDataSetWithoutData(attitudeEstimationData);
    return MEKF_NO_SUS_MGM_STR_DATA;
  }
  if (susData->susVecTot.isValid() and mgmData->mgmVecTot.isValid() and strData.strQuat.valid) {
    sensorsAvailable = SensorAvailability::SUS_MGM_STR;
    matrixDimensionFactor = 9;
  } else if (susData->susVecTot.isValid() and mgmData->mgmVecTot.isValid() and
             not strData.strQuat.valid) {
    sensorsAvailable = SensorAvailability::SUS_MGM;
    matrixDimensionFactor = 6;
  } else if (susData->susVecTot.isValid() and not mgmData->mgmVecTot.isValid() and
             strData.strQuat.valid) {
    sensorsAvailable = SensorAvailability::SUS_STR;
    matrixDimensionFactor = 6;
  } else if (not susData->susVecTot.isValid() and mgmData->mgmVecTot.isValid() and
             strData.strQuat.valid) {
    sensorsAvailable = SensorAvailability::MGM_STR;
    matrixDimensionFactor = 6;
  } else if (susData->susVecTot.isValid() and not mgmData->mgmVecTot.isValid() and
             not strData.strQuat.valid) {
    sensorsAvailable = SensorAvailability::SUS;
    matrixDimensionFactor = 3;
  } else if (not susData->susVecTot.isValid() and mgmData->mgmVecTot.isValid() and
             not strData.strQuat.valid) {
    sensorsAvailable = SensorAvailability::MGM;
    matrixDimensionFactor = 3;
  } else if (not susData->susVecTot.isValid() and not mgmData->mgmVecTot.isValid() and
             strData.strQuat.valid) {
    sensorsAvailable = SensorAvailability::STR;
    matrixDimensionFactor = 3;
  }
  return returnvalue::OK;
}

void MultiplicativeKalmanFilter::kfUpdate(const acsctrl::SusDataProcessed *susData,
                                          const acsctrl::MgmDataProcessed *mgmData,
                                          double *measSensMatrix, double *measCovMatrix,
                                          double *measVec, double *estVec) {
  double normMagB[3] = {0, 0, 0}, normSunB[3] = {0, 0, 0}, normMagI[3] = {0, 0, 0},
         normSunI[3] = {0, 0, 0}, normSunEstB[3] = {0, 0, 0}, normMagEstB[3] = {0, 0, 0};
  VectorOperations<double>::normalize(mgmData->mgmVecTot.value, normMagB, 3);     // b2
  VectorOperations<double>::normalize(susData->susVecTot.value, normSunB, 3);     // b1
  VectorOperations<double>::normalize(mgmData->magIgrfModel.value, normMagI, 3);  // r2
  VectorOperations<double>::normalize(susData->sunIjkModel.value, normSunI, 3);   // r1
  QuaternionOperations::multiplyVector(estimatedQuaternionBI, normSunI, normSunEstB);
  QuaternionOperations::multiplyVector(estimatedQuaternionBI, normMagI, normMagEstB);

  // Measurement Sensitivity Matrix
  double measSensMatrixSun[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};  // H11
  double measSensMatrixMag[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};  // H22
  double measSensMatrixStr[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};  // H33
  MatrixOperations<double>::skewMatrix(normSunEstB, *measSensMatrixSun);
  MatrixOperations<double>::skewMatrix(normMagEstB, *measSensMatrixMag);

  double measVecQuat[3] = {0, 0, 0}, quatEstErr[3] = {0, 0, 0};
  if (strData.strQuat.valid) {
    double quatError[4] = {0, 0, 0, 0};
    double invPropagatedQuat[4] = {0, 0, 0, 0};
    QuaternionOperations::inverse(estimatedQuaternionBI, invPropagatedQuat);
    QuaternionOperations::multiply(strData.strQuat.value, invPropagatedQuat, quatError);
    for (uint8_t i = 0; i < 3; i++) {
      measVecQuat[i] = 2. * quatError[i] / quatError[3];
    }
  }

  switch (sensorsAvailable) {
    case SensorAvailability::SUS_MGM_STR:
      MatrixOperations<double>::writeSubmatrix(measSensMatrix, *measSensMatrixSun, 3, 3,
                                               matrixDimensionFactor, 6, 0, 0);
      MatrixOperations<double>::writeSubmatrix(measSensMatrix, *measSensMatrixMag, 3, 3,
                                               matrixDimensionFactor, 6, 3, 0);
      MatrixOperations<double>::writeSubmatrix(measSensMatrix, *measSensMatrixStr, 3, 3,
                                               matrixDimensionFactor, 6, 6, 0);

      MatrixOperations<double>::writeSubmatrix(measCovMatrix, *covSus, 3, 3, matrixDimensionFactor,
                                               matrixDimensionFactor, 0, 0);
      MatrixOperations<double>::writeSubmatrix(measCovMatrix, *covMgm, 3, 3, matrixDimensionFactor,
                                               matrixDimensionFactor, 3, 3);
      MatrixOperations<double>::writeSubmatrix(measCovMatrix, *covStr, 3, 3, matrixDimensionFactor,
                                               matrixDimensionFactor, 6, 6);

      std::memcpy(measVec, normSunB, sizeof(normSunB));
      std::memcpy(measVec + 3, normMagB, sizeof(normMagB));
      std::memcpy(measVec + 6, measVecQuat, sizeof(measVecQuat));

      std::memcpy(estVec, normSunEstB, sizeof(normSunEstB));
      std::memcpy(estVec + 3, normMagEstB, sizeof(normMagEstB));
      std::memcpy(estVec + 6, quatEstErr, sizeof(quatEstErr));
      return;
    case SensorAvailability::SUS_MGM:
      MatrixOperations<double>::writeSubmatrix(measSensMatrix, *measSensMatrixSun, 3, 3,
                                               matrixDimensionFactor, 6, 0, 0);
      MatrixOperations<double>::writeSubmatrix(measSensMatrix, *measSensMatrixMag, 3, 3,
                                               matrixDimensionFactor, 6, 3, 0);

      MatrixOperations<double>::writeSubmatrix(measCovMatrix, *covSus, 3, 3, matrixDimensionFactor,
                                               matrixDimensionFactor, 0, 0);
      MatrixOperations<double>::writeSubmatrix(measCovMatrix, *covMgm, 3, 3, matrixDimensionFactor,
                                               matrixDimensionFactor, 3, 3);

      std::memcpy(measVec, normSunB, sizeof(normSunB));
      std::memcpy(measVec + 3, normMagB, sizeof(normMagB));

      std::memcpy(estVec, normSunEstB, sizeof(normSunEstB));
      std::memcpy(estVec + 3, normMagEstB, sizeof(normMagEstB));
      return;
    case SensorAvailability::SUS_STR:
      MatrixOperations<double>::writeSubmatrix(measSensMatrix, *measSensMatrixSun, 3, 3,
                                               matrixDimensionFactor, 6, 0, 0);
      MatrixOperations<double>::writeSubmatrix(measSensMatrix, *measSensMatrixStr, 3, 3,
                                               matrixDimensionFactor, 6, 3, 0);

      MatrixOperations<double>::writeSubmatrix(measCovMatrix, *covSus, 3, 3, matrixDimensionFactor,
                                               matrixDimensionFactor, 0, 0);
      MatrixOperations<double>::writeSubmatrix(measCovMatrix, *covStr, 3, 3, matrixDimensionFactor,
                                               matrixDimensionFactor, 3, 3);

      std::memcpy(measVec, normSunB, sizeof(normSunB));
      std::memcpy(measVec + 3, measVecQuat, sizeof(measVecQuat));

      std::memcpy(estVec, normSunEstB, sizeof(normSunEstB));
      std::memcpy(estVec + 3, quatEstErr, sizeof(quatEstErr));
      return;
    case SensorAvailability::MGM_STR:
      MatrixOperations<double>::writeSubmatrix(measSensMatrix, *measSensMatrixMag, 3, 3,
                                               matrixDimensionFactor, 6, 0, 0);
      MatrixOperations<double>::writeSubmatrix(measSensMatrix, *measSensMatrixStr, 3, 3,
                                               matrixDimensionFactor, 6, 3, 0);

      MatrixOperations<double>::writeSubmatrix(measCovMatrix, *covMgm, 3, 3, matrixDimensionFactor,
                                               matrixDimensionFactor, 0, 0);
      MatrixOperations<double>::writeSubmatrix(measCovMatrix, *covStr, 3, 3, matrixDimensionFactor,
                                               matrixDimensionFactor, 3, 3);

      std::memcpy(measVec, normMagB, sizeof(normMagB));
      std::memcpy(measVec + 3, measVecQuat, sizeof(measVecQuat));

      std::memcpy(estVec, normMagEstB, sizeof(normMagEstB));
      std::memcpy(estVec + 3, quatEstErr, sizeof(quatEstErr));
      return;
    case SensorAvailability::SUS:
      MatrixOperations<double>::writeSubmatrix(measSensMatrix, *measSensMatrixSun, 3, 3,
                                               matrixDimensionFactor, 6, 0, 0);

      MatrixOperations<double>::writeSubmatrix(measCovMatrix, *covSus, 3, 3, matrixDimensionFactor,
                                               matrixDimensionFactor, 0, 0);

      std::memcpy(measVec, normSunB, sizeof(normSunB));

      std::memcpy(estVec, normSunEstB, sizeof(normSunEstB));
      return;
    case SensorAvailability::MGM:
      MatrixOperations<double>::writeSubmatrix(measSensMatrix, *measSensMatrixMag, 3, 3,
                                               matrixDimensionFactor, 6, 0, 0);

      MatrixOperations<double>::writeSubmatrix(measCovMatrix, *covMgm, 3, 3, matrixDimensionFactor,
                                               matrixDimensionFactor, 0, 0);

      std::memcpy(measVec, normMagB, sizeof(normMagB));

      std::memcpy(estVec, normMagEstB, sizeof(normMagEstB));
      return;
    case SensorAvailability::STR:
      MatrixOperations<double>::writeSubmatrix(measSensMatrix, *measSensMatrixStr, 3, 3,
                                               matrixDimensionFactor, 6, 0, 0);

      MatrixOperations<double>::writeSubmatrix(measCovMatrix, *covStr, 3, 3, matrixDimensionFactor,
                                               matrixDimensionFactor, 0, 0);

      std::memcpy(measVec, measVecQuat, sizeof(measVecQuat));

      std::memcpy(estVec, quatEstErr, sizeof(quatEstErr));
      return;
    default:
      sif::error << "MEKF::Invalid SensorAvailability Flag" << std::endl;
  }
}

ReturnValue_t MultiplicativeKalmanFilter::kfGain(
    double *measSensMatrix, double *measCovMatrix, double *kalmanGain,
    acsctrl::AttitudeEstimationData *attitudeEstimationData) {
  // Kalman Gain: K = P * H' / (H * P * H' + R)
  double kalmanGainDen[matrixDimensionFactor][matrixDimensionFactor] = {{0}},
         invKalmanGainDen[matrixDimensionFactor][matrixDimensionFactor] = {{0}},
         residualCov[6][matrixDimensionFactor] = {{0}},
         measSensMatrixTransposed[6][matrixDimensionFactor] = {{0}};

  MatrixOperations<double>::transpose(measSensMatrix, *measSensMatrixTransposed,
                                      matrixDimensionFactor, 6);
  MatrixOperations<double>::multiply(*estimatedCovarianceMatrix, *measSensMatrixTransposed,
                                     *residualCov, 6, 6, matrixDimensionFactor);

  MatrixOperations<double>::multiply(measSensMatrix, *residualCov, *kalmanGainDen,
                                     matrixDimensionFactor, 6, matrixDimensionFactor);
  MatrixOperations<double>::add(*kalmanGainDen, measCovMatrix, *kalmanGainDen,
                                matrixDimensionFactor, matrixDimensionFactor);
  ReturnValue_t result = MatrixOperations<double>::inverseMatrix(*kalmanGainDen, *invKalmanGainDen,
                                                                 matrixDimensionFactor);
  if (result != returnvalue::OK) {
    mekfStatus = MekfStatus::COVARIANCE_INVERSION_FAILED;
    updateDataSetWithoutData(attitudeEstimationData);
    return MEKF_COVARIANCE_INVERSION_FAILED;
  }

  MatrixOperations<double>::multiply(*residualCov, *invKalmanGainDen, kalmanGain, 6,
                                     matrixDimensionFactor, matrixDimensionFactor);
  return returnvalue::OK;
}

void MultiplicativeKalmanFilter::kfCovAposteriori(double *kalmanGain, double *measSensMatrix) {
  // Covariance Matrix: P_plus [k] = (I-K*H)*P_min
  double helperMat[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
                            {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
  MatrixOperations<double>::multiply(kalmanGain, measSensMatrix, *helperMat, 6,
                                     matrixDimensionFactor, 6);
  MatrixOperations<double>::subtract(*EYE6, *helperMat, *helperMat, 6, 6);
  MatrixOperations<double>::multiply(*helperMat, *estimatedCovarianceMatrix, *covAposteriori, 6, 6,
                                     6);
}

void MultiplicativeKalmanFilter::kfStateAposteriori(double *kalmanGain, double *measVec,
                                                    double *estVec) {
  double stateVecErr[6] = {0, 0, 0, 0, 0, 0};
  double plantOutputDiff[matrixDimensionFactor] = {0};
  VectorOperations<double>::subtract(measVec, estVec, plantOutputDiff, matrixDimensionFactor);
  MatrixOperations<double>::multiply(kalmanGain, plantOutputDiff, stateVecErr, 6,
                                     matrixDimensionFactor, 1);

  double quatAposteriori[3] = {stateVecErr[0], stateVecErr[1], stateVecErr[2]};
  double biasAposteriori[3] = {stateVecErr[3], stateVecErr[4], stateVecErr[5]};

  double quaternionIdentity[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  double quaternionIdentityUpperMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  double quaternionIdentityUpperMatrixHelper[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  double negQuatVec[3] = {0, 0, 0};
  MatrixOperations<double>::multiplyScalar(*EYE3, estimatedQuaternionBI[3],
                                           *quaternionIdentityUpperMatrix, 3, 3);
  MatrixOperations<double>::skewMatrix(estimatedQuaternionBI, *quaternionIdentityUpperMatrixHelper);
  MatrixOperations<double>::add(*quaternionIdentityUpperMatrix,
                                *quaternionIdentityUpperMatrixHelper,
                                *quaternionIdentityUpperMatrix, 3, 3);

  VectorOperations<double>::mulScalar(estimatedQuaternionBI, -1, negQuatVec, 3);

  MatrixOperations<double>::writeSubmatrix(*quaternionIdentity, *quaternionIdentityUpperMatrix, 3,
                                           3, 4, 3, 0, 0);
  MatrixOperations<double>::writeSubmatrix(*quaternionIdentity, negQuatVec, 1, 3, 4, 3, 3, 0);

  double quatCorrection[4] = {0, 0, 0, 0};
  MatrixOperations<double>::multiply(*quaternionIdentity, quatAposteriori, quatCorrection, 4, 3, 1);
  VectorOperations<double>::mulScalar(quatCorrection, 0.5, quatCorrection, 4);
  VectorOperations<double>::add(estimatedQuaternionBI, quatCorrection, estimatedQuaternionBI, 4);
  QuaternionOperations::normalize(estimatedQuaternionBI);

  VectorOperations<double>::add(estimatedBiasGyr, biasAposteriori, estimatedBiasGyr, 3);
}

void MultiplicativeKalmanFilter::kfPropagate(const acsctrl::GyrDataProcessed *gyrData,
                                             const double timeDelta) {
  // Estimated Rotation Rate
  VectorOperations<double>::subtract(gyrData->gyrVecTot.value, estimatedBiasGyr, estimatedRotRate);
  double estimatedRotRateNorm = VectorOperations<double>::norm(estimatedRotRate, 3);
  double estimatedRotRateSkewed[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  MatrixOperations<double>::skewMatrix(estimatedRotRate, *estimatedRotRateSkewed);

  // Estimated Quaternion
  double diffQuatMatrix[4][4] = {{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}},
         diffQuatMatrixHelper1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
         diffQuatMatrixHelper2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}, diffVector[3] = {0, 0, 0},
         helperQuat[4] = {0, 0, 0, 0};
  double diffScalar = std::cos(.5 * estimatedRotRateNorm * timeDelta);
  VectorOperations<double>::mulScalar(
      estimatedRotRate, std::sin(.5 * estimatedRotRateNorm * timeDelta) / estimatedRotRateNorm,
      diffVector, 3);
  MatrixOperations<double>::multiplyScalar(*EYE3, diffScalar, *diffQuatMatrixHelper1, 3, 3);
  MatrixOperations<double>::skewMatrix(diffVector, *diffQuatMatrixHelper2);
  MatrixOperations<double>::subtract(*diffQuatMatrixHelper1, *diffQuatMatrixHelper2,
                                     *diffQuatMatrixHelper1, 3, 3);

  MatrixOperations<double>::writeSubmatrix(*diffQuatMatrix, *diffQuatMatrixHelper1, 3, 3, 4, 4, 0,
                                           0);
  MatrixOperations<double>::writeSubmatrix(*diffQuatMatrix, diffVector, 3, 1, 4, 4, 0, 3);
  VectorOperations<double>::mulScalar(diffVector, -1, diffVector, 3);
  MatrixOperations<double>::writeSubmatrix(*diffQuatMatrix, diffVector, 1, 3, 4, 4, 3, 0);
  diffQuatMatrix[3][3] = diffScalar;

  std::memcpy(helperQuat, estimatedQuaternionBI, sizeof(helperQuat));
  MatrixOperations<double>::multiply(*diffQuatMatrix, helperQuat, estimatedQuaternionBI, 4, 4, 1);
  QuaternionOperations::normalize(estimatedQuaternionBI);

  // Covariance Matrix: P_minus [k+1] = Phi*P_plus [k]*Phi'+G*Q*G'
  // Phi [Discrete Error State Transition Matrix]
  double errorStateTransition[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
                                       {0, 0, 0, 1, 0, 0}, {0, 0, 0, 0, 1, 0}, {0, 0, 0, 0, 0, 1}};
  double phi11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
         phi12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
         phi11L[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
         phi11R[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
         phi12L[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
         phi12R[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};

  MatrixOperations<double>::multiplyScalar(
      *estimatedRotRateSkewed, std::sin(estimatedRotRateNorm * timeDelta) / estimatedRotRateNorm,
      *phi11L, 3, 3);
  MatrixOperations<double>::multiply(*estimatedRotRateSkewed, *estimatedRotRateSkewed, *phi11R, 3,
                                     3, 3);
  MatrixOperations<double>::multiplyScalar(*phi11R,
                                           (1 - std::cos(estimatedRotRateNorm * timeDelta)) /
                                               (estimatedRotRateNorm * estimatedRotRateNorm),
                                           *phi11R, 3, 3);
  MatrixOperations<double>::subtract(*EYE3, *phi11L, *phi11, 3, 3);
  MatrixOperations<double>::add(*phi11, *phi11R, *phi11, 3, 3);

  MatrixOperations<double>::multiplyScalar(*EYE3, -timeDelta, *phi12, 3, 3);
  MatrixOperations<double>::multiplyScalar(*estimatedRotRateSkewed,
                                           (1 - std::cos(estimatedRotRateNorm * timeDelta)) /
                                               (estimatedRotRateNorm * estimatedRotRateNorm),
                                           *phi12L, 3, 3);
  MatrixOperations<double>::multiply(*estimatedRotRateSkewed, *estimatedRotRateSkewed, *phi12R, 3,
                                     3, 3);
  MatrixOperations<double>::multiplyScalar(
      *phi12R,
      (estimatedRotRateNorm * timeDelta - std::sin(estimatedRotRateNorm * timeDelta)) /
          (estimatedRotRateNorm * estimatedRotRateNorm * estimatedRotRateNorm),
      *phi12R, 3, 3);
  MatrixOperations<double>::add(*phi12, *phi12L, *phi12, 3, 3);
  MatrixOperations<double>::subtract(*phi12, *phi12R, *phi12, 3, 3);

  MatrixOperations<double>::writeSubmatrix(*errorStateTransition, *phi11, 3, 3, 6, 6, 0, 0);
  MatrixOperations<double>::writeSubmatrix(*errorStateTransition, *phi12, 3, 3, 6, 6, 0, 3);

  // G [Discrete Transfer Matrix]
  double transferMatrix[6][6] = {{-1, 0, 0, 0, 0, 0}, {0, -1, 0, 0, 0, 0}, {0, 0, -1, 0, 0, 0},
                                 {0, 0, 0, 1, 0, 0},  {0, 0, 0, 0, 1, 0},  {0, 0, 0, 0, 0, 1}};

  // Q [Discrete Time Covariance]
  double timeCovariance[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
                                 {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};
  double q11[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
         q12[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
         q21[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
         q22[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};

  if (estimatedRotRateNorm * timeDelta < M_PI / 10.) {
    MatrixOperations<double>::multiplyScalar(
        *EYE3,
        (sigmaGyrArw * sigmaGyrArw) * timeDelta +
            ((sigmaGyrBs * sigmaGyrBs) * (timeDelta * timeDelta * timeDelta)) / 3.,
        *q11, 3, 3);
    MatrixOperations<double>::multiplyScalar(
        *EYE3, -(sigmaGyrBs * sigmaGyrBs * timeDelta * timeDelta) / 2., *q12, 3, 3);
  } else {
    double q11h1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
           q11h2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
           q12h1[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}},
           q12h2[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};

    MatrixOperations<double>::multiplyScalar(*EYE3, (sigmaGyrArw * sigmaGyrArw) * timeDelta, *q11,
                                             3, 3);
    MatrixOperations<double>::multiplyScalar(*EYE3, (timeDelta * timeDelta * timeDelta) / 3.,
                                             *q11h1, 3, 3);
    MatrixOperations<double>::multiply(*estimatedRotRateSkewed, *estimatedRotRateSkewed, *q11h2, 3,
                                       3, 3);
    MatrixOperations<double>::multiplyScalar(
        *q11h2,
        (2 * estimatedRotRateNorm * timeDelta - 2 * std::sin(estimatedRotRateNorm * timeDelta) -
         ((estimatedRotRateNorm * estimatedRotRateNorm * estimatedRotRateNorm) *
          (timeDelta * timeDelta * timeDelta) / .3)) /
            (estimatedRotRateNorm * estimatedRotRateNorm * estimatedRotRateNorm *
             estimatedRotRateNorm * estimatedRotRateNorm),
        *q11h2, 3, 3);
    MatrixOperations<double>::add(*q11h1, *q11h2, *q11h1, 3, 3);
    MatrixOperations<double>::multiplyScalar(*q11h1, sigmaGyrBs * sigmaGyrBs, *q11h1, 3, 3);
    MatrixOperations<double>::add(*q11, *q11h1, *q11, 3, 3);

    MatrixOperations<double>::multiplyScalar(*EYE3, -(timeDelta * timeDelta) / 2., *q12, 3, 3);
    MatrixOperations<double>::multiplyScalar(
        *estimatedRotRateSkewed,
        (estimatedRotRateNorm * timeDelta - std::sin(estimatedRotRateNorm * timeDelta)) /
            (estimatedRotRateNorm * estimatedRotRateNorm * estimatedRotRateNorm),
        *q12h1, 3, 3);
    MatrixOperations<double>::multiply(*estimatedRotRateSkewed, *estimatedRotRateSkewed, *q12h2, 3,
                                       3, 3);
    MatrixOperations<double>::multiplyScalar(
        *q12h2,
        (((estimatedRotRateNorm * estimatedRotRateNorm) * (timeDelta * timeDelta)) / 2. +
         std::cos(estimatedRotRateNorm * timeDelta) - 1) /
            (estimatedRotRateNorm * estimatedRotRateNorm * estimatedRotRateNorm *
             estimatedRotRateNorm),
        *q12h2, 3, 3);
    MatrixOperations<double>::add(*q12, *q12h1, *q12, 3, 3);
    MatrixOperations<double>::subtract(*q12, *q12h2, *q12, 3, 3);
    MatrixOperations<double>::multiplyScalar(*q12, sigmaGyrBs * sigmaGyrBs, *q12, 3, 3);
  }

  MatrixOperations<double>::transpose(*q12, *q21, 3);
  MatrixOperations<double>::multiplyScalar(*EYE3, sigmaGyrBs * sigmaGyrBs * timeDelta, *q22, 3, 3);

  MatrixOperations<double>::writeSubmatrix(*timeCovariance, *q11, 3, 3, 6, 6, 0, 0);
  MatrixOperations<double>::writeSubmatrix(*timeCovariance, *q12, 3, 3, 6, 6, 0, 3);
  MatrixOperations<double>::writeSubmatrix(*timeCovariance, *q21, 3, 3, 6, 6, 3, 0);
  MatrixOperations<double>::writeSubmatrix(*timeCovariance, *q22, 3, 3, 6, 6, 3, 3);

  // Estimated Covariance
  double errorStateTransitionTransposed[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
                                                 {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
                                                 {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}},
         estCovarianceMatrixL[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
                                       {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}},
         estCovarianceMatrixR[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
                                       {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}},
         estCovarianceMatrixHelp[6][6] = {{0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
                                          {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0},
                                          {0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0}};

  MatrixOperations<double>::transpose(*errorStateTransition, *errorStateTransitionTransposed, 6);
  MatrixOperations<double>::multiply(*errorStateTransition, *covAposteriori,
                                     *estCovarianceMatrixHelp, 6, 6, 6);
  MatrixOperations<double>::multiply(*estCovarianceMatrixHelp, *errorStateTransitionTransposed,
                                     *estCovarianceMatrixL, 6, 6, 6);
  MatrixOperations<double>::multiply(*transferMatrix, *timeCovariance, *estCovarianceMatrixHelp, 6,
                                     6, 6);
  MatrixOperations<double>::multiply(*estCovarianceMatrixHelp, *transferMatrix,
                                     *estCovarianceMatrixR, 6, 6, 6);
  MatrixOperations<double>::add(*estCovarianceMatrixL, *estCovarianceMatrixR,
                                *estimatedCovarianceMatrix, 6, 6);
}

ReturnValue_t MultiplicativeKalmanFilter::kfFiniteCheck(
    acsctrl::AttitudeEstimationData *attitudeEstimationData) {
  if (not(VectorOperations<double>::isFinite(estimatedQuaternionBI, 4)) or
      not(VectorOperations<double>::isFinite(estimatedRotRate, 3)) or
      not(MatrixOperations<double>::isFinite(*estimatedCovarianceMatrix, 6, 6))) {
    mekfStatus = MekfStatus::NOT_FINITE;
    updateDataSetWithoutData(attitudeEstimationData);
    return MEKF_NOT_FINITE;
  }
  return returnvalue::OK;
}

void MultiplicativeKalmanFilter::updateDataSetWithoutData(
    acsctrl::AttitudeEstimationData *attitudeEstimationData) {
  PoolReadGuard pg(attitudeEstimationData);
  if (pg.getReadResult() == returnvalue::OK) {
    std::memcpy(attitudeEstimationData->quatMekf.value, ZERO_VEC4, 4 * sizeof(double));
    attitudeEstimationData->quatMekf.setValid(false);
    std::memcpy(attitudeEstimationData->satRotRateMekf.value, ZERO_VEC3, 3 * sizeof(double));
    attitudeEstimationData->satRotRateMekf.setValid(false);
    attitudeEstimationData->mekfStatus.value = mekfStatus;
    attitudeEstimationData->mekfStatus.setValid(true);
  }
}

void MultiplicativeKalmanFilter::updateDataSet(
    acsctrl::AttitudeEstimationData *attitudeEstimationData) {
  PoolReadGuard pg(attitudeEstimationData);
  if (pg.getReadResult() == returnvalue::OK) {
    std::memcpy(attitudeEstimationData->quatMekf.value, estimatedQuaternionBI, 4 * sizeof(double));
    attitudeEstimationData->quatMekf.setValid(true);
    std::memcpy(attitudeEstimationData->satRotRateMekf.value, estimatedRotRate, 3 * sizeof(double));
    attitudeEstimationData->satRotRateMekf.setValid(true);
    attitudeEstimationData->mekfStatus.value = mekfStatus;
    attitudeEstimationData->mekfStatus.setValid(true);
  }
}

void MultiplicativeKalmanFilter::setStrData(double qX, double qY, double qZ, double qW, bool valid,
                                            bool allowStr) {
  strData.strQuat.value[0] = qX;
  strData.strQuat.value[1] = qY;
  strData.strQuat.value[2] = qZ;
  strData.strQuat.value[3] = qW;
  strData.strQuat.valid = (valid and allowStr);
}