/*
 * MathOperations.h
 *
 *  Created on: 3 Mar 2022
 *      Author: rooob
 */

#ifndef MATH_MATHOPERATIONS_H_
#define MATH_MATHOPERATIONS_H_

#include <cmath>
#include <stdint.h>
#include <string.h>
#include <fsfw/src/fsfw/globalfunctions/constants.h>
#include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>

using namespace Math;

template<typename T1, typename T2 = T1>
class MathOperations {
public:
	static void skewMatrix(const T1 vector[], T2 *result) {
		// Input Dimension [3], Output [3][3]
		result[0] = 0;
		result[1] = -vector[2];
		result[2] = vector[1];
		result[3] = vector[2];
		result[4] = 0;
		result[5] = -vector[0];
		result[6] = -vector[1];
		result[7] = vector[0];
		result[8] = 0;
	}
	static void vecTransposeVecMatrix(const T1 vector1[], const T1 transposeVector2[],
			T2 *result, uint8_t size = 3) {
		// Looks like MatrixOpertions::multiply is able to do the same thing
		for (uint8_t resultColumn = 0; resultColumn < size; resultColumn++) {
			for (uint8_t resultRow = 0; resultRow < size; resultRow++) {
				result[resultColumn + size * resultRow] = vector1[resultRow]
						* transposeVector2[resultColumn];

			}
		}
		/*matrixSun[i][j] = sunEstB[i] * sunEstB[j];
		 matrixMag[i][j] = magEstB[i] * magEstB[j];
		 matrixSunMag[i][j] = sunEstB[i] * magEstB[j];
		 matrixMagSun[i][j] = magEstB[i] * sunEstB[j];*/
	}

	static void selectionSort(const T1 *matrix, T1 *result, uint8_t rowSize,
			uint8_t colSize) {
		int min_idx;
		T1 temp;
		memcpy(result, matrix, rowSize * colSize * sizeof(*result));
		// One by one move boundary of unsorted subarray
		for (int k = 0; k < rowSize; k++) {
			for (int i = 0; i < colSize - 1; i++) {
				// Find the minimum element in unsorted array
				min_idx = i;
				for (int j = i + 1; j < colSize; j++) {
					if (result[j + k * colSize]
							< result[min_idx + k * colSize]) {
						min_idx = j;
					}
				}
				// Swap the found minimum element with the first element
				temp = result[i + k * colSize];
				result[i + k * colSize] = result[min_idx + k * colSize];
				result[min_idx + k * colSize] = temp;
			}
		}
	}

	static void convertDateToJD2000(const T1 time, T2 julianDate){

		// time = { Y, M, D, h, m,s}
		// time in sec and microsec -> The Epoch (unixtime)
				julianDate = 1721013.5 + 367*time[0]- floor(7/4*(time[0]+(time[1]+9)/12))
						+floor(275*time[1]/9)+time[2]+(60*time[3]+time[4]+(time(5)/60))/1440;
	}

	static T1 convertUnixToJD2000(timeval time){
		//time = {{s},{us}}
		T1 julianDate2000;
		julianDate2000 = (time.tv_sec/86400.0)+2440587.5-2451545;
		return julianDate2000;
	}

	static void dcmFromQuat(const T1 vector[], T1 *outputDcm){
		// convention q = [qx,qy,qz, qw]
		outputDcm[0] = pow(vector[0],2) - pow(vector[1],2) - pow(vector[2],2) + pow(vector[3],2);
		outputDcm[1] = 2*(vector[0]*vector[1] + vector[2]*vector[3]);
		outputDcm[2] = 2*(vector[0]*vector[2] - vector[1]*vector[3]);

		outputDcm[3] = 2*(vector[1]*vector[0] - vector[2]*vector[3]);
		outputDcm[4] = -pow(vector[0],2) + pow(vector[1],2) - pow(vector[2],2) + pow(vector[3],2);
		outputDcm[5] = 2*(vector[1]*vector[2] + vector[0]*vector[3]);

		outputDcm[6] = 2*(vector[2]*vector[0] + vector[1]*vector[3]);
		outputDcm[7] = 2*(vector[2]*vector[1] - vector[0]*vector[3]);
		outputDcm[8] = -pow(vector[0],2) - pow(vector[1],2) + pow(vector[2],2) + pow(vector[3],2);

	}

	static void cartesianFromLatLongAlt(const T1 lat, const T1 longi, const T1 alt, T2 *cartesianOutput){

		double radiusPolar = 6378137;
		double radiusEqua = 6356752.314;

		double eccentricity = sqrt(1 - pow(radiusPolar,2) / pow(radiusEqua,2));
		double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity,2) * pow(sin(lat),2));

		cartesianOutput[0] = (auxRadius + alt) * cos(lat) * cos(longi);
		cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi);
		cartesianOutput[2] = ((1 - pow(eccentricity,2)) * auxRadius + alt) * sin(lat);

	}

	/* @brief:  dcmEJ() - calculates the transformation matrix between ECEF and ECI frame
	 * @param:  time   			Current time
	 * 			outputDcmEJ 	Transformation matrix from ECI (J) to ECEF (E) [3][3]
	 * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.32ff
	 * 			Landis Markley and John L. Crassidis*/
	static void dcmEJ(timeval time, T1 * outputDcmEJ){

		double JD2000Floor = 0;
		double JD2000 = convertUnixToJD2000(time);
		// Getting Julian Century from Day start : JD (Y,M,D,0,0,0)
		JD2000Floor = floor(JD2000);
		if ( ( JD2000 - JD2000Floor) < 0.5) {
			JD2000Floor -= 0.5;
		}
		else {
			JD2000Floor += 0.5;
		}

		double JC2000 = JD2000Floor / 36525;
		double sec = (JD2000 - JD2000Floor) * 86400;
		double gmst = 0; //greenwich mean sidereal time
		gmst = 24110.54841 + 8640184.812866 * JC2000 + 0.093104 * pow(JC2000,2) -
				0.0000062 * pow(JC2000,3) + 1.002737909350795 * sec;
		double rest = gmst / 86400;
		double FloorRest = floor(rest);
		double secOfDay = rest-FloorRest;
		secOfDay *= 86400;
		gmst = secOfDay / 240 * PI / 180;

		outputDcmEJ[0] = cos(gmst);
		outputDcmEJ[1] = sin(gmst);
		outputDcmEJ[2] = 0;
		outputDcmEJ[3] = -sin(gmst);
		outputDcmEJ[4] = cos(gmst);
		outputDcmEJ[5] = 0;
		outputDcmEJ[6] = 0;
		outputDcmEJ[7] = 0;
		outputDcmEJ[8] = 1;

	}

	/* @brief:  ecfToEciWithNutPre() - calculates the transformation matrix between ECEF and ECI frame
	 * 			give also the back the derivative of this matrix
	 * @param:  unixTime   		Current time in Unix format
	 * 			outputDcmEJ 	Transformation matrix from ECI (J) to ECEF (E) [3][3]
	 * 			outputDotDcmEJ	Derivative of transformation matrix [3][3]
	 * @source: Entwicklung einer Simulationsumgebung und robuster Algorithmen für das Lage- und
				Orbitkontrollsystem der Kleinsatelliten Flying Laptop und PERSEUS, P.244ff
	 * 			Oliver Zeile
	 * 			https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110*/
	static void ecfToEciWithNutPre(timeval unixTime, T1 * outputDcmEJ, T1 * outputDotDcmEJ ) {

//		TT = UTC/Unix + 32.184s (TAI Difference) + 27 (Leap Seconds in UTC since 1972) + 10 (initial Offset)
//		International Atomic Time (TAI)

		double JD2000UTC1 = convertUnixToJD2000(unixTime);

//		Julian Date / century from TT
		timeval terestrialTime = unixTime;
		terestrialTime.tv_sec = unixTime.tv_sec + 32.184 + 37;
		double JD2000TT = convertUnixToJD2000(terestrialTime);
		double JC2000TT = JD2000TT / 36525;

//-------------------------------------------------------------------------------------
//		Calculation of Transformation from earth rotation Theta
//-------------------------------------------------------------------------------------
		double theta[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
//		Earth Rotation angle
		double era = 0;
		era = 2* PI *(0.779057273264 + 1.00273781191135448 * JD2000UTC1);
//		Greenwich Mean Sidereal Time
		double gmst2000 = 0.014506 + 4612.15739966 * JC2000TT + 1.39667721 * pow(JC2000TT,2) -
				0.00009344 * pow(JC2000TT,3) + 0.00001882 * pow(JC2000TT,4);
		double arcsecFactor = 1 * PI / (180 * 3600);
		gmst2000 *= arcsecFactor;
		gmst2000 += era;

		theta[0][0] = cos(gmst2000);
		theta[0][1] = sin(gmst2000);
		theta[0][2] = 0;
		theta[1][0] = -sin(gmst2000);
		theta[1][1] = cos(gmst2000);
		theta[1][2] = 0;
		theta[2][0] = 0;
		theta[2][1] = 0;
		theta[2][2] = 1;

//-------------------------------------------------------------------------------------
//		Calculation of Transformation from earth Precession P
//-------------------------------------------------------------------------------------
		double precession[3][3] = {{0,0,0},{0,0,0},{0,0,0}};

        double zeta = 2306.2181 * JC2000TT + 0.30188 * pow(JC2000TT,2) + 0.017998 * pow(JC2000TT,3);
        double theta2 = 2004.3109 * JC2000TT - 0.42665 * pow(JC2000TT,2) - 0.041833 * pow(JC2000TT,3);
        double ze = zeta + 0.79280 * pow(JC2000TT,2) + 0.000205 * pow(JC2000TT,3);

        zeta  *= arcsecFactor;
        theta2 *= arcsecFactor;
        ze    *= arcsecFactor;

        precession[0][0]=-sin(ze)*sin(zeta)+cos(ze)*cos(theta2)*cos(zeta);
        precession[1][0]=cos(ze)*sin(zeta)+sin(ze)*cos(theta2)*cos(zeta);
        precession[2][0]=sin(theta2)*cos(zeta);
        precession[0][1]=-sin(ze)*cos(zeta)-cos(ze)*cos(theta2)*sin(zeta);
        precession[1][1]=cos(ze)*cos(zeta)-sin(ze)*cos(theta2)*sin(zeta);
        precession[2][1]=-sin(theta2)*sin(zeta);
        precession[0][2]=-cos(ze)*sin(theta2);
        precession[1][2]=-sin(ze)*sin(theta2);
        precession[2][2]=cos(theta2);

//-------------------------------------------------------------------------------------
//		Calculation of Transformation from earth Nutation N
//-------------------------------------------------------------------------------------
		double nutation[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
//      lunar asc node
        double Om = 125 * 3600 + 2 * 60 + 40.28 - (1934 * 3600 + 8 * 60 + 10.539) * JC2000TT +
        		7.455 * pow(JC2000TT,2) + 0.008 * pow(JC2000TT,3);
        Om *= arcsecFactor;
//      delta psi approx
        double dp = -17.2 * arcsecFactor *sin(Om);

//      delta eps approx
        double de = 9.203 * arcsecFactor *cos(Om);

//        % true obliquity of the ecliptic eps p.71 (simplified)
        double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180;;

        nutation[0][0]=cos(dp);
        nutation[1][0]=cos(e+de)*sin(dp);
        nutation[2][0]=sin(e+de)*sin(dp);
        nutation[0][1]=-cos(e)*sin(dp);
        nutation[1][1]=cos(e)*cos(e+de)*cos(dp)+sin(e)*sin(e+de);
        nutation[2][1]=cos(e)*sin(e+de)*cos(dp)-sin(e)*cos(e+de);
        nutation[0][2]=-sin(e)*sin(dp);
        nutation[1][2]=sin(e)*cos(e+de)*cos(dp)-cos(e)*sin(e+de);
        nutation[2][2]=sin(e)*sin(e+de)*cos(dp)+cos(e)*cos(e+de);

//-------------------------------------------------------------------------------------
//		Calculation of Derivative of rotation matrix from earth
//-------------------------------------------------------------------------------------
		double thetaDot[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
		double dotMatrix[3][3] = {{0,1,0},{-1,0,0},{0,0,0}};
		double omegaEarth = 0.000072921158553;
		MatrixOperations<double>::multiply(*dotMatrix, *theta, *thetaDot, 3, 3, 3);
		MatrixOperations<double>::multiplyScalar(*thetaDot, omegaEarth, *thetaDot, 3, 3);

//-------------------------------------------------------------------------------------
//		Calculation of transformation matrix and Derivative of transformation matrix
//-------------------------------------------------------------------------------------
		double nutationPrecession[3][3] = {{0,0,0},{0,0,0},{0,0,0}};
		MatrixOperations<double>::multiply(*nutation, *precession, *nutationPrecession, 3, 3, 3);
		MatrixOperations<double>::multiply(*nutationPrecession, *theta, outputDcmEJ, 3, 3, 3);

		MatrixOperations<double>::multiply(*nutationPrecession, *thetaDot, outputDotDcmEJ, 3, 3, 3);

	}

	static void inverseMatrixDimThree(const T1 *matrix, T1 * output){

		int i,j;
		double determinant;
		double mat[3][3] = {{matrix[0], matrix[1], matrix[2]},{matrix[3], matrix[4], matrix[5]},
				{matrix[6], matrix[7], matrix[8]}};

		for(i = 0; i < 3; i++) {
		determinant = determinant + (mat[0][i] * (mat[1][(i+1)%3] * mat[2][(i+2)%3] - mat[1][(i+2)%3] * mat[2][(i+1)%3]));
		}
//		cout<<"\n\ndeterminant: "<<determinant;
//		cout<<"\n\nInverse of matrix is: \n";
		for(i = 0; i < 3; i++){
		for(j = 0; j < 3; j++){
			output[i*3+j] = ((mat[(j+1)%3][(i+1)%3] * mat[(j+2)%3][(i+2)%3]) - (mat[(j+1)%3][(i+2)%3] * mat[(j+2)%3][(i+1)%3]))/ determinant;
		}
		}
	}

};

#endif /* ACS_MATH_MATHOPERATIONS_H_ */