!
!     CalculiX - A 3-dimensional finite element program
!              Copyright (C) 1998-2020 Guido Dhondt
!
!     This program is free software; you can redistribute it and/or
!     modify it under the terms of the GNU General Public License as
!     published by the Free Software Foundation(version 2);
!
!
!     This program is distributed in the hope that it will be useful,
!     but WITHOUT ANY WARRANTY; without even the implied warranty of
!     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
!     GNU General Public License for more details.
!
!     You should have received a copy of the GNU General Public License
!     along with this program; if not, write to the Free Software
!     Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
!
      subroutine incplas_lin(elconloc,plconloc,xstate,xstateini,
     &  elas,emec,ithermal,icmd,beta,stre,vj,kode,
     &  ielas,amat,t1l,dtime,time,ttime,iel,iint,nstate_,mi,
     &  eloc,pgauss,nmethod,pnewdt,depvisc)
!
!     calculates stiffness and stresses for the incremental plasticity
!     material law
!
!     icmd=3: calculates stress at mechanical strain
!     else: calculates stress at mechanical strain and the stiffness
!           matrix
!
!     This routine is meant for small strains. Reference:
!     Dhondt, Guido; The Finite Element Method for Three-dimensional
!     Thermomechanical Applications (Wiley, 2004), Section 5.3
!
      implicit none
!
      character*80 amat
!
      integer ithermal(*),icmd,i,k,l,m,n,kode,ivisco,ielastic,kel(4,21),
     &  niso,nkin,ielas,iel,iint,nstate_,mi(*),id,leximp,lend,layer,
     &  kspt,kstep,kinc,iloop,nmethod,user_hardening,user_creep
!
      real*8 elconloc(*),elas(21),emec(6),beta(6),stre(6),
     &  vj,plconloc(802),stbl(6),stril(6),xitril(6),
     &  ee,un,um,al,cop,dxitril,xn(3,3),epl(6),c1,c2,c3,c4,c7,
     &  c8,ftrial,xiso(200),yiso(200),xkin(200),ykin(200),
     &  fiso,dfiso,fkin,dfkin,fiso0,fkin0,ep,t1l,dtime,
     &  epini,a1,dsvm,xxa,xxn,dkl(3,3),el(6),tracee,traces,
     &  dcop,time,ttime,eloc(6),xstate(nstate_,mi(1),*),
     &  xstateini(nstate_,mi(1),*),decra(5),deswa(5),serd,
     &  esw(2),ec(2),p,qtild,predef(1),dpred(1),timeabq(2),pgauss(3),
     &  dtemp,pnewdt,um2,depvisc
!
!
      kel=reshape((/1,1,1,1,1,1,2,2,2,2,2,2,1,1,3,3,2,2,3,3,3,3,3,3,
     &          1,1,1,2,2,2,1,2,3,3,1,2,1,2,1,2,1,1,1,3,2,2,1,3,
     &          3,3,1,3,1,2,1,3,1,3,1,3,1,1,2,3,2,2,2,3,3,3,2,3,
     &          1,2,2,3,1,3,2,3,2,3,2,3/),(/4,21/))
      dkl=reshape((/1.d0,0.d0,0.d0,0.d0,1.d0,0.d0,0.d0,0.d0,1.d0/),
     &      (/3,3/))
!
      leximp=1
      lend=2
      user_creep=0
!
!     localizing the plastic fields
!
      do i=1,6
         epl(i)=xstateini(1+i,iint,iel)
         stbl(i)=xstateini(7+i,iint,iel)
      enddo
      epini=xstateini(1,iint,iel)
!
      ee=elconloc(1)
      un=elconloc(2)
      um2=ee/(1.d0+un)
      al=um2*un/(1.d0-2.d0*un)
      um=um2/2.d0
!
      ep=epini
!
!     check whether the user activated a viscous calculation
!     (*VISCO instead of *STATIC)
!
      if((nmethod.ne.1).or.(ithermal(1).eq.3)) then
         ivisco=1
      else
         ivisco=0
      endif
!
!     check for user subroutines
!
      if((plconloc(801).lt.0.8d0).and.(plconloc(802).lt.0.8d0)) then
         user_hardening=1
      else
         user_hardening=0
      endif
      if((kode.eq.-52).and.(ivisco.eq.1)) then
         if(elconloc(3).lt.0.d0) then
            user_creep=1
         else
            user_creep=0
            xxa=elconloc(3)*(ttime+time)**elconloc(5)
            if(xxa.lt.1.d-20) xxa=1.d-20
            xxn=elconloc(4)
            a1=xxa*dtime
         endif
      endif
!
!     trial elastic strain
!
      do i=1,6
         el(i)=emec(i)-epl(i)
      enddo
!
!     trial stress
!
      tracee=el(1)+el(2)+el(3)
      do i=1,6
         stre(i)=um2*el(i)-beta(i)
      enddo
      do i=1,3
         stre(i)=stre(i)+al*tracee
      enddo
!
!     trial deviatoric stress
!
      traces=(stre(1)+stre(2)+stre(3))/3.d0
      do i=1,3
         stril(i)=stre(i)-traces
      enddo
      do i=4,6
         stril(i)=stre(i)
      enddo
!
!     subtracting the back stress -> trial radius vector
!
      do i=1,6
         xitril(i)=stril(i)-stbl(i)
      enddo
!
!     size of trial radius vector
!
      dxitril=0.d0
      do i=1,3
         dxitril=dxitril+xitril(i)*xitril(i)
      enddo
      do i=4,6
         dxitril=dxitril+2.d0*xitril(i)*xitril(i)
      enddo
      dxitril=dsqrt(dxitril)
!
!        restoring the hardening curves for the actual temperature
!        plconloc contains the true stresses. By multiplying by
!        the Jacobian, yiso and ykin are Kirchhoff stresses, as
!        required by the hyperelastic theory (cf. Simo, 1988).
!
      niso=int(plconloc(801))
      nkin=int(plconloc(802))
      if(niso.ne.0) then
         do i=1,niso
            xiso(i)=plconloc(2*i-1)
            yiso(i)=plconloc(2*i)
         enddo
      endif
      if(nkin.ne.0) then
         do i=1,nkin
            xkin(i)=plconloc(399+2*i)
            ykin(i)=plconloc(400+2*i)
         enddo
      endif
!
!     if no viscous calculation is performed a pure creep calculatino
!     (without plasticity) is reduced to an elastic calculation
!
      ielastic=0
      if(ivisco.eq.0) then
         if(niso.eq.2) then
            if((dabs(yiso(1)).lt.1.d-10).and.
     &         (dabs(yiso(2)).lt.1.d-10)) then
               ielastic=1
            endif
         endif
      endif
!
!     check for yielding
!
      if(user_hardening.eq.1) then
         call uhardening(amat,iel,iint,t1l,epini,ep,dtime,
     &        fiso,dfiso,fkin,dfkin)
      else
         if(niso.ne.0) then
            call ident(xiso,ep,niso,id)
            if(id.eq.0) then
               fiso=yiso(1)
            elseif(id.eq.niso) then
               fiso=yiso(niso)
            else
               dfiso=(yiso(id+1)-yiso(id))/(xiso(id+1)-xiso(id))
               fiso=yiso(id)+dfiso*(ep-xiso(id))
            endif
         elseif(nkin.ne.0) then
            fiso=ykin(1)
         else
            fiso=0.d0
         endif
      endif
!
      c1=2.d0/3.d0
      c2=dsqrt(c1)
!
      ftrial=dxitril-c2*fiso
      if((ftrial.le.1.d-10).or.(ielas.eq.1).or.(ielastic.eq.1)
     &    .or.(dtime.lt.1.d-30)) then
!
!        updating the plastic fields
!
         do i=1,6
            xstate(1+i,iint,iel)=epl(i)
            xstate(7+i,iint,iel)=stbl(i)
         enddo
         xstate(1,iint,iel)=ep
!
         if(icmd.ne.3) then
            elas(1)=al+um2
            elas(2)=al
            elas(3)=al+um2
            elas(4)=al
            elas(5)=al
            elas(6)=al+um2
            elas(7)=0.d0
            elas(8)=0.d0
            elas(9)=0.d0
            elas(10)=um
            elas(11)=0.d0
            elas(12)=0.d0
            elas(13)=0.d0
            elas(14)=0.d0
            elas(15)=um
            elas(16)=0.d0
            elas(17)=0.d0
            elas(18)=0.d0
            elas(19)=0.d0
            elas(20)=0.d0
            elas(21)=um
         endif
!
         return
      endif
!
!        plastic deformation
!
!        calculating the consistency parameter
!
      iloop=0
      cop=0.d0
!
      loop: do
         iloop=iloop+1
         ep=epini+c2*cop
!
         if(user_hardening.eq.1) then
            call uhardening(amat,iel,iint,t1l,epini,ep,dtime,
     &           fiso,dfiso,fkin,dfkin)
         else
            if(niso.ne.0) then
               call ident(xiso,ep,niso,id)
               if(id.eq.0) then
                  fiso=yiso(1)
                  dfiso=0.d0
               elseif(id.eq.niso) then
                  fiso=yiso(niso)
                  dfiso=0.d0
               else
                  dfiso=(yiso(id+1)-yiso(id))/(xiso(id+1)-xiso(id))
                  fiso=yiso(id)+dfiso*(ep-xiso(id))
               endif
            elseif(nkin.ne.0) then
               fiso=ykin(1)
               dfiso=0.d0
            else
               fiso=0.d0
               dfiso=0.d0
            endif
!
            if(nkin.ne.0) then
               call ident(xkin,ep,nkin,id)
               if(id.eq.0) then
                  fkin=ykin(1)
                  dfkin=0.d0
               elseif(id.eq.nkin) then
                  fkin=ykin(nkin)
                  dfkin=0.d0
               else
                  dfkin=(ykin(id+1)-ykin(id))/(xkin(id+1)-xkin(id))
                  fkin=ykin(id)+dfkin*(ep-xkin(id))
               endif
            elseif(niso.ne.0) then
               fkin=yiso(1)
               dfkin=0.d0
            else
               fkin=0.d0
               dfkin=0.d0
            endif
         endif
!
         if(dabs(cop).lt.1.d-10) then
            fiso0=fiso
            fkin0=fkin
         endif
!
         if((kode.eq.-51).or.(ivisco.eq.0)) then
            dcop=(dxitril-c2*(fiso+fkin-fkin0)-um2*cop)/
     &           (um2+c1*(dfiso+dfkin))
         else
            if(user_creep.eq.1) then
               if(ithermal(1).eq.0) then
                  write(*,*) '*ERROR in incplas: no temperature defined'
                  call exit(201)
               endif
               timeabq(1)=time
               timeabq(2)=ttime+time
!
!              Eqn. (5.139)
!
               qtild=(dxitril-um2*cop)/c2-fiso-(fkin-fkin0)
!
!              the Von Mises stress must be positive
!
               if(qtild.lt.1.d-10) qtild=1.d-10
               ec(1)=epini
               call creep(decra,deswa,xstateini(1,iint,iel),serd,ec,
     &             esw,p,qtild,t1l,dtemp,predef,dpred,timeabq,dtime,
     &             amat,leximp,lend,pgauss,nstate_,iel,iint,layer,kspt,
     &             kstep,kinc)
               dsvm=1.d0/decra(5)
               dcop=(decra(1)-c2*cop)/
     &                 (c2*(decra(5)*(dfiso+um*(3.d0+dfkin/um))+1.d0))
            else
               qtild=(dxitril-um2*cop)/c2-fiso-(fkin-fkin0)
!
!              the Von Mises stress must be positive
!
               if(qtild.lt.1.d-10) qtild=1.d-10
               decra(1)=a1*qtild**xxn
               decra(5)=xxn*decra(1)/qtild
               dsvm=1.d0/decra(5)
               dcop=(decra(1)-c2*cop)/
     &                 (c2*(decra(5)*(dfiso+um*(3.d0+dfkin/um))+1.d0))
            endif
         endif
         cop=cop+dcop
!
         if((dabs(dcop).lt.cop*1.d-4).or.
     &      (dabs(dcop).lt.1.d-10)) exit
!
!        check for endless loops or a negative consistency
!        parameter
!
         if((iloop.gt.15).or.(cop.le.0.d0)) then
            pnewdt=0.25d0
            return
         endif
!
      enddo loop
!
!        updating the equivalent plastic strain
!
      ep=epini+c2*cop
!
!     updating the back stress
!
      c7=c2*(fkin-fkin0)/dxitril
      do i=1,6
         stbl(i)=stbl(i)-c7*xitril(i)
      enddo
!
!     updating the plastic strain tensor
!
      c7=cop/dxitril
      do i=1,6
         epl(i)=epl(i)+c7*xitril(i)
      enddo
!
!     trial elastic strain
!
      do i=1,6
         el(i)=emec(i)-epl(i)
      enddo
!
!     trial stress
!
      tracee=el(1)+el(2)+el(3)
      do i=1,6
         stre(i)=um2*el(i)-beta(i)
      enddo
      do i=1,3
         stre(i)=stre(i)+al*tracee
      enddo
!
!     calculating the local stiffness matrix
!
      if(icmd.ne.3) then
         c7=um2**2*cop/dxitril
!
         if((kode.eq.-51).or.(ivisco.eq.0)) then
            c8=um2**2/(um2+c1*(dfiso+dfkin))
         else
            c8=um2**2/(um2+c1*(dfiso+dfkin+1.d0/decra(5)))
         endif
!
         c1=um-c7/2.d0
         c2=al+c7/3.d0
         c3=(c7-c8)/(dxitril**2)
!
         xn(1,1)=xitril(1)
         xn(2,2)=xitril(2)
         xn(3,3)=xitril(3)
         xn(1,2)=xitril(4)
         xn(1,3)=xitril(5)
         xn(2,3)=xitril(6)
         xn(2,1)=xn(1,2)
         xn(3,1)=xn(1,3)
         xn(3,2)=xn(2,3)
!
         do i=1,21
            k=kel(1,i)
            l=kel(2,i)
            m=kel(3,i)
            n=kel(4,i)
            elas(i)=c1*(dkl(k,m)*dkl(l,n)+dkl(k,n)*dkl(l,m))
     &             +c2*dkl(k,l)*dkl(m,n)
     &             +c3*xn(k,l)*xn(m,n)
         enddo
      endif
!
!        updating the plastic fields
!
      do i=1,6
         xstate(1+i,iint,iel)=epl(i)
         xstate(7+i,iint,iel)=stbl(i)
      enddo
      xstate(1,iint,iel)=ep
!
!     maximum difference in the equivalent viscoplastic strain
!     in this increment based on the viscoplastic strain rate at
!     the start and the end of the increment
!
      if(ivisco.eq.1) then
         depvisc=max(depvisc,abs(decra(1)-dtime*xstateini(14,iint,iel)))
         xstate(14,iint,iel)=decra(1)/dtime
      endif
!
!     maximum difference in the equivalent viscoplastic strain
!     in this increment based on the viscoplastic strain rate at
!     the start and the end of the increment
!
      if(ivisco.eq.1) then
         depvisc=max(depvisc,abs(decra(1)-dtime*xstateini(14,iint,iel)))
         xstate(14,iint,iel)=decra(1)/dtime
      endif
c      depvisc=max(depvisc,ep-epini)
!
      return
      end


module KUST_STI
! Description:
!    Contains the procedures necessary for the Kustaanheimo-Stiefel
!    formulation. The procedures evaluate the right-hand side of the KS
!    equations (KS_RHS), provide the event function for LSODAR (KS_EVT),
!    provide coordinate and time transformations (KS_TE2TIME, KS2CART, CART2KS),
!    and compute auxiliary quantities (KS2MAT).
!
! References:
!    [1] Stiefel, E. L. and Scheifele, G. "Linear and Regular Celestial Mechanics",
!     Springer-Verlag, 1971.
!
! Author:
!    Davide Amato
!    Space Dynamics Group - Technical University of Madrid
!    The University of Arizona
!    davideamato@email.arizona.edu
!
! Revisions:
!    180608: change interface to PERT_EJ2K to add iF107.
!    180801: change call to perturbation routine, PERT_EJ2K. Eliminate needless
!            use associations. Refine comments.
!    190110: Add check for collision with the Moon.
!
! ==============================================================================

use KINDS, only: dk
implicit none

contains

! ==============================================================================
! 01. INTEGRATION PROCEDURES
! ==============================================================================

subroutine KS_RHS(neq,s,u,udot)
! Description:
!    Computes the value of the right-hand side of the equations of motion of the
!    Kustaanheimo-Stiefel formulation. The equation for the perturbed harmonic
!    oscillator integrated here is written as:
!
!    u'' + (h/2) * u = -(u/2) * V + (u^2/2) L^T * F
!
!    where F = P - dV/dx is the total perturbation. This is equivalent to
!    Eq. (9, 53) of Ref. [1].
!
! Author:
!    Davide Amato
!    The University of Arizona
!    davideamato@email.arizona.edu
!
! Revisions:
!    180608: change interface to PERT_EJ2K to add iF107.
!    180801: change call to perturbation routine, PERT_EJ2K. Eliminate needless
!            use associations.
!
! ==============================================================================

! MODULES
use PHYS_CONST,    only: GE_nd
use SETTINGS,      only: eqs,insgrav,isun,imoon,idrag,iF107,iSRP
use PERTURBATIONS, only: PERT_EJ2K

! VARIABLES
implicit none
! Arguments
integer,intent(in)       ::  neq                  ! Number of equations.
real(dk),intent(in)      ::  s                    ! Value of fictitious time.
real(dk),intent(in)      ::  u(1:neq)             ! KS state vector.
real(dk),intent(out)     ::  udot(1:neq)          ! RHS of EoM's, ND.
! Local variables
! -- State variables
real(dk)    ::  x(1:4),xdot(1:4)                  ! Radius and velocity in R^4, ND
real(dk)    ::  rmag                              ! Radius magnitude, ND
real(dk)    ::  t                                 ! Physical time, ND
real(dk)    ::  lte1,lte2,lte20(1:4),lte3         ! LTE, auxiliary quantities
integer     ::  flag_time                         ! Time element flag
! -- Perturbations
real(dk)    ::  F(1:4)               ! Total perturbation
real(dk)    ::  P(1:4)               ! Non-potential part
real(dk)    ::  Vpot                 ! Perturbing potential (dimensionless)
real(dk)    ::  mdVdr(1:4),dVdt      ! Partial derivatives of the potential
!real(dk)    ::  rVpot               ! Perturbing potential term
!real(dk)    ::  drVdu(1:4)          ! Derivatives of the perturbing potential term
! -- Misc
real(dk)    ::  L(1:4,1:4)           ! KS matrix

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

! STATE VECTOR DICTIONARY
! u(1:4)        u1,...,u4; KS-position, in R^4
! u(5:8)        u1',...,u4'; KS-velocity, in R^4
! u(9)          h; (-total energy) = (-Keplerian energy) + (-potential)
! u(10)         t or tau; non-dimensional physical time OR time element

! ==============================================================================
! 01. CARTESIAN COORDINATES
! ==============================================================================

call KS2CART(u,x,xdot)
rmag = sqrt(dot_product(x,x))

flag_time = eqs - 5
t = KS_TE2TIME(u,flag_time)

! ==============================================================================
! 02. POTENTIAL PERTURBATIONS
! ==============================================================================

Vpot = 0._dk; mdVdr = 0._dk; dVdt = 0._dk
call PERT_EJ2K(insgrav,0,0,0,0,0,x(1:3),xdot(1:3),rmag,t,mdVdr(1:3),Vpot,dVdt)

! ==============================================================================
! 03. NON-POTENTIAL PERTURBATIONS
! ==============================================================================

P = 0._dk
call PERT_EJ2K(0,isun,imoon,idrag,iF107,iSRP,x(1:3),xdot(1:3),rmag,t,P(1:3))
F = mdVdr + P;

! ==============================================================================
! 04. EVALUATE RIGHT-HAND SIDE
! ==============================================================================

udot = 0._dk; L = 0._dk

! KS matrix
L = KSMAT(u(1:4))

! Velocities
udot(1:4) = u(5:8)

! Accelerations
udot(5:8) = -0.5_dk * ( u(1:4) * (u(9) + Vpot) - rmag * matmul(transpose(L),F) )

! Total energy
udot(9) = -rmag * dVdt - 2._dk * dot_product(u(5:8),matmul(transpose(L),P))

! Time
if (flag_time == 0) then
  ! Generalized Sundman transformation
  udot(10) = rmag

else if (flag_time == 1) then ! Linear Time Element
  ! Eq. (19,62) * 2\omega
  lte1  = (GE_nd - 2._dk*rmag*Vpot)/(2._dk*u(9))
  lte20 = 2._dk * matmul(transpose(L),-F)
  lte2  = (rmag/(4._dk * u(9))) * dot_product(u(1:4),lte20)
  lte3  = udot(9)/(u(9)**2) * dot_product(u(1:4),u(5:8))
  udot(10) = lte1 - lte2 - lte3

end if

end subroutine KS_RHS

function KSMAT(u)
! Description:
!    Computes the KS-matrix from the K-S state vector.
!
! ==============================================================================

! VARIABLES
implicit none
! Arguments
real(dk),intent(in)  ::  u(:)                ! K-S state vector
real(dk)             ::  KSMAT(1:4,1:4)      ! K-S-matrix

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

KSMAT = RESHAPE([ u(1),  u(2),  u(3),  u(4)&
                &,-u(2),  u(1),  u(4), -u(3)&
                &,-u(3), -u(4),  u(1),  u(2)&
                &, u(4), -u(3),  u(2), -u(1)],[4,4])

end function KSMAT

subroutine KS2CART(u,x,xdot)
! Description:
!    Computes the Cartesian state (dimensionless) from the KS state vector.
!
! ==============================================================================

! VARIABLES
implicit none
! Arguments
real(dk),intent(in)   ::  u(:)                  ! KS (extended) state vector
real(dk),intent(out)  ::  x(:),xdot(:)          ! Position and velocity, ND
! Locals
real(dk)              ::  r                     ! Radius magnitude, ND

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

! Position
x(1) = u(1)**2 - u(2)**2 - u(3)**2 + u(4)**2
x(2) = 2._dk*(u(1)*u(2) - u(3)*u(4))
x(3) = 2._dk*(u(1)*u(3) + u(2)*u(4))
r    = u(1)**2 + u(2)**2 + u(3)**2 + u(4)**2

! Velocity
xdot(1) = 2._dk*(u(1)*u(5) - u(2)*u(6) - u(3)*u(7) + u(4)*u(8))/r
xdot(2) = 2._dk*(u(2)*u(5) + u(1)*u(6) - u(4)*u(7) - u(3)*u(8))/r
xdot(3) = 2._dk*(u(3)*u(5) + u(4)*u(6) + u(1)*u(7) + u(2)*u(8))/r

! If x, xdot are in R^4 their fourth component is null.
if (size(x,1) > 3)    x(4:) = 0._dk
if (size(xdot,1) > 3) xdot(4:) = 0._dk

end subroutine KS2CART

subroutine CART2KS(R0_d,V0_d,t0_d,mu,DU,TU,u0,Vpot_d,flag_time)
! Description:
!     Transforms Cartesian position and velocity into the KS state vector.
!
! ==============================================================================

! VARIABLES
implicit none
! Arguments
real(dk),intent(in)     ::  R0_d(1:3),V0_d(1:3)  ! Dimensional position and velocity [km,km/s]
real(dk),intent(in)     ::  t0_d                 ! Dimensional time [s]
real(dk),intent(in)     ::  mu                   ! Grav parameter of the main body [km^3/s^2]
real(dk),intent(in)     ::  DU,TU                ! Ref. quantities for ND [km,1/s]
real(dk),intent(in)     ::  Vpot_d               ! Dimensional potential [km^2/s^2]
integer,intent(in)      ::  flag_time            ! = 0: u(10) is physical time
                                                 ! = 1: u(10) is linear time element
real(dk),intent(out)    ::  u0(:)                ! Extended KS state vector

! Locals
! -- State vectors and associated quantities
real(dk)            ::  x0(1:4)              ! Radius vector in R^4, ND
real(dk)            ::  xdot0(1:4)           ! Velocity vector in R^4, ND
real(dk)            ::  t0                   ! Initial time, ND
real(dk)            ::  r                    ! Radius magnitude, ND
! -- Energies and potentials
real(dk)            ::  h                    ! Total energy, ND
real(dk)            ::  Ksq                  ! Grav parameter of the main body, ND
real(dk)            ::  Kin                  ! Kinetic energy, ND
real(dk)            ::  Vpot                 ! Perturbing potential, ND

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

! STATE VECTOR DICTIONARY
! u(1:4)        u1,...,u4; KS-position, in R^4
! u(5:8)        u1',...,u4'; KS-velocity, in R^4
! u(9)          h; (-total energy) = (-Keplerian energy) + (-potential)
! u(10)         t; non-dimensional physical time or time element

! ==============================================================================
! 01. INITIALIZE POSITION AND VELOCITY IN R^4, NON-DIMENSIONALIZATIONS
! ==============================================================================

x0(1:3) = R0_d/DU
x0(4)   = 0._dk
r       = sqrt(dot_product(x0,x0))
t0      = t0_d*TU

xdot0(1:3) = V0_d/(DU*TU)
xdot0(4)   = 0._dk

! Also, non-dimensionalize potential and grav parameter
Vpot = Vpot_d/(DU*TU)**2
Ksq  = mu/(DU**3*TU**2)

! ==============================================================================
! 02. INITIALIZE KS-POSITION AND KS-VELOCITY (state vector u)
! ==============================================================================

! KS-POSITION
if ( x0(1) >= 0.) then
    u0(1) = 0._dk
    u0(4) = sqrt(.5_dk*(r + x0(1)) - u0(1)**2)
    u0(2) = (x0(2)*u0(1) + x0(3)*u0(4))/(r + x0(1))
    u0(3) = (x0(3)*u0(1) - x0(2)*u0(4))/(r + x0(1))
else
    u0(2) = 0._dk
    u0(3) = sqrt(.5_dk*(r - x0(1)) - u0(2)**2)
    u0(1) = (x0(2)*u0(2) + x0(3)*u0(3))/(r - x0(1))
    u0(4) = (x0(3)*u0(2) - x0(2)*u0(3))/(r - x0(1))
end if

! KS-VELOCITY
u0(5) = .5_dk*dot_product(u0(1:3),xdot0(1:3))
u0(6) = .5_dk*dot_product([-u0(2),u0(1),u0(4)],xdot0(1:3))
u0(7) = .5_dk*dot_product([-u0(3),-u0(4),u0(1)],xdot0(1:3))
u0(8) = .5_dk*dot_product([u0(4),-u0(3),u0(2)],xdot0(1:3))

! Total energy
Kin   = .5_dk*dot_product(xdot0,xdot0)
h     = Ksq/r - Kin - Vpot
u0(9) = h

! Initial time
if (flag_time == 0) then
  u0(10) = t0

else if (flag_time == 1) then
  u0(10) = t0 + dot_product(u0(1:4),u0(5:8))/u0(9)

end if

end subroutine CART2KS

subroutine KS_EVT(neq,phi,u,ng,roots)
! Description:
!    Finds roots to stop the integration for the KS formulation.
!
! Revisions:
!    190110: Add header comments. Add check for collision with the Moon.
!
! ==============================================================================

! MODULES
use SUN_MOON,    only: EPHEM
use AUXILIARIES, only: MJD0,MJDnext,MJDf,DU,TU
use PHYS_CONST,  only: secsPerDay,RE,reentry_radius_nd,ReqM
use SETTINGS,    only: eqs,imoon,imcoll

! VARIABLES
implicit none
! Arguments IN
integer,intent(in)    ::  neq
integer,intent(in)    ::  ng
real(dk),intent(in)   ::  phi
real(dk),intent(in)   ::  u(1:neq)
! Arguments OUT
real(dk),intent(out)  ::  roots(1:ng)

! Locals
real(dk)  ::  t         ! Current time [-]
real(dk)  ::  rmag, dmag, x(1:4), xdot(1:4), r_vec(1:3)
real(dk)  ::  rMoon(1:3), vMoon(1:3)
integer   ::  flag_time

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

roots = 1._dk

! Get time
flag_time = eqs - 5
t = KS_TE2TIME(u,flag_time)

! ==============================================================================
! 01. Next timestep
! ==============================================================================

roots(1) = t - (MJDnext - MJD0)*secsPerDay*TU

! ==============================================================================
! 02. Stop integration
! ==============================================================================

roots(2) = t - (MJDf - MJD0)*secsPerDay*TU

! ==============================================================================
! 03. Earth re-entry
! ==============================================================================

rmag  = dot_product(u(1:4),u(1:4))

roots(3) = rmag - reentry_radius_nd

! ==============================================================================
! 04. Moon collision (only active when Moon is present)
! ==============================================================================

! All quantities are dimensional. Accuracy could be improved by using
! dimensionless quantities, but this would require
! non-dimensionalizing ReqM in some part of the code, which would worsen the
! code reliability.
roots(4) = 1.
if (imoon > 0 .and. (imcoll /= 0)) then
  call KS2CART(u,x,xdot)
  r_vec = x(1:3) * DU
  call EPHEM(2, 1._dk, 1._dk, t, rMoon, vMoon)
  dmag = sqrt(dot_product( (r_vec - rMoon), (r_vec - rMoon) ) )
  roots(4) = dmag - ReqM

end if

end subroutine KS_EVT

function KS_TE2TIME(u,flag_time)
! Description:
!    Gets the value of physical time from the KS state vector "u" and
!    fictitious time "s".
!
! ==============================================================================

! VARIABLES
implicit none
! Arguments IN
real(dk),intent(in)     ::  u(1:10)
integer,intent(in)      ::  flag_time
! Function definition
real(dk)                ::  KS_TE2TIME

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

if ( flag_time == 0 ) then
    ! Physical time
    KS_TE2TIME = u(10)

else if  ( flag_time == 1 ) then
    ! Linear time element
    KS_TE2TIME = u(10) - dot_product(u(1:4),u(5:8))/u(9)

end if

end function KS_TE2TIME

end module KUST_STI


!
!    Copyright 2011 Francesco Pacchiani & Sebastian Heimann
!
!    Licensed under the Apache License, Version 2.0 (the "License");
!    you may not use this file except in compliance with the License.
!    You may obtain a copy of the License at
!
!        http://www.apache.org/licenses/LICENSE-2.0
!
!    Unless required by applicable law or agreed to in writing, software
!    distributed under the License is distributed on an "AS IS" BASIS,
!    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
!    See the License for the specific language governing permissions and
!    limitations under the License.
!

module interpolation

    use constants
    use util
    implicit none
    include 'fftw3.f'

    public gulunay2d, gulunay3d

  contains

    subroutine gulunay2d(A,Inter, ntmargin, nxmargin)

      ! Interpolate Green's function using the Generalized f-k interpolation method,
      ! also known as Gulunay's [2003] method.

        real, intent(inout), dimension(:,:)   :: A       ! (t,s)
        real, intent(out), dimension(:,:)     :: Inter   ! (t,kk)
        integer, intent(in)                   :: ntmargin, nxmargin

        integer                               :: t,s, kk, ff, fny
        integer                               :: x
        integer*8                             :: plan
        real                                  :: m
        integer                               :: l, il

        real,    dimension(size(A,1),                           size(Inter,2))  :: B
        real,    dimension(size(A,1)*(size(Inter,2)/size(A,2)), size(Inter,2))  :: C,D
        complex, dimension(size(A,1)/2+1,                       size(Inter,2))  :: fB, fInter, Operator   ! (t/2+1,kk)
        complex, dimension(size(C,1)/2+1,                       size(Inter,2))  :: fC, fD                 ! (ff/2+1,kk)

      ! check sizes

        if (size(Inter,1) /= size(A,1)) then
            call die( "gulunay2d(): time length of input and output arrays do not match")
        end if
        if (mod(size(Inter,2),size(A,2)) /= 0) then
            call die( "gulunay2d(): number of traces in ouput array is not a multiple of size of input array")
        end if

        l = size(Inter,2)/size(A,2)

        t=size(A,1)
        s=size(A,2)

        kk=size(Inter,2)
        ff=l*t

        ! --- Taper ---
        !
        do x=1,nxmargin/l
            A(:,x) = A(:,x) * (1. - cos(2.*pi*((x-1)/(2.*nxmargin/l)))) / 2.
        end do

        do x=s-nxmargin/l+1,s
            A(:,x) = A(:,x) * (1. - cos(2.*pi*((s-x)/(2.*nxmargin/l)))) / 2.
        end do

        do x=1,ntmargin/l
            A(x,:) = A(x,:) * (1. - cos(2.*pi*((x-1)/(2.*ntmargin/l)))) / 2.
        end do

        do x=t-ntmargin/l+1,t
            A(x,:) = A(x,:) * (1. - cos(2.*pi*((t-x)/(2.*ntmargin/l)))) / 2.
        end do

        !
        ! --- Insert zero traces in original data + 2-D FFT ---
        !
        B(:,1:kk:l) = A(:,:)
        do il=2,l
            B(:,il:kk:l) = 0.
        end do

        call sfftw_plan_dft_r2c_2d(plan,t,kk,B,fB,FFTW_ESTIMATE)
        call sfftw_execute(plan)
        call sfftw_destroy_plan(plan)

        !
        ! --- Zero pad the original data in all dimensions + 2-D FFT ---
        !
        C(:,:) = 0.
        C(1:t,1:s) = A(:,:)

        call sfftw_plan_dft_r2c_2d(plan,ff,kk,C,fC,FFTW_ESTIMATE)
        call sfftw_execute(plan)
        call sfftw_destroy_plan(plan)

        !
        ! --- Replace even traces in C with zeros + 2-D FFT ---
        !
        D(:,1:s:l) = C(:,1:s:l)
        do il=2,l
            D(:,il:s:l) = 0.
        end do

        call sfftw_plan_dft_r2c_2d(plan,ff,kk,D,fD,FFTW_ESTIMATE)
        call sfftw_execute(plan)
        call sfftw_destroy_plan(plan)

        !
        ! --- Add white noise to spectrum ---
        !
        fny = t/2+1
        m = 0.01*maxval( abs(fD(fny,:)) )
        where (abs(fD(1:fny,:)) .lt. m/1000.)
            fD(1:fny,:) = cmplx(m,aimag(fD(1:fny,:)))
        end where

        where (abs(fD(1:fny,:)) .lt. m)
            fD(1:fny,:) = m/abs(fD(1:fny,:)) * fD(1:fny,:)
        end where

        !
        ! --- Calculate the interpolator ---
        !
        Operator(:,:) = fC(1:fny,:) / fD(1:fny,:)

        !
        ! --- Clip the iterpolation operator ---
        !
        where (abs(Operator) .gt. l)
            Operator = l/abs(Operator) * Operator ! cmplx(l,aimag(Operator))
        end where

        where (abs(Operator) .lt. l*0.5)
            Operator = 0. ! cmplx(0.,aimag(Operator))
        end where

        !
        ! --- Compute interpolated traces ---
        !
        fInter(:,:) = fB(:,:) * Operator(:,:) / (t*kk)

        !
        ! --- Return in the time domain ---
        !
        call sfftw_plan_dft_c2r_2d(plan,t,kk,fInter,Inter,FFTW_ESTIMATE)
        call sfftw_execute(plan)
        call sfftw_destroy_plan(plan)

    end subroutine

    subroutine gulunay3d(A,Inter, ntmargin, nxmargin, nzmargin)

        !  Interpolate Green's function using the Generalized f-k interpolation method,
        !  also known as Gulunay's [2003] method.

        real, intent(inout), dimension(:,:,:)   :: A       ! (t,sz,sx)
        real, intent(out), dimension(:,:,:)     :: Inter   ! (t,kkz,kkx)
        integer, intent(in)                   :: ntmargin, nxmargin, nzmargin

        integer                               :: t,sx,sz, kkx,kkz, ff, fny
        integer                               :: x
        integer*8                             :: plan
        integer                               :: l, il
        real                                  :: m, ls, lowcut

        real,    dimension(size(A,1),                           size(Inter,2), size(Inter,3))  :: B
        real,    dimension(size(A,1)*(size(Inter,2)/size(A,2)), size(Inter,2), size(Inter,3))  :: C,D
        complex, dimension(size(A,1)/2+1,                       size(Inter,2), size(Inter,3))  :: fB, fInter, Operator   ! (t/2+1,kk)
        complex, dimension(size(C,1)/2+1,                       size(Inter,2), size(Inter,3))  :: fC, fD                 ! (ff/2+1,kk)

        if (size(Inter,1) /= size(A,1)) then
            call die( "gulunay3d(): time length of input and output arrays do not match")
        end if

        if (size(Inter,2)/size(A,2) /= size(Inter,3)/size(A,3)) then
            call die( "gulunay3d(): proportions of dimensions 2 and 3 between input and output arrays do not match")
        end if

        if (mod(size(Inter,2),size(A,2)) /= 0 .or. mod(size(Inter,3),size(A,3)) /= 0 ) then
            call die( "gulunay3d(): number of traces in ouput array is not a multiple of size of input array")
        end if

        l = size(Inter,2)/size(A,2)

        t=size(A,1)
        sz=size(A,2)
        sx=size(A,3)

        kkx=l*sx
        kkz=l*sz

        ff=l*t

        !
        ! --- Taper ---
        !
        do x=1,nxmargin/l
            A(:,:,x) = A(:,:,x) * (1. - cos(2.*pi*((x-1)/(2.*nxmargin/l)))) / 2.
        end do

        do x=sx-nxmargin/l+1,sx
            A(:,:,x) = A(:,:,x) * (1. - cos(2.*pi*((sx-x)/(2.*nxmargin/l)))) / 2.
        end do

        do x=1,nzmargin/l
            A(:,x,:) = A(:,x,:) * (1. - cos(2.*pi*((x-1)/(2.*nzmargin/l)))) / 2.
        end do

        do x=sz-nzmargin/l+1,sz
            A(:,x,:) = A(:,x,:) * (1. - cos(2.*pi*((sz-x)/(2.*nzmargin/l)))) / 2.
        end do

        do x=1,ntmargin/l
            A(x,:,:) = A(x,:,:) * (1. - cos(2.*pi*((x-1)/(2.*ntmargin/l)))) / 2.
        end do

        do x=t-ntmargin/l+1,t
            A(x,:,:) = A(x,:,:) * (1. - cos(2.*pi*((t-x)/(2.*ntmargin/l)))) / 2.
        end do

        !
        ! --- Insert zero traces in original data + 2-D FFT ---
        !
        B(:,1:kkz:l,1:kkx:l) = A(:,:,:)
        do il=2,l
            B(:,il:kkz:l,il:kkx:l) = 0.
        end do

        call sfftw_plan_dft_r2c_3d(plan,t,kkz,kkx,B,fB,FFTW_ESTIMATE)
        call sfftw_execute(plan)
        call sfftw_destroy_plan(plan)

        !
        ! --- Zero pad the original data in all dimensions + 2-D FFT ---
        !
        C(:,:,:) = 0.
        C(1:t,1:sz,1:sx) = A(:,:,:)

        call sfftw_plan_dft_r2c_3d(plan,ff,kkz,kkx,C,fC,FFTW_ESTIMATE)
        call sfftw_execute(plan)
        call sfftw_destroy_plan(plan)

        !
        ! --- Replace even traces in C with zeros + 2-D FFT ---
        !
        D(:,1:sz:l,1:sx:l) = C(:,1:sz:l,1:sx:l)
        do il=2,l
            D(:,il:sz:l,il:sx:l) = 0.
        end do

        call sfftw_plan_dft_r2c_3d(plan,ff,kkz,kkx,D,fD,FFTW_ESTIMATE)
        call sfftw_execute(plan)
        call sfftw_destroy_plan(plan)

        !
        ! --- Add white noise to spectrum ---
        !
        fny = t/2+1
        m = 0.01*maxval( abs(fD(fny,:,:)) )
        where (abs(fD(1:fny,:,:)) .lt. m/1000.)
            fD(1:fny,:,:) = cmplx(m,aimag(fD(1:fny,:,:)))
        end where

        where (abs(fD(1:fny,:,:)) .lt. m)
            fD(1:fny,:,:) = m/abs(fD(1:fny,:,:)) * fD(1:fny,:,:)
        end where

        !
        ! --- Calculate the interpolator ---
        !
        Operator(:,:,:) = fC(1:fny,:,:) / fD(1:fny,:,:)

        !
        ! --- Clip the iterpolation operator ---
        !

        lowcut = 0.5*l**2
        ls = l**2

        where (abs(Operator) .gt. ls)
            Operator = ls/abs(Operator) * Operator
        end where

        where (abs(Operator) .lt. lowcut)
            Operator = 0.
        end where

        !
        ! --- Compute interpolated traces ---
        !
        fInter(:,:,:) = fB(:,:,:) * Operator(:,:,:) / (t*kkx*kkz)

        !
        ! --- Return in the time domain ---
        !
        call sfftw_plan_dft_c2r_3d(plan,t,kkz,kkx,fInter,Inter,FFTW_ESTIMATE)
        call sfftw_execute(plan)
        call sfftw_destroy_plan(plan)

    end subroutine

end module


c
c    This file contains the translation operator for the Laplacd
c      3D FMM
c
c     l3mpmp - shift center of multipole expansion
c     l3dmploc - convert multipole expansion to a local expansion
c     l3locloc - shift center of local expansion
c
c
C
c
C***********************************************************************
      subroutine l3dmpmp(nd,sc1,x0y0z0,mpole,nterms,sc2,
     1           xnynzn,mpolen,nterms2,dc,lca)
C***********************************************************************
C
C     Usage:
C
C     Shift center of multipole expansion mpole and add to output
C     This is a reasonably fast "point and shoot" version which
C     first rotates the coordinate system, then doing the shifting
C     along the Z-axis, and then rotating back to the original
C     coordinates.
C
C---------------------------------------------------------------------
C     INPUT:
C
C     nd      : number of mulitpole expansions
C     sc1     : scaling parameter for mpole expansion
C     x0y0z0  : center of original multiple expansion
C     mpole   : coefficients of original multiple expansion
C     nterms  : order of multipole expansion
C     sc2     : scaling parameter for shifted expansion
C     xnynzn  : center of shifted expansion
C     nterms2 : order of shifted expansion
C     dc      : square root of binomial coefficients
C     lca     : length of binomial coefficients
C
C---------------------------------------------------------------------
C     OUTPUT:
C
C     mpolen  : coefficients of shifted expansion
C
C***********************************************************************
C
      implicit none
      integer  nterms, lw, lused, ier, nq, nquad, nquse,ldc,nterms2
      integer lca
      integer idim,nd
      double precision x0y0z0(3),xnynzn(3)
      double precision rshift
      double precision d,theta,ctheta,phi,sc1,sc2,rvec(3)
      double precision dc(0:lca,0:lca)
      double complex mpole(nd,0:nterms,-nterms:nterms)
      double complex mpolen(nd,0:nterms2,-nterms2:nterms2)
      double complex, allocatable :: marray1(:,:,:)
      double complex, allocatable :: marray(:,:,:)
      double complex, allocatable :: ephi(:)
c
      double complex imag
      integer  l,m,jnew,knew
      data imag/(0.0d0,1.0d0)/
C
      rvec(1) = xnynzn(1) - x0y0z0(1)
      rvec(2) = xnynzn(2) - x0y0z0(2)
      rvec(3) = xnynzn(3) - x0y0z0(3)
      call cart2polar(rvec,d,theta,phi)

      ldc = max(nterms,nterms2)
      allocate(marray1(nd,0:ldc,-ldc:ldc))
      allocate(marray(nd,0:ldc,-ldc:ldc))
      allocate(ephi(-ldc-1:ldc+1))

c
      ephi(1) = exp(imag*phi)
      ephi(0)=1.0d0
      ephi(-1)=dconjg(ephi(1))
c
c----- create array of powers e^(i*m*phi).
c
      do l = 1,ldc
         ephi(l+1) = ephi(l)*ephi(1)
         ephi(-1-l) = dconjg(ephi(l+1))
      enddo
c
c----- a rotation of THETA radians about the Yprime axis after PHI
c      radians about the z-axis.
c      The PHI rotation is carried out on the fly by multiplying
c      mpole and ephi inside the following loop.
c
      do l=0,nterms
        do m=-l,l
          do idim=1,nd
            marray1(idim,l,m)=mpole(idim,l,m)*ephi(m)
          enddo
        enddo
      enddo

      if(nterms.ge.30) then
        call rotviaproj(nd,theta,nterms,nterms,nterms,marray1,
     1         ldc,marray,ldc)
      else
        call rotviarecur(nd,theta,nterms,nterms,nterms,marray1,
     1        ldc,marray,ldc)
      endif

c
c
c----- shift the mpole expansion from X0Y0Z0 to XNYNZN along
c      the Z-axis.
c
      rshift = d
      call l3dmpmpzshift(nd,sc1,marray,ldc,nterms,sc2,marray1,
     1           ldc,nterms2,rshift,dc,lca)
c
c
c     Reverse THETA rotation.
c     I.e. rotation of -THETA radians about Yprime axis.
c

      if(nterms2.ge.30) then
        call rotviaproj(nd,-theta,nterms2,nterms2,nterms2,marray1,
     1        nterms2,marray,ldc)
      else
        call rotviarecur(nd,-theta,nterms2,nterms2,nterms2,marray1,
     1        nterms2,marray,ldc)
      endif
c
c
c----- rotate back PHI radians about the Z-axis in the above system.
c
      do l=0,nterms2
        do m=-l,l
          do idim=1,nd
            mpolen(idim,l,m)=mpolen(idim,l,m)+ephi(-m)*marray(idim,l,m)
          enddo
        enddo
      enddo

      return
      end
c
c
c
c
c**********************************************************************
      subroutine l3dmpmpzshift(nd,scale,mpole,lmp,nterms,scale2,mpolen,
     1      lmpn,nterms2,zshift,dc,lca)
c***********************************************************************
c
c     This subroutine converts a multipole expansion centered at the
c     origin to a multipole expansion centered at (0,0,zhift).
c     The expansion is rescaled to that of the shifted expansion.
c
c     INPUT:
c
c     nd       : number of multipole expansions
c     scale    : scale parameter for mpole
c     mpole    : coefficients of original multipole exp.
c     lmp      : leading dim of mpole (may be a work array)
c     nterms   : number of terms in the orig. expansion
c     scale2   : scale parameter for new expansion (mpolen)
c     lmpn     : leading dim of shifted (may be work array)
c     nterms2  : number of terms in output expansion
c     zshift   : shifting distance along z-axis
c                              (always assumed positive)
c     dc       : square root of binomial coefficients
c     lca      : length of binomial coefficient array
c
c     OUTPUT:
c
c     mpolen  (double complex)  : coefficients of shifted exp.
c
c-----------------------------------------------------------------------
      implicit none
      integer l0,nmax,nterms,nterms2,nquad,ier,lmp,lmpn,ldc,nd
      integer lca
      integer l,m,jnew,knew,i,idim
      double precision d,zshift,scale,scale2,rat
      double precision, allocatable :: rscpow(:)
      double complex mpole(nd,0:lmp,-lmp:lmp)
      double complex mpolen(nd,0:lmpn,-lmpn:lmpn)
      double precision dc(0:lca,0:lca)
      double precision, allocatable :: fr(:)
C
C----- shift along z-axis
C
      nmax = max(nterms,nterms2)
      allocate(fr(0:nmax))
c
      fr(0) = 1.0d0
      d = -zshift/scale
      fr(1) = d
      do l=2,nmax
         fr(l) = fr(l-1)*d
      enddo

      d = scale/scale2
      allocate(rscpow(nterms2))
      rscpow(1) = d
      do i=2,nterms2
        rscpow(i) = rscpow(i-1)*d
      enddo
c
      do jnew = 0,nterms2
        l0 = max(0,jnew-nterms)
        do knew = -jnew,jnew
          do idim=1,nd
	        mpolen(idim,jnew,knew) = 0.0d0
          enddo
	      do l=l0,jnew-iabs(knew)
            do idim=1,nd
              mpolen(idim,jnew,knew)=mpolen(idim,jnew,knew)+
     1              mpole(idim,jnew-l,knew)*fr(l)*dc(jnew-knew,l)*
     2              dc(jnew+knew,l)
            enddo
          enddo
        enddo
      enddo

      do jnew = 1,nterms2
        do knew = -jnew,jnew
          do idim=1,nd
            mpolen(idim,jnew,knew)=mpolen(idim,jnew,knew)*rscpow(jnew)
          enddo
        enddo
      enddo

      return
      end
C
c
c
c
c
C***********************************************************************
      subroutine l3dmploc(nd,sc1,x0y0z0,mpole,nterms,
     1           sc2,xnynzn,local,nterms2,dc,lca)
C***********************************************************************

C     USAGE:
C
C           Convert multipole expansion to a local expansion and add
C           to existing expansion
C           This is a reasonably fast "point and shoot" version which
C           first rotates the coordinate system, then doing the shifting
C           along the Z-axis, and then rotating back to the original
C           coordinates.
C
C---------------------------------------------------------------------
C     INPUT:
C     nd         number of multipole expansions
C     sc1        scaling parameter for mpole expansion
C     x0y0z0     center of original multiple expansion
C     mpole      coefficients of original multiple expansion
C     nterms     order of multipole expansion
C     sc2        scaling parameter for local expansion
C     xnynzn     center of shifted local expansion
C     nterms2    order of local expansion
C     dc         precomputed square roots of binomial coefficients
C     lca        length of precomputed binomial coefficients
C
C---------------------------------------------------------------------
C     OUTPUT:
C
C     local      coefficients of shifted local expansion
C---------------------------------------------------------------------
      implicit none
      integer idim,nd,lca
      integer  nterms,ier,l,m,jnew,knew,nterms2,ldc,mp
      double precision d,theta,ctheta,phi,sc1,sc2
      double precision x0y0z0(3),xnynzn(3)
      double precision rvec(3)
      double precision rshift
      double precision dc(0:lca,0:lca)
      double complex, allocatable :: marray1(:,:,:),marray(:,:,:),
     1    ephi(:)
      double complex mpole(nd,0:nterms,-nterms:nterms)
      double complex local(nd,0:nterms2,-nterms2:nterms2)
      double complex imag
      data imag/(0.0d0,1.0d0)/
C
      rvec(1) = xnynzn(1) - x0y0z0(1)
      rvec(2) = xnynzn(2) - x0y0z0(2)
      rvec(3) = xnynzn(3) - x0y0z0(3)
      call cart2polar(rvec,d,theta,phi)
c

      ldc = max(nterms,nterms2)
      allocate(ephi(-ldc-1:ldc+1))
      allocate(marray(nd,0:ldc,-ldc:ldc),marray1(nd,0:ldc,-ldc:ldc))

      ephi(1) = exp(imag*phi)
      ephi(0)=1.0d0
      ephi(-1)=dconjg(ephi(1))
c
c     create array of powers e^(i*m*phi).
c
      do l = 1,ldc
         ephi(l+1) = ephi(l)*ephi(1)
         ephi(-1-l) = dconjg(ephi(l+1))
      enddo
c
c     a rotation of THETA radians about the Yprime axis after PHI
c     radians about the z-axis.
c     The PHI rotation is carried out on the fly by multiplying
c     mpole and ephi inside the following loop.
c
      do l=0,nterms
        do mp=-l,l
          do idim=1,nd
            marray1(idim,l,mp) = mpole(idim,l,mp)*ephi(mp)
          enddo
        enddo
      enddo

      if(nterms.ge.30) then
        call rotviaproj(nd,theta,nterms,nterms,nterms,marray1,
     1     ldc,marray,ldc)
      else
        call rotviarecur(nd,theta,nterms,nterms,nterms,marray1,
     1     ldc,marray,ldc)
      endif
c
c
c----- shift the local expansion from X0Y0Z0 to XNYNZN along
c      the Z-axis.
c
      rshift = d
      call l3dmploczshift(nd,marray,sc1,ldc,nterms,marray1,
     1      sc2,nterms2,nterms2,rshift,dc,lca)

c
c     reverse THETA rotation.
c     I.e. rotation of -THETA radians about the Yprime axis.
c
      if(nterms.ge.30) then
        call rotviaproj(nd,-theta,nterms2,nterms2,nterms2,marray1,
     1     ldc,marray,ldc)
      else
        call rotviarecur(nd,-theta,nterms2,nterms2,nterms2,marray1,
     1     ldc,marray,ldc)
      endif
c
c----- rotate back PHI radians about the Z-axis in the above system.
c
      do l=0,nterms2
        do m=-l,l
          do idim=1,nd
            local(idim,l,m)=local(idim,l,m)+ephi(-m)*marray(idim,l,m)
          enddo
        enddo
      enddo

      return
      end
c
c
c***********************************************************************
      subroutine l3dmploczshift(nd,mpole,scale,lmp,nterms,local,
     2      scale2,lmpn,nterms2,zshift,dc,lca)
c***********************************************************************
c
c     This subroutine converts a multipole expansion centered at the
c     origin to a local expansion centered at (0,0,zhift).
c     The expansion is rescaled to that of the local expansion.
c
C---------------------------------------------------------------------
c     INPUT:
c     nd       : number of multipole expansions
c     mpole    : coefficients of original multipole exp.
c     scale    : scale parameter for mpole
c     lmp      : leading dim of mpole (may be a work array)
c     nterms   : number of terms in original expansion
c
c     scale2   : scale parameter for local
c     lmpn     : leading dim of local (may be a work array)
c     nterms2  : number of terms in output local exp.
c     zshift   : shifting distance along z-axis
c                             (always assumed positive)
c     dc       : square root of binomial coefficients
c     lca      : length of dc
c
C---------------------------------------------------------------------
c     OUTPUT:
c
c     local    : coefficients of shifted local exp.
C---------------------------------------------------------------------
      implicit none
      integer idim,nd,lca
      integer nterms,nterms2,nquad,ier
      integer l,lw,m,jnew,knew,kk,ll,msign,nmax,lmpn,lmp
      double precision scale,scale2
      double precision zshift,d
      double precision dc(0:lca,0:lca)
      double precision, allocatable :: fr(:)
      double precision, allocatable :: rscpow(:)
      double complex mpole(nd,0:lmp,-lmp:lmp)
      double complex local(nd,0:lmpn,-lmpn:lmpn)
C
C----- shift along z-axis by evaluating field on target sphere and
C     projecting onto spherical harmonics and scaling by j_n(kR).
C
      nmax = max(nterms,nterms2)
      allocate(rscpow(nmax),fr(0:2*nmax))
c
      d = 1.0d0/zshift
      fr(0) = d
      d = d*scale
      fr(1) = fr(0)*d
      do l=2,2*nmax
        fr(l) = fr(l-1)*d
      enddo

      d = scale2/scale
      rscpow(1) = d
      do l=2,nmax
        rscpow(l) = rscpow(l-1)*d
      enddo
c
      do jnew = 0,nterms2
        do knew = -jnew,jnew
          do idim=1,nd
   	        local(idim,jnew,knew) = 0.0d0
          enddo
          kk  =iabs(knew)
          msign = (-1)**(jnew+knew)
          do l=kk,nterms
            ll=l+jnew
            do idim=1,nd
              local(idim,jnew,knew)=local(idim,jnew,knew)+
     1          mpole(idim,l,knew)*fr(ll)*dc(ll,l-knew)*
     2          dc(ll,l+knew)*msign
            enddo
          enddo
        enddo
      enddo

      do jnew = 1,nterms2
        do knew = -jnew,jnew
          do idim=1,nd
            local(idim,jnew,knew)=local(idim,jnew,knew)*rscpow(jnew)
          enddo
        enddo
      enddo

      return
      end
C
c
c
c
c
c
C***********************************************************************
C     Local -> Local routines
C***********************************************************************
c
c
C***********************************************************************
      subroutine l3dlocloc(nd,sc1,x0y0z0,locold,nterms,sc2,
     1           xnynzn,local,nterms2,dc,lda)
C***********************************************************************
C
C     Usage:
C
C           Shifts center of a local expansion and add to existing
C           expansion
C           This is a reasonably fast "point and shoot" version which
C           first rotates the coordinate system, then doing the shifting
C           along the Z-axis, and then rotating back to the original
C           coordinates.
C
C---------------------------------------------------------------------
C     INPUT:
C
C     nd      : number of local expansions
C     sc1     : scaling parameter for locold expansion
C     x0y0z0  : center of original multiple expansion
C     locold  : coefficients of original multiple expansion
C     nterms  : order of original local expansion
C     sc2     : scaling parameter for local expansion
C     xnynzn  : center of shifted local expansion
C     nterms2 : order of new local expansion
C
C---------------------------------------------------------------------
C     OUTPUT:
C
C     local   : coefficients of shifted local expansion
C
C***********************************************************************
C
      implicit none
      integer idim,lda,nd
      integer nterms,ier,l,m,jnew,knew,nterms2,ldc,mp
      double precision x0y0z0(3),xnynzn(3),rvec(3)
      double precision d,theta,ctheta,phi,sc1,sc2
      double precision dc(0:lda,0:lda)
      double precision rshift
      double complex locold(nd,0:nterms,-nterms:nterms)
      double complex local(nd,0:nterms2,-nterms2:nterms2)
      double complex, allocatable :: marray(:,:,:),marray1(:,:,:),
     1                  ephi(:)
      double complex imag,ephi1
      data imag/(0.0d0,1.0d0)/
C
      rvec(1) = xnynzn(1) - x0y0z0(1)
      rvec(2) = xnynzn(2) - x0y0z0(2)
      rvec(3) = xnynzn(3) - x0y0z0(3)

      call cart2polar(rvec,d,theta,phi)

      ldc = max(nterms,nterms2)
      allocate(ephi(-ldc-1:ldc+1))

      ephi1 = exp(imag*phi)
      ephi(0)=1.0d0
      ephi(1)=ephi1
      ephi(-1)=dconjg(ephi1)
c
c----- create array of powers e^(i*m*phi).
c
      do l = 1,ldc
         ephi(l+1) = ephi(l)*ephi(1)
         ephi(-1-l) = dconjg(ephi(l+1))
      enddo

      allocate(marray(nd,0:ldc,-ldc:ldc),marray1(nd,0:ldc,-ldc:ldc))
c
c      a rotation of THETA radians about the Yprime-axis after PHI
c      radians about the z-axis.
c      The PHI rotation is carried out on the fly by multiplying
c      locold and ephi inside the following loop.
c
      do l=0,nterms
        do mp=-l,l
          do idim=1,nd
            marray1(idim,l,mp) = locold(idim,l,mp)*ephi(mp)
          enddo
        enddo
      enddo

      if(nterms.ge.30) then
        call rotviaproj(nd,theta,nterms,nterms,nterms,marray1,
     1     ldc,marray,ldc)
      else
        call rotviarecur(nd,theta,nterms,nterms,nterms,marray1,
     1     ldc,marray,ldc)
      endif
c
c----- shift the local expansion from X0Y0Z0 to XNYNZN along
c      the Z-axis.
c
      rshift = d
       call l3dlocloczshift(nd,sc1,marray,ldc,nterms,sc2,marray1,
     1           nterms2,nterms2,rshift,dc,lda)
c
c     reverse THETA rotation.
c     I.e. rotation of -THETA radians about the Yprime axis.
c
      if(nterms.ge.30) then
        call rotviaproj(nd,-theta,nterms2,nterms2,nterms2,marray1,
     1     ldc,marray,ldc)
      else
        call rotviarecur(nd,-theta,nterms2,nterms2,nterms2,marray1,
     1     ldc,marray,ldc)
      endif
c
c----- rotate back PHI radians about the Z-axis in the above system.
c
      do l=0,nterms2
        do m=-l,l
          do idim=1,nd
            local(idim,l,m)=local(idim,l,m)+ephi(-m)*marray(idim,l,m)
          enddo
        enddo
      enddo

      return
      end
c
c
c
c
c***********************************************************************
      subroutine l3dlocloczshift(nd,scale,locold,lmp,nterms,scale2,
     1  local,lmpn,nterms2,zshift,dc,lda)
c***********************************************************************
c
c     This subroutine converts a multipole expansion centered at the
c     origin to a local expansion centered at (0,0,zhift).
c     The expansion is rescaled to that of the local expansion.
c
c     INPUT:
c
c     nd       : number of local expansions
c     scale    : scaling parameter for locold
c     locold   : coefficients of original multipole exp.
c     lmp      : leading dim of locold (may be a work array)
c     nterms   : number of terms in the orig. expansion
c
c     scale2   : scaling parameter for output expansion (local)
c     lmpn     : leading dim of local (may be a work array)
c     nterms2  : number of terms in output local exp.
c     zshift   : shifting distance along z-axis (assumed positive)
c
c     OUTPUT:
c
c     local    : coefficients of shifted local exp.
c-----------------------------------------------------------------------
      implicit none
      integer idim,nd,lda
      integer nmax,nterms,nterms2,nquad,ier,l,lw,m,jnew,knew
      integer lmp,lmpn,ll
      double precision  zshift,d
      double precision  scale,scale2
      double precision, allocatable :: fr(:),rscpow(:)
      double complex locold(nd,0:lmp,-lmp:lmp)
      double complex local(nd,0:lmpn,-lmpn:lmpn)
      double precision dc(0:lda,0:lda)
C
C----- shift along z-axis
C
      nmax = max(nterms,nterms2)

      allocate(fr(0:nmax),rscpow(nmax))
c
c
      d = zshift
      fr(0) = 1.0d0
      d = d/scale
      fr(1) = d
      do l=2,nterms
         fr(l) = fr(l-1)*d
      enddo

      d = scale2/scale
      rscpow(1) = d
      do l=2,nterms
        rscpow(l) = rscpow(l-1)*d
      enddo

      do jnew = 0,nterms2
        do knew = -jnew,jnew
          do idim=1,nd
            local(idim,jnew,knew) = locold(idim,jnew,knew)
          enddo

          do l=1,nterms-jnew
            ll = l+jnew
            do idim=1,nd
              local(idim,jnew,knew)=local(idim,jnew,knew)+
     1         locold(idim,ll,knew)*fr(l)*dc(ll+knew,l)*dc(ll-knew,l)
            enddo
          enddo
        enddo
      enddo

      do jnew = 1,nterms2
        do knew = -jnew,jnew
          do idim=1,nd
            local(idim,jnew,knew)=local(idim,jnew,knew)*rscpow(jnew)
          enddo
        enddo
      enddo

      return
      end
c


program test_sum_vec ! GitHub FortranTip xsum_vec.f90
use iso_c_binding, only: c_double, c_int
implicit none
interface
pure function sum_vec(x, n) result(xsum) bind(c)
import c_double, c_int
integer(kind=c_int), intent(in), value :: n
real(kind=c_double), intent(in)        :: x(n)
real(kind=c_double)                    :: xsum
end function sum_vec
end interface
integer(kind=c_int), parameter :: n = 3
real(kind=c_double) :: x(n) = real([10,20,30], kind=c_double)
print "(*(1x,f0.1))",x,sum_vec(x,n) ! 10.0 20.0 30.0 60.0
end program test_sum_vec
! compile with
! gcc -c -o sum_vec.o sum_vec.c
! gfortran sum_vec.o xsum_vec.f90


      subroutine daxpy(n,da,dx,incx,dy,incy)
c
c     constant times a vector plus a vector.
c     uses unrolled loops for increments equal to one.
c     jack dongarra, linpack, 3/11/78.
c
      double precision dx(1),dy(1),da
      integer i,incx,incy,ix,iy,m,mp1,n
c
      if(n.le.0)return
      if (da .eq. 0.0d0) return
      if(incx.eq.1.and.incy.eq.1)go to 20
c
c        code for unequal increments or equal increments
c          not equal to 1
c
      ix = 1
      iy = 1
      if(incx.lt.0)ix = (-n+1)*incx + 1
      if(incy.lt.0)iy = (-n+1)*incy + 1
      do 10 i = 1,n
        dy(iy) = dy(iy) + da*dx(ix)
        ix = ix + incx
        iy = iy + incy
   10 continue
      return
c
c        code for both increments equal to 1
c
c
c        clean-up loop
c
   20 m = mod(n,4)
      if( m .eq. 0 ) go to 40
      do 30 i = 1,m
        dy(i) = dy(i) + da*dx(i)
   30 continue
      if( n .lt. 4 ) return
   40 mp1 = m + 1
      do 50 i = mp1,n,4
        dy(i) = dy(i) + da*dx(i)
        dy(i + 1) = dy(i + 1) + da*dx(i + 1)
        dy(i + 2) = dy(i + 2) + da*dx(i + 2)
        dy(i + 3) = dy(i + 3) + da*dx(i + 3)
   50 continue
      return
      end
      double precision function ddot(n,dx,incx,dy,incy)
c
c     forms the dot product of two vectors.
c     uses unrolled loops for increments equal to one.
c     jack dongarra, linpack, 3/11/78.
c
      double precision dx(1),dy(1),dtemp
      integer i,incx,incy,ix,iy,m,mp1,n
c
      ddot = 0.0d0
      dtemp = 0.0d0
      if(n.le.0)return
      if(incx.eq.1.and.incy.eq.1)go to 20
c
c        code for unequal increments or equal increments
c          not equal to 1
c
      ix = 1
      iy = 1
      if(incx.lt.0)ix = (-n+1)*incx + 1
      if(incy.lt.0)iy = (-n+1)*incy + 1
      do 10 i = 1,n
        dtemp = dtemp + dx(ix)*dy(iy)
        ix = ix + incx
        iy = iy + incy
   10 continue
      ddot = dtemp
      return
c
c        code for both increments equal to 1
c
c
c        clean-up loop
c
   20 m = mod(n,5)
      if( m .eq. 0 ) go to 40
      do 30 i = 1,m
        dtemp = dtemp + dx(i)*dy(i)
   30 continue
      if( n .lt. 5 ) go to 60
   40 mp1 = m + 1
      do 50 i = mp1,n,5
        dtemp = dtemp + dx(i)*dy(i) + dx(i + 1)*dy(i + 1) +
     *   dx(i + 2)*dy(i + 2) + dx(i + 3)*dy(i + 3) + dx(i + 4)*dy(i + 4)
   50 continue
   60 ddot = dtemp
      return
      end
      subroutine  dscal(n,da,dx,incx)
c
c     scales a vector by a constant.
c     uses unrolled loops for increment equal to one.
c     jack dongarra, linpack, 3/11/78.
c     modified 3/93 to return if incx .le. 0.
c
      double precision da,dx(1)
      integer i,incx,m,mp1,n,nincx
c
      if( n.le.0 .or. incx.le.0 )return
      if(incx.eq.1)go to 20
c
c        code for increment not equal to 1
c
      nincx = n*incx
      do 10 i = 1,nincx,incx
        dx(i) = da*dx(i)
   10 continue
      return
c
c        code for increment equal to 1
c
c
c        clean-up loop
c
   20 m = mod(n,5)
      if( m .eq. 0 ) go to 40
      do 30 i = 1,m
        dx(i) = da*dx(i)
   30 continue
      if( n .lt. 5 ) return
   40 mp1 = m + 1
      do 50 i = mp1,n,5
        dx(i) = da*dx(i)
        dx(i + 1) = da*dx(i + 1)
        dx(i + 2) = da*dx(i + 2)
        dx(i + 3) = da*dx(i + 3)
        dx(i + 4) = da*dx(i + 4)
   50 continue
      return
      end
      integer function idamax(n,dx,incx)
c
c     finds the index of element having max. absolute value.
c     jack dongarra, linpack, 3/11/78.
c     modified 3/93 to return if incx .le. 0.
c
      double precision dx(1),dmax
      integer i,incx,ix,n
c
      idamax = 0
      if( n.lt.1 .or. incx.le.0 ) return
      idamax = 1
      if(n.eq.1)return
      if(incx.eq.1)go to 20
c
c        code for increment not equal to 1
c
      ix = 1
      dmax = dabs(dx(1))
      ix = ix + incx
      do 10 i = 2,n
         if(dabs(dx(ix)).le.dmax) go to 5
         idamax = i
         dmax = dabs(dx(ix))
    5    ix = ix + incx
   10 continue
      return
c
c        code for increment equal to 1
c
   20 dmax = dabs(dx(1))
      do 30 i = 2,n
         if(dabs(dx(i)).le.dmax) go to 30
         idamax = i
         dmax = dabs(dx(i))
   30 continue
      return
      end
      subroutine  dswap (n,dx,incx,dy,incy)
c
c     interchanges two vectors.
c     uses unrolled loops for increments equal one.
c     jack dongarra, linpack, 3/11/78.
c
      double precision dx(1),dy(1),dtemp
      integer i,incx,incy,ix,iy,m,mp1,n
c
      if(n.le.0)return
      if(incx.eq.1.and.incy.eq.1)go to 20
c
c       code for unequal increments or equal increments not equal
c         to 1
c
      ix = 1
      iy = 1
      if(incx.lt.0)ix = (-n+1)*incx + 1
      if(incy.lt.0)iy = (-n+1)*incy + 1
      do 10 i = 1,n
        dtemp = dx(ix)
        dx(ix) = dy(iy)
        dy(iy) = dtemp
        ix = ix + incx
        iy = iy + incy
   10 continue
      return
c
c       code for both increments equal to 1
c
c
c       clean-up loop
c
   20 m = mod(n,3)
      if( m .eq. 0 ) go to 40
      do 30 i = 1,m
        dtemp = dx(i)
        dx(i) = dy(i)
        dy(i) = dtemp
   30 continue
      if( n .lt. 3 ) return
   40 mp1 = m + 1
      do 50 i = mp1,n,3
        dtemp = dx(i)
        dx(i) = dy(i)
        dy(i) = dtemp
        dtemp = dx(i + 1)
        dx(i + 1) = dy(i + 1)
        dy(i + 1) = dtemp
        dtemp = dx(i + 2)
        dx(i + 2) = dy(i + 2)
        dy(i + 2) = dtemp
   50 continue
      return
      end
      subroutine dgedi(a,lda,n,ipvt,det,work,job)
      integer lda,n,ipvt(1),job
      double precision a(lda,1),det(2),work(1)
c
c     dgedi computes the determinant and inverse of a matrix
c     using the factors computed by dgeco or dgefa.
c
c     on entry
c
c        a       double precision(lda, n)
c                the output from dgeco or dgefa.
c
c        lda     integer
c                the leading dimension of the array  a .
c
c        n       integer
c                the order of the matrix  a .
c
c        ipvt    integer(n)
c                the pivot vector from dgeco or dgefa.
c
c        work    double precision(n)
c                work vector.  contents destroyed.
c
c        job     integer
c                = 11   both determinant and inverse.
c                = 01   inverse only.
c                = 10   determinant only.
c
c     on return
c
c        a       inverse of original matrix if requested.
c                otherwise unchanged.
c
c        det     double precision(2)
c                determinant of original matrix if requested.
c                otherwise not referenced.
c                determinant = det(1) * 10.0**det(2)
c                with  1.0 .le. dabs(det(1)) .lt. 10.0
c                or  det(1) .eq. 0.0 .
c
c     error condition
c
c        a division by zero will occur if the input factor contains
c        a zero on the diagonal and the inverse is requested.
c        it will not occur if the subroutines are called correctly
c        and if dgeco has set rcond .gt. 0.0 or dgefa has set
c        info .eq. 0 .
c
c     linpack. this version dated 08/14/78 .
c     cleve moler, university of new mexico, argonne national lab.
c
c     subroutines and functions
c
c     blas daxpy,dscal,dswap
c     fortran dabs,mod
c
c     internal variables
c
      double precision t
      double precision ten
      integer i,j,k,kb,kp1,l,nm1
c
c
c     compute determinant
c
      if (job/10 .eq. 0) go to 70
         det(1) = 1.0d0
         det(2) = 0.0d0
         ten = 10.0d0
         do 50 i = 1, n
            if (ipvt(i) .ne. i) det(1) = -det(1)
            det(1) = a(i,i)*det(1)
c        ...exit
            if (det(1) .eq. 0.0d0) go to 60
   10       if (dabs(det(1)) .ge. 1.0d0) go to 20
               det(1) = ten*det(1)
               det(2) = det(2) - 1.0d0
            go to 10
   20       continue
   30       if (dabs(det(1)) .lt. ten) go to 40
               det(1) = det(1)/ten
               det(2) = det(2) + 1.0d0
            go to 30
   40       continue
   50    continue
   60    continue
   70 continue
c
c     compute inverse(u)
c
      if (mod(job,10) .eq. 0) go to 150
         do 100 k = 1, n
            a(k,k) = 1.0d0/a(k,k)
            t = -a(k,k)
            call dscal(k-1,t,a(1,k),1)
            kp1 = k + 1
            if (n .lt. kp1) go to 90
            do 80 j = kp1, n
               t = a(k,j)
               a(k,j) = 0.0d0
               call daxpy(k,t,a(1,k),1,a(1,j),1)
   80       continue
   90       continue
  100    continue
c
c        form inverse(u)*inverse(l)
c
         nm1 = n - 1
         if (nm1 .lt. 1) go to 140
         do 130 kb = 1, nm1
            k = n - kb
            kp1 = k + 1
            do 110 i = kp1, n
               work(i) = a(i,k)
               a(i,k) = 0.0d0
  110       continue
            do 120 j = kp1, n
               t = work(j)
               call daxpy(n,t,a(1,j),1,a(1,k),1)
  120       continue
            l = ipvt(k)
            if (l .ne. k) call dswap(n,a(1,k),1,a(1,l),1)
  130    continue
  140    continue
  150 continue
      return
      end
      subroutine dgefa(a,lda,n,ipvt,info)
      integer lda,n,ipvt(1),info
      double precision a(lda,1)
c
c     dgefa factors a double precision matrix by gaussian elimination.
c
c     dgefa is usually called by dgeco, but it can be called
c     directly with a saving in time if  rcond  is not needed.
c     (time for dgeco) = (1 + 9/n)*(time for dgefa) .
c
c     on entry
c
c        a       double precision(lda, n)
c                the matrix to be factored.
c
c        lda     integer
c                the leading dimension of the array  a .
c
c        n       integer
c                the order of the matrix  a .
c
c     on return
c
c        a       an upper triangular matrix and the multipliers
c                which were used to obtain it.
c                the factorization can be written  a = l*u  where
c                l  is a product of permutation and unit lower
c                triangular matrices and  u  is upper triangular.
c
c        ipvt    integer(n)
c                an integer vector of pivot indices.
c
c        info    integer
c                = 0  normal value.
c                = k  if  u(k,k) .eq. 0.0 .  this is not an error
c                     condition for this subroutine, but it does
c                     indicate that dgesl or dgedi will divide by zero
c                     if called.  use  rcond  in dgeco for a reliable
c                     indication of singularity.
c
c     linpack. this version dated 08/14/78 .
c     cleve moler, university of new mexico, argonne national lab.
c
c     subroutines and functions
c
c     blas daxpy,dscal,idamax
c
c     internal variables
c
      double precision t
      integer idamax,j,k,kp1,l,nm1
c
c
c     gaussian elimination with partial pivoting
c
      info = 0
      nm1 = n - 1
      if (nm1 .lt. 1) go to 70
      do 60 k = 1, nm1
         kp1 = k + 1
c
c        find l = pivot index
c
         l = idamax(n-k+1,a(k,k),1) + k - 1
         ipvt(k) = l
c
c        zero pivot implies this column already triangularized
c
         if (a(l,k) .eq. 0.0d0) go to 40
c
c           interchange if necessary
c
            if (l .eq. k) go to 10
               t = a(l,k)
               a(l,k) = a(k,k)
               a(k,k) = t
   10       continue
c
c           compute multipliers
c
            t = -1.0d0/a(k,k)
            call dscal(n-k,t,a(k+1,k),1)
c
c           row elimination with column indexing
c
            do 30 j = kp1, n
               t = a(l,j)
               if (l .eq. k) go to 20
                  a(l,j) = a(k,j)
                  a(k,j) = t
   20          continue
               call daxpy(n-k,t,a(k+1,k),1,a(k+1,j),1)
   30       continue
         go to 50
   40    continue
            info = k
   50    continue
   60 continue
   70 continue
      ipvt(n) = n
      if (a(n,n) .eq. 0.0d0) info = n
      return
      end
      subroutine dgesl(a,lda,n,ipvt,b,job)
      integer lda,n,ipvt(1),job
      double precision a(lda,1),b(1)
c
c     dgesl solves the double precision system
c     a * x = b  or  trans(a) * x = b
c     using the factors computed by dgeco or dgefa.
c
c     on entry
c
c        a       double precision(lda, n)
c                the output from dgeco or dgefa.
c
c        lda     integer
c                the leading dimension of the array  a .
c
c        n       integer
c                the order of the matrix  a .
c
c        ipvt    integer(n)
c                the pivot vector from dgeco or dgefa.
c
c        b       double precision(n)
c                the right hand side vector.
c
c        job     integer
c                = 0         to solve  a*x = b ,
c                = nonzero   to solve  trans(a)*x = b  where
c                            trans(a)  is the transpose.
c
c     on return
c
c        b       the solution vector  x .
c
c     error condition
c
c        a division by zero will occur if the input factor contains a
c        zero on the diagonal.  technically this indicates singularity
c        but it is often caused by improper arguments or improper
c        setting of lda .  it will not occur if the subroutines are
c        called correctly and if dgeco has set rcond .gt. 0.0
c        or dgefa has set info .eq. 0 .
c
c     to compute  inverse(a) * c  where  c  is a matrix
c     with  p  columns
c           call dgeco(a,lda,n,ipvt,rcond,z)
c           if (rcond is too small) go to ...
c           do 10 j = 1, p
c              call dgesl(a,lda,n,ipvt,c(1,j),0)
c        10 continue
c
c     linpack. this version dated 08/14/78 .
c     cleve moler, university of new mexico, argonne national lab.
c
c     subroutines and functions
c
c     blas daxpy,ddot
c
c     internal variables
c
      double precision ddot,t
      integer k,kb,l,nm1
c
      nm1 = n - 1
      if (job .ne. 0) go to 50
c
c        job = 0 , solve  a * x = b
c        first solve  l*y = b
c
         if (nm1 .lt. 1) go to 30
         do 20 k = 1, nm1
            l = ipvt(k)
            t = b(l)
            if (l .eq. k) go to 10
               b(l) = b(k)
               b(k) = t
   10       continue
            call daxpy(n-k,t,a(k+1,k),1,b(k+1),1)
   20    continue
   30    continue
c
c        now solve  u*x = y
c
         do 40 kb = 1, n
            k = n + 1 - kb
            b(k) = b(k)/a(k,k)
            t = -b(k)
            call daxpy(k-1,t,a(1,k),1,b(1),1)
   40    continue
      go to 100
   50 continue
c
c        job = nonzero, solve  trans(a) * x = b
c        first solve  trans(u)*y = b
c
         do 60 k = 1, n
            t = ddot(k-1,a(1,k),1,b(1),1)
            b(k) = (b(k) - t)/a(k,k)
   60    continue
c
c        now solve trans(l)*x = y
c
         if (nm1 .lt. 1) go to 90
         do 80 kb = 1, nm1
            k = n - kb
            b(k) = b(k) + ddot(n-k,a(k+1,k),1,b(k+1),1)
            l = ipvt(k)
            if (l .eq. k) go to 70
               t = b(l)
               b(l) = b(k)
               b(k) = t
   70       continue
   80    continue
   90    continue
  100 continue
      return
      end


! fortran helper functions for special mat calculation

MODULE IZMAT

CONTAINS

! LSVD compute the svd of a matrix and output the singular value and the left
! singular vector
! -----------------------------------------------------------------------------
SUBROUTINE LSVD(sz1, sz2, su, ss, z, u, s)
    IMPLICIT NONE
    INTEGER(8) :: sz1, sz2, su, ss
    REAL(8) :: z(sz1, sz2), u(su), s(ss)
    INTEGER(8) :: lwork, info
    REAL(8), allocatable :: work(:)

    !f2py INTENT(IN) :: z
    !f2py INTENT(HIDE), DEPEND(z) :: sz1 = SIZE(z, 1), sz2 = SIZE(z, 2)
    !f2py INTENT(HIDE), DEPEND(u) :: su = SIZE(u)
    !f2py INTENT(HIDE), DEPEND(s) :: sp = SIZE(s)
    !f2py INTENT(INOUT) :: u, s

    lwork = 2*MAX(1, 3*ss + MAX(sz1, sz2), 5*ss)

    ALLOCATE(work(lwork))
    CALL DGESVD('S', 'N', sz1, sz2, z, sz1, s, u, sz1, z, ss, &
                work, lwork, info)
    DEALLOCATE(work)

END SUBROUTINE LSVD

! ZDECOMP compute the svd of the block of Z matrix
! -----------------------------------------------------------------------------
SUBROUTINE ZDECOMP(sz1, sz2, su, ss, m, nz, nu, ns, z, u, s)
    IMPLICIT NONE
    INTEGER(8) :: sz1, sz2, su, ss, m
    INTEGER(8) :: nz(m), nu(m), ns(m)
    REAL(8) :: z(sz1, sz2), u(su), s(ss)
    INTEGER(8) :: az, bz, au, bu, as, bs, i

    !f2py INTENT(IN) :: nz, nu, ns, z
    !f2py INTENT(HIDE), DEPEND(nz) :: m = SIZE(nz)
    !f2py INTENT(HIDE), DEPEND(z) :: sz1 = SIZE(z, 1), sz2 = SIZE(z, 2)
    !f2py INTENT(HIDE), DEPEND(u) :: su = SIZE(u)
    !f2py INTENT(HIDE), DEPEND(s) :: ss = SIZE(s)
    !f2py INTENT(INOUT) :: u, s

    az = 1
    au = 1
    as = 1
    bz = 0
    bu = 0
    bs = 0

    DO i = 1, m
        bz = bz + nz(i)
        bu = bu + nu(i)
        bs = bs + ns(i)

        CALL LSVD(nz(i), sz2, nu(i), ns(i), z(az:bz, :), u(au:bu), s(as:bs))

        az = az + nz(i)
        au = au + nu(i)
        as = as + ns(i)
    END DO

END SUBROUTINE ZDECOMP

! BLOCK_IZMV calculate matrix vector multiplication of block izmat
! -----------------------------------------------------------------------------
SUBROUTINE BLOCK_IZMV(su, sd, sx, u, d, x, y)
    IMPLICIT NONE
    INTEGER(8) :: su, sd, sx
    REAL(8) :: u(su), d(sd), x(sx), y(sx)
    REAL(8) :: c(sd)

    !f2py INTENT(IN) :: u, d, x
    !f2py INTENT(HIDE), DEPEND(u) :: su = SIZE(u)
    !f2py INTENT(HIDE), DEPEND(d) :: sd = SIZE(d)
    !f2py INTENT(HIDE), DEPEND(x) :: sx = SIZE(x)
    !f2py INTENT(INOUT) :: y

    CALL DGEMV('T', sx, sd, 1.d0, u, sx, x, 1, 0.d0, c, 1)
    c = c * d
    CALL DGEMV('N', sx, sd, 1.d0, u, sx, c, 1, 0.d0, y, 1)
    y = y + x

END SUBROUTINE BLOCK_IZMV

! IZMV calculate matrix vector multiplication of izmat
! -----------------------------------------------------------------------------
FUNCTION IZMV(su, sd, sx, m, nu, nd, nx, u, d, x) RESULT(y)
    IMPLICIT NONE
    INTEGER(8) :: su, sd, sx, m
    INTEGER(8) :: nu(m), nd(m), nx(m)
    REAL(8) :: u(su), d(sd), x(sx)
    REAL(8) :: y(sx)
    INTEGER(8) :: ax, bx, au, bu, ad, bd, i

    !f2py INTENT(IN) :: u, d, x
    !f2py INTENT(HIDE), DEPEND(u) :: su = SIZE(u)
    !f2py INTENT(HIDE), DEPEND(d) :: sd = SIZE(d)
    !f2py INTENT(HIDE), DEPEND(x) :: sx = SIZE(x)
    !f2py INTENT(HIDE), DEPEND(nx) :: m = SIZE(nx)

    ax = 1
    au = 1
    ad = 1
    bx = 0
    bu = 0
    bd = 0

    DO i = 1, m
        bx = bx + nx(i)
        bu = bu + nu(i)
        bd = bd + nd(i)

        CALL BLOCK_IZMV(nu(i), nd(i), nx(i), u(au:bu), d(ad:bd), x(ax:bx), &
                        y(ax:bx))

        ax = ax + nx(i)
        au = au + nu(i)
        ad = ad + nd(i)
    END DO

END FUNCTION IZMV

! BLOCK_IZMM calculate matrix matrix multiplication of block izmat
! -----------------------------------------------------------------------------
SUBROUTINE BLOCK_IZMM(su, sd, sx1, sx2, u, d, x, y)
    IMPLICIT NONE
    INTEGER(8) :: su, sd, sx1, sx2
    REAL(8) :: u(su), d(sd), x(sx1, sx2), y(sx1, sx2)
    INTEGER(8) :: i
    REAL(8) :: c(sd, sx2)

    !f2py INTENT(IN) :: u, d, x
    !f2py INTENT(HIDE), DEPEND(u) :: su = SIZE(u)
    !f2py INTENT(HIDE), DEPEND(d) :: sd = SIZE(d)
    !f2py INTENT(HIDE), DEPEND(x) :: sx1 = SIZE(x, 1)
    !f2py INTENT(HIDE), DEPEND(x) :: sx2 = SIZE(x, 2)
    !f2py INTENT(INOUT) :: y

    CALL DGEMM('T', 'N', sd, sx2, sx1, 1.d0, u, sx1, x, sx1, 0.d0, c, sd)
    DO i = 1, sx2
        c(:, i) = c(:, i) * d
    END DO
    CALL DGEMM('N', 'N', sx1, sx2, sd, 1.d0, u, sx1, c, sd, 0.d0, y, sx1)
    y = y + x

END SUBROUTINE BLOCK_IZMM

! IZMM calculate matrix matrix multiplication of izmat
! -----------------------------------------------------------------------------
FUNCTION IZMM(su, sd, sx1, sx2, m, nu, nd, nx, u, d, x) RESULT(y)
    IMPLICIT NONE
    INTEGER(8) :: su, sd, sx1, sx2, m
    INTEGER(8) :: nu(m), nd(m), nx(m)
    REAL(8) :: u(su), d(sd), x(sx1, sx2)
    REAL(8) :: y(sx1, sx2)
    INTEGER(8) :: ax, bx, au, bu, ad, bd, i

    !f2py INTENT(IN) :: u, d, x
    !f2py INTENT(HIDE), DEPEND(u) :: su = SIZE(u)
    !f2py INTENT(HIDE), DEPEND(d) :: sd = SIZE(d)
    !f2py INTENT(HIDE), DEPEND(x) :: sx1 = SIZE(x, 1)
    !f2py INTENT(HIDE), DEPEND(x) :: sx2 = SIZE(x, 2)
    !f2py INTENT(HIDE), DEPEND(nx) :: m = SIZE(nx)

    ax = 1
    au = 1
    ad = 1
    bx = 0
    bu = 0
    bd = 0

    DO i = 1, m
        bx = bx + nx(i)
        bu = bu + nu(i)
        bd = bd + nd(i)

        CALL BLOCK_IZMM(nu(i), nd(i), nx(i), sx2, u(au:bu), d(ad:bd), &
                        x(ax:bx, :), y(ax:bx, :))

        ax = ax + nx(i)
        au = au + nu(i)
        ad = ad + nd(i)
    END DO

END FUNCTION IZMM

! IZEIG calculate the eigenvalues of the matrix in the iz form
! -----------------------------------------------------------------------------
FUNCTION IZEIG(sd, m, sz, nz, nd, d) RESULT(v)
    IMPLICIT NONE
    INTEGER(8) :: sd, m, sz
    INTEGER(8) :: nz(m), nd(m)
    INTEGER(8) :: i, ad, bd, av, bv
    REAL(8) :: d(sd), v(sz)

    !f2py INTENT(IN) :: sz, nz, nd, d
    !f2py INTENT(HIDE), DEPEND(d) :: sd = SIZE(d)
    !f2py INTENT(HIDE), DEPEND(nd) :: m = SIZE(nd)

    ad = 1
    bd = 0
    av = 1
    bv = 0

    v = 1.d0

    DO i = 1, m
        bd = bd + nd(i)
        bv = av + nd(i) - 1

        v(av:bv) = v(av:bv) + d(ad:bd)

        ad = ad + nd(i)
        av = av + nz(i)
    END DO

END FUNCTION IZEIG

! BLOCK_IZDIAG return the diagonal of the izmat block
! -----------------------------------------------------------------------------
SUBROUTINE BLOCK_IZDIAG(su, sd, sx, u, d, x)
    IMPLICIT NONE
    INTEGER(8) :: su, sd, sx
    REAL(8) :: u(su), d(sd), x(sx)

    !f2py INTENT(IN) :: u, d
    !f2py INTENT(HIDE), DEPEND(u) :: su = SIZE(u)
    !f2py INTENT(HIDE), DEPEND(d) :: sd = SIZE(d)
    !f2py INTENT(HIDE), DEPEND(x) :: sx = SIZE(x)
    !f2py INTENT(INOUT) :: x

    CALL DGEMV('N', sx, sd, 1.d0, u**2, sx, d, 1, 0.d0, x, 1)

    x = x + 1.d0

END SUBROUTINE BLOCK_IZDIAG

! IZDIAG return the diagonal of the izmat
! -----------------------------------------------------------------------------
FUNCTION IZDIAG(su, sd, sx, m, nu, nd, nx, u, d) RESULT(x)
    IMPLICIT NONE
    INTEGER(8) :: su, sd, sx, m
    INTEGER(8) :: nu(m), nd(m), nx(m)
    REAL(8) :: u(su), d(sd), x(sx)
    INTEGER(8) :: au, bu, ad, bd, ax, bx, i

    !f2py INTENT(IN) :: sx, nu, nd, nx, u, d
    !f2py INTENT(HIDE), DEPEND(u) :: su = SIZE(u)
    !f2py INTENT(HIDE), DEPEND(d) :: sd = SIZE(d)
    !f2py INTENT(HIDE), DEPEND(nd) :: m = SIZE(nd)

    au = 1
    bu = 0
    ad = 1
    bd = 0
    ax = 1
    bx = 0

    DO i = 1, m
        bu = bu + nu(i)
        bd = bd + nd(i)
        bx = bx + nx(i)

        CALL BLOCK_IZDIAG(nu(i), nd(i), nx(i), u(au:bu), d(ad:bd), x(ax:bx))

        au = au + nu(i)
        ad = ad + nd(i)
        ax = ax + nx(i)
    END DO

END FUNCTION IZDIAG

END MODULE IZMAT


program test
use activations
implicit none

call test_cost()

contains

subroutine test_cost()
	double precision, dimension(5) :: out
	double precision, dimension(5) :: target
	double precision :: cost_output

	out = (/ 1.0, 2.0, 3.0, 4.0, 5.0/)
	target = (/2.0, 3.0, 4.0, 5.0, 6.0/)

	cost_output = cost(out, target)
	if(cost_output == 2.50) then
		write(*,*) "test_cost: success"
	else
		write(*,*) "test_cost: failure"
	end if

end subroutine test_cost

end program test


subroutine AspirationUpdate(nodeUpdate, payoffUpdate, randUpdate)
    use global
    implicit none
    integer,intent(in)::nodeUpdate
    real,intent(in)::payoffUpdate, randUpdate

    real::switchProb
    real::AspirationMinusPayoff
    !!

    AspirationMinusPayoff = aspirationValueNode(nodeUpdate) - payoffUpdate
    switchProb = 1.0 / (1 + exp(-1.0 * selectionIntensity * AspirationMinusPayoff))
    !call RANDOM_NUMBER(randUpdate)
    !call RNUN(randUpdateArray)
    !randUpdate = randUpdateArray(1)

    if(randUpdate < switchProb)then
        strategyPGG(nodeUpdate) = 1 - strategyPGG(nodeUpdate)
    end if

    !write(*,*) "Aspiration Value"
    !write(*,*) aspirationValueNode(nodeUpdate)
    !write(*,*) "Payoff Update Node"
    !write(*,*) payoffUpdate
    !write(*,*) "randUpdate"
    !write(*,*) randUpdate
    !write(*,*) "Switching Probability"
    !write(*,*) switchProb
    !pause
    return
end subroutine AspirationUpdate


      subroutine lk_comp_joint(bev,xiv,k,piv,Pi,si2,
     c typ,n,M,npX,XM,YM,PM,btv,Pp1,tv,lk)

      integer k,n,M,npX,npW,i,mi,m1,PM(n,M)
      double precision bev(npX),xiv(k),piv(k),Pi(k)
      double precision si2,XM(n,M,npX),YM(n,M),btv(M)
      double precision Pp1(k,M,n),tv(n)
      double precision Wp(n),muv(k),lk,lsi2pi
      character (len=4) typ
      parameter (pig=3.1415926535897932)

c to compute the second and third components of the expect complete log-likelihood

c preliminaries
	  if (typ=="norm") then
        lsi2pi = log(si2)+log(2*pig)
	  end if
c compute functions
	  do i=1,n
		mi = count(btv.le.tv(i))
		do m1 = 1,mi
          if (PM(i,m1)==1) then
            muv = xiv+sum(XM(i,m1,:)*bev)
            if (typ=="norm") then
              lk = lk-0.5*sum((lsi2pi+(YM(i,m1)-muv)**2/si2)*
     c             Pp1(:,m1,i))
            end if
            if (typ=="bino") then
              muv = exp(muv)/(1+exp(muv))
              lk = lk+sum((YM(i,m1)*log(muv)+(1-YM(i,m1))*log(1-muv))*
     c             Pp1(:,m1,i))
            end if
          end if
		end do
      end do

      end


      implicit double precision (a-h,o-z)
c
      integer, parameter :: max_nodes = 100000
      dimension x(max_nodes), y(max_nodes), z(max_nodes)
      integer :: bound_nodes(max_nodes)
      double precision :: nu, KI, kappa, rad, zero
      logical, parameter :: debug = .false.

      factor = 1.0d0  ! already in mm

      radius_mm = 2000.d0
      radius_m = 2.0d0
      pi = 3.14159265359d0
      zero = 0.0d0
c
      read(*,*) nnode
c
      do i = 1, nnode
       read(*,*,end=100) j, x(j), y(j), z(j)
       x(i) = x(i) * factor
       y(i) = y(i) * factor
       z(i) = z(i) * factor
      end do
      write(*,*) '.... coordinates read '
c
c             find boundary nodes (R = 2000.)
c
      nboundary = 0
      do i = 1, nnode
       rad = sqrt( x(i)*x(i) + y(i)*y(i) )
       if( rad < 0.9d0*radius_mm ) cycle
       nboundary = nboundary + 1
       bound_nodes(nboundary) = i
      end do
      write(*,*) '.... # boundary nodes: ', nboundary
c
c             fix radius on boundary nodes
c
      do i = 1, nboundary
        node = bound_nodes(i)
        xb = x(node)
        yb = y(node)
        angle = atan2(yb,xb)
        x(node) = radius_mm * cos(angle)
        y(node) = radius_mm * sin(angle)
      end do
c
      open(unit=11,file="coords_mm.inp")
      write(11,1002) "*echo off"
      write(11,1002) " coordinates"
      do i = 1, nnode
       write(11,1000) i, x(i), y(i), z(i)
      end do
      write(11,1002) "*echo on"
      close(unit=11)
c
c             du, dv displacements for K_I = 1 MPa sqrt(m) on bound
c
      ymod = 88000.d0
      nu = 0.34d0
      gmod = ymod / 2.0d0 / (1.0d0 + nu )
      KI = 1.0d0
      applied_k = 1.0d0
      write(*,*) ' '
      write(*,*) ' '
c
c            make KI displacements on boundary for 1.0 MPa-sqrt(m)
c            Zr4 properties.
c            compute in meters, final in mm

c
      do i = 1, nboundary
        node = bound_nodes(i)
        xb = x(node)
        yb = y(node)
        theta = atan2( yb, xb )
        ct2 = cos(theta*0.5d0)
        st2 = sin(theta*0.5d0)
        ct  = cos(theta)
        st  = sin(theta)
        term1 = (3.0-4.0*nu-ct) * ct2
        term2 = (3.0-4.0*nu-ct) * st2
        term3 = sqrt( radius_m * 0.5d0 / pi )
        term4 = (1.0d0 + nu ) * applied_k / ymod
        u_k = 1000.0d0 * term4 * term3 * term1
        v_k = 1000.0d0 * term4 * term3 * term2
        write(*,2000) node, u_k, v_k ! mm
      end do

c
 100  stop
 1000 format(i6,3e17.9)
 1002 format(a)
 2000 format(i9,1x,'u ',e17.9,3x,'v ',e17.9)
      end


subroutine sail_BDRF(w,lai,sumint,tsstoo,rsoil, &
        rdd,tdd,tsd,rsd,tdo,rdo,tss,too,rsod, &
        rddt,rsdt,rdot,rsot)

    use mod_dataSpec_wavelength
    implicit none
    real*8,intent(in) :: lai,tss,too,sumint,tsstoo
    real*8,intent(in),dimension(nw) :: w,rsoil,rdd,tdd,tsd,rsd,tdo,rdo,rsod
    real*8,intent(out),dimension(nw) :: rddt,rsdt,rdot,rsot

    real*8,dimension(nw) :: rsos,rso,dn,rsodt,rsost

    rsos  = w*lai*sumint        ! Single scattering contribution
    rso   = rsos+rsod           !Total canopy contribution
    dn    = 1.-rsoil*rdd            !Interaction with the soil

    ! rddt: bi-hemispherical reflectance factor
    rddt  = rdd+tdd*rsoil*tdd/dn

    ! rsdt: directional-hemispherical reflectance factor for solar incident flux
    rsdt  = rsd+(tsd+tss)*rsoil*tdd/dn

    ! rdot: hemispherical-directional reflectance factor in viewing direction
    rdot  = rdo+tdd*rsoil*(tdo+too)/dn

    ! rsot: bi-directional reflectance factor
    rsodt = rsod+((tss+tsd)*tdo+(tsd+tss*rsoil*rdd)*too)*rsoil/dn
    rsost = rsos+tsstoo*rsoil
    rsot  = rsost+rsodt

    return
end subroutine sail_BDRF