program Sphtst

   use Sph_utils

   implicit none

   integer :: nArcs = 10

   integer :: n
   real(8) :: R, Err, T
   real(8) :: wrkLon, wrkLat
   real(8) :: delLon, delLat
   real(8) :: Area(2,2)
   real(8) :: angles(2)
   real(8) :: A(2,2)
   real(8) :: b(2)
   real(8) :: S(2), x(2), gP(2)
   real(8) :: U(3), V(3), W(3), Z(3)
   real(8) :: normW(3), normP(3)
   real(8) :: Q(3), P(3)
   logical :: sameGC, samePnt
   logical :: inArc(4)

   type(geodeticPnt) :: wrkPnt
   type(arc)         :: Arc1, Arc2
   type(arc)         :: midArc
   type(arc)         :: theArcs(20)
   type(arc)         :: Quad(4,2)
   type(geodeticPnt) :: thePnts(20)
   type(geodeticPnt) :: GCXsects(2)
   type(geodeticPnt) :: Midpnt(4)

   PI = FOUR*atan(ONE)
   D2R = PI/ONE80
   R2D = ONE80/PI

   Midpnt(1)%lon = TEN ; Midpnt(1)%lat = FIVE
   U(:) = geod2cart( Midpnt(1)%lon,Midpnt(1)%lat )
   gP(:) = R2D*cart2geod( U(1), U(2), U(3) )
   Midpnt(1)%lon = ONE80 - TEN ; Midpnt(1)%lat = FIVE
   U(:) = geod2cart( Midpnt(1)%lon,Midpnt(1)%lat )
   gP(:) = R2D*cart2geod( U(1), U(2), U(3) )
!-----------------------------------------------------------
!  test MatSlv
!-----------------------------------------------------------
   A(1,1) = 1._8 ; A(2,1) = 5._8 ; A(1,2) = 4._8 ; A(2,2) = 2._8
   b(1) = sum( A(1,:) ) ; b(2) = sum( A(2,:) )

   x(:) = MatSlv( A, b )

!-----------------------------------------------------------
!  test "distance" calculation
!-----------------------------------------------------------
   thePnts(1)%lon = ONE ; thePnts(1)%lat = ONE
   U(:) =  geod2cart( thePnts(1)%lon,thePnts(1)%lat )
   
   R = dot_product( U,U )
   Err = abs( ONE - R )
   if( Err > ROUNDOFF ) then
     Stop 'Roundoff'
   endif

   thePnts(2)%lon = TEN ; thePnts(2)%lat = TEN
   V(:) =  geod2cart( thePnts(2)%lon,thePnts(2)%lat )
   
   R = dot_product( V,V )
   Err = abs( ONE - R )
   if( Err > ROUNDOFF ) then
     Stop 'Roundoff'
   endif

   angles(1) = dot_product( U,V )
   angles(1) = acos( angles(1) )

   angles(2) = Distance( thePnts(1),thePnts(2) )
   angles(2) = R2D*angles(1)

!-----------------------------------------------------------
!  test "parametric" equation for arc,great circle
!-----------------------------------------------------------
!  call cross_prod( U, V, W )
!  call cross_prod( W, U, Z )
   W(:) = cross_prod( U, V )
   Z(:) = cross_prod( W, U )
   R = sqrt( dot_product( Z,Z ) )
   Z(:) = Z(:)/R
   R = sqrt( dot_product( Z,Z ) )

   R = sqrt( dot_product( W,W ) )
   normW(:) = W(:)/R

   A(1,1) = U(1) ; A(2,1) = U(2) ; A(1,2) = Z(1) ; A(2,2) = Z(2)
   b(:) = V(1:2)
   x(:) = MatSlv( A, b )

   R = dot_product( x,x )
   S(:) = (/ acos( x(1) ),asin( x(2) ) /)

   T = D2R
   P(:) = cos(T)*U(:) + sin(T)*Z(:)
   gP(:) =  cart2geod( P(1), P(2), P(3) )
   gP(:) = R2D*gP(:)

   T = -D2R
   P(:) = cos(T)*U(:) + sin(T)*Z(:)
   gP(:) =  cart2geod( P(1), P(2), P(3) )
   gP(:) = R2D*gP(:)

   T = ONEHALF*S(1)
   P(:) = cos(T)*U(:) + sin(T)*Z(:)
   gP(:) =  cart2geod( P(1), P(2), P(3) )
   gP(:) = R2D*gP(:)

!  call cross_prod( U, P, Q )
   Q(:) = cross_prod( U, P )
   R = sqrt( dot_product( Q,Q ) )
   normP(:) = Q(:)/R

   T = 1.5_8*S(1)
   P(:) = cos(T)*U(:) + sin(T)*Z(:)
   gP(:) =  cart2geod( P(1), P(2), P(3) )
   gP(:) = R2D*gP(:)

!  call cross_prod( U, P, Q )
   Q(:) = cross_prod( U, P )
   R = sqrt( dot_product( Q,Q ) )
   normP(:) = Q(:)/R

   T = ONEHALF*PI
   P(:) = cos(T)*U(:) + sin(T)*Z(:)
   gP(:) =  cart2geod( P(1), P(2), P(3) )
   gP(:) = R2D*gP(:)

   P(:) =  geod2cart( FIVE, TEN )
   Q(:) = cross_prod( U, P )
   R = sqrt( dot_product( Q,Q ) )
   normP(:) = Q(:)/R

!-----------------------------------------------------------
!  the geodetic points "bin"
!-----------------------------------------------------------
   thePnts(3)%lon = FIVE ; thePnts(3)%lat = FIVE
   thePnts(4)%lon = TEN ; thePnts(4)%lat = FIVE
   thePnts(5)%lon = FIVE ; thePnts(5)%lat = TEN
 
   thePnts(6)%lon = FIVE ; thePnts(6)%lat = -FIVE
   thePnts(7)%lon = -FIVE ; thePnts(7)%lat = FIVE
   thePnts(8)%lon = -FIVE ; thePnts(8)%lat = -FIVE
   thePnts(9)%lon = 30._8 ; thePnts(9)%lat = -FIVE
   thePnts(10)%lon = 30._8 ; thePnts(10)%lat = FIVE
   thePnts(11)%lon = 29._8 ; thePnts(11)%lat = ZERO
   thePnts(12)%lon = 31._8 ; thePnts(12)%lat = ZERO

!-----------------------------------------------------------
!  the arcs bin
!-----------------------------------------------------------
   theArcs(1)%sPnt = thePnts(3) ; theArcs(1)%ePnt = thePnts(2)
   theArcs(2)%sPnt = thePnts(4) ; theArcs(2)%ePnt = thePnts(5)
 
   theArcs(3)%sPnt = thePnts(3) ; theArcs(3)%ePnt = thePnts(4)
   theArcs(4)%sPnt = thePnts(4) ; theArcs(4)%ePnt = thePnts(2)

   theArcs(5)%sPnt = thePnts(8) ; theArcs(5)%ePnt = thePnts(3)
   theArcs(6)%sPnt = thePnts(6) ; theArcs(6)%ePnt = thePnts(7)

   theArcs(7)%sPnt = thePnts(9) ; theArcs(7)%ePnt = thePnts(10)
   theArcs(8)%sPnt = thePnts(11) ; theArcs(8)%ePnt = thePnts(12)

   theArcs(9)%sPnt = thePnts(3) ; theArcs(9)%ePnt = thePnts(5)
   theArcs(10)%sPnt = thePnts(4) ; theArcs(10)%ePnt = thePnts(2)

!-----------------------------------------------------------
!  Make arcs more like model
!-----------------------------------------------------------
!  theArcs(1:6)%sPnt%lon = 1.e-2_8*theArcs(1:6)%sPnt%lon
!  theArcs(1:6)%sPnt%lat = 1.e-2_8*theArcs(1:6)%sPnt%lat
!  theArcs(1:6)%ePnt%lon = 1.e-2_8*theArcs(1:6)%ePnt%lon
!  theArcs(1:6)%ePnt%lat = 1.e-2_8*theArcs(1:6)%ePnt%lat

   do n = 1,nArcs
     theArcs(n)%sxyz = geod2cart( theArcs(n)%sPnt ) 
     theArcs(n)%exyz = geod2cart( theArcs(n)%ePnt ) 
     Z(:) = cross_prod( theArcs(n) )
     theArcs(n)%Normal(:) = Z(:)
     theArcs(n)%RHS = Z(3) > ZERO
     if( .not. theArcs(n)%RHS ) then
       wrkPnt = theArcs(n)%sPnt
       theArcs(n)%sPnt = theArcs(n)%ePnt
       theArcs(n)%ePnt = wrkPnt
       theArcs(n)%RHS = .true.
       Z(:) =  cross_prod( theArcs(n) )
       theArcs(n)%Normal(:) = -theArcs(n)%Normal(:)
     endif
     theArcs(n)%Angle    = Distance( theArcs(n)%sPnt,theArcs(n)%ePnt )
     theArcs(n)%isLatArc = theArcs(n)%sPnt%lat == theArcs(n)%ePnt%lat
   enddo
 
Arc_loop: &
   do n = 1,nArcs/2
     Arc1 = theArcs(2*n-1)
     Arc2 = theArcs(2*n)
!-----------------------------------------------------------
!  Get mid points
!-----------------------------------------------------------
   Midpnt(1) = ArcMidpnt( Arc1 )
   Midpnt(2) = ArcMidpnt( Arc2 )
   midArc%sPnt = Arc1%sPnt
   midArc%ePnt = Midpnt(1)
   sameGC = CommonGC( midArc,Arc1 )
   P(:) = getXYZ( Arc1,ONEHALF*Arc1%Angle )
   Q(:) = geod2cart( Midpnt(1)%lon,Midpnt(1)%lat )
   samePnt = cartsEqual( P,Q )


!-----------------------------------------------------------
!  Do sanity checks
!-----------------------------------------------------------
   inArc(1) = Pnt_in_Arc( Arc1,Midpnt(1) )
   inArc(2) = Pnt_in_Arc( Arc2,Midpnt(2) )
   inArc(3) = Pnt_in_Arc( Arc1,Midpnt(2) )
   inArc(4) = Pnt_in_Arc( Arc2,Midpnt(1) )

!-----------------------------------------------------------
!  test great circle intersections
!-----------------------------------------------------------

   sameGC = CommonGC( Arc1,Arc2 )
   if( .not. sameGC ) then
     call GCXsect( (/Arc1,Arc2/), GCXsects )
     inArc(1) = Pnt_in_Arc( Arc1,GCXsects(1) )
     inArc(2) = Pnt_in_Arc( Arc2,GCXsects(1) )
     inArc(3) = Pnt_in_Arc( Arc1,GCXsects(2) )
     inArc(4) = Pnt_in_Arc( Arc2,GCXsects(2) )
   endif

   enddo Arc_loop

!-----------------------------------------------------------
!  Test tri area routine
!-----------------------------------------------------------
   wrkLat = 60._8
   thePnts(19)%lon = ZERO ; thePnts(19)%lat = ZERO
   thePnts(20)%lon = NINETY ; thePnts(20)%lat = ZERO
   quad(1,1)%sPnt = thePnts(19) ; quad(1,1)%ePnt = thePnts(20)

   thePnts(19) = thePnts(20)
   thePnts(20)%lon = NINETY ; thePnts(20)%lat = wrkLat
   quad(2,1)%sPnt = thePnts(19) ; quad(2,1)%ePnt = thePnts(20)

   thePnts(19) = thePnts(20)
   thePnts(20)%lon = ZERO ; thePnts(20)%lat = ZERO
   quad(3,1)%sPnt = thePnts(19) ; quad(3,1)%ePnt = thePnts(20)
   Area(1,1) = sTriArea( quad(1:3,1) )

   thePnts(19)%lon = ZERO ; thePnts(19)%lat = ZERO
   thePnts(20)%lon = NINETY ; thePnts(20)%lat = wrkLat
   quad(1,1)%sPnt = thePnts(19) ; quad(1,1)%ePnt = thePnts(20)

   thePnts(19) = thePnts(20)
   thePnts(20)%lon = ZERO ; thePnts(20)%lat = wrkLat
   quad(2,1)%sPnt = thePnts(19) ; quad(2,1)%ePnt = thePnts(20)

   thePnts(19) = thePnts(20)
   thePnts(20)%lon = ZERO ; thePnts(20)%lat = ZERO
   quad(3,1)%sPnt = thePnts(19) ; quad(3,1)%ePnt = thePnts(20)
   Area(2,1) = sTriArea( quad(1:3,1) )

!-----------------------------------------------------------
!  Test quad area routine with a simple area
!-----------------------------------------------------------
!  wrkLon = 30._8 ; wrkLat = 80._8
   wrkLon = 30._8 ; wrkLat = 20._8
   delLon = ONE  ; delLat = ONE  
   thePnts(1)%lon = wrkLon ;        thePnts(1)%lat = wrkLat
   thePnts(2)%lon = wrkLon+delLon ; thePnts(2)%lat = wrkLat
   thePnts(3)%lon = wrkLon+delLon ; thePnts(3)%lat = wrkLat+delLat
   thePnts(4)%lon = wrkLon ;        thePnts(4)%lat = wrkLat+delLat

   do n = 1,3
     quad(n,1)%sPnt = thePnts(n)
     quad(n,1)%ePnt = thePnts(n+1)
   enddo
   quad(4,1)%sPnt = thePnts(4)
   quad(4,1)%ePnt = thePnts(1)

   Area(1,1) = sQuadArea( Quad(:,1) )
   Area(2,1) = delLon*D2R*(sin((wrkLat+delLat)*D2R) - sin(wrkLat*D2R))

   delLon = .25_8*delLon  ; delLat = .25_8*delLat
   thePnts(1)%lon = wrkLon ;        thePnts(1)%lat = wrkLat
   thePnts(2)%lon = wrkLon+delLon ; thePnts(2)%lat = wrkLat
   thePnts(3)%lon = wrkLon+delLon ; thePnts(3)%lat = wrkLat+delLat
   thePnts(4)%lon = wrkLon ;        thePnts(4)%lat = wrkLat+delLat

   do n = 1,3
     quad(n,1)%sPnt = thePnts(n)
     quad(n,1)%ePnt = thePnts(n+1)
   enddo
   quad(4,1)%sPnt = thePnts(4)
   quad(4,1)%ePnt = thePnts(1)

   Area(1,2) = sQuadArea( Quad(:,1) )
   Area(2,2) = delLon*D2R*(sin((wrkLat+delLat)*D2R) - sin(wrkLat*D2R))

!-----------------------------------------------------------

   thePnts(19)%lon = wrkLon ; thePnts(19)%lat = .25_8*wrkLat
   thePnts(20)%lon = TWO*wrkLon ; thePnts(20)%lat = .25_8*wrkLat
   quad(1,1)%sPnt = thePnts(19) ; quad(1,1)%ePnt = thePnts(20)

   thePnts(19) = thePnts(20)
   thePnts(20)%lon = TWO*wrkLon ; thePnts(20)%lat = .75_8*wrkLat
   quad(2,1)%sPnt = thePnts(19) ; quad(2,1)%ePnt = thePnts(20)

   thePnts(19) = thePnts(20)
   thePnts(20)%lon = wrkLon ; thePnts(20)%lat = .75_8*wrkLat
   quad(3,1)%sPnt = thePnts(19) ; quad(3,1)%ePnt = thePnts(20)

   thePnts(19) = thePnts(20)
   thePnts(20)%lon = wrkLon ; thePnts(20)%lat = .25_8*wrkLat
   quad(4,1)%sPnt = thePnts(19) ; quad(4,1)%ePnt = thePnts(20)

   Area(1,2) = sQuadArea( Quad(:,1) )
   Area(2,2) = wrkLon*D2R*(sin(wrkLat**D2R) - sin(ZERO*D2R))
!-----------------------------------------------------------
!    Near the equator
!-----------------------------------------------------------
!   first the enclosing quad
!-----------------------------------------------------------
   quad(1:2,1) = theArcs(3:4)
   quad(3,1)%sPnt = thePnts(2) ; quad(3,1)%ePnt = thePnts(5)
   quad(4,1)%sPnt = thePnts(5) ; quad(4,1)%ePnt = thePnts(3)
   Area(1,1) = sQuadArea( Quad(:,1) )
   Area(2,1) = FIVE*D2R*(sin(TEN*D2R) - sin(FIVE*D2R))
!-----------------------------------------------------------
!   now the enclosed quad
!-----------------------------------------------------------
   thePnts(19)%lon = 6.5_8 ; thePnts(19)%lat = 6.5_8
   thePnts(20)%lon = 8.5_8 ; thePnts(20)%lat = 6.5_8
   quad(1,2)%sPnt = thePnts(19) ; quad(1,2)%ePnt = thePnts(20)
   thePnts(19) = thePnts(20)
   thePnts(20)%lon = 8.5_8 ; thePnts(20)%lat = 8.5_8
   quad(2,2)%sPnt = thePnts(19) ; quad(2,2)%ePnt = thePnts(20)
   thePnts(19) = thePnts(20)
   thePnts(20)%lon = 6.5_8 ; thePnts(20)%lat = 8.5_8
   quad(3,2)%sPnt = thePnts(19) ; quad(3,2)%ePnt = thePnts(20)
   thePnts(19) = thePnts(20)
   thePnts(20)%lon = 6.5_8 ; thePnts(20)%lat = 6.5_8
   quad(4,2)%sPnt = thePnts(19) ; quad(4,2)%ePnt = thePnts(20)
   Area(1,2) = sQuadArea( Quad(:,2) )
   Area(2,2) = TWO*D2R*(sin(8.5_8*D2R) - sin(6.5_8*D2R))

!-----------------------------------------------------------
!    High latitude
!-----------------------------------------------------------
!   first the enclosing quad
!-----------------------------------------------------------
   thePnts(19)%lon = FIVE ; thePnts(19)%lat = 70._8
   thePnts(20)%lon = FIVE+ONEHALF ; thePnts(20)%lat = 70._8
   quad(1,1)%sPnt = thePnts(19) ; quad(1,1)%ePnt = thePnts(20)
   thePnts(19) = thePnts(20)
   thePnts(20)%lon = FIVE+ONEHALF ; thePnts(20)%lat = 70.5_8
   quad(2,1)%sPnt = thePnts(19) ; quad(2,1)%ePnt = thePnts(20)
   thePnts(19) = thePnts(20)
   thePnts(20)%lon = FIVE ; thePnts(20)%lat = 70.5_8
   quad(3,1)%sPnt = thePnts(19) ; quad(3,1)%ePnt = thePnts(20)
   thePnts(19) = thePnts(20)
   thePnts(20)%lon = FIVE ; thePnts(20)%lat = 70._8
   quad(4,1)%sPnt = thePnts(19) ; quad(4,1)%ePnt = thePnts(20)
   Area(1,1) = sQuadArea( Quad(:,1) )
   Area(2,1) = ONEHALF*D2R*(sin(70.5_8*D2R) - sin(70._8*D2R))
!-----------------------------------------------------------
!   now the enclosed quad
!-----------------------------------------------------------
   thePnts(19)%lon = 6.5_8 ; thePnts(19)%lat = 71.5_8
   thePnts(20)%lon = 8.5_8 ; thePnts(20)%lat = 71.5_8
   quad(1,2)%sPnt = thePnts(19) ; quad(1,2)%ePnt = thePnts(20)
   thePnts(19) = thePnts(20)
   thePnts(20)%lon = 8.5_8 ; thePnts(20)%lat = 73.5_8
   quad(2,2)%sPnt = thePnts(19) ; quad(2,2)%ePnt = thePnts(20)
   thePnts(19) = thePnts(20)
   thePnts(20)%lon = 6.5_8 ; thePnts(20)%lat = 73.5_8
   quad(3,2)%sPnt = thePnts(19) ; quad(3,2)%ePnt = thePnts(20)
   thePnts(19) = thePnts(20)
   thePnts(20)%lon = 6.5_8 ; thePnts(20)%lat = 71.5_8
   quad(4,2)%sPnt = thePnts(19) ; quad(4,2)%ePnt = thePnts(20)
   Area(1,2) = sQuadArea( Quad(:,2) )
   Area(2,2) = TWO*D2R*(sin(73.5_8*D2R) - sin(71.5_8*D2R))

!-----------------------------------------------------------
!  Test arcs for common great circle
!-----------------------------------------------------------
   theArcs(7)%sPnt%lon = FIVE+ONE ; theArcs(7)%sPnt%lat = FIVE+ONE
   theArcs(7)%ePnt%lon = TEN-ONE ; theArcs(7)%ePnt%lat = TEN-ONE
   Arc1%sPnt%lon = 1.e-2_8*theArcs(1)%sPnt%lon
   Arc1%sPnt%lat = 1.e-2_8*theArcs(1)%sPnt%lat
   Arc1%ePnt%lon = 1.e-2_8*theArcs(1)%ePnt%lon
   Arc1%ePnt%lat = 1.e-2_8*theArcs(1)%ePnt%lat
   Arc2%sPnt%lon = 1.e-2_8*theArcs(7)%sPnt%lon
   Arc2%sPnt%lat = 1.e-2_8*theArcs(7)%sPnt%lat
   Arc2%ePnt%lon = 1.e-2_8*theArcs(7)%ePnt%lon
   Arc2%ePnt%lat = 1.e-2_8*theArcs(7)%ePnt%lat
   sameGC = CommonGC( Arc1,Arc2 )
   call GCXsect( (/Arc1,Arc2/), GCXsects )
!-----------------------------------------------------------
!  Test arc formula for points outside the "distance"
!-----------------------------------------------------------
   theArcs(1)%sxyz = geod2cart( theArcs(1)%sPnt )
   theArcs(1)%exyz = geod2cart( theArcs(1)%ePnt )

!-----------------------------------------------------------
!  first try s = 2*d
!-----------------------------------------------------------
   R = TWO*cos(theArcs(1)%Angle)
   P(1) = -theArcs(1)%sxyz%x + R*theArcs(1)%exyz%x
   P(2) = -theArcs(1)%sxyz%y + R*theArcs(1)%exyz%y
   P(3) = -theArcs(1)%sxyz%z + R*theArcs(1)%exyz%z

   gP(:) = R2D*cart2geod( P(1), P(2), P(3) )
   T = dot_product( P,P )
   angles(1) = getS( theArcs(1),P )
   angles(1) = getS( theArcs(1),P ) - TWO*thearcs(1)%Angle

   theArcs(7)%sPnt = theArcs(1)%sPnt
   theArcs(7)%ePnt%lon = gP(1) ; theArcs(7)%ePnt%lat = gP(2)
   sameGC = CommonGC( theArcs(1),theArcs(7) )

!-----------------------------------------------------------
!  next try s = -d
!-----------------------------------------------------------
   P(1) = R*theArcs(1)%sxyz%x - theArcs(1)%exyz%x
   P(2) = R*theArcs(1)%sxyz%y - theArcs(1)%exyz%y
   P(3) = R*theArcs(1)%sxyz%z - theArcs(1)%exyz%z

   gP(:) = R2D*cart2geod( P(1), P(2), P(3) )
   T = dot_product( P,P )
   angles(1) = getS( theArcs(1),P )

   theArcs(7)%ePnt = theArcs(1)%sPnt
   theArcs(7)%sPnt%lon = gP(1) ; theArcs(7)%sPnt%lat = gP(2)
   sameGC = CommonGC( theArcs(1),theArcs(7) )

   end program Sphtst