module driver_module
  use precision_module, only : wp
  use input_module
  use vib_module
  use rot_module
  use convolution_module
  use prob_module
  use rate_module

  implicit none
  public

contains
subroutine run

 implicit none
 real(wp), allocatable :: vibR(:), vibX(:), vibTP(:), vibP(:)
 real(wp), allocatable :: rotR(:), rotX(:), rotTP(:), rotP(:)
 real(wp), allocatable :: dosR(:), dosP(:), dosP_full(:), dosX(:), dosTP(:)
 real(wp), allocatable :: dosX_full(:), dosX_TP_rev(:)
 real(wp), allocatable :: bbDia(:), bbDia_rev(:)
 real(wp)              :: aaDia
 real(wp), allocatable :: probLZ(:), probWC(:), probZN(:), probLZS(:,:)
 real(wp), allocatable :: probLZ_rev(:), probWC_rev(:)
 real(wp), allocatable :: nosLZ(:), nosWC(:), nosZN(:), nosLZ_full(:)
 real(wp), allocatable :: nosWC_full(:), nosLZS(:,:)
 real(wp), allocatable :: nosTST(:), nosTST_full(:)

allocate (vibR  (maxn))
allocate (vibX  (maxn))
allocate (vibTP (maxn))
allocate (rotR  (maxn))
allocate (rotX  (maxn))
allocate (rotTP (maxn))
allocate (dosR  (maxn))
allocate (dosX  (maxn))
allocate (dosTP (maxn))
allocate (bbDia (maxn))

! ~~~~ Program begins.

! Densities of states.

write(66,'(/,2X,A)') '---------------------------------------------------------'
write(66,'(15X,A)') 'Start NAST calculation'
write(66,'(2X,A)') '---------------------------------------------------------'
write(66,'(/,3X,A,/)') '1. Calculating densities of states'

call vib_dos(vibR,vibX,vibTP)
call asym_top(rotR,rotX,rotTP)

call rovib_dos(vibR,vibX,vibTP,rotR,rotX,rotTP, &
               dosR,dosX,dosTP)

if (printmore) then
  call write_dos(vibR,vibX,vibTP,rotR,rotX,rotTP, &
                dosR,dosX,dosTP)
end if

! Deallocation.

deallocate (vibR)
deallocate (vibX)
deallocate (vibTP)
deallocate (rotR)
deallocate (rotX)
deallocate (rotTP)

! Microcanonical probabilities.

if (.not. tst) then
  write(66,'(/,3X,A)') '2. Calculating microcanonical probabilities'
  allocate (probLZ (maxn))
  allocate (probWC (maxn))
  probLZ = zero
  probWC = zero
  bbDia  = zero
  call diab_param(aaDia,bbDia)
  call LZ(aaDia,bbDia,probLZ)
  call WC(aaDia,bbDia,probWC)
  if (zn) then
    allocate (probZN (maxn))
    call ZhN(probZN)
  else
    allocate (probZN (1))
    call ZNempty(probZN)
  endif
  if (sp) then
    allocate (probLZS (maxn,icp))
    call LZS(probLZS)
  else
    allocate (probLZS (1,1))
  end if
 else
! Do formal allocation of non-used array(s).
    allocate (probLZ (1))
    allocate (probWC (1))
    allocate (probZN (1))
    allocate (probLZS (1,1))
end if

! Microcanonical and canonical rate constants.

if (.not. tst) then
  allocate (nosLZ  (maxn))
  allocate (nosWC  (maxn))
  allocate (nosZN  (maxn))
  nosLZ = zero
! Do formal allocation of non-used array(s).
  allocate (nosTST  (1))
  write(66,'(/,3X,A)') '3. Calculating microcanonical NAST rate constant'
  call nast_en_rate(dosR,dosX,dosTP,probLZ,probWC,probZN,nosLZ,nosWC,nosZN)
  call nast_can_rate(dosR,nosLZ,nosWC,nosZN)
else
  allocate (nosTST (maxn))
! Do formal allocation of non-used array(s)
  allocate (nosLZ (1))
  allocate (nosWC (1))
  allocate (nosZN (1))
  call tst_en_rate(dosR,dosX,nosTST)
  call tst_can_rate(dosR,nosTST)
end if

! Microcanonical rate constants for transitions between MS components.

if (sp) then
  allocate (nosLZS (maxn,icp))
  nosLZS = zero
  call nast_en_rate_split(dosR,dosX,probLZS,nosLZS)
else
  allocate (nosLZS (1,1))
end if

! Deallocation.

deallocate (dosR)
deallocate (dosX)
deallocate (dosTP)

! Velocity-averaged transition probabilities (VATP).

 if (.not. tst) then
    call VATP()
 end if

! Reverse rate constants.

if (rev) then
 allocate (vibP          (maxn + binGAP))
 allocate (rotP          (maxn + binGAP))
 allocate (dosP          (maxn))
 allocate (dosP_full     (maxn + binGAP))
 allocate (dosX_full     (maxn + binGAP))
 allocate (dosX_TP_rev   (maxn + binGAP))
 call rev_vib_dos(vibP)
 call rev_asym_top(rotP)
 call rev_rovib_dos(vibP,rotP,dosP,dosP_full,dosX_full,dosX_TP_rev)
 if (.not. tst) then
   allocate (bbDia_rev   (maxn + binGAP))
   allocate (probLZ_rev  (maxn + binGAP))
   allocate (probWC_rev  (maxn + binGAP))
   allocate (nosLZ_full  (maxn + binGAP))
   allocate (nosWC_full  (maxn + binGAP))
   bbDia_rev  = zero
   probLZ_rev = zero
   probWC_rev = zero
   call LZ_rev (aaDia,bbDia_rev,probLZ_rev)
   call WC_rev (aaDia,bbDia_rev,probWC_rev)
   call nast_en_rate_rev(dosP_full,dosX_full,dosX_TP_rev,probLZ_rev,probWC_rev,nosLZ_full,nosWC_full)
   call nast_can_rate_rev(dosP_full,nosLZ_full,nosWC_full)
 else
   allocate (nosTST_full (maxn + binGAP))
   call tst_en_rate_rev(dosP_full,dosX_full,nosTST_full)
   call tst_can_rate_rev(dosP_full,nosTST_full)
 end if
else
! Do formal allocation of non-used array(s)
  allocate (vibP       (1))
  allocate (rotP       (1))
  allocate (dosP_full  (1))
  allocate (dosX_full  (1))
  allocate (nosLZ_full (1))
  allocate (dosP       (1))
  allocate (bbDia_rev  (1))
  allocate (probLZ_rev (1))
endif

if (.not. tst) then
  call write_prob(aaDia,bbDia,probLZ,probWC,probZN,probLZS,bbDia_rev,probLZ_rev,probWC_rev)
end if

! Deallocation.

 deallocate (vibP)
 deallocate (rotP)
 deallocate (dosP)
 deallocate (dosP_full)
 deallocate (probLZ)
 deallocate (probWC)
 deallocate (probZN)
 deallocate (nosLZ)
 deallocate (nosWC)
 deallocate (nosZN)
 deallocate (nosTST)
 deallocate (bbDia)
 deallocate (probLZS)
 deallocate (nosLZS)

end subroutine run
end module driver_module


program ft_test
  implicit none
!!$  integer, parameter :: size1=7, size2=9
!!$  integer, parameter :: size1=70000, size2=480
!!$  integer, parameter :: size1=70000, size2=60
  integer, parameter :: size1=7, size2=60
!!$  integer, parameter :: size1=35, size2=12
!!$  integer, parameter :: size1=13, size2=12
!!$  integer, parameter :: size1=26, size2=6
!!$  integer, parameter :: size1=52, size2=3

  integer, parameter :: size=size1*size2
  complex*16 :: input(size), output(size),zoutput1(size),&
       input1(size) !!,output1(size)
  real*8 :: realarray(size),randomamount
  integer :: primefactors(7),numfactors,i

  do i=1,size
     input(i)=exp(-249*(i-(size+1)*0.5d0)**2/size**2)  * (-1)**i
  enddo

  randomamount=1d-2

  call RANDOM_NUMBER(realarray)
  input(:)=input(:)+realarray(:) * randomamount

  call RANDOM_NUMBER(realarray)
  input(:)=input(:)+realarray(:)**3 * (0d0,4d0) * randomamount

  print *, "Go ft_test. Dimensions are ",size1,size2
  call getallprimefactors(size2,numfactors,primefactors)
  print *, "     Prime factors of ",size2," are"
  print *, primefactors(1:numfactors)
  print *

  write(*,*) "randomamount is ", randomamount
  write(*,'(A50,$)') "## call ft.  "
  call system('date')
  call myzfft1d(input,output,size,1)

  write(*,'(A50,$)') "## done ft   "
  call system('date')

  write(*,'(A50,$)') "## call cooleytukey_outofplace  "
  call system('date')
  call cooleytukey_outofplace(input,zoutput1,size1,primefactors,1)

  print *, "## call cooleytukey_outofplace_inverse"
  call cooleytukey_outofplace_inverse(zoutput1,input1,size1,primefactors,1)
  write(*,'(A50,$)') "## done cooleytukey_outofplace_inverse  "
  call system('date')

!!$  call cooleytukey_replace(1,zoutput1,output1,size1,primefactors,1)

  print *, "## ok"
  print *
  write(*,'(A5,100A10)') " ","input","input" !!,"output","output"
  print *

  do i=1,size
     write(*,'(I20,100F10.5)') i, &
          abs(input(i)), &
          abs(input1(i))
!!$          abs(output(i)), &
!!$          abs(output3(i))
     write(*,'(I20,100F10.5)') i, &
          real(input(i)), &
          real(input1(i))
!!$          real(output(i)), &
!!$          real(output3(i))
     print *
  enddo

end program ft_test


module mod_constant
  implicit none
 real(8),parameter::pi=4*datan(1.d0)
 real(8),parameter::pois=0.25d0
 real(8),parameter::sigma0=100d0
 real(8),parameter::vref=1.d-6
 real(8),parameter::rigid=32.04d0
 real(8),parameter::vs=3.464d0
 real(8),parameter::vp=6.d0
 !real(8),parameter::tr=1d12
end module


!*****************************************************************************************
!> author: Jacob Williams
!
!  This module contains various routines related to relative motion.
!
!## Axis systems
!
!  Three different axis systems are used here.  They are:
!
!  * The **IJK** frame
!
!  * The **LVLH** frame, defined by:
!
!     * x-axis : completes the right handed system
!       (for a perfectly-circular orbit, the x-axis is \( \hat{\mathbf{v}} \))
!     * y-axis : \( -\hat{\mathbf{h}} \)
!     * z-axis : \( -\hat{\mathbf{r}} \)
!
!  * The **RSW** frame, defined by:
!
!     * x-axis : \( \hat{\mathbf{r}} \)
!     * y-axis : completes the right handed system
!       (for a perfectly-circular orbit, the y-axis is \( \hat{\mathbf{v}} \))
!     * z-axis : \( \hat{\mathbf{h}} \)

    module relative_motion_module

    use kind_module,    only: wp
    use numbers_module

    implicit none

    private

    abstract interface
        subroutine report_func(t,rv)  !! for reporting the points in the [[cw_propagator]].
        import :: wp
        implicit none
        real(wp),intent(in)              :: t   !! time [sec]
        real(wp),dimension(6),intent(in) :: rv  !! state [km,km/s]
        end subroutine report_func
    end interface

    interface from_ijk_to_lvlh  !! Conversion from IJK to LVLH
        procedure :: from_ijk_to_lvlh_mat  !! just returns matrices
        procedure :: from_ijk_to_lvlh_rv   !! transforms the r,v vectors
    end interface from_ijk_to_lvlh
    public :: from_ijk_to_lvlh

    interface from_lvlh_to_ijk  !! Conversion from LVLH to IJK
        procedure :: from_lvlh_to_ijk_mat  !! just returns matrices
        procedure :: from_lvlh_to_ijk_rv   !! transforms the r,v vectors
    end interface from_lvlh_to_ijk
    public :: from_lvlh_to_ijk

    interface from_ijk_to_rsw !! Conversion from IJK to RSW
        procedure :: from_ijk_to_rsw_mat  !! just returns matrices
        procedure :: from_ijk_to_rsw_rv   !! transforms the r,v vectors
    end interface from_ijk_to_rsw
    public :: from_ijk_to_rsw

    interface from_rsw_to_ijk  !! Conversion from RSW to IJK
        procedure :: from_rsw_to_ijk_mat  !! just returns matrices
        procedure :: from_rsw_to_ijk_rv   !! transforms the r,v vectors
    end interface from_rsw_to_ijk
    public :: from_rsw_to_ijk

    interface from_lvlh_to_rsw  !! Conversion from LVLH to RSW
        procedure :: from_lvlh_to_rsw_rv   !! transforms the r,v vectors
    end interface from_lvlh_to_rsw
    public :: from_lvlh_to_rsw

    interface from_rsw_to_lvlh !! Conversion from RSW to LVLH
        procedure :: from_rsw_to_lvlh_rv   !! transforms the r,v vectors
    end interface from_rsw_to_lvlh
    public :: from_rsw_to_lvlh

    public :: cw_equations
    public :: cw_propagator

    public :: relative_motion_test  !test routine

    contains
!*****************************************************************************************

!*****************************************************************************************
!> author: Jacob Williams
!  date: 6/14/2015
!
!  Clohessy-Wiltshire equations for relative motion.
!
!  These apply to an RSW frame centered at the target spacecraft.
!
!# References
!   * [The Clohessy Wiltshire Model](http://courses.ae.utexas.edu/ase366k/cw_equations.pdf)

    function cw_equations(x0,dt,n) result(x)

    implicit none

    real(wp),dimension(6),intent(in) :: x0    !! initial state [r,v] of chaser (at t0) [km, km/s]
    real(wp),intent(in)              :: dt    !! elapsed time from t0 [sec]
    real(wp),intent(in)              :: n     !! mean motion of target orbit (`sqrt(mu/a**3)`) [1/sec]
    real(wp),dimension(6)            :: x     !! final state [r,v] of chaser [km, km/s]

    real(wp) :: nt,cnt,snt

    if (dt==zero) then
        x = x0
    else

        if (n==zero) then
            error stop 'Error: Target orbit mean motion must be non-zero.'
        else

            nt = n*dt
            cnt = cos(nt)
            snt = sin(nt)

            x(1) = (four-three*cnt)*x0(1) + (snt/n)*x0(4) + (two/n)*(one-cnt)*x0(5)
            x(2) = six*(snt-nt)*x0(1) + x0(2) - (two/n)*(one-cnt)*x0(4) + one/n*(four*snt-three*nt)*x0(5)
            x(3) = cnt*x0(3) + (snt/n)*x0(6)
            x(4) = three*n*snt*x0(1) + cnt*x0(4) + two*snt*x0(5)
            x(5) = -(six*n*(one-cnt))*x0(1) - two*snt*x0(4) + (four*cnt-three)*x0(5)
            x(6) = -n*snt*x0(3) + cnt*x0(6)

        end if

    end if

    end function cw_equations
!*****************************************************************************************

!*****************************************************************************************
!> author: Jacob Williams
!  date: 8/23/2015
!
!  Clohessy-Wiltshire propagation routine.
!
!# See also
!   * [[rk_module]]

    subroutine cw_propagator(t0,x0,h,n,tf,xf,report)

    implicit none

    real(wp),intent(in)               :: t0      !! initialize time [sec]
    real(wp),dimension(6),intent(in)  :: x0      !! initial state in RST coordinates [km,km/s]
    real(wp),intent(in)               :: h       !! abs(time step) [sec]
    real(wp),intent(in)               :: n       !! mean motion of target orbit (`sqrt(mu/a**3)`) [1/sec]
    real(wp),intent(in)               :: tf      !! final time [sec]
    real(wp),dimension(6),intent(out) :: xf      !! final state in RST coordinates [km,km/s]
    procedure(report_func),optional   :: report  !! to report each point

    real(wp) :: t,dt,t2
    real(wp),dimension(6) :: x
    logical :: last,export

    export = present(report)

    if (export) call report(t0,x0)  !first point

    if (h==zero) then
        xf = x0
    else

        t = t0
        x = x0
        dt = sign(h,tf-t0)  !time step  (correct sign)
        do
            t2 = t + dt
            last = ((dt>=zero .and. t2>=tf) .or. &  !adjust last time step
                    (dt<zero .and. t2<=tf))         !
            if (last) dt = tf-t                     !
            xf = cw_equations(x,dt,n)  ! propagate
            if (last) exit
            if (export) call report(t2,xf)   !intermediate point
            x = xf
            t = t2
        end do

    end if

    if (export) call report(tf,xf)   !last point

    end subroutine cw_propagator
!*****************************************************************************************

!*****************************************************************************************
!> author: Jacob Williams
!  date: 4/19/2014
!
!  Compute the transformation matrices to convert IJK to LVLH.
!
!# See also
!   * [LVLH Transformations](http://degenerateconic.com/wp-content/uploads/2015/03/lvlh.pdf)

    subroutine from_ijk_to_lvlh_mat(r,v,a,c,cdot)

    use vector_module, only: unit, cross, uhat_dot

    implicit none

    real(wp),dimension(3),intent(in)             :: r      !! position vector of target [km]
    real(wp),dimension(3),intent(in)             :: v      !! velocity vector of target [km/s]
    real(wp),dimension(3),intent(in),optional    :: a      !! acceleration vector of target [km/s^2]
                                                           !! (if not present, then a torque-free force model is assumed)
    real(wp),dimension(3,3),intent(out)          :: c      !! C transformation matrix
    real(wp),dimension(3,3),intent(out),optional :: cdot   !! CDOT transformation matrix

    real(wp),dimension(3) :: ex_hat,ex_hat_dot
    real(wp),dimension(3) :: ey_hat,ey_hat_dot
    real(wp),dimension(3) :: ez_hat,ez_hat_dot
    real(wp),dimension(3) :: h,h_hat,h_dot

    h      = cross(r,v)
    h_hat  = unit(h)
    ez_hat = -unit(r)
    ey_hat = -h_hat
    ex_hat = cross(ey_hat,ez_hat)

    c(1,:) = ex_hat
    c(2,:) = ey_hat
    c(3,:) = ez_hat

    if (present(cdot)) then

        ez_hat_dot = -uhat_dot(r,v)

        if (present(a)) then
            h_dot      = cross(r,a)
            ey_hat_dot = -uhat_dot(h, h_dot)
            ex_hat_dot = cross(ey_hat_dot,ez_hat) + cross(ey_hat,ez_hat_dot)
        else !assume no external torque
            ey_hat_dot = zero
            ex_hat_dot = cross(ey_hat,ez_hat_dot)
        end if

        cdot(1,:) = ex_hat_dot
        cdot(2,:) = ey_hat_dot
        cdot(3,:) = ez_hat_dot

    end if

    end subroutine from_ijk_to_lvlh_mat
!*****************************************************************************************

!*****************************************************************************************
!> author: Jacob Williams
!  date: 8/23/2014
!
!  Transform a position (and optionally velocity) vector from IJK to LVLH.

    subroutine from_ijk_to_lvlh_rv(rt_ijk,vt_ijk,r_ijk,v_ijk,dr_lvlh,dv_lvlh)

    implicit none

    real(wp),dimension(3),intent(in)           :: rt_ijk  !! Target IJK absolute position vector [km]
    real(wp),dimension(3),intent(in)           :: vt_ijk  !! Target IJK absolute position vector [km]
    real(wp),dimension(3),intent(in)           :: r_ijk   !! Chaser IJK absolute position vector [km]
    real(wp),dimension(3),intent(in)           :: v_ijk   !! Chaser IJK absolute position vector [km]
    real(wp),dimension(3),intent(out)          :: dr_lvlh !! Chaser LVLH position vector relative to target [km]
    real(wp),dimension(3),intent(out),optional :: dv_lvlh !! Chaser LVLH position vector relative to target [km]

    real(wp),dimension(3,3) :: c
    real(wp),dimension(3,3) :: cdot
    real(wp),dimension(3) :: dr_ijk, dv_ijk

    !IJK state of chaser relative to target:
    dr_ijk = r_ijk - rt_ijk    ! [target + delta = chaser]

    if (present(dv_lvlh)) then

        dv_ijk = v_ijk - vt_ijk ! [target + delta = chaser]

        call from_ijk_to_lvlh(rt_ijk,vt_ijk,c=c,cdot=cdot)

        dr_lvlh = matmul( c, dr_ijk )
        dv_lvlh = matmul( cdot, dr_ijk ) + matmul( c, dv_ijk )

    else

        call from_ijk_to_lvlh(r_ijk,v_ijk,c=c)

        dr_lvlh = matmul( c, dr_ijk )

    end if

    end subroutine from_ijk_to_lvlh_rv
!*****************************************************************************************

!*****************************************************************************************
!> author: Jacob Williams
!  date: 4/19/2014
!
!  Compute the transformation matrices to convert LVLH to IJK.
!
!# See also
!   * [LVLH Transformations](http://degenerateconic.com/wp-content/uploads/2015/03/lvlh.pdf)

    subroutine from_lvlh_to_ijk_mat(r,v,a,c,cdot)

    use vector_module, only: unit, cross

    implicit none

    real(wp),dimension(3),intent(in)             :: r     !! position vector of target [km]
    real(wp),dimension(3),intent(in)             :: v     !! velocity vector of target [km/s]
    real(wp),dimension(3),intent(in),optional    :: a     !! acceleration vector of target [km/s^2]
                                                          !! (if not present, then a torque-free force model is assumed)
    real(wp),dimension(3,3),intent(out)          :: c     !! C transformation matrix
    real(wp),dimension(3,3),intent(out),optional :: cdot  !! CDOT transformation matrix

    call from_ijk_to_lvlh(r,v,a,c,cdot)

    c = transpose(c)

    if (present(cdot)) cdot = transpose(cdot)

    end subroutine from_lvlh_to_ijk_mat
!*****************************************************************************************

!*****************************************************************************************
!> author: Jacob Williams
!  date: 8/23/2014
!
!  Transform a position (and optionally velocity) vector from LVLH to IJK.

    subroutine from_lvlh_to_ijk_rv(rt_ijk,vt_ijk,dr_lvlh,dv_lvlh,r_ijk,v_ijk)

    implicit none

    real(wp),dimension(3),intent(in)            :: rt_ijk   !! Target IJK absolute position vector [km]
    real(wp),dimension(3),intent(in)            :: vt_ijk   !! Target IJK absolute position vector [km]
    real(wp),dimension(3),intent(in)            :: dr_lvlh  !! Chaser LVLH position vector relative to target [km]
    real(wp),dimension(3),intent(in)            :: dv_lvlh  !! Chaser LVLH position vector relative to target [km]
    real(wp),dimension(3),intent(out)           :: r_ijk    !! Chaser IJK absolute position vector [km]
    real(wp),dimension(3),intent(out),optional  :: v_ijk    !! Chaser IJK absolute position vector [km]

    real(wp),dimension(3,3) :: c
    real(wp),dimension(3,3) :: cdot

    if (present(v_ijk)) then

        call from_lvlh_to_ijk(rt_ijk,vt_ijk,c=c,cdot=cdot)

        !chaser = target + delta:
        r_ijk = rt_ijk + matmul( c, dr_lvlh )
        v_ijk = vt_ijk + matmul( cdot, dr_lvlh ) + matmul( c, dv_lvlh )

    else

        call from_lvlh_to_ijk(rt_ijk,vt_ijk,c=c)

        r_ijk = rt_ijk + matmul( c, dr_lvlh )

    end if

    end subroutine from_lvlh_to_ijk_rv
!*****************************************************************************************

!*****************************************************************************************
!> author: Jacob Williams
!  date: 4/19/2014
!
!  Compute the transformation matrices to convert IJK to RSW.

    subroutine from_ijk_to_rsw_mat(r,v,a,c,cdot)

    use vector_module, only: unit, cross, uhat_dot

    implicit none

    real(wp),dimension(3),intent(in)             :: r      !! position vector of target [km]
    real(wp),dimension(3),intent(in)             :: v      !! velocity vector of target [km/s]
    real(wp),dimension(3),intent(in),optional    :: a      !! acceleration vector of target [km/s^2]
                                                           !! (if not present, then a torque-free force model is assumed)
    real(wp),dimension(3,3),intent(out)          :: c      !! C transformation matrix
    real(wp),dimension(3,3),intent(out),optional :: cdot   !! CDOT transformation matrix

    real(wp),dimension(3) :: ex_hat,ex_hat_dot
    real(wp),dimension(3) :: ey_hat,ey_hat_dot
    real(wp),dimension(3) :: ez_hat,ez_hat_dot
    real(wp),dimension(3) :: h,h_hat,h_dot

    h      = cross(r,v)
    h_hat  = unit(h)
    ex_hat = unit(r)
    ez_hat = h_hat
    ey_hat = cross(ez_hat,ex_hat)

    c(1,:) = ex_hat
    c(2,:) = ey_hat
    c(3,:) = ez_hat

    !... need to verify the following ...

    if (present(cdot)) then

        ex_hat_dot = uhat_dot(r,v)

        if (present(a)) then
            h_dot      = cross(r,a)
            ez_hat_dot = uhat_dot(h, h_dot)
            ey_hat_dot = cross(ez_hat_dot,ex_hat) + cross(ez_hat,ex_hat_dot)
        else !assume no external torque
            ez_hat_dot = zero
            ey_hat_dot = cross(ez_hat,ex_hat_dot)
        end if

        cdot(1,:) = ex_hat_dot
        cdot(2,:) = ey_hat_dot
        cdot(3,:) = ez_hat_dot

    end if

    end subroutine from_ijk_to_rsw_mat
!*****************************************************************************************

!*****************************************************************************************
!> author: Jacob Williams
!  date: 8/23/2014
!
!  Transform a position (and optionally velocity) vector from IJK to RSW.

    subroutine from_ijk_to_rsw_rv(rt_ijk,vt_ijk,r_ijk,v_ijk,dr_rsw,dv_rsw)

    implicit none

    real(wp),dimension(3),intent(in)           :: rt_ijk   !! Target IJK absolute position vector [km]
    real(wp),dimension(3),intent(in)           :: vt_ijk   !! Target IJK absolute position vector [km]
    real(wp),dimension(3),intent(in)           :: r_ijk    !! Chaser IJK absolute position vector [km]
    real(wp),dimension(3),intent(in)           :: v_ijk    !! Chaser IJK absolute position vector [km]
    real(wp),dimension(3),intent(out)          :: dr_rsw   !! Chaser RSW position vector relative to target [km]
    real(wp),dimension(3),intent(out),optional :: dv_rsw   !! Chaser RSW position vector relative to target [km]

    real(wp),dimension(3,3) :: c
    real(wp),dimension(3,3) :: cdot
    real(wp),dimension(3) :: dr_ijk, dv_ijk

    dr_ijk = r_ijk - rt_ijk  ! delta = chaser - target

    if (present(dv_rsw)) then

        dv_ijk = v_ijk - vt_ijk  ! delta = chaser - target

        call from_ijk_to_rsw(rt_ijk,vt_ijk,c=c,cdot=cdot)

        dr_rsw = matmul( c, dr_ijk )
        dv_rsw = matmul( cdot, dr_ijk ) + matmul( c, dv_ijk )

    else

        call from_ijk_to_rsw(rt_ijk,vt_ijk,c=c)

        dr_rsw = matmul( c, dr_ijk )

    end if

    end subroutine from_ijk_to_rsw_rv
!*****************************************************************************************

!*****************************************************************************************
!> author: Jacob Williams
!  date: 4/19/2014
!
!  Compute the transformation matrices to convert RSW to IJK.

    subroutine from_rsw_to_ijk_mat(r,v,a,c,cdot)

    use vector_module, only: unit, cross

    implicit none

    real(wp),dimension(3),intent(in)             :: r     !! position vector of target [km]
    real(wp),dimension(3),intent(in)             :: v     !! velocity vector of target [km/s]
    real(wp),dimension(3),intent(in),optional    :: a     !! acceleration vector of target [km/s^2]
                                                          !! (if not present, then a torque-free force model is assumed)
    real(wp),dimension(3,3),intent(out)          :: c     !! C transformation matrix
    real(wp),dimension(3,3),intent(out),optional :: cdot  !! CDOT transformation matrix

    call from_ijk_to_rsw(r,v,a,c,cdot)

    c = transpose(c)

    if (present(cdot)) cdot = transpose(cdot)

    end subroutine from_rsw_to_ijk_mat
!*****************************************************************************************

!*****************************************************************************************
!> author: Jacob Williams
!  date: 8/23/2014
!
!  Transform a position (and optionally velocity) vector from RSW to IJK.

    subroutine from_rsw_to_ijk_rv(rt_ijk,vt_ijk,dr_rsw,dv_rsw,r_ijk,v_ijk)

    implicit none

    real(wp),dimension(3),intent(in)            :: rt_ijk   !! Target IJK absolute position vector [km]
    real(wp),dimension(3),intent(in)            :: vt_ijk   !! Target IJK absolute position vector [km]
    real(wp),dimension(3),intent(in)            :: dr_rsw   !! Chaser RSW position vector [km]
    real(wp),dimension(3),intent(in)            :: dv_rsw   !! Chaser RSW velocity vector [km/s]
    real(wp),dimension(3),intent(out)           :: r_ijk    !! Chaser IJK absolute position vector [km]
    real(wp),dimension(3),intent(out),optional  :: v_ijk    !! Chaser IJK absolute velocity vector [km/s]

    real(wp),dimension(3,3) :: c
    real(wp),dimension(3,3) :: cdot

    if (present(v_ijk)) then

        call from_rsw_to_ijk(rt_ijk,vt_ijk,c=c,cdot=cdot)

        r_ijk = rt_ijk + matmul( c, dr_rsw )
        v_ijk = vt_ijk + matmul( cdot, dr_rsw ) + matmul( c, dv_rsw )

    else

        call from_rsw_to_ijk(rt_ijk,vt_ijk,c=c)

        r_ijk = rt_ijk + matmul( c, dr_rsw )

    end if

    end subroutine from_rsw_to_ijk_rv
!*****************************************************************************************

!*****************************************************************************************
!> author: Jacob Williams
!  date: 8/23/2014
!
!  Transform a position (and optionally velocity) vector from RSW to LVLH.

    subroutine from_rsw_to_lvlh_rv(dr_rsw,dv_rsw,dr_lvlh,dv_lvlh)

    implicit none

    real(wp),dimension(3),intent(in)           :: dr_rsw  !! Chaser RSW position vector relative to target [km]
    real(wp),dimension(3),intent(in)           :: dv_rsw  !! Chaser RSW velocity vector relative to target [km/s]
    real(wp),dimension(3),intent(out)          :: dr_lvlh !! Chaser LVLH position vector relative to target [km]
    real(wp),dimension(3),intent(out),optional :: dv_lvlh !! Chaser LVLH position vector relative to target [km]

    dr_lvlh(1) =  dr_rsw(2)
    dr_lvlh(2) = -dr_rsw(3)
    dr_lvlh(3) = -dr_rsw(1)

    if (present(dv_lvlh)) then
        dv_lvlh(1) =  dv_rsw(2)
        dv_lvlh(2) = -dv_rsw(3)
        dv_lvlh(3) = -dv_rsw(1)
    end if

    end subroutine from_rsw_to_lvlh_rv
!*****************************************************************************************

!*****************************************************************************************
!> author: Jacob Williams
!  date: 8/23/2014
!
!  Transform a position (and optionally velocity) vector from LVLH to RSW.

    subroutine from_lvlh_to_rsw_rv(dr_lvlh,dv_lvlh,dr_rsw,dv_rsw)

    implicit none

    real(wp),dimension(3),intent(in)           :: dr_lvlh !! Chaser LVLH position vector relative to target [km]
    real(wp),dimension(3),intent(in)           :: dv_lvlh !! Chaser LVLH position vector relative to target [km]
    real(wp),dimension(3),intent(out)          :: dr_rsw  !! Chaser RSW position vector relative to target [km]
    real(wp),dimension(3),intent(out),optional :: dv_rsw  !! Chaser RSW velocity vector relative to target [km/s]

    dr_rsw(2) =  dr_lvlh(1)
    dr_rsw(3) = -dr_lvlh(2)
    dr_rsw(1) = -dr_lvlh(3)

    if (present(dv_rsw)) then
        dv_rsw(2) =  dv_lvlh(1)
        dv_rsw(3) = -dv_lvlh(2)
        dv_rsw(1) = -dv_lvlh(3)
    end if

    end subroutine from_lvlh_to_rsw_rv
!*****************************************************************************************

!*****************************************************************************************
!> author: Jacob Williams
!  date: 8/22/2015
!
!  Unit tests for the [[relative_motion_module]].

    subroutine relative_motion_test()

    implicit none

    real(wp),dimension(6),parameter :: target_eci_state = [-2301672.24489839_wp, &
                                                           -5371076.10250925_wp, &
                                                           -3421146.71530212_wp, &
                                                            6133.8624555516_wp,  &
                                                            306.265184163608_wp, &
                                                           -4597.13439017524_wp  ]

    real(wp),dimension(6),parameter :: chaser_eci_state = [-2255213.51862763_wp, &
                                                           -5366553.94133467_wp, &
                                                           -3453871.15040494_wp, &
                                                            6156.89588163809_wp, &
                                                            356.79933181917_wp,  &
                                                           -4565.88915429063_wp  ]

    real(wp),dimension(3) :: r_12_I, r1_I, r2_I
    real(wp),dimension(3) :: v_12_I, v1_I, v2_I
    real(wp),dimension(3) :: r_12_R, v_12_R
    real(wp),dimension(3,3) :: c,cdot

    write(*,*) ''
    write(*,*) '---------------'
    write(*,*) ' relative_motion_test'
    write(*,*) '---------------'
    write(*,*) ''

    r1_I = target_eci_state(1:3)
    v1_I = target_eci_state(4:6)
    r2_I = chaser_eci_state(1:3)
    v2_I = chaser_eci_state(4:6)
    r_12_I = r2_I - r1_I
    v_12_I = v2_I - v1_I

    call from_ijk_to_lvlh(r1_I,v1_I,c=c,cdot=cdot)

    r_12_R = matmul( c, r_12_I )
    v_12_R = matmul( cdot, r_12_I ) + matmul( c, v_12_I )

    write(*,'(A,*(D30.16,1X))') 'r_12_LVLH : ', r_12_R
    write(*,'(A,*(D30.16,1X))') 'v_12_LVLH : ', v_12_R
    write(*,*) ''

    call from_ijk_to_rsw(r1_I,v1_I,c=c,cdot=cdot)

    r_12_R = matmul( c, r_12_I )
    v_12_R = matmul( cdot, r_12_I ) + matmul( c, v_12_I )

    write(*,'(A,*(D30.16,1X))') 'r_12_RSW : ', r_12_R
    write(*,'(A,*(D30.16,1X))') 'v_12_RSW : ', v_12_R
    write(*,*) ''

    end subroutine relative_motion_test
!*****************************************************************************************

!*****************************************************************************************
    end module relative_motion_module
!*****************************************************************************************


C=======================================================================
C     Other subroutines from Numerical Recipes
C=======================================================================
      SUBROUTINE broydn(x,n,check)
      INTEGER n,nn,NP,MAXITS
      DOUBLE PRECISION x(n),fvec,EPS,TOLF,TOLMIN,TOLX,STPMX
      REAL*8  massa,bag
      LOGICAL check
      PARAMETER (NP=40,MAXITS=2000,EPS=3.d-16,TOLF=1.d-12,TOLMIN=1.d-12,
     *TOLX=EPS,STPMX=100.d0)
      COMMON /newtv/ fvec(NP),nn
      SAVE/newtv/
CU    USES fdjac,fmin,lnsrch,qrdcmp,qrupdt,rsolv
      INTEGER i,its,j,k
      DOUBLE PRECISION den,f,fold,stpmax,sum,temp,test,c(NP),d(NP)
     *,fvcold(NP),g(NP),
     *p(NP),qt(NP,NP),r(NP,NP),s(NP),t(NP),w(NP),xold(NP),fmin
      LOGICAL restrt,sing,skip
      EXTERNAL fmin
      nn=n
      f=fmin(x)
      test=0.d0
      do 11 i=1,n
        if(abs(fvec(i)).gt.test)test=abs(fvec(i))
11    continue
      if(test.lt..01d0*TOLF)then
        check=.false.
        return
      endif
      sum=0.d0
      do 12 i=1,n
        sum=sum+x(i)**2
12    continue
      stpmax=STPMX*max(sqrt(sum), dble(n))
      restrt=.true.
      do 44 its=1,MAXITS
        if(restrt)then
          call fdjac(n,x,fvec,NP,r)
          call qrdcmp(r,n,NP,c,d,sing)
          if(sing) stop 'singular Jacobian in broydn'
          do 14 i=1,n
            do 13 j=1,n
              qt(i,j)=0.d0
13          continue
            qt(i,i)=1.d0
14        continue
          do 18 k=1,n-1
            if(c(k).ne.0.d0)then
              do 17 j=1,n
                sum=0.d0
                do 15 i=k,n
                  sum=sum+r(i,k)*qt(i,j)
15              continue
                sum=sum/c(k)
                do 16 i=k,n
                  qt(i,j)=qt(i,j)-sum*r(i,k)
16              continue
17            continue
            endif
18        continue
          do 21 i=1,n
            r(i,i)=d(i)
            do 19 j=1,i-1
              r(i,j)=0.d0
19          continue
21        continue
        else
          do 22 i=1,n
            s(i)=x(i)-xold(i)
22        continue
          do 24 i=1,n
            sum=0.d0
            do 23 j=i,n
              sum=sum+r(i,j)*s(j)
23          continue
            t(i)=sum
24        continue
          skip=.true.
          do 26 i=1,n
            sum=0.d0
            do 25 j=1,n
              sum=sum+qt(j,i)*t(j)
25          continue
            w(i)=fvec(i)-fvcold(i)-sum
            if(abs(w(i)).ge.EPS*(abs(fvec(i))+abs(fvcold(i))))then
              skip=.false.
            else
              w(i)=0.d0
            endif
26        continue
          if(.not.skip)then
            do 28 i=1,n
              sum=0.d0
              do 27 j=1,n
                sum=sum+qt(i,j)*w(j)
27            continue
              t(i)=sum
28          continue
            den=0.d0
            do 29 i=1,n
              den=den+s(i)**2
29          continue
            do 31 i=1,n
              s(i)=s(i)/den
31          continue
            call qrupdt(r,qt,n,NP,t,s)
            do 32 i=1,n
              if(r(i,i).eq.0.d0) stop 'r singular in broydn'
              d(i)=r(i,i)
32          continue
          endif
        endif
        do 34 i=1,n
          sum=0.d0
          do 33 j=1,n
            sum=sum+qt(i,j)*fvec(j)
33        continue
          g(i)=sum
34      continue
        do 36 i=n,1,-1
          sum=0.d0
          do 35 j=1,i
            sum=sum+r(j,i)*g(j)
35        continue
          g(i)=sum
36      continue
        do 37 i=1,n
          xold(i)=x(i)
          fvcold(i)=fvec(i)
37      continue
        fold=f
        do 39 i=1,n
          sum=0.d0
          do 38 j=1,n
            sum=sum+qt(i,j)*fvec(j)
38        continue
          p(i)=-sum
39      continue
        call rsolv(r,n,NP,d,p)
        call lnsrch(n,xold,fold,g,p,x,f,stpmax,check,fmin)
        test=0.d0
        do 41 i=1,n
          if(abs(fvec(i)).gt.test)test=abs(fvec(i))
41      continue
        if(test.lt.TOLF)then
          check=.false.
          return
        endif
        if(check)then
          if(restrt)then
            return
          else
            test=0.d0
            den=max(f,.5d0*n)
            do 42 i=1,n
              temp=abs(g(i))*max(abs(x(i)),1.d0)/den
              if(temp.gt.test)test=temp
42          continue
            if(test.lt.TOLMIN)then
              return
            else
              restrt=.true.
            endif
          endif
        else
          restrt=.false.
          test=0.d0
          do 43 i=1,n
            temp=(abs(x(i)-xold(i)))/max(abs(x(i)),1.d0)
            if(temp.gt.test)test=temp
43        continue
          if(test.lt.TOLX)return
        endif
44    continue
c      pause 'MAXITS exceeded in broydn'
      Write(*,*)'MAXITS exceeded in broydn'
      stop
      END
C-----------------------------------------------------------------------
      SUBROUTINE fdjac(n,x,fvec,np,df)
      INTEGER n,np,NMAX
      DOUBLE PRECISION df(np,np),fvec(n),x(n),EPS
      PARAMETER (NMAX=40,EPS=1.d-8)
CU    USES funcv
      INTEGER i,j
      DOUBLE PRECISION h,temp,f(NMAX)
      do 12 j=1,n
        temp=x(j)
        h=EPS*abs(temp)
        if(h.eq.0.d0)h=EPS
        x(j)=temp+h
        h=x(j)-temp
        call funcv(n,x,f)
        x(j)=temp
        do 11 i=1,n
          df(i,j)=(f(i)-fvec(i))/h
11      continue
12    continue
      return
      END
C-----------------------------------------------------------------------
      FUNCTION fmin(x)
      INTEGER n,NP
      DOUBLE PRECISION fmin,x(*),fvec
      PARAMETER (NP=40)
      COMMON /newtv/ fvec(NP),n
      SAVE /newtv/
CU    USES funcv
      INTEGER i
      DOUBLE PRECISION sum
      call funcv(n,x,fvec)
      sum=0.d0
      do 11 i=1,n
        sum=sum+fvec(i)**2
11    continue
      fmin=0.5d0*sum
      return
      END
C-----------------------------------------------------------------------
      SUBROUTINE lnsrch(n,xold,fold,g,p,x,f,stpmax,check,func)
      INTEGER n
      LOGICAL check
      DOUBLE PRECISION f,fold,stpmax,g(n),p(n),x(n),xold(n),func,ALF
     *,TOLX
      PARAMETER (ALF=1.d-4,TOLX=1.d-16)
      EXTERNAL func
CU    USES func
      INTEGER i
      DOUBLE PRECISION a,alam,alam2,alamin,b,disc,f2,fold2,rhs1,rhs2
     *,slope,sum,temp,
     *test,tmplam
      check=.false.
      sum=0.d0
      do 11 i=1,n
        sum=sum+p(i)*p(i)
11    continue
      sum=sqrt(sum)
      if(sum.gt.stpmax)then
        do 12 i=1,n
          p(i)=p(i)*stpmax/sum
12      continue
      endif
      slope=0.d0
      do 13 i=1,n
        slope=slope+g(i)*p(i)
13    continue
      test=0.d0
      do 14 i=1,n
        temp=abs(p(i))/max(abs(xold(i)),1.d0)
        if(temp.gt.test)test=temp
14    continue
      alamin=TOLX/test
      alam=1.d0
1     continue
        do 15 i=1,n
          x(i)=xold(i)+alam*p(i)
15      continue
        f=func(x)
        if(alam.lt.alamin)then
          do 16 i=1,n
            x(i)=xold(i)
16        continue
          check=.true.
          return
        else if(f.le.fold+ALF*alam*slope)then
          return
        else
          if(alam.eq.1.d0)then
            tmplam=-slope/(2.d0*(f-fold-slope))
          else
            rhs1=f-fold-alam*slope
            rhs2=f2-fold2-alam2*slope
            a=(rhs1/alam**2-rhs2/alam2**2)/(alam-alam2)
            b=(-alam2*rhs1/alam**2+alam*rhs2/alam2**2)/(alam-alam2)
            if(a.eq.0.d0)then
              tmplam=-slope/(2.d0*b)
            else
              disc=b*b-3.d0*a*slope
              if(disc.lt.0.d0) stop 'roundoff problem in lnsrch'
              tmplam=(-b+sqrt(disc))/(3.d0*a)
            endif
            if(tmplam.gt..5d0*alam)tmplam=.5d0*alam
          endif
        endif
        alam2=alam
        f2=f
        fold2=fold
        alam=max(tmplam,.1d0*alam)
      goto 1
      END
C-----------------------------------------------------------------------
      SUBROUTINE qrdcmp(a,n,np,c,d,sing)
      INTEGER n,np
      DOUBLE PRECISION a(np,np),c(n),d(n)
      LOGICAL sing
      INTEGER i,j,k
      DOUBLE PRECISION scale,sigma,sum,tau
      sing=.false.
      do 17 k=1,n-1
        scale=0.d0
        do 11 i=k,n
          scale=max(scale,abs(a(i,k)))
11      continue
        if(scale.eq.0.d0)then
          sing=.true.
          c(k)=0.d0
          d(k)=0.d0
        else
          do 12 i=k,n
            a(i,k)=a(i,k)/scale
12        continue
          sum=0.d0
          do 13 i=k,n
            sum=sum+a(i,k)**2
13        continue
          sigma=sign(sqrt(sum),a(k,k))
          a(k,k)=a(k,k)+sigma
          c(k)=sigma*a(k,k)
          d(k)=-scale*sigma
          do 16 j=k+1,n
            sum=0.d0
            do 14 i=k,n
              sum=sum+a(i,k)*a(i,j)
14          continue
            tau=sum/c(k)
            do 15 i=k,n
              a(i,j)=a(i,j)-tau*a(i,k)
15          continue
16        continue
        endif
17    continue
      d(n)=a(n,n)
      if(d(n).eq.0.d0)sing=.true.
      return
      END
C-----------------------------------------------------------------------
      SUBROUTINE qrupdt(r,qt,n,np,u,v)
      INTEGER n,np
      DOUBLE PRECISION r(np,np),qt(np,np),u(np),v(np)
CU    USES rotate
      INTEGER i,j,k
      do 11 k=n,1,-1
        if(u(k).ne.0.d0)goto 1
11    continue
      k=1
1     do 12 i=k-1,1,-1
        call rotate(r,qt,n,np,i,u(i),-u(i+1))
        if(u(i).eq.0.d0)then
          u(i)=abs(u(i+1))
        else if(abs(u(i)).gt.abs(u(i+1)))then
          u(i)=abs(u(i))*sqrt(1.d0+(u(i+1)/u(i))**2)
        else
          u(i)=abs(u(i+1))*sqrt(1.d0+(u(i)/u(i+1))**2)
        endif
12    continue
      do 13 j=1,n
        r(1,j)=r(1,j)+u(1)*v(j)
13    continue
      do 14 i=1,k-1
        call rotate(r,qt,n,np,i,r(i,i),-r(i+1,i))
14    continue
      return
      END
C-----------------------------------------------------------------------
      SUBROUTINE rsolv(a,n,np,d,b)
      INTEGER n,np
      DOUBLE PRECISION a(np,np),b(n),d(n)
      INTEGER i,j
      DOUBLE PRECISION sum
      b(n)=b(n)/d(n)
      do 12 i=n-1,1,-1
        sum=0.d0
        do 11 j=i+1,n
          sum=sum+a(i,j)*b(j)
11      continue
        b(i)=(b(i)-sum)/d(i)
12    continue
      return
      END
C-----------------------------------------------------------------------
      SUBROUTINE rotate(r,qt,n,np,i,a,b)
      INTEGER n,np,i
      DOUBLE PRECISION a,b,r(np,np),qt(np,np)
      INTEGER j
      DOUBLE PRECISION c,fact,s,w,y
      if(a.eq.0.d0)then
        c=0.d0
        s=sign(1.d0,b)
      else if(abs(a).gt.abs(b))then
        fact=b/a
        c=sign(1.d0/sqrt(1.d0+fact**2),a)
        s=fact*c
      else
        fact=a/b
        s=sign(1.d0/sqrt(1.d0+fact**2),b)
        c=fact*s
      endif
      do 11 j=i,n
        y=r(i,j)
        w=r(i+1,j)
        r(i,j)=c*y-s*w
        r(i+1,j)=s*y+c*w
11    continue
      do 12 j=1,n
        y=qt(i,j)
        w=qt(i+1,j)
        qt(i,j)=c*y-s*w
        qt(i+1,j)=s*y+c*w
12    continue
      return
      END
C========================================================================


*     MC01QD EXAMPLE PROGRAM TEXT
*     Copyright (c) 2002-2020 NICONET e.V.
*
*     .. Parameters ..
      INTEGER          NIN, NOUT
      PARAMETER        ( NIN = 5, NOUT = 6 )
      INTEGER          DAMAX, DBMAX
      PARAMETER        ( DAMAX = 10, DBMAX = 10 )
*     .. Local Scalars ..
      INTEGER          DA, DB, DBB, DQ, DR, I, IMAX, INFO, IWARN
*     .. Local Arrays ..
      DOUBLE PRECISION A(DAMAX+1), B(DBMAX+1), RQ(DAMAX+1)
*     .. External Subroutines ..
      EXTERNAL         MC01QD
*     .. Executable Statements ..
*
      WRITE ( NOUT, FMT = 99999 )
*     Skip the heading in the data file and read the data.
      READ ( NIN, FMT = '()' )
      READ ( NIN, FMT = * ) DA
      IF ( DA.LE.-2 .OR. DA.GT.DAMAX ) THEN
         WRITE ( NOUT, FMT = 99991 ) DA
      ELSE
         READ ( NIN, FMT = * ) ( A(I), I = 1,DA+1 )
         READ ( NIN, FMT = * ) DB
         DBB = DB
         IF ( DB.LE.-1 .OR. DB.GT.DBMAX ) THEN
            WRITE ( NOUT, FMT = 99990 ) DB
         ELSE
            READ ( NIN, FMT = * ) ( B(I), I = 1,DB+1 )
*           Compute Q(x) and R(x) from the given A(x) and B(x).
            CALL MC01QD( DA, DB, A, B, RQ, IWARN, INFO )
*
            IF ( INFO.NE.0 ) THEN
               WRITE ( NOUT, FMT = 99998 ) INFO
            ELSE
               IF ( IWARN.NE.0 ) THEN
                  WRITE ( NOUT, FMT = 99997 ) IWARN
                  WRITE ( NOUT, FMT = 99996 ) DBB, DB
               END IF
               WRITE ( NOUT, FMT = 99995 )
               DQ = DA - DB
               DR = DB - 1
               IMAX = DQ
               IF ( DR.GT.IMAX ) IMAX = DR
               DO 20 I = 0, IMAX
                  IF ( I.LE.DQ .AND. I.LE.DR ) THEN
                     WRITE ( NOUT, FMT = 99994 ) I, RQ(DB+I+1), RQ(I+1)
                  ELSE IF ( I.LE.DQ ) THEN
                     WRITE ( NOUT, FMT = 99993 ) I, RQ(DB+I+1)
                  ELSE
                     WRITE ( NOUT, FMT = 99992 ) I, RQ(I+1)
                  END IF
   20          CONTINUE
            END IF
         END IF
      END IF
*
      STOP
*
99999 FORMAT (' MC01QD EXAMPLE PROGRAM RESULTS',/1X)
99998 FORMAT (' INFO on exit from MC01QD = ',I2)
99997 FORMAT (' IWARN on exit from MC01QD = ',I2,/)
99996 FORMAT (' The degree of the denominator polynomial B(x) has been',
     $       ' reduced from ',I2,' to ',I2,/)
99995 FORMAT (' The coefficients of the polynomials Q(x) and R(x) are ',
     $       //'                    Q(x)            R(x) ',/' power of',
     $       ' x     coefficient     coefficient ')
99994 FORMAT (2X,I5,9X,F9.4,7X,F9.4)
99993 FORMAT (2X,I5,9X,F9.4)
99992 FORMAT (2X,I5,25X,F9.4)
99991 FORMAT (/' DA is out of range.',/' DA = ',I5)
99990 FORMAT (/' DB is out of range.',/' DB = ',I5)
      END


program main
    use sim
    implicit none

    type(WaterAbsorption_)::seed
    type(FEMDomain_),target :: water,tissue,tissue2,tissue3
    type(MaterialProp_):: Permiability, YoungsModulus,PoissonRatio
    type(MaterialProp_):: density,Cohesion,phi,psi
    type(Boundary_) :: disp_x,disp_y,disp_z
    type(Boundary_) :: traction_x,traction_y,traction_z
    type(Boundary_) :: flux, const

    !  create mesh
    call water%create(Name="water",MeshType="rectangular3D",x_num=20,y_num=10,x_len=300.0d0, y_len=50.0d0,&
        thickness=50.0d0,division=5)
    call tissue%copy(water,onlyMesh=.true.)
    call tissue%rename("tissue")

    ! create material
    ! for deformation analysis
    call YoungsModulus%create(Name="YoungsModulus",ParaValue=12000.0d0,Layer=1)
    call YoungsModulus%create(Name="YoungsModulus",x_max=150.0d0,x_min=0.0d0,y_max=70.0d0,y_min=0.0d0,&
    z_max=70.0d0,z_min=0.0d0,ParaValue=6000.0d0,Layer=1)
    call PoissonRatio%create(Name="PoissonRatio",ParaValue=0.30d0,Layer=2)
    !call PoissonRatio%create(Name="PoissonRatio",x_max=100.0d0,x_min=0.0d0,y_max=50.0d0,y_min=0.0d0,&
    !z_max=50.0d0,z_min=0.0d0,ParaValue=0.10d0,Layer=2)
    call Density%create(Name="Density",ParaValue=0.00d0,Layer=3)
    call Cohesion%create(Name="Cohesion",ParaValue=100000000.0d0,Layer=4)
    call Phi%create(Name="Psi",ParaValue=0.0d0,Layer=5)
    call psi%create(Name="psi",ParaValue=0.0d0,Layer=6)

    ! for diffusion analysis
    call Permiability%create(Name="Permiability",x_max=300.0d0,x_min=100.0d0,y_max=50.0d0,y_min=0.0d0,&
    z_max=50.0d0,z_min=0.0d0,ParaValue=-0.0010d0,Layer=1)
    call Permiability%create(Name="Permiability",x_max=100.0d0,x_min=0.0d0,y_max=50.0d0,y_min=0.0d0,&
    z_max=50.0d0,z_min=0.0d0,ParaValue=-0.000010d0,Layer=1)

    ! import Materials onto FEMDomains
    call tissue%import(Materials=.true., Material=YoungsModulus)
    call tissue%import(Materials=.true., Material=PoissonRatio)
    call tissue%import(Materials=.true., Material=Density)
    call tissue%import(Materials=.true., Material=Cohesion)
    call tissue%import(Materials=.true., Material=Phi)
    call tissue%import(Materials=.true., Material=Psi)

    call water%import(Materials=.true., Material=Permiability)

    ! visualize material
    call YoungsModulus%gmsh(Name="YoungsModulus",Tag="M : YoungsModulus (kPa)")
    call PoissonRatio%gmsh(Name="PoissonRatio",Tag="M : PoissonRatio")
    call Permiability%gmsh(Name="Permiability",Tag="M : Permiability (cm/s)")

    ! createBoundary Conditions
    ! for Deformation analysis
    ! fixed boundary
    ! If multiple values are to be set for a Dirichlet/Neumann/Time Boundary condition,
    ! please use Layer (1, 2, 3...).
    call disp_x%create(Category="Dirichlet",x_max=1.0d0,x_min=-5.0d0,y_max=100.0d0,y_min=-100.0d0,&
    z_max=100.0d0,z_min=-100.0d0,BoundValue=0.0d0,Layer=1,Name="disp_x")
    call disp_y%create(Category="Dirichlet",x_max=1.0d0,x_min=-5.0d0,y_max=100.0d0,y_min=-100.0d0,&
    z_max=100.0d0,z_min=-100.0d0,BoundValue=0.0d0,Layer=2,Name="disp_y")
    call disp_z%create(Category="Dirichlet",x_max=1.0d0,x_min=-5.0d0,y_max=100.0d0,y_min=-100.0d0,&
    z_max=100.0d0,z_min=-100.0d0,BoundValue=0.0d0,Layer=3,Name="disp_z")

    ! displacement-loading
    call disp_z%create(Category="Dirichlet",x_max=310.0d0,x_min=299.0d0,y_max=100.0d0,y_min=-100.0d0,&
    z_max=100.0d0,z_min=-100.0d0,BoundValue=-100.0d0,Layer=3,Name="disp_z")

    ! traction boundary conditions
    !call traction_x%create("Neumann",x_max=300.0d0,x_min=30.0d0,y_max=120.0d0,y_min=-10.0d0,&
    !    z_max=10.0d0,z_min=-100.0d0,BoundValue=5.0d0)
    !call traction_x%create("Neumann",x_max=-300.0d0,x_min=-400.0d0,y_max=10.0d0,y_min=-1.0d0,&
    !    z_max=1.0d0,z_min=-1.0d0,BoundValue=0.0d0)
    !call traction_x%create("Neumann",x_max=100.0d0,x_min=30.0d0,y_max=520.0d0,y_min=500.0d0,&
    !    z_max=-10.0d0,z_min=-100.0d0,BoundValue=-2.0d0)

    ! for Flow analysis
    ! known value
    call const%create(category="Dirichlet",x_max=5.0d0,x_min=-1.0d0,y_max=100.0d0,y_min=-100.0d0,&
    z_max=100.0d0,z_min=-100.0d0,BoundValue=0.0d0,Name="const",Layer=1)
    ! flux boundary
    call const%create(category="Dirichlet",x_max=310.0d0,x_min=299.0d0,y_max=100.0d0,y_min=-100.0d0,&
    z_max=100.0d0,z_min=-100.0d0,BoundValue=20.0d0,Name="const",Layer=1)

    ! visualize on Gmsh
    !call disp_x%gmsh(Dirichlet=.true.,Name="disp_x",Tag="B : disp_x")
    !call disp_y%gmsh(Dirichlet=.true.,Name="disp_y",Tag="B : disp_y")
    !call disp_z%gmsh(Dirichlet=.true.,Name="disp_z",Tag="B : disp_z")

    !call traction_x%gmsh(Neumann=.true.,Name="traction_x",Tag="traction_x")
    !call traction_y%gmsh(Neumann=.true.,Name="traction_y",Tag="traction_y")
    !call traction_z%gmsh(Neumann=.true.,Name="traction_z",Tag="traction_z")

    !call const%gmsh(Dirichlet=.true.,Name="Constant",Tag="B : Constant")

    !call Flux%gmsh(Neumann=.true.,Name="Flux",Tag="Flux")

    ! import boundaries onto FEMDomains
    call tissue%import(Boundaries=.true., Boundary=disp_x)
    call tissue%import(Boundaries=.true., Boundary=disp_y)
    call tissue%import(Boundaries=.true., Boundary=disp_z)
    call water%import(Boundaries=.true., Boundary=const)
    !call water%import(Boundaries=.true., Boundary=Flux)

    call tissue%show()
    call water%show()

    ! bake data by using templates
    call tissue%bake(template="FiniteDeform_")
    call water%bake(template="DiffusionEq_")

    ! visualize Domains
    call tissue%gmsh(Name="tissue",Tag="Tissue Domain")
    call water%gmsh(Name="water",Tag="Water Domain")

    call tissue%export(Name="tissue")
    call water%export(Name="water")

    ! mount data into a seed (An instance of Water-Absorption Solver)

    call seed%import(Water=water,Tissue=tissue)

    ! run simulation
    call seed%run(timestep=10,dt=0.0010d0,SolverType="BiCGSTAB",Display=.true.)

    ! visualize data
    !call seed%display()

end program main


subroutine performance
use fdeclarations

implicit none

!For 5 classes of IHS
do i=1,z
if (suitability(i,zz)<=0.2) then
comatrix(i,1,zz)=1
else if (suitability(i,zz)>0.2 .and. suitability(i,zz)<=0.4) then
comatrix(i,1,zz)=2
else if (suitability(i,zz)>0.4 .and. suitability(i,zz)<=0.6) then
comatrix(i,1,zz)=2
else if (suitability(i,zz)>0.6 .and. suitability(i,zz)<=0.8) then
comatrix(i,1,zz)=3
else if (suitability(i,zz)>0.8) then
comatrix(i,1,zz)=3
end if
end do

!do i=1,z
!if (suitability(i,zz)<=0.3) then
!comatrix(i,1,zz)=1
!else if (suitability(i,zz)>0.3 .and. suitability(i,zz)<=0.6) then
!comatrix(i,1,zz)=2
!else if (suitability(i,zz)>0.6 .and. suitability(i,zz)<=1) then
!comatrix(i,1,zz)=3
!end if
!end do

do i=1,z
if (co1matrix(i,zz)<=0.2) then
comatrix(i,2,zz)=1
else if (co1matrix(i,zz)>0.2 .and. co1matrix(i,zz)<=0.4) then
comatrix(i,2,zz)=2
else if (co1matrix(i,zz)>0.4 .and. co1matrix(i,zz)<=0.6) then
comatrix(i,2,zz)=2
else if (co1matrix(i,zz)>0.6 .and. co1matrix(i,zz)<=0.8) then
comatrix(i,2,zz)=3
else if (co1matrix(i,zz)>0.8) then
comatrix(i,2,zz)=3
end if
end do

!do i=1,z
!if (co1matrix(i,zz)<=0.3) then
!comatrix(i,2,zz)=1
!else if (co1matrix(i,zz)>0.3 .and. co1matrix(i,zz)<=0.6) then
!comatrix(i,2,zz)=2
!else if (co1matrix(i,zz)>0.6 .and. co1matrix(i,zz)<=1) then
!comatrix(i,2,zz)=3
!end if
!end do

do i=1,z
if (comatrix(i,1,zz) .eq. comatrix(i,2,zz)) then
comatrix(i,3,zz)=1
else
comatrix(i,3,zz)=0
end if
icci(zz)=(sum(comatrix(1:z,3,zz)))/z
end do
end subroutine performance


   PROGRAM GALAHAD_LSTR_EXAMPLE  !  GALAHAD 2.2 - 27/05/2008 AT 16:45 GMT.
   USE GALAHAD_LSTR_DOUBLE                            ! double precision version
   IMPLICIT NONE
   INTEGER, PARAMETER :: working = KIND( 1.0D+0 ) ! set precision
   REAL ( KIND = working ), PARAMETER :: one = 1.0_working, zero = 0.0_working
   INTEGER, PARAMETER :: n = 50, m = 2 * n            ! problem dimensions
   INTEGER :: i
   REAL ( KIND = working ) :: radius = 1.0_working  ! radius of one
   REAL ( KIND = working ), DIMENSION( n ) :: X, V
   REAL ( KIND = working ), DIMENSION( m ) :: U, RES
   TYPE ( LSTR_data_type ) :: data
   TYPE ( LSTR_control_type ) :: control
   TYPE ( LSTR_inform_type ) :: inform
   CALL LSTR_initialize( data, control )  ! Initialize control parameters
   control%steihaug_toint = .FALSE.       ! Try for an accurate solution
   control%fraction_opt = 0.99            ! Only require 99% of the best
   U = one                                ! The term b is a vector of ones
   inform%status = 1
   DO                                     ! Iteration to find the minimizer
      CALL LSTR_solve( m, n, radius, X, U, V, data, control, inform )
      SELECT CASE( inform%status )  !  Branch as a result of inform%status
      CASE( 2 )                     !  Form u <- u + A * v
        U( : n ) = U( : n ) + V     !  A^T = ( I : diag(1:n) )
        DO i = 1, n
          U( n + i ) = U( n + i ) + i * V( i )
        END DO
      CASE( 3 )                     !  Form v <- v + A^T * u
        V = V + U( : n )
        DO i = 1, n
          V( i ) = V( i ) + i * U( n + i )
        END DO
      CASE ( 4 )                    !  Restart
         U = one                    !  re-initialize u to b
      CASE ( - 30, 0 )              !  Successful return
         RES = one                  !  Compute the residuals for checking
         RES( : n )  = RES( : n ) - X
         DO i = 1, n
           RES( n + i ) = RES( n + i ) - i * X( i )
         END DO
         WRITE( 6, "( 1X, I0, ' 1st pass and ', I0, ' 2nd pass iterations' )" ) &
           inform%iter, inform%iter_pass2
         WRITE( 6, "( '   ||x||  recurred and calculated = ', 2ES16.8 )" )      &
           inform%x_norm, SQRT( DOT_PRODUCT( X, X ) )
         WRITE( 6, "( ' ||Ax-b|| recurred and calculated = ', 2ES16.8 )" )      &
           inform%r_norm, SQRT( DOT_PRODUCT( RES, RES ) )
         CALL LSTR_terminate( data, control, inform ) ! delete internal workspace
         EXIT
      CASE DEFAULT                  !  Error returns
         WRITE( 6, "( ' LSTR_solve exit status = ', I6 ) " ) inform%status
         CALL LSTR_terminate( data, control, inform ) ! delete internal workspace
         EXIT
      END SELECT
   END DO
   END PROGRAM GALAHAD_LSTR_EXAMPLE


!-----------------------------------------------------------------------
      program test

      include 'ceedf.h'

      integer ceed,err,i,j
      integer stridesu(3)
      integer erestrictx,erestrictui
      integer erestrictucoarse,erestrictufine
      integer bx,bufine,bucoarse,bctof
      integer qf_setup,qf_mass
      integer op_setup,op_masscoarse,op_massfine
      integer op_prolong,op_restrict
      integer qdata,x,ucoarse,ufine,vcoarse,vfine,pmultfine
      integer nelem,pcoarse,pfine,q
      parameter(nelem=15)
      parameter(pcoarse=3)
      parameter(pfine=5)
      parameter(q=8)
      integer nx,nucoarse,nufine
      parameter(nx=nelem+1)
      parameter(nucoarse=nelem*(pcoarse-1)+1)
      parameter(nufine=nelem*(pfine-1)+1)
      integer indx(nelem*2)
      integer inducoarse(nelem*pcoarse)
      integer indufine(nelem*pfine)
      real*8 arrx(nx)
      integer*8 voffset,xoffset,ioffset
      real*8 val
      integer interpsize
      parameter(interpsize=pcoarse*pfine);
      real*8 interp1d(interpsize),interpctof(interpsize)

      real*8 hv(nufine)
      real*8 total

      character arg*32

      call getarg(1,arg)
      call ceedinit(trim(arg)//char(0),ceed,err)

      do i=0,nx-1
        arrx(i+1)=i/(nx-1.d0)
      enddo
      do i=0,nelem-1
        indx(2*i+1)=i
        indx(2*i+2)=i+1
      enddo
      call ceedelemrestrictioncreate(ceed,nelem,2,1,1,nx,ceed_mem_host,&
     & ceed_use_pointer,indx,erestrictx,err)

      do i=0,nelem-1
        do j=0,pcoarse-1
          inducoarse(pcoarse*i+j+1)=i*(pcoarse-1)+j
        enddo
      enddo
      call ceedelemrestrictioncreate(ceed,nelem,pcoarse,1,1,nucoarse,&
     & ceed_mem_host,ceed_use_pointer,inducoarse,erestrictucoarse,err)

      do i=0,nelem-1
        do j=0,pfine-1
          indufine(pfine*i+j+1)=i*(pfine-1)+j
        enddo
      enddo
      call ceedelemrestrictioncreate(ceed,nelem,pfine,1,1,nufine,&
     & ceed_mem_host,ceed_use_pointer,indufine,erestrictufine,err)

     stridesu=[1,q,q]
      call ceedelemrestrictioncreatestrided(ceed,nelem,q,1,q*nelem,stridesu,&
     & erestrictui,err)

      call ceedbasiscreatetensorh1lagrange(ceed,1,1,2,q,ceed_gauss,bx,err)
      call ceedbasiscreatetensorh1lagrange(ceed,1,1,pfine,q,ceed_gauss,&
     & bufine,err)

      call ceedqfunctioncreateinteriorbyname(ceed,"Mass1DBuild",qf_setup,err)
      call ceedqfunctioncreateinteriorbyname(ceed,"MassApply",qf_mass,err)

      call ceedoperatorcreate(ceed,qf_setup,ceed_qfunction_none,&
     & ceed_qfunction_none,op_setup,err)
      call ceedoperatorcreate(ceed,qf_mass,ceed_qfunction_none,&
     & ceed_qfunction_none,op_massfine,err)

      call ceedvectorcreate(ceed,nx,x,err)
      xoffset=0
      call ceedvectorsetarray(x,ceed_mem_host,ceed_use_pointer,arrx,xoffset,err)
      call ceedvectorcreate(ceed,nelem*q,qdata,err)

      call ceedoperatorsetfield(op_setup,'weights',ceed_elemrestriction_none,&
     & bx,ceed_vector_none,err)
      call ceedoperatorsetfield(op_setup,'dx',erestrictx,bx,&
     & ceed_vector_active,err)
      call ceedoperatorsetfield(op_setup,'qdata',erestrictui,&
     & ceed_basis_collocated,ceed_vector_active,err)
      call ceedoperatorsetfield(op_massfine,'qdata',erestrictui,&
     & ceed_basis_collocated,qdata,err)
      call ceedoperatorsetfield(op_massfine,'u',erestrictufine,bufine,&
     & ceed_vector_active,err)
      call ceedoperatorsetfield(op_massfine,'v',erestrictufine,bufine,&
     & ceed_vector_active,err)

      call ceedoperatorapply(op_setup,x,qdata,ceed_request_immediate,err)

! Create multigrid level
      call ceedvectorcreate(ceed,nufine,pmultfine,err)
      val=1.0
      call ceedvectorsetvalue(pmultfine,val,err)
      call ceedbasiscreatetensorh1lagrange(ceed,1,1,pcoarse,q,ceed_gauss,&
     & bucoarse,err)
      call ceedbasiscreatetensorh1lagrange(ceed,1,1,pcoarse,pfine,&
     & ceed_gauss_lobatto,bctof,err)
      call ceedbasisgetinterp1d(bctof,interp1d,ioffset,err)
      do i=1,interpsize
        interpctof(i)=interp1d(ioffset+i)
      enddo
      call ceedoperatormultigridlevelcreateh1(op_massfine,pmultfine,&
     & erestrictucoarse,bucoarse,interpctof,op_masscoarse,&
     & op_prolong,op_restrict,err)

! Coarse problem
      call ceedvectorcreate(ceed,nucoarse,ucoarse,err)
      val=1.0
      call ceedvectorsetvalue(ucoarse,val,err)
      call ceedvectorcreate(ceed,nucoarse,vcoarse,err)
      call ceedoperatorapply(op_masscoarse,ucoarse,vcoarse,&
     & ceed_request_immediate,err)

! Check output
      call ceedvectorgetarrayread(vcoarse,ceed_mem_host,hv,voffset,err)
      total=0.
      do i=1,nucoarse
        total=total+hv(voffset+i)
      enddo
      if (abs(total-1.)>1.0d-10) then
! LCOV_EXCL_START
        write(*,*) 'Computed Area: ',total,' != True Area: 1.0'
! LCOV_EXCL_STOP
      endif
      call ceedvectorrestorearrayread(vcoarse,hv,voffset,err)

! Prolong coarse u
      call ceedvectorcreate(ceed,nufine,ufine,err)
      call ceedoperatorapply(op_prolong,ucoarse,ufine,&
     & ceed_request_immediate,err)

! Fine problem
      call ceedvectorcreate(ceed,nufine,vfine,err)
      call ceedoperatorapply(op_massfine,ufine,vfine,&
     & ceed_request_immediate,err)

! Check output
      call ceedvectorgetarrayread(vfine,ceed_mem_host,hv,voffset,err)
      total=0.
      do i=1,nufine
        total=total+hv(voffset+i)
      enddo
      if (abs(total-1.)>1.0d-10) then
! LCOV_EXCL_START
        write(*,*) 'Computed Area Fine Grid: ',total,' != True Area: 1.0'
! LCOV_EXCL_STOP
      endif
      call ceedvectorrestorearrayread(vfine,hv,voffset,err)

! Restrict state to coarse grid
      call ceedoperatorapply(op_restrict,vfine,vcoarse,&
     & ceed_request_immediate,err)

! Check output
      call ceedvectorgetarrayread(vcoarse,ceed_mem_host,hv,voffset,err)
      total=0.
      do i=1,nucoarse
        total=total+hv(voffset+i)
      enddo
      if (abs(total-1.)>1.0d-10) then
! LCOV_EXCL_START
        write(*,*) 'Computed Area Coarse Grid: ',total,' != True Area: 1.0'
! LCOV_EXCL_STOP
      endif
      call ceedvectorrestorearrayread(vcoarse,hv,voffset,err)

      call ceedvectordestroy(qdata,err)
      call ceedvectordestroy(x,err)
      call ceedvectordestroy(ucoarse,err)
      call ceedvectordestroy(ufine,err)
      call ceedvectordestroy(vcoarse,err)
      call ceedvectordestroy(vfine,err)
      call ceedvectordestroy(pmultfine,err)
      call ceedoperatordestroy(op_masscoarse,err)
      call ceedoperatordestroy(op_massfine,err)
      call ceedoperatordestroy(op_prolong,err)
      call ceedoperatordestroy(op_restrict,err)
      call ceedoperatordestroy(op_setup,err)
      call ceedqfunctiondestroy(qf_mass,err)
      call ceedqfunctiondestroy(qf_setup,err)
      call ceedbasisdestroy(bucoarse,err)
      call ceedbasisdestroy(bufine,err)
      call ceedbasisdestroy(bctof,err)
      call ceedbasisdestroy(bx,err)
      call ceedelemrestrictiondestroy(erestrictucoarse,err)
      call ceedelemrestrictiondestroy(erestrictufine,err)
      call ceedelemrestrictiondestroy(erestrictx,err)
      call ceedelemrestrictiondestroy(erestrictui,err)
      call ceeddestroy(ceed,err)
      end
!-----------------------------------------------------------------------


PROGRAM main
USE PARAM_MOD
IMPLICIT NONE

!constant temperature
real, parameter   :: temp_ = 15.

!constant light
real, parameter   :: PAR_  = 1000.

! Time step
real, parameter   :: dtsec   = 300.   ! Unit: seconds
real, parameter   :: d_per_s = 864d2 ! how many seconds in one day
integer, parameter:: nsave   = INT(d_per_s)/INT(dtsec) ! Save results once per day
!Total time (days) of model running
integer, parameter:: NDAYS = 360

!Total number of time steps
integer, parameter:: Nstep = NDAYS*INT(d_per_s)/INT(dtsec)

! Total nutrient concentration:
real, parameter   :: A    = 2.

! Number of beta values
integer, parameter:: NB   = 5
real, parameter   :: beta(NB) = [0.01, 0.1,0.5, 1., 2.]

! Number of parameters to test
integer, parameter:: NPS  = 20

real    :: B
real    :: current_sec

! Biological tracers
real    :: NO3, PHY, ZOO, VNO3, VPHY, VZOO, COVNP, COVNZ, COVPZ
real    :: NO3_,PHY_,ZOO_
! Fluxes
real    :: PP_P_N   =0d0
real    :: PP_N_P   =0d0
real    :: PP_PP_NP =0d0
real    :: PP_NP_NN =0d0
real    :: PP_NP_PP =0d0
real    :: PP_PZ_PP =0d0
real    :: PP_Z_P   =0d0
real    :: PP_N_Z   =0d0
real    :: PP_NP_PZ =0d0
real    :: PP_NN_NP =0d0
real    :: PP_NN_NZ =0d0
real    :: PP_ZZ_PZ =0d0
real    :: PP_PZ_NZ =0d0
real    :: PP_NZ_PZ =0d0
real    :: PP_NZ_NP =0d0
real    :: PP_NZ_ZZ =0d0

! To count time
real    :: start, finish
integer :: i,j,k,p,it, current_day = 0
integer :: stab = 1
character(20)       :: str

! File saving simulation results for each parameter combination
character(LEN=50)   :: outfile  = '.out'

! File saving which parameter combination leads to stable model output
character(LEN=50)   :: stabfile = '.stb'

call cpu_time(start)

!the fraction of a time step in one day
dtdays = dtsec/d_per_s

!Set parameters (the parameters will be back transformed to normal values in the main subroutine):
params(imu0)    = log(1.)
params(iIopt)   = log(1d3)
params(iaI0_C)  = log(0.05)
params(iDp)     = log(0.1)
params(iKPHY)   = log(1d0)

! Test the sensitivities of gmax and mz
MaxValue(igmax) = 5.
MinValue(igmax) = 0.01
MaxValue(imz)   = 0.8
MinValue(imz)   = 0.01
MaxValue(iKN)   = 3.
MinValue(iKN)   = 0.01

DO k = 1, NB
    ! beta = B/A**2
    B = A*A*beta(k)

    DO p = 1, NPS
      params(iKN) = log(A) + log((MaxValue(iKN) - MinValue(iKN))/dble(NPS-1)*dble(p-1)&
           + MinValue(iKN))

      ! Create data out file and stabfile
      write(str, '(F12.2)') beta(k)
      str      = adjustl(str)
      outfile  = 'beta'//trim(str)//'.out'
      stabfile = 'beta'//trim(str)//'.stb'

      write(str, '(F12.2)') exp(params(iKN))
      str      = adjustl(str)
      outfile  = 'KN'//trim(str)//trim( outfile)
      stabfile = 'KN'//trim(str)//trim(stabfile)

      open (unit=11, file = stabfile,status = 'replace')
      write(11, 101) 'Gm','mz','STAB'

      open (unit=10, file = outfile, status = 'replace')
      write(10, 101) 'Gm','mz','Timestep', 'Days', 'NO3', 'PHY', 'ZOO', 'VNO3', 'VPHY', &
        'VZOO', 'COVNP', 'COVNZ', 'COVPZ','P_N', 'N_P', 'Z_P','N_Z','PP_NP','NP_NN',&
        'NP_PP','PZ_PP', 'NP_PZ',                                             &
        'NN_NP','NN_NZ','ZZ_PZ','PZ_NZ','NZ_PZ','NZ_NP','NZ_ZZ'

     DO j = 1, NPS

       params(igmax) = log((MaxValue(igmax) - MinValue(igmax))/dble(NPS-1)*dble(j-1)&
           + MinValue(igmax))

       DO i = 1, NPS
         params(imz) = log((MaxValue(imz) - MinValue(imz))/dble(NPS-1)*dble(i-1)&
           + MinValue(imz))

         !Initialize:
         NO3   = 1.0
         PHY   = 0.6
         ZOO   = A - NO3 - PHY

         ! B   = VNO3 + VPHY + VZOO + 2*(COVNP + COVPZ + COVNZ)
         VNO3     =B*0.2
         VPHY     =B*0.3
         VZOO     =B*0.2
         COVNP    =B*.05
         COVPZ    =B*.05
         COVNZ    =B*.05
         PP_P_N   =0d0
         PP_N_P   =0d0
         PP_PP_NP =0d0
         PP_NP_NN =0d0
         PP_NP_PP =0d0
         PP_PZ_PP =0d0
         PP_Z_P   =0d0
         PP_N_Z   =0d0
         PP_NP_PZ =0d0
         PP_NN_NP =0d0
         PP_NN_NZ =0d0
         PP_ZZ_PZ =0d0
         PP_PZ_NZ =0d0
         PP_NZ_PZ =0d0
         PP_NZ_NP =0d0
         PP_NZ_ZZ =0d0
         stab = 1

         do it = 1, (NSTEP+1)
         ! Calculate current timing (zero is starting time):
           current_sec = float(it-1)*dtsec

           IF (mod(it, nsave) .EQ. 1) THEN !Save results
              ! Calculate model time in days:
               current_day = int(current_sec/d_per_s)

              write(10, 200)  exp(params(igmax)), exp(params(imz)),  &
               it, current_day, NO3,PHY,ZOO, VPHY,VNO3,VZOO,COVNP,COVNZ,COVPZ,PP_P_N,&
               PP_N_P, PP_Z_P,PP_N_Z, PP_PP_NP, PP_NP_NN,PP_NP_PP,PP_PZ_PP,PP_NP_PZ,PP_NN_NP,&
               PP_NN_NZ,PP_ZZ_PZ,PP_PZ_NZ,PP_NZ_PZ,PP_NZ_NP,PP_NZ_ZZ
           ENDIF

           call NPZ_CLOSURE

           !Exit the loop if unrealistic conditions occur
           IF (NO3 < -0.1 .or. PHY < -0.1 .or. ZOO < -0.1 .or. VNO3 < -0.1 .or. &
               VPHY< -0.1 .or.VZOO < -0.1 .or.VPHY > 1D5  .or. VZOO > 1D5  .or. &
               NO3.ne.NO3 .or. PHY.ne.PHY .or.ZOO.ne.ZOO  .or. VZOO.ne.VZOO.or. &
               NO3 > 1D5  .or. PHY > 1D5  .or. ZOO > 1D5  .or. VNO3 > 1D5) THEN

               stab = 0

               ! Write into stabfile:
               write(11,201) exp(params(igmax)), exp(params(imz)),stab
               EXIT
           ENDIF

         enddo

         ! Write into stabfile after simulation is finished:
         if (stab .eq. 1) &
         write(11,201) exp(params(igmax)), exp(params(imz)), stab
       ENDDO
   ENDDO
   close(10) !Close outfile (to avoid very large file)
   close(11) !Close the stabfile
 ENDDO
ENDDO
Call cpu_time(finish)
print '("Time = ",f8.3," seconds.")', (finish-start)
101 format(100(A5,3x))
200 format(2(1pe12.2, 2x),I10, 2x, I5, 100(2x, 1pe12.2))
201 format(2(1pe12.2, 2x),I2)

CONTAINS

SUBROUTINE NPZ_CLOSURE
! This NPZ_closure model has 5 tracers: <N>, <P>, <Z>,<N'>^2, <P'>^2,<Z'>^2, <N'P'>,<N'Z'>,<P'Z'>
! Governing functions modified following Priyadarshi et al. JTB (2017)
USE PARAM_MOD
IMPLICIT NONE
real :: QN  ! cell quota related variables
real :: muNet, Dp, muSI, KN, gmax
real :: SI, CFF1, CFF2
real :: theta,  Kp
real :: Chl, NPPt, Zmort1, Zmort2

!Zooplankton feeding threshold for phytoplankton
real, parameter :: Pt_bio = 0.001d0

!-----------------------------------------------------------------------
KN   = exp(params(iKN))
Kp   = exp(params(iKPHY))

!Phytoplankton equations
call PHY_NPCLOSURE(NO3,PAR_,temp_,PHY,VPHY,VNO3, COVNP,muSI,  &
        muNet,SI,theta,QN, PP_P_N, PP_N_P, Chl, NPPt, PP_PP_NP, PP_NP_NN)
!=============================================================
!! Solve ODE functions:
!All rates have been multiplied by dtdays to get the real rate correponding to the actual time step
Dp     = exp(params(iDp))*exp(params(imu0))  ! Mortality rate of phytoplankton
tf_P   = tempBOL(Ep, temp_)
tf_Z   = tempBOL(Ez, temp_)
PP_N_P = PP_N_P + PHY*Dp*tf_P

! params(igmax) is scaled factor to imu0
! Because temp = 15 ºC, no effect of temperature actually
gmax  = exp(params(igmax)) * exp(params(imu0)) * tf_Z
Zmort = exp(params(imz))   * exp(params(imu0)) * tf_Z   ! Mortality rate of zooplankton
Select case(ZOO_MORT)
case(LINEAR)
   Zmort1 = Zmort * ZOO
   Zmort2 = Zmort
case(QUADRATIC)
   Zmort1 = Zmort *(ZOO*ZOO + VZOO)
   Zmort2 = 2.*Zmort * ZOO
case default
   stop "The correct zooplankton mortality option is NOT selected!"
end select

! Define two scratch variables that are repeatly used below
select case(grazing_formulation)
case(LINEAR)
   CFF1  = PHY
   CFF2  = 1d0
   INGES = gmax*(CFF1 * ZOO + COVPZ)
case(H2T) !Holling type 2 with threshold
   NPPt   = PHY - Pt_bio
   if (NPPt < 0.) then
      CFF1  = 0d0
      CFF2  = 1d0
      INGES = 0d0
   else
      CFF1  = NPPt/ (Kp + NPPt)
      CFF2  = Kp  / (Kp + NPPt)**2
      INGES = gmax*( CFF1 * ZOO                                      &
            - Kp * ZOO * VPHY /(Kp + NPPt)**3 + CFF2 * COVPZ)
   endif

case(H3)  !Holling type 3
   CFF1  =    PHY**2 / (Kp**2 + PHY**2)
   CFF2  = 2.*Kp**2 * PHY / (Kp**2 + PHY**2)**2
   INGES = gmax*( CFF1 * ZOO                                          &
         + Kp**2 * ZOO * VPHY*(Kp**2 - 3.*PHY**2)/(Kp**2 + PHY**2)**3 &
         + CFF2 * COVPZ )
case default
   stop "The correct option for zooplankton grazing function is NOT selected!"
end select

PP_N_Z = (1. - GGE)*INGES + Zmort1
PP_Z_P = INGES

!Update tracers:
NO3_ = NO3  + (PP_N_Z - PP_P_N + PP_N_P)*dtdays
PHY_ = PHY  + (PP_P_N - PP_Z_P - PP_N_P)*dtdays
ZOO_ = ZOO  + (PP_Z_P - PP_N_Z)*dtdays

SI   = NO3/(NO3 + KN)

PP_NP_PP = Dp * tf_P * VPHY
PP_PZ_PP = gmax*(CFF2*ZOO*VPHY + CFF1*COVPZ)

PP_PZ_NZ = muSI*(SI * COVPZ + PHY * COVNZ * KN / (KN + NO3)**2 )
PP_ZZ_PZ = gmax * (CFF2 * ZOO * COVPZ + CFF1 * VZOO)
PP_NZ_PZ = Dp*tf_P*COVPZ
PP_NZ_NP = gmax * (CFF2 * COVNP * ZOO + CFF1 * COVNZ)
PP_NZ_ZZ = Zmort2 * VZOO + gmax * (1.-GGE)*(CFF2*ZOO*COVPZ + CFF1 *VZOO)
PP_NP_PZ = Zmort2*COVPZ + gmax*(1.-GGE)*(COVPZ*CFF1 + VPHY*ZOO*CFF2)
PP_NN_NP = Dp*COVNP*tf_P
PP_NN_NZ = (CFF2*ZOO*COVNP + CFF1*COVNZ)*(1.-GGE)*gmax + Zmort2*COVNZ
VPHY     = VPHY + 2.*(PP_PP_NP - PP_NP_PP - PP_PZ_PP)*dtdays
VNO3     = VNO3 + 2.*(PP_NN_NP + PP_NN_NZ - PP_NP_NN)*dtdays
VZOO     = VZOO + 2.*(PP_ZZ_PZ - PP_NZ_ZZ           )*dtdays
COVNP    = COVNP+    (PP_NP_PZ + PP_NP_PP + PP_NP_NN - PP_PP_NP - PP_NN_NP - PP_NZ_NP)*dtdays
COVNZ    = COVNZ+    (PP_NZ_NP + PP_NZ_ZZ + PP_NZ_PZ - PP_NN_NZ - PP_PZ_NZ)*dtdays
COVPZ    = COVPZ+    (PP_PZ_NZ + PP_PZ_PP - PP_NZ_PZ - PP_NP_PZ - PP_ZZ_PZ)*dtdays
NO3      = NO3_
PHY      = PHY_
ZOO      = ZOO_
!VPHY = VPHY + (SVPHY - 2.*Dp*VPHY*tf_P                    &
!   - 2.*gmax*(CFF2 * ZOO * VPHY + CFF1 * COVPZ) )*dtdays

!VNO3 = VNO3 + (SVNO3 + 2.*Dp*COVNP*tf_P                   &
!     + 2.*(1.-GGE)*gmax*(CFF2 * ZOO * COVNP + CFF1 * COVNZ)          &
!     + 2.* Zmort2 * COVNZ) * dtdays

!VZOO = VZOO + 2.*dtdays*(GGE*gmax*( CFF2 * ZOO * COVPZ    &
!     + CFF1 * VZOO) - 2. * Zmort2 * VZOO )

!COVNP = COVNP + (SCOVNP + Dp*tf_P*(VPHY - COVNP)                     &
!    +     Zmort2    * COVPZ                                           &
!    +     gmax*ZOO * CFF2 * ((1.-GGE)* VPHY - COVNP)                 &
!    +     gmax     * CFF1 * ((1.-GGE)*COVPZ - COVNZ) )*dtdays

!COVPZ = COVPZ + ( NPPt                                               &
!    - (Dp*tf_P +  Zmort2)* COVPZ                                     &
!    + gmax * ZOO * CFF2 * (GGE * VPHY - COVPZ)                       &
!    + gmax       * CFF1 * (GGE * COVPZ - VZOO))*dtdays

!COVNZ = COVNZ + (-NPPt + Dp*tf_P*COVPZ                               &
!    + Zmort2*(VZOO - COVNZ)                                          &
!    + gmax * ZOO  * CFF2 * (GGE*COVNP + (1.-GGE)*COVPZ)              &
!    + gmax        * CFF1 * (GGE*COVNZ + (1.-GGE)*VZOO))*dtdays

return
END SUBROUTINE NPZ_CLOSURE

END PROGRAM


      SUBROUTINE TB01PX( JOB, EQUIL, N, M, P, A, LDA, B, LDB, C, LDC,
     $                   NR, INFRED, TOL, IWORK, DWORK, LDWORK, INFO )
C
C     SLICOT RELEASE 5.7.
C
C     Copyright (c) 2002-2020 NICONET e.V.
C
C     PURPOSE
C
C     To find a reduced (controllable, observable, or minimal) state-
C     space representation (Ar,Br,Cr) for any original state-space
C     representation (A,B,C). The matrix Ar is in an upper block
C     Hessenberg staircase form.
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     JOB     CHARACTER*1
C             Indicates whether the user wishes to remove the
C             uncontrollable and/or unobservable parts as follows:
C             = 'M':  Remove both the uncontrollable and unobservable
C                     parts to get a minimal state-space representation;
C             = 'C':  Remove the uncontrollable part only to get a
C                     controllable state-space representation;
C             = 'O':  Remove the unobservable part only to get an
C                     observable state-space representation.
C
C     EQUIL   CHARACTER*1
C             Specifies whether the user wishes to preliminarily balance
C             the triplet (A,B,C) as follows:
C             = 'S':  Perform balancing (scaling);
C             = 'N':  Do not perform balancing.
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The order of the original state-space representation, i.e.
C             the order of the matrix A.  N >= 0.
C
C     M       (input) INTEGER
C             The number of system inputs.  M >= 0.
C
C     P       (input) INTEGER
C             The number of system outputs.   P >= 0.
C
C     A       (input/output) DOUBLE PRECISION array, dimension (LDA,N)
C             On entry, the leading N-by-N part of this array must
C             contain the original state dynamics matrix A.
C             On exit, if INFRED(1) >= 0 and/or INFRED(2) >= 0, the
C             leading NR-by-NR part of this array contains the upper
C             block Hessenberg state dynamics matrix Ar of a minimal,
C             controllable, or observable realization for the original
C             system, depending on the value of JOB, JOB = 'M',
C             JOB = 'C', or JOB = 'O', respectively.
C             The block structure of the resulting staircase form is
C             contained in the leading INFRED(4) elements of IWORK.
C             If INFRED(1:2) < 0, then A contains the original matrix.
C
C     LDA     INTEGER
C             The leading dimension of the array A.  LDA >= MAX(1,N).
C
C     B       (input/output) DOUBLE PRECISION array, dimension (LDB,M),
C             if JOB = 'C', or (LDB,MAX(M,P)), otherwise.
C             On entry, the leading N-by-M part of this array must
C             contain the original input/state matrix B; if JOB = 'M',
C             or JOB = 'O', the remainder of the leading N-by-MAX(M,P)
C             part is used as internal workspace.
C             On exit, if INFRED(1) >= 0 and/or INFRED(2) >= 0, the
C             leading NR-by-M part of this array contains the
C             input/state matrix Br of a minimal, controllable, or
C             observable realization for the original system, depending
C             on the value of JOB, JOB = 'M', JOB = 'C', or JOB = 'O',
C             respectively. If JOB = 'C', only the first IWORK(1) rows
C             of B are nonzero.
C             If INFRED(1:2) < 0, then B contains the original matrix.
C
C     LDB     INTEGER
C             The leading dimension of the array B.  LDB >= MAX(1,N).
C
C     C       (input/output) DOUBLE PRECISION array, dimension (LDC,N)
C             On entry, the leading P-by-N part of this array must
C             contain the original state/output matrix C; if JOB = 'M',
C             or JOB = 'O', the remainder of the leading MAX(M,P)-by-N
C             part is used as internal workspace.
C             On exit, if INFRED(1) >= 0 and/or INFRED(2) >= 0, the
C             leading P-by-NR part of this array contains the
C             state/output matrix Cr of a minimal, controllable, or
C             observable realization for the original system, depending
C             on the value of JOB, JOB = 'M', JOB = 'C', or JOB = 'O',
C             respectively. If JOB = 'M', or JOB = 'O', only the last
C             IWORK(1) columns (in the first NR columns) of C are
C             nonzero.
C             If INFRED(1:2) < 0, then C contains the original matrix.
C
C     LDC     INTEGER
C             The leading dimension of the array C.
C             LDC >= MAX(1,M,P), if N > 0.
C             LDC >= 1,          if N = 0.
C
C     NR      (output) INTEGER
C             The order of the reduced state-space representation
C             (Ar,Br,Cr) of a minimal, controllable, or observable
C             realization for the original system, depending on
C             JOB = 'M', JOB = 'C', or JOB = 'O'.
C
C     INFRED  (output) INTEGER array, dimension 4
C             This array contains information on the performed reduction
C             and on structure of resulting system matrices, as follows:
C             INFRED(k) >= 0 (k = 1 or 2) if Phase k of the reduction
C                            (see METHOD) has been performed. In this
C                            case, INFRED(k) is the achieved order
C                            reduction in Phase k.
C             INFRED(k) < 0  (k = 1 or 2) if Phase k was not performed.
C                            This can also appear when Phase k was
C                            tried, but did not reduce the order, if
C                            enough workspace is provided for saving the
C                            system matrices (see LDWORK description).
C             INFRED(3)  -   the number of nonzero subdiagonals of A.
C             INFRED(4)  -   the number of blocks in the resulting
C                            staircase form at the last performed
C                            reduction phase. The block dimensions are
C                            contained in the first INFRED(4) elements
C                            of IWORK.
C
C     Tolerances
C
C     TOL     DOUBLE PRECISION
C             The tolerance to be used in rank determinations when
C             transforming (A, B, C). If the user sets TOL > 0, then
C             the given value of TOL is used as a lower bound for the
C             reciprocal condition number (see the description of the
C             argument RCOND in the SLICOT routine MB03OD);  a
C             (sub)matrix whose estimated condition number is less than
C             1/TOL is considered to be of full rank.  If the user sets
C             TOL <= 0, then an implicitly computed, default tolerance
C             (determined by the SLICOT routine TB01UD) is used instead.
C
C     Workspace
C
C     IWORK   INTEGER array, dimension (c*N+MAX(M,P)), where
C             c = 2, if JOB = 'M', and c = 1, otherwise.
C             On exit, if INFO = 0, the first INFRED(4) elements of
C             IWORK return the orders of the diagonal blocks of A.
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C             On exit, if INFO = 0, DWORK(1) returns the optimal value
C             of LDWORK.
C
C     LDWORK  INTEGER
C             The length of the array DWORK.  LDWORK >= 1, and if N > 0,
C             LDWORK >= N + MAX(N, 3*M, 3*P).
C             For optimum performance LDWORK should be larger.
C             If LDWORK >= MAX(1, N + MAX(N, 3*M, 3*P) + N*(N+M+P) ),
C             then more accurate results are to be expected by accepting
C             only those reductions phases (see METHOD), where effective
C             order reduction occurs. This is achieved by saving the
C             system matrices before each phase and restoring them if
C             no order reduction took place in that phase.
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0:  successful exit;
C             < 0:  if INFO = -i, the i-th argument had an illegal
C                   value.
C
C     METHOD
C
C     The order reduction is performed in two phases:
C
C     Phase 1:
C     If JOB = 'M' or 'C', the pair (A,B) is reduced by orthogonal
C     similarity transformations to the controllability staircase form
C     (see [1]) and a controllable realization (Ac,Bc,Cc) is extracted.
C     Ac results in an upper block Hessenberg form.
C
C     Phase 2:
C     If JOB = 'M' or 'O', the same algorithm is applied to the dual
C     of the controllable realization (Ac,Bc,Cc), or to the dual of
C     the original system, respectively, to extract an observable
C     realization (Ar,Br,Cr). If JOB = 'M', the resulting realization
C     is also controllable, and thus minimal.
C     Ar results in an upper block Hessenberg form.
C
C     REFERENCES
C
C     [1] Van Dooren, P.
C         The Generalized Eigenstructure Problem in Linear System
C         Theory. (Algorithm 1)
C         IEEE Trans. Auto. Contr., AC-26, pp. 111-129, 1981.
C
C     NUMERICAL ASPECTS
C                               3
C     The algorithm requires 0(N ) operations and is backward stable.
C
C     CONTRIBUTOR
C
C     V. Sima, Katholieke Univ. Leuven, Belgium.
C     A. Varga, DLR - Oberpfaffenhofen, April 1999.
C
C     REVISIONS
C
C     A. Varga, DLR - Oberpfaffenhofen, March 2002.
C     V.Sima, Dec. 2016, Mar. 2017.
C
C     KEYWORDS
C
C     Hessenberg form, minimal realization, multivariable system,
C     orthogonal transformation, state-space model, state-space
C     representation.
C
C     ******************************************************************
C
C     .. Parameters ..
      INTEGER           LDIZ
      PARAMETER         ( LDIZ = 1 )
      DOUBLE PRECISION  ONE, HUNDR
      PARAMETER         ( ONE = 1.0D0, HUNDR = 100.0D0 )
C     .. Scalar Arguments ..
      CHARACTER         EQUIL, JOB
      INTEGER           INFO, LDA, LDB, LDC, LDWORK, M, N, NR, P
      DOUBLE PRECISION  TOL
C     .. Array Arguments ..
      INTEGER           INFRED(*), IWORK(*)
      DOUBLE PRECISION  A(LDA,*), B(LDB,*), C(LDC,*), DWORK(*)
C     .. Local Scalars ..
      LOGICAL           LEQUIL, LNJOBC, LNJOBO, LSPACE
      INTEGER           I, IB, ICON, INDCON, ITAU, IZ, JWORK, KL, KWA,
     $                  KWB, KWC, LDWMIN, MAXMP, NCONT, WRKOPT
      DOUBLE PRECISION  ANORM, BNORM, CNORM, MAXRED
C     .. External Functions ..
      LOGICAL           LSAME
      DOUBLE PRECISION  DLANGE
      EXTERNAL          DLANGE, LSAME
C     .. External Subroutines ..
      EXTERNAL          AB07MD, DLACPY, TB01ID, TB01UD, TB01XD, XERBLA
C     .. Intrinsic Functions ..
      INTRINSIC         INT, MAX
C     .. Executable Statements ..
C
      INFO   = 0
      MAXMP  = MAX( M, P )
      LDWMIN = N + MAX( N, 3*MAXMP )
      LNJOBC = .NOT.LSAME( JOB,   'C' )
      LNJOBO = .NOT.LSAME( JOB,   'O' )
      LEQUIL =      LSAME( EQUIL, 'S' )
C
C     Test the input scalar arguments.
C
      IF( LNJOBC .AND. LNJOBO .AND. .NOT.LSAME( JOB, 'M' ) ) THEN
         INFO = -1
      ELSE IF( .NOT.LEQUIL .AND. .NOT.LSAME( EQUIL, 'N' ) ) THEN
         INFO = -2
      ELSE IF( N.LT.0 ) THEN
         INFO = -3
      ELSE IF( M.LT.0 ) THEN
         INFO = -4
      ELSE IF( P.LT.0 ) THEN
         INFO = -5
      ELSE IF( LDA.LT.MAX( 1, N ) ) THEN
         INFO = -7
      ELSE IF( LDB.LT.MAX( 1, N ) ) THEN
         INFO = -9
      ELSE IF( LDC.LT.1 .OR. ( N.GT.0 .AND. LDC.LT.MAXMP ) ) THEN
         INFO = -11
      ELSE IF( LDWORK.LT.1 .OR. ( N.GT.0 .AND. LDWORK.LT.LDWMIN ) ) THEN
         INFO = -17
      END IF
C
      IF ( INFO.NE.0 ) THEN
C
C        Error return.
C
         CALL XERBLA( 'TB01PX', -INFO )
         RETURN
      END IF
C
      INFRED(1) = -1
      INFRED(2) = -1
      INFRED(4) = 0
C
C     Quick return if possible.
C
      IF ( N.EQ.0 .OR. ( LNJOBC .AND. P.EQ.0 ) .OR.
     $                 ( LNJOBO .AND. M.EQ.0 ) ) THEN
         NR = 0
         INFRED(3) = 0
         DWORK(1)  = ONE
         RETURN
      END IF
      ANORM = DLANGE( '1-norm', N, N, A, LDA, DWORK )
      BNORM = DLANGE( '1-norm', N, M, B, LDB, DWORK )
      CNORM = DLANGE( '1-norm', P, N, C, LDC, DWORK )
*
*     In case of absolute error control, the following
*     squence can be employed:
*
*     IF( .NOT. LEQUIL .AND. BNORM.LE.TOL ) THEN
*        NR = 0
*        INFRED(1) = N
*        DWORK(1)  = ONE
*        RETURN
*     END IF
*     IF( .NOT. LEQUIL .AND. CNORM.LE.TOL ) THEN
*        NR = 0
*        INFRED(2) = N
*        DWORK(1)  = ONE
*        RETURN
*     END IF
C
C     If required, balance the triplet (A,B,C).
C     Workspace: need N.
C
C     (Note: Comments in the code beginning "Workspace:" describe the
C     minimal amount of real workspace needed at that point in the code,
C     as well as the preferred amount for good performance.)
C
      IF ( LEQUIL ) THEN
         MAXRED = MAX( HUNDR, ANORM, BNORM, CNORM )
         CALL TB01ID( 'A', N, M, P, MAXRED, A, LDA, B, LDB, C, LDC,
     $                DWORK, INFO )
      END IF
C
C     Set large workspace option and determine offsets.
C
      LSPACE = LDWORK.GE.N*( N + M + P ) + LDWMIN
      IF ( LSPACE ) THEN
         KWA = 1
         KWB = KWA + N*N
         KWC = KWB + N*M
         IZ  = KWC + P*N
      ELSE
         IZ  = 1
      END IF
C
      ITAU  = IZ
      JWORK = ITAU + N
      KL = MAX( 0, N-1 )
      IB = 1
C
      IF ( LNJOBO ) THEN
C
C        Phase 1: Eliminate uncontrolable eigenvalues.
C
         IF( LSPACE ) THEN
C
C           Save system matrices.
C
            CALL DLACPY( 'Full', N, N, A, LDA, DWORK(KWA), N )
            CALL DLACPY( 'Full', N, M, B, LDB, DWORK(KWB), N )
            CALL DLACPY( 'Full', P, N, C, LDC, DWORK(KWC), MAX( 1, P ) )
         END IF
C
C        Separate out controllable subsystem (of order NCONT):
C        A <-- Z'*A*Z,  B <-- Z'*B,  C <-- C*Z.
C
C        Workspace: need   N + MAX(N, 3*M, P).
C                   prefer larger.
C
         CALL TB01UD( 'No Z', N, M, P, A, LDA, B, LDB, C, LDC, NCONT,
     $                ICON, IWORK, DWORK(IZ), LDIZ, DWORK(ITAU), TOL,
     $                IWORK(N+1), DWORK(JWORK), LDWORK-JWORK+1, INFO )
C
         WRKOPT = INT( DWORK(JWORK) ) + JWORK - 1
C
         IF( NCONT.LT.N .OR. .NOT.LSPACE ) THEN
            IF( ICON.GT.1 ) THEN
               KL = IWORK(1) + IWORK(2) - 1
            ELSE IF( ICON.EQ.1 ) THEN
               KL = IWORK(1) - 1
            ELSE
               KL = 0
            END IF
            INFRED(1) = N - NCONT
            INFRED(4) = ICON
            IF ( LNJOBC )
     $         IB = N + 1
         ELSE
C
C           Restore system matrices.
C
            CALL DLACPY( 'Full', N, N, DWORK(KWA), N, A, LDA )
            CALL DLACPY( 'Full', N, M, DWORK(KWB), N, B, LDB )
            CALL DLACPY( 'Full', P, N, DWORK(KWC), MAX( 1, P ), C, LDC )
         END IF
      ELSE
         NCONT = N
      END IF
C
      IF ( LNJOBC ) THEN
C
C        Phase 2: Eliminate unobservable eigenvalues.
C
         IF( LSPACE .AND. ( ( LNJOBO .AND. NCONT.LT.N ) .OR.
     $                   .NOT.LNJOBO ) ) THEN
C
C           Save system matrices.
C
            CALL DLACPY( 'Full', NCONT, NCONT, A, LDA, DWORK(KWA), N )
            CALL DLACPY( 'Full', NCONT, M,     B, LDB, DWORK(KWB), N )
            CALL DLACPY( 'Full', P,     NCONT, C, LDC, DWORK(KWC),
     $                   MAX( 1, P ) )
         END IF
C
C        Separate out the observable subsystem (of order NR):
C        Form the dual of the subsystem of order NCONT (which is
C        controllable, if JOB = 'M'), leaving the rest as it is.
C
         CALL AB07MD( 'Z', NCONT, M, P, A, LDA, B, LDB, C, LDC, DWORK,
     $                1, INFO )
C
C        And separate out the controllable part of this dual subsystem.
C
C        Workspace: need   NCONT + MAX(NCONT, 3*P, M).
C                   prefer larger.
C
         CALL TB01UD( 'No Z', NCONT, P, M, A, LDA, B, LDB, C, LDC, NR,
     $                INDCON, IWORK(IB), DWORK(IZ), LDIZ, DWORK(ITAU),
     $                TOL, IWORK(IB+N), DWORK(JWORK), LDWORK-JWORK+1,
     $                INFO )
C
         WRKOPT = MAX( WRKOPT, INT( DWORK(JWORK) ) + JWORK - 1 )
C
C        Transpose and reorder (to get a block upper Hessenberg
C        matrix A), giving, for JOB = 'M', the controllable and
C        observable (i.e., minimal) part of original system.
C
         IF( NR.LT.NCONT .OR. .NOT.LSPACE ) THEN
            IF( INDCON.GT.1 ) THEN
               KL = IWORK(IB) + IWORK(IB+1) - 1
            ELSE IF( INDCON.EQ.1 ) THEN
               KL = IWORK(IB) - 1
            ELSE
               KL = 0
            END IF
            CALL TB01XD( 'Zero D', NR, P, M, KL, MAX( 0, NR-1 ),
     $                   A, LDA, B, LDB, C, LDC, DWORK, 1, INFO )
            INFRED(2) = NCONT - NR
            INFRED(4) = INDCON
            IF ( LNJOBO ) THEN
               DO 10 I = 1, INDCON
                  IWORK(I) = IWORK(IB+I-1)
   10          CONTINUE
            END IF
         ELSE
C
C           Restore system matrices.
C
            CALL DLACPY( 'Full', NCONT, NCONT, DWORK(KWA), N, A, LDA )
            CALL DLACPY( 'Full', NCONT, M,     DWORK(KWB), N, B, LDB )
            CALL DLACPY( 'Full', P,     NCONT, DWORK(KWC), MAX( 1, P ),
     $                   C, LDC )
         END IF
      ELSE
         NR = NCONT
      END IF
C
C     Set structure information and optimal workspace dimension.
C
      INFRED(3) = KL
      DWORK(1)  = MAX( WRKOPT, LDWMIN + N*( N + M + P ) )
      RETURN
C *** Last line of TB01PX ***
      END


subroutine val_fit(lon,lat,ndeg,mdeg,coef_psi,latmin,latmax,latequi,psi,psi_est,psi_nord)
use comm,only : dp, deg2rad,rad2deg,pi
implicit none

real(dp),intent(in) :: lon,lat,coef_psi(:),latmin,latmax,latequi
integer,intent(in) :: ndeg,mdeg
Real(dp),intent(out), optional :: psi,psi_est,psi_nord
!
Integer,parameter::	npt=100000

Real(dp),parameter ::     coeffc=6._dp,re=6.378_dp,lat_top=89.9_dp
Integer	::	i,j,k,l,rang

Logical :: mode
Real(dp)	::  LM,DL
Real(dp)	::	coef_est,coef_nord
Real(dp)	::	psi0
Real(dp)  ::      alpha,xmin,xequi,coslinf,dxmin,d12,d11,d210,d21,d22,det_1,xmax,coslsup
Real(dp)  ::      anm,bnm,cnm(0:npt),cosphi,sinphi
Real(dp)  ::	A(npt),A_est(npt),A_nord(npt),Pn(0:npt),dPn(0:npt)
Real(dp)  ::	xinf,xfit,xsup,phi,expdx
Real(dp)  ::	dxequi,dxequi2,expjdx,expcdx,expcdx_1,Fnm_inf,Fnm_sup,dFnm_inf,dFnm_sup,sum

mode=present(psi_est)

rang=(ndeg+1)*(2*mdeg+1)
LM=(latmax+latmin)/2.0_dp
DL=2.0_dp/(latmax-latmin)
alpha=-DL*rad2deg

phi=deg2rad*lon

coef_est=-1.0_dp/re/cos(deg2rad*min(lat,lat_top))

if (lat<latmin.and.lat>=latequi) then
  xinf=log(tan(pi/4.0_dp-lat*deg2rad/2.0_dp))

  coef_nord=coef_est

  xmin =log(tan(pi/4.0_dp-latmin*deg2rad/2.0_dp))
  xequi=log(tan(pi/4.0_dp-latequi*deg2rad/2.0_dp))
  coslinf=cos(latmin*deg2rad)
  dxequi=(xinf-xequi)
  dxequi2=dxequi*dxequi
  dxmin =xmin-xequi
  d12=dxmin*dxmin
  d11=xmin*d12
  d210=2.0_dp*dxmin*xmin+d12

  k=0
  do i=0,ndeg
    k=k+1
    call plegendr(i,1.0_dp,Pn(i),dPn(i))
    cnm(i)=-alpha*dPn(i)*coslinf
    A(k)=Pn(i)+cnm(i)*(xinf-xmin)
    if (mode) then
      A_est(k)=0.0_dp
      A_nord(k)=-cnm(i)
    endif
  enddo
  do j=1,mdeg
    d21=d210+j*d11
    d22=2.0_dp*dxmin+j*d12
    det_1=1.0_dp/(d11*d22-d12*d21)
    cosphi=cos(real(j,dp)*phi)
    sinphi=sin(real(j,dp)*phi)
    do i=0,ndeg
      k=k+1
      anm=(d22-cnm(i)*d12)*det_1
      bnm=(-d21+cnm(i)*d11)*det_1
      bnm=anm*xinf+bnm
      Fnm_inf=bnm*dxequi2
      A(k)=Fnm_inf*cosphi
      if (mode) then
        dFnm_inf=2.0_dp*dxequi*bnm+anm*dxequi2+j*Fnm_inf
        A_est(k)=-Fnm_inf*j*sinphi
        A_nord(k)=-dFnm_inf*cosphi
      endif

      k=k+1
      A(k)=Fnm_inf*sinphi
      if (mode) then
        A_est(k)=Fnm_inf*j*cosphi
        A_nord(k)=-dFnm_inf*sinphi
      endif
    enddo
  enddo

elseif(lat>=latmin.and.lat<=latmax) then
  xfit=(LM-lat)*DL
  coef_nord=alpha/re

  k=0
  do i=0,ndeg
    k=k+1
    call plegendr(i,xfit,Pn(i),dPn(i))
    A(k)=Pn(i)
    if (mode) then
      A_est(k)=0.0_dp
      A_nord(k)=-dPn(i)
    endif
  enddo
  do j=1,mdeg
    cosphi=dcos(real(j,dp)*phi)
    sinphi=dsin(real(j,dp)*phi)
    do i=0,ndeg
      k=k+1
      A(k)=Pn(i)*cosphi
      if (mode) then
        A_est(k)=-Pn(i)*j*sinphi
        A_nord(k)=-dPn(i)*cosphi
      endif

      k=k+1
      A(k)=Pn(i)*sinphi
      if (mode) then
        A_est(k)=Pn(i)*j*cosphi
        A_nord(k)=-dPn(i)*sinphi
      endif
    enddo
  enddo
elseif(lat>latmax) then

  xmax =log(tan(pi/4.0_dp-latmax*deg2rad/2.0_dp))

!  if (lat<lat_top) then
    xsup=log(tan(deg2rad*(45.0_dp-min(lat,lat_top)/2.0_dp)))
    expcdx_1=xsup-xmax
    expdx=exp(expcdx_1)
    expjdx=1.0_dp
    if (coeffc/=0.0_dp) then
      expcdx=exp(coeffc*expcdx_1)
      expcdx_1=(expcdx-1.0_dp)/coeffc
    else
      expcdx=1.0_dp
    endif
!  else
!    xsup=xmax
!    expcdx=1.0_dp
!    expcdx_1=0.0_dp
!    expdx=0.0_dp
!    expjdx=1.0_dp
!  endif
  coef_nord=coef_est

  coslsup=cos(latmax*deg2rad)

  k=0
  do i=0,ndeg
    k=k+1
    call plegendr(i,-1.0_dp,Pn(i),dPn(i))
    cnm(i)=-alpha*dPn(i)*coslsup
    A(k)=Pn(i)+cnm(i)*expcdx_1
    if (mode) then
      dFnm_sup=cnm(i)*expcdx
      A_est(k)=0.0_dp
      A_nord(k)=-dFnm_sup
    endif
  enddo
  do j=1,mdeg
    cosphi=cos(real(j,dp)*phi)
    sinphi=sin(real(j,dp)*phi)
    expjdx=expdx*expjdx
    do i=0,ndeg
      k=k+1
      anm=(cnm(i)-j*Pn(i))
      Fnm_sup=(Pn(i)+anm*expcdx_1)*expjdx
      A(k)=Fnm_sup*cosphi
      if (mode) then
        dFnm_sup=(anm*(expcdx+j*expcdx_1)+j*Pn(i))*expjdx
        A_est(k)=-Fnm_sup*j*sinphi
        A_nord(k)=-dFnm_sup*cosphi
      endif

      k=k+1
      A(k)=Fnm_sup*sinphi
      if (mode) then
        A_est(k)=Fnm_sup*j*cosphi
        A_nord(k)=-dFnm_sup*sinphi
      endif
    enddo
  enddo

endif

psi=dot_product(A(1:rang),coef_psi(1:rang))
if (mode) then
  psi_est =coef_est*dot_product(A_est(1:rang),coef_psi(1:rang))
  psi_nord=coef_nord*dot_product(A_nord(1:rang),coef_psi(1:rang))
endif

end subroutine val_fit