!##############################################################################
!# ****************************************************************************
!# <name> chemotaxis_pattern (FCT) 3D </name>
!# ****************************************************************************
!#
!# <purpose>
!#
!# This is a solver for the problem proposed by Aida, Tsujikawa, Efendiev, Yagi and Mimura in their paper
!# "LOWER ESTIMATE OF THE ATTRACTOR DIMENSION FOR A CHEMOTAXIS GROWTH SYSTEM"
!#
!#	d/dt u =  0.0625 * \Delta u - \nu \nabla * ( u * \nabla c ) + u^2 ( 1-u )
!#      d/dt c = \Delta c - 32 * c + u
!#
!#
!# 	with a proper NBC for u and c as well as an IC
!#      CHI may depend on u and c , e.g. CHI=CHI(u,c)
!#      for a nonlinear CHI a simple defect correction is proposed
!#      for this case CHI should be defined in the underlying callback file chemotaxis_callback.f90
!#
!# The equation is discretised in time by an implicit Euler Method, providing the
!# following discrete equation:
!#
!#
!#	!!!! imlicit Euler !!!!!
!#
!#	1/dt * ( u_{n+1}-u{n} ) = Laplace u_{n+1} - grad (CHI*u_{n+1} * grad c_{n+1})	/ multiplying with test func and dt, take int
!#	1/dt * ( c_{n+1}-c{n} ) = Laplace c_{n+1} - c_{n+1} + u_{n}
!#	__________________
!#
!#	(u_{n+1},phi) 		=  dt*( Laplace u_{n+1},phi ) - ( grad (CHI*u_{n+1} * grad c_{n+1} ),phi ) + ( u_{n},phi )
!#
!#      (c_{n+1},phi) 		=  dt*( Laplace c_{n+1},phi ) - dt*( c_{n+1} +u_{n},phi ) + ( c_{n},phi )
!#	__________________
!#
!#	(u_{n+1},phi) 		= - dt*( grad u_{n+1},grad phi ) + dt*( CHI * u_{n+1}* grad_c, grad_phi ) + ( u_{n},phi )
!#
!#      (c_{n+1},phi) 		= - dt*( grad c_{n+1},grad phi ) - dt*( u_{n} + c_{n+1},phi ) + ( c_{n},phi )
!#	__________________
!#
!#	[M + dt*L - dt*M_2] u_{n+1} 	= [ M ] u_{n}
!#
!#   [M + dt*L + dt*M] c_{n+1} 		= [ M ] c_{n} + dt* M u_n
!#
!#
!# The whole solution process is divided into serveral subroutines, using
!# the BiCG-solver (defect correction ) for the linear (nonlinear) subproblems in every
!# timestep
!#
!# The module bases on the standard heatconduction example and shows which
!# modifications are necessary to create a chemotaxis solver from a heatconduction
!# solver. Boundary conditions, matrices and vectors are not all reassembled in
!# every timestep, in contrast to the heatcond_method1.f90
!# </purpose>
!##############################################################################

module chemotaxis_pattern_FCT
  use fsystem
  use genoutput
  use storage
  use linearsolver
  use boundary
  use trilinearformevaluation
  use bilinearformevaluation
  use linearformevaluation
  use cubature
  use filtersupport
  use linearsystemscalar
  use linearsystemblock
  use matrixfilters
  use vectorfilters
  use discretebc
  use bcassembly
  use triangulation
  use spatialdiscretisation
  use scalarpde
  use ucd
  use pprocerror
  use genoutput
  use analyticprojection
  use matrixio
  use vectorio
  use collection
  use paramlist
  use linearalgebra
  use analyticprojection
  use feevaluation

  use chemotaxis_callback

  implicit none

contains

!<subroutine>

  subroutine chemotaxispatternFCT

!<description>
  ! The routine performs the following tasks:
  !
  ! 1.) Read in parametrisation
  ! 2.) Read in triangulation
  ! 3.) Set up RHS
  ! 4.) Set up matrix
  ! 5.) Create solver structure
  ! 6.) Solve the problem
  ! 7.) Write solution to GMV file if desired
  ! 8.) Release all variables, finish
!</description>

!</subroutine>

    !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    !!!!! Definitions of variables. !!!!!
    !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    ! A parameter list structure accepting the parameters from the DAT file.
    type(t_parlist) :: rparams
    !
    !!!!! parameters' variables !!!!!
    ! maximum level of refinment
    integer :: NLMAX
    !
    ! Defining the output level
    ! =0 means no gmv files generated
    ! =1 means gmv files printed
    ! =2 means just the gmv file is printed
    ! =3 means just the ISTEP_GMV-th gmv file are printed
    integer :: OUTPUT, ISTEP_GMV
    !
    ! Whether or not taking an const initial sol
    ! =0 means const initial sol
    ! =1 means exponential initial sol
    ! Whether or not we set the IC by l_2 projection ( = 1 ) or by interpolation ( =0 )
    integer :: INITSOL , L2PROJ
    !
    ! Defining the gmvoutputfolder
    integer :: gmvfolder, checkneg
    !
    ! Variables for the defect correction ( nonlinear loop )
    real(DP) :: defect , defectTol , negthres
    !
    ! Some params used to adjust the model
    real(DP) :: CHI, D_1, D_2, A_CHEMO , A_CELLS , B_CHEMO , B_CELLS, W, SIGMA, BETA, ALPHA, R,  PHI, GAMMA, N
    !
    ! If INITSOL = 0 , meaning const init sol, we use these variables to
    ! define the const value
    real(DP) :: C_0, U_0
    !
    ! Scaling params for the analytic given sols
    real(DP) :: SCALE_U , SCALE_C
    !
    ! Time step size, number of timesteps.(also for a fixed amount of steps, since there is
    ! a basic loop control implemented dealing w/ the steady state analysis)
    real(DP) :: dtstep
    integer :: ntimesteps, steps
    !
    ! Time variables and counters
    real(DP) :: dtime
    integer :: itimestep
    real(DP):: time_start
    !
    ! maximal iterations for the defect correction . Since we allow possible nonlinearities
    integer :: maxiterationdef
    !
    ! error-norm constant
    ! To be set in the .dat file
    integer :: CTRLNORM
    !
    ! error analysis
    real(DP) :: uerror, cerror, tol

    !!!!! triangulation variables !!!!!
    ! maximum level of refinment
    !
    ! An object for saving the triangulation on the domain
    type(t_triangulation) :: rtriangulation
    !
    ! Path to the mesh
    character(len=SYS_STRLEN) :: spredir
    !
    ! An object specifying the discretisation.
    ! This contains also information about trial/test functions,...
    type(t_blockDiscretisation) :: rdiscretisation

    !!!!! solution + rhs + def vectors !!!!!
    type(t_vectorScalar) :: ranalyticcells , ranalyticchemo, rcold, ruold
    !A pointer to the entries of vector rchemoattract
    real(DP), dimension(:), pointer ::  p_analyticcells, p_analyticchemo, p_cold, p_uold
    real(DP), dimension(:), pointer ::  p_vectordata, p_chemodata
    !
    ! cell and chemoattractant solution vectors
    type(t_vectorScalar)  :: rchemoattract, rrhschemo, rcell, rrhscell
    !
    ! defect vectors
    type(t_vectorScalar)  :: rdef
    !
    ! block vectors
    type(t_vectorBlock) :: rchemoattractBlock, rdefBlock, rrhsBlockchemo
    type(t_vectorBlock) :: rcellBlock
    type(t_vectorBlock) :: rtempBlock

    !!!!! matrices !!!!!
    type(t_matrixScalar)  :: rmassmatrix, rsysmatrix, rlaplace, rmatrixchemo, rtemp
    !
    ! block matrices
    type(t_matrixBlock) :: rmatrixBlockchemo
    !

    !!!!! boundary conditions !!!!!
    ! A variable describing the discrete boundary conditions.
    type(t_discreteBC), target :: rdiscreteBC, rdiscreteBCchemo

    !!!!! Filter chains !!!!!
    type(t_filterChain), dimension(1), target :: RfilterChain
    type(t_filterChain), dimension(:), pointer :: p_RfilterChain

    !!!!! solver stuff !!!!!
    ! A solver node that accepts parameters for the linear solver
    type(t_linsolNode), pointer :: p_rsolverNode,p_rpreconditioner,p_rsolverNode_cells,p_rpreconditioner_cells
    ! An array for the system matrix(matrices) during the initialisation of
    ! the linear solver.
    type(t_matrixBlock), dimension(1) :: Rmatrices

    !!!!! GMV output !!!!!
    ! Output block for UCD output to GMV file
    type(t_ucdExport) :: rexport
    real(DP), dimension(:), pointer :: p_Ddata

    !!!!! Error indicators !!!!!!
    ! Error indicator during initialisation of the solver
    integer :: ierror

    !!!!!!!!!!!!!!!!!!!!!!!!!!!!
    !!!!! Ok, let's start. !!!!!
    !!!!!!!!!!!!!!!!!!!!!!!!!!!!
    call parlst_init(rparams)

     call parlst_readfromfile(rparams, 'data/chemotaxis.dat')

    ! Getting some general params for the pb
    call parlst_getvalue_int (rparams, 'GENERAL', 'NLMAX', NLMAX, 7)
    call parlst_getvalue_int (rparams, 'GENERAL' , 'OUTPUT' , OUTPUT , 0)
    call parlst_getvalue_int (rparams, 'GENERAL' , 'ISTEP_GMV' , ISTEP_GMV , 1)
    call parlst_getvalue_int (rparams, 'GENERAL' , 'INITSOL' , INITSOL , 1)
    call parlst_getvalue_int ( rparams, 'GENERAL' , 'L2PROJ' , L2PROJ , 0)
    call parlst_getvalue_int(rparams, 'GENERAL', 'GMVFOLDER', gmvfolder, 0)
    call parlst_getvalue_int(rparams, 'GENERAL', 'CHECKNEG', checkneg , 0)
    call parlst_getvalue_double(rparams, 'GENERAL', 'DEFECTTOL', defectTol , 0.0000001_DP)
    call parlst_getvalue_double(rparams, 'GENERAL', 'NEGTHRES', negthres , -0.1_DP)

    ! We set the chemotaxis and diffusion coefficient
    call parlst_getvalue_double (rparams, 'COEFFICIENTS', 'CHI', CHI, 1.0_DP)
    call parlst_getvalue_double (rparams, 'COEFFICIENTS', 'W', W, 1.0_DP)
    call parlst_getvalue_double (rparams, 'COEFFICIENTS', 'BETA', BETA, 1.0_DP)
    call parlst_getvalue_double (rparams, 'COEFFICIENTS', 'SIGMA', SIGMA, 1.0_DP)
    call parlst_getvalue_double (rparams, 'COEFFICIENTS', 'ALPHA', ALPHA, 1.0_DP)
    call parlst_getvalue_double (rparams, 'COEFFICIENTS', 'R', R, 1.0_DP)
    call parlst_getvalue_double (rparams, 'COEFFICIENTS', 'D_1', D_1, 1.0_DP)
    ! Robert_4_2: D_1 = 0.0625_DP ! This is the diffusioncoefficient in the paper
    call parlst_getvalue_double (rparams, 'COEFFICIENTS', 'PHI', PHI, 1.0_DP)
    call parlst_getvalue_double (rparams, 'COEFFICIENTS', 'GAMMA', GAMMA, 1.0_DP)
    call parlst_getvalue_double (rparams, 'COEFFICIENTS', 'N', N, 1.0_DP)
    call parlst_getvalue_double (rparams, 'COEFFICIENTS', 'D_2', D_2, 1.0_DP)
    D_2 = 1.0_DP  ! This is the diffusioncoefficient in the paper
    call parlst_getvalue_double (rparams, 'coefficients', 'A_CHEMO', A_CHEMO, 10.0_dp)
    call parlst_getvalue_double (rparams, 'coefficients', 'A_CELLS', A_CELLS, 0.6_dp)
    call parlst_getvalue_double (rparams, 'coefficients', 'B_CHEMO', B_CHEMO, 10.0_dp)
    call parlst_getvalue_double (rparams, 'COEFFICIENTS', 'B_CELLS', B_CELLS, 10.0_DP)
    call parlst_getvalue_double (rparams, 'COEFFICIENTS', 'U_0', U_0, 2.0_DP)
    call parlst_getvalue_double (rparams, 'COEFFICIENTS', 'C_0', C_0, 1.0_DP)
    call parlst_getvalue_double (rparams, 'COEFFICIENTS', 'SCALE_U', SCALE_U, 2.0_DP)
    call parlst_getvalue_double (rparams, 'COEFFICIENTS', 'SCALE_C', SCALE_C, 1.0_DP)

     ! Initialise time step size and number of timesteps
    call parlst_getvalue_int (rparams, 'TIMESTEPPING', 'NTIMESTEPS', ntimesteps, 100)
    call parlst_getvalue_double (rparams, 'TIMESTEPPING', 'DTSTEP', dtstep, 0.00001_DP)
    call parlst_getvalue_int (rparams, 'TIMESTEPPING', 'STEPS', steps, 0)
    call parlst_getvalue_int (rparams, 'TIMESTEPPING', 'MAXITERATIONDEF', maxiterationdef, 10)

    ! We start at a certain time ...
    call parlst_getvalue_double(rparams, 'TIMESTEPPING' , 'STARTTIME' , dtime , 0.0_DP)

    ! Get the errorctrl norm
    call parlst_getvalue_int (rparams, 'NORM', 'CONTROLNORM', CTRLNORM, 2)

    ! Get the tolerance threshold value for steady state issues
    call parlst_getvalue_double (rparams, 'ERROR', 'TOL', tol, 0.0001_DP)

    !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    !!!!!  Reading the mesh file + triangulation !!!!!
    !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    ! Get the path $PREDIR from the environment, where to read .prm/.tri files
    ! from. If that does not exist, write to the directory "./pre".
    if (.not. sys_getenv_string("PREDIR", spredir)) spredir = './tri'

    ! At first, read in the basic triangulation.
    call tria_readTriFile3D (rtriangulation, trim(spredir)//'/cCube3.tri')

    ! Refine it.
    call tria_quickRefine2LevelOrdering (NLMAX-1,rtriangulation)

    ! And create information about adjacencies and everything one needs from
    ! a triangulation.
    call tria_initStandardMeshFromRaw (rtriangulation)

    ! Now we can start to initialise the discretisation. At first, set up
    ! a block discretisation structure that specifies the blocks in the
    ! solution vector. In this simple problem, we only have one block.
    call spdiscr_initBlockDiscr (rdiscretisation,1,rtriangulation)

    ! rdiscretisation%Rdiscretisations is a list of scalar discretisation
    ! structures for every component of the solution vector.
    ! Initialise the first element of the list to specify the element
    ! and cubature rule for this solution component:
    call spdiscr_initDiscr_simple (rdiscretisation%RspatialDiscr(1), &
                                   EL_Q1_3D,CUB_G3_3D,rtriangulation)

    !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    !!!!!  Creating and initializing matrices !!!!!
    !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    call chemo_creatematvec ( rmassmatrix , rsysmatrix, rlaplace , rmatrixchemo ,&
                                             rchemoattract ,rrhschemo , rcell , rrhscell , rdef , rdiscretisation)

    ! We construct some matrices in advance, so the actual loop computations
    ! will be much faster
    ! This routine is defined in this file
    call chemo_initmat ( rmassmatrix, rsysmatrix, rlaplace , dtstep , D_1 , D_2 )

    !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    !!!!!  Working with initial  conditions !!!!!
    !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    ! Get some pointers  for the errorctrl
    call lsyssc_createVecByDiscr(rdiscretisation%RspatialDiscr(1),ruold,.true.)
    call lsyssc_createVecByDiscr(rdiscretisation%RspatialDiscr(1),rcold,.true.)
    call lsyssc_getbase_double(rcold,p_cold)
    call lsyssc_getbase_double(ruold,p_uold)
    !
    ! introducing vector blocks
    call lsysbl_createVecFromScalar (rcell,rcellBlock,rdiscretisation)
    call lsysbl_createVecFromScalar (rchemoattract,rchemoattractBlock,rdiscretisation)
    call lsysbl_createVecFromScalar (rdef,rdefBlock,rdiscretisation)
    ! introducing matrix blocks
    call lsysbl_createMatFromScalar (rsysmatrix,rmatrixBlockchemo,rdiscretisation)

    !!!!! initial conditions !!!!!
    ! setting the initial conditions for the two solution vectors  rcell and chemoattract.
    call chemo_initIC ( rcellBlock, rchemoattractBlock, rmassmatrix, rdiscretisation, rtriangulation )

    !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    !!!!!  Working with boundary conditions                 !!!!!
    !!!!!  1) ~~~ chemoattractant (+solver preparation) ~~~ !!!!!
    !!!!!  2) ~~~ cells                                 ~~~ !!!!!
    !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    !~~~~~ chemoattractant preparation for the time loop ~~~~~!
    !!!!! Missed the boundary conditions ??? !!!!!
    ! Create a t_discreteBC structure for the chemoattractant (where we store all discretised boundary conditions)
    call chemo_initBC ( rdiscreteBCchemo )

    !!!!! solver/filter preparations for the chemoattract !!!!!
    ! Hang the pointer into the vector and matrix. That way, these
    ! boundary conditions are always connected to that matrix and that
    ! vector.
    rmatrixBlockchemo%p_rdiscreteBC => rdiscreteBCchemo
    rrhsBlockchemo%p_rdiscreteBC => rdiscreteBCchemo
    rchemoattractBlock%p_rdiscreteBC => rdiscreteBCchemo

    ! The discrete boundary conditions are already attached to the
    ! vectors/matrix. Call the appropriate vector/matrix filter that
    ! modifies the vectors/matrix according to the boundary conditions.
    call matfil_discreteBC (rmatrixBlockchemo)
    ! Set up a filter chain that filters the defect vector
    ! during the solution process to implement discrete boundary conditions.
    RfilterChain(1)%ifilterType = FILTER_DISCBCDEFREAL
    ! Create a BiCGStab-solver. Attach the above filter chain
    ! to the solver, so that the solver automatically filters
    ! the vector during the solution process.
    p_RfilterChain => RfilterChain
    nullify(p_rpreconditioner)
    call linsol_initBiCGStab (p_rsolverNode,p_rpreconditioner,p_RfilterChain)

    ! Set the output level of the solver to 2 for some output
    p_rsolverNode%ioutputLevel = 0

    ! Attach the system matrix to the solver.
    Rmatrices = (/rmatrixBlockchemo/)
    call linsol_setMatrices(p_RsolverNode,Rmatrices)

    ! Initialise structure/data of the solver.
    call linsol_initStructure (p_rsolverNode, ierror)
    if (ierror .ne. LINSOL_ERR_NOERROR) stop
    call linsol_initData (p_rsolverNode, ierror)
    if (ierror .ne. LINSOL_ERR_NOERROR) stop
    !~~~~~ end: with chemoattractant preparation for the time loop is done ~~~~~!

    !~~~~~ cell preparation for the time loop ~~~~~!
    !!!!! Missed the boundary conditions ??? !!!!!
    ! Create a t_discreteBC structure for the chemoattractant (where we store all discretised boundary conditions)
    call chemo_initBC ( rdiscreteBC )

    rcellBlock%p_rdiscreteBC => rdiscreteBC
    rdefBlock%p_rdiscreteBC => rdiscreteBC
    !~~~~~ cell preparation for the time loop ~~~~~!

    !!!!! printing out the initial conditions into a gmv_file !!!!!
    call ucd_startGMV (rexport,UCD_FLAG_STANDARD,rtriangulation,&
            'gmvcpld/solution_init.gmv.pattern')
    call lsyssc_getbase_double (rcell,p_Ddata)
    call ucd_addVariableVertexBased (rexport,'cells',UCD_VAR_STANDARD, p_Ddata)
    call lsyssc_getbase_double (rchemoattract,p_Ddata)
    call ucd_addVariableVertexBased (rexport,'chemoattractant',UCD_VAR_STANDARD, p_Ddata)
    ! Write the file to disc, that's it.
    call ucd_write (rexport)
    call ucd_release (rexport)

    !!!!!!!!!!!!!!!!!!!!!!!!!!!!
    !!!!! now the time loop !!!!
    !!!!!!!!!!!!!!!!!!!!!!!!!!!!
    ! Start the timeloop
    timeloop : do itimestep=1,ntimesteps

      ! time stats
      call cpu_time(time_start)

      ! Next time step.
      dtime = dtime + dtstep

      ! Store the old sols for the errorctrl
      call lsyssc_getbase_double ( rcell, p_vectordata )
      call lsyssc_getbase_double ( rchemoattract, p_chemodata)
      call lalg_copyVector ( p_vectordata, p_uold )
      call lalg_copyVector ( p_chemodata, p_cold )

      ! STEP 1.1: Form the right hand side for c:
      ! It consists of M c_n +dt * ( u_{n} , phi )
      call chemo_initrhsC (rchemoattract, rrhschemo, rcell, rdiscretisation, rmassmatrix, dtstep, PHI)

      call lsysbl_createVecFromScalar (rrhschemo, rrhsBlockchemo, rdiscretisation)
      ! Next step is to implement boundary conditions into the RHS, solution and matrix.
      call vecfil_discreteBCrhs (rrhsBlockchemo)
      call vecfil_discreteBCsol (rchemoattractBlock)

      ! STEP 6: Solve the system
      !
      ! Now we have block vectors for the RHS and the matrix. What we
      ! need additionally is a block vector for the solution and temporary data.
      call lsysbl_createVecBlockIndirect (rrhsBlockchemo, rtempBlock, .false.)
      ! Finally solve the system. As we want to solve Ax=b with
      ! b being the real RHS and x being the real solution vector,
      ! we use linsol_solveAdaptively. If b is a defect
      ! RHS and x a defect update to be added to a solution vector,
      call linsol_solveAdaptively (p_rsolverNode,rchemoattractBlock,rrhsBlockchemo,rtempBlock)

     ! Store the iterationstats FINISHED HERE!!!!
     if ( p_rsolverNode%iiterations >=  iteration_c_max ) then
        iteration_c_max = p_rsolverNode%iiterations
     end if
     if ( p_rsolverNode%iiterations <= iteration_c_min ) then
        iteration_c_min = p_rsolverNode%iiterations
     end if
     iteration_c_average = iteration_c_average + p_rsolverNode%iiterations

    end do timeloop
    !!!!! end: now the time loop !!!!

    !!!!!!!!!!!!!!!!!!!!!!!!!!!!
    !!!!!  Releasing data  !!!!!
    !!!!!!!!!!!!!!!!!!!!!!!!!!!!
    !
    ! releasing old vectors
    call lsyssc_releaseVector (rcold)
    call lsyssc_releaseVector (ruold)

    ! releasing matrices and vectors
     call chemo_releasematvec ( rmassmatrix , rsysmatrix, rlaplace , rmatrixchemo ,&
                                             rchemoattract ,rrhschemo , rcell , rrhscell , rdef)

    ! Release parameterlist
     call parlst_done (rparams)

    ! releasing block vectors
    call lsysbl_releaseVector (rcellBlock)
    call lsysbl_releaseVector (rchemoattractBlock)
    call lsysbl_releaseVector (rdefBlock)
    ! releasing block matrices
    call lsysbl_releaseMatrix (rmatrixBlockchemo)

    ! Release the discretisation structure and all spatial discretisation
    ! structures in it.
    call spdiscr_releaseBlockDiscr(rdiscretisation)

    ! Release the triangulation.
    call tria_done (rtriangulation)

    ! Definitions of variables.
    print *,' 3D version responds ... '
    print *,''

  end subroutine
  !!! end of chemotaxispatternFCT !!!

  !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  !!!!!!!!!!!!!!! Auxiliary subroutines !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

    ! In this subroutine the basic matrices and vectors are initialized
    subroutine chemo_creatematvec ( rmassmatrix , rsysmatrix, rlaplace , rmatrixchemo ,&
                                                        rchemoattract , rrhschemo , rcell , rrhscell , rdef , rdiscretisation )

    ! This is where the matrices shoiuld be stored
    type(t_matrixScalar) , intent(INOUT)  ::rmassmatrix, rsysmatrix, rlaplace , rmatrixchemo

    ! This are the vectors
    type(t_vectorScalar) , intent(INOUT) :: rchemoattract, rcell, rdef ,rrhschemo , rrhscell

    ! The underlying discretisation
    type(t_blockDiscretisation) , intent (IN) :: rdiscretisation

    call bilf_createMatrixStructure (rdiscretisation%RspatialDiscr(1),&
                                    LSYSSC_MATRIX9,rmassmatrix)
    call bilf_createMatrixStructure (rdiscretisation%RspatialDiscr(1),&
                                    LSYSSC_MATRIX9,rsysmatrix)
    call bilf_createMatrixStructure (rdiscretisation%RspatialDiscr(1),&
                                    LSYSSC_MATRIX9,rlaplace)
    call bilf_createMatrixStructure (rdiscretisation%RspatialDiscr(1),&
                                    LSYSSC_MATRIX9,rmatrixchemo)

    call lsyssc_createVecByDiscr(rdiscretisation%RspatialDiscr(1),rchemoattract,.true.)
    call lsyssc_createVecByDiscr(rdiscretisation%RspatialDiscr(1), rrhschemo, .true.)
    call lsyssc_createVecByDiscr(rdiscretisation%RspatialDiscr(1),rcell,.true.)
    call lsyssc_createVecByDiscr(rdiscretisation%RspatialDiscr(1),rrhscell,.true.)
    call lsyssc_createVecByDiscr(rdiscretisation%RspatialDiscr(1),rdef,.true.)

    end subroutine

    ! Here we assemble the matrices in advance ( to save computation time )
    subroutine chemo_initmat (rmassmatrix, rsysmatrix, rlaplace , dtstep , D_1 , D_2)

    ! This is where the matrices shoiuld be stored
    type(t_matrixScalar), intent(INOUT):: rmassmatrix , rlaplace , rsysmatrix

    real(DP) , intent (INOUT) :: dtstep , D_1 , D_2
    ! local bilinearform to construct the matrices
    type(t_bilinearForm) :: rform

    rform%itermCount = 1
    rform%Idescriptors(1,1) = DER_FUNC
    rform%Idescriptors(2,1) = DER_FUNC
    rform%ballCoeffConstant = .true.
    rform%BconstantCoeff = .true.
    rform%Dcoefficients(1)  = 1.0
    call bilf_buildMatrixScalar (rform,.true.,rmassmatrix)

    rform%itermCount = 3
    rform%Idescriptors(1,1) = DER_DERIV_X
    rform%Idescriptors(2,1) = DER_DERIV_X
    rform%Idescriptors(1,2) = DER_DERIV_Y
    rform%Idescriptors(2,2) = DER_DERIV_Y
    rform%Idescriptors(1,3) = DER_DERIV3D_Z
    rform%Idescriptors(2,3) = DER_DERIV3D_Z
    rform%ballCoeffConstant = .true.
    rform%BconstantCoeff = .true.
    !rform%Dcoefficients(1)  = dtstep * D_1
    rform%Dcoefficients(1)  = 1.0
    !rform%Dcoefficients(2)  = dtstep * D_1
    rform%Dcoefficients(2)  = 1.0
    !rform%Dcoefficients(2)  = dtstep * D_1
    rform%Dcoefficients(3)  = 1.0
    call bilf_buildMatrixScalar (rform,.true.,rlaplace)

    call lsyssc_copymatrix(rmassmatrix,rsysmatrix)

    ! Since we want to save time, we use already constructed matrices
    rform%itermCount = 1
    rform%Idescriptors(1,1) = DER_FUNC
    rform%Idescriptors(2,1) = DER_FUNC
    rform%ballCoeffConstant = .true.
    rform%BconstantCoeff = .true.
    ! Robert_4_2:  rform%Dcoefficients(1)  = dtstep * 32.0_DP  ! This is the coefficient in the paper
    rform%Dcoefficients(1)  = dtstep
    call bilf_buildMatrixScalar (rform,.false.,rsysmatrix)

    ! Since we want to save time, we use already constructed matrices
    rform%itermCount = 3
    rform%Idescriptors(1,1) = DER_DERIV_X
    rform%Idescriptors(2,1) = DER_DERIV_X
    rform%Idescriptors(1,2) = DER_DERIV_Y
    rform%Idescriptors(2,2) = DER_DERIV_Y
    rform%Idescriptors(1,3) = DER_DERIV3D_Z
    rform%Idescriptors(2,3) = DER_DERIV3D_Z
    rform%ballCoeffConstant = .true.
    rform%BconstantCoeff = .true.
    rform%Dcoefficients(1)  = dtstep * D_2
    rform%Dcoefficients(2)  = dtstep * D_2
    rform%Dcoefficients(3)  = dtstep * D_2
    call bilf_buildMatrixScalar (rform,.false.,rsysmatrix)

    end subroutine

    !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    !!!!! Initial conditions for cells and chemoattractant !!!!!

    ! This is called to implement the initial conditions, e.g. to set the initial sol vectors
    subroutine chemo_initIC ( rcellBlock, rchemoattractBlock, rmassmatrix, rdiscretisation, rtriangulation )

    ! sol vectors
    type(t_vectorBlock), intent (INOUT) :: rcellBlock, rchemoattractBlock

    ! mass matrix
    type(t_matrixScalar), intent (IN) :: rmassmatrix

    ! Underlying discretisation
    type(t_blockDiscretisation) , intent (IN) :: rdiscretisation
    ! Underlying triangulation to derive vector-coords
    type(t_triangulation) , intent (IN) :: rtriangulation

    ! initial solution vector
    type(t_vectorScalar) :: rinitSolVector
    type(t_vectorBlock), target :: rinitSolVectorBlock

    ! A local collection
    type(t_collection) :: rcollection

    ! Some pointers
    real(DP) , dimension(:,:) , pointer :: p_DvertexCoords
    real(DP), dimension(:) , pointer :: p_vectordata

    ! integer loop
    integer :: i

    !AS, remarks: the simplified version of the initial boundary conditions (no L2PROJ and INITSOL are used)
        ! Now we're setting up our collection
        call collct_init (rcollection)

        !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
        !!!!! setting the initial conditions for cells !!!!!
        !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
        call lsyssc_createVecByDiscr(rdiscretisation%RspatialDiscr(1), rinitSolVector,.true.)
        !
        ! user defined subroutine for initial conditions
        call lsyssc_getbase_double(rinitSolVector, p_vectordata)
        ! get coordinates
        call storage_getbase_double2D(rtriangulation%h_DvertexCoords,p_DvertexCoords)
        ! prescribe initial conditions
        do i=1,rtriangulation%NVT
           p_vectordata(i) = userPresc_cellsInitCond(p_DvertexCoords(1,i),p_DvertexCoords(2,i),p_DvertexCoords(3,i))
        end do

        call lsyssc_copyVector (rinitSolVector,rchemoattractBlock%RvectorBlock(1))

        !!!!! perform projection !!!!!
        ! create initSolVectorBlock (block structure)
        !>>>call lsysbl_createVecFromScalar (rinitSolVector,rinitSolVectorBlock,rdiscretisation)
        ! we put the initial_condition vector to the collection (needed in fcoeff_solCells)
        !>>>rcollection%p_rvectorQuickAccess1 => rinitSolVectorBlock
        ! setting the initial conditions for cells
        !>>>rcollection%IquickAccess(1) = 1
        !>>>call anprj_analytL2projectionByMass (rcellBlock%RvectorBlock(1), rmassmatrix, &
        !>>>                                     fcoeff_solCells, rcollection)

        !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
        !!!!! setting the initial conditions for chemoattractant !!!!!
        !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
        !call lsyssc_clearVector(rvector)

        !release rinitSolVector and rinitSolVectorBlock
        call lsyssc_releaseVector (rinitSolVector)
        call lsysbl_releaseVector (rinitSolVectorBlock)

        ! Releasing the collection
        call collct_done (rcollection)

    end subroutine

   !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
   !!!!! Construction of the RHS for the chemoattractant !!!!!
   !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    subroutine chemo_initrhsC (rchemoattract, rhschemo, rcell, &
                                                 rdiscretisation, rmassmatrix, dtstep,  PHI)

    type(t_vectorScalar) , intent(IN) :: rchemoattract , rcell

    type(t_matrixScalar) , intent(IN) :: rmassmatrix

    type(t_vectorScalar) , intent(INOUT) :: rhschemo

    type(t_Blockdiscretisation) , intent(IN) :: rdiscretisation

    ! param
    real(DP) ,intent (IN) :: dtstep, PHI

    ! local linform
    type(t_linearForm) :: rlinform

    ! local collection
    type(t_collection) :: rcollection

    call collct_init (rcollection)
    call collct_setvalue_vecsca (rcollection, 'cbvector', rcell, .true.)
    rcollection%DquickAccess(1) = PHI
    ! To assemble the RHS , set up the corresponding linear  form (u*g(u),Phi_j):
    rlinform%itermCount = 1
    rlinform%Idescriptors(1) = DER_FUNC
    call linf_buildVectorScalar (rdiscretisation%RspatialDiscr(1), &
                                rlinform, .true., rhschemo, coeff_hillenX_RHS_c, rcollection)
    ! remark: subroutine coeff_hillenX_RHS_c sets M u^n into rhschemo

    ! Since we approx. the 1st deriv. by the backward euler, there's still one component
    !missing in the RHS:
    ! rmassmatrix* c_{old}
    ! The corressponding massmatrix is already built ( just before the loop starts )
    call lsyssc_scalarMatVec(rmassmatrix,rchemoattract,rhschemo,1.0_DP,dtstep)
    ! rhschemo=1*rmassmatrix*rchemoattractant + dstep*rhschemo

    ! Release the collection structure
    call collct_done(rcollection)

    end subroutine

    ! In this subroutine the basic matrices and vectors are initialized
    subroutine chemo_releasematvec ( rmassmatrix , rsysmatrix, rlaplace , rmatrixchemo ,&
                                                        rchemoattract , rrhschemo , rcell , rrhscell , rdef)

    ! This is where the matrices shoiuld be stored
    type(t_matrixScalar) , intent(INOUT)  ::rmassmatrix, rsysmatrix, rlaplace , rmatrixchemo

    ! This are the vectors
    type(t_vectorScalar) , intent(INOUT) :: rchemoattract, rcell, rdef ,rrhschemo , rrhscell

    ! Release the preallocated matrix and the solution vector.
    call lsyssc_releaseMatrix (rmassmatrix)
    call lsyssc_releaseMatrix (rsysmatrix)
    call lsyssc_releaseMatrix (rlaplace)
    call lsyssc_releaseMatrix (rmatrixchemo)

    call lsyssc_releaseVector (rchemoattract)
    call lsyssc_releaseVector (rrhschemo)
    call lsyssc_releaseVector (rcell)
    call lsyssc_releaseVector (rrhscell)
    call lsyssc_releaseVector (rdef)

    end subroutine

    ! Initialize the boundary conditions (used for both, the cell density sol and the chemoattractant sol)
    subroutine chemo_initBC (rdiscreteBC)

    ! The BC_type which should be initialized
    type(t_discreteBC) , intent (INOUT) ::rdiscreteBC

    call bcasm_initDiscreteBC(rdiscreteBC)

    end subroutine

  !<subroutine>
  ! initial condition for cells
  !<subroutine/>
    subroutine fcoeff_solCells (rdiscretisation, rform, &
                  nelements, npointsPerElement, Dpoints, &
                  IdofsTest, rdomainIntSubset, &
                  Dcoefficients, rcollection)

    use fsystem
    use basicgeometry
    use triangulation
    use scalarpde
    use domainintegration
    use spatialdiscretisation
    use collection

  !<description>
    ! Called when the initial solution has to be projected into another FEM
    ! space. Evaluated the current initial solution in cubature points.
  !</description>

  !<input>
    ! The discretisation structure that defines the basic shape of the
    ! triangulation with references to the underlying triangulation,
    ! analytic boundary boundary description etc.
    type(t_spatialDiscretisation), intent(IN) :: rdiscretisation

    ! The linear form which is currently to be evaluated:
    type(t_linearForm), intent(IN) :: rform

    ! Number of elements, where the coefficients must be computed.
    integer, intent(IN) :: nelements

    ! Number of points per element, where the coefficients must be computed
    integer, intent(IN) :: npointsPerElement

    ! This is an array of all points on all the elements where coefficients
    ! are needed.
    ! Remark: This usually coincides with rdomainSubset%p_DcubPtsReal.
    ! DIMENSION(dimension,npointsPerElement,nelements)
    real(DP), dimension(:,:,:), intent(IN) :: Dpoints

    ! An array accepting the DOF`s on all elements test in the test space.
    ! DIMENSION(\#local DOF`s in test space,Number of elements)
    integer, dimension(:,:), intent(IN) :: IdofsTest

    ! This is a t_domainIntSubset structure specifying more detailed information
    ! about the element set that is currently being integrated.
    ! It is usually used in more complex situations (e.g. nonlinear matrices).
    type(t_domainIntSubset), intent(IN) :: rdomainIntSubset

    ! Optional: A collection structure to provide additional
    ! information to the coefficient routine.
    type(t_collection), intent(INOUT), optional :: rcollection

  !</input>

  !<output>
    ! A list of all coefficients in front of all terms in the linear form -
    ! for all given points on all given elements.
    !   DIMENSION(itermCount,npointsPerElement,nelements)
    ! with itermCount the number of terms in the linear form.
    real(DP), dimension(:,:,:), intent(OUT) :: Dcoefficients
  !</output>

  !</subroutine>

    integer :: icomponent

    ! loop-indices
    integer :: icub, iel

    !DO iel = 1, nelements
    !  DO icub = 1, npointsPerElement
    !     Dcoefficients( 1, icub, iel ) = userPresc_cellsInitCond(Dpoints ( 1, icub, iel ), &
    !                                                       Dpoints ( 2, icub, iel ), &
    !                                                       Dpoints ( 3, icub, iel ))
    !    END DO
    !END DO

    ! Evaluate copmponent icomponent
    icomponent = rcollection%IquickAccess(1)

    call fevl_evaluate_sim (&
        rcollection%p_rvectorQuickAccess1%RvectorBlock(icomponent), &
        rdomainIntSubset, DER_FUNC, Dcoefficients, 1)

    end subroutine

    !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    !!!!! User prescribed function for setting initial conditions for cells !!!!!
    !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
	function userPresc_cellsInitCond(x,y,z) result (func_result)
	    !
	    ! coordinates
	    real(DP) :: x, y, z
	    !
	    ! function value
		real(DP) :: func_result

        ! part of a user code: prescribe initial conditions for cells
        if( ((x-8)*(x-8) + (y-8)*(y-8) + (z-8)*(z-8)) < 20.25 ) then
            func_result = 100_DP
        else
            func_result = 0_DP
        endif
	end function userPresc_cellsInitCond

end module


MODULE NESDIS_SEAICE_PHYEM_MODULE

  ! -----------------
  ! Environment setup
  ! -----------------
  ! Module use
  USE Type_Kinds, ONLY: fp
  ! Disable implicit typing
  IMPLICIT NONE

  ! ------------
  ! Visibilities
  ! ------------
  PRIVATE
  PUBLIC :: NESDIS_SIce_Phy_EM

  ! -----------------
  ! Module parameters
  ! -----------------

CONTAINS

  subroutine NESDIS_SIce_Phy_EM(Frequency,                                                 &  ! INPUT
                                Angle,                                                     &  ! INPUT
                                Ts_ice,                                                    &  ! INPUT
                                Salinity,                                                  &  ! INPUT
                                Emissivity_H,                                              &  ! OUTPUT
                                Emissivity_V)                                                 ! OUPTUT

!-------------------------------------------------------------------------------------------------------------
!
! NAME:
!       NESDIS_SIce_Phy_EM
!
! PURPOSE:
!       Subroutine to simulate microwave sea ice emissivity
!
! REFERENCES:
!
! Ulaby, F.T., R. K., Moore, and A. K., Fung, Microwave remote sensing: active and passive, III,
! from theory to Applications, 2020-2025, Artech House Publishers, 1990.
!
! CATEGORY:
!       CRTM : Surface : MW SEA ICE EM
!
! LANGUAGE:
!       Fortran-95
!
! CALLING SEQUENCE:
!       CALL NESDIS_SIce_Phy_EM
!
! INPUT ARGUMENTS:
!
!         Frequency                Frequency User defines
!                                  This is the "I" dimension
!                                  UNITS:      GHz
!                                  TYPE:       REAL( fp )
!                                  DIMENSION:  Scalar
!
!
!         Angle                    The angle values in degree
!                                  ** NOTE: THIS IS A MANDATORY MEMBER **
!                                  **       OF THIS STRUCTURE          **
!                                  UNITS:      Degrees
!                                  TYPE:       REAL( fp )
!                                  DIMENSION:  Rank-1, (I)
!
!         Ts_ice                   Sea ice temperature
!                                  UNITS:      Kelvin, K
!                                  TYPE:       REAL( fp )
!                                  DIMENSION:  Scalar
!
!        Salinity                  Sea water salinity (1/thousand)
!                                  UNITS:      N/A
!                                  TYPE:       REAL( fp )
!                                  DIMENSION:  Scalar
!
!
!  INTERNAL ARGUMENTS:
!
!         theta                    viewing zenith angle
!                                  UNITS:  radian
!                                  TYPE:       REAL( fp )
!                                  DIMENSION:  Scalar
!
!
! OUTPUT ARGUMENTS:
!
!         Emissivity_H:            The surface emissivity at a horizontal polarization.
!                                  ** NOTE: THIS IS A MANDATORY MEMBER **
!                                  **       OF THIS STRUCTURE          **
!                                  UNITS:      N/A
!                                  TYPE:       REAL( fp )
!                                  DIMENSION:  Scalar
!
!         Emissivity_V:            The surface emissivity at a vertical polarization.
!                                  ** NOTE: THIS IS A MANDATORY MEMBER **
!                                  **       OF THIS STRUCTURE          **
!                                  UNITS:      N/A
!                                  TYPE:       REAL( fp )
!                                  DIMENSION:  Scalar
! CALLS:
!
!       permitivity    : Function to calculate the dielectric constant of saline water
!
!reflection_coefficient: Function to calculate the surface reflection coefficient using Fresnel equations
!
! RESTRICTIONS:
!       None.
!
!
! CREATION HISTORY:
!       Written by:     Banghua Yan, QSS Group Inc., Banghua.Yan@noaa.gov (28-May-2005)
!
!
!       and             Fuzhong Weng, NOAA/NESDIS/ORA, Fuzhong.Weng@noaa.gov
!
!  Copyright (C) 2005 Fuzhong Weng and Banghua Yan
!
!
!------------------------------------------------------------------------------------------------------------

     real(fp) ::  Angle,theta, Ts_ice, Salinity
     real(fp) ::  Frequency, Emissivity_H, Emissivity_V, slam, f1,epsx, epsy, pi
     integer :: ITYPE
     COMPLEX(fp) :: eair, eice
     COMPLEX(fp) :: cos_theta_i,cos_theta_t,sin_theta_i,sin_theta_t
     COMPLEX(fp) :: rv_ice, rh_ice

      pi = acos(-1.0_fp)

      theta = Angle*pi/180.0_fp

      slam = 300.0_fp/Frequency

      f1=Frequency*1.0e+9_fp

      eair = cmplx(1.0_fp, 0.0_fp, fp)

      ITYPE = 2

      call  permitivity(ITYPE,Ts_ice, Salinity, f1, epsx, epsy)

      eice = cmplx(epsx, -epsy, fp)

      sin_theta_i = sin(cmplx(theta,0.0_fp, fp))

      cos_theta_i = sqrt(1.0_fp - sin_theta_i*sin_theta_i)

      sin_theta_t = sin_theta_i*sqrt(eair)/sqrt(eice)

      cos_theta_t = sqrt(1.0_fp - sin_theta_t*sin_theta_t)

      call reflection_coefficient(eair, eice, cos_theta_i, cos_theta_t, rv_ice, rh_ice)

      Emissivity_V = 1.0_fp  - abs(rv_ice)*abs(rv_ice)

      Emissivity_H = 1.0_fp  - abs(rh_ice)*abs(rh_ice)

     return

     end subroutine NESDIS_SIce_Phy_EM

     subroutine permitivity(itype, temp, s, f, e_real, e_img)

     real(fp) ::  eo, esw, eswo, a, tswo, tsw, b, sswo, c,d, fi, ssw
     real(fp) ::  temp,t, t2, t3, s2, s3, epsp, epspp
     real(fp) ::  ESWI, EW0, TPTW, FH, X, Y, TPTWFQ
     real(fp) ::  e_real, e_img

     real(fp) ::  s,t4,t5,sigma
     real(fp) ::  ebo,eb,sb,nb,eb_real,eb_imag
     real(fp) ::   den_ice,den_sice,den_brine

     real(fp) ::   vss, cc, den_ss

     real(fp) ::   f,pi,vb,sigmao,vi

     integer :: itype

     COMPLEX(fp) :: ebrine,eice,eair,esice,xx

     ! Silence gfortran complaints about maybe-used-uninit by init to HUGE()
     sb = HUGE(sb)
     pi=acos(-1.0_fp)
     ESWI=4.9_fp
     t=temp-273.15_fp
     if (t.le.-65.0_fp) t = -65.0_fp
     t2=t*t
     t3=t2*t
     t4=t3*t
     t5=t4*t
     FH=f

   GET_DielectricC: SELECT CASE (ITYPE)

   CASE (1)

      s2 = s*s
      s3 = s**3
      eo   = 8.854e-12
      eswo = 87.134 - 1.949e-1 * t - 1.276e-2 * t2 + 2.491e-4 * t3
      a =1.0 +1.613e-5 * t*s - 3.656e-3*s + 3.210e-5*s2 - 4.232e-7*s3
      esw = eswo * a
      tswo = 1.1109e-10 - 3.824e-12 * t + 6.938e-14*t2 -5.096e-16*t3
      b = 1.0 + 2.282e-5*t*s -7.638e-4*s - 7.760e-6 *s2 + 1.105e-8*s3
      tsw = tswo*b
      epsp = ESWI + (esw - ESWI)/ (1.0 + (f*tsw)**2)
      sswo = s*(0.18252-1.4619e-3*s+2.093e-5*s2-1.282e-7*s3)
      d  = 25.0 - t
      fi = d * (2.033e-2 + 1.266e-4 * d + 2.464e-6 * d * d -   &
        s * (1.849e-5 - 2.551e-7 * d + 2.551e-8 * d * d))

      ssw = sswo * exp(-fi)
      epspp = tsw * f * (esw - ESWI) / (1.0 + (tsw * f)**2)
      epspp = epspp + ssw/(2.0 * pi * eo * f)

      e_real =   epsp
      e_img = - epspp

Case (2) ! sea ice

      eice = cmplx(3.15_fp, -0.001_fp,fp)
      eair = cmplx(1.0_fp, 0.0_fp,fp)

      if (t .le. -8.2) then
          vb = 0.001*s*(-43.795/t + 1.189)
          if (t .lt. -22.9) vb = 0.001*s*(-43.795/(-22.9) + 1.189)
      else
         if (t .le. -2.06) then
             vb = 0.001*s*(-45.917/t + 0.930)
        else
            vb = 0.001*s*(-52.56/t  - 2.28)
          if (t .gt. -0.5)  vb = 0.001*s*(-52.56/(-0.5)  - 2.28)
        endif
      endif

      if (t .le. -36.8) then
         sb = 508.18 + 14.535*t + 0.2018*t2
        if (t .lt. -43.2)sb = 508.18 + 14.535*(-43.2) + 0.2018*(-43.2)*(-43.2)
      endif
      if ((t .gt. -36.8) .and. (t .le. -22.9))   &
                        sb = 242.94 + 1.5299*t + 0.0429*t2
      if ((t .gt. -22.9) .and. (t .le. 8.2))       &
                        sb = 57.041 - 9.929*t - 0.16204*t2 - 0.002396*t3
      if(t .ge. -8.2) then
         sb = 1.725 - 18.756*t - 0.3964*t2
            if(t .gt. -2.0) sb = 1.725 - 18.756*(-2.0) - 0.3964*(-2.0)*(-2.0)
      endif

      nb  = sb*(1.707*0.01 + 1.205*1.0E-5*sb+4.058E-9*sb*sb)
      d  = 25.0 - t
      a = 1.0 - 0.255*nb + 5.15*0.01*nb*nb-6.89*1.0e-3*nb*nb*nb
      b = 1.0 + 0.146*0.01*t*nb-4.89*0.01*nb                  &
          -2.97*0.01*nb*nb +  5.64 *1.0e-3*nb*nb*nb
      c = 1.0 - 1.96*0.01*d + 8.08*1.0e-5*d*d                 &
          - nb*d*(3.02*1.0e-5 + 3.92*1.0e-5*d+nb*(1.72*1.0e-5 - 6.58*1.0e-6*d ))

      ebo = 88.045-0.4147*t+6.295E-4*t2+1.075E-5*t3

      tswo = 1.1109E-10-3.824E-12*t+6.938E-14*t2-5.096E-16*t3
      sigmao = nb*(10.39 - 2.378*nb+0.683*nb*nb-0.135*nb*nb*nb+1.01*0.01*nb**4.0)
      eb = ebo*a
      tsw = tswo*b
      sigma = sigmao*c
      eo   = 8.854e-12   ! the permittivity of free space (p. 2024)
      eb_real = ESWI + (eb - ESWI)/ (1.0 + (f*tsw)**2.)
      epspp = tsw * f * (eb - ESWI) / (1.0 + (tsw * f)**2.)
      eb_imag = epspp + sigma/(2.0 * pi * eo * f)
      ebrine=cmplx(eb_real,-eb_imag,fp)

    den_sice = 0.87
    if(t .lt. -18.0) den_sice = 0.77
    den_ice   = 0.917 - 1.403e-4*t
    den_brine = 1.0 + 0.0008*sb

    vi = (den_sice - vb*den_brine)/den_ice
    if ((vi+vb) .gt. 1.0) vi = 1.0 - vb
    if (vi .lt. 0.0) vi = 0.0

    xx = vb*sqrt(ebrine)+vi*sqrt(eice)+(1.0-vi-vb)*sqrt(eair)
    esice = xx*xx
    e_real = real(esice,fp)
    e_img =  -aimag(esice)
    cc = 0.387
    den_ss = 1.5
    vss =cc * den_brine*vb/den_ss

CASE Default

    EW0=88.045-0.4147*t+6.295E-4*t2+1.075E-5*t3

    TPTW=1.1109E-10-3.824E-12*t+6.938E-14*t2-5.096E-16*t3

    X=TPTW*FH

    TPTWFQ=1+X*X

    Y=(EW0-ESWI)/TPTWFQ

    e_real=ESWI+Y

    e_img=-X*Y

END SELECT GET_DielectricC

return

end subroutine permitivity

     SUBROUTINE reflection_coefficient(em1, em2, cos_theta_i, cos_theta_t, rv, rh)

    COMPLEX(fp) :: rh, rv,cos_theta_i,cos_theta_t

    COMPLEX(fp) :: em1, em2, m1, m2

    m1 = sqrt(em1)

    m2 = sqrt(em2)

    rv = (m1*cos_theta_t- m2*cos_theta_i)/(m1*cos_theta_t + m2*cos_theta_i)

    rh = (m1*cos_theta_i- m2*cos_theta_t)/(m1*cos_theta_i + m2*cos_theta_t)

    return

    end subroutine reflection_coefficient

END MODULE NESDIS_SEAICE_PHYEM_MODULE


Program CalcPressZ

  Implicit None
  Real, Parameter :: PI =4*atan(1.0)
  Real :: Dens
  Real :: a, b, adev
  Real :: phs, pA, p
  Real :: trash
  Integer :: State

  Open(14, file="PrssZwanzig.dat", action="write")
  Open(15, file="b.dat", action="read", status="old")

  Read :Do
     Read(15,*,iostat = state) dens, trash , b

     a = 1.99 + 3.35*dens - 0.21*dens*dens
     adev = 3.35 - 0.42*dens
     !b= (2*Pi)/3 * ghs1

     phs = dens * ( 1 + dens* b  )
     pA = dens*dens*( a + dens* adev  )

     P = phs - pA
     Write(*,*) dens, p
     Write(14,*) dens , P
     If ( state .LT. 0 ) Exit
  End Do Read

End Program CalcPressZ


c      *************************************************************
c      * This package provides a general interpolaton routine      *
c      *************************************************************

c     The main interface routines are:
c         get_index3,4 : get the grid indices for interpolation
c         int_index3,4 : interpolate to the grid position

c     -------------------------------------------------------------
c     Get index in grid space for interpolation
c     -------------------------------------------------------------

      subroutine get_index4 (rid,rjd,rkd,xpo,ypo,ppo,rtp,
     >                       vert0,vert1,surf0,surf1,mode,
     >                       nx,ny,nz,lonw,lats,dlon,dlat,misdat)

c     Purpose:
c        This subroutine determines the indices (rid,rjd,rkd) in grid
c        space for a point in physical space (xpo,ypo,ppo). The
c        horizontal grid is specified by the south-west point (lats,lonw)
c        and the grid spacing (dlat,dlon). The vertical grid is given
c        by <vert(n1,n2,n3)>. The lower boundary (typicall surface
c        pressure) is given by <surf(n1,n2)>.
c     Arguments:
c        rid,rjd,rkd  real  output   grid location to be interpolated to
c        xpo,ypo,ppo  real  input    physical coordinates
c        rtp          real  input    relative time position (0=beginning, 1=end)
c        n1,n2,n3     int   input    grid dimensions in x-, y- and p-direction
c        lats,lonw    real  input    south and west boundary of grid space
c        vert         real  input    vertical coordinate grid
c        surf         real  input    lower boundary (surface pressure)
c        mode         int   input    direction of vertical axis (p=1,th=-1)
c                                        1: linear, 1 -> nz (th)
c                                        2: linear, nz -> 1 (pv)
c                                        3: binary (p)

      implicit none

c     Declartion of function parameters
      integer   nx,ny,nz
      real      xpo,ypo,ppo,rtp
      real      vert0(nx*ny*nz),vert1(nx*ny*nz)
      real      surf0(nx*ny)   ,surf1(nx*ny*nz)
      real      rid,rjd,rkd
      real      dlat,dlon,lats,lonw
      real      misdat
      integer   mode

c     Set numerical parameters
      real      eps
      parameter (eps=1.e-8)

c     Auxiliary variables
      real      rid0,rjd0,rkd0,rid1,rjd1,rkd1
      integer   i

c     Externals
      real      int_time
      external  int_time

c     Get the inidices
      if (abs(rtp).lt.eps) then
         call  get_index3 (rid,rjd,rkd,xpo,ypo,ppo,mode,
     >                     vert0,surf0,nx,ny,nz,lonw,lats,dlon,dlat)
      elseif (abs(rtp-1.).lt.eps) then
         call  get_index3 (rid,rjd,rkd,xpo,ypo,ppo,mode,
     >                     vert1,surf1,nx,ny,nz,lonw,lats,dlon,dlat)
      else
         call  get_index3 (rid0,rjd0,rkd0,xpo,ypo,ppo,mode,
     >                     vert0,surf0,nx,ny,nz,lonw,lats,dlon,dlat)
         call  get_index3 (rid1,rjd1,rkd1,xpo,ypo,ppo,mode,
     >                     vert1,surf1,nx,ny,nz,lonw,lats,dlon,dlat)
         rid = int_time (rid0,rid1,rtp,misdat)
         rjd = int_time (rjd0,rjd1,rtp,misdat)
         rkd = int_time (rkd0,rkd1,rtp,misdat)

      endif

      end

c     -------------------------------------------------------------
c     Interpolate to an arbitrary position in grid space and time
c     -------------------------------------------------------------

      real function int_index4 (ar0,ar1,n1,n2,n3,rid,rjd,rkd,rtp,misdat)

c     Purpose:
c        This subroutine interpolates a 3d-array to an arbitrary
c        location within the grid including a linear time-interpolation.
c     Arguments:
c        rid,rjd,rkd  real  output   grid location to be interpolated to
c        xpo,ypo,ppo  real  input    physical coordinates
c        n1,n2,n3     int   input    grid dimensions in x-, y- and p-direction
c        lats,lonw    real  input    south and west boundary of grid space
c        vert         real  input    vertical coordinate grid
c        surf         real  input    lower boundary (surface pressure)

      implicit none

c     Declartion of function parameters
      integer   n1,n2,n3
      real      ar0(n1*n2*n3),ar1(n1*n2*n3)
      real      rid,rjd,rkd
      real      rtp
      real      misdat

c     Set numerical parameters
      real      eps
      parameter (eps=1.e-8)

c     Externals
      real      int_index3,int_time
      external  int_index3,int_time

c     Auxiliary variables
      real      val0,val1,val

c     Do the 3d-interpolation
      if (abs(rtp).lt.eps) then
         val = int_index3 (ar0,n1,n2,n3,rid,rjd,rkd,misdat)
      elseif (abs(rtp-1.).lt.eps) then
         val = int_index3 (ar1,n1,n2,n3,rid,rjd,rkd,misdat)
      else
         val0 = int_index3 (ar0,n1,n2,n3,rid,rjd,rkd,misdat)
         val1 = int_index3 (ar1,n1,n2,n3,rid,rjd,rkd,misdat)
         val  = int_time (val0,val1,rtp,misdat)
      endif

c     Return value
      int_index4 = val

      return
      end

c     -------------------------------------------------------------
c     Interpolate to an arbitrary position in grid space
c     -------------------------------------------------------------

      real function int_index3 (ar,n1,n2,n3,rid,rjd,rkd,misdat)

c     Purpose:
c        This subroutine interpolates a 3d-array to an arbitrary
c        location within the grid. The interpolation includes the
c        testing of the missing data flag 'misdat'. If one dimension
c        is 1, a 2d-interpolation is performed; if two dimensions
c        are 1, it is a 1d-interpolation; if all three dimensions are
c        1, no interpolation is performed and the input value is
c        returned.
c     Arguments:
c        ar        real  input   input data array
c        n1,n2,n3  int   input   dimensions of ar
c        ri,rj,rk  real  input   grid location to be interpolated to
c        misdat    real  input   missing data flag (on if misdat<>0)

      implicit none

c     Declartion of function parameters
      integer   n1,n2,n3
      real      ar(n1*n2*n3)
      real      rid,rjd,rkd
      real      misdat

c     Set numerical parameters
      real      eps
      parameter (eps=1.e-8)

c     Local variables
      integer   i,j,k,ip1,jp1,kp1
      real      frac0i,frac0j,frac0k,frac1i,frac1j,frac1k
      real      ri,rj,rk
      real      val000,val001,val010,val011,val100,val101,val110,val111
      real      frc000,frc001,frc010,frc011,frc100,frc101,frc110,frc111
      real      frc
      real      mdv
      real      val

c     Elementary test for dimensions
      if ( (n1.lt.1).or.(n2.lt.1).or.(n3.lt.1) ) then
         print*,'Invalid grid dimensions ',n1,n2,n3
         stop
      endif

c     Activate or inactive the missing data check (quick and dirty)
      if (misdat.ne.0.) then
         mdv = misdat
      else
         mdv = 257.22725394015
      endif

c     Bring the indices into the grid space
      ri = amax1(1.,amin1(float(n1),rid))
      rj = amax1(1.,amin1(float(n2),rjd))
      rk = amax1(1.,amin1(float(n3),rkd))

c     Get the index of the west-south-bottom corner of the box
      i   = min0(int(ri),n1-1)
      ip1 = i+1
      j   = min0(int(rj),n2-1)
      jp1 = j+1
      k   = min0(int(rk),n3-1)
      kp1 = k+1

c     Special handling for 2d arrays
      if (n3.eq.1) then
         k=1
         kp1=1
      endif

c     Get location relative to grid box
      if ( i.ne.ip1 ) then
         frac0i = ri-float(i)
         frac1i = 1.-frac0i
      else
         frac0i = 0.
         frac1i = 1.
      endif
      if ( j.ne.jp1 ) then
         frac0j = rj-float(j)
         frac1j = 1.-frac0j
      else
         frac0j = 0.
         frac1j = 1.
      endif
      if ( k.ne.kp1 ) then
         frac0k = rk-float(k)
         frac1k = 1.-frac0k
      else
         frac0k = 0.
         frac1k = 1.
      endif

c     On a grid point - take the grid point value
      if ( ( abs(frac0i).lt.eps ).and.
     >     ( abs(frac0j).lt.eps ).and.
     >     ( abs(frac0k).lt.eps ) ) then

         val = ar( i + n1*(j -1) + n1*n2*(k -1) )
         goto 100

      endif

c     Init the fractions
      frc000 = frac1i * frac1j * frac1k
      frc001 = frac0i * frac1j * frac1k
      frc010 = frac1i * frac0j * frac1k
      frc011 = frac0i * frac0j * frac1k
      frc100 = frac1i * frac1j * frac0k
      frc101 = frac0i * frac1j * frac0k
      frc110 = frac1i * frac0j * frac0k
      frc111 = frac0i * frac0j * frac0k

c     Init the values
      val000 = ar( i   + n1*(j  -1) + n1*n2*(k  -1) )
      val001 = ar( ip1 + n1*(j  -1) + n1*n2*(k  -1) )
      val010 = ar( i   + n1*(jp1-1) + n1*n2*(k  -1) )
      val011 = ar( ip1 + n1*(jp1-1) + n1*n2*(k  -1) )
      val100 = ar( i   + n1*(j  -1) + n1*n2*(kp1-1) )
      val101 = ar( ip1 + n1*(j  -1) + n1*n2*(kp1-1) )
      val110 = ar( i   + n1*(jp1-1) + n1*n2*(kp1-1) )
      val111 = ar( ip1 + n1*(jp1-1) + n1*n2*(kp1-1) )

c     Handle missing data
      if ( abs(val000-mdv).lt.eps ) frc000 = 0.
      if ( abs(val001-mdv).lt.eps ) frc001 = 0.
      if ( abs(val010-mdv).lt.eps ) frc010 = 0.
      if ( abs(val011-mdv).lt.eps ) frc011 = 0.
      if ( abs(val100-mdv).lt.eps ) frc100 = 0.
      if ( abs(val101-mdv).lt.eps ) frc101 = 0.
      if ( abs(val110-mdv).lt.eps ) frc110 = 0.
      if ( abs(val111-mdv).lt.eps ) frc111 = 0.

c     Build the final value
      frc = frc000 + frc001 + frc010 + frc011 +
     >      frc100 + frc101 + frc110 + frc111
      if ( frc.gt.0. ) then
         val = 1./frc * ( frc000 * val000 + frc001 * val001 +
     >                    frc010 * val010 + frc011 * val011 +
     >                    frc100 * val100 + frc101 * val101 +
     >                    frc110 * val110 + frc111 * val111 )
      else
         val = misdat
      endif

c     Return the value
 100  continue

      int_index3 = val

      end

c     -------------------------------------------------------------
c     Time interpolation
c     -------------------------------------------------------------

      real function int_time (val0,val1,reltpos,misdat)

c     Purpose:
c        This subroutine interpolates linearly in time between two
c        values.
c     Arguments:
c        val0      real  input   value at time 0
c        val1      real  input   value at time 1
c        reltpos   real  input   relative time (between 0 and 1)
c        misdat    real  input   missing data flag (on if misdat<>0)

      implicit none

c     Declaration of parameters
      real      val0
      real      val1
      real      reltpos
      real      misdat

c     Numerical epsilon
      real      eps
      parameter (eps=1.e-8)

c     Local variables
      real      val
      real      mdv

c     Activate or inactive the missing data check (quick and dirty)
      if (misdat.ne.0.) then
         mdv = misdat
      else
         mdv = 257.22725394015
      endif

c     Do the linear interpolation
      if ( abs(reltpos).lt.eps ) then
         val = val0
      elseif ( abs(reltpos-1.).lt.eps ) then
         val = val1
      elseif ( (abs(val0-mdv).gt.eps).and.
     >         (abs(val1-mdv).gt.eps) ) then
         val = (1.-reltpos)*val0+reltpos*val1
      else
         val = mdv
      endif

c     Return value
      int_time = val

      end

c     -------------------------------------------------------------
c     Get the position of a physical point in grid space
c     -------------------------------------------------------------

      subroutine get_index3 (rid,rjd,rkd,xpo,ypo,ppo,mode,
     >                       vert,surf,nx,ny,nz,lonw,lats,dlon,dlat)

c     Purpose:
c        This subroutine determines the indices (rid,rjd,rkd) in grid
c        space for a point in physical space (xpo,ypo,ppo). The
c        horizontal grid is specified by the south-west point (lats,lonw)
c        and the grid spacing (dlat,dlon). The vertical grid is given
c        by <vert(n1,n2,n3)>. The lower boundary (typicall surface
c        pressure) is given by <surf(n1,n2)>.
c     Arguments:
c        rid,rjd,rkd  real  output   grid location to be interpolated to
c        xpo,ypo,ppo  real  input    physical coordinates
c        n1,n2,n3     int   input    grid dimensions in x-, y- and p-direction
c        lats,lonw    real  input    south and west boundary of grid space
c        vert         real  input    vertical coordinate grid
c        surf         real  input    lower boundary (surface pressure)
c        mode         int   input    direction of vertical axis
c                                        1: linear, 1 -> nz (th)
c                                        2: linear, nz -> 1 (pv)
c                                        3: binary (p)

      implicit none

c     Declartion of function parameters
      integer   nx,ny,nz
      real      vert(nx*ny*nz)
      real      surf(nx*ny)
      real      rid,rjd,rkd
      real      xpo,ypo,ppo
      real      dlat,dlon,lats,lonw
      integer   mode

c     Numerical epsilon
      real      eps
      parameter (eps=1.e-8)

c     Local variables
      integer   i,j,k
      real      ppo0,ppo1,ppom,psur
      integer   i0,im,i1

c     Externals
      real      int_index3
      external  int_index3

c     Get the horizontal grid indices
      rid=(xpo-lonw)/dlon+1.
      rjd=(ypo-lats)/dlat+1.

c     Two-dimensional interpolation on horizontal plane: return
      if ( nz.eq.1 ) then
         rkd = 1.
         goto 100
      endif

c     Lowest-level interpolation: return
      if ( abs(ppo-1050.).lt.eps ) then
         rkd = 1.
         goto 100
      endif

c     Get the pressure at the lowest level and at the surface
      ppo0 = int_index3(vert,nx,ny,nz,rid,rjd,real(1),0.)
      psur = int_index3(surf,nx,ny, 1,rid,rjd,real(1),0.)

c     Decide whether the pressure is ascending or descending
      ppo1 = int_index3(vert,nx,ny,nz,rid,rjd,real(nz),0.)
      if ( ppo1.gt.ppo0 ) then
         ppo0 = ppo1
      endif

c     The point is between the surface and the lowest level: return
      if ( mode.eq.3 ) then
        if ( (ppo.ge.ppo0).and.(ppo.le.psur).or.
     >       (ppo.le.ppo0).and.(ppo.ge.psur) )
     >  then
           psur = int_index3(surf,nx,ny, 1,rid,rjd,real(1),0.)
           rkd  = (psur-ppo)/(psur-ppo0)
           goto 100
        endif
      endif

c     Full-level search (TH): linear ascending scanning through all levels
      if ( mode.eq.1 ) then

         ppo0 = int_index3(vert,nx,ny,nz,rid,rjd,real(1),0.)
         rkd=0
         do i=1,nz-1
            ppo1 = int_index3(vert,nx,ny,nz,rid,rjd,real(i+1),0.)
            if ( (ppo.gt.ppo0).and.(ppo.le.ppo1) ) then
               rkd=real(i)+(ppo-ppo0)/(ppo1-ppo0)
               goto 100
            endif
            ppo0 = ppo1
         enddo

c     Full-level search (PV): linear descending scanning through all levels
      elseif ( mode.eq.2 ) then

         ppo1 = int_index3(vert,nx,ny,nz,rid,rjd,real(nz),0.)
         rkd=0
         do i=nz-1,1,-1
            ppo0 = int_index3(vert,nx,ny,nz,rid,rjd,real(i),0.)
            if ( (ppo1.gt.ppo).and.(ppo0.le.ppo) ) then
               rkd=real(i)+(ppo0-ppo)/(ppo0-ppo1)
               goto 100
            endif
            ppo1 = ppo0
         enddo

c     Full-level search (P):  binary search
      elseif ( mode.eq.3 ) then

         rkd  = 0
         i0   = 1
         i1   = nz
         ppo0 = int_index3(vert,nx,ny,nz,rid,rjd,real( 1),0.)
         ppo1 = int_index3(vert,nx,ny,nz,rid,rjd,real(nz),0.)

         do while ( i1.gt.(i0+1) )
            im   = (i0+i1)/2
            ppom = int_index3(vert,nx,ny,nz,rid,rjd,real(im),0.)
            if (ppom.lt.ppo) then
               i1   = im
               ppo1 = ppom
            else
               i0   = im
               ppo0 = ppom
            endif
         enddo

         rkd=real(i0)+(ppo0-ppo)/(ppo0-ppo1)

      endif

c     Exit point for subroutine
 100  continue

      end


MODULE RHS
  !Module to compute the right hand side of the Panel Code

  use precise, only : defaultp

  IMPLICIT NONE
  INTEGER, PARAMETER, PRIVATE :: WP=defaultp

CONTAINS

  SUBROUTINE computeRHS(npanels, nfspanels, coordsys, vinf, b)
    INTEGER                                     :: i
    INTEGER                                     :: Row, Col
    INTEGER                                     :: npanels, nfspanels
    REAL(WP), DIMENSION(3)                      :: vinf
    REAL(wp), DIMENSION(3,3,npanels+nfspanels)  :: coordsys
    REAL(WP), DIMENSION(npanels+nfspanels)      :: b

    write(*,*) ''
    write(*,*) 'Computing RHS.......'

    Do i=1, npanels
       b(i)= DOT_PRODUCT(coordsys(:,3,i),vinf)
    End Do
    Do i=1, nfspanels
       row=i+npanels
       b(row)=0.
    End Do
  END SUBROUTINE computeRHS

  SUBROUTINE computeNonLinearRHS(npanels, nfspanels, coordsys, vinf, &
       dphi, ddphi, b, &
                                       ntransompanels, ntransoml, ntransomt,nhpl,center)
    INTEGER                                                      :: i
    INTEGER                                                      :: Row, Col
    INTEGER                                                      :: npanels, nfspanels
    integer :: ntransompanels, ntransoml, ntransomt
    integer :: nhpl,hullpt
    REAL(WP), parameter                                          :: gravity = 9.81
    REAL(WP), DIMENSION(3)                                       :: vinf
    REAL(wp), ALLOCATABLE,DIMENSION(:,:,:)                       :: coordsys
    REAL(WP), ALLOCATABLE,DIMENSION(:)                           :: b
    REAL(WP), ALLOCATABLE,DIMENSION(:,:)                         :: dphi
    REAL(WP), ALLOCATABLE,DIMENSION(:,:,:)                       :: ddphi
    REAL(wp), ALLOCATABLE,DIMENSION(:,:)                         :: center

    write(*,*) ''
    write(*,*) 'Computing full nonlinear RHS.......'

    Do i=1, npanels
       !b(i)= -DOT_PRODUCT(coordsys(:,3,i),dphi(:,i))
       b(i)= DOT_PRODUCT(coordsys(:,3,i),vinf)
    End Do
    Do i=1, nfspanels+ntransompanels
       row=i+npanels
       b(row) = -((dphi(1,Row)**2) * ddphi(1,1,Row) + &
                  dphi(1,Row)*dphi(2,Row)*ddphi(2,1,Row) + &
                  dphi(1,Row)*dphi(3,Row)*ddphi(3,1,Row) + &
                  dphi(2,Row)*dphi(1,Row)*ddphi(1,2,Row) + &
                  (dphi(2,Row)**2)*ddphi(2,2,Row) + &
                  dphi(2,Row)*dphi(3,Row)*ddphi(3,2,Row) + &
                  gravity*dphi(3,Row))

    End Do

    Do i=1,ntransomt
       !Get the field points on the first order free surface, include x offset
       row = npanels + nfspanels + (i-1)*ntransoml+1
       hullpt = nhpl + (i-1)*nhpl
       !b(row) = vinf(1)-sqrt(max(0.,vinf(1)**2.-2.*gravity*center(3,hullpt)))  !!Bertram x-dir linearization
       b(row) = Vinf(1)**2 - 2.*gravity*center(3,hullpt) !!full Bernoulli streamline for the flow
    End Do

  END SUBROUTINE computeNonLinearRHS

END MODULE RHS


      subroutine estim (xleft,right,a,wl,npts,ileft,iright)
c******************************************************************************
c     This routine returns channel (pixel) values
c******************************************************************************

      real*4 wl(1)
      real*8 a(9)

      if (a(9).eq.1 .or. a(9).eq.2 .or. a(9).eq.3) go to 40
      if (a(4).eq.0.0 .and. a(3).eq.0.0) go to 60

c*****do the estimation for quadratics and cubics
      ileft = int(sngl((-a(2) + dsqrt(a(2)**2-4.*a(3)*(a(1)-xleft)))/
     . (2.*a(3)) - 0.5))
      iright = int(sngl((-a(2) + dsqrt(a(2)**2-4.*a(3)*(a(1)-right)))/
     . (2.*a(3)) + 0.5))

      if (a(4) .ne. 0.) then
         ipt = max0(ileft-3,1)
         jpt = min0(ileft+3,npts)
         ileft = ipt + int(6.*(xleft-wl(ipt))/(wl(jpt)-wl(ipt)))
         ipt = max0(iright-3,1)
         jpt = min0(iright+3,npts)
         iright = ipt + int(6.*(right-wl(ipt))/(wl(jpt)-wl(ipt)))
      endif
      ileft = max0(1,ileft)
      iright = min0(npts,iright)
      return

c*****do the estimation for linears
60    if (a(2) .eq. 0.) then
         ileft = int(xleft)
         iright = int(right)
      else
         ileft = max0(1,int(sngl((xleft-a(1))/a(2))))
         iright = min0(npts,int(sngl((right-a(1))/a(2))))
      endif
      return

c*****do the estimation for Chebyshevs, Splines, and Legendres
40    if (xleft .lt. wl(1)) then
         ileft = 1
         go to 55
      endif
      do i=2,npts
         if (xleft .le. wl(i)) then
            if (abs(xleft-wl(i)) .gt. abs(xleft-wl(i-1))) then
               ileft = i-1
            else
               ileft = i
            endif
            go to 55
         endif
      enddo
55    if (right .gt. wl(npts)) then
         iright = npts
         return
      endif
      do i=npts-1,1,-1
         if (right .ge. wl(i)) then
            if (abs(right-wl(i)) .gt. abs(right-wl(i+1))) then
               iright = i+1
            else
               iright = i
            endif
            return
         endif
      enddo
      return

      end


SUBROUTINE DGGSVD_F95( A, B, ALPHA, BETA, K, L, U, V, Q, IWORK, INFO )
!
!  -- LAPACK95 interface driver routine (version 3.0) --
!     UNI-C, Denmark; Univ. of Tennessee, USA; NAG Ltd., UK
!     September, 2000
!
!  .. USE STATEMENTS ..
   USE LA_PRECISION, ONLY: WP => DP
   USE LA_AUXMOD, ONLY: ERINFO
   USE F77_LAPACK, ONLY: GGSVD_F77 => LA_GGSVD
!  .. IMPLICIT STATEMENT ..
   IMPLICIT NONE
!  .. SCALAR ARGUMENTS ..
   INTEGER, INTENT(OUT), OPTIONAL :: INFO, K, L
!  .. ARRAY ARGUMENTS ..
   REAL(WP), INTENT(INOUT) :: A(:,:), B(:,:)
   REAL(WP), INTENT(OUT) :: ALPHA(:), BETA(:)
   REAL(WP), INTENT(OUT), OPTIONAL, TARGET :: U(:,:), V(:,:), Q(:,:)
   INTEGER, INTENT(OUT), OPTIONAL, TARGET :: IWORK(:)
!----------------------------------------------------------------------
!
! Purpose
! =======
!
!      LA_GGSVD computes the generalized singular values and, optionally,
! the transformation matrices from the generalized singular value
! decomposition (GSVD) of a real or complex matrix pair (A,B), where A
! is m by n and B is p by n. The GSVD of (A,B) is written
!       A = U * SIGMA1(0, R)*Q^H , B = V * SIGMA2(0, R)*Q^H
! where U , V and Q are orthogonal (unitary) matrices of dimensions m by m,
! p by p and n by n, respectively. Let l be the rank of B and r the rank of
! the (m + p) * n matrix ( A )
!                        ( B )
! , and let k = r-l. Then SIGMA1 and SIGMA2 are m*(k + l) and p * (k + l)
! "diagonal" matrices, respectively, and R is a (k + l) * (k + l)
! nonsingular triangular matrix. The detailed structure of SIGMA1 ,SIGMA2
! and R depends on the sign of (m - k - l) as follows:
!       The case m-k-l>=0:
!
!                               k   l
!                      k      ( I   0 )
!        SIGMA1 =      l      ( 0   C )
!                    m-k-l    ( 0   0 )
!
!
!                           k   l
! 	SIGMA2 =    l   ( 0   S )
! 	          p - l ( 0   S )
!
!
!                          n-k-l   k     l
!          (0, R) =   k   (  0    R11   R12  )
!  	              l   (  0     0    R22  )
!
! where C^2 + S^2 = I . We define
! alpha(1)=alpha(2)=...=alpha(k) = 1, alpha(k+i)=c(i i), i=1,2,...,l
! beta(1) = beta(2) = ... = beta(k) = 0, beta(k+i) = s(i i), i=1,2,...,l
!
! The case m-k-l < 0:
!
!                                k    m-k    k+l-m
!            SIGMA1 =    k    (  I     0       0   )
!   	                m-k   (  0     C       0   )
!
!
! 		               k    m-k     k+l-m
!                        m-k   ( 0     S        0  )
!            SIGMA2 =   k+l-m  ( 0     0        I  )
!  	                 p-l   ( 0     0        0  )
!
!
!                                  n-k-l    k     m-k   k+l-m
! 		  	  k      (   0     R11    R12    R13  )
!              (0,R) =   m-k     (   0      0     R22    R23  )
! 	                k+l-m    (   0      0      0     R33  )
!
! where C^2 + S^2 = I . We define
!  alpha(1)=alpha(2)=...=alpha(k) = 1, alpha(k+i)=c(i i), i =1,2,...,m-k,
!  alpha(m+1) = alpha(m+2)=...= alpha(k+l) = 0
!  beta(1)=beta(2)= ... =beta(k)=0, beta(k+i)=s(i i), i=1,2,...,m-k,
!  beta(m+1) = beta(m+2) = ... = beta(k+l) = 1
!
! In both cases the generalized singular values of the pair (A,B) are the
! ratios
!  sigma(i) = alpha(i)/beta(i), i = 1,2, ... ,k+l
!
! The first k singular values are infinite. The finite singular values
! are real and nonnegative.
!     LA_GGSVD computes the real (nonnegative) scalars alpha(i), beta(i),
! i=1,2,..., k+l , the matrix R, and, optionally, the transformation
! matrices U , V and Q.
!
! =========
!
!      SUBROUTINE LA_GGSVD( A, B, ALPHA, BETA, K=k, L=l, &
!                       U=u, V=v, Q=q, IWORK=iwork, INFO=info )
!          <type>(<wp>), INTENT(INOUT) :: A(:,:), B(:,:)
!          REAL(<wp>), INTENT(OUT) :: ALPHA(:), BETA(:)
!          INTEGER, INTENT(OUT), OPTIONAL :: K, L
!          <type>(<wp>), INTENT(OUT), OPTIONAL :: U(:,:), V(:,:), Q(:,:)
!          INTEGER, INTENT(IN), OPTIONAL :: IWORK(:)
!          INTEGER, INTENT(OUT), OPTIONAL :: INFO
!       where
!          <type> ::= REAL | COMPLEX
!          <wp>   ::= KIND(1.0) | KIND(1.0D0)
!
! Arguments
! =========
!
! A      (input/output) REAL or COMPLEX array, shape (:,:) with
!        size(A,1) = m and size(A,2) = n.
!        On entry, the matrix A.
!        On exit, A contains the triangular matrix R, or part of R, as
!        follows:
!        If m-k-l >= 0, then R is stored in A(1:k+l,n-k-l+1:n).
!        If m-k-l < 0, then the matrix
!                  ( R11     R12    R13 )
! 		 (  0      R22    R23 )
!        is stored in A(1:m,n-k-l+1:n).
! B      (input/output) REAL or COMPLEX array, shape (:,:) with
!        size(B,1) = p and size(B,2) = n.
!        On entry, the matrix B.
!        On exit, if m-k-l < 0, then R33 is stored in
!        B(m-k+1:l,n+m-k-l+1:n).
! ALPHA  (output) REAL array, shape (:) with size(ALPHA) = n
!        The real scalars alpha(i) , i = 1, 2,..., k+l.
! BETA   (output) REAL array, shape (:) with size(BETA) = n.
!        The real scalars beta(i) , i = 1, 2, ..., k+l.
!        Note: The generalized singular values of the pair (A,B) are
!        sigma(i) = ALPHA(i)/BETA(i), i = 1, 2, ...,  k+l.
!        If k + l < n, then ALPHA(k+l+1:n) = BETA(k+l+1:n) = 0.
! K, L   Optional (output) INTEGER.
!        The dimension parameters k and l.
! U      Optional (output) REAL or COMPLEX square array, shape (:,:) with
!        size(U,1) = m.
!        The matrix U .
! V      Optional (output) REAL or COMPLEX square array, shape (:,:) with
!        size(V,1) = p.
!        The matrix V .
! Q      Optional (output) REAL or COMPLEX square array, shape (:,:) with
!        size(Q,1) = n.
!        The matrix Q.
! IWORK  Optional (output) INTEGER array, shape(:) with size(IWORK) = n.
!        IWORK contains sorting information. More precisely, the loop
!             for i = k + 1, min(m, k + l)
!                   swap ALPHA(i) and ALPHA(IWORK(i))
!             end
!        will sort ALPHA so that ALPHA(1) >= ALPHA(2) >= ... >= ALPHA(n).
! INFO   Optional (output) INTEGER.
!        = 0: successful exit.
!        < 0: if INFO = -i, the i-th argument had an illegal value.
!        > 0: if INFO = 1, the algorithm failed to converge.
!        If INFO is not present and an error occurs, then the program is
!        terminated with an error message.
!----------------------------------------------------------------------
!  .. LOCAL PARAMETERS ..
   CHARACTER(LEN=8), PARAMETER :: SRNAME = 'LA_GGSVD'
!  .. LOCAL SCALARS ..
   CHARACTER(LEN=1) :: LJOBU, LJOBV, LJOBQ
   INTEGER :: M, N, P, LINFO, ISTAT, ISTAT1, S1U, S2U, S1V, S2V, &
              S1Q, S2Q, LK, LL
!  .. LOCAL ARRAYS ..
   REAL(WP), TARGET :: LLU(1,1), LLV(1,1), LLQ(1,1)
   INTEGER, POINTER :: LIWORK(:)
   REAL(WP), POINTER :: WORK(:)
!  .. INTRINSIC FUNCTIONS ..
   INTRINSIC MAX, PRESENT, SIZE
!  .. EXECUTABLE STATEMENTS ..
   LINFO = 0; ISTAT = 0; M = SIZE(A,1); N = SIZE(A,2); P = SIZE(B,1)
   IF( PRESENT(U) )THEN; S1U = SIZE(U,1); S2U = SIZE(U,2); LJOBU = 'U'
   ELSE; S1U = 1; S2U = 1; LJOBU = 'N'; END IF
   IF( PRESENT(V) )THEN; S1V = SIZE(V,1); S2V = SIZE(V,2); LJOBV = 'V'
   ELSE; S1V = 1; S2V = 1; LJOBV = 'N'; END IF
   IF( PRESENT(Q) )THEN; S1Q = SIZE(Q,1); S2Q = SIZE(Q,2); LJOBQ = 'Q'
   ELSE; S1Q = 1; S2Q = 1; LJOBQ = 'N'; END IF
!  .. TEST THE ARGUMENTS
   IF( M < 0 .OR. N < N )THEN; LINFO = -1
   ELSE IF( SIZE(B,2) /= N .OR. P < 0 ) THEN; LINFO = -2
   ELSE IF( SIZE( ALPHA ) /= N )THEN; LINFO = -3
   ELSE IF( SIZE( BETA ) /= N )THEN; LINFO = -4
   ELSE IF( PRESENT(U) .AND. ( S1U /= MAX(1,M) .OR.  S2U /= M ) )THEN; LINFO = -7
   ELSE IF( PRESENT(V) .AND. ( S1V /= MAX(1,P) .OR.  S2V /= P ) )THEN; LINFO = -8
   ELSE IF( PRESENT(Q) .AND. ( S1Q /= MAX(1,N) .OR.  S2Q /= N ) )THEN; LINFO = -9
   ELSE
    IF (PRESENT(IWORK)) THEN
      LIWORK => IWORK
    ELSE
      ALLOCATE( LIWORK(MAX(1,N)), STAT=ISTAT )
    ENDIF
      IF( ISTAT == 0 ) THEN
         ALLOCATE( WORK( MAX(1,MAX(3*N,M,P)+N) ), STAT=ISTAT)
      END IF
      IF( ISTAT == 0 ) THEN
         IF( PRESENT(U) )THEN
           IF( PRESENT(V) )THEN
             IF( PRESENT(Q) )THEN
                CALL GGSVD_F77( LJOBU, LJOBV, LJOBQ, M, N, P, LK, LL, A, MAX(1,M), &
                         B, MAX(1,P), ALPHA, BETA, U, MAX(1,S1U), &
                        V, MAX(1,S1V), Q, MAX(1,S1Q), WORK, LIWORK, LINFO )
	      ELSE
	        CALL GGSVD_F77( LJOBU, LJOBV, LJOBQ, M, N, P, LK, LL, A, MAX(1,M), &
                         B, MAX(1,P), ALPHA, BETA, U, MAX(1,S1U), &
                        V, MAX(1,S1V), LLQ, MAX(1,S1Q), WORK, LIWORK, LINFO )
	      ENDIF
	     ELSE
	      IF( PRESENT(Q) )THEN
	        CALL GGSVD_F77( LJOBU, LJOBV, LJOBQ, M, N, P, LK, LL, A, MAX(1,M), &
                         B, MAX(1,P), ALPHA, BETA, U, MAX(1,S1U), &
                        LLV, MAX(1,S1V), Q, MAX(1,S1Q), WORK, LIWORK, LINFO )
	      ELSE
	        CALL GGSVD_F77( LJOBU, LJOBV, LJOBQ, M, N, P, LK, LL, A, MAX(1,M), &
                         B, MAX(1,P), ALPHA, BETA, U, MAX(1,S1U), &
                        LLV, MAX(1,S1V), LLQ, MAX(1,S1Q), WORK, LIWORK, LINFO )
	      ENDIF
	    ENDIF
	   ELSE
	     IF( PRESENT(V) )THEN
	       IF( PRESENT(Q) )THEN
	         CALL GGSVD_F77( LJOBU, LJOBV, LJOBQ, M, N, P, LK, LL, A, MAX(1,M), &
                         B, MAX(1,P), ALPHA, BETA, LLU, MAX(1,S1U), &
                        V, MAX(1,S1V), Q, MAX(1,S1Q), WORK, LIWORK, LINFO )
	       ELSE
	         CALL GGSVD_F77( LJOBU, LJOBV, LJOBQ, M, N, P, LK, LL, A, MAX(1,M), &
                         B, MAX(1,P), ALPHA, BETA, LLU, MAX(1,S1U), &
                        V, MAX(1,S1V), LLQ, MAX(1,S1Q), WORK, LIWORK, LINFO )
		ENDIF
	      ELSE
	       IF( PRESENT(Q) )THEN
	         CALL GGSVD_F77( LJOBU, LJOBV, LJOBQ, M, N, P, LK, LL, A, MAX(1,M), &
                         B, MAX(1,P), ALPHA, BETA, LLU, MAX(1,S1U), &
                        LLV, MAX(1,S1V), Q, MAX(1,S1Q), WORK, LIWORK, LINFO )
		ELSE
		 CALL GGSVD_F77( LJOBU, LJOBV, LJOBQ, M, N, P, LK, LL, A, MAX(1,M), &
                         B, MAX(1,P), ALPHA, BETA, LLU, MAX(1,S1U), &
                        LLV, MAX(1,S1V), LLQ, MAX(1,S1Q), WORK, LIWORK, LINFO )
		ENDIF
	       ENDIF
	      ENDIF
         IF( PRESENT(K) ) K = LK
         IF( PRESENT(L) ) L = LL
      ELSE; LINFO = -100; ENDIF
      DEALLOCATE(WORK, STAT=ISTAT1)
      IF (.NOT. PRESENT(IWORK)) DEALLOCATE(LIWORK, STAT=ISTAT1)
   ENDIF
   CALL ERINFO(LINFO,SRNAME,INFO,ISTAT)
END SUBROUTINE DGGSVD_F95


C    Copyright(C) 1999-2020 National Technology & Engineering Solutions
C    of Sandia, LLC (NTESS).  Under the terms of Contract DE-NA0003525 with
C    NTESS, the U.S. Government retains certain rights in this software.
C
C    See packages/seacas/LICENSE for details

      SUBROUTINE CIAPAL (MXND, XN, YN, LXK, KXL, NXL, LXN, NNN, NNNOLD,
     &   NIT, EPS, RO, ALPHA)
C***********************************************************************

C  SUBROUTINE CIAPAL = CENTROID INVERSE AREA PUSH AND LAPLACIAN SMOOTH

C***********************************************************************

C  NOTE:
C     IN THIS METHOD EACH CENTROID OF AN ELEMENT PUSHES OUT
C     ON THE SURROUNDING NODES WITH A FORCE INVERSELY PROPORTIONAL
C     TO THE AREA OF THE ELEMENT WHILE IT SIMULTANEOUSLY PULLS ON
C     EACH NODE WITH A FORCE PROPORTIONAL TO THE LENGTH OF THE LINE
C     CONNECTING THE CENTROID WITH EACH NODE.

C***********************************************************************

C  VARIABLES USED:
C     NIT   = MAX NUMBER OF ITERATIONS TO DO
C     EPS   = NODE MOVEMENT TOLERANCE FOR CONVERGENCE
C     RO    = UNDER OR OVER-RELAXATION FACTOR.
C     ALPHA = WEIGHT GIVEN TO AREA-PUSH VECTOR.  USUALLY=0.5.
C             WEIGHT GIVEN TO LAPLACIAN VECTOR = 1.-ALPHA.

C***********************************************************************

      DIMENSION NODES (4)
      DIMENSION KLIST (20), AREA (20), XCEN (20), YCEN (20)
      DIMENSION LXK (4, MXND), KXL (2, 3*MXND)
      DIMENSION NXL (2, 3*MXND), LXN (4, MXND)
      DIMENSION XN (MXND), YN (MXND)

      LOGICAL BIG, CCW, ERR
      EPS2 =  (EPS * RO) **2
      BETA = 1.0 - ALPHA

C  ITERATION LOOP

      DO 140 IT = 1, NIT
         BIG = .FALSE.

C  NODE LOOP

         DO 130 NODE = NNNOLD + 1, NNN

C  SKIP CONTINUATION LINES AND BOUNDARY LINES

            IF ((LXN (1, NODE).GT.0) .AND. (LXN (2, NODE).GT.0)) THEN

C  GET ELEMENT LIST  (IGNORE ERR IF IT IS BECAUSE TOO MANY WERE FOUND)

               CALL GKXN (MXND, KXL, LXN, NODE, NUMK, KLIST, ERR)
               IF ((ERR) .AND. (NUMK.LT.20))RETURN

C  GET AREAS AND CENTROIDS

               DO 110 IK = 1, NUMK
                  KK = KLIST (IK)
                  CCW = .TRUE.
                  CALL GNXKA (MXND, XN, YN, KK, NODES, AREA (IK), LXK,
     &               NXL, CCW)
                  XSUM = 0.
                  YSUM = 0.
                  DO 100 I = 1, 4
                     NN = NODES (I)
                     XSUM = XSUM + XN (NN)
                     YSUM = YSUM + YN (NN)
  100             CONTINUE
                  XCEN (IK) = 0.25 * XSUM
                  YCEN (IK) = 0.25 * YSUM
  110          CONTINUE

C  COMPUTE AND SUM THE FORCE VECTORS

               FX = 0.
               FY = 0.
               SUMW = 0.
               SDX = 0.
               SDY = 0.
               DO 120 IK = 1, NUMK
                  DX = XCEN (IK) - XN (NODE)
                  DY = YCEN (IK) - YN (NODE)
                  AL2 = DX * DX + DY * DY
                  ARL = AREA (IK) * SQRT (AL2)
                  WEIGHT = 1.0E20
                  IF (ARL.GT.0.)WEIGHT = 1.0/ARL
                  SUMW = SUMW + WEIGHT
                  FX = FX - WEIGHT * DX
                  FY = FY - WEIGHT * DY
                  SDX = SDX + DX
                  SDY = SDY + DY
  120          CONTINUE

C  MOVE THE NODE

               RSUMW = 1.0/SUMW
               RNUMK = 1.0/DBLE(NUMK)
               FX = ALPHA * FX * RSUMW + BETA * SDX * RNUMK
               FY = ALPHA * FY * RSUMW + BETA * SDY * RNUMK
               DELX = RO * FX
               DELY = RO * FY
               XN (NODE) = XN (NODE) + DELX
               YN (NODE) = YN (NODE) + DELY
               IF (DELX ** 2 + DELY ** 2 .GT. EPS2)BIG = .TRUE.
            ENDIF
  130    CONTINUE
         IF (.NOT.BIG) RETURN
  140 CONTINUE
      RETURN
      END


*=======================================================================
        SUBROUTINE SETUP(A,INDROW,INDCOL,IUPPER,NZER,TM,TP,
     :                   N,LIM,NUME,HIEND,DIAG,MINELEM,
     :                   BASIS,AB,S,NIV)
*=======================================================================
*       Subroutine for setting up (i) the initial BASIS if not provided,
*       (ii) the product of the matrix A with the Basis into matrix AB,
*       and (iii) the small matrix S=B^TAB. If no initial estimates are
*       available, the BASIS =(e_i1,e_i2,...,e_iNUME), where i1,i2,...,
*       iNUME are the indices of the NUME lowest diagonal elements, and
*       e_i the i-th unit vector. (ii) and (iii) are handled by ADDABS.
*-----------------------------------------------------------------------
        IMPLICIT DOUBLE PRECISION(A-H,O-Z)
        LOGICAL IUPPER
        DIMENSION A(NZER),INDROW(NZER),INDCOL(N)
        DIMENSION TM(N),TP(N)
        DIMENSION DIAG(N),BASIS(N*LIM),AB(N*LIM)
        DIMENSION S(LIM*(LIM+1)/2),MINELEM(LIM)
*-----------------------------------------------------------------------
*   on entry
*   --------
*   OP          The block matrix-vector operation, passed to ADDABS
*   N           the order of the matrix A
*   LIM         The limit on the size of the expanding Basis
*   NUME        Largest index of the wanted eigenvalues.
*   HIEND       Logical. True only if the highest eigenpairs are needed.
*   DIAG        Array of size N with the diagonal elements of A
*   MINELEM     Array keeping the indices of the NUME lowest diagonals.
*
*   on exit
*   -------
*   BASIS       The starting basis.
*   AB, S       The starting D=AB, and small matrix S=B^TAB
*   NIV         The starting dimension of BASIS.
*-----------------------------------------------------------------------

        IF ((NIV.GT.LIM).OR.(NIV.LT.NUME)) THEN
*
*          ..Initial estimates are not available. Give as estimates unit
*          ..vectors corresponding to the NUME minimum diagonal elements
*          ..First find the indices of these NUME elements (in MINELEM).
*          ..Array AB is used temporarily as a scratch array.
*
           CALL DINIT(N,-1.D0,AB,1)
           DO 10 I=1,NUME
*             ..imin= the first not gotten elem( NUME<=N )
              DO 20 J=1,N
 20              IF (AB(J).LT.0) GOTO 30
 30           IMIN=J
              DO 40 J=IMIN+1,N
 40              IF ((AB(J).LT.0).AND.
     :              (DIAG(J).LT.DIAG(IMIN))) IMIN=J
              MINELEM(I)=IMIN
              AB(IMIN)=1.D0
 10        CONTINUE
*
*          ..Build the Basis. B_i=e_(MINELEM(i))
*
           CALL DINIT(N*LIM,0.D0,BASIS,1)
           DO 50 J=1,NUME
              I=(J-1)*N+MINELEM(J)
              BASIS(I)=1
 50        CONTINUE

           NIV=NUME
        ENDIF
*
* Find the matrix AB by matrix-vector multiplies, as well as the
* small matrix S = B^TAB.
*
        KPASS=0
        CALL ADDABS(A,INDROW,INDCOL,IUPPER,NZER,TM,TP,
     :              N,LIM,HIEND,KPASS,NIV,BASIS,AB,S)

        RETURN
        END


      DOUBLE PRECISION FUNCTION CDFGLO(X,PARA)
C***********************************************************************
C*                                                                     *
C*  FORTRAN CODE WRITTEN FOR INCLUSION IN IBM RESEARCH REPORT RC20525, *
C*  'FORTRAN ROUTINES FOR USE WITH THE METHOD OF L-MOMENTS, VERSION 3' *
C*                                                                     *
C*  J. R. M. HOSKING                                                   *
C*  IBM RESEARCH DIVISION                                              *
C*  T. J. WATSON RESEARCH CENTER                                       *
C*  YORKTOWN HEIGHTS                                                   *
C*  NEW YORK 10598, U.S.A.                                             *
C*                                                                     *
C*  VERSION 3     AUGUST 1996                                          *
C*                                                                     *
C***********************************************************************
C
C  DISTRIBUTION FUNCTION OF THE GENERALIZED LOGISTIC DISTRIBUTION
C
      IMPLICIT DOUBLE PRECISION (A-H,O-Z)
      DOUBLE PRECISION PARA(3)
      DATA ZERO/0D0/,ONE/1D0/
C
C         SMALL IS USED TO TEST WHETHER X IS EFFECTIVELY AT
C         THE ENDPOINT OF THE DISTRIBUTION
C
      DATA SMALL/1D-15/
C
      U=PARA(1)
      A=PARA(2)
      G=PARA(3)
      IF(A.LE.ZERO)GOTO 1000
      Y=(X-U)/A
      IF(G.EQ.ZERO)GOTO 20
      ARG=ONE-G*Y
      IF(ARG.GT.SMALL)GOTO 10
      IF(G.LT.ZERO)CDFGLO=ZERO
      IF(G.GT.ZERO)CDFGLO=ONE
      RETURN
   10 Y=-DLOG(ARG)/G
   20 CDFGLO=ONE/(ONE+DEXP(-Y))
      RETURN
C
 1000 WRITE(6,7000)
      CDFGLO=ZERO
      RETURN
C
 7000 FORMAT(' *** ERROR *** ROUTINE CDFGLO : PARAMETERS INVALID')
      END