      subroutine icse(ind,nu,u,co,g,itv,rtv,dtv,icsef,icsec2,icsei)
c!but
c     Le logiciel ICSE est un outil de resolution de problemes de
c     CONTROLE OPTIMAL de systemes decrits par des equations
c     differentielles ou algebrico-differentielles, NON LINEAIRES.
c     Dans la mesure ou dans la methode d'integration qu'il utilise
c     est inconditionnellement stable,il peut aussi etre utilise pour
c     resoudre des problemes de controle d'EQUATIONS AUX DERIVEES
c     PARTIELLES DYNAMIQUES (reaction-diffusion, par exemple), ou
c     plus generalement pour controler des systemes raides.
c     Le controle se decompose en une partie independante du temps et
c     une partie dependante du temps.Cette structure permet a
c     l'utilisateur de resoudre facilement les problemes
c     d'IDENTIFICATION DE PARAMETRES d'un systeme dynamique par des
c     methodes de MOINDRES CARRES. Diverses facilites sont d'ailleurs
c     prevues pour ce cas.
c     Dans le cas d'un SYSTEME LINEAIRE,l'integration de l'etat et de
c     l'etat adjoint sont effectuees de maniere a tirer parti de la
c     linearite de l'equation.
c
c     Resolution de problemes de controle ou d'identification de
c      parametres de systemes dynamiques du type:
c          0=fi(t,y,u),i<=nea et dyj/dt=fj(t,y,u),nea<j<=ny pour t>=t0
c          et y(t0)=y0 , en notant ny la dimension de l'etat y et
c          fk la keme composante de la fonction f
c      On effectue un certain nombre (nex) d'experiences identiques
c      au cours desquelles certaines donnees sont mesurees.
c      Le critere a minimiser est de la forme c(tob,ytob,ob) avec:
c        tob(ntob)       :instants de mesure
c        ytob(ny,ntob)   :valeurs de l'etat aux instants de mesure
c        ob(nex,ntob,nob):mesures
c      L'equation d'etat est discretisee par la methode
c      de Crank-Nicolson.
c      Le gradient du cout est calcule en utilisant l'etat adjoint du
c      systeme discretise.
c
c! DESCRIPTION FORMELLE DES PROBLEMES DE CONTROLE CONSIDERES
c       L'equation du systeme est de la forme
c              0     =  fi(t,y(t),uc,uv(t)) ,  i<=nea,
c          dyi(t)/dt =  fi(t,y(t),uc,uv(t)) ,  i> nea,
c       et
c          t0<= t <=tf ,    y(t0)=y0 .
c     avec :
c       y(t )  : etat du systeme,
c       uc     : partie  du controle independante du temps
c               (controle constant),
c       uv     : partie  du controle dependante du temps
c                (controle variable)
c       t0, tf : instant initial et instant final,
c       y0     : etat initial. Il est soit fixe,soit fonction
c                du controle [y0=ei(uc,uv)]
c
c       Le critere a minimiser est de la forme  :
c
c                  tp
c                   ____
c                  \
c                   |    c2(ti,y(ti),uc) .
c                  /___
c                  ti =t1
c
c        Sont resolus, soit des problemes sans contraintes,  soit des
c     problemes comportant des contraintes de borne sur le controle.On
c     peut aussi utiliser ICSE pour resoudre des problemes comportant
c     des contraintes sur  l'etat, en traitant  ces contraintes par
c     penalisation ou lagrangien augmente [Ber,82].
c        Les fonctions  f,c1,c2,ei et leurs derivees  partielles sont
c     fournies par l'utilisateur sous forme de subroutines Fortran.
c
c
c
c! OUTILS POUR L'IDENTIFICATION DE PARAMETRES
c     Le probleme de l'identification de parametres (ou d'ajustement
c     de parametres a des mesures) a les caracteristiques suivantes :
c     le critere est fonction seulement de mesures faites a certains
c     instants et des valeurs de l'etat a ces instants.Seule la seconde
c     partie du cout intervient donc.Les mesures peuvent avoir ete
c     obtenues lors de plusieurs experiences.En general,le critere
c     est du type MOINDRES CARRES associe a une OBSERVATION LINEAIRE
c     .Autrement dit, il est de la forme
c
c                nex      ntob
c                ____     ____                              2
c           1    \        \      ||                       ||
c           -     |        |     || obs*y(ti) - z(iex,ti) || ,
c           2    /___     /___   ||                       ||
c                iex=1    ti =t1
c
c     ou obs est  la  matrice  d'observation et  z(iex,ti) represente
c     l'ensemble  des  mesures  faites  lors  de l'experience iex,  a
c     l'instant ti.Dans ce cas,l'utilisateur n'a aucune modification a
c     a apporter  a la subroutine de calcul  de cout  de l'exemple de
c     demonstration.
c!liste d'appel:
c      icse(ind,nu,u,co,g,itv,rtv,dtv)
c      en entree:
c
c      ind        entier egal a 2,3,ou4
c
c      nu         entier
c                 dimension du vecteur des parametres
c
c      u          double precision (nu)
c                 vecteur des parametres
c
c      en sortie:
c
c      co         double precision
c                 cout
c
c      g          double precision (nu)
c                 gradient de la fonction de cout c
c
c      itv        entier (nitv)
c                 tableau de travail entier
c
c      rtv        reel (nrtv)
c                 tableau de travail reel
c
c      dtv        double precision (ndtv)
c                 tableau de travail double precision
c
c!subroutines utilisees
c      Linpack    :dadd,daxpy,dcopy,dmmul,dnrm2,dscal,dset,
c                  dgefa,dgesl
c                 :icse0,icse1,icse2,icscof,icsef,icsei
c      common/nird/nitv,nrtv,ndtv
c!utilisation
c
c     Le probleme a traiter doit etre defini par 3 routines
c     fortran ecrites par l'utilisateur :
c
c      -Second membre de l'equation d'etat :
c       icsef(indf,t,y,uc,uv,f,fy,fu,b,itu,dtu,
c     & t0,tf,dti,dtf,ermx,iu,nuc,nuv,ilin,nti,ntf,ny,nea,
c     & itmx,nex,nob,ntob,ntobi,nitu,ndtu)
c       Parametres d'entree :
c        indf     : vaut 1,2,3 suivant qu'on veut calculer f,fy,fu
c        t        : instant courant
c        y(ny)    : etat a un instant donne
c        uc(nuc)  : controle independant du temps
c        uv(nuv)  : controle dependant du temps, a l'instant t
c        b(ny)    : terme constant dans le cas lineaire quadratique
c       Parametres de sortie :
c         indf    : >0 si  le calcul s'est  correctement effectue,<=0
c                   sinon
c        f(ny)    : second membre
c        fy(ny,ny): jacobien de f par rapport a y
c        fu(ny,nuc+nuv) : derivee de f par rapport au controle
c       Tableaux de travail reserves a l'utilisateur :
c        itu(nitu): tableau entier
c        dtu(ndtu): tableau double precision
c       (nitu et ndtu sont initialises par le common icsez).
c
c      -Cout ponctuel :
c       icsec2(indc,nu,tob,obs,cof,ytob,ob,u,c2,c2y,g,yob,d,itu,dtu,
c     & t0,tf,dti,dtf,ermx,iu,nuc,nuv,ilin,nti,ntf,ny,nea,
c     & itmx,nex,nob,ntob,ntobi,nitu,ndtu)
c       Parametres d'entree :
c        indc     : 1 si on desire calculer c2,2 si on desire
c                   calculer c2y,c2u
c        tob      : instants de mesure
c        obs      : matrice d'observation
c        cof      : coefficients de ponderation du cout
c        ytob     : valeur de l'etat aux instants d'observation
c        ob       : mesures
c        u(nu)    : controle.Le controle variable est stocke a la
c                   suite du controle suite du constant.
c       Parametres de sortie :
c        indc     : comme pour icsec1
c        c2       : cout
c        c2y(ny,ntob) : derivee de c2 par rapport a y
c        g(nu)  : derivee de c2 par rapport a u
c
c      -Etat initial (s'il est variable)
c       icsei(indi,nui,ui,y0,y0ui,itu,dtu,
c     & t0,tf,dti,dtf,ermx,iu,nuc,nuv,ilin,nti,ntf,ny,nea,
c     & itmx,nex,nob,ntob,ntobi,nitu,ndtu)
c       Parametres d'entree :
c        indi     : 1 si on desire calculer y0, 2 si on  desire
c                     calculer y0ui
c        nui      : dimension du tableau ui defini ci-dessous,
c        ui       : partie du controle intervenant dans l'etat initial,
c                   determinee par iu;vaut uc,uv,ou [uc,uv].
c
c       Parametres de sortie :
c        indc     : >0  si le calcul  s'est correctement effectue,<=0
c                   sinon,
c        y0       : etat initial,
c        y0ui     : derivee de l'etat initial par rapport au controle.
c
c
c
c!vue d'ensemble
c         Pour  utiliser la  subroutine icse, il faut  disposer d'un
c     optimiseur (code d'implementation d'un algorithme d'optimisation)
c     a  la norme  MODULOPT.Il  faut  ensuite  ecrire  le  programme
c     principal, constitue de quatre parties :
c       1. Initialisation des variables du common icsez,
c       2. Initialisation des tableaux itv et dtv et du common nird,
c       3. Appel de l'optimiseur,
c       4. Traitement des resultats.
c
c
c     1. INITIALISATION DU COMMON ICSEZ
c        common/icsez/t0,tf,dti,dtf,ermx,iu,nuc,nuv,ilin,nti,ntf,ny,nea,
c        itmx,nex,nob,ntob,ntobi,nitu,ndtu
c
c       Liste des variables a initialiser :
c       t0    : instant initial
c       tf    : instant final
c       dti   : premier pas de temps
c       dtf   : second pas de temps
c       ermx  : test d'arret absolu sur la valeur du  second membre
c               dans la resolution de l'equation d'etat
c       iu(5) : tableau  parametrant le  probleme : seuls iu(1:3)
c               sont utilises.
c            iu(1)=1 si l'etat initial depend du  controle constant
c                  0 sinon
c            iu(2)=1 si l'etat initial  depend du controle variable
c                  0 sinon
c            iu(3)=1 si le second membre depend du controle constant,
c                  0 sinon
c
c       nuc   : dimension du controle constant.
c       nuv   : dimension du controle variable a un instant donne.
c       ilin  : indicateur de linearite
c       nti   : nombre de pas de temps correspondant a dti (premier
c                 pas de temps)
c       ntf   : nombre de pas  de temps correspondant a dtf (second
c               pas de temps)
c       ny    : dimension de l'etat a un instant donne
c       nea   : nombre d'equations algebriques (eventuellement nul)
c       itmx  : nombre  maximal  d'iterations dans la resolution
c               de l'equation d'etat discrete a un pas de temps
c               donne
c       nex   : nombre d'experiences effectuees
c       nob   : dimension du  vecteur des mesures  pour une
c               experience donnee en un instant donne
c       ntob  : nombre d'instants de mesure pour une experience donnee
c       ntobi : nombre d'instants de mesure correspondant a dti
c               (premier pas de temps)
c       nitu  : longueur de  itu,tableau de  travail entier reserve
c               a l'utilisateur
c       ndtu  : longueur de dtu,  tableau de travail  double
c               precision reserve a l'utilisateur
c       u(nu)       : parametres initiaux
c       y0(ny)      : etat initial
c       tob(ntob)   : instants de mesure
c       binf(nu)    : borne inferieures sur les parametres
c       bsup(nu)    : borne superieures sur les parametres
c       obs(nob,ny) : matrice d'observation
c
c     Bien noter que
c         nu = nuc + nuv*(nti+ntf+1),
c         nui= iu(1)*nuc+ui(2)*nuv*(nti+ntf+1)
c     et que les dimensions suivantes peuvent etre nulles :
c         nuc,nuv,ntf,nea.
c
c
c     2 INITIALISATION DES TABLEAUX ENTIER ET DOUBLE PRECISION.
c       Le tableau itv (entier) contient le tableau :
c       itu    dimension nitu      : reserve a l'utilisateur,
c     le reste du tableau etant reserve au systeme ICSE.
c
c     Le tableau dtv (reel double precision) contient les tableaux :
c       dtu    dimension  ndtu     : reserve a l'utilisateur,
c       y0                ny       : etat initial,
c       tob               ntob     : instants d'observation,
c       obs               nob,ny   : matrice d'observation,
c       ob            nex,ntob,nob : observations (mesures),
c       ech               nu       : coefficients de mise a l'echelle de
c                                    u,
c       cof              nob,ntob  : coefficients de ponderation du
c                                    cout,
c
c       Les dimensions nitu et ndtu sont passees par le common icsez,
c                      nitv et ndtv sont passees par le common nird.
c
c
c!
c     Copyright INRIA
      implicit double precision (a-h,o-z)
      real rtv
      dimension u(nu),g(nu),itv(*),rtv(*),dtv(*),iu(5)
      external icsef,icsec2,icsei
c
      common/icsez/ t0,tf,dti,dtf,ermx,iu,nuc,nuv,ilin,nti,ntf,ny,nea,
     &itmx,nex,nob,ntob,ntobi,nitu,ndtu
      common/nird/nitv,nrtv,ndtv
c
c
c     lui et nui servent quand l'etat initial depend du controle
      if (iu(2).gt.0) lui=min(nu,1+nuc)
      if (iu(1).gt.0) lui=1
      nui=iu(1)*nuc+iu(2)*nuv*(nti+ntf+1)
c
c     decoupage de itv
c     nitu longueur de itu tableau de travail entier reserve
c     a l'utilisateur
c     nitvt longueur de itvt tableau de travail entier de
c     icse1 et icse2
c
      litu=1
      litvt=litu+nitu
c
c     decoupage de dtv
c     ndtu longueur de dtu tableau de travail double precision
c     reserve a l'utilisateur
c     ndtvt longueur de dtvt tableau de travail double precision
c     de icse1 et icse2
c
      ldtu=1
      ly0=ldtu+ndtu
      ltob=ly0+ny
      lobs=ltob+ntob
      lob=lobs+nob*ny
      lech=lob+nex*ntob*nob
      lcof=lech+nu
c       ********************** Modif 88
      lb=lcof+nob*ntob
      lfy=lb+ny
      lfu=lfy+ny*ny
      ludep=lfu+ny*(nuc+nuv)
      lytot=ludep+nu
      lf=lytot+ny*(nti+ntf)
      ldtvt=lf+ny
c
c     decoupage de itvt pour icse1
c
      lipv1=litvt
      mitv1=lipv1+ny-1
c
c     decoupage de itvt pour icse2
c
      litob=litvt
      lipv2=litob+ntob
      mitv2=lipv2+ny-1
c
      mitv=max(mitv1,mitv2)
c
c     decoupage de dtvt pour icse1
c
      ldm=ldtvt
      lyold=ldm+ny*ny
      lsmold=lyold+ny
      lyint=lsmold+ny
      lyerr=lyint+ny
      ldif1=lyerr+ny
      ldif2=ldif1+ny
      ldif3=ldif2+ny
      mdtv1=ldif3+ny-1
c
c     decoupage de dtvt pour icse2
c
      lytob=ldtvt
      lc2y=lytob+ny*ntob
      ly0u=lc2y+ny*ntob
      ldmy=ly0u+ny*nu
      lsmy=ldmy+ny*ny
      loldmu=lsmy+ny*ny
      ly=loldmu+ny*(nuc+nuv)
      loldp=ly+ny
      lp=loldp+ny
      lp0=lp+ny
      lgt=lp0+ny
      lyob=lgt+max(nuc+nuv,nui)
      ld=lyob+nob*ntob
      mdtv2=ld+nob-1
c
      mdtv=max(mdtv1,mdtv2)
      if (mitv.gt.nitv.or.mdtv.gt.ndtv) then
        if (nitv+ndtv.gt.0) write(*,8003) mitv,mdtv
        nitv=mitv
        ndtv=mdtv
        return
      endif
      do 10 i=1,nu
      dtv(ludep+i-1)=u(i)
      u(i)=dtv(lech+i-1)*u(i)
10    continue
c
c     etat initial dependant du controle
c
      if (iu(1).gt.0) then
        indi=1
        call icsei(indi,nui,u(lui),dtv(ly0),dtv(ly0u),
     &    itv(litu),dtv(ldtu),
     & t0,tf,dti,dtf,ermx,iu,nuc,nuv,ilin,nti,ntf,ny,nea,
     & itmx,nex,nob,ntob,ntobi,nitu,ndtu)
        if (indi.le.0) then
           ind=indi
           return
        endif
      endif
c
c appel de icse1
c but
c     icse1 resout les systemes dynamiques du type:
c        0=fi(t,y,u),i<=nea et dyj/dt=fj(t,y,u),nea<j<=ny pour t>=t0
c        et y(t0)=y0,en notant ny la dimension de l'etat y et
c        fk la keme composante de la fonction f
c algorithme
c
c     on procede a pas de temps constant:dt suivant deux echelles
c     apres p pas,on a obtenu y_p valeur de l'etat a l'instant t_p
c     soit y_p= (y_p(1),....,y_p(ny))
c     on veut calculer d(y_p)=y_p+1-y_p
c     on note dt=t_(p+1)-t_p et I la matrice diagonale d'ordre ny
c     dont les nea premiers coefficients diagonaux valent 0 et les
c     autres 1.
c
c     prediction:
c     I*d(0,y_p)=dt f(t_p,y_p,u)
c
c     correction:
c     apres q corrections,on approche l'egalite souhaitee:
c     (1/dt)I*d(q+1,y_p)=(1/2)[I*f(t_p,y_p,u)+f(t_p+1,y_p+d(q+1,y_p),u)]
c     par:
c     (1/dt)I*d(q+1,y_p)=(1/2)dfy(t_p,y_p,u)dqp+
c            (1/2)[I*f(t_p,y_p,u)+f(t_p+1,y_p+d(q+1,y_p),u)]
c        en notant dqp=d(q+1,y_p)-d(q,y_p)
c     soit par:
c        ((1/dt)I-(1/2)dfy(t_p,y_p,u))dqp=
c        (1/2)[I*f(t_p,y_p,u)+f(t_p+1,y_p+d(q,y_p),u)]-(1/dt)d(q,y_p)
c
c     on retient d(y_p)=d(q0,y_p),ou q0 est le premier entier non nul
c     tel que la norme l2 de:(1/2)[I*f(t_p,y_p,u)+
c                            f(t_p+1,y_p+d(q0,y_p),u)]-(1/dt)d(q0,y_p)
c     est inferieure a ermx
c     de plus le nombre des corrections ne doit pas depasser:
c     itmx
c
c remarque:
c     L'erreur de discretisation est en O(dt**2) dans cette methode
c     car l'erreur e commise a chaque pas dt est en (dt**3),et si
c     l'on prend dt'=(1/s)*dt,l'erreur e' commise a chaque pas dt'
c     est e'=(1/s**3)*e;alors pour atteindre tf=nt*dt,il faut
c     nt pas dt,d'ou l'erreur E_nt=nt*O(dt**3),et il faut s*nt pas dt'
c     d'ou l'erreur E'_nt=s*nt*e'=(1/s**2)*E_nt
c
c liste d'appel:
c     icse1(ind,nu,u,icsef,y0,ytot,f,b,fy,fu,ipv1,dm,yold,smold,yint,
c     yerr,dif1,dif2,dif3,itu,dtu,
c     t0,tf,dti,dtf,ermx,iu,nuc,nuv,ilin,nti,ntf,ny,nea,
c     itmx,nex,nob,ntob,ntobi,nitu,ndtu)
c     en entree:
c
c     ind,u,nu figurent dans la liste d'appel de icse.fortran
c
c     icsef    nom de subroutine appelee par icse1
c
c     y0         double precision (ny)
c                etat initial
c
c     b          double precision (ny)
c                terme constant dans le cas lineaire quadratique
c
c     en sortie:
c
c     ytot       double precision (ny,nti+ntf)
c                valeurs calculees de l'etat aux pas de temps
c
c     variables internes:
c
c     f          double precision (ny)
c                stockage d'un calcul inutile des seconds membres
c                (au cas ou l'on n'utiliserait pas indf)
c
c     fy         double precision (ny,ny)
c                stockage de la derivee des seconds membres
c                par rapport a l'etat
c
c     fu         double precision (ny,nuc+nuv)
c                stockage de la derivee des seconds membres
c                par rapport aux parametres
c
c     ipv1       entier (ny)
c                stockage du vecteur des indices des pivots donne par
c                dgefa a chaque factorisation du jacobien
c
c     dm         double precision (ny,ny)
c                matrices successives des systemes lineaires
c                donnant l'etat discretise
c                dm=(1/dt)I-(1/2)dfy
c
c     yold       double precision (ny)
c                valeurs calculees successives de l'etat
c
c     smold      double precision (ny)
c                stockage de I*f(yold)
c
c     yint       double precision (ny)
c                yint=yold+dif1,ou dif1 est l'ecart donne
c                par prediction
c
c     yerr       double precision (ny)
c                yerr=yold+dif3,ou dif3 est l'ecart donne
c                par correction
c
c     dif1       double precision (ny)
c                ecart donne par prediction
c
c     dif2       double precision (ny)
c                stockage des differences entre les ecarts
c                consecutifs et des erreurs apres correction
c
c     dif3       double precision (ny)
c                ecart donne par correction
c
c     itu        entier (nitu)
c                tableau de travail entier reserve a l'utilisateur
c
c     dtu        double precision (ndtu)
c                tableau de travail double precision reserve
c                a l'utilisateur
c
c     enfin:
c
c     kt         entier
c                indice de comptage des pas de temps
c
c     dt         double precision
c                pas de temps,egal a dti ou a dtf
c
c     dtinv      double precision
c                dtinv=1/dt
c
c     t          double precision
c                instant(a l'instant t on travaille sur [t-dt,t])
c
c     told       double precision
c                instant anterieur a t:told=t-dt
c
c     indf       entier
c                indicateur figurant dans la liste d'appel de icsef
c
c     it         entier
c                indice de comptage des corrections
c
c     err        double precision
c                norme l2 de dif2
c
      call icse1(ind,nu,u,icsef,dtv(ly0),dtv(lytot),dtv(lf),dtv(lb),
     &dtv(lfy),dtv(lfu),itv(lipv1),dtv(ldm),dtv(lyold),dtv(lsmold),
     &dtv(lyint),dtv(lyerr),dtv(ldif1),dtv(ldif2),dtv(ldif3),
     &itv(litu),dtv(ldtu),
     &t0,tf,dti,dtf,ermx,iu,nuc,nuv,ilin,nti,ntf,ny,nea,
     &itmx,nex,nob,ntob,ntobi,nitu,ndtu)
c
      if (ind.le.0) return
c
c appel de icse2
c but
c     icse2 calcule le gradient du cout en utilisant l'etat
c     adjoint du systeme discretise
c algorithme
c     On procede a pas de temps constant:dt suivant deux echelles
c     si nt est le nombre total de pas de temps,notons:
c     y_1,...,y_nt les valeurs de la variable d'etat y a chaque pas
c     p_1,...,p_nt l'etat adjoint discretise
c     avec pour tout l=1,...,nt
c     y_l=(y_l(1),...,y_l(ny)) et p_l=(p_l(1),...,p_l(ny))
c     c2 la fonction cout
c     Id la matrice identite d'ordre ny
c     I la matrice diagonale d'ordre ny dont les nea premiers
c       coefficients diagonaux valent 0 et les autres 1
c     (M)t la transposee de la matrice M
c     L'etat adjoint discretise est la solution du systeme:
c        dc2/dy_nt=(I-(dt/2)dfy(t_nt,y_nt,u))t*p_nt avec
c                  dt=t_nt-t_(nt-1)
c        dc2/dy_k+(Id+(dt/2)dfy(t_k,y_k,u))t*I*p_k+1=
c        (I-(dt2new)dfy(t_k,y_k,u))t*p_k pour k=1,...,nt-1
c        avec dc2/dy_l nul quand l n'est pas un indice d'instant de
c        mesure
c     A chaque pas,apres avoir calcule p_l,on calcule
c     la contribution au gradient au pas l+1:
c     (dt/2)(dfu(t_l,y_l,u)+dfu(t_l+1,y_l+1,u))t*p_(l+1)) avec
c     dt=t_(l+1)-t_l
c     qu'on ajoute pour obtenir finalement le gradient en prenant
c     si l=1 la contribution:
c     ((t_1-t0)/2)(dfu(t0,y0,u)+dfu(t_1,y_1,u))t*p_1.
c     L'etat adjoint n'est pas stocke.
c
c liste d'appel:
c     icse2(ind,u,nu,co,g,icsef,icsec2,icsei,y0,tob,obs,ob,ytot,f,b,
c     fy,fu,ipv2,itob,cof,ytob,c2y,y0u,dmy,smy,oldmu,y,oldp,p,p0,gt,
c     yob,d,itu,dtu,
c     t0,tf,dti,dtf,ermx,iu,nuc,nuv,ilin,nti,ntf,ny,nea,
c     itmx,nex,nob,ntob,ntobi,nitu,ndtu)
c     en entree:
c
c     ind,u,nu   figurent dans la liste d'appel de icse.fortran
c
c     icsef      nom de subroutine appelee par icse2
c
c     icsec2     nom de subroutine appelee par icse2
c
c     icsei      nom de subroutine appelee par icse2
c
c     y0         double precision (ny)
c                etat initial
c
c     tob        double precision (ntob)
c                instants de mesure
c
c     obs        double precision (nob,ny)
c                matrice d'observation
c                figure dans la liste d'appel de icsec2,qui calcule
c                le cout quadratique dans le cas d'un observateur
c                lineaire
c
c     ob         double precision (nex,ntob,nob)
c                mesures
c
c     ytot       double precision (ny,nti+ntf)
c                valeurs calculees de l'etat aux pas de temps
c
c     b          double precision (ny)
c                terme constant dans le cas lineaire quadratique
c
c     en sortie:
c
c     co,g       figurent dans la liste d'appel de icse.fortran
c
c     variables internes:
c
c     f          double precision (ny)
c                stockage d'un calcul inutile des seconds membres
c                (au cas ou l'on n'utiliserait pas indf)
c
c     fy         double precision (ny,ny)
c                stockage de la derivee des seconds membres
c                par rapport a l'etat
c
c     fu         double precision (ny,nuc+nuv)
c                stockage de la derivee des seconds membres
c                par rapport aux parametres
c
c     ipv2       entier (ny)
c                stockage du vecteur des indices des pivots donne par
c                dgefa a chaque factorisation du jacobien
c
c     itob       entier (ntob)
c                indices des instants de mesure
c
c     cof        double precision (nob,ntob)
c                coefficients de ponderation du cout
c
c     ytob       double precision (ny,ntob)
c                valeurs calculees de l'etat aux instants de mesure
c
c     y0u        double precision (ny,nui)
c                derivee de l'etat initial par rapport au controle
c
c     dmy        double precision (ny,ny)
c                matrices successives des systemes lineaires
c                donnant l'etat adjoint discretise
c                dmy=I-(dt/2)dfy
c
c     smy        double precision (ny,ny)
c                matrices successives conduisant aux seconds membres
c                des systemes lineaires de l'etat adjoint discretise
c
c     oldmu      double precision (ny,nuc+nuv)
c                stockage de df/du a l'instant posterieur
c
c     y          double precision (ny)
c                stockage de la valeur calculee de l'etat a un pas de
c                temps
c
c     oldp       double precision (ny)
c                stockage de la valeur calculee de l'etat adjoint au
c                pas de temps posterieur
c
c     p          double precision (ny)
c                stockage de la valeur calculee de l'etat adjoint a
c                un pas de temps
c
c     p0         double precision (ny)
c                etape dans le calcul des seconds membres des
c                systemes lineaires donnant l'etat adjoint dicretise
c
c     gt         double precision (nu)
c                stockage de la contribution au gradient a chaque
c                pas de temps
c
c     yob,d      figurent dans la liste d'appel de icsec2,qui calcule
c                le cout quadratique dans le cas d'un observateur
c                lineaire
c
c     itu        entier (nitu)
c                tableau de travail entier reserve a l'utlisateur
c
c     dtu        double precision (ndtu)
c                tableau de travail double precision reserve
c                a l'utilisateur
c
c     enfin:
c
c     kt         entier
c                indice de comptage des pas de temps
c
c     ktob       entier
c                indice de comptage des instants de mesure
c
c     dt         double precision
c                pas de temps,egal a dti ou a dtf
c
c     dt2        double precision
c                dt2=dt/2
c
c     dt2new      double precision
c                dt2 a l'instant posterieur
c
c     t          double precision
c                instant (a l'instant t on travaille sur [t,t+dt])
c
c     c2         double precision
c                stockage d'un calcul inutile du cout
c                (au cas ou l'on n'utiliserait pas indc)
c
c     indf       entier
c                indicateur figurant dans la liste d'appel de icsef
c
c     indi       entier
c                indicateur figurant dans la liste d'appel de icsei
c
c     nui        entier
c                nombre de parametres dont depend l'etat initial
c                (figure dans la liste d'appel de icsei)
c
      call icse2(ind,nu,u,co,g,icsef,icsec2,icsei,dtv(ly0),dtv(ltob),
     &dtv(lobs),dtv(lob),dtv(lytot),dtv(lf),dtv(lb),dtv(lfy),dtv(lfu),
     &itv(lipv2),itv(litob),dtv(lcof),dtv(lytob),dtv(lc2y),dtv(ly0u),
     &dtv(ldmy),dtv(lsmy),dtv(loldmu),dtv(ly),dtv(loldp),
     &dtv(lp),dtv(lp0),dtv(lgt),dtv(lyob),dtv(ld),itv(litu),dtv(ldtu),
     &t0,tf,dti,dtf,ermx,iu,nuc,nuv,ilin,nti,ntf,ny,nea,
     &itmx,nex,nob,ntob,ntobi,nitu,ndtu)
      do 20 i=1,nu
      g(i)=dtv(lech+i-1)*g(i)
      u(i)=dtv(ludep+i-1)
20    continue
      return
c
c format
c
 8003 format(1x,'icse : taille des tableaux itv,dtv insuffisante',/,
     +       8x,'valeurs minimales ',i6,2x,i6)
c
c fin
c
      end


!-------------------------------------------------------------------
! Fortran auxiliary module for Kinetic.jl
!-------------------------------------------------------------------

module Kinetic

    implicit none

    real(kind=8), parameter :: PI = 3.141592654d0
    integer, parameter :: MNUM = 6 !number of normal moments
    integer, parameter :: MTUM = 6 !number of tangential moments

    contains

    include "method_1d.f90"
    include "method_2d.f90"
    include "kfvs.f90"
    include "ugks.f90"
    include "kcu.f90"
    !include "fsm.f90"

end


      subroutine qng(f,a,b,epsabs,epsrel,result,abserr,neval,ier)
c***begin prologue  qng
c***date written   800101   (yymmdd)
c***revision date  830518   (yymmdd)
c***category no.  h2a1a1
c***keywords  automatic integrator, smooth integrand,
c             non-adaptive, gauss-kronrod(patterson)
c***author  piessens,robert,appl. math. & progr. div. - k.u.leuven
c           de doncker,elise,appl math & progr. div. - k.u.leuven
c           kahaner,david,nbs - modified (2/82)
c***purpose  the routine calculates an approximation result to a
c            given definite integral i = integral of f over (a,b),
c            hopefully satisfying following claim for accuracy
c            abs(i-result).le.max(epsabs,epsrel*abs(i)).
c***description
c
c non-adaptive integration
c standard fortran subroutine
c real version
c
c           f      - real version
c                    function subprogram defining the integrand function
c                    f(x). the actual name for f needs to be declared
c                    e x t e r n a l in the driver program.
c
c           a      - real version
c                    lower limit of integration
c
c           b      - real version
c                    upper limit of integration
c
c           epsabs - real
c                    absolute accuracy requested
c           epsrel - real
c                    relative accuracy requested
c                    if  epsabs.le.0
c                    and epsrel.lt.max(50*rel.mach.acc.,0.5d-28),
c                    the routine will end with ier = 6.
c
c         on return
c           result - real
c                    approximation to the integral i
c                    result is obtained by applying the 21-point
c                    gauss-kronrod rule (res21) obtained by optimal
c                    addition of abscissae to the 10-point gauss rule
c                    (res10), or by applying the 43-point rule (res43)
c                    obtained by optimal addition of abscissae to the
c                    21-point gauss-kronrod rule, or by applying the
c                    87-point rule (res87) obtained by optimal addition
c                    of abscissae to the 43-point rule.
c
c           abserr - real
c                    estimate of the modulus of the absolute error,
c                    which should equal or exceed abs(i-result)
c
c           neval  - integer
c                    number of integrand evaluations
c
c           ier    - ier = 0 normal and reliable termination of the
c                            routine. it is assumed that the requested
c                            accuracy has been achieved.
c                    ier.gt.0 abnormal termination of the routine. it is
c                            assumed that the requested accuracy has
c                            not been achieved.
c           error messages
c                    ier = 1 the maximum number of steps has been
c                            executed. the integral is probably too
c                            difficult to be calculated by dqng.
c                        = 6 the input is invalid, because
c                            epsabs.le.0 and
c                            epsrel.lt.max(50*rel.mach.acc.,0.5d-28).
c                            result, abserr and neval are set to zero.
c
c***references  (none)
c***routines called  r1mach,xerror
c***end prologue  qng
c
      real a,absc,abserr,b,centr,dhlgth,epmach,epsabs,epsrel,f,fcentr,
     *  fval,fval1,fval2,fv1,fv2,fv3,fv4,hlgth,result,res10,res21,res43,
     *  res87,resabs,resasc,reskh,r1mach,savfun,uflow,w10,w21a,w43a,
     *  w43b,w87a,w87b,x1,x2,x3,x4
      integer ier,ipx,k,l,neval
      external f
c
      dimension fv1(5),fv2(5),fv3(5),fv4(5),x1(5),x2(5),x3(11),x4(22),
     *  w10(5),w21a(5),w21b(6),w43a(10),w43b(12),w87a(21),w87b(23),
     *  savfun(21)
c
c           the following data statements contain the
c           abscissae and weights of the integration rules used.
c
c           x1      abscissae common to the 10-, 21-, 43-
c                   and 87-point rule
c           x2      abscissae common to the 21-, 43- and
c                   87-point rule
c           x3      abscissae common to the 43- and 87-point
c                   rule
c           x4      abscissae of the 87-point rule
c           w10     weights of the 10-point formula
c           w21a    weights of the 21-point formula for
c                   abscissae x1
c           w21b    weights of the 21-point formula for
c                   abscissae x2
c           w43a    weights of the 43-point formula for
c                   abscissae x1, x3
c           w43b    weights of the 43-point formula for
c                   abscissae x3
c           w87a    weights of the 87-point formula for
c                   abscissae x1, x2, x3
c           w87b    weights of the 87-point formula for
c                   abscissae x4
c
      data x1(1),x1(2),x1(3),x1(4),x1(5)/
     *     0.9739065285171717e+00,     0.8650633666889845e+00,
     *     0.6794095682990244e+00,     0.4333953941292472e+00,
     *     0.1488743389816312e+00/
      data x2(1),x2(2),x2(3),x2(4),x2(5)/
     *     0.9956571630258081e+00,     0.9301574913557082e+00,
     *     0.7808177265864169e+00,     0.5627571346686047e+00,
     *     0.2943928627014602e+00/
      data x3(1),x3(2),x3(3),x3(4),x3(5),x3(6),x3(7),x3(8),
     *  x3(9),x3(10),x3(11)/
     *     0.9993333609019321e+00,     0.9874334029080889e+00,
     *     0.9548079348142663e+00,     0.9001486957483283e+00,
     *     0.8251983149831142e+00,     0.7321483889893050e+00,
     *     0.6228479705377252e+00,     0.4994795740710565e+00,
     *     0.3649016613465808e+00,     0.2222549197766013e+00,
     *     0.7465061746138332e-01/
      data x4(1),x4(2),x4(3),x4(4),x4(5),x4(6),x4(7),x4(8),x4(9),
     *  x4(10),x4(11),x4(12),x4(13),x4(14),x4(15),x4(16),x4(17),x4(18),
     *  x4(19),x4(20),x4(21),x4(22)/   0.9999029772627292e+00,
     *     0.9979898959866787e+00,     0.9921754978606872e+00,
     *     0.9813581635727128e+00,     0.9650576238583846e+00,
     *     0.9431676131336706e+00,     0.9158064146855072e+00,
     *     0.8832216577713165e+00,     0.8457107484624157e+00,
     *     0.8035576580352310e+00,     0.7570057306854956e+00,
     *     0.7062732097873218e+00,     0.6515894665011779e+00,
     *     0.5932233740579611e+00,     0.5314936059708319e+00,
     *     0.4667636230420228e+00,     0.3994248478592188e+00,
     *     0.3298748771061883e+00,     0.2585035592021616e+00,
     *     0.1856953965683467e+00,     0.1118422131799075e+00,
     *     0.3735212339461987e-01/
      data w10(1),w10(2),w10(3),w10(4),w10(5)/
     *     0.6667134430868814e-01,     0.1494513491505806e+00,
     *     0.2190863625159820e+00,     0.2692667193099964e+00,
     *     0.2955242247147529e+00/
      data w21a(1),w21a(2),w21a(3),w21a(4),w21a(5)/
     *     0.3255816230796473e-01,     0.7503967481091995e-01,
     *     0.1093871588022976e+00,     0.1347092173114733e+00,
     *     0.1477391049013385e+00/
      data w21b(1),w21b(2),w21b(3),w21b(4),w21b(5),w21b(6)/
     *     0.1169463886737187e-01,     0.5475589657435200e-01,
     *     0.9312545458369761e-01,     0.1234919762620659e+00,
     *     0.1427759385770601e+00,     0.1494455540029169e+00/
      data w43a(1),w43a(2),w43a(3),w43a(4),w43a(5),w43a(6),w43a(7),
     *  w43a(8),w43a(9),w43a(10)/      0.1629673428966656e-01,
     *     0.3752287612086950e-01,     0.5469490205825544e-01,
     *     0.6735541460947809e-01,     0.7387019963239395e-01,
     *     0.5768556059769796e-02,     0.2737189059324884e-01,
     *     0.4656082691042883e-01,     0.6174499520144256e-01,
     *     0.7138726726869340e-01/
      data w43b(1),w43b(2),w43b(3),w43b(4),w43b(5),w43b(6),
     *  w43b(7),w43b(8),w43b(9),w43b(10),w43b(11),w43b(12)/
     *     0.1844477640212414e-02,     0.1079868958589165e-01,
     *     0.2189536386779543e-01,     0.3259746397534569e-01,
     *     0.4216313793519181e-01,     0.5074193960018458e-01,
     *     0.5837939554261925e-01,     0.6474640495144589e-01,
     *     0.6956619791235648e-01,     0.7282444147183321e-01,
     *     0.7450775101417512e-01,     0.7472214751740301e-01/
      data w87a(1),w87a(2),w87a(3),w87a(4),w87a(5),w87a(6),
     *  w87a(7),w87a(8),w87a(9),w87a(10),w87a(11),w87a(12),
     *  w87a(13),w87a(14),w87a(15),w87a(16),w87a(17),w87a(18),
     *  w87a(19),w87a(20),w87a(21)/
     *     0.8148377384149173e-02,     0.1876143820156282e-01,
     *     0.2734745105005229e-01,     0.3367770731163793e-01,
     *     0.3693509982042791e-01,     0.2884872430211531e-02,
     *     0.1368594602271270e-01,     0.2328041350288831e-01,
     *     0.3087249761171336e-01,     0.3569363363941877e-01,
     *     0.9152833452022414e-03,     0.5399280219300471e-02,
     *     0.1094767960111893e-01,     0.1629873169678734e-01,
     *     0.2108156888920384e-01,     0.2537096976925383e-01,
     *     0.2918969775647575e-01,     0.3237320246720279e-01,
     *     0.3478309895036514e-01,     0.3641222073135179e-01,
     *     0.3725387550304771e-01/
      data w87b(1),w87b(2),w87b(3),w87b(4),w87b(5),w87b(6),w87b(7),
     *  w87b(8),w87b(9),w87b(10),w87b(11),w87b(12),w87b(13),w87b(14),
     *  w87b(15),w87b(16),w87b(17),w87b(18),w87b(19),w87b(20),
     *  w87b(21),w87b(22),w87b(23)/    0.2741455637620724e-03,
     *     0.1807124155057943e-02,     0.4096869282759165e-02,
     *     0.6758290051847379e-02,     0.9549957672201647e-02,
     *     0.1232944765224485e-01,     0.1501044734638895e-01,
     *     0.1754896798624319e-01,     0.1993803778644089e-01,
     *     0.2219493596101229e-01,     0.2433914712600081e-01,
     *     0.2637450541483921e-01,     0.2828691078877120e-01,
     *     0.3005258112809270e-01,     0.3164675137143993e-01,
     *     0.3305041341997850e-01,     0.3425509970422606e-01,
     *     0.3526241266015668e-01,     0.3607698962288870e-01,
     *     0.3669860449845609e-01,     0.3712054926983258e-01,
     *     0.3733422875193504e-01,     0.3736107376267902e-01/
c
c           list of major variables
c           -----------------------
c
c           centr  - mid point of the integration interval
c           hlgth  - half-length of the integration interval
c           fcentr - function value at mid point
c           absc   - abscissa
c           fval   - function value
c           savfun - array of function values which
c                    have already been computed
c           res10  - 10-point gauss result
c           res21  - 21-point kronrod result
c           res43  - 43-point result
c           res87  - 87-point result
c           resabs - approximation to the integral of abs(f)
c           resasc - approximation to the integral of abs(f-i/(b-a))
c
c           machine dependent constants
c           ---------------------------
c
c           epmach is the largest relative spacing.
c           uflow is the smallest positive magnitude.
c
c***first executable statement  qng
      epmach = r1mach(4)
      uflow = r1mach(1)
c
c           test on validity of parameters
c           ------------------------------
c
      result = 0.0e+00
      abserr = 0.0e+00
      neval = 0
      ier = 6
      if(epsabs.le.0.0e+00.and.epsrel.lt.amax1(0.5e-14,0.5e+02*epmach))
     *  go to 80
      hlgth = 0.5e+00*(b-a)
      dhlgth = abs(hlgth)
      centr = 0.5e+00*(b+a)
      fcentr = f(centr)
      neval = 21
      ier = 1
c
c          compute the integral using the 10- and 21-point formula.
c
      do 70 l = 1,3
      go to (5,25,45),l
    5 res10 = 0.0e+00
      res21 = w21b(6)*fcentr
      resabs = w21b(6)*abs(fcentr)
      do 10 k=1,5
        absc = hlgth*x1(k)
        fval1 = f(centr+absc)
        fval2 = f(centr-absc)
        fval = fval1+fval2
        res10 = res10+w10(k)*fval
        res21 = res21+w21a(k)*fval
        resabs = resabs+w21a(k)*(abs(fval1)+abs(fval2))
        savfun(k) = fval
        fv1(k) = fval1
        fv2(k) = fval2
   10 continue
      ipx = 5
      do 15 k=1,5
        ipx = ipx+1
        absc = hlgth*x2(k)
        fval1 = f(centr+absc)
        fval2 = f(centr-absc)
        fval = fval1+fval2
        res21 = res21+w21b(k)*fval
        resabs = resabs+w21b(k)*(abs(fval1)+abs(fval2))
        savfun(ipx) = fval
        fv3(k) = fval1
        fv4(k) = fval2
   15 continue
c
c          test for convergence.
c
      result = res21*hlgth
      resabs = resabs*dhlgth
      reskh = 0.5e+00*res21
      resasc = w21b(6)*abs(fcentr-reskh)
      do 20 k = 1,5
        resasc = resasc+w21a(k)*(abs(fv1(k)-reskh)+abs(fv2(k)-reskh))
     *                  +w21b(k)*(abs(fv3(k)-reskh)+abs(fv4(k)-reskh))
   20 continue
      abserr = abs((res21-res10)*hlgth)
      resasc = resasc*dhlgth
      go to 65
c
c          compute the integral using the 43-point formula.
c
   25 res43 = w43b(12)*fcentr
      neval = 43
      do 30 k=1,10
        res43 = res43+savfun(k)*w43a(k)
   30 continue
      do 40 k=1,11
        ipx = ipx+1
        absc = hlgth*x3(k)
        fval = f(absc+centr)+f(centr-absc)
        res43 = res43+fval*w43b(k)
        savfun(ipx) = fval
   40 continue
c
c          test for convergence.
c
      result = res43*hlgth
      abserr = abs((res43-res21)*hlgth)
      go to 65
c
c          compute the integral using the 87-point formula.
c
   45 res87 = w87b(23)*fcentr
      neval = 87
      do 50 k=1,21
        res87 = res87+savfun(k)*w87a(k)
   50 continue
      do 60 k=1,22
        absc = hlgth*x4(k)
        res87 = res87+w87b(k)*(f(absc+centr)+f(centr-absc))
   60 continue
      result = res87*hlgth
      abserr = abs((res87-res43)*hlgth)
   65 if(resasc.ne.0.0e+00.and.abserr.ne.0.0e+00)
     *  abserr = resasc*amin1(0.1e+01,
     *  (0.2e+03*abserr/resasc)**1.5e+00)
      if (resabs.gt.uflow/(0.5e+02*epmach)) abserr = amax1
     *  ((epmach*0.5e+02)*resabs,abserr)
      if (abserr.le.amax1(epsabs,epsrel*abs(result))) ier = 0
c ***jump out of do-loop
      if (ier.eq.0) go to 999
   70 continue
   80 call xerror(26habnormal return from  qng ,26,ier,0)
  999 return
      end


MODULE m_orb_estimator

! ----------------------------------------------------------------------
! MODULE: m_orb_estimator
! ----------------------------------------------------------------------
! Purpose:
!  Module for calling the following subroutines
!
! Subroutines contained within the module:
! - orb_estimator
! ----------------------------------------------------------------------
! Author :	Dr. Thomas Papanikolaou
!			Cooperative Research Centre for Spatial Information, Australia
! Created:	19 March 2018
! ----------------------------------------------------------------------

      IMPLICIT NONE
      !SAVE

Contains

SUBROUTINE orb_estimator(orbref, veqZarray, veqParray, orbobs, Xmatrix, Wmatrix, Amatrix, Vmatrix, Xsigma)

! ----------------------------------------------------------------------
! SUBROUTINE: orb_estimator
! ----------------------------------------------------------------------
! Purpose:
!  Orbit parameter estimation based on least-squares method and pseudo-observations
! ----------------------------------------------------------------------
! Input arguments:
! - obs_orb:	Pseudo-observations matrix with dimensions N1x5; obs_orb(ti)=[MJD_ti Sec_ti X Y Z]
! - orb_dyn:	Orbit computed from numerical integration solution (dimensions N2x8)
!				orb_dyn(ti)=[MJD_ti Sec_ti X Y Z Vx Vy Vz]
!
! Output arguments:
! - Xmatrix: 	Optimum estimated parameters corrections (initial state vector + additional parameters)
! - Wmatrix:    Observed-Computed matrix
! - Amatrix:    Design matrix
! - Vmatrix:    Residual matrix
! - Xsigma :    Unknow parameter sigma matrix
! ----------------------------------------------------------------------
! Dr. Thomas Papanikolaou, Geoscience Australia            19 March 2018
! ----------------------------------------------------------------------
!
! Changes: 22-10-2020 Tzupang Tseng: Parameter correlation matrix, including the initial state vector and SRP parameters
!          09-03-2021 Tzupang Tseng: Minimum constraint on stochastic pulse parameter
!

      USE mdl_precision
      USE mdl_num
      USE m_matrixinv
      USE mdl_param
      IMPLICIT NONE

! ----------------------------------------------------------------------
! Dummy arguments declaration
! ----------------------------------------------------------------------
! IN
      REAL (KIND = prec_d), INTENT(IN), DIMENSION(:,:), ALLOCATABLE :: orbref, veqZarray, veqParray, orbobs
! ----------------------------------------------------------------------
! OUT
      REAL (KIND = prec_d), DIMENSION(:,:), ALLOCATABLE, INTENT(OUT) :: Xmatrix
      REAL (KIND = prec_d), DIMENSION(:,:), ALLOCATABLE, INTENT(OUT) :: Wmatrix
      REAL (KIND = prec_d), DIMENSION(:,:), ALLOCATABLE, INTENT(OUT) :: Amatrix
      REAL (KIND = prec_d), DIMENSION(:,:), ALLOCATABLE, INTENT(OUT) :: Vmatrix
      REAL (KIND = prec_d), DIMENSION(:,:), ALLOCATABLE, INTENT(OUT) :: Xsigma
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Local variables declaration
! ----------------------------------------------------------------------
      INTEGER (KIND = prec_int8) :: sz1, sz2, sz3, sz4, sz5, sz6, sz7, sz8
      INTEGER (KIND = prec_int8) :: Norb, Nobs, Nparam, n_NEQn
      INTEGER (KIND = prec_int8) :: Nepochs, Ncommon, iobs0, iobs, iref, iepoch, jepoch
      INTEGER (KIND = prec_int2) :: Nobsset
      INTEGER (KIND = prec_int2) :: i, j, k
      REAL (KIND = prec_d) :: tiref, tiobs, dt_limit
      REAL (KIND = prec_d), DIMENSION(:,:), ALLOCATABLE :: AmatrixZ
      REAL (KIND = prec_d), DIMENSION(:,:), ALLOCATABLE :: AmatrixP
      REAL (KIND = prec_d), DIMENSION(:,:), ALLOCATABLE :: Amatrix_T
      REAL (KIND = prec_d), DIMENSION(:,:), ALLOCATABLE :: Vmatrix_T
      REAL (KIND = prec_d), DIMENSION(:,:), ALLOCATABLE :: NEQn, NEQu, NEQn_inv
!      REAL (KIND = prec_d), DIMENSION(:,:), ALLOCATABLE :: WP
      REAL (KIND = prec_d), DIMENSION(:,:), ALLOCATABLE :: ObsEpochs
      INTEGER (KIND = prec_int2) :: AllocateStatus, DeAllocateStatus
! ----------------------------------------------------------------------
      INTEGER (KIND = prec_int8) :: An
      !REAL (KIND = prec_q) :: Nmatrix(An,An)
      !REAL (KIND = prec_q) :: Nmatrix_inv(An,An)
      REAL (KIND = prec_q) :: Nmatrix(3,3)
      REAL (KIND = prec_q) :: Nmatrix_inv(3,3)
      REAL (KIND = prec_q) :: sigma, VTV(1,1)
!      REAL (KIND = prec_q) :: pulse_sig

! ----------------------------------------------------------------------
sz1 = size(orbobs, DIM = 1)
sz2 = size(orbobs, DIM = 2)
Nobs = sz1

sz3 = size(orbref, DIM = 1)
sz4 = size(orbref, DIM = 2)
Norb = sz3

sz5 = size(veqZarray, DIM = 1)
sz6 = size(veqZarray, DIM = 2)

!sz7 = size(veqParray, DIM = 1)
!sz8 = size(veqParray, DIM = 2)
!Nparam = (sz8 - 2) / 6
Nparam = NPARAM_glb
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Number of common epochs
! Set the numerical differences digits limit for the common epochs
dt_limit = 1.D-08

Nepochs = 0
iref = 0
iobs = 0
iobs0 = 1
Do iref = 1 , Norb
    ! Reference Orbit
    tiref = orbref(iref,1)
    ! Pseduo-Observations (sp3 Orbit)
    Do iobs = iobs0 , Nobs
        tiobs = orbobs(iobs,1)
        ! Common Epochs
        If ( ABS(tiref - tiobs) < dt_limit) Then
            Nepochs = Nepochs + 1
            iobs0 = iobs + 1
        end IF
    end Do
end Do
Ncommon = Nepochs
! ----------------------------------------------------------------------
!print *,"Ncommon", Ncommon

! ----------------------------------------------------------------------
! Dynamic Allocatable arrays
! Initialise dyn memory
! TODO: check the alloc status for each of the below
ALLOCATE (Xmatrix(6+Nparam,1), STAT = AllocateStatus)
ALLOCATE (Wmatrix(3*Ncommon,1), STAT = AllocateStatus)
ALLOCATE (Vmatrix(3*Ncommon,1), STAT = AllocateStatus)
ALLOCATE (Vmatrix_T(1,3*Ncommon), STAT = AllocateStatus)
!ALLOCATE (WP(6+Nparam,6+Nparam), STAT = AllocateStatus)

ALLOCATE (Amatrix(3*Ncommon,6+Nparam), STAT = AllocateStatus)
ALLOCATE (AmatrixZ(3*Ncommon,6), STAT = AllocateStatus)
AmatrixZ = 0.d0
ALLOCATE (AmatrixP(3*Ncommon,Nparam), STAT = AllocateStatus)
AmatrixP = 0.d0
ALLOCATE (Amatrix_T(6+Nparam,3*Ncommon), STAT = AllocateStatus)

ALLOCATE (NEQn(6+Nparam,6+Nparam), STAT = AllocateStatus)
ALLOCATE (NEQu(6+Nparam,1), STAT = AllocateStatus)
ALLOCATE (NEQn_inv(6+Nparam,6+Nparam), STAT = AllocateStatus)

ALLOCATE (corrl(6+Nparam,6+Nparam), STAT = AllocateStatus)
ALLOCATE (Xsigma(6+Nparam,6+Nparam), STAT = AllocateStatus)

ALLOCATE (ObsEpochs(Ncommon,2), STAT = AllocateStatus)
ObsEpochs = 0.d0
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
iobs0 = 1
Nepochs = 0
iref = 0
iobs = 0
Do iref = 1 , Norb
    ! Reference Orbit
    tiref = orbref(iref,1)
    ! Pseduo-Observations (sp3 Orbit)
    Do iobs = iobs0 , Nobs
        tiobs = orbobs(iobs,1)
        ! Common Epochs
        If ( ABS(tiref - tiobs) < dt_limit) Then
            Nepochs = Nepochs + 1
            ObsEpochs(Nepochs,1) = orbref(iref,1)
            ObsEpochs(Nepochs,2) = orbref(iref,2)
! ----------------------------------------------------------------------
			Nobsset = 3
			! Wmatrix
            Wmatrix( (Nepochs-1)*3+1 : Nepochs*3 , 1) = orbobs(iobs,3:5) - orbref(iref,3:5)
            ! Design Matrix values obtained by VEQ arrays values
            ! veqZarray
			! dX/dZo, dY/dZo, dZ/dZo
			AmatrixZ((Nepochs-1)*Nobsset + 1,:) = veqZarray(iref,3:8)
			AmatrixZ((Nepochs-1)*Nobsset + 2,:) = veqZarray(iref,9:14)
			AmatrixZ((Nepochs-1)*Nobsset + 3,:) = veqZarray(iref,15:20)
			If (Nparam /= 0) Then
			! veqParray
!            AmatrixP((Nepochs-1)*Nobsset+1, : ) = ...
            AmatrixP((Nepochs-1)*Nobsset + 1, : ) = veqParray(iref,3:Nparam+2)
            AmatrixP((Nepochs-1)*Nobsset + 2, : ) = veqParray(iref,  Nparam+3:2*Nparam+2)
            AmatrixP((Nepochs-1)*Nobsset + 3, : ) = veqParray(iref,2*Nparam+3:3*Nparam+2)
! ----------------------------------------------------------------------
			! Design Matrix
			!Amatrix((Nepochs-1)*Nobsset + 1,1:6) = AmatrixZ((Nepochs-1)*Nobsset + 1,:)
			!Amatrix((Nepochs-1)*Nobsset + 1,7:6+Nparam) = AmatrixP((Nepochs-1)*Nobsset + 1, : )

			!Amatrix((Nepochs-1)*Nobsset + 2,1:6) = AmatrixZ((Nepochs-1)*Nobsset + 2,:)
			!Amatrix((Nepochs-1)*Nobsset + 2,7:6+Nparam) = AmatrixP((Nepochs-1)*Nobsset + 2, : )

			!Amatrix((Nepochs-1)*Nobsset + 3,1:6) = AmatrixZ((Nepochs-1)*Nobsset + 3,:)
			!Amatrix((Nepochs-1)*Nobsset + 3,7:6+Nparam) = AmatrixP((Nepochs-1)*Nobsset + 3, : )
			!Else
			!Amatrix = AmatrixZ
			End If
! ----------------------------------------------------------------------
            iobs0 = iobs + 1
        end IF
    end Do
end Do

! ----------------------------------------------------------------------
! Design matrix
! Amatrix summarized
!Amatrix = [AmatrixZ AmatrixP]
!Amatrix = AmatrixZ
!sz7 = size(AmatrixZ, DIM = 1)
!sz8 = size(AmatrixZ, DIM = 2)
!print *,"AmatrixZ DIM",sz7,sz8
!sz7 = size(AmatrixP, DIM = 1)
!sz8 = size(AmatrixP, DIM = 2)
!print *,"AmatrixP DIM",sz7,sz8

!print *,"Nparam",Nparam
!print *,"Ncommon",Ncommon,Nepochs

if(1<0) then
If (Nparam /= 0) Then
Do iepoch = 1 , 3*Ncommon
	Do jepoch = 1 , 6
	Amatrix (iepoch,jepoch) = AmatrixZ(iepoch,jepoch)
	End Do
	Do jepoch = 1 , Nparam
	Amatrix (iepoch,jepoch+6) = AmatrixP(iepoch,jepoch)
	End Do
End Do
Else
	Amatrix = AmatrixZ
End IF
end if

If (Nparam /= 0) Then
Do jepoch = 1 , 6
Do iepoch = 1 , 3*Ncommon
	Amatrix (iepoch,jepoch) = AmatrixZ(iepoch,jepoch)
End Do
End Do

Do jepoch = 1 , Nparam
Do iepoch = 1 , 3*Ncommon
	Amatrix (iepoch,jepoch+6) = AmatrixP(iepoch,jepoch)
End Do
End Do
Else
	Amatrix = AmatrixZ
End IF
! ----------------------------------------------------------------------

! ----------------------------------------------------------------------
! Normal Equations
Amatrix_T = TRANSPOSE(Amatrix)
NEQn = MATMUL(Amatrix_T , Amatrix)
NEQu = MATMUL(Amatrix_T , Wmatrix)

! N matrix inversion
!NEQn_inv = inv(NEQn)
n_NEQn = size(Amatrix, DIM = 2)

! put the weight on pulse parameters
!(09-Mar-2021 Tzupang Tseng: constrain the pulses close to zero)
!WP =0.d0
!pulse_sig = 1d-8
!do i =1, 6+Nparam
!      if (i > 6+EMPNUM+ECOMNUM)then
!             WP(i,i)=(1.d0/pulse_sig)**2
!      end if
!end do
!NEQn = NEQn+WP
! --------------------------------------------------------------------------------------
Call matrixinv (NEQn, NEQn_inv, n_NEQn)
!Print *,"NEQn", NEQn
!Print *,"NEQn_inv", NEQn_inv

! NEQ matrices test
! NEQ : I test

! ----------------------------------------------------------------------
! Parameters Estimation
Xmatrix = MATMUL(NEQn_inv , NEQu)
! ----------------------------------------------------------------------

! Residuals of psudo observations
Vmatrix = MATMUL(Amatrix , Xmatrix) - Wmatrix
Vmatrix_T = TRANSPOSE(Vmatrix)

! Posteriori Sigma
VTV   = MATMUL(Vmatrix_T, Vmatrix)
sigma = VTV(1,1)/(Nobs-6-Nparam)

!PRINT*,'Posteriori sigma =', SQRT(sigma)

! Sigma of unknow parameters
Xsigma = SQRT(sigma*NEQn_inv)

!PRINT*,'Xsigma =', Xsigma(7,7), Xsigma(8,8),Xsigma(9,9)

! Parameter correlation matrix(22-10-2020 Tzupang Tseng)
DO i = 1, 6+Nparam
   DO j = 1, 6+Nparam
      corrl(i,j)=NEQn_inv(i,j)/sqrt(NEQn_inv(i,i))/sqrt(NEQn_inv(j,j))
   END DO
END DO

END SUBROUTINE

END Module


module fermi_dirac_module

  implicit none

  private
  public :: fermi_dirac
  public :: entropy_fd

contains

  subroutine fermi_dirac( occ,Efermi,esp,ekbt,ZNel,wbz,dspn )
    implicit none
    real(8),intent(inout) :: occ(:,:,:)
    real(8),intent(out) :: Efermi
    real(8),intent(in) :: esp(:,:,:)
    real(8),intent(in) :: ekbt, ZNel
    real(8),intent(in) :: wbz(:)
    real(8),optional,intent(in) :: dspn
    real(8) :: ef1,ef2,ef
    integer :: ib,id,n,k,s,efconv
    real(8) :: ctime0,ctime1,etime0,etime1
    real(8) :: zne,octmp,ff,xx,ff0
    real(8),parameter :: eps=1.0d-10, big=36.9d0
    integer,parameter :: mxcycl=100
    integer,parameter :: kinteg=-1
    integer :: MB, MBZ, nspin, mb1, mb2
    logical :: flag, disp_switch
    real(8) :: diff

    call write_border( 1, "fermi_dirac(start)" )
    call check_disp_switch( disp_switch, 0 )

    MB = size( occ, 1 )
    MBZ = size( occ, 2 )
    nspin = size( occ, 3 )

    flag=.false.
    if ( present(dspn) ) then
       if ( nspin == 2 ) flag=.true.
    end if

    diff=abs(2*MB-ZNel)
    if ( diff < eps ) then
       occ(1:MB,1:MBZ,1:nspin)=1.0d0
       ef=maxval(esp)
       goto 100
    end if

    mb2=MB
    loop_mb1: do mb1 = 1, MB

! Set upper & lower boundarires of Fermi energy

       ef1=minval( esp(mb1:mb2,1:MBZ,1:nspin) )
       ef2=maxval( esp(mb1:mb2,1:MBZ,1:nspin) )
       if ( ef1==ef2 ) then
          ef1=ef1-0.01d0
          ef2=ef2+0.01d0
       end if

! Safety margin for highly degenerate systems & artificial fault

       ef2 = ef2 + min( ekbt*1.0d2, 0.1d0 )

       if ( .not.flag ) then
          zne=ZNel-2.0d0*(mb1-1)
          do id=1,mxcycl
             ef=0.5d0*(ef1+ef2)
             octmp=0.0d0
             do ib=mb1,mb2
             do s=1,nspin
             do k=1,MBZ
                select case (kinteg)
                !case default
                !   xx=(esp(ib,k,s)-ef)/ekbt
                !   ff=ff0(kinteg,xx)
                case (-1)
                   if ( ekbt<eps ) then
                      if ( (esp(ib,k,s)-ef )>0.0d0 ) then
                         ff=0.0d0
                      else if( ( esp(ib,k,s)-ef)<0.0d0 ) then
                         ff=1.0d0
                      else
                         ff=0.5d0
                      end if
                   else
                      xx=( esp(ib,k,s)-ef )/ekbt
                      if ( xx>big ) then
                         ff=0.0d0
                      else if ( xx<-big ) then
                         ff=1.0d0
                      else
                         ff=1.0d0/(exp(xx)+1.0d0)
                      end if
                   end if
                end select
                octmp=octmp+ff*wbz(k)*2.0d0/dble(nspin)
                occ(ib,k,s)=ff
             end do !k
             end do !s
             end do !ib
             if ( octmp-zne>eps ) then
                ef2=ef
             else if ( octmp-zne<-eps ) then
                ef1=ef
             else
                goto 100
             end if
          end do !id

       else !flag

          efconv=0
          if (disp_switch) write(*,*) "total spin density is fixed!!"
          do s=1,nspin
             ef1=minval( esp(mb1:mb2,1:MBZ,s) )
             ef2=maxval( esp(mb1:mb2,1:MBZ,s) )
             if ( ef1==ef2 ) then
                ef1=ef1-0.01d0
                ef2=ef2+0.01d0
             end if
             ef2 = ef2 + min( ekbt*1.d2, 0.1d0 )
             zne=(ZNel+dble(3-2*s)*dspn)/2.0d0-dble(mb1-1)
             do id=1,mxcycl
                ef=0.5d0*(ef1+ef2)
                octmp=0.0d0
                do ib=mb1,mb2
                do k=1,MBZ
                   select case(kinteg)
                   !case default
                   !   xx=(esp(ib,k,s)-ef)/ekbt
                   !   ff=ff0(kinteg,xx)
                   case(-1)
                      if ( ekbt<eps ) then
                         if ( (esp(ib,k,s)-ef )>0.0d0 ) then
                            ff=0.0d0
                         else if( ( esp(ib,k,s)-ef)<0.0d0 ) then
                            ff=1.0d0
                         else
                            ff=0.5d0
                         end if
                      else
                         xx=( esp(ib,k,s)-ef )/ekbt
                         if ( xx>big ) then
                            ff=0.0d0
                         else if ( xx<-big ) then
                            ff=1.0d0
                         else
                            ff=1.0d0/(exp(xx)+1.0d0)
                         end if
                      end if
                   end select
                   octmp=octmp+ff*wbz(k)
                   occ(ib,k,s)=ff
                end do !k
                end do !ib
                if ( octmp-zne>eps ) then
                   ef2=ef
                else if ( octmp-zne<-eps ) then
                   ef1=ef
                else
                   efconv=efconv+1
                   exit
                end if
             end do !id
          end do !s
          if ( efconv==2 ) goto 100

       end if !flag

       !if (DISP_SWITCH) then
       !   write(6,*)' EF IS NOT CONVERGED'
       !   write(6,*)' Check the # of electron, mb1, and mb2'
       !   write(6,*)' EF1 & EF2=',ef1,ef2
       !   write(6,*)' octmp,zne=',octmp,zne
       !   do s=1,nspin
       !   do k=1,MBZ
       !      write(*,*) "s,k =",s,k
       !      do ib=mb1,mb2
       !         write(*,*) ib,occ(ib,k,s),esp(ib,k,s)
       !      end do
       !   end do
       !   end do
       !end if

    end do loop_mb1

    call stop_program('FERMI(1)')

100 continue

    do s=1,nspin
    do k=1,MBZ
       do ib=1,mb1-1
          occ(ib,k,s)=2.0d0*wbz(k)/dble(nspin)
       end do
       do ib=mb1,mb2
          occ(ib,k,s)=2.0d0*occ(ib,k,s)*wbz(k)/dble(nspin)
       end do
       do ib=mb2+1,MB
          occ(ib,k,s)=0.0d0
       end do
    end do
    end do

    octmp=sum(occ(1:MB,1:MBZ,1:nspin))
    octmp=ZNel-octmp
!      if ( abs(octmp)/=0.d0 ) then
!         loop1 : do ib=mb2,mb1,-1
!            do s=1,nspin
!            do k=1,MBZ
!               if ( occ(ib,k,s)>0.d0 ) then
!                  n=ib
!                  exit loop1
!               end if
!            end do
!            end do
!         end do loop1
!         octmp=octmp/real(MBZ*nspin,8)
!         do s=1,nspin
!         do k=1,MBZ
!            occ(n,k,s)=occ(n,k,s)+octmp/dble(MBZ*nspin)
!         end do
!         end do
!      end if
!!SF120502
!!      if ( abs(octmp)/=0.d0 ) then
!!         do s=1,nspin
!!            loop1 : do ib=mb2,mb1,-1
!!            do k=1,MBZ
!!               if ( occ(ib,k,s)>0.d0 ) then
!!                  n=ib
!!                  exit loop1
!!               end if
!!            end do
!!            end do loop1
!!            do k=1,MBZ
!!               occ(n,k,s)=occ(n,k,s)+octmp/dble(MBZ*nspin)
!!            end do
!!         end do
!!      end if
    if ( abs(octmp)>0.0d0 ) then
       do s=1,nspin
          do k=1,MBZ
             loop1 : do ib=mb2,mb1,-1
                if ( occ(ib,k,s)>0.0d0 ) then
                   occ(ib,k,s)=occ(ib,k,s)+octmp/dble(MBZ*nspin)
                   exit loop1
                end if
             end do loop1
          end do
       end do
    end if
!!/SF120502
    !if ( nspin == 2 ) then
    !   octmp = sum(occ(1:MB,1:MBZ,1:nspin))
    !   do s=1,nspin
    !      Ntot(s) = sum(occ(1:MB,1:MBZ,s))*ZNel/octmp &
    !           & +ZNext/2.d0
    !   end do
    !end if

    Efermi=ef

    call write_border( 1, "fermi_dirac(end)" )

    return
  end subroutine fermi_dirac

  !FUNCTION ff0(n,x)
  !  implicit none
  !  integer :: n,i
  !  real(8) :: x,ff,sqrtPi,ff0
  !  INTERFACE
  !     FUNCTION bberf(x)
  !       real(8) :: bberf,x
  !     END FUNCTION bberf
  !     FUNCTION factorial(n)
  !       integer :: n
  !       real(8) :: factorial
  !     END FUNCTION factorial
  !     FUNCTION hermitepoly(n,x)
  !       integer :: n
  !       real(8) :: hermitepoly,x
  !     END FUNCTION hermitepoly
  !  END INTERFACE
  !  sqrtPi=sqrt(acos(-1.d0))
  !  ff0 = 0.5d0*(1.d0-bberf(x))
  !  if ( n<=0 ) return
  !  do i=1,n
  !     ff0 = ff0 +(-1.d0)**i/(factorial(i)*4.d0**i*sqrtPi) &
  !          & *HermitePoly(2*i-1,x) *exp(-x**2)
  !  end do
  !  return
  !END FUNCTION ff0

  !FUNCTION factorial(n)
  !  implicit none
  !  integer :: n,i,m
  !  real(8) :: factorial
  !  m=1
  !  do i=2,n
  !     m=m*i
  !  end do
  !  factorial=dble(m)
  !  return
  !END FUNCTION factorial

  !RECURSIVE FUNCTION HermitePoly(n,x) RESULT(HerPol)
  !  implicit none
  !  integer :: n
  !  real(8) :: x,HerPol
  !  if ( n <= 0 ) then
  !     HerPol = 1.d0
  !  else if ( n == 1 ) then
  !     HerPol = 2.d0*x
  !  else
  !     HerPol = 2.d0*x*HermitePoly(n-1,x) &
  !          & -2.d0*dble(n-1)*HermitePoly(n-2,x)
  !  end if
  !  return
  !END FUNCTION HermitePoly

  subroutine entropy_fd( Eentropy, ekbt, occ )
    implicit none
    real(8), intent(out) :: Eentropy
    real(8), intent(in)  :: ekbt
    real(8), intent(in)  :: occ(:,:,:)
    real(8) :: factor1,factor2,Sf,f,g
    integer :: n,k,s,nb,nk,ns
    nb=size(occ,1)
    nk=size(occ,2)
    ns=size(occ,3)
    factor1=ns/2.0d0  ! ns=1: factor1=0.5,  ns=2: factor1=1.0
    factor2=2.0d0/ns  ! ns=1: factor2=2.0,  ns=2: factor2=1.0
    Eentropy=0.0d0
    do s=1,ns
    do k=1,nk
    do n=1,nb
       Sf=0.0d0
       f=occ(n,k,s)*factor1
       if ( f > 0.0d0 ) Sf=Sf-f*log(f)
       g=1.0d0-f
       if ( g > 0.0d0 ) Sf=Sf-g*log(g)
       Eentropy=Eentropy+factor2*ekbt*Sf
    end do
    end do
    end do
  end subroutine entropy_fd

end module fermi_dirac_module


      SUBROUTINE CBALEV5
C
C CHANGE RECORD
C **  SUBROUTINES CBALEV CALCULATE GLOBAL VOLUME, MASS, MOMENTUM,
C **  AND ENERGY BALANCES
C
      USE GLOBAL
C
C **  CHECK FOR END OF BALANCE PERIOD
C
      NTMPD2=NTSMMT/2
      IF(NBALE.EQ.NTMPD2)THEN
C
C **  CALCULATE ENDING VOLUME, SALT MASS, DYE MASS, MOMENTUM, KINETIC
C **  ENERGY AND POTENTIAL ENERGY, AND ASSOCIATED FLUXES
C
        VOLENDE=0.
        SALENDE=0.
        DYEENDE=0.
        UMOENDE=0.
        VMOENDE=0.
        UUEENDE=0.
        VVEENDE=0.
        PPEENDE=0.
        BBEENDE=0.
        DO L=2,LA
          LN=LNC(L)
          VOLENDE=VOLENDE+SPB(L)*DXYP(L)*HP(L)
          UMOENDE=UMOENDE+SPB(L)*0.5*DXYP(L)*HP(L)*(DYIU(L)*HUI(L)
     &*UHDYE(L)                +DYIU(LEAST(L))*HUI(LEAST(L))*UHDYE(LEAST(L)))
          VMOENDE=VMOENDE+SPB(L)*0.5*DXYP(L)*HP(L)*(DXIV(L)*HVI(L)
     &*VHDXE(L)                +DXIV(LN)*HVI(LN)*VHDXE(LN))
          PPEENDE=PPEENDE+SPB(L)*0.5*DXYP(L)
     &        *(GI*P(L)*P(L)-G*BELV(L)*BELV(L))
        ENDDO
        AMOENDE=SQRT(UMOENDE*UMOENDE+VMOENDE*VMOENDE)
        DO K=1,KC
          DO L=2,LA
            LN=LNC(L)
            SALENDE=SALENDE+SCB(L)*DXYP(L)*HP(L)*SAL(L,K)*DZC(K)
            DYEENDE=DYEENDE+SCB(L)*DXYP(L)*HP(L)*DYE(L,K)*DZC(K)
            UUEENDE=UUEENDE+SPB(L)*0.125*DXYP(L)*HP(L)*DZC(K)
     &          *( (U(L,K)+U(LEAST(L),K))*(U(L,K)+U(LEAST(L),K)) )
            VVEENDE=VVEENDE+SPB(L)*0.125*DXYP(L)*HP(L)*DZC(K)
     &          *( (V(L,K)+V(LN,K))*(V(L,K)+V(LN,K)) )
            BBEENDE=BBEENDE+SPB(L)*GP*DXYP(L)*HP(L)*DZC(K)*( BELV(L)
     &          +0.5*HP(L)*(Z(K)+Z(K-1)) )*B(L,K)
          ENDDO
        ENDDO
        UUEOUTE=DT2*UUEOUTE
        VVEOUTE=DT2*VVEOUTE
        PPEOUTE=DT2*PPEOUTE
        BBEOUTE=DT2*BBEOUTE
        VOLOUTE=DT2*VOLOUTE
        SALOUTE=DT2*SALOUTE
        DYEOUTE=DT2*DYEOUTE
        UMOOUTE=DT2*UMOOUTE
        VMOOUTE=DT2*VMOOUTE
        ENEBEGE=UUEBEGE+VVEBEGE+PPEBEGE+BBEBEGE
        ENEENDE=UUEENDE+VVEENDE+PPEENDE+BBEENDE
        ENEOUTE=UUEOUTE+VVEOUTE+PPEOUTE+BBEOUTE
        VOLBMOE=VOLBEGE-VOLOUTE
        SALBMOE=SALBEGE-SALOUTE
        DYEBMOE=DYEBEGE-DYEOUTE
        UMOBMOE=UMOBEGE-DYEOUTE
        VMOBMOE=VMOBEGE-DYEOUTE
        ENEBMOE=ENEBEGE-ENEOUTE
        VOLERR=VOLENDE-VOLBMOE
        SALERR=SALENDE-SALBMOE
        DYEERR=DYEENDE-DYEBMOE
        UMOERR=UMOENDE-UMOBMOE
        VMOERR=VMOENDE-VMOBMOE
        ENEERR=ENEENDE-ENEBMOE
        RVERDE=-9999.
        RSERDE=-9999.
        RDERDE=-9999.
        RUERDE=-9999.
        RVERDE=-9999.
        REERDE=-9999.
        RVERDO=-9999.
        RSERDO=-9999.
        RDERDO=-9999.
        RUERDO=-9999.
        RVERDO=-9999.
        REERDO=-9999.
        IF(VOLENDE.NE.0.) RVERDE=VOLERR/VOLENDE
        IF(SALENDE.NE.0.) RSERDE=SALERR/SALENDE
        IF(DYEENDE.NE.0.) RDERDE=DYEERR/DYEENDE
        IF(UMOENDE.NE.0.) RUMERDE=UMOERR/UMOENDE
        IF(VMOENDE.NE.0.) RVMERDE=VMOERR/VMOENDE
        IF(ENEENDE.NE.0.) REERDE=ENEERR/ENEENDE
        IF(VOLOUTE.NE.0.) RVERDO=VOLERR/VOLOUTE
        IF(SALOUTE.NE.0.) RSERDO=SALERR/SALOUTE
        IF(DYEOUTE.NE.0.) RDERDO=DYEERR/DYEOUTE
        IF(UMOOUTE.NE.0.) RUMERDO=UMOERR/UMOOUTE
        IF(VMOOUTE.NE.0.) RVMERDO=VMOERR/VMOOUTE
        IF(ENEOUTE.NE.0.) REERDO=ENEERR/ENEOUTE
C
C **  OUTPUT BALANCE RESULTS TO FILE BALE.OUT
C
        IF(JSBALE.EQ.1)THEN
          OPEN(89,FILE='BALE.OUT',STATUS='UNKNOWN')
          CLOSE(89,STATUS='DELETE')
          OPEN(89,FILE='BALE.OUT',STATUS='UNKNOWN')
          JSBALE=0
        ELSE
          OPEN(89,FILE='BALE.OUT',POSITION='APPEND',STATUS='UNKNOWN')
        ENDIF
        WRITE(89,890)NTMPD2,N
        WRITE(89,891)
        WRITE(89,892)VOLBEGE,SALBEGE,DYEBEGE,ENEBEGE,UMOBEGE,VMOBEGE,
     &      AMOBEGE
        WRITE(89,900)
        WRITE(89,893)
        WRITE(89,892)VOLOUTE,SALOUTE,DYEOUTE,ENEOUTE,UMOOUTE,VMOOUTE
        WRITE(89,900)
        WRITE(89,894)
        WRITE(89,892)VOLBMOE,SALBMOE,DYEBMOE,ENEBMOE,UMOBMOE,VMOBMOE
        WRITE(89,900)
        WRITE(89,895)
        WRITE(89,892)VOLENDE,SALENDE,DYEENDE,ENEENDE,UMOENDE,VMOENDE,
     &      AMOENDE
        WRITE(89,900)
        WRITE(89,896)
        WRITE(89,892)VOLERR,SALERR,DYEERR,ENEERR,UMOERR,VMOERR
        WRITE(89,900)
        WRITE(89,897)
        WRITE(89,892)RVERDE,RSERDE,RDERDE,REERDE,RUMERDE,RVMERDE
        WRITE(89,900)
        WRITE(89,898)
        WRITE(89,892)RVERDO,RSERDO,RDERDO,REERDO,RUMERDO,RVMERDO
        WRITE(89,899)
        UUEBMOE=UUEBEGE-UUEOUTE
        VVEBMOE=VVEBEGE-VVEOUTE
        PPEBMOE=PPEBEGE-PPEOUTE
        BBEBMOE=BBEBEGE-BBEOUTE
        WRITE(89,901)UUEBEGE
        WRITE(89,902)UUEOUTE
        WRITE(89,903)UUEBMOE
        WRITE(89,904)UUEENDE
        WRITE(89,900)
        WRITE(89,905)VVEBEGE
        WRITE(89,906)VVEOUTE
        WRITE(89,907)VVEBMOE
        WRITE(89,908)VVEENDE
        WRITE(89,900)
        WRITE(89,909)PPEBEGE
        WRITE(89,910)PPEOUTE
        WRITE(89,911)PPEBMOE
        WRITE(89,912)PPEENDE
        WRITE(89,900)
        WRITE(89,913)BBEBEGE
        WRITE(89,914)BBEOUTE
        WRITE(89,915)BBEBMOE
        WRITE(89,916)BBEENDE
        WRITE(89,900)
        WRITE(89,899)
        CLOSE(89)
  890 FORMAT (' VOLUME, MASS, AND ENERGY BALANCE OVER',I5,' TIME STEPS'
     &    ,' ENDING AT TIME STEP',I5,//)
  891 FORMAT (' INITIAL VOLUME    INITIAL SALT    INITIAL DYE     '
     &    ,'INITIAL ENER    INITIAL UMO     INITIAL VMO     '
     &    ,'INITIAL AMO',/)
  892 FORMAT (1X,7(E14.6,2X))
  893 FORMAT (' VOLUME OUT        SALT OUT        DYE OUT         '
     &    ,'ENERGY OUT      UMO OUT         VMO OUT',/)
  894 FORMAT (' INITIAL-OUT VOL   INIT-OUT SALT   INIT-OUT DYE    '
     &    ,'INIT-OUT ENER   INIT-OUT UMO    INIT-OUT VMO',/)
  895 FORMAT (' FINAL VOLUME      FINAL SALT      FINAL DYE       '
     &    ,'FINAL ENERGY    FINAL UMO       FINAL VMO       '
     &    ,'FINAL AMO',/)
  896 FORMAT (' VOLUME ERR        SALT ERR        DYE ERR         '
     &    ,'ENERGY ERR      UMO ERR         VMO ERR',/)
  897 FORMAT (' R VOL/END ER      R SAL/END ER    R DYE/END ER    '
     &    ,'R ENE/END ER    R UMO/END ER    R VMO/END ER',/)
  898 FORMAT (' R VOL/OUT ER      R SAL/OUT ER    R DYE/OUT ER    '
     &    ,'R ENE/OUT ER    R UMO/OUT ER    R VMO/OUT ER',/)
  899 FORMAT (////)
  900 FORMAT (//)
  901 FORMAT(' UUEBEGE =  ',E14.6)
  902 FORMAT(' UUEOUTE =  ',E14.6)
  903 FORMAT(' UUEBMOE =  ',E14.6)
  904 FORMAT(' UUEENDE =  ',E14.6)
  905 FORMAT(' VVEBEGE =  ',E14.6)
  906 FORMAT(' VVEOUTE =  ',E14.6)
  907 FORMAT(' VVEBMOE =  ',E14.6)
  908 FORMAT(' VVEENDE =  ',E14.6)
  909 FORMAT(' PPEBEGE =  ',E14.6)
  910 FORMAT(' PPEOUTE =  ',E14.6)
  911 FORMAT(' PPEBMOE =  ',E14.6)
  912 FORMAT(' PPEENDE =  ',E14.6)
  913 FORMAT(' BBEBEGE =  ',E14.6)
  914 FORMAT(' BBEOUTE =  ',E14.6)
  915 FORMAT(' BBEBMOE =  ',E14.6)
  916 FORMAT(' BBEENDE =  ',E14.6)
        NBALE=0
      ENDIF
      NBALE=NBALE+1
      RETURN
      END


subroutine migrate_planets
!
! Subroutine uses the torque exerted by each planet to find the
! migration timescale for each planet
!

use planetdata
use gravdata
use unitdata

implicit none

integer :: iplanet,i

!
! Assume that planet torque contributes to its migration only
! (Detailed torque balance between disc and individual bodies)
!

do iplanet=1,nplanet

   ! Integrate torque*sigma over the entire disc
   adot(iplanet) = 0.0

   do i=isr,ier
      adot(iplanet) = adot(iplanet) + torquei(iplanet,i)*sigma(i)/drzm1(i)
   enddo

    ! Multiply by appropriate factors to get adot
   adot(iplanet) = -adot(iplanet)*4.0*pi* &
        omegaK(iplanetrad(iplanet))*ap(iplanet)*ap(iplanet)/mp(iplanet)
   !adot(iplanet) = -adot(iplanet)*4.0*pi*ap(iplanet)/mp(iplanet)

    ! get numerically determined migration timescale

    if(abs(adot(iplanet))>1.0e-30) then
       tmig(iplanet) = ap(iplanet)/(-adot(iplanet))
    else
       tmig(iplanet) = 0.0
    endif

    ! Move planets
    ap(iplanet) = ap(iplanet) + adot(iplanet)*dt

enddo

end subroutine migrate_planets


      DOUBLE PRECISION FUNCTION apser(a,b,x,eps)
C-----------------------------------------------------------------------
C     APSER YIELDS THE INCOMPLETE BETA RATIO I(SUB(1-X))(B,A) FOR
C     A .LE. MIN(EPS,EPS*B), B*X .LE. 1, AND X .LE. 0.5. USED WHEN
C     A IS VERY SMALL. USE ONLY IF ABOVE INEQUALITIES ARE SATISFIED.
C-----------------------------------------------------------------------
C     .. Scalar Arguments ..
      DOUBLE PRECISION a,b,eps,x
C     ..
C     .. Local Scalars ..
      DOUBLE PRECISION aj,bx,c,g,j,s,t,tol
C     ..
C     .. External Functions ..
      DOUBLE PRECISION psi
      EXTERNAL psi
C     ..
C     .. Intrinsic Functions ..
      INTRINSIC abs,dlog
C     ..
C     .. Data statements ..
C--------------------
      DATA g/.577215664901533D0/
C     ..
C     .. Executable Statements ..
C--------------------
      bx = b*x
      t = x - bx
      IF (b*eps.GT.2.D-2) GO TO 10
      c = dlog(x) + psi(b) + g + t
      GO TO 20

   10 c = dlog(bx) + g + t
C
   20 tol = 5.0D0*eps*abs(c)
      j = 1.0D0
      s = 0.0D0
   30 j = j + 1.0D0
      t = t* (x-bx/j)
      aj = t/j
      s = s + aj
      IF (abs(aj).GT.tol) GO TO 30
C
      apser = -a* (c+s)
      RETURN

      END


      double precision function Hqaggvsq(j1,j2,j3,j4)
      implicit none
      include 'epinv.f'
      include 'epinv2.f'
      include 'first_time.f'
C---  matrix element squared for H--q(j1)+a(j2)+g(j3)+g(j4)
      integer al,j1,j2,j3,j4
      double precision q(4,5),qswap(4,5),sqres(-2:0)
      common/GZmom/q
!$omp threadprivate(/GZmom/)

      do al=1,4
      qswap(al,1)=q(al,j1)
      qswap(al,2)=q(al,j2)
      qswap(al,3)=q(al,j3)
      qswap(al,4)=q(al,j4)
      qswap(al,5)=-q(al,j1)-q(al,j2)-q(al,j3)-q(al,j4)
      enddo

C      write(*,*) 'scale, hmass',scale, hmass
      call GZHqaggvsqPoles(qswap,sqres)
c      call GZHqaggvsq(qswap,sqres,first_time,new_event,scale,hmass)
      Hqaggvsq=epinv*epinv2*sqres(-2)+epinv*sqres(-1)+sqres(0)
      if (first_time) first_time = .false.
      !if (new_event) new_event = .false.
      return
      end


MODULE Cropheatfluxmod
    IMPLICIT NONE
CONTAINS

    SUBROUTINE model_cropheatflux(netRadiationEquivalentEvaporation, &
        soilHeatFlux, &
        potentialTranspiration, &
        cropHeatFlux)
        IMPLICIT NONE
        REAL, INTENT(IN) :: netRadiationEquivalentEvaporation
        REAL, INTENT(IN) :: soilHeatFlux
        REAL, INTENT(IN) :: potentialTranspiration
        REAL, INTENT(OUT) :: cropHeatFlux
        !- Description:
    !            * Title: CropHeatFlux Model
    !            * Author: Pierre Martre
    !            * Reference: abModelling energy balance in the wheat crop model SiriusQuality2:
    !            Evapotranspiration and canopy and soil temperature calculations
    !            * Institution: INRA/LEPSE Montpellier
    !            * Abstract: It is calculated from net Radiation, soil heat flux and potential transpiration
        !- inputs:
    !            * name: netRadiationEquivalentEvaporation
    !                          ** variablecategory : auxiliary
    !                          ** description : net Radiation Equivalent Evaporation
    !                          ** datatype : DOUBLE
    !                          ** default : 638.142
    !                          ** min : 0
    !                          ** max : 10000
    !                          ** unit : g m-2 d-1
    !                          ** uri : http://www1.clermont.inra.fr/siriusquality/?page_id=547
    !                          ** inputtype : variable
    !            * name: soilHeatFlux
    !                          ** description : soil Heat Flux
    !                          ** variablecategory : rate
    !                          ** datatype : DOUBLE
    !                          ** default : 188.817
    !                          ** min : 0
    !                          ** max : 1000
    !                          ** unit : g m-2 d-1
    !                          ** uri : http://www1.clermont.inra.fr/siriusquality/?page_id=547
    !                          ** inputtype : variable
    !            * name: potentialTranspiration
    !                          ** description : potential Transpiration
    !                          ** variablecategory : rate
    !                          ** datatype : DOUBLE
    !                          ** default :  1.413
    !                          ** min : 0
    !                          ** max : 1000
    !                          ** unit : g m-2 d-1
    !                          ** uri : http://www1.clermont.inra.fr/siriusquality/?page_id=547
    !                          ** inputtype : variable
        !- outputs:
    !            * name: cropHeatFlux
    !                          ** description :  crop Heat Flux
    !                          ** variablecategory : rate
    !                          ** datatype : DOUBLE
    !                          ** min : 0
    !                          ** max : 10000
    !                          ** unit : g m-2 d-1
    !                          ** uri : http://www1.clermont.inra.fr/siriusquality/?page_id=547
        cropHeatFlux = netRadiationEquivalentEvaporation - soilHeatFlux -  &
                potentialTranspiration
    END SUBROUTINE model_cropheatflux

END MODULE


program mean
!******************************************
! Calculate mean of a given variable
! inspired by Hansaki et al,. (2018)
! created by Menaka@IIS
!******************************************
implicit none
character(len=128)              :: fname,buf,camadir,expdir,DAdir,patchdir,hydrowebdir,mapname,varname
character(len=8)                :: yyyymmdd,nxtyyyymmdd
!-map variables
real                            :: gsize,west, north, east, south ! map boundries
integer                         :: latpx,lonpx,nflp    ! pixel size, calculated
!==================================
if(iargc().lt.5) then
  write(*,*) 'Usage: mean'
  write(*,*) '      yearmin yearmax yearout CaMadir mapname'
  stop
end if

call getarg(1,buf)
read(buf,*) yearmin ! minimum year

call getarg(2,buf)
read(buf,*) yearmax ! maximum year

call getarg(3,buf)
read(buf,*) yearout ! out year

call getarg(4,buf)
read(buf,"(A)") camadir ! CaMadir

call getarg(5,buf)
read(buf,*) mapname ! name of the map

call getarg(6,buf)
read(buf,"(A)") expdir

call getarg(7,buf)
read(buf,"(A)") DAdir

call getarg(8,buf)
read(buf,*) varname

!==
fname=trim(camadir)//"/map/"//trim(mapname)//"/params.txt"
open(11,file=fname,form='formatted')
read(11,*) lonpx
read(11,*) latpx
read(11,*) nflp
read(11,*) gsize
read(11,*) west
read(11,*) east
read(11,*) south
read(11,*) north
close(11)
!-------

end program mean


FUNCTION dayear(dd,mm,yyyy)
!
IMPLICIT NONE
!
! Function arguments
!
INTEGER :: dd,mm,yyyy
INTEGER :: dayear
!
! Local variables
!
INTEGER :: difdat
!
!     + + + purpose + + +
!     given a date in dd/mm/yyyy format,
!     dayear will return the number of days
!     from the first of that year.
!
!     + + + keywords + + +
!     date, utility
!
!     + + + argument declarations + + +
!
!     + + + argument definitions + + +
!     dayear - returns the number of days from the first of that year
!     dd     - day
!     mm     - month
!     yyyy   - year
!
!     + + + local variable definitions + + +
!     difdat - the number of days between two dates. a function. This
!              variable holds the value returned by the Diffdat function.
!              Debe assumed this definition

!     + + + function declarations + + +
!
!     + + + end specifications + + +
!
!     get the difference in days + 1
!
dayear = difdat(1,1,yyyy,dd,mm,yyyy) + 1
!
END FUNCTION dayear

Project Documents/Source Code Original/Dayear.f90


* -*- fortran -*-
C     STOPTEST2

*  Purpose
*  =======
*
*  Computes the stopping criterion 2.
*
*  Arguments
*  =========
*
*  N       (input) INTEGER.
*          On entry, the dimension of the matrix.
*          Unchanged on exit.
*
*  INFO    (output) INTEGER
*          On exit, 1/0 depending on whether stopping criterion
*          was met or not.
*
*  BNRM2   (input/output) DOUBLE PRECISION.
*          On first time entry, will be -1.0.
*          On first time exit will contain norm2(B)
*          On all subsequent entry/exit's unchanged.
*
*  RESID   (output) DOUBLE PRECISION.
*          On exit, the computed stopping measure.
*
*  TOL     (input) DOUBLE PRECISION.
*          On input, the allowable convergence measure.
*
*  R       (input) DOUBLE PRECISION array, dimension N.
*          On entry, the residual.
*          Unchanged on exit.
*
*  B       (input) DOUBLE PRECISION array, dimension N.
*          On entry, right hand side vector B.
*          Unchanged on exit.
*
*  BLAS CALLS:   DNRM2
*  ============================================================
*

      SUBROUTINE sSTOPTEST2( N, R, B, BNRM2, RESID, TOL, INFO )
      INTEGER            N, INFO
      real RESID, TOL, BNRM2
      real   R( * ), B( * )
      real   ZERO, ONE
      PARAMETER        ( ZERO = 0.0D+0, ONE = 1.0D+0 )
      real wsNRM2
      EXTERNAL         wsNRM2
      IF( INFO.EQ.-1 ) THEN
         BNRM2 = wsNRM2( N, B, 1 )
         IF ( BNRM2.EQ.ZERO ) BNRM2 = ONE
      ENDIF
      RESID = wsNRM2( N, R, 1 ) / BNRM2
      INFO = 0
      IF ( RESID.LE.TOL )
     $     INFO = 1
      RETURN
      END
*     END SUBROUTINE sSTOPTEST2

      SUBROUTINE dSTOPTEST2( N, R, B, BNRM2, RESID, TOL, INFO )
      INTEGER            N, INFO
      double precision RESID, TOL, BNRM2
      double precision   R( * ), B( * )
      double precision   ZERO, ONE
      PARAMETER        ( ZERO = 0.0D+0, ONE = 1.0D+0 )
      double precision dNRM2
      EXTERNAL         dNRM2
      IF( INFO.EQ.-1 ) THEN
         BNRM2 = dNRM2( N, B, 1 )
         IF ( BNRM2.EQ.ZERO ) BNRM2 = ONE
      ENDIF
      RESID = dNRM2( N, R, 1 ) / BNRM2
      INFO = 0
      IF ( RESID.LE.TOL )
     $     INFO = 1
      RETURN
      END
*     END SUBROUTINE dSTOPTEST2

      SUBROUTINE cSTOPTEST2( N, R, B, BNRM2, RESID, TOL, INFO )
      INTEGER            N, INFO
      real RESID, TOL, BNRM2
      complex   R( * ), B( * )
      real   ZERO, ONE
      PARAMETER        ( ZERO = 0.0D+0, ONE = 1.0D+0 )
      real wscNRM2
      EXTERNAL         wscNRM2
      IF( INFO.EQ.-1 ) THEN
         BNRM2 = wscNRM2( N, B, 1 )
         IF ( BNRM2.EQ.ZERO ) BNRM2 = ONE
      ENDIF
      RESID = wscNRM2( N, R, 1 ) / BNRM2
      INFO = 0
      IF ( RESID.LE.TOL )
     $     INFO = 1
      RETURN
      END
*     END SUBROUTINE cSTOPTEST2

      SUBROUTINE zSTOPTEST2( N, R, B, BNRM2, RESID, TOL, INFO )
      INTEGER            N, INFO
      double precision RESID, TOL, BNRM2
      double complex   R( * ), B( * )
      double precision   ZERO, ONE
      PARAMETER        ( ZERO = 0.0D+0, ONE = 1.0D+0 )
      double precision dzNRM2
      EXTERNAL         dzNRM2
      IF( INFO.EQ.-1 ) THEN
         BNRM2 = dzNRM2( N, B, 1 )
         IF ( BNRM2.EQ.ZERO ) BNRM2 = ONE
      ENDIF
      RESID = dzNRM2( N, R, 1 ) / BNRM2
      INFO = 0
      IF ( RESID.LE.TOL )
     $     INFO = 1
      RETURN
      END
*     END SUBROUTINE zSTOPTEST2


! { dg-options "-O3 -ffast-math -floop-interchange -floop-block -fdump-tree-graphite-all" }

  INTEGER, PARAMETER :: N=1024
  REAL*8 :: A(N,N), B(N,N), C(N,N)
  REAL*8 :: t1,t2
  A=0.1D0
  B=0.1D0
  C=0.0D0
  CALL cpu_time(t1)
  CALL mult(A,B,C,N)
  CALL cpu_time(t2)
  write(6,*) t2-t1,C(1,1)
END program

SUBROUTINE mult(A,B,C,N)
  REAL*8 :: A(N,N), B(N,N), C(N,N)
  INTEGER :: I,J,K,N
  DO J=1,N
     DO I=1,N
        DO K=1,N
           C(I,J)=C(I,J)+A(I,K)*B(K,J)
        ENDDO
     ENDDO
  ENDDO
END SUBROUTINE mult

! { dg-final { scan-tree-dump "tiled by" "graphite" } }


module test_cases
	use iso_c_binding
	use fruit
	use constants
	use randomness
	implicit none

contains

	subroutine randomint_ranges
		integer(bigint) :: result
		logical(sint)	:: test

		! typical range
		result = random_int(2, 21)
		test = (result .ge. 2) .and. (result .le. 21)
		call assert_true(test, 'random_int(2, 21) failed')

		! negative range
		result = random_int(-43, -8)
		test = (result .ge. -43) .and. (result .le. -8)
		call assert_true(test, 'random_int(-43, -8) failed')

		! large range
		result = random_int(-509, 12044)
		test = (result .ge. -509) .and. (result .le. 12044)
		call assert_true(test, 'random_int(-509, 12044) failed')
	end subroutine randomint_ranges

	subroutine randomint_special
		integer(bigint) :: result
		logical(sint)	:: test

		! adjacent values
		result = random_int(0, 1)
		test = result .eq. 0
		call assert_true(test, 'random_int(0, 1) failed')

		! equal values
		result = random_int(4, 4)
		test = result .eq. 4
		call assert_true(test, 'random_int(4, 4) failed')

		! reverse minv and maxv
		result = random_int(80, 21)
		test = (result .ge. 21) .and. (result .le. 80)
		call assert_true(test, 'random_int(80, 21) failed')
	end subroutine randomint_special

	subroutine nrandomint_test
		integer(bigint), dimension(32) :: results
		logical(sint)				   :: test
		integer						   :: ix

		results = 0
		call n_random_ints(32, 1, 100, results)
		test = .true.
		main: do ix=1,32
			test = test .and. ((results(ix) .ge. 1) .and. (results(ix) .le. 100))
		end do main
		call assert_true(test, 'nrandomint_test failed')
	end subroutine nrandomint_test

	subroutine nrandomint_weighted_uniform
		! n = 20, nvals = 5
		integer(bigint), dimension(2)	:: plusminus
		real(dbl), dimension(5)			:: weights
		integer(bigint), dimension(20)	:: result
		integer, dimension(5)			:: counter
		logical(sint)					:: test
		integer							:: ix, jx, bigdiff

		plusminus(1) = 10
		plusminus(2) = 10
		weights = 1d0 ! uniformly weighted

		call n_random_ints_weighted(20, 5, plusminus, weights, result)
		test = .true.
		counter = 0
		do ix=1,20
			test = test .and. ((result(ix) .ge. 1) .and. (result(ix) .le. 5))
			if (test) counter(result(ix)) = counter(result(ix)) + 1
		end do
		call assert_true(test, 'n_random_ints_weighted out of bounds error')
	end subroutine nrandomint_weighted_uniform

	subroutine nrandomint_weighted_binomial
		! n = 14, nvals = 6
		integer(bigint), dimension(2)	:: plusminus
		real(dbl), dimension(6)			:: weights
		integer(bigint), dimension(14)	:: result
		logical(sint)					:: test
		integer							:: ix

		plusminus(1) = 9
		plusminus(2) = 5
		weights(1) = 1d0 ! binomially weighted
		weights(2) = 5d0
		weights(3) = 10d0
		weights(4) = 10d0
		weights(5) = 5d0
		weights(6) = 1d0

		call n_random_ints_weighted(14, 6, plusminus, weights, result)
		test = .true.
		do ix=1,14
			test = test .and. ((result(ix) .ge. 1) .and. (result(ix) .le. 6))
		end do
		call assert_true(test, 'n_random_ints_weighted binomial weights failed')
	end subroutine nrandomint_weighted_binomial

	subroutine nrandomint_weighted_zero
		! n = 5, nvals = 3
		integer(bigint), dimension(2)	:: plusminus
		real(dbl), dimension(3)			:: weights
		integer(bigint), dimension(5)	:: result
		logical(sint)					:: test
		integer							:: ix

		plusminus(1) = 2
		plusminus(2) = 3
		weights = 0d0 ! all weights zero

		call n_random_ints_weighted(5, 3, plusminus, weights, result)
		test = .true.
		do ix=1,5
			test = test .and. ((result(ix) .ge. 1) .and. (result(ix) .le. 3))
		end do
		call assert_true(test, 'n_random_ints_weighted zero weights failed')
	end subroutine nrandomint_weighted_zero

end module test_cases

program random_test
  use fruit
  use test_cases

  implicit none

  logical(sint) :: ok

  call init_fruit

  ! call all tests
  call randomint_ranges
  call randomint_special
  call nrandomint_test
  call nrandomint_weighted_uniform
  call nrandomint_weighted_binomial
  call nrandomint_weighted_zero

  call fruit_summary
  call fruit_finalize

  call is_all_successful(ok)
  if (.not. ok) call exit(1)

end program random_test


module element_class

  implicit none

  private

  public :: spring

  type :: spring

     ! Unique global identifier for this element
     integer :: element_id

     ! Global node identifiers associated with this element
     integer :: nnodes
     integer, allocatable :: node_ids(:)
     real(8), allocatable :: nodes(:,:)

     ! DOF calculations
     integer :: ndof_per_node = 1
     integer :: ndof = 0
     real(8) :: k
     integer :: proc_id

   contains

     procedure :: add_residual
     procedure :: add_jacobian
     procedure :: add_jacobian_vector_pdt
     procedure :: to_string

  end type spring

  ! Constructor
  interface spring
     module procedure construct_spring
  end interface spring

contains

  !===================================================================!
  ! Constructor for spring
  !===================================================================!

  type(spring) function construct_spring( element_id, node_ids, &
       & nodes, spring_constant, proc_id ) result (this)

    integer, intent(in) :: element_id
    integer, intent(in) :: node_ids(:)
    real(8), intent(in) :: nodes(:,:)
    real(8), intent(in) :: spring_constant
    integer, intent(in) :: proc_id

    ! Mesh props
    this % element_id = element_id
    allocate(this % node_ids, mold = node_ids)
    this % node_ids = node_ids
    allocate(this % nodes, mold = nodes)
    this % nodes = nodes

    ! Material props
    this % k = spring_constant

    this % proc_id = proc_id
    this % ndof = this % ndof_per_node * size(node_ids)

  end function construct_spring

  !===================================================================!
  ! Print object info
  !===================================================================!

  subroutine to_string(this)

    class(spring) :: this

    print *, "eid", this % element_id,  "nids", this % node_ids, "x", &
         & this % nodes, "proc", this % proc_id

  end subroutine to_string

  !===================================================================!
  ! Add the residual
  !===================================================================!

  subroutine add_residual(this, x, res, bc, bcval)

    class(spring) :: this
    real(8), intent(in) :: x(:)
    real(8), intent(inout) :: res(:)
    integer, intent(in)    :: bc
    real(8), intent(in)    :: bcval

    ! Add nodal residuals [k]{x} - {f} = 0
    res(1) = res(1) + this % k * x(1) - this % k * x(2)
    res(2) = res(2) - this % k * x(1) + this % k * x(2)

    ! Penalize for BC
    if (bc .eq. 1) then
       res(1) = 100000000.0d0 * bcval
    end if

  end subroutine add_residual

  !===================================================================!
  ! Add the jacobian
  !===================================================================!

  subroutine add_jacobian(this, x, jac, bc)

    class(spring)          :: this
    real(8), intent(in)    :: x(:)
    real(8), intent(inout) :: jac(:,:)
    integer, intent(in)    :: bc

    ! First column
    jac(1,1) = jac(1,1) + this % k
    jac(2,1) = jac(2,1) - this % k

    ! Second column
    jac(1,2) = jac(1,2) - this % k
    jac(2,2) = jac(2,2) + this % k

    ! Penalize for BC
    if (bc .eq. 1) then
       jac(1,1) = this % k * 100000000.0d0
    end if

  end subroutine add_jacobian

  !===================================================================!
  ! Add the jacobian-vector product
  !===================================================================!

  subroutine add_jacobian_vector_pdt(this, x, pdt, bc)

    class(spring)          :: this
    real(8), intent(in)    :: x(:)
    real(8), intent(inout) :: pdt(:)
    integer                :: bc
    real(8)                :: elemjac(this % ndof, this % ndof)

    elemjac = 0.0d0
    call this % add_jacobian(x, elemjac, bc)
    pdt = pdt + matmul(elemjac, x)

  end subroutine add_jacobian_vector_pdt

end module element_class

!=====================================================================!
! Assembler class for spring elements
!=====================================================================!

module assembler_class

  use element_class

  implicit none

  type :: assembler

     type(spring), allocatable :: elems(:)
     integer :: nelems
     integer :: ndof

   contains

     procedure :: residual
     procedure :: jacobian
     procedure :: jacobian_vector_product
     procedure :: solve

  end type assembler

  !
  interface assembler
     module procedure create_assembler
  end interface assembler

contains

  type(assembler) function create_assembler(elems) result (this)

    type(spring), intent(in) :: elems(:)
    integer :: i

    allocate(this % elems, source = elems)
    this % nelems = size(elems)

!!$    do i = 1, this % nelems
!!$       this % ndof = this % ndof + this % elems(i) % ndof
!!$    end do

    ! removing common dofs
    this % ndof = this % nelems + 1

    print *, "NDOF=", this % ndof, "proc", this_image()
    print *, "how to apply bc?"
    print *, "how to set rhs?"
    print *, "implement destructor"

  end function create_assembler

  !===================================================================!
  ! Add the residual
  !===================================================================!

  subroutine residual(this, x, res)

    class(assembler)       :: this
    real(8), intent(in)    :: x(:)
    real(8), intent(inout) :: res(:)
    integer                :: nids(2)
    integer                :: e

    res = 0.0d0
    do e = 2, this % nelems
       nids = this % elems(e) % node_ids
       call this % elems(e) % add_residual( &
            & x(nids(1):nids(2)), &
            & res(nids(1):nids(2)), &
            & 0, 0.0d0)
       if (e .eq. this % nelems .AND. THIS_IMAGE() .EQ. NUM_images()) res(nids(2)) = res(nids(2)) + 1.0d0
    end do

    IF (THIS_IMAGE() .EQ. 1) then
       ! Apply BC on the first element
       nids = this % elems(1) % node_ids
       call this % elems(1) % add_residual( &
            & x(nids(1):nids(2)), &
            & res(nids(1):nids(2)), &
            & 1, 0.0d0)
    END IF
    !call this % apply_bc(res)
    ! Right hand side for the last element (we apply a tail force)

  end subroutine residual

  !===================================================================!
  ! Add the jacobian
  !===================================================================!

  subroutine jacobian(this, x, jac)

    class(assembler)       :: this
    real(8), intent(in)    :: x(:)
    real(8), intent(inout) :: jac(:,:)
    integer                :: nids(2)
    integer                :: e

    ! Zero the jacobian matrix entries
    jac = 0.0d0

    IF (THIS_IMAGE() .EQ. 1) then

       ! Add individual blocks to the matrix
       do e = 1, this % nelems

          associate ( nids => this % elems(e) % node_ids )
            associate( jmat => jac(nids(1):nids(2), nids(1):nids(2)), &
                 & xvec => x(nids(1):nids(2)) )
              call this % elems(e) % add_jacobian(xvec, jmat, e)
            end associate
          end associate

       end do

    else

       ! Add individual blocks to the matrix
       do e = 1, this % nelems

          associate ( nids => this % elems(e) % node_ids )
            associate( jmat => jac(nids(1):nids(2), nids(1):nids(2)), &
             & xvec => x(nids(1):nids(2)) )
              call this % elems(e) % add_jacobian(xvec, jmat, 0)
            end associate
          end associate

       end do

    end if

  end subroutine jacobian

  !===================================================================!
  ! Add the jacobian-vector product
  !===================================================================!

  subroutine jacobian_vector_product(this, x, Jx)

    class(assembler) :: this
    real(8), intent(in) :: x(:)
    real(8), intent(inout) :: Jx(:)
    integer :: e

    Jx = 0.0d0

    IF (THIS_IMAGE() .EQ. 1) then

       do e = 1, this % nelems
          associate (nids => this % elems(e) % node_ids)
            call this % elems(e) % add_jacobian_vector_pdt( &
                 & x(nids(1):nids(2)), &
                 & Jx(nids(1):nids(2)), &
                 & e) ! applies bc
          end associate
       end do

    else

       do e = 1, this % nelems
          associate (nids => this % elems(e) % node_ids)
            call this % elems(e) % add_jacobian_vector_pdt( &
                 & x(nids(1):nids(2)), &
                 & Jx(nids(1):nids(2)), &
                 & 0) ! applies bc
          end associate
       end do

    end if

    ! call this % apply_bc(res)
    ! Any communication with neighbor?

  end subroutine jacobian_vector_product

  !===================================================================!
  ! Solve the linear system using iterative method : conjugate gradient
  !===================================================================!

  subroutine solve(this, max_it, max_tol, x, iter, tol, flag)

    use coarray_util, only : co_norm2, co_dot_product
    implicit none

    class(assembler)        :: this
    integer , intent(in)    :: max_it
    real(8) , intent(in)    :: max_tol

    real(8) , intent(inout) :: x(:)
    integer , intent(out)   :: iter
    real(8) , intent(out)   :: tol
    integer , intent(out)   :: flag

    ! create local data
    real(8) , allocatable   :: p(:), r(:), w(:), b(:)
    real(8) , allocatable   :: rho(:)
    real(8)                 :: alpha, beta
    real(8)                 :: bnorm, rnorm

    ! Memory allocations
    allocate(r, p, w, b, mold=x)
    allocate(rho(max_it))

    ! Start the iteration counter
    iter = 1

    call this % residual(x, b)

    ! Norm of the right hand side
    bnorm = co_norm2(b)

    ! Norm of the initial residual
    call this % jacobian_vector_product(x, r)
    r         = b - r !co_matmul(A, x)
    rnorm     = co_norm2(r)
    tol       = rnorm/bnorm
    rho(iter) = rnorm*rnorm

    print * , "Missing sync in CG"

    if (this_image() .eq. 1) then
       open(10, file='cg.log', action='write', position='append')
    end if

    ! Apply Iterative scheme until tolerance is achieved
    do while ((tol .gt. max_tol) .and. (iter .lt. max_it))

       ! step (a) compute the descent direction
       if ( iter .eq. 1) then
          ! steepest descent direction p
          p = r
       else
          ! take a conjugate direction
          beta = rho(iter)/rho(iter-1)
          p = r + beta*p
       end if

       ! step (b) compute the solution update
       call this % jacobian_vector_product(p, w)
       !w = co_matmul(A,p)

       ! step (c) compute the step size for update
       alpha = rho(iter)/co_dot_product(p, w)

       ! step (d) Add dx to the old solution
       x = x + alpha*p

       ! step (e) compute the new residual
       r = r - alpha*w
       !r = b - matmul(A, x)

       ! step(f) update values before next iteration
       rnorm = co_norm2(r)
       tol = rnorm/bnorm

       if (this_image() .eq. 1) then
          write(10,*) iter, tol
          print *, iter, tol
       end if

       iter = iter + 1

       rho(iter) = rnorm*rnorm

    end do

    close(10)

    deallocate(r, p, w, rho)

    flag = 0

  end subroutine solve

end module assembler_class