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

#include "Navigation.h"
#include <math.h>
#include <acs/math/MathOperations.h>
#include <fsfw/src/fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h>
#include <acs/math/CholeskyDecomposition.h>

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

Navigation::~Navigation(){

}

void Navigation::useMekf(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues, ReturnValue_t *mekfValid){

	if (kalmanInit) {
		*mekfValid = multiplicativeKalmanFilter.mekfEst(sensorValues->quatJB, &sensorValues->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 ?
	}
}