include file: modgps02wgs84grav.cdk90
! -*- F90 -*-

!
! Josep M. Aparicio, 2003-2012.
! ARMA/ASTD
! Environment Canada
!


module modgps02wgs84grav 12,4
  use modgps00base      , only : i4, dp
  use modgps02wgs84const, only : WGS_a, WGS_f, WGS_m,                      &
       WGS_TNGk, WGS_e2, WGS_GammaE
  implicit none

contains

  !  Normal gravity on ellipsoidal surface:
  !  Input:  Latitude
  !          sin(Latitude)
  !
  !  Output: Normal gravity
  !          gpsgravitysrf         : m/s2
  !

  pure function gpsgravitysrf(sLat) 6
    real(dp), intent(in)  :: sLat
    real(dp)              :: gpsgravitysrf
    
    real(dp)              :: ks2
    real(dp)              :: e2s

    ks2 = WGS_TNGk * sLat*sLat
    e2s = 1._dp - WGS_e2 * sLat*sLat
    gpsgravitysrf = WGS_GammaE * (1._dp + ks2) / sqrt(e2s)
  end function gpsgravitysrf

  ! Normal gravity above the ellipsoidal surface:
  ! Input:  Latitude, altitude
  !         sin(Latitude)
  !         Altitude               : m
  !
  ! Output: Normal gravity
  !         gpsgravityalt          : m/s2
  !

  pure function gpsgravityalt(sLat, Altitude) 26,2
    real(dp), intent(in)  :: sLat
    real(dp), intent(in)  :: Altitude
    real(dp)              :: gpsgravityalt

    real(dp)              :: C1
    real(dp)              :: C2

    C1 =-2._dp/WGS_a*(1._dp+WGS_f+WGS_m-2*WGS_f*sLat*sLat)
    C2 = 3._dp/WGS_a**2
    gpsgravityalt = gpsgravitysrf(sLat)*                                   &
         (1._dp + C1 * Altitude + C2 * Altitude**2)
  end function gpsgravityalt

  ! Geopotential energy at a given point.
  ! Result is based on the WGS84 approximate expression for the
  ! gravity acceleration as a function of latitude and altitude,
  ! integrated with the trapezoidal rule.
  ! Input:  Latitude, altitude
  !         Latitude               : rad
  !         Altitude               : m
  !
  ! Output: Geopotential
  !         gpsgeopotential                              : m2/s2

  pure function gpsgeopotential(Latitude, Altitude),4
    real(dp), intent(in)  :: Latitude
    real(dp), intent(in)  :: Altitude
    real(dp)              :: gpsgeopotential

    real(dp)              :: dh, sLat
    integer               :: n, i
    real(dp), allocatable :: hi(:)
    real(dp), allocatable :: gi(:)
    
    dh = 500._dp
    n = 1 + int(Altitude/dh)

    allocate(hi(0:n))
    allocate(gi(0:n))

    sLat=sin(Latitude)

    do i = 0, n-1
       hi(i) = i * dh
       gi(i) = gpsgravityalt(sLat, hi(i))
    enddo
    hi(n) = Altitude
    gi(n) = gpsgravityalt(sLat, hi(n))

    gpsgeopotential = 0._dp
    do i = 1, n
       gpsgeopotential = gpsgeopotential + 0.5_dp * (gi(i)+gi(i-1)) * (hi(i)-hi(i-1))
    enddo

    deallocate(hi)
    deallocate(gi)
  end function gpsgeopotential


  subroutine gpsRadii(Latitude, RadN, RadM) 2
    real(dp), intent(in)  :: Latitude
    real(dp), intent(out) :: RadN, RadM
    real(dp)              :: sLat, e2s

    sLat = sin(Latitude)
    e2s = 1._dp - WGS_e2 * sLat * sLat
    RadN = WGS_a / sqrt(e2s)
    RadM = WGS_a * (1._dp - WGS_e2) / (e2s*sqrt(e2s))
  end subroutine gpsRadii

end module modgps02wgs84grav