====================================================
c
c	xgconv: test driver for "gconv"
c
c      Reference: T. Fukushima (1999) J. Geodesy, 73, 603-610.
c	Author: Toshio Fukushima
c       Affil.: National Astronomical Observatory of Japan
c       Addr.: 2-21-1, Ohsawa, Mitaka, Tokyo 181-8588, Japan
c       E-mail: Toshio.Fukushima@nao.ac.jp
c       Phone: +81-422-34-3613
c       Fax: +81-422-34-3793
c
	integer ilam,ip,ih
	real*8 PI,raddeg
	real*8 a,finv,f,e2
	real*8 lambda,slam,clam
	real*8 phi,sphi,cphi
	real*8 n,height,rho,rhoz
	real*8 r,x,y,z,p,lam,h
	real*8 sp,cp,np,rhop,rhozp,rp,zp
c
	PI=3.141592653589793238462d0
      	raddeg=PI/180.d0
c
c	GRS1980 System
c
	a=6378137.d0
	finv=298.257222101d0
	f=1.d0/finv
	e2=(2.d0-f)*f
c
c	Title
c
	write(*,'(1x,2a15,2a10)') "Latitude(deg)","Height(m)",
      &		"dLat(deg)","dH(m)"
c
c	We fixed the longitude as 45 degree for it is trivial to solve w.r.t. lambda
c
	ilam=45
	lambda=ilam*raddeg
	slam=sin(lambda)
	clam=cos(lambda)
c
c	Loop for Latitude (You may increase the number of test data set)
c
	do ip=-1,10
		phi=ip*10
c
c	Include Near-Equator/Polar Cases c
		if(phi.lt.0.d0) then
			phi=0.d0
		elseif(phi.gt.90.d0) then
			phi=90.d0
		else
			phi=min(89.999999999d0,max(phi,0.000000001d0))
		endif
c
		phi=phi*raddeg
		sphi=sin(phi)
		cphi=cos(phi)
		n=a/sqrt(1.d0-e2*sphi*sphi)
c
c	Loop for Height (You may increase the number of test data set)
c
		do ih=0,10
			height=-10000.d0+ih*2000.d0
c
c	Compute (x,y,z)
c
			rho=n+height
			rhoz=(1.d0-e2)*n+height
			if(rho.le.0.d0.or.rhoz.le.0.d0) goto 8
			r=rho*cphi
			x=r*clam
			y=r*slam
			z=rhoz*sphi
c
			call gconv(a,finv,x,y,z,p,lam,h)
c
c	Check the residuals
c
			sp=sin(p)
			cp=cos(p)
			np=a/sqrt(1.d0-e2*sp*sp)
			rhop=np+h
			rhozp=(1.d0-e2)*np+h
			rp=rhop*cp
			zp=rhozp*sp
			dphi=phi-p
			dh=height-h
	write(*,'(1x,0pf15.10,0pf15.5,1p2e10.1)')
      &		phi/raddeg,height,dphi/raddeg,dh
     8			continue
     		enddo
	enddo
	stop
	end


====================================================
	subroutine gconv (a,finv,x,y,z,phi,lambda,h)
c
c	Program to Convert Geocentric Rectangular to Geodetic Coordinates
c
c	Date: 1999 Nov. 10
c
c	Author: Toshio Fukushima
c
c	Institution: National Astronomical Observatory of Japan (NAOJ)
c     Addrezprime: 2-21-1, Ohsawa, Mitaka, Tokyo 181-8588, Japan
c	Phone: +81-422-34-3613
c	FAX: +81-422-34-3793
c	E-mail: Toshio.Fukushima@nao.ac.jp
c
c	Ref: T. Fukushima (1999, J. Geodesy, in printing)
c
c	Input Paramaters: a,finv
c
c	Input Variables: x,y,z
c
c	Output Variables: phi,lambda,h
c
	real*8 a,finv,x,y,z,phi,lambda,h
c
	real*8 A1916,ainv,fiold,f,e2,ep,r
	real*8 fiold2,fold,fold2,e2old,e2old2,epold,epold2
	real*8 p,absz,zprime,u,v,t0
	real*8 tau,tau2,tau3,tau4,dtau
	real*8 tau21,tau21m,tauep2,p4,p2,u3,u2,fx,fp,dp
	logical first
c
	data first/.TRUE./
	if(first) then
		first=.FALSE.
		A1916=19.d0/16.d0
c
c	Default Values: GRS1980 System
c
		a=6378137.d0
		finv=298.257222101d0
		f=1.d0/finv
		e2=(2.d0-f)*f
		ep=sqrt(1.d0-e2)
c
c	Store 2 stacks
c
		fiold=finv
		fold=f
		e2old=e2
		epold=ep
		fiold2=finv
		fold2=f
		e2old2=e2
		epold2=ep
	endif
c
c	Care for Invalid  Earth Parameters
c
	if(a.le.0.d0) then
		a=6378137.d0
	endif
	ainv=1.d0/a
c
c	Retrieve Earth Parameters from 2 stacks: "old" and "old2"
c
	if(abs(finv-fiold).le.1.d-14) then
		f=fold
		e2=e2old
		ep=epold
	elseif(abs(finv-fiold2).le.1.d-14) then
		f=fold2
		e2=e2old2
		ep=epold2
	elseif(finv.gt.0.d0) then
		fiold2=fiold
		fold2=fold
		e2old2=e2old
		epold2=epold
		fiold=finv
		f=1.d0/finv
		e2=(2.d0-f)*f
		ep=sqrt(1.d0-e2)
		fold=f
		e2old=e2
		epold=ep
	else
		fiold2=fiold
		fold2=fold
		e2old2=e2old
		epold2=epold
		finv=298.257d0
		fiold=finv
		f=1.d0/finv
		e2=(2.d0-f)*f
		ep=sqrt(1.d0-e2)
		fold=f
		e2old=e2
		epold=ep
	endif
c	write(*,'(1x,a15,4f30.15) ') "a,f,e^2,ep=", a,f,e2,ep
c
c	Derive Longitude	
c
	lambda=atan2(y,x)
c
c	Distance from Polar Axis: r
c
	r=sqrt(x*x+y*y)
c
c	Compute Coefficients of (Modified) Quartic Equation
c
c	Remark: Coefficients are rescaled by dividing by 'a'
c
	p=r*ainv
	absz=abs(z)*ainv
	zprime=ep*absz
	u=2.d0*(zprime-e2)
	v=2.d0*(zprime+e2)
	p4=4.d0*p
	p2=2.d0*p
	u3=3.d0*u
	u2=2.d0*u
c
c	Select Suitable Starter
c
	if(p.lt.1.d-16) then
c
c	Case 0: p = 0
c
		t0=0.d0
	elseif(u.ge.0.d0) then
c
c	Case 1: u >= 0 (equiv. with t_M=(-u)/(2*p) <= 0)
c
		t0=(p4+u2)/(p4+u3+v)
	else
c
c	Case u < 0
c
		if((-u).gt.p2) then
c
c	Case 2: t_M > 1
c
			t0=p/v
		else
c
c	Case 3: 0 < t_M <= 1
c
			tm=-u/p2
			fm=tm*(tm*tm*(tm*p+u)+v)-p
			if(fm.gt.0.d0) then
c
c	Case 3a: f(t_M) > 0
c
				t0=p/v
			else
c
c	Case 3b: f(t_M) <= 0
c
				t0=(p4+u2)/(p4+u3+v)
			endif
		endif
	endif
c
c	Newton Method
c
	tau=t0
	do iter=1,20
		tau2=tau*tau
		tau3=tau2*tau
		tau4=tau2*tau2
		fx=p*tau4+u*tau3+v*tau-p
		if(abs(fx).lt.1.e-15) goto 2
		fp=p4*tau3+u3*tau2+v
		dtau=-fx/fp
		tau=tau+dtau
		if(abs(dtau).lt.1.e-8) goto 1
	enddo
c
c	Converged
c
     1 continue
		tau2=tau*tau
     2	continue
		tau21=1.d0+tau2
		tau21m=1.d0-tau2
		tauep2=2.d0*ep*tau
c
c	After Care
c
		phi=atan2(tau21m,tauep2)
	if(z.lt.0.d0) then
		phi=-phi
	endif
	dp=sqrt(tau21*tau21-4.d0*e2*tau2)
	h=a*(p*tauep2+absz*tau21m-ep*tau21)/dp
	return
	end
