/*
 * Guidance.cpp
 *
 *  Created on: 6 Jun 2022
 *      Author: Robin Marquardt
 */


#include "Guidance.h"
#include "string.h"
#include <math.h>
#include <acs/util/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/util/CholeskyDecomposition.h>

Guidance::Guidance(AcsParameters *acsParameters_) {
	acsParameters = *acsParameters_;

}

Guidance::~Guidance() {

}

void Guidance::getTargetParamsSafe(double sunTargetSafe[3], double satRateSafe[3]) {

	for (int i = 0; i < 3; i++) {

		sunTargetSafe[i] =
				acsParameters.safeModeControllerParameters.sunTargetDir[i];
		satRateSafe[i] =
				acsParameters.safeModeControllerParameters.satRateRef[i];

	}


//	memcpy(sunTargetSafe, acsParameters.safeModeControllerParameters.sunTargetDir, 24);

}

void Guidance::targetQuatPtg(ACS::SensorValues* sensorValues, ACS::OutputValues *outputValues,
		timeval now, double targetQuat[4], double refSatRate[3]){
//-------------------------------------------------------------------------------------
//	Calculation of target quaternion to groundstation
//-------------------------------------------------------------------------------------
//	Transform longitude, latitude and altitude of groundstation to cartesian coordiantes (earth fixed/centered frame)
	double groundStationCart[3] = {0, 0, 0};

	MathOperations<double>::cartesianFromLatLongAlt(acsParameters.groundStationParameters.latitudeGs,
			acsParameters.groundStationParameters.longitudeGs, acsParameters.groundStationParameters.altitudeGs,
			groundStationCart);

//	Position of the satellite in the earth/fixed frame via GPS
	double posSatE[3] = {0, 0, 0};
	MathOperations<double>::cartesianFromLatLongAlt(sensorValues->gps0latitude, sensorValues->gps0longitude,
			sensorValues->gps0altitude, posSatE);

//	Target direction in the ECEF frame
	double targetDirE[3] = {0, 0, 0};
	VectorOperations<double>::subtract(groundStationCart, posSatE, targetDirE, 3);

//	Transformation between ECEF and IJK frame
	double dcmEJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
	double dcmJE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
	MathOperations<double>::dcmEJ(now, *dcmEJ);
	MathOperations<double>::inverseMatrixDimThree(*dcmEJ, *dcmJE);
//	Derivative of dmcEJ WITHOUT PRECISSION AND NUTATION
	double dcmEJDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
	double dcmJEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
	double dcmDot[3][3] = {{0, 1, 0}, {-1, 0, 0}, {0, 0, 0}};
	double omegaEarth = acsParameters.targetModeControllerParameters.omegaEarth;

//	TEST SECTION !
	double dcmTEST[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
	MatrixOperations<double>::multiply(&acsParameters.magnetorquesParameter.mtq0orientationMatrix, dcmTEST, dcmTEST, 3, 3, 3);

	MatrixOperations<double>::multiply(*dcmDot, *dcmEJ, *dcmEJDot, 3, 3, 3);
	MatrixOperations<double>::multiplyScalar(*dcmEJDot, omegaEarth, *dcmEJDot, 3, 3);
	MathOperations<double>::inverseMatrixDimThree(*dcmEJDot, *dcmJEDot);

//	Transformation between ECEF and Body frame
	double dcmBJ[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
	double dcmBE[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
	double quatBJ[4] = {0, 0, 0, 0};
	quatBJ[0] = outputValues->quatMekfBJ[0];
	quatBJ[1] = outputValues->quatMekfBJ[1];
	quatBJ[2] = outputValues->quatMekfBJ[2];
	quatBJ[3] = outputValues->quatMekfBJ[3];
	QuaternionOperations::toDcm(quatBJ, dcmBJ);
	MatrixOperations<double>::multiply(*dcmBJ, *dcmJE, *dcmBE, 3, 3, 3);

//	Target Direction in the body frame
	double targetDirB[3] = {0, 0, 0};
	MatrixOperations<double>::multiply(*dcmBE, targetDirE, targetDirB, 3, 3, 1);

//	rotation quaternion from two vectors
	double refDir[3] = {0, 0, 0};
	refDir[0] = acsParameters.targetModeControllerParameters.refDirection[0];
	refDir[1] = acsParameters.targetModeControllerParameters.refDirection[1];
	refDir[2] = acsParameters.targetModeControllerParameters.refDirection[2];
	double noramlizedTargetDirB[3] =  {0, 0, 0};
	VectorOperations<double>::normalize(targetDirB, noramlizedTargetDirB, 3);
	VectorOperations<double>::normalize(refDir, refDir, 3);
	double normTargetDirB = VectorOperations<double>::norm(noramlizedTargetDirB, 3);
	double normRefDir = VectorOperations<double>::norm(refDir, 3);
	double crossDir[3] = {0, 0, 0};
	double dotDirections = VectorOperations<double>::dot(noramlizedTargetDirB, refDir);
	VectorOperations<double>::cross(noramlizedTargetDirB, refDir, crossDir);
	targetQuat[0] = crossDir[0];
	targetQuat[1] = crossDir[1];
	targetQuat[2] = crossDir[2];
	targetQuat[3] = sqrt(pow(normTargetDirB,2) * pow(normRefDir,2) + dotDirections);
	VectorOperations<double>::normalize(targetQuat, targetQuat, 4);

//-------------------------------------------------------------------------------------
//	Calculation of reference rotation rate
//-------------------------------------------------------------------------------------
	double velSatE[3] = {0, 0, 0};
	velSatE[0] = sensorValues->gps0Velocity[0];
	velSatE[1] = sensorValues->gps0Velocity[1];
	velSatE[2] = sensorValues->gps0Velocity[2];
	double velSatB[3] = {0, 0, 0}, velSatBPart1[3] = {0, 0, 0},
			velSatBPart2[3] = {0, 0, 0};
//	Velocity: v_B = dcm_BI * dcmIE * v_E + dcm_BI * DotDcm_IE * v_E
	MatrixOperations<double>::multiply(*dcmBE, velSatE, velSatBPart1, 3, 3, 1);
	double dcmBEDot[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
	MatrixOperations<double>::multiply(*dcmBJ, *dcmJEDot, *dcmBEDot, 3, 3, 3);
	MatrixOperations<double>::multiply(*dcmBEDot, posSatE, velSatBPart2, 3, 3, 1);
	VectorOperations<double>::add(velSatBPart1, velSatBPart2, velSatB, 3);

	double normVelSatB = VectorOperations<double>::norm(velSatB, 3);
	double normRefSatRate = normVelSatB / normTargetDirB;

	double satRateDir[3] = {0, 0, 0};
	VectorOperations<double>::cross(velSatB, targetDirB, satRateDir);
	VectorOperations<double>::normalize(satRateDir, satRateDir, 3);
	VectorOperations<double>::mulScalar(satRateDir, normRefSatRate, refSatRate, 3);

//-------------------------------------------------------------------------------------
//	Calculation of reference rotation rate in case of star tracker blinding
//-------------------------------------------------------------------------------------
	if ( acsParameters.targetModeControllerParameters.avoidBlindStr ) {

		double sunDirJ[3] = {0, 0, 0};
		double sunDirB[3] = {0, 0, 0};

		if ( outputValues->sunDirModelValid ) {

			sunDirJ[0] = outputValues->sunDirModel[0];
			sunDirJ[1] = outputValues->sunDirModel[1];
			sunDirJ[2] = outputValues->sunDirModel[2];
			MatrixOperations<double>::multiply(*dcmBJ, sunDirJ, sunDirB, 3, 3, 1);
		}

		else {
			sunDirB[0] = outputValues->sunDirEst[0];
			sunDirB[1] = outputValues->sunDirEst[1];
			sunDirB[2] = outputValues->sunDirEst[2];

		}

		double exclAngle = acsParameters.strParameters.exclusionAngle,
				blindStart = acsParameters.targetModeControllerParameters.blindAvoidStart,
				blindEnd = acsParameters.targetModeControllerParameters.blindAvoidStop;

		double sightAngleSun = VectorOperations<double>::dot(acsParameters.strParameters.boresightAxis, sunDirB);

		if ( !(strBlindAvoidFlag) ) {

			double critSightAngle = blindStart * exclAngle;

			if ( sightAngleSun < critSightAngle) {
				strBlindAvoidFlag = true;
			}

		}

		else {
			if ( sightAngleSun < blindEnd * exclAngle) {

				double normBlindRefRate = acsParameters.targetModeControllerParameters.blindRotRate;
				double blindRefRate[3] = {0, 0, 0};


				if ( sunDirB[1] < 0) {
					blindRefRate[0] = normBlindRefRate;
					blindRefRate[1] = 0;
					blindRefRate[2] = 0;
				}
				else {
					blindRefRate[0] = -normBlindRefRate;
					blindRefRate[1] = 0;
					blindRefRate[2] = 0;
				}

				VectorOperations<double>::add(blindRefRate, refSatRate, refSatRate, 3);

			}
			else {
				strBlindAvoidFlag = false;
			}
		}
	}
}


void Guidance::comparePtg(double targetQuat[4], ACS::OutputValues *outputValues, double refSatRate[3], double quatError[3], double deltaRate[3] ) {

	double quatRef[4] = {0, 0, 0, 0};
	quatRef[0] = acsParameters.targetModeControllerParameters.quatRef[0];
	quatRef[1] = acsParameters.targetModeControllerParameters.quatRef[1];
	quatRef[2] = acsParameters.targetModeControllerParameters.quatRef[2];
	quatRef[3] = acsParameters.targetModeControllerParameters.quatRef[3];

	double satRate[3] = {0, 0, 0};
	satRate[0] = outputValues->satRateMekf[0];
	satRate[1] = outputValues->satRateMekf[1];
	satRate[2] = outputValues->satRateMekf[2];
	VectorOperations<double>::subtract(satRate, refSatRate, deltaRate, 3);
	// valid checks ?
	double quatErrorMtx[4][4] = {{ quatRef[3], quatRef[2], -quatRef[1], -quatRef[0] },
			{ -quatRef[2], quatRef[3], quatRef[0], -quatRef[1] },
			{ quatRef[1], -quatRef[0], quatRef[3], -quatRef[2] },
			{ quatRef[0], -quatRef[1], quatRef[2], quatRef[3] }};

	double quatErrorComplete[4] = {0, 0, 0, 0};
	MatrixOperations<double>::multiply(*quatErrorMtx, targetQuat, quatErrorComplete, 4, 4, 1);

	if (quatErrorComplete[3] < 0) {
		quatErrorComplete[3] *= -1;
	}

	quatError[0] = quatErrorComplete[0];
	quatError[1] = quatErrorComplete[1];
	quatError[2] = quatErrorComplete[2];

	// target flag in matlab, importance, does look like it only gives
	//	feedback if pointing control is under 150 arcsec ??

}


void Guidance::getDistributionMatrixRw(ACS::SensorValues* sensorValues, double *rwPseudoInv) {

	if (sensorValues->validRw0 && sensorValues->validRw1 && sensorValues->validRw2 && sensorValues->validRw3) {

		rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0];
		rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1];
		rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2];
		rwPseudoInv[3] = acsParameters.rwMatrices.pseudoInverse[1][0];
		rwPseudoInv[4] = acsParameters.rwMatrices.pseudoInverse[1][1];
		rwPseudoInv[5] = acsParameters.rwMatrices.pseudoInverse[1][2];
		rwPseudoInv[6] = acsParameters.rwMatrices.pseudoInverse[2][0];
		rwPseudoInv[7] = acsParameters.rwMatrices.pseudoInverse[2][1];
		rwPseudoInv[8] = acsParameters.rwMatrices.pseudoInverse[2][2];
		rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0];
		rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1];
		rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2];

	}

	else if (!(sensorValues->validRw0) && sensorValues->validRw1 && sensorValues->validRw2 && sensorValues->validRw3) {

		rwPseudoInv[0] = acsParameters.rwMatrices.without0[0][0];
		rwPseudoInv[1] = acsParameters.rwMatrices.without0[0][1];
		rwPseudoInv[2] = acsParameters.rwMatrices.without0[0][2];
		rwPseudoInv[3] = acsParameters.rwMatrices.without0[1][0];
		rwPseudoInv[4] = acsParameters.rwMatrices.without0[1][1];
		rwPseudoInv[5] = acsParameters.rwMatrices.without0[1][2];
		rwPseudoInv[6] = acsParameters.rwMatrices.without0[2][0];
		rwPseudoInv[7] = acsParameters.rwMatrices.without0[2][1];
		rwPseudoInv[8] = acsParameters.rwMatrices.without0[2][2];
		rwPseudoInv[9] = acsParameters.rwMatrices.without0[3][0];
		rwPseudoInv[10] = acsParameters.rwMatrices.without0[3][1];
		rwPseudoInv[11] = acsParameters.rwMatrices.without0[3][2];
	}

	else if ((sensorValues->validRw0) && !(sensorValues->validRw1) && sensorValues->validRw2 && sensorValues->validRw3) {

		rwPseudoInv[0] = acsParameters.rwMatrices.without1[0][0];
		rwPseudoInv[1] = acsParameters.rwMatrices.without1[0][1];
		rwPseudoInv[2] = acsParameters.rwMatrices.without1[0][2];
		rwPseudoInv[3] = acsParameters.rwMatrices.without1[1][0];
		rwPseudoInv[4] = acsParameters.rwMatrices.without1[1][1];
		rwPseudoInv[5] = acsParameters.rwMatrices.without1[1][2];
		rwPseudoInv[6] = acsParameters.rwMatrices.without1[2][0];
		rwPseudoInv[7] = acsParameters.rwMatrices.without1[2][1];
		rwPseudoInv[8] = acsParameters.rwMatrices.without1[2][2];
		rwPseudoInv[9] = acsParameters.rwMatrices.without1[3][0];
		rwPseudoInv[10] = acsParameters.rwMatrices.without1[3][1];
		rwPseudoInv[11] = acsParameters.rwMatrices.without1[3][2];
	}

	else if ((sensorValues->validRw0) && (sensorValues->validRw1) && !(sensorValues->validRw2) && sensorValues->validRw3) {

		rwPseudoInv[0] = acsParameters.rwMatrices.without2[0][0];
		rwPseudoInv[1] = acsParameters.rwMatrices.without2[0][1];
		rwPseudoInv[2] = acsParameters.rwMatrices.without2[0][2];
		rwPseudoInv[3] = acsParameters.rwMatrices.without2[1][0];
		rwPseudoInv[4] = acsParameters.rwMatrices.without2[1][1];
		rwPseudoInv[5] = acsParameters.rwMatrices.without2[1][2];
		rwPseudoInv[6] = acsParameters.rwMatrices.without2[2][0];
		rwPseudoInv[7] = acsParameters.rwMatrices.without2[2][1];
		rwPseudoInv[8] = acsParameters.rwMatrices.without2[2][2];
		rwPseudoInv[9] = acsParameters.rwMatrices.without2[3][0];
		rwPseudoInv[10] = acsParameters.rwMatrices.without2[3][1];
		rwPseudoInv[11] = acsParameters.rwMatrices.without2[3][2];
	}

	else if ((sensorValues->validRw0) && (sensorValues->validRw1) && (sensorValues->validRw2) && (sensorValues->validRw3)) {

		rwPseudoInv[0] = acsParameters.rwMatrices.without3[0][0];
		rwPseudoInv[1] = acsParameters.rwMatrices.without3[0][1];
		rwPseudoInv[2] = acsParameters.rwMatrices.without3[0][2];
		rwPseudoInv[3] = acsParameters.rwMatrices.without3[1][0];
		rwPseudoInv[4] = acsParameters.rwMatrices.without3[1][1];
		rwPseudoInv[5] = acsParameters.rwMatrices.without3[1][2];
		rwPseudoInv[6] = acsParameters.rwMatrices.without3[2][0];
		rwPseudoInv[7] = acsParameters.rwMatrices.without3[2][1];
		rwPseudoInv[8] = acsParameters.rwMatrices.without3[2][2];
		rwPseudoInv[9] = acsParameters.rwMatrices.without3[3][0];
		rwPseudoInv[10] = acsParameters.rwMatrices.without3[3][1];
		rwPseudoInv[11] = acsParameters.rwMatrices.without3[3][2];
	}

	else {
// 	    @note: This one takes the normal pseudoInverse of all four raction wheels valid.
//		Does not make sense, but is implemented that way in MATLAB ?!
//		Thought: It does not really play a role, because in case there are more then one
//		reaction wheel the pointing control is destined to fail.
		rwPseudoInv[0] = acsParameters.rwMatrices.pseudoInverse[0][0];
		rwPseudoInv[1] = acsParameters.rwMatrices.pseudoInverse[0][1];
		rwPseudoInv[2] = acsParameters.rwMatrices.pseudoInverse[0][2];
		rwPseudoInv[3] = acsParameters.rwMatrices.pseudoInverse[1][0];
		rwPseudoInv[4] = acsParameters.rwMatrices.pseudoInverse[1][1];
		rwPseudoInv[5] = acsParameters.rwMatrices.pseudoInverse[1][2];
		rwPseudoInv[6] = acsParameters.rwMatrices.pseudoInverse[2][0];
		rwPseudoInv[7] = acsParameters.rwMatrices.pseudoInverse[2][1];
		rwPseudoInv[8] = acsParameters.rwMatrices.pseudoInverse[2][2];
		rwPseudoInv[9] = acsParameters.rwMatrices.pseudoInverse[3][0];
		rwPseudoInv[10] = acsParameters.rwMatrices.pseudoInverse[3][1];
		rwPseudoInv[11] = acsParameters.rwMatrices.pseudoInverse[3][2];

	}
}