module Sph_utils

   implicit none

   private
   public :: ArcsOvrlap, Pnt_in_Arc
   public :: Distance, PntsEqual, ArcsEqual, cartsEqual
   public :: ArcMidpnt, cross_prod, geod2cart, cart2geod
   public :: MatSlv, GCXsect, CommonGC, getS, getXYZ, sTriArea
   public :: cartPnt, geodeticPnt, arc, spolygon

   real, parameter :: ZERO = 0.
   real, parameter :: ONEHALF = .5
   real, parameter :: ONE  = 1.
   real, parameter :: FOUR = 4.
   real, parameter :: FIVE = 5.
   real, parameter :: TEN  = 10.
   real, parameter :: NINETY = 90.
   real, parameter :: ONE80  = 180.
   real, parameter :: NOTSET = -1000.
   real, parameter :: PI  = 3.141592653589793
   real, parameter :: D2R = PI/ONE80
   real, parameter :: R2D = ONE80/PI

   real      :: SMALL    = FIVE*tiny(SMALL)
   real      :: ROUNDOFF = TEN*epsilon(ROUNDOFF)

!---------------------------------------------------
!  define the types
!---------------------------------------------------
   type cartPnt
     real :: x, y, z
   end type cartPnt

   type geodeticPnt
     real :: lon
     real :: lat
   end type geodeticPnt

   type geodeticPnt_8
     real(8) :: lon
     real(8) :: lat
   end type geodeticPnt_8

   type arc
     type(geodeticPnt) :: sPnt
     type(cartPnt)     :: sxyz
     type(geodeticPnt) :: ePnt
     type(cartPnt)     :: exyz
     real              :: Angle = NOTSET
     real              :: Normal(3) = (/ ZERO, ZERO, ZERO /)
     logical           :: RHS = .true.
     logical           :: IsLatArc = .false.
   end type arc

   type spolygon
     type(arc), allocatable :: arcs(:)
   end type spolygon

   interface geod2cart
     module procedure ftngeod2cart, subgeod2cart, typgeod2cart, ftntypgeod2cart
   end interface

   interface cart2geod
     module procedure ftncart2geod, subcart2geod
   end interface

   interface cartNorm
     module procedure ftncartNorm, typcartNorm
   end interface

   interface cross_prod
     module procedure cross_prod_xyz, cross_prod_typ
   end interface

   CONTAINS

   function ArcMidpnt( Arc1 ) result(midpnt)
!--------------------------------------------------------
!  get arc midpoint (lon,lat)
!--------------------------------------------------------

   type(arc), intent(inout)       :: Arc1
   type(geodeticPnt), intent(out) :: midpnt

   real :: dist, dist2
   real :: wght
   real :: x, y, z
   real :: Pnt(2)

   dist = Distance( Arc1%sPnt,Arc1%ePnt )
   dist2 = .5*dist

   Arc1%sxyz = geod2cart( Arc1%sPnt )
   Arc1%exyz = geod2cart( Arc1%ePnt )
   wght = sin(dist2)/sin(dist)
   x    = wght*(Arc1%sxyz%x + Arc1%exyz%x)
   y    = wght*(Arc1%sxyz%y + Arc1%exyz%y)
   z    = wght*(Arc1%sxyz%z + Arc1%exyz%z)
   Pnt(:) = cart2geod( x, y, z )
   midpnt%lon = R2D*Pnt(1)
   midpnt%lat = R2D*Pnt(2)
   
   end function ArcMidpnt

   function ArcsOvrlap( Arc1, Arc2 ) result(Ovrlap)
!--------------------------------------------------------
!  Do arcs overlap?
!--------------------------------------------------------

!--------------------------------------------------------
!  dummy arguments
!--------------------------------------------------------
   type(arc), intent(in) :: Arc1, Arc2
   logical, intent(out)  :: Ovrlap

   Ovrlap = ArcsEqual( Arc1, Arc2 )
   if( .not. Ovrlap ) then
     Ovrlap = Pnt_in_Arc( Arc1,Arc2%sPnt )
     if( .not. Ovrlap ) then
       Ovrlap = Pnt_in_Arc( Arc1,Arc2%ePnt )
       if( .not. Ovrlap ) then
         Ovrlap = Pnt_in_Arc( Arc2,Arc1%sPnt )
         if( .not. Ovrlap ) then
           Ovrlap = Pnt_in_Arc( Arc2,Arc1%ePnt )
         endif
       endif
     endif
   endif

   end function ArcsOvrlap

   function CommonGC( Arc1, Arc2 ) result(sameGC)
!--------------------------------------------------------
!  Are arcs in same GC?
!--------------------------------------------------------

!--------------------------------------------------------
!  dummy arguments
!--------------------------------------------------------
   type(arc), intent(in) :: Arc1, Arc2
   logical, intent(out)  :: sameGC

!--------------------------------------------------------
!  local variables
!--------------------------------------------------------
   integer :: n
   real :: XYZ(3,2)
   real :: Norm(3,2)

   XYZ(:,1) = geod2cart( Arc1%sPnt%lon,Arc1%sPnt%lat )
   XYZ(:,2) = geod2cart( Arc1%ePnt%lon,Arc1%ePnt%lat )
!  call cross_prod( XYZ(:,1),XYZ(:,2), Norm(:,1) )
   Norm(:,1) = cross_prod( XYZ(:,1),XYZ(:,2) )
   Norm(:,1) = cartNorm( Norm(:,1) )

   XYZ(:,1) = geod2cart( Arc2%sPnt%lon,Arc2%sPnt%lat )
   XYZ(:,2) = geod2cart( Arc2%ePnt%lon,Arc2%ePnt%lat )
!  call cross_prod( XYZ(:,1),XYZ(:,2), Norm(:,2) )
   Norm(:,2) = cross_prod( XYZ(:,1),XYZ(:,2) )
   Norm(:,2) = cartNorm( Norm(:,2) )

   do n = 1,3
     sameGC = PntsEqual( norm(n,1),norm(n,2) )
     if( .not. sameGC ) then
       exit
     endif
   enddo

   end function CommonGC

   function Pnt_in_Arc( Arc_in, Pnt ) result(InArc)
!--------------------------------------------------------
!  Is point in arc?
!--------------------------------------------------------

!--------------------------------------------------------
!  dummy arguments
!--------------------------------------------------------
   type(arc), intent(in)         :: Arc_in
   type(geodeticPnt), intent(in) :: Pnt
   logical, intent(out)          :: InArc

!--------------------------------------------------------
!  local variables
!--------------------------------------------------------
   type(cartPnt    ) :: tstcart, chkcart
   type(arc)         :: theArc
   real              :: tstDist
   real              :: arcDist
   real              :: factor1, factor2
   real              :: Cart(3,3)
   real              :: X(3,2)
   logical           :: onGC

!--------------------------------------------------------
!  Does Pnt match either beg or end pnt of arc?
!--------------------------------------------------------
   theArc = Arc_in
   InArc = PntsEqual( theArc%sPnt%lon,Pnt%lon ) .and. &
                PntsEqual( theArc%sPnt%lat,Pnt%lat )
   if( .not. InArc ) then
     InArc = PntsEqual( theArc%ePnt%lon,Pnt%lon ) .and. &
             PntsEqual( theArc%ePnt%lat,Pnt%lat )
   endif

fullTest: &
   if( .not. InArc ) then
     arcDist = Distance( theArc%sPnt,theArc%ePnt )
     tstDist = Distance( theArc%sPnt,Pnt )
     InArc = .not. (tstDist > arcDist)
     if( InArc ) then
       call geod2cart( theArc%sPnt, theArc%sxyz )
       call geod2cart( theArc%ePnt, theArc%exyz )
       call geod2cart( Pnt, tstcart )
       factor1 = sin(arcDist-tstDist)/sin(arcDist)
       factor2 = sin(tstDist)/sin(arcDist)
       chkcart%x = factor1*theArc%sxyz%x + factor2*theArc%exyz%x 
       chkcart%y = factor1*theArc%sxyz%y + factor2*theArc%exyz%y 
       chkcart%z = factor1*theArc%sxyz%z + factor2*theArc%exyz%z 
       InArc = .false.
       if( PntsEQual( chkcart%x,tstcart%x ) ) then
         if( PntsEQual( chkcart%y,tstcart%y ) ) then
           if( PntsEQual( chkcart%z,tstcart%z ) ) then
             InArc = .true.
           endif
         endif
       endif
     endif
   endif fullTest

   end function Pnt_in_Arc

   function Distance( pnt1, pnt2 ) result(Angle)
!--------------------------------------------------------
!  Get distance between to geodetic pnts on unit sphere
!--------------------------------------------------------

   implicit none

   type(geodeticPnt), intent(in) :: pnt1
   type(geodeticPnt), intent(in) :: pnt2
   real, intent(out)             :: Angle

   real(8), parameter :: ONE = 1._8

   real(8) :: dprod
   real(8) :: X(3,2)

   X(:,1) = real(geod2cart( pnt1%lon,pnt1%lat ),8)
   X(:,2) = real(geod2cart( pnt2%lon,pnt2%lat ),8)

   dprod = dot_product(X(:,1),X(:,2))
   Angle = acos( max( min(ONE,dprod),-ONE ) )
   if( abs(Angle) < SMALL ) then
     write(*,'(''Distance '',1pg15.7,'' is too small'')') Angle
     Stop 'Distance-ERR'
   endif

   end function Distance

   function cross_prod_xyz( A, B )  result(C)
   
   implicit none

   real, intent(in)  :: A(3), B(3)
   real, intent(out) :: C(3)

   C(1) = A(2)*B(3) - A(3)*B(2)
   C(2) = A(3)*B(1) - A(1)*B(3)
   C(3) = A(1)*B(2) - A(2)*B(1)

   end function cross_prod_xyz

   function cross_prod_typ( theArc ) result(C)
   
   implicit none

   type(arc), intent(inout) :: theArc
   real, intent(out) :: C(3)

   theArc%sxyz = geod2cart( theArc%sPnt )
   theArc%exyz = geod2cart( theArc%ePnt )

   C(1) = theArc%sxyz%y*theArc%exyz%z - theArc%sxyz%z*theArc%exyz%y
   C(2) = theArc%sxyz%z*theArc%exyz%x - theArc%sxyz%x*theArc%exyz%z
   C(3) = theArc%sxyz%x*theArc%exyz%y - theArc%sxyz%y*theArc%exyz%x

   end function cross_prod_typ

   function ftngeod2cart( lon, lat ) result( Cart )
!---------------------------------------------------
!  map geodetic (lambda,phi) -> cartesian (x,y,z)
!---------------------------------------------------
   
   implicit none

   real, intent(in)  :: lon, lat
   real, intent(out) :: Cart(3)

   real :: tmp
   real :: rlon, rlat
   real :: x, y, z

   rlon = D2R*lon ; rlat = D2R*lat
   tmp = cos(rlat)
   z   = sin(rlat)
   x   = tmp*sin(rlon)
   y   = tmp*cos(rlon)
   Cart(:) = (/ x,y,z /)

   end function ftngeod2cart

   subroutine subgeod2cart( lon, lat, x, y, z )
!---------------------------------------------------
!  map geodetic (lambda,phi) -> cartesian (x,y,z)
!---------------------------------------------------
   
   implicit none

   real, intent(in)  :: lon, lat
   real, intent(out) :: x, y, z

   real :: tmp
   real :: rlon, rlat

   rlon = D2R*lon ; rlat = D2R*lat
   tmp = cos(rlat)
   z   = sin(rlat)
   x   = tmp*sin(rlon)
   y   = tmp*cos(rlon)

   end subroutine subgeod2cart

   subroutine typgeod2cart( geodPnt, xyzPnt )
!---------------------------------------------------
!  map geodetic (lambda,phi) -> cartesian (x,y,z)
!---------------------------------------------------
   
   implicit none

   type(geodeticPnt), intent(in)  :: geodPnt
   type(cartPnt), intent(out)     :: xyzPnt

   real :: tmp
   real :: rlon, rlat

   rlon = D2R*geodPnt%lon ; rlat = D2R*geodPnt%lat
   tmp = cos(rlat)
   xyzPnt%z   = sin(rlat)
   xyzPnt%x   = tmp*sin(rlon)
   xyzPnt%y   = tmp*cos(rlon)

   end subroutine typgeod2cart

   function ftntypgeod2cart( geodPnt ) result(xyzPnt)
!---------------------------------------------------
!  map geodetic (lambda,phi) -> cartesian (x,y,z)
!---------------------------------------------------
   
   implicit none

   type(geodeticPnt), intent(in)  :: geodPnt
   type(cartPnt), intent(out)     :: xyzPnt

   real :: tmp
   real :: rlon, rlat

   rlon = D2R*geodPnt%lon ; rlat = D2R*geodPnt%lat
   tmp = cos(rlat)
   xyzPnt%z   = sin(rlat)
   xyzPnt%x   = tmp*sin(rlon)
   xyzPnt%y   = tmp*cos(rlon)

   end function ftntypgeod2cart

   function ftncart2geod( x, y, z ) result( Pnt )
   
   implicit none

   real, intent(in)  :: x, y, z
   real, intent(out) :: Pnt(2)

   real :: tmp
   real :: lon, lat

   lat = asin( z )
   if( y == ZERO ) then
     lon = NINETY
   else
     tmp = atan(x/y)
     if( y > ZERO ) then
       lon = tmp
     elseif( y < ZERO ) then
       lon = tmp + PI
     endif
   endif

   Pnt(:) = (/ lon,lat /)

   end function ftncart2geod

   subroutine subcart2geod( x, y, z, lon, lat )
   
   implicit none

   real, intent(in)  :: x, y, z
   real, intent(out) :: lon, lat

   real :: tmp

   lat = asin( z )
   if( y == ZERO ) then
     lon = NINETY
   else
     tmp = x/y
     tmp = atan(tmp)
     if( y > ZERO ) then
       lon = tmp
     elseif( y < ZERO ) then
       lon = tmp + PI
     endif
   endif

   end subroutine subcart2geod

   function ArcsEqual( arc1, arc2 ) result( Equal )
!---------------------------------------------------
!  check if arcs are identical
!---------------------------------------------------
   type(arc), intent(in) :: arc1, arc2
   logical, intent(out)  :: Equal

   Equal = PntsEqual( arc1%sPnt%lon,arc2%sPnt%lon )
   if( Equal ) then
     Equal = PntsEqual( arc1%sPnt%lat,arc2%sPnt%lat )
   endif

   end function ArcsEqual

   function cartsEqual( P1, P2 ) result( Equal )
!---------------------------------------------------
!  check if arcs are identical
!---------------------------------------------------
   real, intent(in)      :: P1(3), P2(3)
   logical, intent(out)  :: Equal

   integer :: n

   do n = 1,3
     Equal = PntsEqual( P1(n),P2(n) )
     if( .not. Equal ) then
       exit
     endif
   enddo

   end function cartsEqual

   function PntsEqual( pnt1, pnt2 ) result( Equal )
!---------------------------------------------------
!  check if arcs are identical
!---------------------------------------------------
   real, intent(in) :: pnt1, pnt2
   logical, intent(out)  :: Equal

   real :: tmp

!  PntsEqual = abs(pnt2 - pnt1) <= ROUNDOFF
   if( pnt1 /= ZERO .and. pnt2 /= ZERO ) then
     Equal = abs(pnt2 - pnt1) <= ROUNDOFF*max( abs(pnt1),abs(pnt2) )
   else
     tmp = max(abs(pnt1),abs(pnt2))
     Equal = tmp <= ROUNDOFF
   endif

   end function PntsEqual

   function ftncartNorm( x, y, z ) result( Norm )
!---------------------------------------------------
!  Form normed 3d vector
!---------------------------------------------------

   real, intent(in) :: x, y, z
   real, intent(out) :: Norm(3)

   real :: Factor

   Factor = sqrt( x*x + y*y + z*z )
   Norm(:) = (/ x/Factor, y/Factor, z/Factor /)

   end function ftncartNorm

   function typcartNorm( X ) result( Norm )
!---------------------------------------------------
!  Form normed 3d vector
!---------------------------------------------------

   real, intent(in)  :: X(3)
   real, intent(out) :: Norm(3)

   real :: Factor

   Factor = sqrt( sum(X(:)*X(:)) )
   Norm(:) = X(:)/Factor

   end function typcartNorm

   function MatSlv( A, b ) result(x)
!---------------------------------------------------
!  Solve 2x2 linear system
!---------------------------------------------------

   real, intent(in)  :: A(2,2)
   real, intent(in)  :: b(2)
   real, intent(out) :: x(2)
   
   real :: det
   real :: cofac(2,2)

   det = A(1,1)*A(2,2) - A(2,1)*A(1,2) 
   if( Det == ZERO ) then
     write(*,*) 'MatSlv: zero determinant'
     Stop 'Singular matrix'
   endif
   
   cofac(1,1) = A(2,2)  ; cofac(2,2) = A(1,1)
   cofac(1,2) = -A(1,2) ; cofac(2,1) = -A(2,1)

   x(:) = matmul( cofac,b )
   x(:) = x(:)/det
   
   end function MatSlv

   subroutine GCXsect( ArcsIn, geodXsect, cartXsect )
!---------------------------------------------------
!  get great cirle intersection
!---------------------------------------------------

!---------------------------------------------------
!  dummy arguments
!---------------------------------------------------
   type(arc), intent(in)                    :: ArcsIn(2)
   type(geodeticPnt), intent(out), optional :: geodXsect(2)
   type(cartPnt), intent(inout), optional   :: cartXsect(2)

!---------------------------------------------------
!  local variables
!---------------------------------------------------
   real(8), parameter :: ONEHALF = .5_8
   integer   :: n
   real      :: lon, lat
   real(8)   :: nFactor, Mag
   real      :: gP(2,2)
   real      :: E1(3,2), E2(3,2)
   real      :: X(3,2)
   real      :: XYZ(3)
   real(8)   :: dlambda(2), dphi(2)
   real(8)   :: dlambdad2(2), dphid2(2)
   real(8)   :: mlambdax2(2), mphix2(2)
   real(8)   :: mlambda(2), mphi(2)
   real(8)   :: Normals(3,2)
   real(8)   :: NormalxNormal(3)
   type(arc) :: Arcs(2)

   Arcs(:) = ArcsIn(:)
!---------------------------------------------------
!  degrees -> radians
!---------------------------------------------------
!  Arcs(:)%sPnt%lon = D2R*Arcs(:)%sPnt%lon
!  Arcs(:)%ePnt%lon = D2R*Arcs(:)%ePnt%lon
!  Arcs(:)%sPnt%lat = D2R*Arcs(:)%sPnt%lat
!  Arcs(:)%ePnt%lat = D2R*Arcs(:)%ePnt%lat

!---------------------------------------------------
!  arc1
!---------------------------------------------------
   E1(:,1) = geod2cart( Arcs(1)%sPnt%lon,Arcs(1)%sPnt%lat )
   E1(:,2) = geod2cart( Arcs(1)%ePnt%lon,Arcs(1)%ePnt%lat )

!---------------------------------------------------
!  arc2
!---------------------------------------------------
   E2(:,1) = geod2cart( Arcs(2)%sPnt%lon,Arcs(2)%sPnt%lat )
   E2(:,2) = geod2cart( Arcs(2)%ePnt%lon,Arcs(2)%ePnt%lat )

!---------------------------------------------------
!  cross for arc1, arc2; then normalize
!---------------------------------------------------
!  call cross_prod( E1(:,1), E1(:,2), X(:,1) )
!  call cross_prod( E2(:,1), E2(:,2), X(:,2) )
   X(:,1) = cross_prod( E1(:,1), E1(:,2) )
   X(:,2) = cross_prod( E2(:,1), E2(:,2) )

   Mag = sqrt( sum(X(:,1)*X(:,1)) )
   X(:,1) = X(:,1)/Mag
   Mag = sqrt( sum(X(:,1)*X(:,1)) )

   Mag = sqrt( sum(X(:,2)*X(:,2)) )
   X(:,2) = X(:,2)/Mag
   Mag = sqrt( sum(X(:,2)*X(:,2)) )

!  call cross_prod( X(:,1),X(:,2),XYZ )
   XYZ(:) = cross_prod( X(:,1),X(:,2) )

   gP(2,1) = R2D*atan2( XYZ(3),sqrt(XYZ(1)*XYZ(1)+XYZ(2)*XYZ(2)) )
   gP(1,1) = R2D*atan2( XYZ(1),XYZ(2) )
   gP(1,1) = mod( gp(1,1),360. )
   if( present(geodXsect) ) then
     geodXsect(1)%lon = gP(1,1) ; geodXsect(1)%lat = gP(2,1) 
   endif

   gP(2,2) = -gP(2,1)
   gP(1,2) = gP(1,1) + ONE80
   gP(1,2) = mod( gp(1,2),360. )
   if( present(geodXsect) ) then
     geodXsect(2)%lon = gP(1,2) ; geodXsect(2)%lat = gP(2,2) 
   endif

   end subroutine GCXsect

   function getS( AnArc, xyzPnt ) result( S )
!---------------------------------------------------
!  Calculate the distance from 3 pnts on the GC
!---------------------------------------------------

   type(arc), intent(inout) :: AnArc
   real, intent(in)         :: xyzPnt(3)
   real, intent(out)        :: S
 
   real :: A(2,2)
   real :: B(2), X(2)

   AnArc%sxyz = geod2cart( AnArc%spnt )
   AnArc%exyz = geod2cart( AnArc%epnt )
   AnArc%Angle = Distance( AnArc%sPnt,AnArc%ePnt )

   A(1,1) = AnArc%sxyz%x ; A(1,2) = AnArc%exyz%x
   A(2,1) = AnArc%sxyz%y ; A(2,2) = AnArc%exyz%y
   B(:)   = xyzPnt(1:2)

   X(:) = MatSlv( A, B )

   S = asin( X(2)*sin(AnArc%Angle) )

   end function getS

   function getXYZ( AnArc, S ) result(XYZ)
!---------------------------------------------------
!  Parametric point on arc GC at "distance" = S
!---------------------------------------------------

   type(arc), intent(inout) :: AnArc
   real, intent(in)         :: S
   real                     :: XYZ(3) 

   real :: sind, sins, Factor
   real :: Sxyz(3), Exyz(3)

   if( AnArc%Angle == NOTSET ) then
     AnArc%Angle = Distance( AnArc%sPnt,AnArc%ePnt )
   endif
   sind = sin( AnArc%Angle )
   sins = sin( S )
   Factor = sin(AnArc%Angle - S)

   if( S /= ZERO .and. S /= ONE ) then
     Sxyz(:) = geod2cart( AnArc%sPnt%lon,AnArc%sPnt%lat ) 
     Exyz(:) = geod2cart( AnArc%ePnt%lon,AnArc%ePnt%lat ) 
     XYZ(:) = (Factor*Sxyz(:) + sins*Exyz(:))/sind
   elseif( S == ZERO ) then
     XYZ(:) = Sxyz(:)
   else
     XYZ(:) = Exyz(:)
   endif
  
   end function getXYZ

   function sTriArea( TriArcs ) result( Area )
!---------------------------------------------------
!  Compute area of spherical triange
!---------------------------------------------------

   type(arc), intent(in)    :: TriArcs(3)
   real, intent(out)        :: Area

   integer :: n
   real, pointer :: a, b, c
   real, target  :: ang(3)
   real    :: s, E

   a => ang(1) ; b => ang(2) ; c => ang(3)
   do n = 1,3
     if( TriArcs(n)%Angle == NOTSET ) then
       ang(n) = Distance( TriArcs(n)%sPnt,TriArcs(n)%ePnt )
     else
       ang(n) = TriArcs(n)%Angle
     endif
   enddo

   s = ONEHALF*sum( ang(:) )
   E = tan(ONEHALF*s)*tan(ONEHALF*(s-a))*tan(ONEHALF*(s-b))*tan(ONEHALF*(s-c))
   E = sqrt(E)
   Area = FOUR*tan(E)

   end function sTriArea

   function sQuadArea( QuadArcs ) result( Area )
!---------------------------------------------------
!  Compute area of spherical triange
!---------------------------------------------------

   type(arc), intent(in)    :: QuadArcs(4)
   real, intent(out)        :: Area

   integer   :: n
   type(arc) :: sTriArcs(3)

   sTriArcs(1:2) = QuadArcs(1:2)
   sTriArcs(3)%sPnt = QuadArcs(2)%ePnt
   sTriArcs(3)%ePnt = QuadArcs(1)%sPnt
   Area = sTriArea( sTriArcs )

   sTriArcs(1:2) = QuadArcs(3:4)
   sTriArcs(3)%sPnt = QuadArcs(1)%sPnt
   sTriArcs(3)%ePnt = QuadArcs(2)%ePnt
   Area = Area + sTriArea( sTriArcs )

   end function sQuadArea

   end module Sph_utils