/*
 * Navigation.cpp
 *
 *  Created on: 23 May 2022
 *      Author: Robin Marquardt
 */

#include "Navigation.h"

#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <math.h>

#include "util/CholeskyDecomposition.h"
#include "util/MathOperations.h"

Navigation::Navigation(AcsParameters *acsParameters_) : multiplicativeKalmanFilter(acsParameters_) {
  acsParameters = *acsParameters_;
}

Navigation::~Navigation() {}

void Navigation::useMekf(ACS::SensorValues *sensorValues, ACS::OutputValues *outputValues,
                         ReturnValue_t *mekfValid) {
  double quatJB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
                      sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value};
  bool quatJBValid =
      (sensorValues->strSet.caliQx.isValid() && sensorValues->strSet.caliQy.isValid() &&
       sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid());

  if (kalmanInit) {
    *mekfValid = multiplicativeKalmanFilter.mekfEst(
        quatJB, &quatJBValid, outputValues->satRateEst, &outputValues->satRateEstValid,
        outputValues->magFieldEst, &outputValues->magFieldEstValid, outputValues->sunDirEst,
        &outputValues->sunDirEstValid, outputValues->sunDirModel, &outputValues->sunDirModelValid,
        outputValues->magFieldModel, &outputValues->magFieldModelValid, outputValues->quatMekfBJ,
        outputValues->satRateMekf);  // VALIDS FOR QUAT AND RATE ??
  }
  else {
    multiplicativeKalmanFilter.init(outputValues->magFieldEst, &outputValues->magFieldEstValid,
                                    outputValues->sunDirEst, &outputValues->sunDirEstValid,
                                    outputValues->sunDirModel, &outputValues->sunDirModelValid,
                                    outputValues->magFieldModel, &outputValues->magFieldModelValid);
    kalmanInit = true;
    *mekfValid = 0;

    //		Maybe we need feedback from kalmanfilter to identify if there was a problem with the init
    //of kalman filter 		where does this class know from that kalman filter was not initialized ?
  }
}