#include <fsfw/src/fsfw/globalfunctions/constants.h>
#include "AcsParameters.h"

//#include <stddef.h>
#include <math.h>

AcsParameters::AcsParameters() {

	onBoardParams.sampleTime = 0.1;

	inertiaEIVE.inertiaMatrix = {
			{  1.0, 0.0, 0.0},
			{  0.0, 1.0, 0.0},
			{  0.0, 0.5, 1.0}};

	mgmHandlingParameters.mgm0orientationMatrix = {
			{ 0, 0,-1},
			{ 0, 1, 0},
			{ 1, 0, 0}};
	mgmHandlingParameters.mgm1orientationMatrix = {
			{ 0, 0, 1},
			{ 0,-1, 0},
			{ 1, 0, 0}};
	mgmHandlingParameters.mgm2orientationMatrix = {
			{ 0, 0,-1},
			{ 0, 1, 0},
			{ 1 ,0, 0}};
    mgmHandlingParameters.mgm3orientationMatrix = {
    		{ 0, 0, 1},
			{ 0,-1, 0},
			{ 1, 0, 0}};
	mgmHandlingParameters.mgm4orientationMatrix = {
			{ 0, 0,-1},
			{-1, 0, 0},
			{ 0, 1, 0}};

	rwHandlingParameters.inertiaWheel = 0.000028198;
	rwHandlingParameters.maxTrq = 0.0032; //3.2 [mNm]

	//Geometry frame
	rwMatrices.alignmentMatrix = {
			{  0.9205,  0.0000, -0.9205,  0.0000},
			{  0.0000, -0.9205,  0.0000,  0.9205},
			{  0.3907,  0.3907,  0.3907,  0.3907}};

	rwMatrices.pseudoInverse = {
			{  0.4434, -0.2845,  0.3597},
			{  0.2136, -0.3317,  1.0123},
			{ -0.8672, -0.1406,  0.1778},
			{  0.6426,  0.4794,  1.3603}};

	rwMatrices.nullspace = { -0.7358,  0.5469, -0.3637, -0.1649};

	kalmanFilterParameters.sensorNoiseSS = 8 * Math::PI / 180;
	kalmanFilterParameters.sensorNoiseMAG = 4 * Math::PI / 180;
	kalmanFilterParameters.sensorNoiseSTR = 0.1 * Math::PI / 180;

	sunModelParameters.domega = 36000.771;
	sunModelParameters.omega_0 = 282.94 * Math::PI / 180.;
	sunModelParameters.dm = 35999.049;
	sunModelParameters.m_0 = 357.5256;
	sunModelParameters.e = 23.4392911 * Math::PI / 180.;
	sunModelParameters.e = 0.74508 * Math::PI / 180.;
	sunModelParameters.p1 = 6892. / 3600. * Math::PI / 180.;
	sunModelParameters.p2 = 72. / 3600. * Math::PI / 180.;

	safeModeControllerParameters.k_rate_mekf = 0.00059437;
	safeModeControllerParameters.k_align_mekf = 0.000056875;

	safeModeControllerParameters.sunTargetDir = {1,0,0};

	detumbleCtrlParameters.gainD = pow(10.0,-3.3);

	// Stuttgart
	groundStationParameters.latitudeGs = 48.7495 * Math::PI / 180.;
	groundStationParameters.longitudeGs = 9.10384 * Math::PI / 180.;
	groundStationParameters.altitudeGs = 500;

	groundStationParameters.earthRadiusEquat = 6378137;
	groundStationParameters.earthRadiusPolar = 6356752.314;

//	Geometry frame
	targetModeControllerParameters.refDirection = {1,0,0};
	targetModeControllerParameters.refRotRate = {0,0,0};
	targetModeControllerParameters.quatRef = {0,0,0,1};

	targetModeControllerParameters.avoidBlindStr = true;
	targetModeControllerParameters.blindAvoidStart = 1.5;
	targetModeControllerParameters.blindAvoidStop = 2.5;
	targetModeControllerParameters.blindRotRate = 1 * Math::PI /180;

	targetModeControllerParameters.zeta = 0.3;
	targetModeControllerParameters.om = 0.3;
	targetModeControllerParameters.qiMin = 0.1;
	targetModeControllerParameters.omMax = 1 * Math::PI / 180;
	targetModeControllerParameters.gainNullspace = 0.01;

	targetModeControllerParameters.desatMomentumRef = {0,0,0};
	targetModeControllerParameters.deSatGainFactor = 1000;
	targetModeControllerParameters.desatOn = true;

	targetModeControllerParameters.omegaEarth = 0.000072921158553;

	strParameters.boresightAxis = { 0.7593, 0,-0.6508};
	strParameters.exclusionAngle = 20 * Math::PI /180;

	magnetorquesParameter.mtq0orientationMatrix = {
			{ 1, 0, 0},
			{ 0, 0, 1},
			{ 0,-1, 0}};
	magnetorquesParameter.mtq1orientationMatrix = {
			{ 1, 0, 0},
			{ 0, 1, 0},
			{ 0, 0, 1}};
	magnetorquesParameter.mtq2orientationMatrix = {
			{ 0, 0, 1},
			{ 0, 1, 0},
			{-1, 0, 0}};
	magnetorquesParameter.alignmentMatrixMtq = {
			{ 0, 0,-1},
			{-1, 0, 0},
			{ 0, 1, 0}};
	magnetorquesParameter.inverseAlignment = {
			{ 0,-1, 0},
			{ 0, 0, 1},
			{-1, 0, 0}};
	magnetorquesParameter.DipolMax = 0.2;

}

AcsParameters::~AcsParameters() {
}

/*
ReturnValue_t AcsParameters::getParameter(uint8_t domainId,
		uint16_t parameterId, ParameterWrapper* parameterWrapper,
		const ParameterWrapper *newValues, uint16_t startAtIndex) {

	return 0;

}*/