!===============================================================================
! One of Andy Nowacki's Fortran utility modules for dealing with seismic
! anisotropy and other problems.
!
! Andy Nowacki <andy.nowacki@bristol.ac.uk>
!
! See the file LICENCE for licence details.
!===============================================================================
!  Module containing spherical geometry helper functions and subroutines.
!  Andy Nowacki, University of Bristol
!  andy.nowacki@bristol.ac.uk
!
!  History:
!      2011-04-12:  Added sphere_sample subroutine to return an array of points
!                   which evenly sample a sphere.
!      2011-07-18:  Added routines to find Earth radial direction
!      2011-11-08:  Added sph_poly_inout: determines if point is inside or outside
!                   a set of points (ordered) on a sphere.
!
!===============================================================================
module spherical_geometry

   implicit none

!  ** size constants
   integer, parameter, private :: i4 = selected_int_kind(9)       ! long int
   integer, parameter, private :: r4 = selected_real_kind(6,37)   ! SP
   integer, parameter, private :: r8 = selected_real_kind(15,307) ! DP

!  ** precision selector
   integer, parameter, private :: rs = r8

!  ** maths constants and other useful things
   real(rs), parameter, private :: pi = 3.141592653589793238462643_rs
   real(rs), parameter, private :: pi2 = pi/2._rs
   real(rs), parameter, private :: twopi = 2._rs*pi
   real(rs), parameter, private :: to_rad = 1.74532925199433e-002
   real(rs), parameter, private :: to_deg = 57.2957795130823e0
   real(rs), parameter, private :: to_km = 111.194926644559

   real(rs), parameter, private :: big_number = 10.e36
   real(rs), parameter, private :: angle_tol = 1.e-5

   ! Random number sampler state
   logical, save, private :: rng_initialised = .false.

   contains

!------------------------------------------------------------------------------
   function delta(lon1_in,lat1_in,lon2_in,lat2_in,degrees)
!------------------------------------------------------------------------------
!  delta returns the angular distance between two points on a sphere given
!  the lat and lon of each using the Haversine formula
!
     implicit none
     real(rs) :: delta,lat1,lon1,lat2,lon2
     real(rs),intent(in) :: lat1_in,lon1_in,lat2_in,lon2_in
     logical,optional :: degrees

     lat1 = lat1_in;  lon1 = lon1_in
     lat2 = lat2_in;  lon2 = lon2_in
     if (present(degrees)) then
        if (degrees) then
         lat1=lat1*pi/1.8D2 ; lon1=lon1*pi/1.8D2
         lat2=lat2*pi/1.8D2 ; lon2=lon2*pi/1.8D2
        endif
     endif

     delta=atan2( sqrt( (cos(lat2)*sin(lon2-lon1))**2 + (cos(lat1)*sin(lat2) - &
           sin(lat1)*cos(lat2)*cos(lon2-lon1))**2) , &
           sin(lat1)*sin(lat2) + cos(lat1)*cos(lat2)*cos(lon2-lon1))

     if (present(degrees)) then
         if (degrees) delta = delta * 1.8D2/pi
      endif

     return

   end function delta
!==============================================================================

!------------------------------------------------------------------------------
!   function dist_vincenty(lon1_in,lat1_in,lon2_in,lat2_in,R,&
!                           a_in,b_in,azi,baz,degrees)
   subroutine dist_vincenty(lon1_in,lat1_in,lon2_in,lat2_in,dist,R,a_in,b_in,degrees)
!------------------------------------------------------------------------------
!  delta_vincenty uses the Vincenty algorithm to find accurate great circle
!  distances on a flattened sphere.  Input in radians unless specified.
!  Unless R is specified, then distances are on the surface of the Earth.
!  R is fractional radius of points, given the ellipsoid of the Earth.
!  Also computes the azimuth and backazimuth, if asked for
!  Fom: http://www.movable-type.co.uk/scripts/latlong-vincenty.html

      implicit none

      real(rs),intent(in)  :: lon1_in,lat1_in,lon2_in,lat2_in
      real(rs),intent(in),optional :: a_in,b_in,R
      real(rs)              :: conversion
      logical,intent(in),optional :: degrees
      real(rs),intent(out)  :: dist
      real(rs)             :: a,b,f,lon1,lat1,lon2,lat2,L,u1,u2,lambda1,lambda2,&
                              sin_s,cos_s,s,sin_a,cos_a_sq,cos_2sm,C,u_2,&
                              big_A,big_B,ds
      logical              :: isnan
!      real,intent(out),optional :: azi,baz

      write(*,'(a)') 'subroutine dist_vincenty is not working yet.'
      stop

!  Convert from degrees if necessary
      conversion = 1.d0
      if (present(degrees)) then
         if (degrees) conversion = pi/1.8d2
      endif

!  Get the inputs
      lon1 = conversion * (lon1_in)  ;  lat1 = conversion * (lat1_in)
      lon2 = conversion * (lon2_in)  ;  lat2 = conversion * (lat2_in)

!  Default ellipsoidal shape is WGS-84: override these if both a and b are supplied
      a = 6378.137d0  ;  b = 6356.752314245d0
      if (present(a_in).and.present(b_in)) then
         a = a_in  ;  b = b_in
      endif
!  Scale by the fractional radius of the ellipsoid if present
      if (present(R)) then
         a = a*R  ; b = b*R
      endif
      f = (a-b)/a

!  Constants
      L = lon2 - lon1
      u1 = atan((1-f)*tan(lat1))
      u2 = atan((1-f)*tan(lat2))
!  Starting guess of L for lambda
      lambda1 = 0.d0
      lambda2 = L
!  Iterate until convergence
      do while (abs(lambda2-lambda1) < 1.d-12)
         sin_s = sqrt( (cos(u2)*sin(lambda2))**2 + (cos(u1)*sin(u2) - &
                      sin(u1)*cos(u2)*cos(lambda2))**2 )
         if (sin_s==0.d0) then
            dist = 0.d0
            return
         endif
         cos_s = sin(u1)*sin(u2) + cos(u1)*cos(u2)*cos(lambda2)
         s = atan2(sin_s,cos_s)
         sin_a = cos(u1)*cos(u2)*sin(lambda2)/sin_s
         cos_a_sq = 1.d0 - sin_a**2
         cos_2sm = cos_s - 2.d0*sin(u1)*sin(u2)/(cos_a_sq)
         if (isnan(cos_2sm)) cos_2sm = 0.d0
         C = (f/16.d0)*(cos_a_sq)*(4.d0+f*(4.d0-3.d0*cos_a_sq))
         lambda1 = lambda2
         lambda2 = L+(1.d0-C)*f*sin_a*(s+C*sin_s*(cos_2sm+C*cos_s*(-1.d0+2.d0*cos_2sm**2)))
      enddo

      u_2 = cos_a_sq*(a**2-b**2)/(b**2)
      big_A = 1.d0 + (u_2/16384.d0)*(4096.d0+u_2*(-768.d0+u_2*(320.d0-175.d0*u_2)))
      big_B = (u_2/1024.d0)*(256.d0+u_2*(-128.d0+u_2*(74.d0-47.d0*u_2)))
      ds = big_B*sin_s*(cos_2sm+(big_B/4.d0)*(cos_s*(-1.d0+2.d0*cos_2sm**2)-&
           (big_B/6.d0)*cos_2sm*(-3.d0+4.d0*sin_s**2)*(-3.d0+4.d0*cos_2sm**2)))
      dist = b*big_A*(s-ds)
!      if (present(azi)) &
!         azi = atan2(cos(u2)*sin(lambda2), &
!                     cos(u1)*sin(u2)-sin(u1)*cos(u2)*cos(lambda2))
!      if (present(baz)) &
!         baz = atan2(cos(u1)*sin(lambda2), &
!                     -sin(u1)*cos(u2)+cos(u1)*sin(u2)*cos(lambda2))

      write(*,*) 'lambda =',lambda2

   end subroutine dist_vincenty
!==============================================================================

!------------------------------------------------------------------------------
!   function test_dist_vincenty(lon1_in,lat1_in,lon2_in,lat2_in,a_in,b_in,R,degrees)
!------------------------------------------------------------------------------
!   implicit none
!
!   real(rs),intent(in)    :: lon1_in,lon2_in,lat1_in,lat2_in
!   real(rs),intent(in),optional :: a_in,b_in,R
!   real(rs)               :: lon1,lon2,lat1,lat2
!   real(rs)               :: test_dist_vincenty
!   logical,intent(in),optional :: degrees
!   real(rs)               :: a,b,f,L,u1,u2,lambda1,lambda2,sin_s,cos_s,s,sin_a,&
!                             cos_a_sq,cos_2sm,C,u_2,big_A,big_B,Delta_s,dist,&
!                             conversion
!   real(rs),parameter     :: convergence_limit = 1.d-12  !  To within ~6mm
!
!
!   conversion = 1.d0
!   if (present(degrees)) then
!      if (degrees) conversion = pi/1.8d2
!   endif
!
!!  Get the inputs
!   lon1 = conversion * (lon1_in)  ;  lat1 = conversion * (lat1_in)
!   lon2 = conversion * (lon2_in)  ;  lat2 = conversion * (lat2_in)
!
!!  Default ellipsoidal shape is WGS-84: override these if both a and b are supplied
!   a = 6378.137d0 ; b = 6356.752314245d0
!   if (present(a_in).and.present(b_in)) then
!      a = a_in  ;  b = b_in
!   endif
!
!!  Scale by the fractional radius of the ellipsoid if present
!   if (present(R)) then
!      a = a*R  ; b = b*R
!   endif
!
!!  Constants
!   f = (a-b)/a ;
!   L = lon2 - lon1 ;
!   u1 = atan2((1-f)*tan(lat1),1.d0)
!   u2 = atan2((1-f)*tan(lat2),1.d0)
!
!!  Starting guess for the iteration variables
!   lambda1 = big_number
!   lambda2 = L
!
!   do while ( abs(lambda2-lambda1) > convergence_limit )
!     sin_s = sqrt((cos(u2)*sin(lambda2))**2 + (cos(u1)*sin(u2)-sin(u1)*cos(u2)*cos(lambda2))**2)
!     cos_s = sin(u1)*sin(u2) + cos(u1)*cos(u2)*cos(lambda2)
!     s = atan2(sin_s,cos_s)
!     sin_a = cos(u1)*cos(u2)*sin(lambda2)/sin_s
!     cos_a_sq = 1.d0 - sin_a**2
!     cos_2sm = cos_s - s*sin(u1)*sin(u2)/cos_a_sq
!      if (isnan(cos_2sm)) cos_2sm = 0.d0
!     C = (f/16.d0)*cos_a_sq*(4.d0+f*(4.d0-3.d0*cos_a_sq))
!     lambda1 = lambda2
!     lambda2 = L + (1.d0-C)*f*sin_a*(s+C*sin_a*(cos_2sm+C*cos_s*(-1.d0+2.d0*cos_2sm**2)))
!   enddo
!
!   u_2 = cos_a_sq*(a**2-b**2)/b**2
!   big_A = 1.d0+(u_2/16384.d0)*(4096.d0+u_2*(-768.d0+u_2*(320.d0-175.d0*u_2)))
!   big_B = (u_2/1024.d0)*(256.d0+u_2*(-128.d0+u_2*(74.d0-47.d0*u_2)))
!   Delta_s = big_B*sin_s*(cos_2sm+(big_B/4.d0)*(cos_s*(-1.d0+2.d0*cos_2sm**2)- &
!          (big_B/6.d0)*cos_2sm*(-3.d0+4.d0*sin_s**2)*(-3.d0+4.d0*cos_2sm**2)))
!   test_dist_vincenty = b*big_A*(s-Delta_s) ;
!
!
!   return
!
!   end function test_dist_vincenty
!==============================================================================

!------------------------------------------------------------------------------
   subroutine step(lon1_in,lat1_in,az_in,delta_in,lon2,lat2,degrees)
!------------------------------------------------------------------------------
! Computes the endpoint given a starting point lon,lat, azimuth and angular distance

     implicit none

     real(rs),intent(in)  :: lon1_in,lat1_in,az_in,delta_in
     real(rs),intent(out) :: lon2,lat2
     real(rs)             :: lon1,lat1,az,delta
     logical,optional,intent(in) :: degrees
     logical          :: deg

     lon1=lon1_in ; lat1=lat1_in ; az=az_in ; delta=delta_in

     deg = .false.
     if (present(degrees)) deg = degrees

     if (deg) then
       if (delta > 360.) then
          write(*,*)'spherical_geometry: step: Error: distance must be less than 180 degrees.'
          stop
       else if (lon1 <-180 .or. lon1 > 180) then
          write(*,*)'spherical_geometry: step: Error: longitude must be in range -180 - 180.'
          stop
       else if (lat1 <-90 .or. lat1 > 90) then
          write(*,*)'spherical_geometry: step: Error: latitude must be in range -90 - 90.'
          stop
       endif
     else
       if (delta > twopi) then
            write(*,*)'spherical_geometry: step: Error: distance must be less than 2pi radians.'
            stop
        else if (lon1 < -pi .or. lon1 > pi) then
            write(*,*)'spherical_geometry: step: Error: longitude must be in range -2pi - 2pi.'
            stop
        else if (lat1 < -pi/2.d0 .or. lat1 > pi/2.d0) then
            write(*,*)'spherical_geometry: step: Error: latitude must be in range -pi - pi.'
          stop
        endif
     endif

     if (deg) then
!  Convert to radians
        lon1=lon1*pi/1.8D2 ; lat1=lat1*pi/1.8D2
        az=az*pi/1.8D2     ; delta=delta*pi/1.8D2
     endif

!  Calculate point which is delta degrees/radians from lon1,lat1 along az
     lat2 = asin(sin(lat1)*cos(delta) + cos(lat1)*sin(delta)*cos(az))
     lon2 = lon1 + atan2(sin(az)*sin(delta)*cos(lat1),  &
                         cos(delta)-sin(lat1)*sin(lat2) )

     if (deg) then
!  Convert to degrees
        lat2=1.8D2*lat2/pi  ; lon2=1.8D2*lon2/pi
        if(lon2>1.8D2) lon2=lon2-3.6D2 ; if(lon2<-1.8D2) lon2=lon2+3.6D2
     end if

     return

   end subroutine step
!==============================================================================

!-------------------------------------------------------------------------------
   subroutine gcp_points(lon1,lat1,lon2,lat2,ptslon,ptslat,npts,ds,n,degrees)
!-------------------------------------------------------------------------------
!  Returns arrays of lon and lat points (including the end points)
!  along a great circle path between two endpoints.  The user must specify
!  one of the separation distance, ds (degrees or radians), or number of points
!  (including the end points), n.  geographic coordinates can be in degrees or
!  radians, and the points are returned in the same format.  The array must be
!  at least npts long (optionally returned by the subroutine).

      implicit none
      real(rs),intent(in) :: lon1,lat1,lon2,lat2
      real(rs) :: x1,x2,y1,y2,d,ddelta,azi
      real(rs),dimension(:),intent(out) :: ptslon,ptslat
      integer,intent(out),optional :: npts
      real(rs),intent(in),optional :: ds
      integer,intent(in),optional :: n
      logical,intent(in),optional :: degrees
      real(rs) :: conversion
      integer :: i,npoints

      ! Default to radians; convert from degrees if necessary
      conversion = 1._rs
      if (present(degrees)) then
         if (degrees) conversion = pi/180._rs
      endif
      x1 = lon1*conversion;  x2 = lon2*conversion
      y1 = lat1*conversion;  y2 = lat2*conversion

      ! Check one of ds or n is present
      if (.not.present(ds) .and. .not.present(n)) then
         write(*,'(a)') 'spherical_geometry: gcp_points: Error: one of ds or n must be specified'
         stop
      endif

      ! Get distance and azimuth between two points
      d = delta(x1,y1,x2,y2,degrees=.false.)
      azi = azimuth(x1,y1,x2,y2,degrees=.false.)

      ! Using a fixed distance ds (same units as coordinates)
      if (present(ds)) then
         if (.not.present(npts)) then
            write(*,'(a)') 'spherical_geometry: gcp_points: Error: must supply npts as well as ds if using constant spacing'
            stop
         endif
         ddelta = ds*conversion
         ! Check we haven't asked for too much
         if (ddelta > d) then
            write(*,'(a)') 'spherical_geometry: gcp_points: Error: requested point spacing is larger than distance between points'
            stop
         endif
         ! Calculate number of points
         npoints = ceiling(d/ddelta) + 1
         npts = npoints

      ! Using a fixed number of points
      else
         npoints = n
         ! Have supplied too few points
         if (npoints < 2) then
            write(*,'(a)') 'spherical_geometry: gcp_points: Error: n must be at least 2'
            stop
         ! Have asked for two points--which are the end points we've supplied!
         else if (npoints == 2) then
            write(*,'(a)') &
            'spherical_geometry: gcp_points: Warning: have asked for only two points on path, so returning end points'
            ptslon(2) = x2
            ptslat(2) = y2
            ptslon(1:2) = ptslon(1:2) / conversion
            ptslat(1:2) = ptslat(1:2) / conversion
            return
         endif
         ! Calculate distance between points
         ddelta = d/npoints
      endif

      ! Check we have room for all the points
      if (size(ptslon) < npoints .or. size(ptslat) < npoints) then
         write(*,'(a)') 'spherical_geometry: gcp_points: Error: arrays to hold lon and lat points are not long enough'
         stop
      endif

      ! Fill in points
      ptslon(1) = x1
      ptslat(1) = y1
      do i=2,npoints-1
         call step(x1,y1,azi,real(i-1)*ddelta,ptslon(i),ptslat(i),degrees=.false.)
      enddo
      ptslon(npoints) = x2
      ptslat(npoints) = y2

      ! Convert back
      ptslon(1:npoints) = ptslon(1:npoints) / conversion
      ptslat(1:npoints) = ptslat(1:npoints) / conversion

   end subroutine gcp_points
!===============================================================================

!------------------------------------------------------------------------------
   function azimuth(lon1,lat1,lon2,lat2,degrees)
!  Returns azimuth from point 1 to point 2.
!  From: http://www.movable-type.co.uk/scripts/latlong.html

     implicit none

     real(rs) :: azimuth,lon1,lat1,lon2,lat2
     real(rs) :: rlon1,rlat1,rlon2,rlat2,conversion
     logical,optional :: degrees

     conversion = 1._rs
     if (present(degrees)) then
        if (degrees) conversion = pi/180._rs
     endif

     rlon1 = conversion*lon1  ;  rlon2 = conversion*lon2
     rlat1 = conversion*lat1  ;  rlat2 = conversion*lat2

     azimuth = atan2(sin(rlon2-rlon1)*cos(rlat2) , &
               cos(rlat1)*sin(rlat2) - sin(rlat1)*cos(rlat2)*cos(rlon2-rlon1) )

     if (azimuth < 0) then
        azimuth = azimuth+2._rs*pi
     endif

     azimuth = azimuth / conversion

!     write(*,*)'Azimuth',azimuth

     return

   end function azimuth
!==============================================================================

!------------------------------------------------------------------------------
   subroutine geog2cart(phi_in,theta_in,r,x,y,z,degrees)
!  Returns the cartesian coordinates from geographical ones
!  Theta is latitude, phi is longitude and r is radius

      implicit none

      real(rs),intent(in)  :: theta_in,phi_in,r
      real(rs),intent(out) :: x,y,z
      real(rs)             :: theta,phi,conversion
      logical,optional,intent(in)  :: degrees

      conversion = 1._rs
      if (present(degrees)) then
         if (degrees) conversion = pi/180._rs
      endif
      theta = theta_in * conversion
      phi = phi_in * conversion

      if (theta < -pi/2._rs .or. theta > pi/2._rs) then
         write(*,'(a)') 'Latitude must be in range -pi/2 - pi/2 (-90 - 90 deg).'
         stop
      endif

      x = r * sin(pi/2._rs - theta) * cos(phi)
      y = r * sin(pi/2._rs - theta) * sin(phi)
      z = r * cos(pi/2._rs - theta)

      return

   end subroutine geog2cart
!==============================================================================

!------------------------------------------------------------------------------
   subroutine sph2cart(phi_in,theta_in,r,x,y,z,degrees)
!  Returns the cartesian coordinates from spherical ones
!  Theta is colatitude, phi is longitude and r is radius

      implicit none

      real(rs),intent(in)  :: theta_in,phi_in,r
      real(rs),intent(out) :: x,y,z
      real(rs)             :: theta,phi
      logical,optional,intent(in)  :: degrees

      if (present(degrees)) then
         if (degrees) then
            theta = theta_in * pi / 1.8d2
            phi = phi_in * pi / 1.8d2
         else
            theta = theta_in
            phi = phi_in
         endif
      else
         theta = theta_in
         phi = phi_in
      endif

       if (theta < 0.d0 .or. theta > pi ) then
          write(*,'(a)') 'Colatitude must be in range 0--pi (0--180deg).'
          stop
       endif

       x = r * sin(theta) * cos(phi)
       y = r * sin(theta) * sin(phi)
       z = r * cos(theta)

       return

   end subroutine sph2cart
!==============================================================================

!------------------------------------------------------------------------------
   subroutine cart2geog(x,y,z,theta,phi,r,degrees)
!  Returns the geographic coordinates from cartesian ones.

   implicit none

   real(rs),intent(in)  :: x,y,z
   real(rs),intent(out) :: theta,phi,r
   real(rs)             :: t,p,r_temp
   logical,optional     :: degrees

   r_temp = sqrt(x**2 + y**2 + z**2)

   t = acos(z/r_temp)
   p = atan2(y,x)

   r = r_temp

   if (present(degrees)) then
      if (degrees) then
         theta = 90.d0 - t * 1.8d2/pi
         phi   = p   * 1.8d2/pi
      else
         theta = pi/2.d0 - t
         phi = p
      endif
   else
      theta = pi/2.d0 - t
      phi = p
   endif

   return

   end subroutine cart2geog
!==============================================================================

!------------------------------------------------------------------------------
   subroutine cart2sph(x,y,z,theta,phi,r,degrees)
!  Returns the spherical coordinates from cartesian ones.

   implicit none

   real(rs),intent(in)  :: x,y,z
   real(rs),intent(out) :: theta,phi,r
   real(rs)             :: t,p,r_temp
   logical,optional     :: degrees

   r_temp = sqrt(x**2 + y**2 + z**2)

   t = acos(z/r_temp)
   p = atan2(y,x)

   r = r_temp

   if (present(degrees)) then
      if (degrees) then
         theta = t * 1.8d2/pi
         phi   = p   * 1.8d2/pi
      else
         theta =  t
         phi = p
      endif
   else
      theta = t
      phi = p
   endif

   return

   end subroutine cart2sph
!==============================================================================

!------------------------------------------------------------------------------
   function inclination(r_in,lon_in,lat_in,degrees)
!  Give the inclination of a vector in cartesian coordinates, given
!  the latitude and longitude.
!  Inclination is measured away from the Earth radial direction, hence
!  0 for an upward, vertical ray, 90° for a horizontal ray, 180° for a downward,
!  vertical ray

     implicit none

     real(rs),intent(in)   :: r_in(3),lon_in,lat_in
     real(rs)              :: inclination
     real(rs)              :: r(3),lon,lat,radial(3),conversion,dot
     logical,intent(in),optional :: degrees

!  Convert to radians if necessary
     if (present(degrees)) then
       if (degrees) conversion = pi/1.8d2
       if (.not.degrees) conversion = 1.d0
     else
       conversion = 1.d0
     endif
     lon = conversion * lon_in ; lat = conversion * lat_in

!  Create the (unit) cartesian vector along the Earth radial direction
     radial(1) = cos(lat)*cos(lon)
     radial(2) = cos(lat)*sin(lon)
     radial(3) = sin(lat)

!  Make r into unit vector
     r = r_in / sqrt(r_in(1)**2 + r_in(2)**2 + r_in(3)**2)

!  Compute the dot product and the inclination
     dot = r(1)*radial(1) + r(2)*radial(2) * r(3)*radial(3)
     inclination = abs(acos(dot))

     if (inclination > pi/2.d0) inclination = pi - inclination

     inclination = inclination / conversion
!      write(*,*)'Inclination',inclination

     return

   end function inclination
!==============================================================================

!-------------------------------------------------------------------------------
   function xyz2radial(x,y,z)
!  Given Cartesian coordinates of convention
!     1 goes through (0,0)
!     2 goes through (90E,0)
!     3 goes through N pole,
!  produce the Earth radial direction in Cartesian coordinates

      implicit none
      real(rs),intent(in) :: x,y,z
      real(rs)            :: xyz2radial(3), r

      r = sqrt(x**2 + y**2 + z**2)
      xyz2radial(1) = x / r
      xyz2radial(2) = y / r
      xyz2radial(3) = z / r

      return
   end function xyz2radial
!===============================================================================

!-------------------------------------------------------------------------------
   function lonlat2radial(lon,lat,degrees)
!  Given a longitude and latitude, give the Earth radial direction in the standard
!  Cartesian reference system (see e.g. xyz2radial)
!  Default is input in radians: override with degrees=.true.
      implicit none
      real(rs),intent(in) :: lon,lat
      real(rs)            :: lonlat2radial(3),x,y,z,r
      logical,optional,intent(in) :: degrees
      logical                     :: degrees_in

!  Check for input in degrees and pass on as appropriate
      degrees_in = .false.
      if (present(degrees)) degrees_in = degrees

      r = 1000._rs ! Dummy radius
      call geog2cart(lon, lat, r, x, y, z, degrees=degrees_in)
      lonlat2radial = xyz2radial(x,y,z)

      return
   end function lonlat2radial
!===============================================================================

!-------------------------------------------------------------------------------
   subroutine sg_sphere_sample(d,lon_out,lat_out,n_out)
!  Evenly sample a sphere given an input distance d between adjacent points.
!  Points are in longitude range -180 to 180.
!  lon and lat are column vectors which are assigned within the subroutine.

      implicit none

      real(rs),intent(in)  :: d
      real(rs),allocatable,intent(out) :: lon_out(:), lat_out(:)
      integer,intent(out)  :: n_out
      integer,parameter    :: nmax=50000
      real(rs)             :: lon(nmax),lat(nmax)
      real(rs)             :: dlon,dlat,dlon_i,lon_i,lat_i
      integer              :: i,n,n_i

      n = 1

      dlat = d
      dlon = dlat  ! At the equator

      lat(n)=90.; lon(n)=0.

      lon_i = 0.
      lat_i = lat(1) - dlat
      do while (lat_i > -90.)
         dlon_i = dlon/sin((90.-lat_i)*pi/180.)
         n_i = nint(360./dlon_i)
         do i=1,n_i
            n = n + 1
            if (n > nmax) then
               write(0,'(a)') 'sphere_sample: number of points greater than nmax',&
                              '  Change compiled limits or increase point spacing d.'
               stop
            endif
            lat(n) = lat_i
            lon(n) = lon_i
            lon_i = modulo(lon_i + dlon_i, 360.)
         enddo
         lon_i = modulo(lon_i + dlon_i, 360.)
         lat_i = lat_i - dlat
      enddo

      n = n + 1
      lat(n)=-90. ; lon(n) = 0.

      if (allocated(lon_out)) then
         if (size(lon_out) /= n) then
            deallocate(lon_out)
            allocate(lon_out(n))
         endif
      else
         allocate(lon_out(n))
      endif

      if (allocated(lat_out)) then
         if (size(lat_out) /= n) then
            deallocate(lat_out)
            allocate(lat_out(n))
         endif
      else
         allocate(lat_out(n))
      endif

      lon_out(1:n) = mod(lon(1:n) + 180., 360.) - 180.
      lat_out(1:n) = lat(1:n)
      n_out = n

      return

    end subroutine sg_sphere_sample
!===============================================================================

!-------------------------------------------------------------------------------
   subroutine sphere_sample(d,lon_out,lat_out,n_out)
! Deprecated synonym for sg_sphere_sample: we provide this wrapper subroutine to
! for backwards compatibility.
      implicit none
      real(rs), intent(in) :: d
      real(rs), intent(out), allocatable, dimension(:) :: lon_out, lat_out
      integer, intent(out) :: n_out
      call sg_sphere_sample(d,lon_out,lat_out,n_out)
   end subroutine sphere_sample
!===============================================================================

!-------------------------------------------------------------------------------
   function sph_poly_inout(x,y,px,py,degrees)
!  Takes in assumed-shape arrays (vectors) for points on a sphere, which must be
!  ordered either clockwise or anticlockwise.  The function assumes that the first
!  and last points are not the same, but this doesn't matter anyway.
!  x,y:         trial point in lon,lat
!  px(:),py(:): polygon vertices in lon,lat
!
!  NOTE: This algorithm won't work for sample points on the north or south poles,
!        because the azimuths will always be 0 or 180, and hence the total will
!        always be zero.  This can be alleviated by implementing the algorithm
!        described in:
!                  Schettino (1999). Polygon intersections in spherical
!        topology: applications to plate tectonics.  Computers & Geosciences, 25
!        (1) 61-69. doi:10.1016/S0098-3004(98)00081-8

      implicit none

      real(rs),intent(in) :: x,y
      real(rs),intent(in),dimension(:) :: px,py
      logical,intent(in),optional :: degrees
      logical :: sph_poly_inout
      logical :: deg
      real(rs) :: conversion, s, a0, a1, da, tx, ty, tpx0, tpy0, tpx1, tpy1
      integer :: i,n
      real(rs),parameter :: tol = 1._rs  ! Tolerance in *degrees*

!  Check for same size arrays
      if (size(px) /= size(py)) then
         write(0,'(a)') &
'spherical_geometry: sph_poly_inout: Error: polygon coordinate vectors must be same length.'
         stop
      endif

!  Check for degrees/radians
      deg = .false.
      conversion = 1._rs
      if (present(degrees)) then
         deg = degrees
         if (degrees) conversion = pi/180._rs
      endif

      tx = conversion*x
      ty = conversion*y

!  Check for point on vertex
      if (any(x == px .and. y == py)) then
         write(0,'(a)') 'spherical_geometry: sph_poly_inout: point is on vertex.'
         stop
      endif

!  Check for point on poles
      if (y == 90. .or. y == -90.) then
         write(0,'(a)') 'spherical_geometry: sph_poly_inout: point is on one of the poles.'
         stop
      endif

!  Loop over sides and calculate sum of angles.  If ~360, inside.  If ~0, outside
      n = size(px)
      s = 0.
      do i = 1,n-1
         tpx0 = conversion*px(i)
         tpy0 = conversion*py(i)
         tpx1 = conversion*px(i+1)
         tpy1 = conversion*py(i+1)
         a0 = azimuth(tx,ty,tpx0,tpy0,degrees=.true.)
         a1 = azimuth(tx,ty,tpx1,tpy1,degrees=.true.)
         da = a1 - a0
         do while (da > 180._rs)
            da = da - 360._rs
         enddo
         do while (da < -180._rs)
            da = da + 360._rs
         enddo
         s = s + da
      enddo
!  Calculate difference between last and first.  da == 0 if given a closed set of points.
      tpx0 = conversion*px(n)
      tpy0 = conversion*py(n)
      tpx1 = conversion*px(1)
      tpy1 = conversion*py(1)
      a0 = azimuth(tx,ty,tpx0,tpy0,degrees=.true.)
      a1 = azimuth(tx,ty,tpx1,tpy1,degrees=.true.)
      da = a1 - a0
      do while (da > 180._rs)
         da = da - 360._rs
      enddo
      do while (da < -180._rs)
         da = da + 360._rs
      enddo
      s = s + da
!      write(*,*) s

!  Test for in or out
      if (360._rs - abs(s) <= tol) then
         sph_poly_inout = .true.
      else
         sph_poly_inout = .false.
      endif

      return
   end function sph_poly_inout
!===============================================================================

!===============================================================================
function sg_torad(a)
!===============================================================================
!  Convert from degrees to radians
   implicit none
   real(rs), intent(in) :: a
   real(rs) :: sg_torad
   sg_torad = a*to_rad
end function sg_torad
!-------------------------------------------------------------------------------

!===============================================================================
function sg_todeg(a)
!===============================================================================
!  Convert from radians to degrees
   implicit none
   real(rs), intent(in) :: a
   real(rs) :: sg_todeg
   sg_todeg = a*to_deg
end function sg_todeg
!-------------------------------------------------------------------------------

!===============================================================================
subroutine sg_project_to_gcp(long,latg,lonp,latp,lon,lat,degrees)
!===============================================================================
!  Using the pole to a great circle path about a sphere with coordinates long,
!  latg, project a point onto that path (point at lonp,latp)
!  Input is assumed to be in radians.
   implicit none
   real(rs), intent(in) :: long,latg,lonp,latp
   real(rs), intent(out) :: lon,lat
   logical, optional, intent(in) :: degrees
   real(rs), dimension(3) :: g,p,gp,pp
   real(rs) :: r
   logical :: deg

   deg = .false.
   if (present(degrees)) deg = degrees
   ! Convert to vectors
   g = sg_lonlat2vec(long,latg,degrees=deg)
   p = sg_lonlat2vec(lonp,latp,degrees=deg)

   ! Check whether the point already lies on the gcp
   if (abs(dot_product(g,p)) < angle_tol) then
      call cart2geog(p(1),p(2),p(3),lat,lon,r,degrees=deg)
      return
   endif

   ! Compute the pole to the gcp containing g and the point
   gp = sg_cross_prod(g,p)

   ! Check for the point and the pole being the same--we can't handle this
   if (sqrt(gp(1)**2 + gp(2)**2 + gp(3)**2) < angle_tol) then
      write(0,'(a)') 'spherical_geometry: sg_project_to_gcp: Error: point and pole to plane are the same'
      stop
   endif

   gp = gp/sqrt(sum(gp**2))  ! Normalise to unit vector
   pp = sg_cross_prod(gp,g)
   ! Have to swap the sign of the point if the angle between the starting and
   ! projected point is more than 90 deg.  Everything is a unit vector.
   if (acos(dot_product(p,pp)) > pi2) pp = -pp

   call cart2geog(pp(1),pp(2),pp(3),lat,lon,r,degrees=deg)

end subroutine sg_project_to_gcp
!-------------------------------------------------------------------------------

!===============================================================================
function sg_cross_prod(a,b) result(c)
!===============================================================================
!  Compute the cross product for two three-vectors.
   implicit none
   real(rs), dimension(3), intent(in) :: a,b
   real(rs) :: c(3)
   c = (/a(2)*b(3) - a(3)*b(2),  a(3)*b(1) - a(1)*b(3),  a(1)*b(2) - a(2)*b(1)/)
end function sg_cross_prod
!-------------------------------------------------------------------------------

!===============================================================================
function sg_lonlat2vec(lon,lat,degrees) result(v)
!===============================================================================
!  Convert a longitude and latitude on a unit sphere to a vector.
   implicit none
   real(rs), intent(in) :: lon,lat
   logical, optional, intent(in) :: degrees
   real(rs) :: r,x,y,z,v(3)
   logical :: deg
   deg = .false.
   if (present(degrees)) deg = degrees
   r = 1._rs
   call geog2cart(lon,lat,r,x,y,z,degrees=deg)
   v = (/x, y, z/)
end function sg_lonlat2vec
!-------------------------------------------------------------------------------

!===============================================================================
subroutine sg_gcp_from_point_azi(lon,lat,azi,lonp,latp,degrees)
!===============================================================================
!  Given a point on a unit sphere and and azimuth from that point, return the
!  pole to the great cricle path created by that point and azimuth
   implicit none
   real(rs), intent(in) :: lon,lat,azi
   real(rs), intent(out) :: lonp,latp
   logical, optional, intent(in) :: degrees
   logical :: deg
   real(rs), dimension(3) :: pstart, pstep, gp
   real(rs) :: r,conversion
   real(rs), parameter :: step_dist = 45._rs

   deg = .false.
   if (present(degrees)) deg = degrees
   conversion = 1._rs
   if (deg) conversion = to_rad
   ! Convert to a vector
   pstart = sg_lonlat2vec(lon, lat, degrees=deg)
   ! Find a point along the gcp
   call step(lon, lat, azi, step_dist*conversion, lonp, latp, degrees=deg)
   pstep = sg_lonlat2vec(lonp,latp,degrees=deg)
   ! Find the pole to the two points
   gp = sg_cross_prod(pstart,pstep)
   call cart2geog(gp(1),gp(2),gp(3),latp,lonp,r,degrees=deg)

end subroutine sg_gcp_from_point_azi
!-------------------------------------------------------------------------------

!===============================================================================
subroutine sg_gcp_from_points(lon1,lat1,lon2,lat2,lonp,latp,degrees)
!===============================================================================
!  Given two points on a sphere (geographic coordinates), return the coordinates
!  of the pole to the great circle containing the two points.
   implicit none
   real(rs), intent(in) :: lon1,lat1,lon2,lat2
   real(rs), intent(out) :: lonp,latp
   logical, optional, intent(in) :: degrees
   logical :: deg
   real(rs), dimension(3) :: p1,p2,g
   real(rs) :: r,conversion

   deg = .false.
   if (present(degrees)) deg = degrees
   conversion = 1._rs
   if (deg) conversion = to_rad
   ! Check the points don't overlap
   if (conversion*abs(lon1-lon2) < angle_tol .and. conversion*abs(lat1-lat2) < angle_tol) then
      write(0,'(a)') 'spherical_geometry: sg_gcp_from_points: Error: two points overlap'
      stop
   endif
   ! Check the points aren't antipodal
   if (abs(delta(lon1,lat1,lon2,lat2,degrees=deg) - pi/conversion) < angle_tol) then
      write(0,'(a)') 'spherical_geometry: sg_gcp_from_points: Error: two points are antipodal'
      stop
   endif
   ! Convert the coordinates to vectors and compute the cross product, which
   ! gives the pole to the great circle
   p1 = sg_lonlat2vec(lon1,lat1,degrees=deg)
   p2 = sg_lonlat2vec(lon2,lat2,degrees=deg)
   g = sg_cross_prod(p1,p2)
   g = g/sqrt(sum(g**2))
   call cart2geog(g(1),g(2),g(3),latp,lonp,r,degrees=deg)

end subroutine sg_gcp_from_points
!-------------------------------------------------------------------------------

!===============================================================================
function sg_gcp_to_azimuth(long,latg,lonp,latp,degrees) result(azi)
!===============================================================================
!  Calculate the local azimuth of the great circle defined by pole (long,latg)
!  at a point (lonp,latp).  Default is for input/output in radians; use
!  degrees=.true. for azimuth in degrees.
   implicit none
   real(rs), intent(in) :: long,latg,lonp,latp
   real(rs) :: azi
   logical, optional, intent(in) :: degrees
   real(rs) :: lon,lat,conversion

   conversion = 1._rs
   if (present(degrees)) then
      if (degrees) conversion = to_rad
   endif
   ! Check values
   if (latg < -pi2/conversion .or. latg > pi2/conversion .or. &
       latp < -pi2/conversion .or. latp > pi2/conversion) then
      write(0,'(a)') 'spherical_geometry: sg_gcp_to_azimuth: Error: '// &
         'latitude must be in range -pi/2 to pi/2 (-90 to 90 deg)'
      stop
   endif
   ! Project the point onto the great circle
   call sg_project_to_gcp(long,latg,lonp,latp,lon,lat,degrees=degrees)
   ! The local azimuth is pi/2 (90 deg) from the azimuth to the pole, given
   ! in the range 0-pi or 0-360
   azi = modulo(azimuth(lon,lat,long,latg,degrees=degrees) + pi2/conversion + &
         twopi/conversion, twopi/conversion)
end function sg_gcp_to_azimuth
!-------------------------------------------------------------------------------

!===============================================================================
function sg_angle_180(a) result(angle)
!===============================================================================
!  Make an angle be in the range -180 to 180 deg
   implicit none
   real(rs), intent(in) :: a
   real(rs) :: angle
   angle = modulo(a+180._rs,360._rs) - 180._rs
end function sg_angle_180
!-------------------------------------------------------------------------------

!===============================================================================
function sg_angle_360(a) result(angle)
!===============================================================================
!  Make an angle be in the range 0 to 360 deg
   implicit none
   real(rs), intent(in) :: a
   real(rs) :: angle
   angle = modulo(a,360._rs)
end function sg_angle_360
!-------------------------------------------------------------------------------

!===============================================================================
function sg_angle_diff(a,b) result(diff)
!===============================================================================
!  Provide the difference between two angles, handling cases where they are
!  either side of 0/360 or -180/180
   implicit none
   real(rs), intent(in) :: a,b
   real(rs) :: diff
   diff = abs(sg_angle_360(b) - sg_angle_360(a))
   if (diff > 180._rs) diff = 360._rs - diff
end function sg_angle_diff
!-------------------------------------------------------------------------------

!===============================================================================
subroutine sg_initialise_rng(s)
!===============================================================================
! Initialise the random number generator.  This involves seeding the RNG with
! an array of integers of a minimum length.  This is 12 with gfortran 4.6.
! OPTIONAL: Supply an integer to mix in to the seed.  This may be useful when
! calling in parallel to make sure the PRNG state is initialised differently.
   implicit none
   integer, optional, intent(in) :: s
   integer :: values(8), n, i
   integer, dimension(:), allocatable :: seed
   if (.not. rng_initialised) then
      call random_seed(size=n)  ! Find minimum size of array used to seed
      allocate(seed(n))
      call date_and_time(values=values)
      if (present(s)) values(7) = s*values(7)
      seed = values(8) + values(7)*(/ (i-1, i=1,n) /)
      call random_seed(put=seed)
      deallocate(seed)
      rng_initialised = .true.
   endif
end subroutine sg_initialise_rng
!-------------------------------------------------------------------------------

!===============================================================================
subroutine sg_random_points_sph(lon, colat, degrees)
!===============================================================================
! Return a set of points on a sphere randomly distributed, in spherical
! coordinates.
! From: http://mathworld.wolfram.com/SpherePointPicking.html
! (NB: They use (to me) non-standard theta=lon and phi=colat!)
   implicit none
   real(rs), intent(out), dimension(:) :: lon, colat
   logical, optional, intent(in) :: degrees
   logical :: radians
   ! Check array lengths
   if (size(lon) /= size(colat)) then
      write(0,'(a)') 'spherical_geometry: sg_random_points_sph: Error: arrays ' &
         // 'for lon and colat are not the same length'
      stop
   endif
   radians = .true.
   if (present(degrees)) radians = .not.degrees
   call sg_initialise_rng
   call random_number(colat)
   colat = acos(2._rs*colat - 1._rs)
   call random_number(lon)
   lon = twopi*lon

   if (.not.radians) then
      lon = to_deg*lon
      colat = to_deg*colat
   endif
end subroutine sg_random_points_sph
!-------------------------------------------------------------------------------

!===============================================================================
subroutine sg_random_points_geog(lon, lat, degrees)
!===============================================================================
! Return a set of points on a sphere randomly distributed, in geographical
! coordinates.
   implicit none
   real(rs), intent(out), dimension(:) :: lon, lat
   logical, optional :: degrees
   logical :: radians
   radians = .true.
   if (present(degrees)) radians = .not.degrees
   call sg_random_points_sph(lon, lat)
   lat = pi2 - lat
   if (.not.radians) then
      lon = to_deg*lon
      lat = to_deg*lat
   endif
end subroutine sg_random_points_geog
!-------------------------------------------------------------------------------

!===============================================================================
subroutine sg_random_point_sph(lon, colat, degrees)
!===============================================================================
   implicit none
   real(rs), intent(out) :: lon, colat
   logical, optional, intent(in) :: degrees
   logical :: radians
   real(rs), dimension(1) :: alon, acolat
   radians = .true.
   if (present(degrees)) radians = .not.degrees
   call sg_random_points_sph(alon, acolat, degrees=.not.radians)
   lon = alon(1)
   colat = acolat(1)
end subroutine sg_random_point_sph
!-------------------------------------------------------------------------------

!===============================================================================
subroutine sg_random_point_geog(lon, lat, degrees)
!===============================================================================
   implicit none
   real(rs), intent(out) :: lon, lat
   logical, optional, intent(in) :: degrees
   logical :: radians
   real(rs), dimension(1) :: alon, alat
   radians = .true.
   if (present(degrees)) radians = .not.degrees
   call sg_random_points_geog(alon, alat, degrees=.not.radians)
   lon = alon(1)
   lat = alat(1)
end subroutine sg_random_point_geog
!-------------------------------------------------------------------------------

!===============================================================================
function sg_triangle_area(lon1i, lat1i, lon2i, lat2i, lon3i, lat3i, r, degrees, quiet)
!===============================================================================
! Return the area of a spherical triangle defined by the three geographic
! coordinates.
! Default is for input in radians; use degrees=.true. for degrees.
! INPUT:
!    {lon,lat}{1,2,3} : Coordinates of corners of triangles [radians by default]
! INPUT (OPTIONAL):
!    r                : Radius of sphere.  Area is for unit sphere by default.
!    degrees          : Input coordinates are in radians by default; specify
!                       .true. for degrees input.
!    quiet            : If .true., do not complain with a warning about collinear
!                       points.
! OUTPUT              : Area of triangle.  Will be in units of r, which is 1
!                       by default.
!
! Note: where two sides of the triangle are similar in length (and the third
! is about half the other two), l'Huilier's formula becomes unstable.  To guard
! against that case, we use a different expression; see:
!   http://en.wikipedia.org/wiki/Spherical_trigonometry#Area_and_spherical_excess

   real(rs), intent(in) :: lon1i, lat1i, lon2i, lat2i, lon3i, lat3i
   real(rs), optional, intent(in) :: r
   logical, optional, intent(in) :: degrees, quiet
   real(rs) :: sg_triangle_area
   real(rs) :: lon1, lat1, lon2, lat2, lon3, lat3
   real(rs) :: arc1, arc2, r_in, az1, az2, c
   real(rs), parameter :: tol = 2._rs*tiny(0._rs)
   logical :: silent

   silent = .false.
   if (present(quiet)) silent = quiet

   r_in = 1._rs
   if (present(r)) r_in = r
   lon1 = lon1i
   lat1 = lat1i
   lon2 = lon2i
   lat2 = lat2i
   lon3 = lon3i
   lat3 = lat3i
   if (present(degrees)) then
      if (degrees) then
         lon1 = to_rad*lon1i
         lat1 = to_rad*lat1i
         lon2 = to_rad*lon2i
         lat2 = to_rad*lat2i
         lon3 = to_rad*lon3i
         lat3 = to_rad*lat3i
      endif
   endif
   ! Get lengths of adjacent sides to point 3
   arc1 = delta(lon2, lat2, lon3, lat3, degrees=.false.)/2._rs
   arc2 = delta(lon3, lat3, lon1, lat1, degrees=.false.)/2._rs

   ! Get the angle c which is subtended at point 3
   az1 = azimuth(lon3, lat3, lon1, lat1, degrees=.false.)
   az2 = azimuth(lon3, lat3, lon2, lat2, degrees=.false.)
   c = abs(az1 - az2)
   if (c > pi) c = twopi - c
   ! If the azimuths to the other two sides are the same or opposite, then
   ! the points are collinear
   if (abs(c) < tol .or. abs(pi - c) < tol) then
      sg_triangle_area = 0._rs
      if (silent) &
         write(0,'(a)') 'sg_triangle_area: Warning: points are collinear'
      return
   endif
   sg_triangle_area = 2._rs*atan(tan(arc1)*tan(arc2)*sin(c) &
      /(1._rs + tan(arc1)*tan(arc2)*cos(c)))*r_in**2
end function sg_triangle_area
!-------------------------------------------------------------------------------

!______________________________________________________________________________
end module spherical_geometry


*DECK SGLSS
      SUBROUTINE SGLSS (A, MDA, M, N, B, MDB, NB, RNORM, WORK, LW,
     +   IWORK, LIW, INFO)
C***BEGIN PROLOGUE  SGLSS
C***PURPOSE  Solve a linear least squares problems by performing a QR
C            factorization of the matrix using Householder
C            transformations.  Emphasis is put on detecting possible
C            rank deficiency.
C***LIBRARY   SLATEC
C***CATEGORY  D9, D5
C***TYPE      SINGLE PRECISION (SGLSS-S, DGLSS-D)
C***KEYWORDS  LINEAR LEAST SQUARES, LQ FACTORIZATION, QR FACTORIZATION,
C             UNDERDETERMINED LINEAR SYSTEMS
C***AUTHOR  Manteuffel, T. A., (LANL)
C***DESCRIPTION
C
C     SGLSS solves both underdetermined and overdetermined
C     LINEAR systems AX = B, where A is an M by N matrix
C     and B is an M by NB matrix of right hand sides. If
C     M.GE.N, the least squares solution is computed by
C     decomposing the matrix A into the product of an
C     orthogonal matrix Q and an upper triangular matrix
C     R (QR factorization). If M.LT.N, the minimal
C     length solution is computed by factoring the
C     matrix A into the product of a lower triangular
C     matrix L and an orthogonal matrix Q (LQ factor-
C     ization). If the matrix A is determined to be rank
C     deficient, that is the rank of A is less than
C     MIN(M,N), then the minimal length least squares
C     solution is computed.
C
C     SGLSS assumes full machine precision in the data.
C     If more control over the uncertainty in the data
C     is desired, the codes LLSIA and ULSIA are
C     recommended.
C
C     SGLSS requires MDA*N + (MDB + 1)*NB + 5*MIN(M,N) dimensioned
C     real space and M+N dimensioned integer space.
C
C
C   ******************************************************************
C   *                                                                *
C   *         WARNING - All input arrays are changed on exit.        *
C   *                                                                *
C   ******************************************************************
C     SUBROUTINE SGLSS(A,MDA,M,N,B,MDB,NB,RNORM,WORK,LW,IWORK,LIW,INFO)
C
C     Input..
C
C     A(,)          Linear coefficient matrix of AX=B, with MDA the
C      MDA,M,N      actual first dimension of A in the calling program.
C                   M is the row dimension (no. of EQUATIONS of the
C                   problem) and N the col dimension (no. of UNKNOWNS).
C
C     B(,)          Right hand side(s), with MDB the actual first
C      MDB,NB       dimension of B in the calling program. NB is the
C                   number of M by 1 right hand sides. Must have
C                   MDB.GE.MAX(M,N). If NB = 0, B is never accessed.
C
C
C     RNORM()       Vector of length at least NB.  On input the contents
C                   of RNORM are unused.
C
C     WORK()        A real work array dimensioned 5*MIN(M,N).
C
C     LW            Actual dimension of WORK.
C
C     IWORK()       Integer work array dimensioned at least N+M.
C
C     LIW           Actual dimension of IWORK.
C
C
C     INFO          A flag which provides for the efficient
C                   solution of subsequent problems involving the
C                   same A but different B.
C                   If INFO = 0 original call
C                      INFO = 1 subsequent calls
C                   On subsequent calls, the user must supply A, INFO,
C                   LW, IWORK, LIW, and the first 2*MIN(M,N) locations
C                   of WORK as output by the original call to SGLSS.
C
C
C     Output..
C
C     A(,)          Contains the triangular part of the reduced matrix
C                   and the transformation information. It together with
C                   the first 2*MIN(M,N) elements of WORK (see below)
C                   completely specify the factorization of A.
C
C     B(,)          Contains the N by NB solution matrix X.
C
C
C     RNORM()       Contains the Euclidean length of the NB residual
C                   vectors  B(I)-AX(I), I=1,NB.
C
C     WORK()        The first 2*MIN(M,N) locations of WORK contain value
C                   necessary to reproduce the factorization of A.
C
C     IWORK()       The first M+N locations contain the order in
C                   which the rows and columns of A were used.
C                   If M.GE.N columns then rows. If M.LT.N rows
C                   then columns.
C
C     INFO          Flag to indicate status of computation on completion
C                  -1   Parameter error(s)
C                   0 - Full rank
C                   N.GT.0 - Reduced rank  rank=MIN(M,N)-INFO
C
C***REFERENCES  T. Manteuffel, An interval analysis approach to rank
C                 determination in linear least squares problems,
C                 Report SAND80-0655, Sandia Laboratories, June 1980.
C***ROUTINES CALLED  LLSIA, ULSIA
C***REVISION HISTORY  (YYMMDD)
C   810801  DATE WRITTEN
C   890831  Modified array declarations.  (WRB)
C   890831  REVISION DATE from Version 3.2
C   891214  Prologue converted to Version 4.0 format.  (BAB)
C   920501  Reformatted the REFERENCES section.  (WRB)
C***END PROLOGUE  SGLSS
      DIMENSION A(MDA,*),B(MDB,*),RNORM(*),WORK(*)
      INTEGER IWORK(*)
C
C***FIRST EXECUTABLE STATEMENT  SGLSS
      RE=0.
      AE=0.
      KEY=0
      MODE=2
      NP=0
C
C     IF M.GE.N CALL LLSIA
C     IF M.LT.N CALL ULSIA
C
      IF(M.LT.N) GO TO 10
      CALL LLSIA(A,MDA,M,N,B,MDB,NB,RE,AE,KEY,MODE,NP,
     1            KRANK,KSURE,RNORM,WORK,LW,IWORK,LIW,INFO)
      IF(INFO.EQ.-1) RETURN
      INFO=N-KRANK
      RETURN
   10 CALL ULSIA(A,MDA,M,N,B,MDB,NB,RE,AE,KEY,MODE,NP,
     1            KRANK,KSURE,RNORM,WORK,LW,IWORK,LIW,INFO)
      IF(INFO.EQ.-1) RETURN
      INFO=M-KRANK
      RETURN
      END


program main
	USE Plotter
	implicit none
	!Для массива
	integer :: i
	!Массив интервалов исключения
	real,dimension(:,:),allocatable :: restr
	real :: phi,r,PI=DACOS(-1.D0)
	!Массив точек
	real,dimension(:,:),allocatable :: matrix
	phi = 0
	r = 0
	!Три оси - X,Y,Z и 11 точек
	allocate(matrix(3,8400))
	do i=1,8400
		phi = phi+PI/4096.
		r=(1+sin(phi))*(1-0.9*abs(sin(4*phi)))*(0.9+0.05*cos(phi))
		matrix(1,i) = cos(phi)*r*5
		matrix(2,i) = sin(phi)*r*5
		matrix(3,i) = 0.
	end do
	!3 пары координат и два ограничения
	allocate(restr(6,2))
	!По Х
	restr(1,1) = -0.5
	restr(2,1) = 0.5
	!По У - Одинаковые - игнорируются
	restr(3,1) = 0
	restr(4,1) = 0
	!По Z
	restr(5,1) = 0
	restr(6,1) = 0
	restr(1,2) = 0
	restr(2,2) = 0
	restr(3,2) = 0.75
	restr(4,2) = 1.5
	restr(5,2) = 0
	restr(6,2) = 0
	!Передаём размеры окна, массив точек, число точек, массив интервалов исключения, размер массива
	call DrawGraph(960,720,matrix,8400,restr,2)
end


c
c-----------------------------------------------------------------------
c subroutine: fftohm
c compute dft for real, n-point, odd harmonic sequences using an
c n/2 point fft
c odd harmonic means x(2*k)=0, all k where x(k) is the dft of x(m)
c note: index m is sequence index--not fortran index
c-----------------------------------------------------------------------
c
      subroutine fftohm(x, n)
      dimension x(1)
c
c x = real array which on input contains the first n/2 points of the
c     input
c     on output x contains the n/4 complex values of the odd
c     harmonics of the input--stored in the sequence re(x(1)),im(x(1)),
c     re(x(2)),im(x(2)),...
c ****note: x must be dimensioned to size n/2+2 for fft routine
c n = true size of x sequence
c
c first compute real(x(1)) and real(x(n/2-1)) separately
c also simultaneously multiply original sequence by sin(twopi*(m-1)/n)
c sin and cos are computed recursively
c
c
c for n = 2, assume x(1)=x0, x(2)=-x0, compute dft directly
c
      if (n.gt.2) go to 10
      x(1) = 2.*x(1)
      x(2) = 0.
      return
  10  twopi = 8.*atan(1.0)
      tpn = twopi/float(n)
c
c compute x1=real(x(1)) and x2=imaginary(x(n/2-1))
c x(n) = x(n)*4.*sin(twopi*(i-1)/n)
c
      t1 = 0.
c
c cosd and sind are multipliers for recursion for sin and cos
c cosi and sini are initial conditions for recursion for sin and cos
c
      cosd = cos(tpn*2.)
      sind = sin(tpn*2.)
      cosi = 1.
      sini = 0.
      no2 = n/2
      do 20 i=1,no2,2
        t = x(i)*cosi
        x(i) = x(i)*4.*sini
        temp = cosi*cosd - sini*sind
        sini = cosi*sind + sini*cosd
        cosi = temp
        t1 = t1 + t
  20  continue
c
c reset initial conditions (cosi,sini) for new recursion
c
      cosi = cos(tpn)
      sini = sin(tpn)
      t2 = 0.
      do 30 i=2,no2,2
        t = x(i)*cosi
        x(i) = x(i)*4.*sini
        temp = cosi*cosd - sini*sind
        sini = cosi*sind + sini*cosd
        cosi = temp
        t2 = t2 + t
  30  continue
      x1 = 2.*(t1+t2)
      x2 = 2.*(t1-t2)
c
c take n/2 point (real) fft of preprocessed sequence x
c
      call fast(x, no2)
c
c for n = 4--skip recursion and initial conditions
c
      if (n.eq.4) go to 50
c
c initial conditions for recursion
c
      x(2) = -x(1)/2.
      x(1) = x1
c
c for n = 8, skip recursion
c
      if (n.eq.8) go to 50
c
c unscramble y(k) using recursion formula
c
      nind = no2 - 2
      do 40 i=3,nind,2
        t = x(i)
        x(i) = x(i-2) + x(i+1)
        x(i+1) = x(i-1) - t
  40  continue
  50  x(no2) = x(no2+1)/2.
      x(no2-1) = x2
      return
      end


      SUBROUTINE getiota(phipog, bsupu, bsupv)
      USE vmec_main
      USE realspace
      IMPLICIT NONE
C-----------------------------------------------
C   D u m m y   A r g u m e n t s
C-----------------------------------------------
      REAL(rprec), DIMENSION(nrzt), INTENT(in) :: phipog, bsupv
      REAL(rprec), DIMENSION(nrzt), INTENT(inout) :: bsupu
C-----------------------------------------------
C   L o c a l   P a r a m e t e r s
C-----------------------------------------------
      REAL(rprec), PARAMETER :: p5=0.5_dp, c1p5=1.5_dp
C-----------------------------------------------
C   L o c a l   V a r i a b l e s
C-----------------------------------------------
      INTEGER :: js, l
      REAL(rprec) :: top, bot
C-----------------------------------------------

      IF (ncurr .eq. 0) RETURN
      DO js = 2, ns
         top = icurv(js)                                !!Integrated toroidal current
         bot = 0
         DO l = js, nrzt, ns
            top = top - wint(l)*(guu(l)*bsupu(l) + guv(l)*bsupv(l))
            bot = bot + wint(l)*phipog(l)*guu(l)
         END DO
         iotas(js) = top/bot
      END DO

!     Do not compute iota too near origin
      iotaf(1)  = c1p5*iotas(2) - p5*iotas(3)           !!zero gradient near axis
!      iotaf(ns) = c1p5*iotas(ns) - p5*iotas(ns-1)
!      DO js = 2, ns-1
!         iotaf(js) = p5*(iotas(js) + iotas(js+1))
!      END DO
      DO js = 2, ns
         iotaf(js) = 2*iotas(js) - iotaf(js-1)
      END DO

      END SUBROUTINE getiota


!**
!* This module provides the EGM solution algorithm to solve non-concave dynamic programming models.
!**
module ncegm
    use kinds, only: dp
    use iso_fortran_env, only: error_unit
    use nderiv, only: linderiv
    use interpolation, only: interpolate_ordered
    use newton, only: root
    implicit none
    private
    public :: ncegm_setup,ncegm_model,ncegm_solve
    public :: ncegm_getPolicy_c,ncegm_getPolicy_aprime,ncegm_getPolicy_d,ncegm_getValueFunction
    public :: ncegm_getConditionalPolicy_c,ncegm_getConditionalPolicy_aprime,ncegm_getConditionalValueFunction
    save

    ! *******************************************************************************************************
    ! ** Types and interfaces
    ! *******************************************************************************************************
    interface
        function ReturnFunction(c,d,s,z) result(r)
            use kinds, only: dp
            implicit none
            real(dp), dimension(:), intent(in) :: c
            real(dp), intent(in)               :: d,s,z
            real(dp), dimension(size(c))       :: r
        end function
        function ReturnFunctionMarginalInverse(mret,d,s,z) result(c)
            use kinds, only: dp
            implicit none
            real(dp), intent(in)               :: mret,d,s,z
            real(dp)                           :: c
        end function
        function BudgetConstraint(a,d,s,z) result(m)
            use kinds, only: dp
            implicit none
            real(dp), dimension(:), intent(in) :: a
            real(dp), intent(in)               :: d,s,z
            real(dp), dimension(size(a))       :: m
        end function
        function StateTransition(s_index,d_index,z_index) result(s_prime_index)
            use kinds, only: dp
            implicit none
            integer, intent(in)                :: s_index, d_index, z_index
            integer                            :: s_prime_index
        end function
    end interface

    interface F
        module procedure F_sc,F_arr
    end interface F
    interface dF
        module procedure dF_sc,dF_arr
    end interface dF
    interface d2F
        module procedure d2F_sc,d2F_arr
    end interface d2F

    type ncegm_model
        procedure(ReturnFunction), pointer, nopass                   :: F=>null(),dF=>null()                  ! REQUIRED: return function F and first derivative with respect to c Fc
        procedure(ReturnFunction), pointer, nopass                   :: d2F=>null()                           ! OPTIONAL: second derivative of F with respect to c Fcc. In case the marginal inverse is provided, this is optional.
        procedure(ReturnFunctionMarginalInverse), pointer, nopass    :: dF_inv=>null()                        ! OPTIONAL: inverse of marginal return function Fc'
        procedure(BudgetConstraint), pointer, nopass                 :: Lambda=>null()                        ! REQUIRED: budget constraint
        procedure(BudgetConstraint), pointer, nopass                 :: dLambda=>null()                       ! OPTIONAL: derivative of budget constraint with respect to a (if present: slight increase in computation speed and accuracy
        procedure(StateTransition), pointer, nopass                  :: Psi=>null()                           ! REQUIRED if s is used: Transition function from s_t to s{t+1}
        real(dp), dimension(:,:,:), allocatable                      :: V_initial                             ! REQUIRED: initial guess of the value function
        real(dp), dimension(:), allocatable                          :: a_grid,d_grid,&                       ! REQUIRED: grids for A, D
                                                                        s_grid,z_grid                         ! REQUIRED if s or z, respectively, is used.
        real(dp)                                                     :: beta=0                                ! REQUIRED: discount factor Beta in interval (0,1)
        real(dp), dimension(:,:), allocatable                        :: z_transition                          ! REQUIRED if z is used: transition matrix for random variable z
        logical                                                      :: state_independent_foc_and_f = .FALSE. ! OPTIONAL: are the first-order condition and F independent of the state variable? If set to .TRUE., then some optimizations are performed.
    end type ncegm_model

    ! *******************************************************************************************************
    ! ** Module constants
    ! *******************************************************************************************************

    integer, parameter                        :: MAX_ITER_DEFAULT = 10000               ! Default value for maximum amount of iterations
    real(dp), parameter                       :: EPSILON_DEFAULT = 0.00001_dp           ! Default value for convergence criterion epsilon
    integer, parameter                        :: STATUS_PRINT_INTERVAL_DEFAULT = 25     ! Print a status message every STATUS_PRINT_INTERVAL iteration. This is the default value.
    integer, parameter                        :: NO_STATE = -10

    ! *******************************************************************************************************
    ! ** Private model variables
    ! *******************************************************************************************************
    type(ncegm_model)                         :: model
    logical                                   :: is_setup = .FALSE., is_solved = .FALSE.
    real(dp), dimension(:,:,:), allocatable   :: valuef, policy_c, policy_aprime, policy_d
    real(dp), dimension(:,:,:,:), allocatable :: policy_c_con, policy_aprime_con, valuef_next_con
    real(dp), dimension(:,:,:,:), allocatable :: m_grid
    integer                                   :: glen_a,glen_s,glen_z,glen_d
    integer                                   :: max_iter,info_message_inverval
    real(dp)                                  :: epsilon

    contains

        ! *******************************************************************************************************
        ! ** Public subroutines to setup and solve the model
        ! *******************************************************************************************************

        ! **
        ! * Sets up and validates the input model. Note: this subroutine does not solve the model.
        ! * A further call to ncegm_solve is necessary to solve the model.
        ! *
        ! * Input:
        ! *     - dpmodel: the model to be solved
        ! *
        ! * Output:
        ! *     - status: a flag indicating if the model is setup properly. .FALSE. indicates that the model is invalid or the arrays could not be allocated.
        ! *
        ! **
        subroutine ncegm_setup(dpmodel, status)
            type(ncegm_model), intent(in) :: dpmodel
            logical, intent(out)          :: status

            status = .FALSE.
            ! Check if input model is valid
            if (.NOT. validate_model(dpmodel)) return

            ! Store model and create dummy grids if Z or S are not used
            model = dpmodel
            if (.NOT. allocated(model%s_grid)) then
                allocate(model%s_grid(1))
                model%s_grid=0.0_dp
                model%state_independent_foc_and_f = .TRUE.
            end if
            if (.NOT. allocated(model%z_grid)) then
                allocate(model%z_grid(1),model%z_transition(1,1))
                model%z_grid=0.0_dp
                model%z_transition=1.0_dp
            end if

            ! Compute grid sizes
            glen_a = size(model%a_grid)
            glen_s = size(model%s_grid)
            glen_z = size(model%z_grid)
            glen_d = size(model%d_grid)

            ! (Re)allocate all arrays
            if (allocated(valuef)) deallocate(valuef, policy_c, policy_aprime, policy_d, m_grid,policy_c_con, policy_aprime_con, valuef_next_con)
            allocate(valuef(glen_a,glen_s,glen_z), policy_c(glen_a,glen_s,glen_z), policy_aprime(glen_a,glen_s,glen_z), policy_d(glen_a,glen_s,glen_z), m_grid(glen_a,glen_d,glen_s,glen_z))
            allocate(policy_c_con(glen_a,glen_d,glen_s,glen_z), policy_aprime_con(glen_a,glen_d,glen_s,glen_z), valuef_next_con(glen_a,glen_d,glen_s,glen_z))

            ! Ready to solve model
            is_setup = .TRUE.
            status = .TRUE.
            is_solved = .FALSE.
        end subroutine ncegm_setup

        ! **
        ! * Starts the solution algorithm of the model set up by a previous call to ncegm_setup(). The algorithm terminates if a sufficient convergence is achieved
        ! * or if the maximum number of iterations is achieved. The input parameters allow to change parameters of the algorithm. The output parameter status can be used
        ! * to check if the value functions converged. All parameters are optional.
        ! *
        ! * Input:
        ! *     - max_iter_in (optional): The maximum number of value function iterations. If the value functions have not converged, the algorithm will stop after max_iter_in iterations.
        ! *     - epsilon_in (optional): The supremum norm of the difference of two subsequent value functions below which the convergence is deemed sufficient and the algorithm is stopped.
        ! *     - info_message_interval_in (optional): This value specifys the amount of iterations after which a status is printed to the output showing the current convergence.
        ! *
        ! * Output:
        ! *     - status (optional): a flag indicating if the value functions have converged
        ! **
        subroutine ncegm_solve(max_iter_in, epsilon_in, info_message_inverval_in, status)
            integer, intent(in), optional          :: max_iter_in,info_message_inverval_in
            real(dp), intent(in), optional         :: epsilon_in
            logical, intent(out), optional         :: status

            if (.NOT.is_setup) return

            ! Prepare configurations of the algorithm

            if (present(max_iter_in)) then
                max_iter = max_iter_in
            else
                max_iter = MAX_ITER_DEFAULT
            end if

            if (present(epsilon_in)) then
                epsilon = epsilon_in
            else
                epsilon = EPSILON_DEFAULT
            end if

            if (present(info_message_inverval_in)) then
                info_message_inverval = info_message_inverval_in
            else
                info_message_inverval = STATUS_PRINT_INTERVAL_DEFAULT
            end if

            ! Now solve the model
            call valfniteration(is_solved)
            if (present(status)) status = is_solved

        end subroutine ncegm_solve

        ! *******************************************************************************************************
        ! ** EGM core subroutines
        ! *******************************************************************************************************

        ! **
        ! * Performs the actual value function iteration.
        ! *
        ! * Output:
        ! *     - status (optional): a flag indicating if the value functions have converged
        ! **
        subroutine valfniteration(status)
            integer                                          :: index_a,index_d,index_s,index_z,index_s_prime,iter
            real(dp), dimension(glen_a,glen_s,glen_z)        :: valuef_next,dvaluef,dvaluef_next,exp_valuef,exp_dvaluef
            integer, dimension(glen_a,glen_s,glen_z)         :: d_index_choice
            real(dp)                                         :: sup_norm_diff
            real(dp), dimension(glen_a)                      :: m_end, c_end, valuef_end
            integer                                          :: k,d_max
            real(dp)                                         :: a,d,s,z,c,t1,t2
            logical, intent(out)                             :: status

            ! 1. Compute derivative of the initial value function and the m-grid
            valuef = model%V_initial

            do index_z=1, glen_z
                do index_s=1, glen_s
                    dvaluef(:,index_s,index_z) = linderiv(model%a_grid, valuef(:,index_s, index_z))
                    do index_d=1,glen_d
                        m_grid(:,index_d,index_s,index_z) = model%Lambda(model%a_grid,model%d_grid(index_d),model%s_grid(index_s),model%z_grid(index_z))
                    end do
                end do
            end do

            call cpu_time(t1)

            ! 2. Begin value function iteration
            do iter=1,max_iter
                ! 2.1. Calculate the discounted expected value V~(a,s,z) for all a in A, s in S and z in Z
                do index_z=1,glen_z
                    do index_s=1,glen_s
                        do index_a=1,glen_a
                            exp_valuef(index_a,index_s,index_z) = model%beta*dot_product(model%z_transition(index_z,:),valuef(index_a,index_s,:))
                            exp_dvaluef(index_a,index_s,index_z) = model%beta*dot_product(model%z_transition(index_z,:),dvaluef(index_a,index_s,:))
                        end do
                    end do
                end do

                ! 2.2. Perform egm maximization and interpolation step, given (d,s,z) or (d,z) if s is not required for FOCs
                do index_z=1,glen_z
                    z=model%z_grid(index_z)
                    do index_d=1,glen_d
                        d=model%d_grid(index_d)
                        ! In case the FOCs are state-independent, s' can be computed by Psi without knowing the current state s
                        if (model%state_independent_foc_and_f) then
                            if (associated(model%Psi)) then
                                index_s_prime = model%Psi(NO_STATE,index_d,index_z)
                            else
                                index_s_prime = 1 ! Case: no state variable is used --> dummy index
                            end if
                            call egm_maximize(exp_valuef(:,index_s_prime,index_z),exp_dvaluef(:,index_s_prime,index_z),d,0.0_dp,z,m_end,c_end,valuef_end,k)

                        end if

                        do index_s=1,glen_s
                            s=model%s_grid(index_s)
                            ! The FOC depends on the state; therefore, it has to be evaluated for every state
                            if (.NOT.model%state_independent_foc_and_f) then
                                if (associated(model%Psi)) then
                                    index_s_prime = model%Psi(index_s,index_d,index_z)
                                else
                                    index_s_prime = 1
                                end if
                                call egm_maximize(exp_valuef(:,index_s_prime,index_z),exp_dvaluef(:,index_s_prime,index_z),d,s,z,m_end,c_end,valuef_end,k)
                            end if

                            ! 4. Interpolate the value function, for all (d,s,z)
                            call egm_interpolate(d,s,z,m_grid(:,index_d,index_s,index_z),m_end(1:k),c_end(1:k),valuef_end(1:k),valuef_next_con(:,index_d,index_s,index_z),policy_c_con(:,index_d,index_s,index_z))

                            policy_aprime_con(:,index_d,index_s,index_z) = m_grid(:,index_d,index_s,index_z) - policy_c_con(:,index_d,index_s,index_z)
                        end do
                    end do
                end do

                ! 3. compute unconditional value function and and its derivative
                do index_z=1,glen_z
                    z = model%z_grid(index_z)
                    do index_s=1,glen_s
                        s = model%s_grid(index_s)
                        do index_a=1,glen_a
                            d_max = maxloc(valuef_next_con(index_a,:,index_s,index_z),1)
                            d_index_choice(index_a,index_s,index_z) = d_max
                            d = model%d_grid(d_max)
                            a = model%a_grid(index_a)
                            valuef_next(index_a,index_s,index_z) = valuef_next_con(index_a,d_max,index_s,index_z)
                            if (associated(model%dLambda)) then
                                c = policy_c_con(index_a,d_max,index_s,index_z)
                                dvaluef_next(index_a,index_s,index_z) = dLambda_sc(a,d,s,z)*dF(c,d,s,z)
                            end if
                        end do
                        if (.NOT. associated(model%dLambda)) then
                            dvaluef_next(:,index_s,index_z) = linderiv(model%a_grid, valuef_next(:,index_s, index_z))
                        end if
                    end do
                end do

                ! 4. Calculate sup norm distance between last and new value function
                sup_norm_diff = maxval(abs(valuef_next-valuef))/maxval(abs(valuef))

                ! 5. Update value function
                valuef = valuef_next
                dvaluef = dvaluef_next

                ! 6. Check for convergence
                if (sup_norm_diff < epsilon) exit

                ! 7. In case the value functions have not converged yet, print a status
                if (mod(iter,info_message_inverval)==0) then
                    call cpu_time(t2)
                    print '("Iteration #",I5," sup norm distance = ",E14.6," time=",F10.3,"s")',iter,sup_norm_diff,(t2-t1)
                    t1 = t2
                end if
            end do

            if (iter==max_iter+1) then
                print *, "Error: Value functions did not converge within ",max_iter, "iterations."
                status = .FALSE.
            else
                print *, "Sufficient convergence achieved in final iteration", iter, "with supremum norm distance of", sup_norm_diff
                ! Compute unconditional policy functions
                do index_z=1,glen_z
                    do index_s=1,glen_s
                        do index_a=1,glen_a
                            d_max = d_index_choice(index_a,index_s,index_z)
                            policy_c(index_a,index_s,index_z) = policy_c_con(index_a,d_max,index_s,index_z)
                            policy_aprime(index_a,index_s,index_z) = m_grid(index_a,d_max,index_s,index_z) - policy_c(index_a,index_s,index_z)
                            policy_d(index_a,index_s,index_z)=model%d_grid(d_max)
                        end do
                    end do
                end do
                status = .TRUE.
            end if

        end subroutine

        ! **
        ! * Subroutine that maximizes the Bellman maximand based on the EGM algorithm.
        ! *
        ! * Input:
        ! *     - exp_valuef: The discounted expected value of next period's state (a'). Is equal to V tilde.
        ! *     - exp_valuef: The derivative of the discounted expected value of next period's state (a'). This is equal to V' tilde.
        ! *     - d: The discrete choice d
        ! *     - s: The deterministic state s
        ! *     - z: The stochastic state z
        ! *
        ! * Output:
        ! *     - m_end: The endogenous gridpoints for m
        ! *     - c_end: The endogenous gridpoints for c
        ! *     - valuef_end: The value function for the endogenous gridpoints m and c
        ! *     - k: The number of endogenous gridpoints
        ! *
        ! * Note: Code closely follows implementation of Fella (2011). A Generalized Endogenous Grid Method for Non-Smooth and Non-Concave Problems
        ! * 	  (http://webspace.qmul.ac.uk/gfella/research/research.html)
        ! **
        subroutine egm_maximize(exp_valuef, exp_dvaluef, d, s, z, m_end, c_end, valuef_end, k)
            real(dp), dimension(glen_a), intent(in)            :: exp_valuef, exp_dvaluef
            real(dp), intent(in)                               :: d,s,z
            real(dp), dimension(glen_a), intent(out)           :: m_end, c_end, valuef_end
            integer, intent(out)                               :: k

            integer                                            :: index_a_prime, index_nc_a_min, index_nc_a_max, j, j_0, j_bar, max_loc
            integer,   dimension(glen_a)                       :: aprime_node

            real(dp)                                           :: bound, c_bar, exp_dval_down, exp_dval_up, v_bar, z_bar, z_temp, xguess, c_star
            real(dp), dimension(glen_a)                        :: val,c_temp
            logical, dimension(size(exp_valuef)-1)             :: mask

            index_nc_a_min = glen_a
            index_nc_a_max = 1
            exp_dval_up = 0.0_dp

            ! 1. Check for non-concavity and compute bounds of non-concave region

            ! Find discontinuities (upward jumps) in future marginal utility
            mask = (exp_dvaluef(2:glen_a)>exp_dvaluef(1:glen_a-1))
            if (count(mask)>0) then
                ! Non-concavity. Computes bounds of region.
                exp_dval_up = maxval(pack(exp_dvaluef(2:glen_a),mask))
                exp_dval_down = minval(pack(exp_dvaluef(1:glen_a-1),mask))
                if (count(exp_dvaluef>exp_dval_up)==0) then
                    index_nc_a_min=1
                else
                    index_nc_a_min = minloc(exp_dvaluef,1,exp_dvaluef>exp_dval_up)+1
                end if
                if (count(exp_dvaluef<exp_dval_down)==0) then
                    index_nc_a_max=glen_a
                else
                    index_nc_a_max = maxloc(exp_dvaluef,1,exp_dvaluef<exp_dval_down) - 1
                end if
            end if

            xguess = 1.d-1
            k = 0
            j_0 = 1
            j_bar = 0
            z_bar = 10.d6
            v_bar = exp_dval_up + 1.d-5

            do index_a_prime=1,glen_a
                if (index_a_prime<index_nc_a_min .OR. index_a_prime>index_nc_a_max) then
                    if (associated(model%dF_inv)) then
                        c_star = model%dF_inv(exp_dvaluef(index_a_prime),d,s,z)
                    else
                        c_star = root(dF_sc,d2F_sc,xguess,p2=d,p3=s,p4=z,offset_in = exp_dvaluef(index_a_prime), minx_in = 0.0_dp)
                    end if
                    k = k+1
                    c_end(k) = c_star
                    m_end(k) = c_star + model%a_grid(index_a_prime)
                    aprime_node(k) = index_a_prime
                    xguess = c_star
                else
                  if (k>0) then
                    c_bar =max(m_end(k)-model%a_grid(index_a_prime),1.d-5)
                    v_bar=dF(c_bar,d,s,z)
                    j_0 = aprime_node(k)
                  end if
                  bound = df(z_bar-model%a_grid(index_a_prime),d,s,z)

                  if (  (v_bar>exp_dvaluef(index_a_prime) .AND.  exp_dvaluef(index_a_prime).GE.bound) .OR. &
                      (exp_dvaluef(index_a_prime)<bound .AND. index_a_prime.GE.j_bar)  ) then !if_2

                    if (associated(model%dF_inv)) then
                        c_star = model%dF_inv(exp_dvaluef(index_a_prime),d,s,z)
                    else
                        c_star = root(dF_sc,d2F_sc,xguess,p2=d,p3=s,p4=z,offset_in = exp_dvaluef(index_a_prime), minx_in = 0.0_dp)
                    end if

                    z_temp = c_star + model%a_grid(index_a_prime)

                    do j = j_0,index_nc_a_max
                      c_temp(j) = z_temp - model%a_grid(j)
                      if (c_temp(j).LE.0 .AND. j>1) exit
                    end do
                    val(j_0:j-1) =  F(c_temp(j_0:j-1),d,s,z)+ exp_valuef(j_0:j-1)
                    max_loc = maxloc(val(j_0:j-1),1)+j_0-1
                    if (max_loc==index_a_prime) then
                      k = k+1
                      c_end(k) = c_star
                      m_end(k) = z_temp
                      aprime_node(k) = index_a_prime
                      xguess = c_star
                      z_bar = 10.d6
                    elseif (max_loc>index_a_prime) then
                      j_bar=max_loc
                      z_bar = z_temp
                    end if
                  end if
                end if
              end do

              valuef_end(1:k) = F(c_end(1:k),d,s,z) + exp_valuef(aprime_node(1:k))

              if (aprime_node(1)>1) then
                xguess = m_end(1)

                c_star = root(z_find_lowerbound,d_z_find_lowerbound,xguess,p2=d,p3=s,p4=z,p5=model%a_grid(aprime_node(1)),minx_in=model%a_grid(aprime_node(1)),offset_in=exp_valuef(aprime_node(1)) - exp_valuef(1))

                c_end = eoshift(c_end,-1)
                m_end = eoshift(m_end,-1)
                valuef_end = eoshift(valuef_end,-1)
                c_end(1) = c_star - 1.d-5
                m_end(1) = c_end(1) + model%a_grid(1)
                valuef_end(1) = F(c_end(1),d,s,z)+ exp_valuef(1)
                k = k + 1
              end if
        end subroutine

        ! **
        ! * Subroutine that interpolates the value function and the policy functions on the grid for A using the exogenous and the endogenous grid for m.
        ! *
        ! * Input:
        ! *     - d: The discrete choice d
        ! *     - s: The deterministic state s
        ! *     - z: The stochastic state z
        ! *     - m_exo: The exogenous gridpoints for m
        ! *     - m_end: The endogenous gridpoints for m
        ! *     - c_end: The endogenous gridpoints for c
        ! *     - valuef_end: The value function for the endogenous gridpoints m and c
        ! *
        ! * Output:
        ! *     - interpol_valuef: The interpolated value function (defined on grid_a, conditional on d)
        ! *     - interpol_conditional_policy_c: The interpolated policy function for c (conditional on d)
        ! *
        ! * Note: Code closely follows implementation of Fella (2011). A Generalized Endogenous Grid Method for Non-Smooth and Non-Concave Problems
        ! * 	  (http://webspace.qmul.ac.uk/gfella/research/research.html))
        ! **
        subroutine egm_interpolate(d,s,z,m_exo,m_end, c_end,valuef_end,interpol_valuef,interpol_conditional_policy_c)
            real(dp), dimension(:), intent(in)        :: m_end, c_end,valuef_end
            real(dp), intent(in)                      :: d,s,z
            real(dp), dimension(glen_a), intent(in)   :: m_exo
            real(dp), dimension(glen_a), intent(out)  :: interpol_valuef,interpol_conditional_policy_c
            integer, dimension(glen_a)                :: map
            real(dp)                                  :: f_c1

            f_c1 = F(c_end(1),d,s,z)
            ! Compute a mapping from the exogenous m-grid to the endogenous m-grid
            map = interpolate_ordered(m_end,m_exo)

            where (map==1) ! Constraint a'>=a_min is binding!
                where (m_exo < model%a_grid(1))
                    ! Since this case implies a nonpositive continuous choice c, and hence is infeasible, set c close to 0 and valuef as small as possible
                    interpol_conditional_policy_c = 1.d-7
                    interpol_valuef = -1.d7
                elsewhere
                    ! The constraint is binding and there is a feasible choice for c
                    interpol_conditional_policy_c = m_exo - model%a_grid(1)
                    ! Adjust the value computed function by the change in return. Note: the adjusted value function can only be less than or equal to V_end(1)
                    interpol_valuef = valuef_end(1) - f_c1 + min(F(interpol_conditional_policy_c,d,s,z),f_c1)
                end where
            elsewhere
                ! Calculate an interpolation of the consumption and value nodes on the exogenous grid of m
                interpol_conditional_policy_c = c_end(map-1)+(c_end(map)-c_end(map-1)) / (m_end(map)-m_end(map-1)) * (m_exo-m_end(map-1))
                interpol_valuef = valuef_end(map-1)+(valuef_end(map)-valuef_end(map-1)) / (m_end(map)-m_end(map-1)) * (m_exo-m_end(map-1))
            end where
        end subroutine

        ! *******************************************************************************************************
        ! ** Two callback functions used by egm_maximize to solve for the lower bound on m at and below which
        ! ** the constraint a >= a_min is binding
        ! *******************************************************************************************************
        !
        real(dp) function z_find_lowerbound(m,d,s,z,a_prime)
            real(dp), intent(in)   :: m,d,s,z,a_prime
            z_find_lowerbound = F(m-model%a_grid(1),d,s,z) - F(m-a_prime,d,s,z)
        end function
        real(dp) function d_z_find_lowerbound(m,d,s,z,a_prime)
            real(dp), intent(in)   :: m,d,s,z,a_prime
            d_z_find_lowerbound = dF(m-model%a_grid(1),d,s,z) - dF(m-a_prime,d,s,z)
        end function

        ! *******************************************************************************************************
        ! ** Private helper subroutines
        ! *******************************************************************************************************
        subroutine requireSolved()
            if (.NOT.is_solved) then
                print *, "Error: The model must first be solved before obtaining policy and value functions."
                stop
            end if
        end subroutine

    ! **
    ! * Checks if the given input model is valid and can be solved using ncegm.
    ! *
    ! * Input:
    ! *     - m: model to be validated
    ! *
    ! * Return value: boolean; .TRUE. if model is valid, .FALSE. otherwise
    ! **
        logical function validate_model(m) result(valid)
            type(ncegm_model), intent(in) :: m
            valid = .FALSE.
            if (.NOT. (associated(m%F) .AND. associated(m%dF))) then
                write (error_unit,*) "Invalid model: The return function and its first derivative with respect to c must be provided."
            else if (.NOT. (associated(m%dF_inv) .OR. associated(m%d2F))) then
                write (error_unit,*) "Invalid model: Either the marginal inverse of the return function or its second derivative must be provided."
            elseif (.NOT. associated(m%Lambda)) then
                write (error_unit,*) "Invalid model: The budget constraint function Lambda must be provided"
            elseif (.NOT. (allocated(m%a_grid) .AND. allocated(m%d_grid))) then
                write (error_unit,*) "Invalid model: The grids for A and D must be provided."
            else if (allocated(m%s_grid) .NEQV. associated(m%Psi)) then
                write (error_unit,*) "Invalid model: If the state variable s is used, s_grid and the transition function Psi must be provided."
            else if (allocated(m%z_grid) .NEQV. allocated(m%z_transition)) then
                write (error_unit,*) "Invalid model: If the stochastic state variable z is used, z_grid and the transition matrix must be provided."
            elseif (.NOT. allocated(m%V_initial)) then
                write (error_unit,*) "Invalid model: Missing initial guess for value function."
            elseif (m%beta<=0 .OR. m%beta>=1) then
                write (error_unit,*) "Invalid model: Discount factor Beta must be in interval (0,1)."
            else
                valid = .TRUE.
            end if
        end function

        ! *******************************************************************************************************
        ! ** Public getter functions for the module client to obtain the output of the algorithm
        ! *******************************************************************************************************
        function ncegm_getPolicy_d() result(d)
            real(dp), dimension(glen_a,glen_s,glen_z) :: d
            call requireSolved
            d = policy_d
        end function

        function ncegm_getPolicy_c() result(c)
            real(dp), dimension(glen_a,glen_s,glen_z) :: c
            call requireSolved
            c = policy_c
        end function

        function ncegm_getPolicy_aprime() result(aprime)
            real(dp), dimension(glen_a,glen_s,glen_z) :: aprime
            call requireSolved
            aprime = policy_aprime
        end function

        function ncegm_getValueFunction() result(v)
            real(dp), dimension(glen_a,glen_s,glen_z) :: v
            call requireSolved
            v = valuef
        end function

        function ncegm_getConditionalPolicy_c() result(c)
            real(dp), dimension(glen_a,glen_d,glen_s,glen_z) :: c
            call requireSolved
            c = policy_c_con
        end function

        function ncegm_getConditionalPolicy_aprime() result(aprime)
            real(dp), dimension(glen_a,glen_d,glen_s,glen_z) :: aprime
            call requireSolved
            aprime = policy_aprime_con
        end function

        function ncegm_getConditionalValueFunction() result(v)
            real(dp), dimension(glen_a,glen_d,glen_s,glen_z) :: v
            call requireSolved
            v = valuef_next_con
        end function

        ! *******************************************************************************************************
        ! ** Convenience functions to simplify access to array- and scalar-valued model functions
        ! *******************************************************************************************************

        function F_sc(c,d,s,z)
            real(dp), intent(in)   :: c,d,s,z
            real(dp)               :: F_sc
            real(dp), dimension(1) :: c_arr, ret
            c_arr=c
            ret = F(c_arr,d,s,z)
            F_sc = ret(1)
        end function F_sc
        function F_arr(c,d,s,z)
            real(dp), dimension(:), intent(in)  :: c
            real(dp), intent(in)                :: d,s,z
            real(dp), dimension(size(c))        :: F_arr
            F_arr=model%F(c,d,s,z)
        end function F_arr
        function dF_sc(c,d,s,z)
            real(dp), intent(in)   :: c,d,s,z
            real(dp)               :: dF_sc
            real(dp), dimension(1) :: c_arr, ret
            c_arr=c
            ret = dF(c_arr,d,s,z)
            dF_sc = ret(1)
        end function dF_sc
        function dF_arr(c,d,s,z)
            real(dp), dimension(:), intent(in)  :: c
            real(dp), intent(in)                :: d,s,z
            real(dp), dimension(size(c))        :: dF_arr
            dF_arr=model%dF(c,d,s,z)
        end function dF_arr
        function d2F_sc(c,d,s,z)
            real(dp), intent(in)   :: c,d,s,z
            real(dp)               :: d2F_sc
            real(dp), dimension(1) :: c_arr, ret
            c_arr=c
            ret = d2F(c_arr,d,s,z)
            d2F_sc = ret(1)
        end function d2F_sc
        function d2F_arr(c,d,s,z)
            real(dp), dimension(:), intent(in)  :: c
            real(dp), intent(in)                :: d,s,z
            real(dp), dimension(size(c))        :: d2F_arr
            d2F_arr=model%d2F(c,d,s,z)
        end function d2F_arr
        function dLambda_sc(a,d,s,z)
            real(dp),               intent(in)  :: a
            real(dp), intent(in)                :: d,s,z
            real(dp)                             :: dLambda_sc
            real(dp), dimension(1)              :: a_arr,dg_arr
            a_arr = a
            dg_arr = model%dLambda(a_arr,d,s,z)
            dLambda_sc = dg_arr(1)
        end function
end module ncegm


!=================================================
! SUBROUTINE
!	MNEWT(x1, y1, z1, x2, y2, z2, x3, y3, z3, di, da2)
!		intent(in) :: x1, y1, z1, x2, y2, z2, x3, di, da2
!		intent(out) :: y3, z3
!
!=================================================
module mod_mnewt

	implicit none

contains
!==========
subroutine MNEWT(x1,y1,z1, x2,y2,z2, x3,y3,z3, di,da2)

	use mod_intdep

	implicit none

	real(8), intent(in) :: x1, y1, z1, x2, y2, z2, x3, di, da2
	real(8), intent(out) :: y3, z3

	real(8) :: ct, cx, cy, cz, c2
	real(8) :: dx, dl, dr, dz, du
	real(8) :: arr, arl, ari

	ct = 1.d-6
	cx = x2 - x1
	cy = y2 - y1
	cz = z2 - z1
	c2 = cx*cx + cy*cy + cz*cz

	dx = x3 - x1
	dl = di
	dr = 0.d0
	arr = 0.d0

	do
		y3 = y1 + dl
		CALL INTDEP(x3,y3,z3)

		dz = z3 - z1
		CALL AREA2(cx, cy, cz, dx, dl, dz, c2, arl)

		if(dabs(arl-da2) < ct) return
		if(arl < da2) then
			dr = dl
			arr = arl
			dl = dl * da2 / arl
			cycle
		end if

		do
			du = dr + (dl - dr) * (da2 - arr) / (arl - arr)
			y3 = y1 + du
			CALL INTDEP(x3,y3,z3)

			dz = z3 - z1
			CALL AREA2(cx, cy, cz, dx, du, dz, c2, ari)

			if(dabs(ari-da2) < ct) return

			if(ari > da2) then
				arl = ari
				dl = du
			else if(ari < da2) then
				arr = ari
				dr = du
			end if

		end do

	end do

end subroutine MNEWT

end module mod_mnewt