/*
 * Detumble.cpp
 *
 *  Created on: 17 Aug 2022
 *      Author: Robin Marquardt
 */


#include "Detumble.h"
#include "../util/MathOperations.h"
#include <math.h>
#include <fsfw/globalfunctions/constants.h>
#include <fsfw/globalfunctions/math/MatrixOperations.h>
#include <fsfw/globalfunctions/math/QuaternionOperations.h>
#include <fsfw/globalfunctions/math/VectorOperations.h>
#include <fsfw/globalfunctions/sign.h>


Detumble::Detumble(AcsParameters *acsParameters_){
	loadAcsParameters(acsParameters_);
}

Detumble::~Detumble(){

}

void Detumble::loadAcsParameters(AcsParameters *acsParameters_){

	detumbleParameter = &(acsParameters_->detumbleParameter);
	magnetorquesParameter = &(acsParameters_->magnetorquesParameter);

}


ReturnValue_t Detumble::bDotLaw(const double *magRate, const bool *magRateValid,
		const double *magField, const bool *magFieldValid,
		double *magMom) {

	if (!magRateValid || !magFieldValid) {
		return DETUMBLE_NO_SENSORDATA;
	}
	double gain = detumbleParameter->gainD;
	double factor = -gain / pow(VectorOperations<double>::norm(magField,3),2);
	VectorOperations<double>::mulScalar(magRate, factor, magMom, 3);
	return returnvalue::OK;

}

ReturnValue_t Detumble::bangbangLaw(const double *magRate, const bool *magRateValid, double *magMom) {

	if (!magRateValid) {
		return DETUMBLE_NO_SENSORDATA;
	}

	double dipolMax = magnetorquesParameter->DipolMax;
	for (int i = 0; i<3; i++) {

		magMom[i] = - dipolMax * sign(magRate[i]);

	}

	return returnvalue::OK;

}

ReturnValue_t Detumble::bDotLawGyro(const double *satRate, const bool *satRateValid,
		const double *magField, const bool *magFieldValid,
		double *magMom) {

	if (!satRateValid || !magFieldValid) {
		return DETUMBLE_NO_SENSORDATA;
	}
	double gain = detumbleParameter->gainD;
	double factor = -gain / pow(VectorOperations<double>::norm(magField,3),2);
	VectorOperations<double>::mulScalar(satRate, factor, magMom, 3);
	return returnvalue::OK;

}