program Sphtst

   use Sph_utils

   implicit none

   real, parameter :: PI  = 3.141592653589793
   real, parameter :: ONE80  = 180.
   real, parameter :: D2R = PI/ONE80
   real, parameter :: R2D = ONE80/PI
   real, parameter :: ROUNDOFF = 10.*epsilon(ROUNDOFF)

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

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

!-----------------------------------------------------------
!  test MatSlv
!-----------------------------------------------------------
   A(1,1) = 1. ; A(2,1) = 5. ; A(1,2) = 4. ; A(2,2) = 2.
   b(1) = sum( A(1,:) ) ; b(2) = sum( A(2,:) )

   call MatSlv( A, x, b )

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

   thePnts(2)%lon = 10. ; thePnts(2)%lat = 10.
   V(:) =  geod2cart( thePnts(2)%lon,thePnts(2)%lat )
   
   R = dot_product( V,V )
   Err = abs( 1. - 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 )
   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)
   call MatSlv( A, x, 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 = .5*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 )
   R = sqrt( dot_product( Q,Q ) )
   normP(:) = Q(:)/R

   T = 1.5*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 )
   R = sqrt( dot_product( Q,Q ) )
   normP(:) = Q(:)/R

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

   P(:) =  geod2cart( 5., 10. )
   call cross_prod( U, P, Q )
   R = sqrt( dot_product( Q,Q ) )
   normP(:) = Q(:)/R

!-----------------------------------------------------------
!  the geodetic points "bin"
!-----------------------------------------------------------
   thePnts(3)%lon = 5. ; thePnts(3)%lat = 5.
   thePnts(4)%lon = 10. ; thePnts(4)%lat = 5.
   thePnts(5)%lon = 5. ; thePnts(5)%lat = 10.
 
   thePnts(6)%lon = 5. ; thePnts(6)%lat = -5.
   thePnts(7)%lon = -5. ; thePnts(7)%lat = 5.
   thePnts(8)%lon = -5. ; thePnts(8)%lat = -5.

!-----------------------------------------------------------
!  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)

   do n = 1,6
     theArcs(n)%sxyz = geod2cart( theArcs(n)%sPnt ) 
     theArcs(n)%exyz = geod2cart( theArcs(n)%ePnt ) 
     call cross_prod( theArcs(n),Z )
     theArcs(n)%RHS = Z(3) < 0.
     if( .not. theArcs(n)%RHS ) then
       wrkPnt = theArcs(n)%sPnt
       theArcs(n)%sPnt = theArcs(n)%ePnt
       theArcs(n)%ePnt = wrkPnt
       theArcs(n)%RHS = .true.
       call cross_prod( theArcs(n),Z )
     endif
     theArcs(n)%Angle = Distance( theArcs(n)%sPnt,theArcs(n)%ePnt )
     theArcs(n)%isLatArc = theArcs(n)%sPnt%lat == theArcs(n)%ePnt%lat
   enddo
 
   do n = 1,3
     Arc1 = theArcs(2*n-1)
     Arc2 = theArcs(2*n)
!-----------------------------------------------------------
!  Get mid points
!-----------------------------------------------------------
   call ArcMidpnt( Arc1,Midpnt(1) )
   call ArcMidpnt( Arc2,Midpnt(2) )
   midArc%sPnt = Arc1%sPnt
   midArc%ePnt = Midpnt(1)
   sameGC = CommonGC( midArc,Arc1 )
   P(:) = getXYZ( Arc1,.5*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

!-----------------------------------------------------------
!  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 = 2.*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 ) - 2.*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