/*
  Converts latitude & longitude into easting, northing and zone for a Transverse
  Mercator projection - such as that used by the Map Grid of Australia (MGA).

  Equations are based on Redfearn's formulae. For more information, consult:
  "Geocentric Datum of Australia Technical Manual" published by the
  Intergovernmental Committee on Surveying & Mapping. 

  Written by Steven J. Merrifield, Copyright (c) July 2005
*/

#include <stdio.h>
#include <stdlib.h>
#include <termios.h>
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/signal.h>
#include <sys/types.h>
#include <string.h>
#include <errno.h>
#include <ctype.h>
#include <math.h>
#include "gps.h"

int zone;
char Estr[20];
char Nstr[20];
char GRStr[20];

double dec_deg(double deg, double min, double sec)
{
	return(fabs(deg) + fabs(min)/60.0 + fabs(sec)/3600.0);
}

double calc_rho(double lat)
{
	double temp1,temp2;
	temp1 = a * (1.0 - pow(e,2));
	temp2 = pow(1.0 - (pow(e,2) * pow(sin(lat),2)), 1.5);
	return(temp1/temp2);
}

double calc_nu(double lat)
{
	double temp1;
	temp1 = pow((1.0 - pow(e,2) * pow(sin(lat),2)), 0.5);
	return(a/temp1);
}

double calc_m(double lat)
{
	double A0,A2,A4,A6,ret;
	A0 = 1.0 - (pow(e,2)/4.0) - (3.0*pow(e,4)/64.0) - (5.0*pow(e,6)/256.0);
	A2 = 3.0/8.0 * (pow(e,2) + pow(e,4)/4.0 + 15.0*pow(e,6)/128.0);
	A4 = (15.0/256.0) * (pow(e,4) + 3.0*pow(e,6)/4.0);
	A6 = 35.0/3072.0*pow(e,6);
	ret = a*(A0*lat - A2*sin(2.0*lat) + A4*sin(4.0*lat) - A6*sin(6.0*lat));
	return(ret);
}

int grid_square(double lat, double lon)
{
	double lat_decimal,lon_decimal;
	double lat_radians,lon_radians;
	double rho,nu,psi;
	double omega;
	int cent_merid;
	double t;
	double Term1,Term2,Term3,Term4;
	double E,Edash;
	double N,Ndash;
	double m;

#if 0
	/* Buninyong test data */
	lat_deg = -37.0;
	lat_min = 39.0;
	lat_sec = 10.15610;

	lon_deg = 143.0;
	lon_min = 55.0;
	lon_sec = 35.38390;

	/* Flinders Peak test data */
	lat_deg = -37.0;
	lat_min = 57.0;
	lat_sec = 3.72030;

	lon_deg = 144.0;
	lon_min = 25.0;
	lon_sec = 29.52442;
#endif

	lat_decimal = lat;
	lon_decimal = lon;

	lat_radians = Pi * lat_decimal/180.0;
	lon_radians = Pi * lon_decimal/180.0;

	rho = calc_rho(lat_radians);

	nu = calc_nu(lat_radians);

	psi = nu/rho;

	zone = (int) ((lon_decimal - lon_west_edge) / zone_width);
	
	cent_merid = (int) (zone * zone_width + zero_cent_merid);

	omega = Pi * (lon_decimal- cent_merid) / 180.0;

	t = tan(lat_radians);

/* Easting calculations */

	Term1 = ( pow(omega,2)/6.0 * \
		  pow(cos(lat_radians),2) * \
		  (psi - (pow(t,2))) );

	Term2 = ( pow(omega,4)/120.0 * \
		  pow(cos(lat_radians),4) * \
		  (4.0*pow(psi,3)*(1.0-6.0*pow(t,2)) + \
		  pow(psi,2)*(1.0+8.0*pow(t,2)) - \
		  psi*2.0*pow(t,2) + pow(t,4)));

	Term3 = ( pow(omega,6)/5040.0 * \
		  pow(cos(lat_radians),6) * \
		  (61.0-479.0*pow(t,2)+179.0*pow(t,4)-pow(t,6)));

	Edash = K0 * nu * omega * cos(lat_radians) * (1.0+Term1+Term2+Term3);

	E = Edash + FalseE;

/* Northing calculations */

	Term1 = pow(omega,2)/2.0 * nu * sin(lat_radians) * cos(lat_radians);

	Term2 = ( pow(omega,4)/24.0 * nu * sin(lat_radians) * \
		  pow(cos(lat_radians),3) * (4.0*pow(psi,2) + psi-pow(t,2)));

	Term3 = ( pow(omega,6)/720.0 * nu * sin(lat_radians) * \
		  pow(cos(lat_radians),5) * (8.0*pow(psi,4)* \
		  (11.0 - 24.0*pow(t,2)) - 28.0*pow(psi,3)* \
		  (1.0 - 6.0*pow(t,2)) + (pow(psi,2)* \
		  (1.0 - 32.0*pow(t,2))) - psi*2.0*pow(t,2) + pow(t,4)));

	Term4 = ( pow(omega,8)/40320.0 * nu * sin(lat_radians) * \
		  pow(cos(lat_radians),7) * \
		  (1385.0 - 3111.0*pow(t,2)+543.0*pow(t,4) - pow(t,6)));

	m = calc_m(lat_radians);

	Ndash = K0 *(m + Term1 + Term2 + Term3 + Term4);
	N = Ndash + FalseN;
		  
	sprintf(Estr,"%07.0f",E);
	sprintf(Nstr,"%07.0f",N);
	sprintf(GRStr,"%c%c%c %c%c%c",Estr[2],Estr[3],Estr[4],Nstr[2],Nstr[3],Nstr[4]);

#if 0
	printf("Zone = %d\n",zone);
	printf("E = %07.0f\n",E);	
	printf("N = %07.0f\n",N);
	printf("GR %s\n",GRStr);
#endif
	return(0);
}