/*
*  Program name:             polyc
*
*  Program function:         Polyconic map projection, reverse routine
*                            (lat, lon) ==>> (x, y)
*                            Spheroid = WGS84
*  Reference                 Map Projections - A Working Manual, USGS Prof. Paper, 1395
*
*  Programmed by:            Yasuaki MURATA,
*                            Geological Survey of Japan, Tsukuba
*                            Feb 28, 2014
*/

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

#define	Bessel_a	6377397.155
#define	Bessel_1f	299.152813

#define	GRS80_a		6378137.0
#define	GRS80_1f	298.257222101

#define	WGS84_a		6378137.0
#define	WGS84_1f	298.257223563

double   alpha, f, Sec2Rad, lon0, lat0, lon0r, lat0r ;
double   e2, e4, e6, da, db, dc, dd, M0 ;

main(argc, argv)
int     argc ;
char    *argv[] ;

{
	double  lat, lon, x, y ;
	void	polyconic_(), polyconic_r_() ;

// Initial Value
	lon0 = 136.0 * 3600.0 ;
	lat0 = 36.0 * 3600.0 ;
	alpha = WGS84_a ;
	f = 1.0 / WGS84_1f ;
	Sec2Rad = asin(1.0) / 90.0 / 3600.0 ;
	lon0r = lon0 * Sec2Rad ;
	lat0r = lat0 * Sec2Rad ;
	e2 = f * (2.0 - f) ;
	e4  = e2  * e2 ;
	e6  = e4  * e2 ;
	da = 1.0 - e2/4.0 - 3.0*e4/64.0 - 5.0*e6/256.0 ;
	db = 3.0*e2/8.0 + 3.0*e4/32.0 + 45.0*e6/1024.0 ;
	dc = 15.0*e4/256.0 + 45.0*e6/1024.0 ;
	dd = 35.0*e6/3072.0 ;
	M0 = alpha * (da*lat0r - db*sin(2.0*lat0r) + dc*sin(4.0*lat0r) - dd*sin(6.0*lat0r)) ;

// lat.lon -> x,y
	lat = 37.0 * 3600.0 ;
	lon = 137.0 * 3600.0 ;
	polyconic_(&lon, &lat, &x, &y) ;
	lon = lon / 3600.0 ;
	lat = lat / 3600.0 ;
	printf("lon, lat = %lf %lf -> x, y = %lf %lf\n", lon, lat, x, y) ;

// x,y -> lat,lon
	x = 100000.0 ;
	y = 100000.0 ;
	polyconic_r_(&lon, &lat, &x, &y) ;
	lon = lon / 3600.0 ;
	lat = lat / 3600.0 ;
	printf("x, y = %lf %lf -> lon, lat = %lf %lf\n", x, y, lon, lat) ;
}

/* ---- Polyconic Foreward ---- */
void	polyconic_(lon_s, lat_s, x, y)
double	*lon_s, *lat_s, *x, *y ;
{
	double		lat, lon, N, M, E ;

/* ---- x. y Calculation ---- */
	lat = (*lat_s) * Sec2Rad ;
	lon = (*lon_s) * Sec2Rad - lon0r ;
	M = alpha * (da*lat - db*sin(2.0*lat) + dc*sin(4.0*lat) - dd*sin(6.0*lat)) ;
	N = alpha / sqrt(1.0 - e2 * sin(lat) * sin(lat)) ;
	E = lon * sin(lat) ;
	*x = N / tan(lat) * sin(E) ;
	*y = M - M0 + N / tan(lat) * (1.0 - cos(E)) ;
}

/* ---- Polyconic Backward ---- */
void	polyconic_r_(lon, lat, x, y)
double	*lon, *lat, *x, *y ;
{
	double		A, B, C, Md, Mn, Ma, latr, latd ;
	int	n ;

/* ---- Iteration Loop  ---- */
	A = (M0 +(*y)) / alpha ;
	B = ((*x)*(*x)) / (alpha*alpha) + A*A ;
	latr = A ;
	for (n = 1; n < 20; n++) {
		C = sqrt(1.0 - e2*sin(latr)*sin(latr)) * tan(latr) ;
		Md = da - 2.0*db*cos(2.0*latr) + 4.0*dc*cos(4.0*latr) - 6.0*dd*cos(6.0*latr) ;
		Ma = da*latr - db*sin(2.0*latr) + dc*sin(4.0*latr) - dd*sin(6.0*latr) ;
		Mn = alpha * Ma ;
		latd = (A*(C*Ma+1.0) - Ma - 0.5*(Ma*Ma+B)*C) /
			(e2*sin(2.0*latr)*(Ma*Ma+B-2.0*A*Ma)/4.0*C + (A-Ma)*(C*Md - 2.0/sin(2.0*latr)) - Md) ;
		latr = latr - latd ;
		if (fabs(latd) < 1.0e-10) break ;
	}
	*lat = latr / Sec2Rad ;
	C = sqrt(1.0 - e2*sin(latr)*sin(latr)) * tan(latr) ;
	*lon = asin((*x)*C/alpha)/sin(latr) / Sec2Rad + lon0 ;
}