/********************************************************/
/* mercator.c This file contains basic mathmatics		*/
/* functions for coordinate conversion.					*/
/* Alexander Kolesnikov.				*/
/********************************************************/


#include "geo_def.h"

/*------------------------------------------------------*/

#include "mercator.h"

/*------------------------------------------------------*/

#include <math.h>
#include <stdio.h>

/*------------------------------------------------------*/

void ForwardConversion(TCoordinate *Coordinate, double lon_center,
	 TProjected *Projected)
{
  double east, north;
  double r_major, r_minor;
  double lat, lon;
  double temp, es, esp;
  double N, T, C, A;
  double A2, A3, es2, es3;
  double M;

  /* Get Ellipsoid parameters */
  r_major = Ellipsoid[Coordinate->Datum].Major;
  r_minor = Ellipsoid[Coordinate->Datum].Minor;
  temp    = r_minor/r_major;
  es      = 1 - temp*temp;
  esp     = es/(1.0 - es);
  lat     = Coordinate->Latitude;
  lon     = Coordinate->Longitude;

  /*Make sure the longitude is between -180.00 .. 179.9*/
  lon = (lon+180)- (int)((lon+180)/360)*360-180;
  lat = lat * D2R;
  lon = lon * D2R;
  lon_center = lon_center * D2R;

  N  = r_major / sqrt(1-es*sin(lat)*sin(lat));
  T  = tan(lat)*tan(lat);
  C  = esp*cos(lat)*cos(lat);
  A  = cos(lat)*(lon - lon_center);

  A2 = A*A;
  A3 = A*A2;
  es2= es*es;
  es3= es*es2;

  M  = r_major*( (1 - es/4 - 3*es2/64 - 5*es3/256)*lat
                 -(3*es/8 + 3*es2/32 + 45*es3/1024)*sin(2*lat)
                 +(15*es2/256 + 45*es3/1024)*sin(4*lat)
                 -(35*es3/3072)*sin(6*lat));

  east  = N*(A+(1-T+C)*A3/6 + (5.0-18.0*T+T*T+72*C-58*esp)*A2*A3/120);
  north = M + N*tan(lat)*(A2/2+(5-T+9*C+4*C*C)*A2*A2/24.0
             + (61.0-58.0*T+T*T+600.0*C-330.0*esp)*A3*A3/720.0);

  Projected->Northing   = north;
  Projected->Easting    = east;
  Projected->Datum      = Coordinate->Datum;
}

/*------------------------------------------------------*/

void InverseConversion(TProjected *Projected, double lat_origin,
					   double lon_center, double scale_fact,
					   TCoordinate *Coordinate)
{
  double lat, lon;
  double dEast, dNorth;
  long int max_iter;
  double r_major, r_minor;
  double con,phi,delta_phi;
  double sin_phi, cos_phi, tan_phi, c, cs, t, ts, n, r, d, ds, lon_tt;
  double ff,h,g, e, es, e0,e1,e2,e3,ml0,esp,temp, m10, dd;
  int ind, i;
  int Failure;

  max_iter = 8;             /* maximun number of iterations */
  m10 = 0;

  dEast  = Projected->Easting;
  dNorth = Projected->Northing;

  r_major = Ellipsoid[Projected->Datum].Major;
  r_minor = Ellipsoid[Projected->Datum].Minor;

  /* Degrees to radians */
  lat_origin = lat_origin*D2R;
  lon_center = lon_center*D2R;

  temp = r_minor / r_major;
  es = 1.0 - temp*temp;
  e  = sqrt(es);

  e0 = 1.0 - 0.25*es*(1.0 + es/16.0*(3.0 + 1.25*es));
  e1 = 0.375*es*(1.0 + 0.25*es*(1.0 + 0.46875*es));
  e2 = 0.05859375*es*es*(1.0 + 0.75*es);
  e3 = es*es*es*(35.0/3072.0);

  dd = e0*lat_origin-e1*sin(2*lat_origin)+e2*sin(4*lat_origin) - e3*sin(6*lat_origin);

  ml0 = r_major * dd;

  esp = es / (1.0 - es);

  if (es < 0.00001) ind = 1;	/* Spherical*/
  else              ind = 0;	/* Elliptical*/

  if(ind != 0)
  {                            /* Spherical form;*/
    ff   = exp( dEast/(r_major * scale_fact) );
    g    = 0.5 * (ff - 1/ff);
    temp = lat_origin + dNorth/(r_major * scale_fact);
    h    = cos(temp);
    con  = sqrt((1.0 - h*h)/(1.0 + g*g));
    lat  = asinz(con);
    if(temp < 0) lat = -lat;

    if (g == 0 && h == 0) lon = lon_center;
    else
    {
      lon_tt = atan2(g,h) + lon_center;
      lon    = (lon_tt+180) - (int)((lon_tt+180)/360)*360-180;
    }
  }
  else                             /* Ellipsoid form */
  {
    con = (m10 + dNorth/scale_fact) / r_major;
    phi = con;

    i = 0;
    Failure = 0;
    do
	{
      delta_phi = ((con + e1*sin(2.0*phi)-e2*sin(4.0*phi)+e3*sin(6.0*phi)) / e0) - phi;
      phi = phi + delta_phi;

      if (i >= max_iter) Failure=1;
      i++;
	}
    while (!(fabs(delta_phi) <= EPSLN || Failure));

    if (Failure)
    {
      printf("Latitude is failed to converge");
      return;
    }

    if(fabs(phi) < PI/2)
    {
      sin_phi = sin(phi);
      cos_phi = cos(phi);
      tan_phi = tan(phi);

      c   = esp * cos_phi*cos_phi;
      cs  = c*c;
      t   = tan_phi*tan_phi;
      ts  = t*t;
      con = 1.0 - es * sin_phi*sin_phi;
      n   = r_major / sqrt(con);
      r   = n * (1.0 - es) / con;
      d   = dEast / (n * scale_fact);
      ds  = d*d;

      lat = phi - (n*tan_phi*ds/r)*(0.5-ds/24.0* (5.0+3.0*t+10.0*c-4.0*cs -
                    9.0*esp-ds/30.0*(61.0+90.0*t+298.0*c+45.0*ts-252.0*esp-3.0*cs)));

      lon_tt = lon_center+(d*(1.0-ds/6.0*(1.0+2.0*t+
                 c-ds/20.0*(5.0-2.0*c+28.0*t-3.0*cs+8.0*esp+24.0*ts)))/cos_phi);

      lon    = (lon_tt+180) - (int)((lon_tt+180)/360)*360-180;
    }
    else
    {
      lat = PI/2 * signum(dNorth);
      lon = lon_center;
    }
  }                             /* ind==0 */

  lat = lat * R2D;
  lon = lon * R2D;

  Coordinate->Latitude  = lat;
  Coordinate->Longitude = lon;
  Coordinate->Datum     = Projected->Datum;
}

/*------------------------------------------------------*/

double asinz(double con)
{
  if (fabs(con) > 1.0)
  {
    if (con > 1.0) con =  1.0; else con = -1.0;
  }
  return asin(con);
}

/*------------------------------------------------------*/

int signum(double arg)
{
  if (arg>0) return 1;
  if (arg<0) return -1;
  return 0;
}