#include "inc/llutm.h"
#include "inc/constants.h"
#include <math.h>


//Esta funcao recebe como entrada dois reais, especificando Latitude e Longitude, nesta ordem
//e retorna o meridiano origem e as respectivas coordenas EUTM (EUTM - Especial UTM) Northing e Easting.
//O sistema EUTM usa o mesmo principio que as coordenas UTM, com a exceção da origem dos fusos. O fuso original
//pode variar de acordo com o conjunto de coordenadas LatLong fornecidas, de forma que todas elas caiam dentro de
//um mesmo fuso

//Northings sao positivos no norte e negativas no sul
//Easting sao positivas no leste e negativas no oeste

//Se a diferenca entre a longitude maxima e longitude minima for superior a 6 grau, o programa retorna 0
//indicando que nao será possível fazer a conversao
//Caso contrario, ele retornará o codigo 1




//Converte LatLong para coordenadas EUTM. Equaçoes provenientes do USGS Bulletin 1532
//Longitudes Leste são positivas, Longitudes Oeste são negativas.
//Latitudes Norte são positivas. Latitudes Sul são negativas
//Lat e Long são dadas em graus decimais
unsigned int LLtoEUTM(double Lat, double Long, double& dx, double& dy, double LongOrigin)
{

	unsigned int ReferenceEllipsoid;
	double LatRad;
	double eccSquared;
	double eccPrimeSquared;
	double k0;
	double a;
	double N;
	double T;
	double C;
	double A;
	double M;

	ReferenceEllipsoid = 19; //SAD-69

	a = ellipsoid[ReferenceEllipsoid].EquatorialRadius;
	eccSquared = ellipsoid[ReferenceEllipsoid].eccentricitySquared;
	k0 = 0.9996;

	LatRad = Lat*deg2rad;

	eccPrimeSquared = (eccSquared)/(1-eccSquared);
	N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad));
	T = tan(LatRad)*tan(LatRad);
	C = eccPrimeSquared*cos(LatRad)*cos(LatRad);
	A = cos(LatRad)*(Long-LongOrigin)*deg2rad;

	M = a*((1	- eccSquared/4		- 3*eccSquared*eccSquared/64	- 5*eccSquared*eccSquared*eccSquared/256)*LatRad
		- (3*eccSquared/8	+ 3*eccSquared*eccSquared/32	+ 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad)
		+ (15*eccSquared*eccSquared/256 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad)
		- (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad));

	dx = (double)(k0*N*(A+(1-T+C)*A*A*A/6
		+ (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120));

	dy = (double)(k0*(M+N*tan(LatRad)*(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24
		+ (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720)));


	return 1;
}



//converte coordenadas EUTM para coordenadas lat/long. Equaçoes provenientes do USGS Bulletin 1532
//Longitudes Leste são positivas, Longitudes Oeste são negativas.
//Latitudes Norte são positivas. Latitudes Sul são negativas
//Lat e Long são dadas em graus decimais
void  EUTMtoLL(double dx, double dy, double& Lat, double& Long,  double LongOrigin)
{

	unsigned int ReferenceEllipsoid;
	double k0;
	double a;
	double eccSquared;
	double eccPrimeSquared;
	double e1;
	double N1;
	double T1;
	double C1;
	double R1;
	double  D;
	double  M;
	double mu;
	double phi1;
	double phi1Rad;


	ReferenceEllipsoid = 19; //SAD-69
	k0 = 0.9996;
	a = ellipsoid[ReferenceEllipsoid].EquatorialRadius;
	eccSquared = ellipsoid[ReferenceEllipsoid].eccentricitySquared;
	e1 = (1-sqrt(1-eccSquared))/(1+sqrt(1-eccSquared));


	eccPrimeSquared = (eccSquared)/(1-eccSquared);
	M = dy / k0;
	mu = M/(a*(1-eccSquared/4-3*eccSquared*eccSquared/64-5*eccSquared*eccSquared*eccSquared/256));

	phi1Rad = mu	+ (3*e1/2-27*e1*e1*e1/32)*sin(2*mu)
		+ (21*e1*e1/16-55*e1*e1*e1*e1/32)*sin(4*mu)
		+(151*e1*e1*e1/96)*sin(6*mu);
	phi1 = phi1Rad*rad2deg;

	N1 = a/sqrt(1-eccSquared*sin(phi1Rad)*sin(phi1Rad));
	T1 = tan(phi1Rad)*tan(phi1Rad);
	C1 = eccPrimeSquared*cos(phi1Rad)*cos(phi1Rad);
	R1 = a*(1-eccSquared)/pow(1-eccSquared*sin(phi1Rad)*sin(phi1Rad), 1.5);
	D = dx/(N1*k0);

	Lat = phi1Rad - (N1*tan(phi1Rad)/R1)*(D*D/2-(5+3*T1+10*C1-4*C1*C1-9*eccPrimeSquared)*D*D*D*D/24
		+(61+90*T1+298*C1+45*T1*T1-252*eccPrimeSquared-3*C1*C1)*D*D*D*D*D*D/720);
	Lat = Lat * rad2deg;

	Long = (D-(1+2*T1+C1)*D*D*D/6+(5-2*C1+28*T1-3*C1*C1+8*eccPrimeSquared+24*T1*T1)
		*D*D*D*D*D/120)/cos(phi1Rad);
	Long = LongOrigin + Long*rad2deg;

}