/*
 * PtgCtrl.cpp
 *
 *  Created on: 17 Jul 2022
 *      Author: Robin Marquardt
 */



#include "PtgCtrl.h"
#include "../util/MathOperations.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>
#include <math.h>

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

PtgCtrl::~PtgCtrl(){

}

void PtgCtrl::loadAcsParameters(AcsParameters *acsParameters_){
	pointingModeControllerParameters = &(acsParameters_->targetModeControllerParameters);
	inertiaEIVE = &(acsParameters_->inertiaEIVE);
	rwHandlingParameters = &(acsParameters_->rwHandlingParameters);
	rwMatrices =&(acsParameters_->rwMatrices);
}

void PtgCtrl::ptgGroundstation(const double mode, const double *qError, const double *deltaRate,const double *rwPseudoInv, double *torqueRws){

	//------------------------------------------------------------------------------------------------
	// Compute gain matrix K and P matrix
	//------------------------------------------------------------------------------------------------
	double om = pointingModeControllerParameters->om;
	double zeta = pointingModeControllerParameters->zeta;
	double qErrorMin = pointingModeControllerParameters->qiMin;
	double omMax = pointingModeControllerParameters->omMax;

	double cInt = 2 * om * zeta;
	double kInt = 2 * pow(om,2);

	double qErrorLaw[3] = {0, 0, 0};

	for (int i = 0; i < 3; i++) {
		if (abs(qError[i]) < qErrorMin) {
			qErrorLaw[i] = qErrorMin;
		}
		else {
			qErrorLaw[i] = abs(qError[i]);
		}
	}
	double qErrorLawNorm = VectorOperations<double>::norm(qErrorLaw, 3);

	double gain1 = cInt * omMax / qErrorLawNorm;
	double gainVector[3] = {0, 0, 0};
	VectorOperations<double>::mulScalar(qErrorLaw, gain1, gainVector, 3);

	double gainMatrixDiagonal[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
	double gainMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
	gainMatrixDiagonal[0][0] =  gainVector[0];
	gainMatrixDiagonal[1][1] =  gainVector[1];
	gainMatrixDiagonal[2][2] =  gainVector[2];
	MatrixOperations<double>::multiply( *gainMatrixDiagonal, *(inertiaEIVE->inertiaMatrix), *gainMatrix, 3, 3, 3);

	// Inverse of gainMatrix
	double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
	gainMatrixInverse[0][0] = 1 / gainMatrix[0][0];
	gainMatrixInverse[1][1] = 1 / gainMatrix[1][1];
	gainMatrixInverse[2][2] = 1 / gainMatrix[2][2];

	double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
	MatrixOperations<double>::multiply(*gainMatrixInverse, *(inertiaEIVE->inertiaMatrix), *pMatrix, 3, 3, 3);
	MatrixOperations<double>::multiplyScalar(*pMatrix, kInt, *pMatrix, 3, 3);

	//------------------------------------------------------------------------------------------------
	// Torque Calculations for the reaction wheels
	//------------------------------------------------------------------------------------------------

	double pError[3] = {0, 0, 0};
	MatrixOperations<double>::multiply(*pMatrix, qError, pError, 3, 3, 1);
	double pErrorSign[3] = {0, 0, 0};

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

		if (abs(pError[i]) > 1) {
			pErrorSign[i] = sign(pError[i]);
		}
		else {
			pErrorSign[i] = pError[i];
		}
	}
//	Torque for quaternion error
	double torqueQuat[3] = {0, 0, 0};
	MatrixOperations<double>::multiply(*gainMatrix, pErrorSign, torqueQuat, 3, 3, 1);
	VectorOperations<double>::mulScalar(torqueQuat, -1, torqueQuat, 3);

//  Torque for rate error
	double torqueRate[3] = {0, 0, 0};
	MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), deltaRate, torqueRate, 3, 3, 1);
	VectorOperations<double>::mulScalar(torqueRate, cInt, torqueRate, 3);
	VectorOperations<double>::mulScalar(torqueRate, -1, torqueRate, 3);

//	Final commanded Torque for every reaction wheel
	double torque[3] = {0, 0, 0};
	VectorOperations<double>::add(torqueRate, torqueQuat, torque, 3);
	MatrixOperations<double>::multiply(rwPseudoInv, torque, torqueRws, 4, 3, 1);

}

void PtgCtrl::ptgDesaturation(double *magFieldEst, bool *magFieldEstValid, double *satRate,
                              int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2,
                              int32_t *speedRw3, double *mgtDpDes) {
  if (!(magFieldEstValid) || !(pointingModeControllerParameters->desatOn)) {
    mgtDpDes[0] = 0;
    mgtDpDes[1] = 0;
    mgtDpDes[2] = 0;
    return;
  }

  //	calculating momentum of satellite and momentum of reaction wheels
  double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3};
  double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0};
  VectorOperations<double>::mulScalar(speedRws, rwHandlingParameters->inertiaWheel, momentumRwU, 4);
  MatrixOperations<double>::multiply(*(rwMatrices->alignmentMatrix), momentumRwU, momentumRw, 3, 4,
                                     1);
  double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0};
  MatrixOperations<double>::multiply(*(inertiaEIVE->inertiaMatrix), satRate, momentumSat, 3, 3, 1);
  VectorOperations<double>::add(momentumSat, momentumRw, momentumTotal, 3);
  //	calculating momentum error
  double deltaMomentum[3] = {0, 0, 0};
  VectorOperations<double>::subtract(
      momentumTotal, pointingModeControllerParameters->desatMomentumRef, deltaMomentum, 3);
  //	resulting magnetic dipole command
  double crossMomentumMagField[3] = {0, 0, 0};
  VectorOperations<double>::cross(deltaMomentum, magFieldEst, crossMomentumMagField);
  double normMag = VectorOperations<double>::norm(magFieldEst, 3), factor = 0;
  factor = (pointingModeControllerParameters->deSatGainFactor) / normMag;
  VectorOperations<double>::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3);
}

void PtgCtrl::ptgNullspace(const int32_t *speedRw0, const int32_t *speedRw1,
                           const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) {
  double speedRws[4] = {*speedRw0, *speedRw1, *speedRw2, *speedRw3};
  double wheelMomentum[4] = {0, 0, 0, 0};
  double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60;
  // Conversion to [rad/s] for further calculations
  VectorOperations<double>::mulScalar(rpmOffset, factor, rpmOffset, 4);
  VectorOperations<double>::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4);
  double diffRwSpeed[4] = {0, 0, 0, 0};
  VectorOperations<double>::subtract(speedRws, rpmOffset, diffRwSpeed, 4);
  VectorOperations<double>::mulScalar(diffRwSpeed, rwHandlingParameters->inertiaWheel,
                                      wheelMomentum, 4);
  double gainNs = pointingModeControllerParameters->gainNullspace;
  double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
  MathOperations<double>::vecTransposeVecMatrix(rwMatrices->nullspace, rwMatrices->nullspace,
                                                *nullSpaceMatrix, 4);
  MatrixOperations<double>::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1);
  VectorOperations<double>::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4);
  VectorOperations<double>::mulScalar(rwTrqNs, -1, rwTrqNs, 4);
}