#include <framework/coordinates/CoordinateTransformations.h>
#include <framework/globalfunctions/constants.h>
#include <framework/globalfunctions/math/MatrixOperations.h>
#include <framework/globalfunctions/math/VectorOperations.h>
#include <stddef.h>
#include <cmath>


//TODO move time stuff to OSAL

void CoordinateTransformations::positionEcfToEci(const double* ecfPosition,
		double* eciPosition) {
	ecfToEci(ecfPosition, eciPosition, NULL);

}

void CoordinateTransformations::velocityEcfToEci(const double* ecfVelocity,
		const double* ecfPosition, double* eciVelocity) {
	ecfToEci(ecfVelocity, eciVelocity, ecfPosition);
}

double CoordinateTransformations::getEarthRotationAngle(timeval time) {

	double jD2000UTC = (time.tv_sec - 946728000. + time.tv_usec / 1000000.)
			/ 24. / 3600.;

	//value of unix time at J2000TT
	static const double J2000TtUnix = 946727935.816;

	//TT does not have leap seconds
	//so we need to add the leap seconds since J2000 to our UTC based clock
	//Conveniently, GPS gives us access to the leap seconds since 1980
	//between 1980 and 2000 13 leap seconds happened
	uint8_t leapSecondsSinceJ2000 = utcGpsOffset - 13;

	//Julean centuries since J2000 //TODO fails for dates before now?
	double TTt2000 = (time.tv_sec + time.tv_usec / 1000000. - J2000TtUnix
			+ leapSecondsSinceJ2000) / 24. / 3600. / 36525.;

	double theta = 2 * Math::PI
			* (0.779057273264 + 1.00273781191135448 * jD2000UTC);

	//Correct theta according to IAU 2000 precession-nutation model
	theta = theta + 7.03270725817493E-008 + 0.0223603701 * TTt2000
			+ 6.77128219501896E-006 * TTt2000 * TTt2000
			+ 4.5300990362875E-010 * TTt2000 * TTt2000 * TTt2000
			+ 9.12419347848147E-011 * TTt2000 * TTt2000 * TTt2000 * TTt2000;
	return theta;
}

void CoordinateTransformations::getEarthRotationMatrix(timeval time,
		double matrix[][3]) {
	double theta = getEarthRotationAngle(time);

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

void CoordinateTransformations::ecfToEci(const double* ecfCoordinates,
		double* eciCoordinates,
		const double* ecfPositionIfCoordinatesAreVelocity) {
	//TODO all calculations only work with a correct time

	timeval time;
	OSAL::getClock_timeval(&time);

	//value of unix time at J2000TT
	static const double J2000TtUnix = 946727935.816;

	//we need TT which does not have leap seconds
	//so we need to add the leap seconds since J2000 to our UTC based clock
	//Conveniently, GPS gives us access to the leap seconds since 1980
	//between 1980 and 2000 13 leap seconds happened
	uint8_t leapSecondsSinceJ2000 = utcGpsOffset - 13;

	//Julean centuries since J2000 //TODO fails for dates before now?
	double TTt2000 = (time.tv_sec + time.tv_usec / 1000000. - J2000TtUnix
			+ leapSecondsSinceJ2000) / 24. / 3600. / 36525.;

	//////////////////////////////////////////////////////////
	// Calculate Precession Matrix

	double zeta = 0.0111808609 * TTt2000
			+ 1.46355554053347E-006 * TTt2000 * TTt2000
			+ 8.72567663260943E-008 * TTt2000 * TTt2000 * TTt2000;
	double theta_p = 0.0097171735 * TTt2000
			- 2.06845757045384E-006 * TTt2000 * TTt2000
			- 2.02812107218552E-007 * TTt2000 * TTt2000 * TTt2000;
	double z = zeta + 3.8436028638364E-006 * TTt2000 * TTt2000
			+ 0.000000001 * TTt2000 * TTt2000 * TTt2000;

	double mPrecession[3][3];

	mPrecession[0][0] = -sin(z) * sin(zeta) + cos(z) * cos(theta_p) * cos(zeta);
	mPrecession[1][0] = cos(z) * sin(zeta) + sin(z) * cos(theta_p) * cos(zeta);
	mPrecession[2][0] = sin(theta_p) * cos(zeta);

	mPrecession[0][1] = -sin(z) * cos(zeta) - cos(z) * cos(theta_p) * sin(zeta);
	mPrecession[1][1] = cos(z) * cos(zeta) - sin(z) * cos(theta_p) * sin(zeta);
	mPrecession[2][1] = -sin(theta_p) * sin(zeta);

	mPrecession[0][2] = -cos(z) * sin(theta_p);
	mPrecession[1][2] = -sin(z) * sin(theta_p);
	mPrecession[2][2] = cos(theta_p);

	//////////////////////////////////////////////////////////
	// Calculate Nutation Matrix

	double omega_moon = 2.1824386244 - 33.7570459338 * TTt2000
			+ 3.61428599267159E-005 * TTt2000 * TTt2000
			+ 3.87850944887629E-008 * TTt2000 * TTt2000 * TTt2000;

	double deltaPsi = -0.000083388 * sin(omega_moon);
	double deltaEpsilon = 4.46174030725106E-005 * cos(omega_moon);

	double epsilon = 0.4090928042 - 0.0002269655 * TTt2000
			- 2.86040071854626E-009 * TTt2000 * TTt2000
			+ 8.78967203851589E-009 * TTt2000 * TTt2000 * TTt2000;

	double mNutation[3][3];

	mNutation[0][0] = cos(deltaPsi);
	mNutation[1][0] = cos(epsilon + deltaEpsilon) * sin(deltaPsi);
	mNutation[2][0] = sin(epsilon + deltaEpsilon) * sin(deltaPsi);

	mNutation[0][1] = -cos(epsilon) * sin(deltaPsi);
	mNutation[1][1] = cos(epsilon) * cos(epsilon + deltaEpsilon) * cos(deltaPsi)
			+ sin(epsilon) * sin(epsilon + deltaEpsilon);
	mNutation[2][1] = cos(epsilon) * sin(epsilon + deltaEpsilon) * cos(deltaPsi)
			- sin(epsilon) * cos(epsilon + deltaEpsilon);

	mNutation[0][2] = -sin(epsilon) * sin(deltaPsi);
	mNutation[1][2] = sin(epsilon) * cos(epsilon + deltaEpsilon) * cos(deltaPsi)
			- cos(epsilon) * sin(epsilon + deltaEpsilon);
	mNutation[2][2] = sin(epsilon) * sin(epsilon + deltaEpsilon) * cos(deltaPsi)
			+ cos(epsilon) * cos(epsilon + deltaEpsilon);

	//////////////////////////////////////////////////////////
	// Calculate Earth rotation matrix

	//calculate theta

	double mTheta[3][3];
	getEarthRotationMatrix(time, mTheta);

	//polar motion is neglected

	double Tfi[3][3];
	double Ttemp[3][3];
	double Tif[3][3];

	MatrixOperations<double>::multiply(mNutation[0], mPrecession[0], Ttemp[0],
			3, 3, 3);
	MatrixOperations<double>::multiply(mTheta[0], Ttemp[0], Tfi[0], 3, 3, 3);

	MatrixOperations<double>::transpose(Tfi[0], Tif[0], 3);

	MatrixOperations<double>::multiply(Tif[0], ecfCoordinates, eciCoordinates,
			3, 3, 1);

	if (ecfPositionIfCoordinatesAreVelocity != NULL) {

		double Tdotfi[3][3];
		double Tdotif[3][3];
		double Trot[3][3] = { { 0, Earth::OMEGA, 0 },
				{ 0 - Earth::OMEGA, 0, 0 }, { 0, 0, 0 } };
		double Ttemp2[3][3];

		MatrixOperations<double>::multiply(mNutation[0], mPrecession[0],
				Ttemp[0], 3, 3, 3);
		MatrixOperations<double>::multiply(mTheta[0], Ttemp[0], Ttemp2[0], 3, 3,
				3);

		MatrixOperations<double>::multiply(Trot[0], Ttemp2[0], Tdotfi[0], 3, 3,
				3);

		MatrixOperations<double>::transpose(Tdotfi[0], Tdotif[0], 3);

		double velocityCorrection[3];

		MatrixOperations<double>::multiply(Tdotif[0],
				ecfPositionIfCoordinatesAreVelocity, velocityCorrection, 3, 3,
				1);

		VectorOperations<double>::add(velocityCorrection, eciCoordinates,
				eciCoordinates, 3);
	}
}

CoordinateTransformations::CoordinateTransformations(uint8_t offset) :
		utcGpsOffset(offset) {

}

CoordinateTransformations::~CoordinateTransformations() {
}

void CoordinateTransformations::setUtcGpsOffset(uint8_t offset) {
}