!=====================================================================!
! Module that contains all the implementation of a rigid body
!
! Author: Komahan Boopathy (komahan@gatech.edu)
!=====================================================================!

module rigid_body_class

  use utils, only : vector, matrix, skew, unskew, &
       & operator(*), operator(+), operator(-), &
       & norm, array, dp

  use rotation

  implicit none

  private

  public :: rigid_body

  !-------------------------------------------------------------------!
  ! Rigid body type that contains pertaining data and routines that
  ! operate over the type variables
  !
  ! Usage: After creating this body, be sure to call set on
  ! this body with mandatory parameters
  !
  ! type(rigid_body) :: bodyA
  ! .
  ! .
  ! bodyA % set(...)
  ! -------------------------------------------------------------------!

  type :: rigid_body

     !----------------------------------------------------------------!
     ! Position state variables (all in body frame when stored)
     !----------------------------------------------------------------!

     type(vector) :: r
     type(vector) :: theta
     type(vector) :: v
     type(vector) :: omega

     !----------------------------------------------------------------!
     ! Velocity state variables (all in body frame when stored)
     !----------------------------------------------------------------!

     type(vector) :: rdot
     type(vector) :: thetadot
     type(vector) :: vdot
     type(vector) :: omegadot

     !----------------------------------------------------------------!
     ! Inertial Properties (all in body frame when stored)
     !----------------------------------------------------------------!
     ! First moment of mass
     ! C = [ cx,  cy,  cz ]
     !----------------------------------------------------------------!
     ! Second moment of mass
     ! J = [ Jxx,  Jxy,  Jxz ] = [ J[0],  J[1],  J[2] ]
     ! . = [    ,  Jyy,  Jyz ] = [     ,  J[3],  J[4] ]
     ! . = [    ,     ,  Jzz ] = [     ,      ,  J[5] ]
     !----------------------------------------------------------------!

     real(dp)     :: mass    ! mass of the body
     type(vector) :: c       ! first moment of inertia
     type(matrix) :: J       ! second moment of inertia

     !----------------------------------------------------------------!
     ! Rotation matrices (rotates from body to inertial frame)
     !----------------------------------------------------------------!

     type(matrix) :: TIB, TIBdot ! Transformation matrix and its derivatives
     type(matrix) :: S, SDOT     ! angular velocity matrix and its derivative

     !----------------------------------------------------------------!
     ! Handy force and torque vectors
     !----------------------------------------------------------------!

     !     type(vector) :: rforce   ! external/reaction force
     !     type(vector) :: rtorque  ! external/reaction torque
     type(vector) :: grav     ! gravity vector in local frame

     !----------------------------------------------------------------!
     ! Used for energy conservation check
     !----------------------------------------------------------------!

     real(dp) :: KE          ! kinetic energy of the body
     real(dp) :: PE          ! potential energy of the body
     real(dp) :: TE          ! total energy of the body

   contains

     procedure :: set
     procedure :: update
     procedure :: computeEnergies
     procedure :: get_residual
     procedure :: get_jacobian

  end type rigid_body

contains

  !-------------------------------------------------------------------!
  ! Routine to set the states into the body and compute rotation
  ! matrices for the set state. This function is to be called during
  ! initialization or the first time step
  ! -------------------------------------------------------------------!

  subroutine set(body, mass, q, qdot)

    class(rigid_body)    :: body
    real(dp), intent(in), OPTIONAL :: mass
    real(dp), intent(in) :: q(12), qdot(12)

    ! Set the rotation parameters
    body % theta    = vector(q(4:6))

    ! Set inertial properties
    body % mass     = mass
    body % c % x    = 0.0d0       ! Assuming body frame is located at CG
    body % J % PSI  = mass/6.0d0 ! Assuming atleast two planes of symmetry and a cube side = 1

    ! Set the gravity in global frame (this is converted to body frame
    ! during residual and jacobian assembly)
    body % grav % x(3) = -10.0d0

    ! Get rotation and angular rate matrices based on theta
    body % TIB     = get_rotation(body % theta)
    body % S       = get_angrate(body % theta)
    body % SDOT    = get_angrate_dot(body % theta, body % thetadot)

    ! Set the state into the body
    body % r        = body % TIB*vector(q(1:3))   ! convert to body frame
    body % v        = body % TIB*vector(q(7:9))   ! convert to body frame
    body % omega    = body % TIB*vector(q(10:12)) ! convert to body frame

    ! Set the time derivatives of state into the body
    body % rdot     = vector(qdot(1:3))
    body % thetadot = vector(qdot(4:6))
    body % vdot     = vector(qdot(7:9))
    body % omegadot = vector(qdot(10:12))

    ! Compute and set energies into the body
    call body % computeEnergies()

  end subroutine set

  !-------------------------------------------------------------------!
  ! Routine to update the states into the body and compute rotation
  ! matrices for the new state. This function is to be called during
  ! during subsequent time steps
  !-------------------------------------------------------------------!

  subroutine update(body, q, qdot)

    class(rigid_body)    :: body
    real(dp), intent(in) :: q(12), qdot(12)

    body % theta    = vector(q(4:6))

    ! Get rotation and angular rate matrices based on theta
    body % TIB     = get_rotation(body % theta)
    body % S       = get_angrate(body % theta)
    body % SDOT    = get_angrate_dot(body % theta, body % thetadot)

    ! Set the state into the body
    body % r        = vector(q(1:3))
    body % v        = vector(q(7:9))
    body % omega    = vector(q(10:12))

    ! Set the time derivatives of state into the body
    body % rdot     = vector(qdot(1:3))
    body % thetadot = vector(qdot(4:6))
    body % vdot     = vector(qdot(7:9))
    body % omegadot = vector(qdot(10:12))

    ! Update the energies based on the new state
    call body % computeEnergies()

  end subroutine update

  !-------------------------------------------------------------------!
  ! Routine that computes the Kinetic and Potential energies of the
  ! body and sets into the body. All quantities are to be in body
  ! fixed frame of reference.
  !-------------------------------------------------------------------!

  subroutine computeEnergies(this)

    class(rigid_body):: this

    ! KE = 0.5 m v.v + 0.5 omega^T J omega + v.T omega x c (all
    ! quantities are in body frame)

    this % KE = 0.5d0 * this % mass *this % v * this % v &
         & + 0.5d0 *this % omega * this % J * this % omega  &
         & + this % v * skew(this % omega) * this % c

    ! PE =  -m g.r - c.r (all quantities are in body frame)

    this % PE = - this % mass * this % TIB * this % grav * this % r &
         & - this % c * this % TIB * this % grav

    ! find the sum

    this % TE = this % PE + this % KE

  end subroutine computeEnergies

  !-------------------------------------------------------------------!
  ! Residual of the kinematic and dynamic equations in state-space
  ! representation. The vector form of the residual 'R' can be
  ! converted into scalar form of 12 equations just by using
  ! 'array(R)'
  !-------------------------------------------------------------------!

  function get_residual(body) result(R)

    class(rigid_body) :: body
    type(vector) :: R(4)

    !-----------------------------------------------------------------!
    ! Translational Kinematics
    !-----------------------------------------------------------------!
    ! [T] r_dot - v = 0
    !-----------------------------------------------------------------!

    !! May have to transform things into a frame
    R(1)  = body % TIB * body % rdot - body % v

    !-----------------------------------------------------------------!
    ! Rotational Kinematics
    !-----------------------------------------------------------------!
    ! [S] theta_dot - omega = 0
    !-----------------------------------------------------------------!

    !! May have to transform things into a frame
    R(2)  = body % S*body % thetadot - body % omega

    !-----------------------------------------------------------------!
    ! Translational Dynamics (8 terms) (Force Equation)
    !-----------------------------------------------------------------!
    ! m (vdot - TIB*g) - c x omegadot + omega x (m v - c x omega) - fr = 0
    !-----------------------------------------------------------------!

    !! May have to transform things into a frame
    R(3)  =  body % mass*(body % vdot - body % TIB * body % grav) &
         & - skew(body % c)*body % omegadot &
         & + skew(body % omega)*(body % mass * body % v - skew(body % c)*body % omega) !&
    !& - body % rforce

    !-----------------------------------------------------------------!
    ! Rotational Dynamics (9-terms) (Moment Equation)
    !-----------------------------------------------------------------!
    ! c x vdot + J omegadot  + c x omega x v + omega x J - c x TIB*g - gr = 0
    !-----------------------------------------------------------------!

    !! May have to transform things into a frame
    R(4)  = skew(body % c) * body % vdot &
         & + body % J * body % omegadot &
         & + skew(body % c) * skew(body % omega)*body % v &
         & + skew(body % omega) * body % J * body % omega &
         & - skew(body % c) * body % TIB * body % grav !&
    !& - body % rtorque

  end function get_residual

  !-------------------------------------------------------------------!
  ! Jacobian of the kinematic and dynamic equations in state-space
  ! representation
  !-------------------------------------------------------------------!

  subroutine get_jacobian(this)

    class(rigid_body) :: this

    stop "impl_get_jacobian"

  end subroutine get_jacobian

end module rigid_body_class

!=====================================================================!
! Module that contains all the implementation of rigid body dynamics
! related physics.
!
! Author: Komahan Boopathy (komahan@gatech.edu)
!=====================================================================!

module multibody_dynamics_class

  ! parent class
  use physics_class, only : physics

  ! any other classes
  use utils, only : vector, matrix, skew, unskew, &
       & operator(*), operator(+), operator(-), &
       & norm, array, dp

  use rigid_body_class, only : rigid_body

  use physics_class,  only : physics
  use function_class, only : abstract_function

  implicit none

  private

  public :: multibody_dynamics

  !-------------------------------------------------------------------!
  ! Type that models rigid body dynamics
  !-------------------------------------------------------------------!

  type, extends(physics) :: multibody_dynamics

     integer          :: num_state_vars
     integer          :: num_design_vars
     integer          :: num_bodies
     type(rigid_body) :: body ! might wanna create an array of bodies

   contains

     procedure :: initialize
     procedure :: setDesignVars
     procedure :: assembleResidual
     procedure :: assembleJacobian
     procedure :: getInitialStates
     procedure :: getNumStateVars
     procedure :: getResidualDVSens

  end type multibody_dynamics

contains

  !---------------------------------------------------------------------!
  ! Constructor for the aero elastic oscillator
  !---------------------------------------------------------------------!

  subroutine initialize(this, x, function)

    class(multibody_dynamics)                   :: this
    class(abstract_function), target, OPTIONAL  :: function
    real(8), intent(in), dimension(:), OPTIONAl :: x

    ! Set the number of state variables
    this % num_state_vars = 12

    if (present(x)) then
       this % num_design_vars = size(x)
       call this % setDesignVars(x)
    end if

  end subroutine initialize

  !===================================================================!
  ! Sets the design variables into the system
  !===================================================================!

  subroutine setDesignVars(this, x)

    class(multibody_dynamics)         :: this
    real(8), intent(in), dimension(:) :: x

  end subroutine setDesignVars

  !-------------------------------------------------------------------!
  ! Residual assembly at each time step
  !-------------------------------------------------------------------!

  subroutine assembleResidual(this, res, time, u, udot, uddot)

    class(multibody_dynamics) :: this

    real(8), intent(inout), dimension(:) :: res
    real(8), intent(in) :: time
    real(8), intent(in), dimension(:) :: u, udot, uddot

    ! print*, "DUMMY residual"

    call this % body % update (q=u, qdot=udot)

    res = array(this % body % get_residual())

    ! Temporary printing of energy
    ! print *, this % body % KE, this % body % PE, this % body % TE

  end subroutine assembleResidual

  !-------------------------------------------------------------------!
  ! Jacobian assembly at each time step
  !-------------------------------------------------------------------!

  subroutine assembleJacobian(this, jac, alpha, beta, gamma, &
       & time, u, udot, uddot)

    class(multibody_dynamics) :: this
    real(8), intent(inout), dimension(:,:) :: jac
    real(8), intent(in) :: alpha, beta, gamma
    real(8), intent(in) :: time
    real(8), intent(in), dimension(:) :: u, udot, uddot

    stop "DUMMY jacobian"

    ! jac = array(this % body % get_residual())

  end subroutine assembleJacobian

  !-------------------------------------------------------------------!
  ! Sets the initial condition for use in the integator
  !-------------------------------------------------------------------!

  subroutine getInitialStates(this, time, u, udot)

    class(multibody_dynamics) :: this

    real(8), intent(in) :: time
    real(8), intent(inout), dimension(:) :: u, udot

    ! Initial position in inertial frame (r)
    u(1:3) = (/ 0.0d0, 0.0d0, 100.0d0 /)

    ! Initial velocity in inertial frame (v)
    u(7:9) = (/ 0.0d0, 0.0d0, 0.0d0 /)

    ! Initial angular velocity in inertial frame (omega)
    u(10:12) = (/ 0.0d0, 0.0d0, 0.0d0 /)

    call this % body % set(1.0d0, u, udot)

  end subroutine getInitialStates

  !===================================================================!
  ! Return the number of state variables
  !===================================================================!

  function getNumStateVars(this)

    class(multibody_dynamics) :: this
    integer                   :: getNumStateVars

    getNumStateVars = this % num_state_vars

  end function getNumStateVars

  !-------------------------------------------------------------------!
  ! Routine for evaluating the gradient of Residual with respect
  ! to the design X
  !-------------------------------------------------------------------!

  subroutine getResidualDVSens(this, jac, scale, time, x, u, udot, uddot)

    class(multibody_dynamics)              :: this
    real(8), intent(inout), dimension(:,:) :: jac
    real(8), intent(in)                    :: time
    real(8), intent(in), dimension(:)      :: x, u, udot, uddot
    real(8)                                :: scale

    stop"Not implemented"

  end subroutine getResidualDVSens

end module multibody_dynamics_class


module pde

  use global_module

  implicit none

  integer, parameter :: Neq = 3 ! Number of equations

  real(kind=8), parameter :: gam    = 1.6667       ! adiabatic constant

  ! Name of primitive variables, used ONLY for exporting the solution to VTK file
  ! NOTE: To initialize it here, all entries need to have the same length!!!
  !       I'm using three letters for simplicity.
  character(len=20), dimension(Neq) :: prim_names = (/'rho','UUx','PPP'/)

  contains

  ! ============================================================

  subroutine initialize_solution(U)

    implicit none

    real(kind=8), dimension(:,:), intent(inout) :: U

    integer      :: i

    real(kind=8) :: rhoL, uxL, PL
    real(kind=8) :: rhoR, uxR, PR

    ! Left state
    rhoL = 2.0
    uxL  = 0.0
    PL   = 2.0

    ! Right state
    rhoR = 1.0
    uxR  = 0.0
    PR   = 1.0

    ! Initialize solution (all cells, also internal ones)
    U(1,1:floor(Nx/2.0)) = rhoL                             ! Density
    U(2,1:floor(Nx/2.0)) = rhoL*uxL                         ! Momentum
    U(3,1:floor(Nx/2.0)) = rhoL*uxL*uxL/2.0 + PL/(gam-1.0)    ! Energy

    U(1,floor(Nx/2.0):Nx) = rhoR
    U(2,floor(Nx/2.0):Nx) = rhoR*uxR
    U(3,floor(Nx/2.0):Nx) = rhoR*uxR*uxR/2.0 + PR/(gam-1.0)

  end subroutine

  ! ============================================================

  subroutine assign_BCs(U)

    implicit none

    real(kind=8), dimension(:,:), intent(inout) :: U

    integer :: i

    ! ----- LEFT BC --------

    ! Do nothing!

    ! U(1, 1:2) = ...
    ! U(2, 1:2) = ...
    ! U(3, 1:2) = ...

    ! ----- RIGHT BC --------

    ! Do nothing!

    ! U(1, Nx-1:Nx) = ...
    ! U(2, Nx-1:Nx) = ...
    ! U(3, Nx-1:Nx) = ...

  end subroutine

  ! ============================================================

  subroutine compute_conserved_from_primitive(prim, U)

    ! Computes vector of conserved variables "U" from the primitive variables "prim"

    implicit none

    real(kind=8), dimension(Neq), intent(in)  :: prim
    real(kind=8), dimension(Neq), intent(out) :: U

    ! Working variables
    real(kind=8) :: rho, ux, P

    ! Extract primitive variables
    rho = prim(1)
    ux  = prim(2)
    P   = prim(3)

    ! Compose array of primitive variables
    U(1) = rho
    U(2) = rho*ux
    U(3) = rho*ux*ux/2.0d0 + P/(gam - 1.0d0)

  end subroutine

  ! ============================================================

  subroutine compute_primitive_from_conserved(U, prim)

    ! Computes vector of primitive variables "prim" from the conserved variables "U"

    implicit none

    real(kind=8), dimension(Neq), intent(in)  :: U
    real(kind=8), dimension(Neq), intent(out) :: prim

    ! Working variables
    real(kind=8) :: rho, ux, P

    ! Extract primitive variables
    rho = U(1)
    ux  = U(2)/(rho + 1.0d-25) ! Use a small tolerance, since rho may be zero
    P   = (gam-1.0)*(U(3) - rho*ux*ux/2.0)

    ! Compose array of primitive variables
    prim(1) = rho
    prim(2) = ux
    prim(3) = P

  end subroutine

  ! ============================================================

  subroutine compute_flux_ws_x(U, Fx, ws_max, ws_min)

    ! Computes the convective flux along x,
    ! and also the maximum and minimum wave speeds (required by some numerical flux schemes)

    implicit none

    real(kind=8), dimension(Neq), intent(in)  :: U
    real(kind=8), dimension(Neq), intent(out) :: Fx
    real(kind=8), intent(out) :: ws_max, ws_min

    real(kind=8), dimension(Neq) :: prim
    real(kind=8) :: rho, ux, P

    ! Compute primitive variables
    call compute_primitive_from_conserved(U, prim)
    rho = prim(1)
    ux  = prim(2)
    P   = prim(3)

    ! Assemble fluxes Fx
    Fx(1) = rho*ux
    Fx(2) = rho*ux*ux + P
    Fx(3) = U(3)*ux + P*ux ! note: U(3) = rho*E

    ! Maximum and minimum wave speeds (eigenvalues of the Euler system)
    ws_max = ux + sqrt(gam*P/rho)
    ws_min = ux - sqrt(gam*P/rho)

  end subroutine

end module


	SUBROUTINE IN_INTC  ( cint, gmin, gmax, cval, nv, clbl,
     +                        rint, cmin, cmax, iret )
C************************************************************************
C* IN_INTC								*
C*									*
C* This subroutine processes the user contour specification.  GMIN and 	*
C* GMAX are input as the mininum and maximum values in the grid.  	*
C* For loops, this routine is called only once for the first pixmap.	*
C* The list of contour levels is returned in CVAL, along with CMIN and 	*
C* CMAX, the minimum and maximum to be contoured.  			*
C* 									*
C* CINT is expected to be of the form:					*
C*									*
C*	    increment/minimum/maximum/ndigits				*
C*    or								*
C*          value1;value2;...;valueN///ndigits				*
C*    or								*
C*	    value1=label1;value2=label2;...;valueN=labelN		*
C*									*
C* where the minimum and maximum give the range for the contours. If	*
C* the minimum equals the maximum, a single contour with that value	*
C* is assumed.  In the latter specification, the specified contour	*
C* levels are used, and the minimum and maximum are ignored.		*
C* The fourth parameter, ndigits, forces all labels with fewer than	*
C* ndigits digits in the integer portion of the label to expand to	*
C* ndigits by padding the leading digit with zeroes.			*
C*									*
C* IN_INTC  ( CINT, GMIN, GMAX, CVAL, NV, CLBL, RINT, CMIN, CMAX, 	*
C*	      IRET )							*
C*									*
C* Input parameters:							*
C*	CINT		CHAR*		Input for contours		*
C*	GMIN		REAL		Minimum grid value		*
C*	GMAX		REAL		Maximum grid value		*
C*									*
C* Output parameters:							*
C*	CVAL (*)	REAL		List of contour levels		*
C*	NV		INTEGER		Number of contour levels	*
C*	CLBL (*)	CHAR*		Contour labels			*
C*	RINT		REAL		Calculated contour interval	*
C*	CMIN		REAL		Contour minimum			*
C*	CMAX		REAL		Contour maximum			*
C*	IRET		INTEGER		Return code			*
C*					  0 = normal return		*
C*					 -3 = invalid input		*
C**									*
C* Log:									*
C* P. Bruehl/Unidata	08/94	Based on IN_CINT, now returns CMIN,CMAX	*
C* T. Piper/GSC		11/98	Updated prolog				*
C* T. Piper/GSC		 3/99	Corrected prolog			*
C* T. Lee/SAIC		12/01	Increased substr string length to LLMXLN*
C* C. Bailey/HPC	 6/06	Modified to return contour labels	*
C* S. Jacobs/NCEP	11/06	Fixed bug when min=max			*
C* R. Jones/NCEP        12/06   Added number of digits specification    *
C* S. Schotz/NCEP	2/07	Correct case where no cint is set	*
C************************************************************************
	INCLUDE		'GEMPRM.PRM'
C*
	CHARACTER*(*)	cint
	REAL		cval (*)
	CHARACTER*(*)	clbl (*)
C*
	CHARACTER	substr (4) * (LLMXLN), cname (LLCLEV)*24,
     +			carr(2)*24
        CHARACTER       clab1*24, clab2*24
C*
	INCLUDE		'ERMISS.FNC'
C------------------------------------------------------------------------
C
C*  Initialize output parameters
C
	nv = 0
	rint = RMISSD
	cmin = RMISSD
	cmax = RMISSD
	iret = 0
C
C*	Break CINT into 4 substrings.
C
	CALL ST_CLST ( cint, '/', ' ', 4, substr, nst, ier )
	IF  ( ier .ne. 0 )  THEN
	    iret = -3
	    CALL ER_WMSG ( 'IN', iret, cint, ier )
	    RETURN
	END IF
C
C*	Convert the fourth item to an integer. Check for valid values
C*	for the number of leading zeroes to be added to a data value.
C
	CALL ST_NUMB ( substr (4), npad, ier )
	IF  ( ( npad .lt. 0 ) .or. ( npad .gt. 9 ) )  npad = 0
C
C*	Look at the first substring to see if it contains a list of
C*	values or a single number giving the increment.
C
	CALL ST_CLST ( substr (1), ';', ' ', LLCLEV, cname, nv, ier )
	IF  ( ier .ne. 0 )  THEN
	    iret = -3
	    CALL ER_WMSG ( 'IN', iret, cint, ier )
	END IF
C
C	Check for case where no contour interval is specified
C
	IF (nv .eq. 0) THEN
C
C	    Initilize contour interval when not specified
C
	    cval(1) = 0.0
	ELSE
C
C*          Loop through each contour value
C
	    DO i = 1, nv
C
C*	        Separate contour value from label if appropriate
C
	        CALL ST_CLST ( cname (i), '=', ' ', 2, carr, num, ier )
C
C*	        Convert contour value string into a real number
C
	        CALL ST_CRNM(carr(1), rnum, ier )
	        cval(i) = rnum
C
C*	        Check to see if contour label specified with value.
C*	        If not, use contour value as label
C
	        IF (num .gt. 1) then
	      	    clbl(i) = carr(2)
	        ELSE
		    clbl(i) = carr(1)
	        END IF
	    END DO
	END IF
C
	IF ( nv .gt. 1 ) THEN
	    rint = 0.0
	    cmin = 0.0
	    cmax = 0.0
	  ELSE
C
C*	    Contour levels have not been listed.
C*	    Decode range from second and third substrings.
C
	    CALL ST_CRNM ( substr (2), cmin, ier )
	    CALL ST_CRNM ( substr (3), cmax, ier )
C
C*	    Check for single contour level.
C
	    IF ( cmin .eq. cmax .and. .not. ERMISS ( cmin ) ) THEN
C
C*		Check if label has been specified, if not set cval(1)
C*		and contour label to cmin
C
	        cval (1) = cmin
		IF (num .le. 1) then
		    CALL GR_LABL(cval(1), 0, 0, clbl(1), nchar, ier)
		END IF
		rint = 0.0
		nv   = 1
	      ELSE
C
C*		Generate multiple contour levels.
C*		Specified contour label strings not allowed.
C
		rcint = cval (1)
		CALL GR_CLVL ( LLCLEV, cmin, cmax, rcint, gmin, gmax,
     +				nv, cval, rint, iret )
		IF ( iret .ne. 0 )  THEN
		    CALL ER_WMSG ( 'GR', iret, ' ', ier )
		    iret = -3
		    CALL ER_WMSG ( 'IN', iret, ' ', ier )
		    RETURN
		END IF
C
C*		Convert contour values to strings
C
		DO i = 1, nv
		    CALL GR_LABL(cval(i), 0, 0, clbl(i), nchar, ier)
		END DO
	    END IF
	END IF
C
C*	Add zero-padding to the contour values.
C
	IF  ( npad .gt. 0 )  THEN
	    DO  ii = 1, nv
		CALL ST_NULL ( clbl(ii), clab1, lenc, ier )
		CALL CST_ZPAD ( clab1, npad, clab2, ier )
		CALL ST_RNUL ( clab2, clbl(ii), lenc, ier )
	    END DO
	END IF
C*
	RETURN
	END


      implicit none
      double complex qlI1,qlI2,qlI3,qlI4,Ival(-2:0)
      double precision p1sq,p2sq,p3sq,p4sq,s12,s23,
     .     m1sq,m2sq,m3sq,m4sq,musq
      integer ep
      call qlinit
      musq=1.1d0
      m1sq=3d0
      m2sq=5d0
      m3sq=9d0
      m4sq=1.3d0
      p1sq=1.5d0
      p2sq=-1.7d0
      p3sq=2.3d0
      p4sq=2.9d0
      s12=37d0
      s23=-15.7d0
      write(6,*) m1sq,musq
      do ep=-2,0
         Ival(ep)=qlI1(m1sq,musq,ep)
         write(6,*) 'ep,qlI1(m1sq,musq,ep)',ep,Ival(ep)/m1sq
      enddo
      write(6,*)
      do ep=-2,0
         Ival(ep)=qlI2(p1sq,m1sq,m2sq,musq,ep)
         write(6,*) 'ep,qlI2(p1sq,m1sq,m2sq,musq,ep)',
     .        ep,Ival(ep)
      enddo
      write(6,*)
      do ep=-2,0
         Ival(ep)=qlI3(p1sq,p2sq,p3sq,m1sq,m2sq,m3sq,musq,ep)
         write(6,*) 'ep,qlI3(p1sq,p2sq,p3sq,m1sq,m2sq,m3sq,musq,ep)',
     .        ep,Ival(ep)
      enddo
      write(6,*)
      do ep=-2,0
         Ival(ep)=
     .   qlI4(p1sq,p2sq,p3sq,p4sq,s12,s23,m1sq,m2sq,m3sq,m4sq,musq,ep)
         write(6,*)
     .'ep,qlI4(p1sq,p2sq,p3sq,p4sq,s12,s23,m1sq,m2sq,m3sq,m4sq,musq,ep)'
     . ,ep,Ival(ep)
      enddo

      write(6,*)
      write(6,*) 'test of divergent boxes'
      do ep=-2,0
         Ival(ep)=
     .   qlI4(0d0,0d0,p3sq,p4sq,s12,s23,0d0,0d0,0d0,m4sq,musq,ep)
         write(6,*)
     .   'ep,qlI4(0d0,0d0,p3sq,p4sq,s12,s23,0d0,0d0,0d0,m4sq,musq,ep)'
     .        ,ep,Ival(ep)
      enddo

      stop
      end


      subroutine catani(mhsq,k1,k2,k3,k4,k5,k6,coeff2,coeff1)
      implicit none
      include 'constants.f'
      include 'momwbbm.f'
      include 'scale.f'
      integer k1,k2,k3,k4,k5,k6,nu
      double precision mhsq,p2(4),p3(4),p1Dp2,p3Dp4
      double complex xl12,xl34,coeff2,coeff1

      do nu=1,4
      p2(nu)=bp*mom(k2,nu)+bm*mom(k3,nu)
      p3(nu)=bp*mom(k3,nu)+bm*mom(k2,nu)
      enddo
      p1Dp2=mom(k1,4)*p2(4)-mom(k1,1)*p2(1)
     &     -mom(k1,2)*p2(2)-mom(k1,3)*p2(3)
      p3Dp4=mom(k4,4)*p3(4)-mom(k4,1)*p3(1)
     &     -mom(k4,2)*p3(2)-mom(k4,3)*p3(3)
      xl12=dcmplx(dlog(abs(2d0*p1Dp2)/musq))
      xl34=dcmplx(dlog(abs(2d0*p3Dp4)/musq))

      coeff2=-cone
      coeff1=xl12+xl34+dcmplx(8d0/3d0-dlog(mhsq/musq))

      return
      end

c--- Note: this is not Catani's formula, this is my reinterpretation
c---       of the result that we have found
      subroutine catanisl(mhsq,k1,k2,k3,k4,k5,k6,coeff2,coeff1)
      implicit none
      include 'constants.f'
      include 'momwbbm.f'
      include 'scale.f'
      integer k1,k2,k3,k4,k5,k6,nu
      double precision mhsq,p2(4),p3(4),be,p2Dp3,p1Dp4
      double complex xl14,coeff2,coeff1,Lnrat

      do nu=1,4
      p2(nu)=bp*mom(k2,nu)+bm*mom(k3,nu)
      p3(nu)=bp*mom(k3,nu)+bm*mom(k2,nu)
      enddo
      p2Dp3=p2(4)*p3(4)-p2(1)*p3(1)
     &     -p2(2)*p3(2)-p2(3)*p3(3)
      p1Dp4=mom(k1,4)*mom(k4,4)-mom(k1,1)*mom(k4,1)
     &     -mom(k1,2)*mom(k4,2)-mom(k1,3)*mom(k4,3)
      xl14=Lnrat(musq,-2d0*p1Dp4)

      coeff2=-cone
      be=bp-bm
      coeff1=-cone-xl14-dcmplx(0.5d0*(1d0+be**2)/be)
     & *Lnrat(mhsq,-2d0*(p2Dp3+mhsq)*bp**2)

      return
      end


c=======================================================================
c
c     subroutine SHRINKAGEINTENSITY
c
c     Shrinkage intensity in the Ledoit-Wolf formula
c
c-----------------------------------------------------------------------
      SUBROUTINE shrinkageintensity ( n, p, X, cov, f, corrmean, dwork,
     &                                deltah )
c-----------------------------------------------------------------------
c
c     INPUT
c            n      : number of asset return values(p >= 1)       integer
c            p      : number of assets (p > 1)                    integer
c            X      : value(s) (n*p)                              double
c            cov    : covariance matrix (p*p)                     double
c            f      : shrinkage target matrix (p*p)               double
c            corrmean: mean of the non-diagonal elements of
c                            the correlation matrix               double
c     WORKSPACE
c            dwork  : vector (p*p+p)                              double
c
c     OUTPUT
c            deltah   : shrinkage intensity                       double
c
c-----------------------------------------------------------------------
c
      IMPLICIT NONE
c
c     arguments i/o
      INTEGER n, p
      DOUBLE PRECISION X(*), cov(*), deltah, corrmean, f(*)
c     workspace
      double precision dwork(*)
      integer pdrhomean, pdnuh
c     local variables
      double precision rhohat, sum, temp1, temp2
      double precision etah, kappahat, pih
      double precision sqrttemp1, sqrttemp2
      integer i, j

c
c     external subroutines
      EXTERNAL MCM, pihat, nuhat, etahat
c
c     intrinsic functions
      INTRINSIC sqrt
c
c-----------------------------------------------------------------------
c
c     pointers for double precision work space  : dwork
c     -------------------------------------------------
      pdrhomean = 1
c     prho: pointer ont the mean return rho, so p
      pdnuh = pdrhomean + p
c     pdzetah : pointer for relatives rents, so ( p*p ) more
c
c     Total size of dwork array = p*p+p
c
c-----------------------------------------------------------------------
c
      CALL MCM ( n, p, X, dwork(pdrhomean))
      CALL pihat ( n, p, X, dwork(pdrhomean), cov, pih, rhohat)
      CALL nuhat ( n, p, x, dwork(pdrhomean), cov, dwork(pdnuh))

      sum = 0.
      do i=1,p
        do j=1,p
            if (i .ne. j) then
                temp1 = dwork(pdnuh+(i-1)*p+j-1)
                temp2 = dwork(pdnuh+(j-1)*p+i-1)
                sqrttemp1 = cov((j-1)*p+j)
                sqrttemp1 = sqrttemp1/cov((i-1)*p+i)
                sqrttemp1 = sqrt(sqrttemp1)
                sqrttemp2 = 1/sqrttemp1
                sum = sum + temp1*sqrttemp1+temp2*sqrttemp2
            endif
        enddo
      enddo
      sum = sum*corrmean/2
      rhohat = rhohat + sum

      call etahat(p, cov, f, etah)

      kappahat = (pih - rhohat)/etah

      deltah = kappahat/n
      if (rhohat .gt. pih) then
        deltah = 0.
      else if (deltah .gt. 1) then
        deltah = 1.
      endif
c
      RETURN
      END


subroutine rp1(maxm,num_eqn,num_waves,num_aux,num_ghost,num_cells,ql,qr,auxl,auxr,fwave,s,amdq,apdq)

! Riemann solver for the 1D shallow water equations:

! (h)_t + (h*u)_x = 0
! (hu)_t + (h*u^2 + 1/2*grav*h^2)_x = -grav*h*(b)_x
!
! using the f-wave algorithm and Roe's approximate Riemann solver.

! waves:     2
! equations: 2

! Conserved quantities:
!       1 depth
!       2 momentum

! Auxiliary fields:
!       1 bathymetry

! The gravitational constant grav should be in the common block cparam.
!
! See http://www.clawpack.org/riemann.html for a detailed explanation
! of the Riemann solver API.

    implicit none

    integer, intent(in) :: maxm, num_eqn, num_waves, num_ghost, num_aux, num_cells
    real(kind=8), intent(in) :: ql(num_eqn, 1-num_ghost:maxm+num_ghost)
    real(kind=8), intent(in) :: qr(num_eqn, 1-num_ghost:maxm+num_ghost)
    real(kind=8), intent(in) :: auxl(num_aux, 1-num_ghost:maxm+num_ghost)
    real(kind=8), intent(in) :: auxr(num_aux, 1-num_ghost:maxm+num_ghost)
    real(kind=8), intent(out) :: s(num_waves, 1-num_ghost:maxm+num_ghost)
    real(kind=8), intent(out) :: fwave(num_eqn, num_waves, 1-num_ghost:maxm+num_ghost)
    real(kind=8), intent(out) :: amdq(num_eqn, 1-num_ghost:maxm+num_ghost)
    real(kind=8), intent(out) :: apdq(num_eqn, 1-num_ghost:maxm+num_ghost)

    real(kind=8) :: grav, dry_tolerance, sea_level
    common /cparam/ grav, dry_tolerance, sea_level

    real(kind=8) :: hl, ul, hr, ur, hbar, uhat, chat, bl, br, phil, phir
    real(kind=8) :: R(2,2), fluxdiff(2), beta(2)

    integer :: i, k

    amdq = 0.d0
    apdq = 0.d0

    do i=2-num_ghost,num_cells+num_ghost
        ! # Left states
        hl = qr(1, i - 1)
        if (hl > dry_tolerance) then
            ul = qr(2, i - 1) / hl
            phil = qr(1, i - 1) * ul**2 + 0.5d0 * grav * qr(1, i - 1)**2
        else
            ul = 0.d0
            phil = 0.d0
        end if
        bl = auxr(1, i - 1)

        ! # Right states
        hr = ql(1, i)
        if (hr > dry_tolerance) then
            ur = ql(2, i)/hr
            phir = ql(1, i) * ur**2 + 0.5d0 * grav * ql(1, i)**2
        else
            ur = 0.d0
            phir = 0.d0
        end if
        br = auxl(1, i)

        ! # Roe average states
        hbar = 0.5 * (hr + hl)
        uhat = (sqrt(hl) * ul + sqrt(hr) * ur) / (sqrt(hl) + sqrt(hr))
        chat = sqrt(grav * hbar)

        ! # Flux differences
        fluxdiff(1) = (hr * ur) - (hl * ul)
        fluxdiff(2) = phir - phil + grav * hbar * (br - bl)

        ! # Wave speeds
        s(1,i) = min(ul - sqrt(grav * hl), uhat - chat)
        s(2,i) = max(ur + sqrt(grav * hr), uhat + chat)

        ! # Right eigenvectors (column)
        R(1,1) = 1.d0
        R(2,1) = s(1, i)

        R(1,2) = 1.d0
        R(2,2) = s(2, i)

        ! Wave strengths
        beta(1) = (s(2, i) * fluxdiff(1) - fluxdiff(2)) / (s(2, i) - s(1, i))
        beta(2) = (fluxdiff(2) - s(1, i) * fluxdiff(1)) / (s(2, i) - s(1, i))

        ! # Flux waves
        do k=1,num_waves
            fwave(:,k,i) = beta(k) * R(:,k)
        enddo

        ! # Fluctuations
        do k=1, num_waves
            if (s(k, i) < 0.d0) then
                amdq(:, i) = amdq(:, i) + fwave(:, k, i)
            else if (s(k, i) > 0.d0) then
                apdq(:, i) = apdq(:, i) + fwave(:, k, i)
            else
                amdq(:, i) = amdq(:, i) + 0.5d0 * fwave(:, k, i)
                apdq(:, i) = apdq(:, i) + 0.5d0 * fwave(:, k, i)
            end if
        enddo
    enddo

end subroutine rp1


      subroutine resid_dgtheta(ctfm,ctfmp,n_Athth,n_B,n_alpha,n_beta,n_d
     &ivbeta,np1_Athth,np1_B,np1_alpha,np1_beta,np1_divbeta,x,Nx,ht,hx,m
     &yzero,vee,zepsdis,phys_bdy,res)
      implicit none
      integer i
      integer Nx
      real*8 ht
      real*8 hx
      real*8 myzero
      real*8 vee
      real*8 zepsdis
      real*8 ctfm(Nx)
      real*8 ctfmp(Nx)
      real*8 n_Athth(Nx)
      real*8 n_B(Nx)
      real*8 n_alpha(Nx)
      real*8 n_beta(Nx)
      real*8 n_divbeta(Nx)
      real*8 np1_Athth(Nx)
      real*8 np1_B(Nx)
      real*8 np1_alpha(Nx)
      real*8 np1_beta(Nx)
      real*8 np1_divbeta(Nx)
      real*8 x(Nx)
      integer phys_bdy(2)
      real*8 res
      real*8 qb
      res = 0.0D0
      if (phys_bdy(1) .eq. 1) then
      do i=1, 1, 1
      qb = np1_B(i) - 0.1D1 + myzero * x(i)
      res = res + qb**2
      end do
      endif
      if (phys_bdy(1) .eq. 1) then
      do i=2, 2, 1
      qb = -0.5000000000000000D0 * (0.3D1 * np1_B(i - 1) - 0.4D1 * np1_B
     #(i) + np1_B(i + 1)) / hx + 0.3125000000000000D-1 * zepsdis / ht *
     #(0.7D1 * np1_B(i) + np1_B(i + 2) - 0.4D1 * np1_B(i + 1) - 0.4D1 *
     #np1_B(i - 1)) + 0.3125000000000000D-1 * zepsdis / ht * (0.7D1 * n_
     #B(i) + n_B(i + 2) - 0.4D1 * n_B(i + 1) - 0.4D1 * n_B(i - 1))
      res = res + qb**2
      end do
      endif
      do i=3, Nx-2, 1
      qb = (-0.1D1 * n_B(i) + np1_B(i)) / ht + 0.3333333333333333D0 * ve
     #e * np1_divbeta(i) * np1_B(i) - 0.2500000000000000D0 / ctfmp(i) *
     #(-0.1D1 * np1_B(i - 1) + np1_B(i + 1)) / hx * np1_beta(i) + np1_At
     #hth(i) * np1_alpha(i) - 0.1D1 / ctfm(i) * np1_B(i) * np1_beta(i) +
     # 0.3333333333333333D0 * vee * n_divbeta(i) * n_B(i) - 0.2500000000
     #000000D0 / ctfmp(i) * (-0.1D1 * n_B(i - 1) + n_B(i + 1)) / hx * n_
     #beta(i) + n_Athth(i) * n_alpha(i) - 0.1D1 / ctfm(i) * n_B(i) * n_b
     #eta(i) + 0.3125000000000000D-1 * zepsdis / ht * (0.6D1 * np1_B(i)
     #+ np1_B(i + 2) + np1_B(i - 2) - 0.4D1 * np1_B(i + 1) - 0.4D1 * np1
     #_B(i - 1)) + 0.3125000000000000D-1 * zepsdis / ht * (0.6D1 * n_B(i
     #) + n_B(i + 2) + n_B(i - 2) - 0.4D1 * n_B(i + 1) - 0.4D1 * n_B(i -
     # 1))
      res = res + qb**2
      end do
      do i=Nx-1, Nx-1, 1
      qb = (-0.1D1 * n_B(i) + np1_B(i)) / ht + 0.3333333333333333D0 * ve
     #e * np1_divbeta(i) * np1_B(i) - 0.2500000000000000D0 / ctfmp(i) *
     #(-0.1D1 * np1_B(i - 1) + np1_B(i + 1)) / hx * np1_beta(i) + np1_At
     #hth(i) * np1_alpha(i) - 0.1D1 / ctfm(i) * np1_B(i) * np1_beta(i) +
     # 0.3333333333333333D0 * vee * n_divbeta(i) * n_B(i) - 0.2500000000
     #000000D0 / ctfmp(i) * (-0.1D1 * n_B(i - 1) + n_B(i + 1)) / hx * n_
     #beta(i) + n_Athth(i) * n_alpha(i) - 0.1D1 / ctfm(i) * n_B(i) * n_b
     #eta(i)
      res = res + qb**2
      end do
      if (phys_bdy(2) .eq. 1) then
      do i=Nx, Nx, 1
      qb = (-0.1D1 * n_B(i) + np1_B(i)) / ht - 0.1D1 * myzero * x(i)
      res = res + qb**2
      end do
      endif
      res = sqrt(res/(1*Nx))
      END


      real function backtr(vrgs,nt,vr,vrg,zmin,zmax,ltail,ltpar,
     +                     utail,utpar)
c-----------------------------------------------------------------------
c
c           Back Transform Univariate Data from Normal Scores
c           *************************************************
c
c This subroutine backtransforms a standard normal deviate from a
c specified back transform table and option for the tails of the
c distribution.  Call once with "first" set to true then set to false
c unless one of the options for the tail changes.
c
c
c
c INPUT VARIABLES:
c
c   vrgs             normal score value to be back transformed
c   nt               number of values in the back transform tbale
c   vr(nt)           original data values that were transformed
c   vrg(nt)          the corresponding transformed values
c   zmin,zmax        limits possibly used for linear or power model
c   ltail            option to handle values less than vrg(1):
c   ltpar            parameter required for option ltail
c   utail            option to handle values greater than vrg(nt):
c   utpar            parameter required for option utail
c
c
c
c-----------------------------------------------------------------------

      implicit integer (i-n)
      implicit double precision (a-h,o-z)

      parameter(EPSLON=1.0e-20)
      dimension vr(nt),vrg(nt)
      real*8      ltpar,utpar,lambda
      integer   ltail,utail
c
c Value in the lower tail?    1=linear, 2=power, (3 and 4 are invalid):
c
      if(vrgs.le.vrg(1)) then
            backtr = vr(1)
            cdflo  = gcum(vrg(1))
            cdfbt  = gcum(vrgs)
            if(ltail.eq.1) then
                  backtr = powint(0.d0,cdflo,zmin,vr(1),cdfbt,1.0)
            else if(ltail.eq.2) then
                  cpow   = 1.0 / ltpar
                  backtr = powint(0.d0,cdflo,zmin,vr(1),cdfbt,cpow)
            endif
c
c Value in the upper tail?     1=linear, 2=power, 4=hyperbolic:
c
      else if(vrgs.ge.vrg(nt)) then
            backtr = vr(nt)
            cdfhi  = gcum(vrg(nt))
            cdfbt  = gcum(vrgs)
            if(utail.eq.1) then
                  backtr = powint(cdfhi,1.0,vr(nt),zmax,cdfbt,1.0)
            else if(utail.eq.2) then
                  cpow   = 1.0 / utpar
                  backtr = powint(cdfhi,1.0,vr(nt),zmax,cdfbt,cpow)
            else if(utail.eq.4) then
                  lambda = (vr(nt)**utpar)*(1.0-gcum(vrg(nt)))
                  backtr = (lambda/(1.0-gcum(vrgs)))**(1.0/utpar)
            endif
      else
c
c Value within the transformation table:
c
            call locate(vrg,nt,1,nt,vrgs,j)
            j = max(min((nt-1),j),1)
            backtr = powint(vrg(j),vrg(j+1),vr(j),vr(j+1),vrgs,1.0)
      endif
      return
      end


MODULE m_interporb_filt

! ----------------------------------------------------------------------
! MODULE: m_interporb_filt.f03
! ----------------------------------------------------------------------
! Purpose:
!  Module for calling the interp_orb_filt subroutine used for orbit interpolation
!
! Subroutines contained within the module:
! - interp_orb.f03
! ----------------------------------------------------------------------
! Author :	Dr. Thomas Papanikolaou at Geoscience Australia
! Created:	5 May 2020
! ----------------------------------------------------------------------

      IMPLICIT NONE
      !SAVE

Contains

SUBROUTINE interp_orb_filt (ORB_matrix, ORB_filt, interv_in, NPint_in, interpolate_start, orbint)

! ----------------------------------------------------------------------
! SUBROUTINE: interp_orb_filt.f03
! ----------------------------------------------------------------------
! Purpose:
!  GNSS sp3 orbit interpolation based on Lagrange polynomials
! ----------------------------------------------------------------------
! Input arguments:
! - ORB_matrix:  	Input orbit matrix (including position vector per epoch)
! - interv_in: 		Interval of the interpolation epochs (in seconds)
! - NPint:			Number of data points to be used for interpolation (defines the order of the Lagrange polynomial)
!
! Output arguments:
! - orbint:   		Interpolated orbit including position and velocity vetors per epoch (allocatable array)
! ----------------------------------------------------------------------
! Note 1:
!  The input orbit matrix is considered to include satellite position vector
!  only as this is the IGS standard product.
!  During the orbit interpolation, based on Lagrange polynomials, position
!  and velocity vectors are computed
! ----------------------------------------------------------------------
! Author :	Dr. Thomas Papanikolaou at Geoscience Australia
! Created:	5 May 2020
! ----------------------------------------------------------------------

      USE mdl_precision
      USE mdl_num
      !USE m_sp3
      USE m_lagrange
      !USE mdl_arr
      !USE mdl_write
      IMPLICIT NONE

! ----------------------------------------------------------------------
! Dummy arguments declaration
! ----------------------------------------------------------------------
! IN
      !CHARACTER (LEN=300), INTENT(IN) :: fname_sp3
      !CHARACTER (LEN=300), INTENT(IN) :: fname_orbint
      INTEGER (KIND = prec_int8), INTENT(IN) :: interv_in, NPint_in
	  !INTEGER (KIND = 4) :: PRN
      !CHARACTER (LEN=3), INTENT(IN) :: PRN
      REAL (KIND = prec_q), INTENT(IN) :: interpolate_start
      REAL (KIND = prec_q), INTENT(IN), DIMENSION(:,:), ALLOCATABLE :: ORB_matrix, ORB_filt
! OUT
      REAL (KIND = prec_q), INTENT(OUT), DIMENSION(:,:), ALLOCATABLE :: orbint
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Local variables declaration
! ----------------------------------------------------------------------
      INTEGER (KIND = prec_int8) :: Nepochs, epoch_i, sz1, sz2, interv
      INTEGER (KIND = prec_int8) :: Nepochs_0, Nepochs_filt, ORB_filt_epoch, i_filt, NPint
      INTEGER (KIND = prec_int8) :: data_rate, Nepoch_int, Norb_int, NPint_1, NPint_2, Nlimit
	  INTEGER (KIND = prec_int8) :: i, j, i1, i2, ti
	  REAL (KIND = prec_d) :: SecDay_i, MJD_i
	  REAL (KIND = prec_d) :: MJD_tint, tint, Xint, Xint_1, Yint, Yint_1, Zint, Zint_1
	  REAL (KIND = prec_d) :: X, Y, Z, Vx, Vy, Vz
      INTEGER (KIND = prec_int2) :: AllocateStatus, DeAllocateStatus
      CHARACTER (LEN=300) :: fname_write
! ----------------------------------------------------------------------
      REAL (KIND = prec_q), DIMENSION(:,:), ALLOCATABLE :: orbsp3
      REAL (KIND = prec_q), DIMENSION(:), ALLOCATABLE :: X_interp
      REAL (KIND = prec_q), DIMENSION(:), ALLOCATABLE :: Y_interp
! ----------------------------------------------------------------------
      REAL (KIND = prec_q), DIMENSION(:,:), ALLOCATABLE :: clock_matrix
	  REAL (KIND = prec_d) :: delta_min, delta_epoch

sz1 = size(ORB_matrix, DIM = 1)
Nepochs_0 = sz1

sz1 = size(ORB_filt, DIM = 1)
Nepochs_filt = sz1

! Test of numerical interpolation order with available data points
IF (NPint_in > Nepochs_filt) THEN
! Reduce the interpolator order in case that input data points are less than the order value
	NPint = Nepochs_filt
ELSE
	NPint = NPint_in
END IF

! ----------------------------------------------------------------------
! Interpolation epoch limits (24h orbit arc or less):
! 1. Interpolation is applied only within the data points (15 min less than 24h arc):  Set Nlimit to 1
! 2. Interpolation is applied also after the last data point's epoch (23h 45min 00sec) in order to cover a 24h arc:  Set Nlimit to 0
Nlimit = 1
! default initial value?
data_rate = 1
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
	  ! Data rate of intial orbit matrix (in sec)
      DO i = 1 , Nepochs_0-1
		if ( INT(ORB_matrix(i+1,1)) - INT(ORB_matrix(i,1)) == 0 ) then
			data_rate = INT( ORB_matrix(2,2) - ORB_matrix(1,2) )
			EXIT
		end if
	  End Do
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
IF (interv_in < 0 .OR. interv_in > data_rate) THEN
	interv = data_rate
ELSE
	interv = interv_in
END IF
! ----------------------------------------------------------------------

      ! Number of points to be interpolated during the span between two epochs of the sp3 file (15min, 5min)
	  Nepoch_int = data_rate / interv - 1

      ! Number of epochs of interpolated orbit (excluding extrapolation at the last point i.e. the start of the next day's sp3: 00h 00min 00sec)
      Norb_int = (data_rate / interv) * (Nepochs_0 - Nlimit) + Nlimit

! Allocatable arrays for Lagrange Interpolation (declared in mdl_arr.f90)
      ALLOCATE (X_interp(NPint), STAT = AllocateStatus)
      ALLOCATE (Y_interp(NPint), STAT = AllocateStatus)
! Allocatable arrays  for the interpolated orbit array
      ALLOCATE (orbint(Norb_int, 8) , STAT = AllocateStatus)

! Number of data points (used for the interpolation) before and after of each epoch of sp3 file
      NPint_1 = INT(NPint/2)
      NPint_2 = NPint - NPint_1

! ----------------------------------------------------------------------
! Orbit interpolation loop
	  epoch_i = 0
      DO i = 1 , Nepochs_0
		epoch_i  = epoch_i + 1
		MJD_i    = ORB_matrix(i,1)
		SecDay_i = ORB_matrix(i,2)

! ----------------------------------------------------------------------
! Epoch of orbit matrix filtered (without outliers) closest to interpolation epoch
delta_min = MJD_i - ORB_filt(1,1)
ORB_filt_epoch = 1
DO i_filt = 1 , Nepochs_filt
	delta_epoch = MJD_i - ORB_filt(i_filt,1)
	IF ( ABS(delta_epoch) < ABS(delta_min) ) THEN
		delta_min = delta_epoch
		ORB_filt_epoch = i_filt
	END IF
END DO
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Form arrays with data points to be used in the interpolation
		!if ( i < NPint_1 ) then
		if (Nepochs_filt == NPint) then
		! Case: Available data points are equal to the interpolation order
			i1 = 1
			i2 = Nepochs_filt
		else if ( ORB_filt_epoch < NPint_1 ) then
			i1 = ORB_filt_epoch
			i2 = ORB_filt_epoch + (NPint - 1)
		else if ( Nepochs_filt - ORB_filt_epoch < NPint_2 ) then
			i2 = Nepochs_filt
			i1 = i2 - (NPint-1)
		else
			i1 = ORB_filt_epoch - (NPint_1 - 1)
			i2 = ORB_filt_epoch + NPint_2
		end if
		!print *,"NPint_1, NPint_2, NPint", NPint_1, NPint_2, NPint
		!print *,"i, epoch_i             ", i, epoch_i
		!print *,"ORB_filt_epoch, i1, i2 ", ORB_filt_epoch, i1, i2
! ----------------------------------------------------------------------
! Time array of the data points used for the interpolation: X_interp
		ti = 0
		Do j = i1 , i2
		ti = ti + 1
			X_interp(ti) = ORB_filt(j , 2) + ( INT(ORB_filt(j,1)) - INT(ORB_filt(i1,1)) ) * 86400.0D0
			!X_interp(ti) = ORB_filt(j , 1)
		End Do
	    j = 0
! ----------------------------------------------------------------------!
!print *,"X_interp", X_interp
!STOP

! ----------------------------------------------------------------------
! Lagrange interpolation
! ----------------------------------------------------------------------
		! Lagrange interpolation at interpolation epoch
		tint = SecDay_i

		! Position and Velocity Vectors
		! X
		Y_interp = ORB_filt(i1:i2, 3)
		Call interp_lag(tint, X_interp, Y_interp, Xint)
		! Vx
		Call interp_lag(tint-1, X_interp, Y_interp, Xint_1)
        !Vx = Xint_1 - Xint
        Vx = Xint - Xint_1

		! Y
		Y_interp = ORB_filt(i1:i2, 4)
		Call interp_lag(tint, X_interp, Y_interp, Yint)
		! Vy
		Call interp_lag(tint-1, X_interp, Y_interp, Yint_1)
        !Vy = Yint_1 - Yint
        Vy = Yint - Yint_1

		! Z
		Y_interp = ORB_filt(i1:i2, 5)
		Call interp_lag(tint, X_interp, Y_interp, Zint)
		! Vz
		Call interp_lag(tint-1, X_interp, Y_interp, Zint_1)
        !Vz = Zint_1 - Zint
        Vz = Zint - Zint_1

! Write interpolated state vector to the matrix
		! Time arguments
		orbint(epoch_i, 1) = ORB_matrix(i,1)
		orbint(epoch_i, 2) = ORB_matrix(i,2)
		! Position vector (interpolated)
		orbint(epoch_i, 3) = Xint
		orbint(epoch_i, 4) = Yint
		orbint(epoch_i, 5) = Zint
		! Velocity vector (interpolated)
		orbint(epoch_i, 6) = Vx
		orbint(epoch_i, 7) = Vy
		orbint(epoch_i, 8) = Vz
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Lagrange Interpolation applied for the new points
! ----------------------------------------------------------------------
! When Nlimit=0 & i=Nepochs_0 : Extrapolation is performed after the last data point of the sp3 file (for completing 24h arc)
If (i <= Nepochs_0 - Nlimit) then
		DO j = 1 , Nepoch_int
		! Lagrange interpolation at interpolation epoch
			tint = j * interv * 1.D0 + SecDay_i

		if (1 < 0) then
		!if (i == Nepochs_0 .and. j == Nepoch_int) then
		! Avoid computing the last point since this is the start point of the next day's sp3 file
		PRINT *,"i,j", i,j
		PRINT *,"orbint(epoch_i,:)", orbint(epoch_i,:)
		PRINT *,"orbint(epoch_i-1,:)", orbint(epoch_i-1,:)
		EXIT
		end if

		! Position and Velocity Vectors
			! X
			Y_interp = ORB_filt(i1:i2, 3)
			Call interp_lag(tint, X_interp, Y_interp, Xint)
			! Vx
			Call interp_lag(tint-1, X_interp, Y_interp, Xint_1)
            !Vx = Xint_1 - Xint
            Vx = Xint - Xint_1

			! Y
			Y_interp = ORB_filt(i1:i2, 4)
			Call interp_lag(tint, X_interp, Y_interp, Yint)
			! Vy
			Call interp_lag(tint-1, X_interp, Y_interp, Yint_1)
            !Vy = Yint_1 - Yint
            Vy = Yint - Yint_1

			! Z
			Y_interp = ORB_filt(i1:i2, 5)
			Call interp_lag(tint, X_interp, Y_interp, Zint)
			! Vz
			Call interp_lag(tint-1, X_interp, Y_interp, Zint_1)
            !Vz = Zint_1 - Zint
            Vz = Zint - Zint_1

! Write interpolated values to the output matrix
! Next epoch
			epoch_i = epoch_i + 1
! MJD epoch
			MJD_tint = INT(MJD_i) * 1.D0 + tint / 86400.D0
			! Write Time arguments of interpolation epoch to the matrix
			orbint(epoch_i, 1) = MJD_tint
			If (tint > 86400.D0) then
			   tint = tint - 86400.D0
			End If
			orbint(epoch_i, 2) = tint
			! Write interpolated state vector to the matrix
			! Position vector (interpolated)
			orbint(epoch_i, 3) = Xint
			orbint(epoch_i, 4) = Yint
			orbint(epoch_i, 5) = Zint
			! Velocity vector (interpolated)
			orbint(epoch_i, 6) = Vx
			orbint(epoch_i, 7) = Vy
			orbint(epoch_i, 8) = Vz
		End DO
End IF
! ----------------------------------------------------------------------
      End Do
	  Deallocate (X_interp)
	  Deallocate (Y_interp)
End subroutine

END Module


      SUBROUTINE cdffnc(which,p,q,f,dfn,dfd,phonc,status,bound)
C**********************************************************************
C
C      SUBROUTINE CDFFNC( WHICH, P, Q, F, DFN, DFD, PNONC, STATUS, BOUND
C               Cumulative Distribution Function
C               Non-central F distribution
C
C
C                              Function
C
C
C     Calculates any one parameter of the Non-central F
C     distribution given values for the others.
C
C
C                              Arguments
C
C
C     WHICH --> Integer indicating which of the next five argument
C               values is to be calculated from the others.
C               Legal range: 1..5
C               iwhich = 1 : Calculate P and Q from F,DFN,DFD and PNONC
C               iwhich = 2 : Calculate F from P,Q,DFN,DFD and PNONC
C               iwhich = 3 : Calculate DFN from P,Q,F,DFD and PNONC
C               iwhich = 4 : Calculate DFD from P,Q,F,DFN and PNONC
C               iwhich = 5 : Calculate PNONC from P,Q,F,DFN and DFD
C                    INTEGER WHICH
C
C       P <--> The integral from 0 to F of the non-central f-density.
C              Input range: [0,1-1E-16).
C                    DOUBLE PRECISION P
C
C       Q <--> 1-P.
C            Q is not used by this subroutine and is only included
C            for similarity with other cdf* routines.
C                    DOUBLE PRECISION Q
C
C       F <--> Upper limit of integration of the non-central f-density.
C              Input range: [0, +infinity).
C              Search range: [0,1E300]
C                    DOUBLE PRECISION F
C
C     DFN < --> Degrees of freedom of the numerator sum of squares.
C               Input range: (0, +infinity).
C               Search range: [ 1E-300, 1E300]
C                    DOUBLE PRECISION DFN
C
C     DFD < --> Degrees of freedom of the denominator sum of squares.
C               Must be in range: (0, +infinity).
C               Input range: (0, +infinity).
C               Search range: [ 1E-300, 1E300]
C                    DOUBLE PRECISION DFD
C
C     PNONC <-> The non-centrality parameter
C               Input range: [0,infinity)
C               Search range: [0,1E4]
C                    DOUBLE PRECISION PHONC
C
C     STATUS <-- 0 if calculation completed correctly
C               -I if input parameter number I is out of range
C                1 if answer appears to be lower than lowest
C                  search bound
C                2 if answer appears to be higher than greatest
C                  search bound
C                3 if P + Q .ne. 1
C                    INTEGER STATUS
C
C     BOUND <-- Undefined if STATUS is 0
C
C               Bound exceeded by parameter number I if STATUS
C               is negative.
C
C               Lower search bound if STATUS is 1.
C
C               Upper search bound if STATUS is 2.
C
C
C                              Method
C
C
C     Formula  26.6.20   of   Abramowitz   and   Stegun,  Handbook  of
C     Mathematical  Functions (1966) is used to compute the cumulative
C     distribution function.
C
C     Computation of other parameters involve a seach for a value that
C     produces  the desired  value  of P.   The search relies  on  the
C     monotinicity of P with the other parameter.
C
C                            WARNING
C
C     The computation time  required for this  routine is proportional
C     to the noncentrality  parameter  (PNONC).  Very large  values of
C     this parameter can consume immense  computer resources.  This is
C     why the search range is bounded by 10,000.
C
C                              WARNING
C
C     The  value  of the  cumulative  noncentral F distribution is not
C     necessarily monotone in either degrees  of freedom.  There  thus
C     may be two values that provide a given  CDF value.  This routine
C     assumes monotonicity  and will find  an arbitrary one of the two
C     values.
C
C**********************************************************************
C     .. Parameters ..
      DOUBLE PRECISION tent4
      PARAMETER (tent4=1.0D4)
      DOUBLE PRECISION tol
      PARAMETER (tol=1.0D-13)
      DOUBLE PRECISION atol
      PARAMETER (atol=1.0D-50)
      DOUBLE PRECISION zero,one,inf
      PARAMETER (zero=1.0D-300,one=1.0D0-1.0D-16,inf=1.0D300)
C     ..
C     .. Scalar Arguments ..
      DOUBLE PRECISION bound,dfd,dfn,f,p,q,phonc
      INTEGER status,which
C     ..
C     .. Local Scalars ..
      DOUBLE PRECISION fx,cum,ccum
      LOGICAL qhi,qleft
C     ..
C     .. External Subroutines ..
      EXTERNAL dinvr,dstinv,cumfnc
C     ..
C     .. Executable Statements ..
C
C     Check arguments
C
      IF (.NOT. ((which.LT.1).OR. (which.GT.5))) GO TO 30
      IF (.NOT. (which.LT.1)) GO TO 10
      bound = 1.0D0
      GO TO 20

   10 bound = 5.0D0
   20 status = -1
      RETURN

   30 IF (which.EQ.1) GO TO 70
C
C     P
C
      IF (.NOT. ((p.LT.0.0D0).OR. (p.GT.one))) GO TO 60
      IF (.NOT. (p.LT.0.0D0)) GO TO 40
      bound = 0.0D0
      GO TO 50

   40 bound = one
   50 status = -2
      RETURN

   60 CONTINUE
   70 IF (which.EQ.2) GO TO 90
C
C     F
C
      IF (.NOT. (f.LT.0.0D0)) GO TO 80
      bound = 0.0D0
      status = -4
      RETURN

   80 CONTINUE
   90 IF (which.EQ.3) GO TO 110
C
C     DFN
C
      IF (.NOT. (dfn.LE.0.0D0)) GO TO 100
      bound = 0.0D0
      status = -5
      RETURN

  100 CONTINUE
  110 IF (which.EQ.4) GO TO 130
C
C     DFD
C
      IF (.NOT. (dfd.LE.0.0D0)) GO TO 120
      bound = 0.0D0
      status = -6
      RETURN

  120 CONTINUE
  130 IF (which.EQ.5) GO TO 150
C
C     PHONC
C
      IF (.NOT. (phonc.LT.0.0D0)) GO TO 140
      bound = 0.0D0
      status = -7
      RETURN

  140 CONTINUE
C
C     Calculate ANSWERS
C
  150 IF ((1).EQ. (which)) THEN
C
C     Calculating P
C
          CALL cumfnc(f,dfn,dfd,phonc,p,q)
          status = 0

      ELSE IF ((2).EQ. (which)) THEN
C
C     Calculating F
C
          f = 5.0D0
          CALL dstinv(0.0D0,inf,0.5D0,0.5D0,5.0D0,atol,tol)
          status = 0
          CALL dinvr(status,f,fx,qleft,qhi)
  160     IF (.NOT. (status.EQ.1)) GO TO 170
          CALL cumfnc(f,dfn,dfd,phonc,cum,ccum)
          fx = cum - p
          CALL dinvr(status,f,fx,qleft,qhi)
          GO TO 160

  170     IF (.NOT. (status.EQ.-1)) GO TO 200
          IF (.NOT. (qleft)) GO TO 180
          status = 1
          bound = 0.0D0
          GO TO 190

  180     status = 2
          bound = inf
  190     CONTINUE
  200     CONTINUE

      ELSE IF ((3).EQ. (which)) THEN
C
C     Calculating DFN
C
          dfn = 5.0D0
          CALL dstinv(zero,inf,0.5D0,0.5D0,5.0D0,atol,tol)
          status = 0
          CALL dinvr(status,dfn,fx,qleft,qhi)
  210     IF (.NOT. (status.EQ.1)) GO TO 220
          CALL cumfnc(f,dfn,dfd,phonc,cum,ccum)
          fx = cum - p
          CALL dinvr(status,dfn,fx,qleft,qhi)
          GO TO 210

  220     IF (.NOT. (status.EQ.-1)) GO TO 250
          IF (.NOT. (qleft)) GO TO 230
          status = 1
          bound = zero
          GO TO 240

  230     status = 2
          bound = inf
  240     CONTINUE
  250     CONTINUE

      ELSE IF ((4).EQ. (which)) THEN
C
C     Calculating DFD
C
          dfd = 5.0D0
          CALL dstinv(zero,inf,0.5D0,0.5D0,5.0D0,atol,tol)
          status = 0
          CALL dinvr(status,dfd,fx,qleft,qhi)
  260     IF (.NOT. (status.EQ.1)) GO TO 270
          CALL cumfnc(f,dfn,dfd,phonc,cum,ccum)
          fx = cum - p
          CALL dinvr(status,dfd,fx,qleft,qhi)
          GO TO 260

  270     IF (.NOT. (status.EQ.-1)) GO TO 300
          IF (.NOT. (qleft)) GO TO 280
          status = 1
          bound = zero
          GO TO 290

  280     status = 2
          bound = inf
  290     CONTINUE
  300     CONTINUE

      ELSE IF ((5).EQ. (which)) THEN
C
C     Calculating PHONC
C
          phonc = 5.0D0
          CALL dstinv(0.0D0,tent4,0.5D0,0.5D0,5.0D0,atol,tol)
          status = 0
          CALL dinvr(status,phonc,fx,qleft,qhi)
  310     IF (.NOT. (status.EQ.1)) GO TO 320
          CALL cumfnc(f,dfn,dfd,phonc,cum,ccum)
          fx = cum - p
          CALL dinvr(status,phonc,fx,qleft,qhi)
          GO TO 310

  320     IF (.NOT. (status.EQ.-1)) GO TO 350
          IF (.NOT. (qleft)) GO TO 330
          status = 1
          bound = 0.0D0
          GO TO 340

  330     status = 2
          bound = tent4
  340     CONTINUE
  350 END IF

      RETURN

      END


C
C $Id: tdcone.f,v 1.3 2008-07-27 00:17:31 haley Exp $
C
C                Copyright (C)  2000
C        University Corporation for Atmospheric Research
C                All Rights Reserved
C
C The use of this Software is governed by a License Agreement.
C
      SUBROUTINE TDCONE (UACC,VACC,WACC,UDCS,VDCS,WDCS,RADI,NPAC,
     +                                       NPCT,UTIP,VTIP,WTIP)
C
C Draw the projection of a cone in 3-space.  The circular base of the
C cone is centered at the point (UACC,VACC,WACC), is perpendicular to
C the vector with direction cosines UDCS, VDCS, and WDCS, has a radius
C of RADI, and is drawn using NPAC equispaced points along the circle;
C NPCT points equispaced along the circle are connected to the tip,
C which is at (UTIP,VTIP,WTIP).
C
        DATA DTOR / .017453292519943 /
        DATA RTOD / 57.2957795130823 /
C
C       RLAT=RTOD*ASIN(WDCS)
C
C       IF (UDCS.NE.0.OR.VDCS.NE.0.) THEN
C         RLON=RTOD*ATAN2(VDCS,UDCS)
C       ELSE
C         RLON=0.
C       END IF
C
C Draw the circular base of the arrowhead.  Essentially, we form the
C circle in the UV plane, perform two rotations (one about the V axis,
C by the angle 90-RLAT, and one about the W axis, by the angle RLON),
C and then translate the circle to its desired final position.  The
C rotations involve sines and cosines of 90-RLAT and of RLON; the code
C is simplified by computing these from the direction cosines.
C
        DNOM=SQRT(UDCS*UDCS+VDCS*VDCS)
C
        DO 101 I=0,NPAC
          UCR1=RADI*COS(DTOR*REAL(I)*(360./REAL(NPAC)))
          UCR2=UCR1*WDCS
          VCR2=RADI*SIN(DTOR*REAL(I)*(360./REAL(NPAC)))
          IF (DNOM.EQ.0.) THEN
            UCR3=UCR2
            VCR3=VCR2
          ELSE
            UCR3=(UCR2*UDCS-VCR2*VDCS)/DNOM
            VCR3=(VCR2*UDCS+UCR2*VDCS)/DNOM
          END IF
          WCR3=-UCR1*SQRT(1.-WDCS*WDCS)
          CALL TDPRPT (UACC+UCR3,VACC+VCR3,WACC+WCR3,XPOS,YPOS)
          CALL PLOTIF (CUFX(XPOS),CUFY(YPOS),MIN(I,1))
  101   CONTINUE
C
C Connect some points along the base of the circle to the tip, using
C very similar code.
C
        CALL TDPRPT (UTIP,VTIP,WTIP,XTIP,YTIP)
C
        XTIP=CUFX(XTIP)
        YTIP=CUFY(YTIP)
C
        DO 102 I=0,NPCT
          CALL PLOTIF (XTIP,YTIP,0)
          UCR1=RADI*COS(DTOR*REAL(I)*(360./REAL(NPCT)))
          UCR2=UCR1*WDCS
          VCR2=RADI*SIN(DTOR*REAL(I)*(360./REAL(NPCT)))
          IF (DNOM.EQ.0.) THEN
            UCR3=UCR2
            VCR3=VCR2
          ELSE
            UCR3=(UCR2*UDCS-VCR2*VDCS)/DNOM
            VCR3=(VCR2*UDCS+UCR2*VDCS)/DNOM
          END IF
          WCR3=-UCR1*SQRT(1.-WDCS*WDCS)
          CALL TDPRPT (UACC+UCR3,VACC+VCR3,WACC+WCR3,XPOS,YPOS)
          CALL PLOTIF (CUFX(XPOS),CUFY(YPOS),1)
  102   CONTINUE
C
C Flush the point-draw buffers.
C
        CALL PLOTIF (0.,0.,2)
C
C Done.
C
        RETURN
C
      END


C++***************************************************************
C Program to compute FFT_1D
C Calling FFT1 (either in "fft_nag.for" or "fft_jlp.for")
C WARNING: NAG FFT divides the result by SQRT(NX)
C
C JLP
C Version of 26-09-95
C--***************************************************************
	PROGRAM FFT1
        INTEGER*4 PNTR_RE, PNTR_IM, MADRID(1)
     	INTEGER KOD
	CHARACTER NAME*40,COMMENTS*80,ANS*1
        COMMON /VMR/MADRID

	CALL JLP_BEGIN

C Inquire the format (input/output) :
	CALL JLP_INQUIFMT

C Option:
19	WRITE(6,15)
15	FORMAT(' Options: ',/,
     1	' 1=direct FFT (in/out: real,imaginary)',/,
     1	' 2=direct FFT (in: real,ima /out: power,phase)',/,
     1	' -1=inverse FFT (in/out: real,imaginary)',/,
     1	' -2=inverse FFT (in: power,phase /out: real,ima)',/,
     1	' Enter your choice:')
	READ(5,*) KOD
	IF((IABS(KOD).NE.1).AND.(IABS(KOD).NE.2))THEN
	   WRITE(6,17)
17	   FORMAT(' Error: enter again the option')
	   GOTO 19
	ENDIF

C Input :
	WRITE(6,*)' Input image (real part):'
	READ(5,10) NAME
	CALL JLP_VM_READIMAG(PNTR_RE,NX,NY,NAME,COMMENTS)
        IF(KOD.EQ.-1)THEN
	  CALL RECENT_FFT_1D_Y_FLOAT(MADRID(PNTR_RE),MADRID(PNTR_RE),NX,NY,NX)
        ENDIF

C Imaginary part:
	  IF(KOD.NE.-2) THEN
	    WRITE(6,22)
22	  FORMAT(' Input image (imaginary part):',/,
     1	' Null imaginary part? (N) ?')
	  ELSE
	    WRITE(6,23)
23	  FORMAT(' Input image (phase):',/,
     1	' Null phase? (N) ?')
	  ENDIF
C
	  READ(5,10) ANS
10	  FORMAT(A)

	IF(ANS.NE.'Y'.AND.ANS.NE.'y')THEN
	  READ(5,10) NAME
	  CALL JLP_VM_READIMAG(PNTR_IM,NX1,NY1,NAME,COMMENTS)
          IF(KOD.EQ.-1)THEN
	  CALL RECENT_FFT_1D_Y_FLOAT(MADRID(PNTR_IM),MADRID(PNTR_IM),
     1                               NX1,NY1,NX1)
          ENDIF
C Check the size:
	  IF((NX1.NE.NX).OR.(NY1.NE.NY))THEN
	    WRITE(6,21)
21	   FORMAT(' FATAL ERROR: Wrong size for the input images',/,
     1	' (should be the same for the real/imaginary parts)')
	    GOTO 999
	  ENDIF
	ELSE
          CALL JLP_GETVM(PNTR_IM,NX*NY*4)
          CALL ZERO(MADRID(PNTR_IM),NX,NY)
	ENDIF

C FFT:
	WRITE(6,*) ' Start FFT'
	CALL FFT_1D(MADRID(PNTR_RE),MADRID(PNTR_IM),NX,NY,NX,KOD)
	WRITE(6,*) ' FFT OK'

C Output :
	WRITE(6,*)' Real part / Power spect.:'
	READ(5,10) NAME
	CALL RECENT_FFT_1D_Y_FLOAT(MADRID(PNTR_RE),MADRID(PNTR_RE),NX,NY,NX)
	CALL JLP_WRITEIMAG(MADRID(PNTR_RE),NX,NY,NX,NAME,COMMENTS)

	WRITE(6,*)' Imaginary part / Phase:'
	READ(5,10) NAME
	CALL RECENT_FFT_1D_Y_FLOAT(MADRID(PNTR_IM),MADRID(PNTR_IM),NX,NY,NX)
	CALL JLP_WRITEIMAG(MADRID(PNTR_IM),NX,NY,NX,NAME,COMMENTS)

999	CALL JLP_END
	STOP
	END
C****************************************************************
        SUBROUTINE ZERO(ARRAY,NX,NY)
        REAL ARRAY(NX,NY)
	  DO J=1,NY
	   DO I=1,NX
	    ARRAY(I,J)=0.
	   ENDDO
	  ENDDO
        RETURN
        END
C****************************************************************
C	include 'fft:fft_nag.for'
C	include 'fft:fft_jlp.for'