#ifndef FRAMEWORK_COORDINATES_JGM3MODEL_H_
#define FRAMEWORK_COORDINATES_JGM3MODEL_H_

#include <stdint.h>
#include "CoordinateTransformations.h"
#include "../globalfunctions/math/VectorOperations.h"
#include "../globalfunctions/timevalOperations.h"
#include "../globalfunctions/constants.h"
#include <memory.h>


template<uint8_t DEGREE,uint8_t ORDER>
class Jgm3Model {
public:
	static const uint32_t factorialLookupTable[DEGREE+3]; //This table is used instead of factorial calculation, must be increased if order or degree is higher

	Jgm3Model() {
		y0[0] = 0;
		y0[1] = 0;
		y0[2] = 0;
		y0[3] = 0;
		y0[4] = 0;
		y0[5] = 0;

		lastExecutionTime.tv_sec = 0;
		lastExecutionTime.tv_usec = 0;
	}
	virtual ~Jgm3Model(){};

	//double acsNavOrbit(double posECF[3],double velECF[3],timeval gpsTime);

	double y0[6]; //position and velocity at beginning of RK step in EC
	timeval lastExecutionTime; //Time of last execution


	void accelDegOrd(const double pos[3],const double S[ORDER+1][DEGREE+1],const double C[ORDER+1][DEGREE+1],double* accel){
		//Get radius of this position
		double r = VectorOperations<double>::norm(pos,3);



		//Initialize the V and W matrix
		double V[DEGREE+2][ORDER+2] = {{0}};
		double W[DEGREE+2][ORDER+2] = {{0}};

		for(uint8_t m=0;m<(ORDER+2);m++){
			for(uint8_t n=m;n<(DEGREE+2);n++){
				if((n==0) && (m==0)){
					//Montenbruck "Satellite Orbits Eq.3.31"
					V[0][0] = Earth::MEAN_RADIUS / r;
					W[0][0] = 0;
				}else{
					if(n==m){
						//Montenbruck "Satellite Orbits Eq.3.29"
						V[m][m] = (2*m-1)* (pos[0]*Earth::MEAN_RADIUS/pow(r,2)*V[m-1][m-1] - pos[1]*Earth::MEAN_RADIUS/pow(r,2)*W[m-1][m-1]);
						W[m][m] = (2*m-1)* (pos[0]*Earth::MEAN_RADIUS/pow(r,2)*W[m-1][m-1] + pos[1]*Earth::MEAN_RADIUS/pow(r,2)*V[m-1][m-1]);
					}else{
						//Montenbruck "Satellite Orbits Eq.3.30"
						V[n][m] = ((2*n-1)/(double)(n-m))*pos[2]*Earth::MEAN_RADIUS / pow(r,2)*V[n-1][m];
						W[n][m] = ((2*n-1)/(double)(n-m))*pos[2]*Earth::MEAN_RADIUS / pow(r,2)*W[n-1][m];
						if(n!=(m+1)){
							V[n][m] = V[n][m] - (((n+m-1)/(double)(n-m)) * (pow(Earth::MEAN_RADIUS,2) / pow(r,2)) * V[n-2][m]);
							W[n][m] = W[n][m] - (((n+m-1)/(double)(n-m)) * (pow(Earth::MEAN_RADIUS,2) / pow(r,2)) * W[n-2][m]);
						}//End of if(n!=(m+1))
					}//End of if(n==m){
				}//End of if(n==0 and m==0)
			}//End of for(uint8_t n=0;n<(DEGREE+1);n++)
		}//End of for(uint8_t m=0;m<(ORDER+1);m++)


		//overwrite accel if not properly initialized
		accel[0] = 0;
		accel[1] = 0;
		accel[2] = 0;

		for(uint8_t m=0;m<(ORDER+1);m++){
			for(uint8_t n=m;n<(DEGREE+1);n++){
				//Use table lookup to get factorial
				double partAccel[3] = {0};
				double factor = Earth::STANDARD_GRAVITATIONAL_PARAMETER/pow(Earth::MEAN_RADIUS,2);
				if(m==0){
					//Montenbruck "Satellite Orbits Eq.3.33"
					partAccel[0] = factor * (-C[n][0]*V[n+1][1]);
					partAccel[1] = factor * (-C[n][0]*W[n+1][1]);
				}else{
					double factMN = static_cast<double>(factorialLookupTable[n-m+2]) / static_cast<double>(factorialLookupTable[n-m]);
					partAccel[0] = factor * 0.5 * ((-C[n][m]*V[n+1][m+1]-S[n][m]*W[n+1][m+1])+factMN*(C[n][m]*V[n+1][m-1]+S[n][m]*W[n+1][m-1]));
					partAccel[1] = factor * 0.5 * ((-C[n][m]*W[n+1][m+1]+S[n][m]*V[n+1][m+1])+factMN*(-C[n][m]*W[n+1][m-1]+S[n][m]*V[n+1][m-1]));
				}

				partAccel[2] = factor * ((n-m+1)*(-C[n][m]*V[n+1][m]-S[n][m]*W[n+1][m]));


				accel[0] += partAccel[0];
				accel[1] += partAccel[1];
				accel[2] += partAccel[2];
			}//End of for(uint8_t n=0;n<DEGREE;n++)
		}//End of uint8_t m=0;m<ORDER;m++
	}

	void initializeNavOrbit(const double position[3],const double velocity[3], timeval timeUTC){
		CoordinateTransformations::positionEcfToEci(position,&y0[0],&timeUTC);
		CoordinateTransformations::velocityEcfToEci(velocity,position,&y0[3],&timeUTC);
		lastExecutionTime = timeUTC;
	}


	void acsNavOrbit(timeval timeUTC, const double S[ORDER+1][DEGREE+1],const double C[ORDER+1][DEGREE+1], double outputPos[3],double outputVel[3]){

		//RK4 Integration for this timestamp
		double deltaT = timevalOperations::toDouble(timeUTC-lastExecutionTime);

		double y0dot[6] = {0,0,0,0,0,0};
		double yA[6] = {0,0,0,0,0,0};
		double yAdot[6] = {0,0,0,0,0,0};
		double yB[6] = {0,0,0,0,0,0};
		double yBdot[6] = {0,0,0,0,0,0};
		double yC[6] = {0,0,0,0,0,0};
		double yCdot[6] = {0,0,0,0,0,0};

		//Step One
		rungeKuttaStep(y0,y0dot,lastExecutionTime,S,C);

		//Step Two
		VectorOperations<double>::mulScalar(y0dot,deltaT/2,yA,6);
		VectorOperations<double>::add(y0,yA,yA,6);
		rungeKuttaStep(yA,yAdot,lastExecutionTime,S,C);

		//Step Three
		VectorOperations<double>::mulScalar(yAdot,deltaT/2,yB,6);
		VectorOperations<double>::add(y0,yB,yB,6);
		rungeKuttaStep(yB,yBdot,lastExecutionTime,S,C);

		//Step Four
		VectorOperations<double>::mulScalar(yBdot,deltaT,yC,6);
		VectorOperations<double>::add(y0,yC,yC,6);
		rungeKuttaStep(yC,yCdot,lastExecutionTime,S,C);

		//Calc new State
		VectorOperations<double>::mulScalar(yAdot,2,yAdot,6);
		VectorOperations<double>::mulScalar(yBdot,2,yBdot,6);
		VectorOperations<double>::add(y0dot,yAdot,y0dot,6);
		VectorOperations<double>::add(y0dot,yBdot,y0dot,6);
		VectorOperations<double>::add(y0dot,yCdot,y0dot,6);
		VectorOperations<double>::mulScalar(y0dot,1./6.*deltaT,y0dot,6);
		VectorOperations<double>::add(y0,y0dot,y0,6);

		CoordinateTransformations::positionEciToEcf(&y0[0],outputPos,&timeUTC);
		CoordinateTransformations::velocityEciToEcf(&y0[3],&y0[0],outputVel,&timeUTC);

		lastExecutionTime = timeUTC;


	}


	void rungeKuttaStep(const double* yIn,double* yOut,timeval time, const double S[ORDER+1][DEGREE+1],const double C[ORDER+1][DEGREE+1]){
		double rECF[3] = {0,0,0};
		double rDotECF[3] = {0,0,0};
		double accelECF[3] = {0,0,0};
		double accelECI[3] = {0,0,0};


		CoordinateTransformations::positionEciToEcf(&yIn[0],rECF,&time);
		CoordinateTransformations::velocityEciToEcf(&yIn[3],&yIn[0],rDotECF,&time);
		accelDegOrd(rECF,S,C,accelECF);
		//This is not correct, as the acceleration would have derived terms but we don't know the velocity and position at that time
		//Tests showed that a wrong velocity does make the equation worse than neglecting it
		CoordinateTransformations::positionEcfToEci(accelECF,accelECI,&time);
		memcpy(&yOut[0],&yIn[3],sizeof(yOut[0])*3);
		memcpy(&yOut[3],accelECI,sizeof(yOut[0])*3);
	}
};






#endif /* FRAMEWORK_COORDINATES_JGM3MODEL_H_ */