c ==================================================================================================================================
c current version: 4.0
c version 1.0(July 31th, 2013)
c version 2.0(August 23th,2013)   works faster, some unnecessary caclulations removed
c version 3.0(September 5th,2014) handles extremely eccentric orbits, corrections
c                                 done thanks to comments submitted by Zong-Fu,Sie NCU Taiwan
c version 4.0(July 30th,2017)     removes important bug injected in version 3.0,
c                                 corrections done thanks to comments submitted by
c                                 Robert Jedicke,University of Hawaii
c--------------------------------
c This program calculates the MOID between two asteroids - Lutetia and Magdalena.
c It uses all ideas and solutions described with details in the paper by
c T.Wisniowski and H.Rickman "A Fast, Geometric Method for Calculating Accurate
c Minimum Orbit Intersection Distances (MOIDs)" published in 2013 in Acta Astronomica.
c The program is free and may be used without limits as the core of any other program.
c The authors will appreciate for mentioning them, when the program will appear to be useful.
c ==================================================================================================================================
      real*8 function moid(saxisA,eccenA,argpeA,omegaA,incliA,saxisB,
     a                     eccenB,argpeB,omegaB,incliB)

      implicit real*8 (a-h,o-z), integer*4 (i-n)
      real*8 longit,longit_m,longit_o,incliA,incliB !moid
      logical aleft,aright,bleft,bright,calc1,calc2,calc3,calc4
c tables
      real*8, dimension(3)::rAt,rBt,Axt,Ayt,Bxt,Byt,Bzt
      real*8, dimension(10)::tmpmoid,tmptrueB,tmplongit

c....parameters of the program
ccc  (these steps are optimized with respect to speed and reliability)
      cstep=0.12d0    ! scanning step of true anomaly/longitude in radians
      stepini=0.07d0  !initial step of first tuning in radians
      steptresh=1E-5  !final step of first tunig (to choose the MOID) in radians

ccc   (this step depends on expected final accuracy)
      stepmin=1E-14   !threshold step of second tuning in radians

c....constants and given values
      pi=3.141592653589793d0
      twopi=2d0*pi
      degrad=pi/180d0
c....orbital parameters of body A - asteroid Magdalena, angles in [deg]
!     saxisA=3.1924186d0  !semiaxis [AU]
!     eccenA=0.0843230d0  !eccenticity [deg]
!     argpeA=298.98851d0  !argument of perihelion [deg]
!     omegaA=161.64831d0  !longitude of ascending node [deg]
!     incliA=10.64148d0   !inclination [deg]
c....orbital parameters of body B - asteroid Lutetia, angles in [deg]
!     saxisB=2.4354656d0  !semiaxis [AU]
!     eccenB=0.1629385d0  !eccenticity [deg]
!     argpeB=250.36793d0  !argument of perihelion [deg]
!     omegaB=80.89426d0  !longitude of ascending node [deg]
!     incliB=3.06405d0   !inclination [deg]

ccccccccc START OF PREPARING THE ORBITS ccccccccccccccc
c....converting angles to [rad]
      argpeA=argpeA*degrad
      omegaA=omegaA*degrad
      incliA=incliA*degrad
      argpeB=argpeB*degrad
      omegaB=omegaB*degrad
      incliB=incliB*degrad
c....computing parameters of transition matrix c11...c33
      c11=dcos(omegaA)*dcos(argpeA)-dsin(omegaA)*dcos(incliA)
     a *dsin(argpeA)
      c12=dsin(omegaA)*dcos(argpeA)+dcos(omegaA)*dcos(incliA)
     a *dsin(argpeA)
      c13=dsin(incliA)*dsin(argpeA)
      c21=-dcos(omegaA)*dsin(argpeA)-dsin(omegaA)*dcos(incliA)
     a *dcos(argpeA)
      c22=-dsin(omegaA)*dsin(argpeA)+dcos(omegaA)*dcos(incliA)
     a *dcos(argpeA)
      c23=dsin(incliA)*dcos(argpeA)
      c31=dsin(incliA)*dsin(omegaA)
      c32=-dsin(incliA)*dcos(omegaA)
      c33=dcos(incliA)
c....calculating new values of Euler angles using transition matrix
        sintmpi=dsin(incliB)
        costmpi=dcos(incliB)
        costmpo=dcos(omegaB)
        sintmpo=dsin(omegaB)
        costmpa=dcos(argpeB)
        sintmpa=dsin(argpeB)
        x1=costmpo*costmpa-sintmpo*costmpi*sintmpa
        x2=sintmpo*costmpa+costmpo*costmpi*sintmpa
        x3=sintmpi*sintmpa
        y1=-costmpo*sintmpa-sintmpo*costmpi*costmpa
        y2=-sintmpo*sintmpa+costmpo*costmpi*costmpa
        y3=sintmpi*costmpa
        z1=sintmpi*sintmpo
        z2=-sintmpi*costmpo
        z3=costmpi
        z1n=c11*z1+c12*z2+c13*z3
        z2n=c21*z1+c22*z2+c23*z3
        z3n=c31*z1+c32*z2+c33*z3
        y3n=c31*y1+c32*y2+c33*y3
        x3n=c31*x1+c32*x2+c33*x3
        incliB=datan2(dsqrt(z1n*z1n+z2n*z2n),z3n)
        omegaB=-datan2(z1n,-z2n)
        argpeB=-datan2(x3n,y3n)
c....helpful precalculated values
        costmpo=dcos(omegaB)
        sintmpo=dsin(omegaB)
        sintmpi=dsin(incliB) !=z1n/sintmpo
        costmpi=z3n          !=dcos(incliB)
        sint=sintmpo*costmpi
        cost=costmpo*costmpi
        radA=saxisA*(1d0-eccenA*eccenA)
        radB=saxisB*(1d0-eccenB*eccenB)
ccccccccc END OF PREPARING THE ORBITS ccccccccccccccc

ccccccccc START OF SCANNING ccccccccccccccccccccccccc
ccc This tool yields a preliminary approach to the minima of the distance function.
ccc By scanning one full revolution of meridional plane we look for the local minima.
c......initial parameters
        trueB=-2d0*cstep
        moid=1E6;dist_o=1E6  !something big
        tmpmoid(1)=1E6;tmpmoid(2)=1E6
        tmpmoid(3)=1E6;tmpmoid(4)=1E6
        iii1=0;jjj1=0
c.....Looking for the minima with rotating meridional plane
c.......a)at first we calculate the coordinates of two additional positions of the plane to create first triplet
          do 307 iii=1,2
            rB=radB/(1d0+eccenB*dcos(trueB))!compute the radius for B
            sintmp=dsin(trueB+argpeB)
            costmp=dcos(trueB+argpeB)
            Bz_sq=sintmpi*sintmp
            Bz_sq=Bz_sq*Bz_sq !square of Z-coordinate for B
            longit=datan2(sintmpo*costmp+sintmp*cost,
     a       costmpo*costmp-sintmp*sint)  !compute the longitude for A
            tmp2=eccenA*dcos(longit)      !temporary value
            rA=radA/(1d0+tmp2)            !compute the radius for A (two possibilities)
            rA2=radA/(1d0-tmp2)
            tmp1=rB*dsqrt(1d0-Bz_sq)      !temporary value
            if (dabs(tmp1-rA)>dabs(tmp1+rA2)) then
              rA=rA2
              longit=longit-pi            !the second possibility gives smaller distance
              tmp1=tmp1+rA2
            else
              tmp1=tmp1-rA
            endif
            dist=rB*rB*Bz_sq+tmp1*tmp1 !square of the distance A-B
            if (iii.eq.1) then
              dist_oo=dist
            else
              dist_o=dist
              trueB_o=trueB
              longit_o=longit
            endif
            trueB=trueB+cstep
  307     continue
c.......b)now we scan doing one full revolution of the meridional plane
          nmax=0 !counts the minima
          dist_min=dist
          do while (trueB<(twopi+cstep))  !loop for true anomaly of B
            rB=radB/(1d0+eccenB*dcos(trueB))!compute the radius for B
            sintmp=dsin(trueB+argpeB)
            costmp=dcos(trueB+argpeB)
            Bz_sq=sintmpi*sintmp
            Bz_sq=Bz_sq*Bz_sq !square of Z-coordinate for B
            longit=datan2(sintmpo*costmp+sintmp*cost,
     a       costmpo*costmp-sintmp*sint) !compute the longitude for A
            tmp2=eccenA*dcos(longit)      !temporary value
            rA=radA/(1d0+tmp2)            !compute the radius for A (two possibilities)
            rA2=radA/(1d0-tmp2)
            tmp1=rB*dsqrt(1d0-Bz_sq)      !temporary value
            if (dabs(tmp1-rA)>dabs(tmp1+rA2)) then
              rA=rA2
              longit=longit-pi            !the second possibility gives smaller distance
              tmp1=tmp1+rA2
            else
              tmp1=tmp1-rA
            endif
            dist=rB*rB*Bz_sq+tmp1*tmp1 !square of the distance A-B
           if ((dist_o<=dist).and.(dist_o<=dist_oo)) then  !the minimum was found
              nmax=nmax+1
              tmptrueB(nmax)=trueB_o
              tmplongit(nmax)=longit_o
              tmpmoid(nmax)=dist_o
            endif
            if (dist_min>dist) dist_min=dist
            dist_oo=dist_o
            trueB_o=trueB
            longit_o=longit
            dist_o=dist
            trueB=trueB+cstep
         end do
ccccccccc END OF SCANNING ccccccccccccccccccccccccc

ccc "WATER" PROCEDURE cccccccccccccccccccccccccccccccccc
ccc In case only one minimum is detected we take a special care
ccc to avoid the risk of missing the minima.
ccc Instead of starting the tuning with one detected minimum,
ccc we start it with four positions of the meridional plane, evenly
ccc distributed along the inclined orbit. The later tuning procedure
ccc moves the points along the orbits similarly as water droplets
ccc are leading by the gravity to the points of minimum height � so
ccc we called this phase �water procedure�. It slightly slows down
ccc the calculations, but minimizes the risk of missing the MOID.
ccc With "water procedure" the speed is 9-12 seconds per 100,000 MOIDs, risk of missing <1E-6
ccc Without "water procedure" the speed is <9 seconds per 100,000 MOIDs, risk of missing about 3E-5
ccc (speed measured on fast single CPU core)

       !goto 510   !if this jump is active - the water procedure is switched off

  405   if (nmax<2) then  !only one minimum was detected
         nmax=4
         do 407 iii=1,4
            tmptrueB(iii)=(.25+.5*iii)*pi  !evenly distributed points
            sintmp=dsin(tmptrueB(iii)+argpeB)
            costmp=dcos(tmptrueB(iii)+argpeB)
            tmplongit(iii)=datan2(sintmpo*costmp+sintmp*cost,
     a       costmpo*costmp-sintmp*sint) !compute the longitude for A
            tmpmoid(iii)=1E6 !something big
  407     continue
        endif
cccccc END OF "WATER" PROCEDURE ccccccccccccccccccccccccc

cccccc START OF PARALLEL TUNING ccccccccccccccccccccccc
ccc After the scanning phase, we typically have a few minima on a meridional plane.
ccc The goal of the tuning procedure is to move objects separately along their orbits
ccc in order to find the smallest possible distance between them, which is no longer a meridional distance.
  510    do 615 jjj=1,nmax+1
         if (jjj.le.nmax) then
            moid=tmpmoid(jjj)
			trueB_m=tmptrueB(jjj)
		    longit_m=tmplongit(jjj)
            step=stepini
            threshold=steptresh
         else
           if (nmax.eq.2) then
             if (dabs(tmpmoid(1)-tmpmoid(2)).lt.1E-4) then
ccc in case of two minima are very close to each other(<1E-4 a.u.) go to "water procedure"
               nmax=1
               goto 405
             else
               if (tmpmoid(1)<moid) then
                  moid=tmpmoid(1)
                  trueB_m=tmptrueB(1)
                  longit_m=tmplongit(1)
               endif
             endif
           else
             do 634 iii=1,nmax-1  !the choice of moids for final tuning
              if (tmpmoid(iii)<moid) then
                moid=tmpmoid(iii)
                trueB_m=tmptrueB(iii)
                longit_m=tmplongit(iii)
               endif
  634        continue
           endif
            step=2d0*stepini  !initial step for final tuning
            threshold=stepmin !terminal step for final tuning
         endif
        rBt(2)=radB/(1d0+eccenB*dcos(trueB_m))
        sintmp=dsin(trueB_m+argpeB)
        costmp=dcos(trueB_m+argpeB)
        Bxt(2)=costmpo*costmp-sintmp*sint
        Byt(2)=sintmpo*costmp+sintmp*cost
        Bzt(2)=sintmpi*sintmp
        rAt(2)=radA/(1d0+eccenA*dcos(longit_m))
        Axt(2)=dcos(longit_m)
        Ayt(2)=dsin(longit_m)
        aleft=.true.;aright=.true.;bleft=.true.;bright=.true.
        do while (step>=threshold)
          lpoints=0
          j1min=1;j1max=3
          i1min=1;i1max=3
          calc1=.false.;calc2=.false.
          calc3=.false.;calc4=.false.
          if (bleft) then
            rBt(1)=radB/(1d0+eccenB*dcos(trueB_m-step))
            sintmp=dsin(trueB_m-step+argpeB)
            costmp=dcos(trueB_m-step+argpeB)
            Bxt(1)=costmpo*costmp-sintmp*sint
            Byt(1)=sintmpo*costmp+sintmp*cost
            Bzt(1)=sintmpi*sintmp
            lpoints=lpoints+1
          endif
          if (bright) then
            rBt(3)=radB/(1d0+eccenB*dcos(trueB_m+step))
            sintmp=dsin(trueB_m+step+argpeB)
            costmp=dcos(trueB_m+step+argpeB)
            Bxt(3)=costmpo*costmp-sintmp*sint
            Byt(3)=sintmpo*costmp+sintmp*cost
            Bzt(3)=sintmpi*sintmp
            lpoints=lpoints+1
          endif
          if (aleft) then
            rAt(1)=radA/(1d0+eccenA*dcos(longit_m-step))
            Axt(1)=dcos(longit_m-step)
            Ayt(1)=dsin(longit_m-step)
            lpoints=lpoints+1
          endif
          if (aright) then
            rAt(3)=radA/(1d0+eccenA*dcos(longit_m+step))
            Axt(3)=dcos(longit_m+step)
            Ayt(3)=dsin(longit_m+step)
            lpoints=lpoints+1
          endif
          j1_t=2;i1_t=2
          if (lpoints.eq.1) then
            if (aleft) i1max=1
            if (aright) i1min=3
            if (bleft) j1max=1
            if (bright) j1min=3
          endif
          if (lpoints.eq.2) then
            if (aleft.and.bright) calc1=.true.
            if (aleft.and.bleft) calc2=.true.
            if (aright.and.bright) calc3=.true.
            if (aright.and.bleft) calc4=.true.
          endif
          do 557 j1=j1min,j1max
            do 555 i1=i1min,i1max
              if (lpoints.eq.2) then
                if (i1.ne.1) then
                  if (((j1.ne.3).and.calc1)
     a             .or.((j1.ne.1).and.calc2)) goto 555
                endif
                if (i1.ne.3) then
                  if (((j1.ne.3).and.calc3)
     a             .or.((j1.ne.1).and.calc4)) goto 555
                endif
              endif
              if ((i1.eq.2).and.(j1.eq.2)) goto 555
              Dx=rBt(j1)*Bxt(j1)-rAt(i1)*Axt(i1)
              Dy=rBt(j1)*Byt(j1)-rAt(i1)*Ayt(i1)
              Dz=rBt(j1)*Bzt(j1)
              dist=(Dx*Dx+Dy*Dy+Dz*Dz)
              if (dist<moid) then
                 moid=dist
                 j1_t=j1;i1_t=i1
              endif
  555       continue
  557     continue
         if ((j1_t.ne.2).or.(i1_t.ne.2)) then
          aleft=.false.;aright=.false.;bleft=.false.;bright=.false.
          if (i1_t.ne.2) then
            if (i1_t.eq.1) then
              aleft=.true.
              longit_m=longit_m-step
              rAt(3)=rAt(2);Axt(3)=Axt(2);Ayt(3)=Ayt(2)
              rAt(2)=rAt(1);Axt(2)=Axt(1);Ayt(2)=Ayt(1)
            else
              aright=.true.
              longit_m=longit_m+step
              rAt(1)=rAt(2);Axt(1)=Axt(2);Ayt(1)=Ayt(2)
              rAt(2)=rAt(3);Axt(2)=Axt(3);Ayt(2)=Ayt(3)
            endif
          endif
          if (j1_t.ne.2) then
            if (j1_t.eq.1) then
              bleft=.true.
              trueB_m=trueB_m-step
              rBt(3)=rBt(2);Bxt(3)=Bxt(2);Byt(3)=Byt(2);Bzt(3)=Bzt(2)
              rBt(2)=rBt(1);Bxt(2)=Bxt(1);Byt(2)=Byt(1);Bzt(2)=Bzt(1)
            else
              bright=.true.
              trueB_m=trueB_m+step
              rBt(1)=rBt(2);Bxt(1)=Bxt(2);Byt(1)=Byt(2);Bzt(1)=Bzt(2)
              rBt(2)=rBt(3);Bxt(2)=Bxt(3);Byt(2)=Byt(3);Bzt(2)=Bzt(3)
            endif
          endif
         else
            aleft=.true.;aright=.true.;bleft=.true.;bright=.true.
            step=step*.15 !.15 is optimal value
         endif
        end do
         if (jjj.le.nmax) then
            tmpmoid(jjj)=moid
            tmptrueB(jjj)=trueB_m
            tmplongit(jjj)=longit_m
         endif
  615   continue
cccccc END OF PARALLEL TUNING ccccccccccccccccccccccc
        moid=dsqrt(moid) !we dealed with squares
        !write (*,*) 'MOID [AU] =', moid
      END


! MIT License
!
! Copyright (c) 2016 Anders Steen Christensen
!
! Permission is hereby granted, free of charge, to any person obtaining a copy
! of this software and associated documentation files (the "Software"), to deal
! in the Software without restriction, including without limitation the rights
! to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
! copies of the Software, and to permit persons to whom the Software is
! furnished to do so, subject to the following conditions:
!
! The above copyright notice and this permission notice shall be included in all
! copies or substantial portions of the Software.
!
! THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
! IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
! FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
! AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
! LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
! OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
! SOFTWARE.

module funcs

contains

function stoch_dist(D1, D2, W1, W2) result(a)

    implicit none

    double precision, intent(in) :: D1
    double precision, intent(in) :: D2
    double precision, intent(in) :: W1
    double precision, intent(in) :: W2

    double precision :: a

    a = W1**2/(W1**2 + D1**2) * W2**2/(W2**2 + D2**2)

end function stoch_dist

function dist(x1, x2, w1, w2) result(a)

    implicit none

    double precision, intent(in) :: x1
    double precision, intent(in) :: x2
    double precision, intent(in) :: w1
    double precision, intent(in) :: w2

    double precision, parameter :: pi = 4.0d0 * atan(1.0d0)

    double precision :: a

    a = exp(-((x1-x2)**2)/(4.0d0*w1**2)) *  (1.0d0 - sin(pi * x1/(2.0d0 * w2))) * &
        & (1.0d0 - sin(pi * x2/(2.0d0 * w2)))

end function dist

function atomic_distl2(X1, X2, N1, N2, sin1, sin2, width, cut_distance, r_width, c_width) result(aadist)

    implicit none

    double precision, dimension(:,:), intent(in) :: X1
    double precision, dimension(:,:), intent(in) :: X2

    integer, intent(in) :: N1
    integer, intent(in) :: N2

    double precision, dimension(:), intent(in) :: sin1
    double precision, dimension(:), intent(in) :: sin2

    double precision, intent(in) :: width
    double precision, intent(in) :: cut_distance
    double precision, intent(in) :: r_width
    double precision, intent(in) :: c_width

    double precision :: aadist

    double precision :: d

    integer :: m_1, m_2

    double precision :: maxgausdist2

    double precision :: inv_width
    double precision :: c_width2, r_width2, r2

    inv_width = -1.0d0 / (4.0d0 * width**2)

    maxgausdist2 = (8.0d0 * width)**2
    r_width2 = r_width**2
    c_width2 = c_width**2

    aadist = 0.0d0

    do m_1 = 1, N1

        if (X1(1, m_1) > cut_distance) exit

        do m_2 = 1, N2

            if (X2(1, m_2) > cut_distance) exit

            r2 = (X2(1,m_2) - X1(1,m_1))**2

            if (r2 < maxgausdist2) then

                d = exp(r2 * inv_width )  * sin1(m_1) * sin2(m_2)

                d = d * (r_width2/(r_width2 + (x1(2,m_1) - x2(2,m_2))**2) * &
                    & c_width2/(c_width2 + (x1(3,m_1) - x2(3,m_2))**2))

                aadist = aadist + d * (1.0d0 + x1(4,m_1)*x2(4,m_2) + x1(5,m_1)*x2(5,m_2))
            end if
        end do
    end do

end function atomic_distl2

function m_dist(X1, X2, N1, N2, width, cut_distance, r_width, c_width) result(aadist)

    implicit none

    double precision, dimension(:,:), intent(in) :: X1
    double precision, dimension(:,:), intent(in) :: X2

    integer, intent(in) :: N1
    integer, intent(in) :: N2

    double precision, intent(in) :: width
    double precision, intent(in) :: cut_distance
    double precision, intent(in) :: r_width
    double precision, intent(in) :: c_width

    double precision :: aadist

    double precision :: maxgausdist
    double precision :: d

    double precision :: r_dist
    double precision :: c_dist

    integer :: m_1, m_2

    maxgausdist = 8.0d0*width
    aadist = 0.0d0

    do m_1 = 1, N1

        if (X1(1, m_1) > cut_distance) exit

        do m_2 = 1, N2

            if (X2(1, m_2) > cut_distance) exit

            if (abs(X2(1,m_2) - X1(1,m_1)) < maxgausdist) then

                r_dist = abs(x1(2,m_1) - x2(2,m_2))
                c_dist = abs(x1(3,m_1) - x2(3,m_2))

                d = dist(x1(1,m_1), x2(1,m_2),width,cut_distance)
                d = d * stoch_dist(r_dist,c_dist,r_width,c_width)
                aadist = aadist + d * (1.0d0 + x1(4,m_1)*x2(4,m_2) + x1(5,m_1)*x2(5,m_2))

            end if
        end do
    end do

end function m_dist

function distl2(X1, X2, Z1, Z2, N1, N2, width, &
    & cut_distance, r_width, c_width, sin1, sin2) result(D12)

    implicit none

    double precision, dimension(:,:,:), intent(in) :: X1
    double precision, dimension(:,:,:), intent(in) :: X2

    integer, dimension(:,:), intent(in) :: Z1
    integer, dimension(:,:), intent(in) :: Z2

    integer, intent(in) :: N1
    integer, intent(in) :: N2

    double precision, intent(in) :: width
    double precision, intent(in) :: cut_distance
    double precision, intent(in) :: r_width
    double precision, intent(in) :: c_width

    double precision, dimension(:,:), intent(in) :: sin1, sin2

    double precision :: D12

    integer :: j_1, j_2

    integer :: m_1, m_2

    double precision :: r_dist, c_dist, aadist, d, maxgausdist2

    double precision :: inv_cut, inv_width
    double precision :: c_width2, r_width2, r2

    double precision, parameter :: pi = 4.0d0 * atan(1.0d0)

    double precision :: killer

    inv_cut = pi / (2.0d0 * cut_distance)
    inv_width = -1.0d0 / (4.0d0 * width**2)
    maxgausdist2 = (8.0d0 * width)**2
    r_width2 = r_width**2
    c_width2 = c_width**2

    D12 = 0.0d0

    do j_1 = 1, N1

!$OMP PARALLEL DO PRIVATE(aadist, r2, d, killer) REDUCTION(+:D12)
        do j_2 = 1, N2

            aadist = 0.0d0

            do m_1 = 1, N1

                killer = 1.0d0
                if (X1(j_1, 1,m_1) > cut_distance) killer = 0.0d0 !exit

                do m_2 = 1, N2

                    if (X2(j_2,1, m_2) > cut_distance) killer = 0.0d0 !exit

                    r2 =  (x1(j_1,1,m_1)-x2(j_2,1,m_2))**2

                    if (r2 < maxgausdist2) then

                        d = exp(r2 * inv_width )  * sin1(j_1, m_1) * sin2(j_2, m_2)

                        d = d * (r_width2/(r_width2 + (x1(j_1,2,m_1) - x2(j_2,2,m_2))**2) * &
                            & c_width2/(c_width2 + (x1(j_1,3,m_1) - x2(j_2,3,m_2))**2))

                        aadist = aadist + d * (1.0d0 + x1(j_1,4,m_1)*x2(j_2,4,m_2) + &
                            & x1(j_1,5,m_1)*x2(j_2,5,m_2)) * killer

                    end if
                end do
            end do

            r_dist = abs(z1(j_1,1) - z2(j_2,1))
            c_dist = abs(z1(j_1,2) - z2(j_2,2))

            D12 = D12 + aadist * (r_width2/(r_width2 + r_dist**2) * c_width2/(c_width2 + c_dist**2))

        enddo
!$OMP END PARALLEL DO
    enddo

end function distl2

end module funcs

subroutine molecular_arad_l2_distance_all(X1, X2, Z1, Z2, N1, N2, nmol1, nmol2, width, &
    & cut_distance, r_width, c_width, D12)

    use funcs, only: distl2, m_dist, stoch_dist

    implicit none

    double precision, dimension(:,:,:,:), intent(in) :: X1
    double precision, dimension(:,:,:,:), intent(in) :: X2

    integer, dimension(:,:,:), intent(in) :: Z1
    integer, dimension(:,:,:), intent(in) :: Z2

    integer, dimension(:), intent(in) :: N1
    integer, dimension(:), intent(in) :: N2

    integer, intent(in) :: nmol1
    integer, intent(in) :: nmol2

    double precision, intent(in) :: width
    double precision, intent(in) :: cut_distance
    double precision, intent(in) :: r_width
    double precision, intent(in) :: c_width

    double precision, dimension(nmol1, nmol2), intent(out) :: D12

    double precision, dimension(nmol1) :: D11
    double precision, dimension(nmol2) :: D22

    double precision, allocatable, dimension(:,:,:) :: sin1, sin2
    double precision :: inv_cut

    integer :: i, j, j_1, m_1

    double precision, parameter :: pi = 4.0d0 * atan(1.0d0)

    double precision, parameter :: eps = 1.0e-11

    inv_cut = pi / (2.0d0 * cut_distance)

    allocate(sin1(nmol1, maxval(N1), maxval(N1)))
    allocate(sin2(nmol2, maxval(N2), maxval(N2)))

    !$OMP PARALLEL DO
    do i = 1, nmol1
        do m_1 = 1, N1(i)
            do j_1 = 1, N1(i)
            sin1(i, j_1, m_1) = 1.0d0 - sin(x1(i,j_1,1,m_1) * inv_cut)
            enddo
        enddo
    enddo
    !$OMP END PARALLEL DO

    !$OMP PARALLEL DO
    do i = 1, nmol2
        do m_1 = 1, N2(i)
            do j_1 = 1, N2(i)
            sin2(i, j_1, m_1) = 1.0d0 - sin(x2(i,j_1,1,m_1) * inv_cut)
            enddo
        enddo
    enddo
    !$OMP END PARALLEL DO

    !$OMP PARALLEL DO
    do i = 1, nmol1
        D11(i) = distl2(X1(i,:,:,:), X1(i,:,:,:), Z1(i,:,:), Z1(i,:,:), N1(i), N1(i), &
            & width, cut_distance, r_width, c_width, sin1(i,:,:), sin1(i,:,:))
    enddo
    !$OMP END PARALLEL DO

    !$OMP PARALLEL DO
    do i = 1, nmol2
        D22(i) = distl2(X2(i,:,:,:), X2(i,:,:,:), Z2(i,:,:), Z2(i,:,:), N2(i), N2(i), &
            & width, cut_distance, r_width, c_width, sin2(i,:,:), sin2(i,:,:))
    enddo
    !$OMP END PARALLEL DO

    !$OMP PARALLEL DO
    do j = 1, nmol2
        do i = 1, nmol1
            D12(i,j) = distl2(X1(i,:,:,:), X2(j,:,:,:), Z1(i,:,:), Z2(j,:,:), N1(i), N2(j), &
                & width, cut_distance, r_width, c_width, sin1(i,:,:), sin2(j,:,:))

            D12(i,j) = D11(i) + D22(j) - 2.0d0 * D12(i,j)

            if (abs(D12(i,j)) < eps) D12(i,j) = 0.0d0

        enddo
    enddo
    !$OMP END PARALLEL DO

    deallocate(sin1)
    deallocate(sin2)

end subroutine molecular_arad_l2_distance_all

subroutine molecular_arad_l2_distance(X1, X2, Z1, Z2, N1, N2, width, &
    & cut_distance, r_width, c_width, distance)

    use funcs, only: m_dist, stoch_dist

    implicit none

    double precision, dimension(:,:,:), intent(in) :: X1
    double precision, dimension(:,:,:), intent(in) :: X2

    integer, dimension(:,:), intent(in) :: Z1
    integer, dimension(:,:), intent(in) :: Z2

    integer, intent(in) :: N1
    integer, intent(in) :: N2

    double precision, intent(in) :: width
    double precision, intent(in) :: cut_distance
    double precision, intent(in) :: r_width
    double precision, intent(in) :: c_width

    double precision, intent(out) :: distance

    integer :: j_1, j_2
    double precision :: D11, D22, D12

    double precision :: dd

    double precision :: rdist
    double precision :: cdist

    D11 = 0.0d0
    write (*,*) "N1:", N1
    write (*,*) "N2:", N2

    do j_1 = 1, N1
        write(*,*) "Z1:", "i", j_1, Z1(j_1,1), Z1(j_1,2)
    enddo

    do j_1 = 1, N1
        do j_2 = 1, N1

            rdist = abs(z1(j_1,1) - z1(j_2,1))
            CDist = abs(Z1(j_1,2) - Z1(j_2,2))

            dd = M_Dist(X1(j_1,:,:),X1(j_2,:,:),n1,n1,width,cut_distance,R_Width,C_width)
            D11 = D11 + dd * stoch_dist(RDist,CDist,R_Width,C_width)

        enddo
    enddo

    write(*,*) "D11:", "i", D11
    D22 = 0.0d0

    do j_1 = 1, N2
        do j_2 = 1, N2

            rdist = abs(z2(j_1,1) - z2(j_2,1))
            CDist = abs(Z2(j_1,2) - Z2(j_2,2))

            dd = M_Dist(X2(j_1,:,:),X2(j_2,:,:),n2,n2,width,cut_distance,R_Width,C_width)
            D22 = D22 + dd * stoch_dist(RDist,CDist,R_Width,C_width)

        enddo
    enddo

    D12 = 0.0d0

    do j_1 = 1, N1
        do j_2 = 1, N2

            rdist = abs(z1(j_1,1) - z2(j_2,1))
            CDist = abs(Z1(j_1,2) - Z2(j_2,2))

            dd = M_Dist(X1(j_1,:,:),X2(j_2,:,:),n1,n2,width,cut_distance,R_Width,C_width)
            D12 = D12 + dd * stoch_dist(RDist,CDist,R_Width,C_width)

        enddo
    enddo

    distance = D11 + D22 - 2.0d0 * D12

end subroutine molecular_arad_l2_distance

subroutine atomic_arad_l2_distance_all(q1, q2, z1, z2, n1, n2, nm1, nm2, width, &
    & cut_distance, r_width, c_width, amax, dmatrix)

    use funcs, only: m_dist, stoch_dist, atomic_distl2

    implicit none

    ! ARAD descriptors for the training set, format (i,j_1,5,m_1)
    double precision, dimension(:,:,:,:), intent(in) :: q1
    double precision, dimension(:,:,:,:), intent(in) :: q2

    ! ARAD atom-types for each atom in each molecule, format (i, j_1, 2)
    double precision, dimension(:,:,:), intent(in) :: z1
    double precision, dimension(:,:,:), intent(in) :: z2

    ! List of numbers of atoms in each molecule
    integer, dimension(:), intent(in) :: n1
    integer, dimension(:), intent(in) :: n2

    ! Number of molecules
    integer, intent(in) :: nm1
    integer, intent(in) :: nm2

    ! Max number of atoms in one descriptor
    integer, intent(in) :: amax

    ! ARAD parameters
    double precision, intent(in) :: width
    double precision, intent(in) :: cut_distance
    double precision, intent(in) :: r_width
    double precision, intent(in) :: c_width

    ! Resulting distance matrix
    double precision, dimension(nm1,nm2,amax,amax), intent(out) :: dmatrix

    ! Pre-computed sine terms
    double precision, allocatable, dimension(:,:,:) :: sin1
    double precision, allocatable, dimension(:,:,:) :: sin2

    ! Pre-computed terms in the full distance matrix
    double precision, allocatable, dimension(:,:) :: selfl21
    double precision, allocatable, dimension(:,:) :: selfl22

    ! Internal counters
    integer :: i, j, k, ni, nj
    integer :: m_1, i_1, j_1

    ! Pre-computed constants
    double precision :: r_width2
    double precision :: c_width2
    double precision :: inv_cut
    double precision :: l2dist

    ! Value of PI at full FORTRAN precision.
    double precision, parameter :: pi = 4.0d0 * atan(1.0d0)

    ! Small number // to test for numerical stability
    double precision, parameter :: eps = 5.0e-12

    r_width2 = r_width**2
    c_width2 = c_width**2

    inv_cut = pi / (2.0d0 * cut_distance)

    allocate(sin1(nm1, maxval(n1), maxval(n1)))
    allocate(sin2(nm2, maxval(n2), maxval(n2)))

    !$OMP PARALLEL DO PRIVATE(ni)
    do i = 1, nm1
        ni = n1(i)
        do m_1 = 1, ni
            do i_1 = 1, ni
                sin1(i, i_1, m_1) = 1.0d0 - sin(q1(i,i_1,1,m_1) * inv_cut)
            enddo
        enddo
    enddo
    !$OMP END PARALLEL DO

    !$OMP PARALLEL DO PRIVATE(ni)
    do i = 1, nm2
        ni = n2(i)
        do m_1 = 1, ni
            do i_1 = 1, ni
                sin2(i, i_1, m_1) = 1.0d0 - sin(q2(i,i_1,1,m_1) * inv_cut)
            enddo
        enddo
    enddo
    !$OMP END PARALLEL DO

    allocate(selfl21(nm1, maxval(n1)))
    allocate(selfl22(nm2, maxval(n2)))

    !$OMP PARALLEL DO PRIVATE(ni)
    do i = 1, nm1
        ni = n1(i)
        do i_1 = 1, ni
            selfl21(i,i_1) = atomic_distl2(q1(i,i_1,:,:), q1(i,i_1,:,:), n1(i), n1(i), &
                & sin1(i,i_1,:), sin1(i,i_1,:), width, cut_distance, r_width, c_width)
        enddo
    enddo
    !$OMP END PARALLEL DO

    !$OMP PARALLEL DO PRIVATE(ni)
    do i = 1, nm2
        ni = n2(i)
        do i_1 = 1, ni
            selfl22(i,i_1) = atomic_distl2(q2(i,i_1,:,:), q2(i,i_1,:,:), n2(i), n2(i), &
                & sin2(i,i_1,:), sin2(i,i_1,:), width, cut_distance, r_width, c_width)
        enddo
    enddo
    !$OMP END PARALLEL DO

    dmatrix = 0.0d0

    !$OMP PARALLEL DO PRIVATE(l2dist,ni,nj)
    do j = 1, nm2
        nj = n2(j)
        do i = 1, nm1
            ni = n1(i)

            ! atomic_distance(:,:) = 0.0d0

            do i_1 = 1, ni
                do j_1 = 1, nj

                    l2dist = atomic_distl2(q1(i,i_1,:,:), q2(j,j_1,:,:), n1(i), n2(j), &
                        & sin1(i,i_1,:), sin2(j,j_1,:), width, cut_distance, r_width, c_width)

                    l2dist = selfl21(i,i_1) + selfl22(j,j_1) - 2.0d0 * l2dist &
                        & * (r_width2/(r_width2 + (z1(i,i_1,1) - z2(j,j_1,1))**2) * &
                        & c_width2/(c_width2 + (z1(i,i_1,2) - z2(j,j_1,2))**2))

                    if (abs(l2dist) < eps) l2dist = 0.0d0

                    dmatrix(i,j,i_1,j_1) = l2dist

                enddo
            enddo

        enddo
    enddo
    !$OMP END PARALLEL DO

    deallocate(selfl21)
    deallocate(selfl22)
    deallocate(sin1)
    deallocate(sin2)
end subroutine atomic_arad_l2_distance_all


      real function cross(x1,y1,x2,y2)
c
c     + + + PURPOSE + + +
c
c     Determines the point where two lines cross
c
c     Called from subroutine EROD.
c     Author(s): M. Nearing
c     Reference in User Guide:
c
c     Changes:
c
c     Version: This module recoded from WEPP version 90.92.
c     Date recoded: 01/08/91.
c     Recoded by: Charles R. Meyer.
c
c
c     + + + KEYWORDS + + +
c
c
c     + + + ARGUMENT DECLARATIONS + + +
c
      real x1, y1, x2, y2
c
c     + + + ARGUMENT DEFINITIONS + + +
c     x1     -
c     y1     -
c     x2     -
c     y2     -
c
c     + + + LOCAL VARIABLES + + +
      real slope
c
c     slope  -   land slope (%)
c
c     + + + END SPECIFICATIONS + + +
c
c       new changes for overland flow elements
c
      if (x1.ne.x2) then
        slope = (y2-y1) / (x2-x1)
      else
        slope = 1.0e6
      end if
c
      cross = -y1 / slope + x1
c
      return
      end


        subroutine pvpl(l,p,d)
        common/gfgmp/p1(2),p2(2)
        real l(3),p(2)
        if(l(2).ne.0.) goto 10
        p1(1)=p(1)+d
        p1(2)=p(2)
        p2(1)=p(1)-d
        p2(2)=p(2)
        return
 10     if(l(1).ne.0.) goto 50
        p1(1)=p(1)
        p1(2)=p(2)+d
        p2(1)=p(1)
        p2(2)=p(2)-d
        return
  50    al=l(2)/l(1)
        be=p(2)-l(2)*p(1)/l(1)
        ga=1.+al**2
        de=al*be-al*p(2)-p(1)
        ep=p(1)**2+(be-p(2))**2-d**2
        h=de**2-ga*ep
        p1(1)=(-de+sqrt(h))/ga
        p1(2)=al*p1(1)+be
        p2(1)=(-de-sqrt(h))/ga
        p2(2)=al*p2(1)+be
        return
        end


    !use crop2mlModules
        REAL::appFLN, ttFromLastLeafToHeading, localDegfm, ttFromLastLeafToAnthesis
        IF ((phase >= 0.0) .AND. (phase < 1.0)) THEN
            IF (cumulTT>= dse) THEN
                phase = 1.0
            END IF
        ELSE IF ((phase >= 1.0) .AND. (phase < 2.0)) THEN
            IF ((isVernalizable==1 .AND. vernaprog >= 1.0) .OR. (isVernalizable==0)) THEN
                IF (dayLength > maxDL) THEN
                    finalLeafNumber = minFinalNumber
                    hasLastPrimordiumAppeared = 1
                ELSE
                    appFLN = minFinalNumber + sLDL * (maxDL - dayLength)
                        !calculation of final leaf number from dayLength at inflexion plus 2 leaves
                    IF ((appFLN / 2.0) <= leafNumber) THEN
                        finalLeafNumber = appFLN;
                        hasLastPrimordiumAppeared =1
                    END IF
                END IF
                IF (hasLastPrimordiumAppeared==1) THEN
                    phase = 2.0
                END IF
            END IF
        ELSE IF ((phase >= 2.0) .AND. (phase < 4.0)) THEN
            IF (isMomentRegistredZC_39==1) THEN
                IF (phase < 3.0) THEN
                    ttFromLastLeafToHeading = 0.0
                    IF(choosePhyllUse=="Default") THEN
                        ttFromLastLeafToHeading =(pFLLAnth - pHEADANTH) * fixPhyll
                    ELSE IF (choosePhyllUse == "PTQ") THEN
                        ttFromLastLeafToHeading = (pFLLAnth - pHEADANTH) * phyllochron;
                    ELSE IF (choosePhyllUse == "Test")  THEN
                        ttFromLastLeafToHeading = (pFLLAnth - pHEADANTH) * p
                    END IF
                    IF (cumulTTFromZC_39 >= ttFromLastLeafToHeading) THEN
                        phase = 3.0
                    END IF
                END IF
                ttFromLastLeafToAnthesis =0.0;
                IF (choosePhyllUse == "Default") THEN
                    ttFromLastLeafToAnthesis = pFLLAnth * fixPhyll
                ELSE IF (choosePhyllUse == "PTQ") THEN
                    ttFromLastLeafToAnthesis = pFLLAnth * phyllochron
                ELSE IF (choosePhyllUse == "Test") THEN
                    ttFromLastLeafToAnthesis = pFLLAnth * p
                END IF
                IF (cumulTTFromZC_39 >= ttFromLastLeafToAnthesis) THEN
                    phase = 4.0
                END IF
            END IF
        ELSE IF (phase == 4.0) THEN
            IF (grainCumulTT >= dcd) THEN
                phase = 4.5
            END IF
        ELSE IF (phase == 4.5) THEN
            IF ((grainCumulTT >= dgf) .OR. (gai <= 0)) THEN
                phase = 5.0
            END IF
        ELSE IF ((phase >= 5.0) .AND. (phase < 6.0)) THEN
            localDegfm = degfm
            IF (ignoreGrainMaturation .EQV. .TRUE.) THEN
                localDegfm = -1
            END IF
            IF (cumulTTFromZC_91 >= localDegfm) THEN
                phase = 6.0
            END IF
        END IF


      SUBROUTINE CONM2S
C
C THIS SUBROUTINE COMPUTES THE CONCENTRATED MASS ELEMENTS MASS MATRIX
C FOR THE M2 TYPE ELEMENT
C SINGLE PRECISION VERSION
C
C ECPTNO  NAME       TYPE  DESCRIPTION
C ******  ****       ****  ***********
C
C   1     IELID      I     ELEMENT ID
C   2     IGP        I     GRID POINT NUMBER
C   3     ICIDT2     I     COORDINATE SYSTEM ID FOR T2
C   4     MASS       R     LUMPED MASS
C   5     OFFSET(1)  R
C   6     OFFSET(2)  R     X,Y, AND Z COORDINATES OF THE
C   7     OFFSET(3)  R     OFFSET
C   8     MMI(1,1)   R
C   9     MMI(2,1)   R     MASS MOMENTS OF INERTIA
C  10     MMI(2,2)   R
C  11     MMI(3,1)   R
C  12     MMI(3,2)   R
C  13     MMI(3,3)   R
C  14     ICIDT1     I     COORDINATE SYSTEM ID FOR T1
C  15     X          R
C  16     Y          R
C  17     Z          R
C
      INTEGER DICT(11), ELID, ESTID, IECPT(14)
      REAL MM(36),TT(36),T(36)
      REAL MB, INER(6)
C
C
      COMMON /EMGEST/ ECPT(100)
C
      COMMON /EMGDIC/ DMM(2),NLOCS,ELID,ESTID
C
      COMMON /EMGPRM/ DUM(15),ISMB(3),IPREC,NOGO
C
      EQUIVALENCE (ECPT(1),IECPT(1),IELID)
      EQUIVALENCE (DICT(5),DICT5), (ECPT(4),MB)
      EQUIVALENCE (ECPT(5),XOF),(ECPT(6),YOF),(ECPT(7),ZOF)
      EQUIVALENCE (INER(1),ECPT(8))
C
C     INITIALIZE
C
      IF (ISMB(2) .EQ. 0) RETURN
      DICT(1) = ESTID
      DICT(2) = 1
      DICT(3) = 6
      DICT(4) = 63
      DICT(5) = 0
      IP = IPREC
C
C COMPUTE NON-TRANSFORMED MASS MATRIX.  INITIALIZE TO ZERO
C THEN FILL IN NON-ZERO TERMS
C
      DO 100 I=1,36
  100 MM(I) = 0.
C
      ICIDT2 = IECPT(3)
      IF (ICIDT2 .GE. 0) GO TO 120
      ICIDT2 = 0
      DO 110 I = 1,3
  110 ECPT (I+4) = ECPT(I+4) - ECPT(I+14)
C
  120 MM(1) = MB
      MM(5) = MB*ZOF
      MM(6) = -MB*YOF
      MM(8) = MB
      MM(10) = -MM(5)
      MM(12) = MB*XOF
      MM(15) = MB
      MM(16) = -MM(6)
      MM(17) = -MM(12)
      MM(20) = MM(10)
      MM(21) = MM(16)
      X2 = XOF**2
      Y2 = YOF**2
      Z2 = ZOF**2
      MM(22) = INER(1) + (Y2 + Z2)*MB
      MM(23) = -INER(2) + MM(6)*XOF
      MM(24) =  -INER(4)+MM(10)*XOF
      MM(25) = MM(5)
      MM(27) = MM(17)
      MM(28) = MM(23)
      MM(29) = INER(3) + (X2 + Z2)*MB
      MM(30) = -INER(5) + MM(6)*ZOF
      MM(31) = MM(6)
      MM(32) = MM(12)
      MM(34) = MM(24)
      MM(35) = MM(30)
      MM(36) = INER(6) + (X2 + Y2)*MB
C
      ICIDT1 = IECPT(14)
C
C PERFORM TRANSFORMATIONS.  IF CSIDS 1 AND 2 ARE EQUAL,
C T1 = T2 SO MASS MATRIX IS COMPLETE
C
      IF (ICIDT2 .EQ. ICIDT1) GO TO 240
C                            T
C NOT EQUAL SO COMPUTE T = (T )(T )
C                            1   2
C GET T1 AND T2 IF NEEDED
      IT = 18
      IF (ICIDT1 .EQ. 0) GO TO 130
C
      CALL TRANSS (ECPT(14),T(1))
      GO TO 140
C ONLY T2 NEEDED SO T = T2
  130 IT = 9
  140 IF (ICIDT2 .EQ. 0) GO TO 150
      ITEMP = IECPT(14)
      IECPT(14) = ICIDT2
      CALL TRANSS (ECPT(14),T(10))
      IECPT(14) = ITEMP
C
      IF(ICIDT1 .EQ. 0) GO TO 210
      CALL GMMATS (T(1),3,3,2,T(10),3,3,0,T(19))
      GO TO 210
C
C HERE T2 IS IDENTITY AND T1 IS AT T(1) SO
C T = T1 (TRANSPOSE).  SO INSERT INTO T
  150 DO 170 I = 1,3
      DO 170 J = 1,3
      IJ = 3*(I-1) + J
      JI = I + 3*(J-1) + 18
170   T(JI) = T(IJ)
C
C T = (T ) (T ) IS COMPLETE. INSERT IT IN THE 6X6 TRANSFORMATION MATRIX.
C       1    2
C
  210 DO 220 I = 1,36
  220 TT(I) = 0.
C
      DO 230 I = 1,3
      IJ = I + IT
      TT(I) = T(IJ)
      TT(I + 6) = T(IJ + 3)
      TT(I + 12) = T(IJ + 6)
      TT(I + 21) = T(IJ)
      TT(I + 27) = T(IJ + 3)
  230 TT(I + 33) = T(IJ + 6)
C           T
C FORM T*M*T  AND STORE IN MM
C
      CALL GMMATS (TT(1),6,6,0,MM(1),6,6,0,T(1))
      CALL GMMATS (T(1),6,6,0,TT(1),6,6,1,MM(1))
C
  240 CALL EMGOUT (MM,MM,36,1,DICT,2,IP)
      RETURN
      END


#include "eiscor.h"
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! z_upr1utri_rot3swap
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! This routine passes a rotation through an unitary plus rank one
! upper-triangular matrix (upr1utri) stored as the product of a diagonal
! matrix and 2 sequences of rotations.
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! INPUT VARIABLES:
!
!  DIR             LOGICAL
!                    .TRUE.: pass rotation from left to right
!                    .FALSE.: pass rotation from right to left
!
!  D               REAL(8) array of dimension (4)
!                    array of generators for complex diagonal matrice
!                    in the upper-triangular matrix
!
!  C               REAL(8) array of dimension (6)
!                    first array of generators for upper-triangular part
!
!  B               REAL(8) array of dimension (6)
!                    second array of generators for upper-triangular part
!
!  G               REAL(8) array of dimension (3)
!                    generators for rotation
!
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
subroutine z_upr1utri_rot3swap(DIR,D,C,B,G)

  implicit none

  ! input variables
  logical, intent(in) :: DIR
  real(8), intent(inout) :: D(4), C(6), B(6), G(3)

  ! compute variables
  logical :: SYM
  real(8) :: T(3)

  ! initialize SYM
  if ((C(1).EQ.B(1)).AND.(C(2).EQ.-B(2)).AND.(C(3).EQ.-B(3)).AND. &
             (C(4).EQ.B(4)).AND.(C(5).EQ.-B(5)).AND.(C(6).EQ.-B(6))) then
    SYM = .TRUE.
  else
    SYM = .FALSE.
  end if

  ! L2R
  if (DIR) then

    ! through D
    call z_rot3_swapdiag(D,G)

    ! through C and B, symmetric case
    if (SYM) then

      ! store G
      T = G

      ! through C
      call z_rot3_turnover(C(1:3),C(4:6),T)

      ! update B
      B(1) = C(1)
      B(2) = -C(2)
      B(3) = -C(3)
      B(4) = C(4)
      B(5) = -C(5)
      B(6) = -C(6)

    ! general case
    else

      ! though C
      call z_rot3_turnover(C(1:3),C(4:6),G)

      ! through B
      call z_rot3_turnover(B(4:6),B(1:3),G)

    end if

  ! R2L
  else

    ! through C and B, symmetric case
    if (SYM) then

      ! store G
      T = G

      ! through B
      call z_rot3_turnover(B(1:3),B(4:6),T)

      ! update B
      C(1) = B(1)
      C(2) = -B(2)
      C(3) = -B(3)
      C(4) = B(4)
      C(5) = -B(5)
      C(6) = -B(6)

    ! general case
    else

      ! through B
      call z_rot3_turnover(B(1:3),B(4:6),G)

      ! through C
      call z_rot3_turnover(C(4:6),C(1:3),G)

    end if

    ! through D
    call z_rot3_swapdiag(D,G)

  end if

end subroutine z_upr1utri_rot3swap


use sim
implicit none

type(SeismicAnalysis_) :: seismic(100)
type(FEMDomain_),target :: cube,original
type(IO_) :: f,response,history_A,history_V,history_U,input_wave
type(Math_) :: math
type(MPI_) :: mpid
real(real64),allocatable :: disp_z(:,:)
real(real64) :: wave(1000,2),T,Duration,dt
integer(int32) :: i,j,cases,stack_id,num_of_cases

num_of_cases = 4

call mpid%start()
call mpid%createStack(num_of_cases)

! create Domain
call cube%create(meshtype="Cube3D",x_num=2,y_num=2,z_num=20)
call cube%resize(x=1.0d0,y=1.0d0,z=5.0d0)
call cube%move(z=-5.0d0)

! Change T = 0.1, 0.2, 0.3 ... 1 (sec.)

do stack_id=1,size(mpid%localstack)
    cases = mpid%localstack(stack_id)
    ! create Wave
    T = 0.0500d0*cases ! sec.
    Duration = T * 10.0d0 ! sec.
    dt = Duration/dble(size(wave,1))

    wave(:,:) = 0.0d0
    do i=1,size(wave,1)
        wave(i,1) = dt*dble(i-1)
        wave(i,2) = sin(2.0d0*math%pi/T*wave(i,1) )
    enddo
    call input_wave%open("input_wave_"//str(cases)//".txt" )
    call input_wave%write(wave)
    call input_wave%close()
    original = cube

    ! set domain
    seismic(cases)%femdomain => cube
    ! set wave
    seismic(cases)%wave = wave

    ! run simulation
    call seismic(cases)%init()
    call seismic(cases)%fixDisplacement(z_max = -4.99d0,direction="x")
    call seismic(cases)%fixDisplacement(z_max = -4.99d0,direction="y")
    call seismic(cases)%fixDisplacement(z_max = -4.99d0,direction="z")

    call seismic(cases)%fixDisplacement(y_max = 0.0d0,direction="y")
    call seismic(cases)%fixDisplacement(y_min = 1.0d0,direction="y")

    call seismic(cases)%fixDisplacement(direction="z")

    call seismic(cases)%femdomain%vtk("mesh"//str(cases)//"_" )

    call seismic(cases)%loadWave(z_max=-4.50d0,direction="x",wavetype=WAVE_ACCEL)

    seismic(cases)%Density(:)      = 17000.0d0 !(N/m/m/m)
    seismic(cases)%YoungModulus(:) = 7000000.0d0 !(N/m/m)
    seismic(cases)%PoissonRatio(:) = 0.40d0

    seismic(cases)%alpha = 0.0d0
    seismic(cases)%beta = 0.0d0

    call history_A%open("history_A"//str(cases)//".txt","w")
    call history_V%open("history_V"//str(cases)//".txt","w")
    call history_U%open("history_U"//str(cases)//".txt","w")
    do i=1,999
        call seismic(cases)%run(timestep=[i,i],AccelLimit=10.0d0**8)
        write(history_A%fh,*) real(dble(i-1)*dt), real(seismic(cases)%A( size(seismic(cases)%A)-2 ))
        write(history_V%fh,*) real(dble(i-1)*dt), real(seismic(cases)%V( size(seismic(cases)%V)-2 ))
        write(history_U%fh,*) real(dble(i-1)*dt), real(seismic(cases)%U( size(seismic(cases)%U)-2 )    )
        call history_A%flush()
        call history_V%flush()
        call history_U%flush()
    enddo
    call history_A%close()
    call history_V%close()
    call history_U%close()

    print *, maxval(seismic(cases)%U)

    seismic(cases)%femdomain%mesh%nodcoord = seismic(cases)%femdomain%mesh%nodcoord + (10.0d0**6)*&
        reshape(seismic(cases)%U, seismic(cases)%femdomain%nn(),seismic(cases)%femdomain%nd() )

    call seismic(cases)%femdomain%vtk("x10"//str(cases)//"_")

    call response%open("T_A"//str(cases)//".txt")
    call response%write(T,seismic(cases)%maxA(1))
    call response%close()

enddo

call mpid%end()

end


!! ------------------------------------------------------------------------- !!
!>
!! An example code of an implementation of the absorbing boundary condition
!! for tsunami wave propagation: Liner Dispersive Wave (LDW) case
!!
!! @license
!!   (c) 2015 Takuto Maeda, Hiroaki Tsushima, and Takashi Furumura
!!   License: MIT
!!
!!   In addition, the users are suggested to cite the following accompanying
!!   paper when you use this code.
!!
!!   Maeda, T., H. Tsushima, and T. Furumura,
!!   An effective absorbing boundary condition for linear long-wave and
!!   linear dispersive wave tsunami simulations,
!!   in press, 2016.
!!
!<
!! ------------------------------------------------------------------------- !!
program ldw

  implicit none

  !! fixed parameters
  integer, parameter :: STDERR  = 0            !< Standard Error
  integer, parameter :: STDOUT  = 6            !< Standard Output
  real,    parameter :: PI      = atan(1.0)*4
  real,    parameter :: g0      = 9.80665      !< gravity
  integer, parameter :: na=20                  !< absorber thickness

  !! control parameters
  integer            :: nx,  ny                !< model size
  integer            :: nt                     !< time step size
  real               :: dx,  dy                !< grid width
  real               :: dt                     !< time step
  character(80)      :: title                  !< model title

  !! derived global variables
  real               :: dxi, dyi               !< inverse grid width
  real               :: xbeg, ybeg             !< model edge
  integer            :: nst                    !< number of stations

  !! arrays
  real,    allocatable :: eta(:,:)             !< tsunami height
  real,    allocatable :: mm(:,:)              !< tsunami vel
  real,    allocatable :: nn(:,:)              !< tsunami vel
  real,    allocatable :: hh(:,:)              !< bathymetry
  real,    allocatable :: hm(:,:)              !< x-averaged bathymetry
  real,    allocatable :: hn(:,:)              !< y-averaged bathymetry
  real,    allocatable :: fh(:,:)              !< land filter
  real,    allocatable :: fm(:,:)              !< land filter
  real,    allocatable :: fn(:,:)              !< land filter
  real,    allocatable :: x(:), y(:)           !< coordinate
  real,    allocatable :: g1x(:,:), g2x(:,:)   !< PML absorber
  real,    allocatable :: g1y(:,:), g2y(:,:)   !< PML absorber
  real,    allocatable :: eta_x(:,:)           !< PML eta(x)
  real,    allocatable :: dtm(:,:)             !< time deriv. of velocity
  real,    allocatable :: dtn(:,:)             !< time deriv. of velocity
  real,    allocatable :: dtm0(:,:)            !< time deriv. of velocity
  real,    allocatable :: dtn0(:,:)            !< time deriv. of velocity
  real,    allocatable :: wav(:,:)             !< waveform at stations
  integer, allocatable :: ist(:), jst(:)       !< station location (i,j)
  real,    allocatable :: xst(:), yst(:)       !< station location (x,y)
  real,    allocatable :: c1x(:,:), c1y(:,:)   !< coefs for LDW calculation
  real,    allocatable :: c2x(:,:), c2y(:,:)   !< coefs for LDW calculation
  real,    allocatable :: c3x(:,:), c3y(:,:)   !< coefs for LDW calculation
  real,    allocatable :: xw(:), yw(:)         !< PML weight function

  !! ----------------------------------------------------------------------- !!

  !! parameter input
  include "blk_param.f90"

  !! ----------------------------------------------------------------------- !!
  !>
  !! memory allocation
  !<
  !! --
  block

    integer :: i, j

    allocate( eta(1:nx,1:ny) )
    allocate( mm (1:nx,1:ny) )
    allocate( nn (1:nx,1:ny) )
    allocate( hh (1:nx,1:ny) )
    allocate( hm (1:nx,1:ny) )
    allocate( hn (1:nx,1:ny) )
    allocate( fh (1:nx,1:ny) )
    allocate( fm (1:nx,1:ny) )
    allocate( fn (1:nx,1:ny) )
    allocate( dtm(0:nx+1,0:ny+1) )
    allocate( dtn(0:nx+1,0:ny+1) )
    allocate( dtm0(0:nx+1,0:ny+1) )
    allocate( dtn0(0:nx+1,0:ny+1) )
    allocate( c1x(nx,ny), c2x(nx,ny), c3x(nx,ny) )
    allocate( c1y(nx,ny), c2y(nx,ny), c3y(nx,ny) )
    allocate( xw(nx), yw(ny) )
    allocate( g1x(2,nx), g2x(2,nx), g1y(2,ny), g2y(2,ny) )
    allocate( eta_x(nx,ny) )

    eta(:,:) = 0.0
    mm (:,:) = 0.0
    nn (:,:) = 0.0
    dtm(0:nx+1,0:ny+1) = 0
    dtn(0:nx+1,0:ny+1) = 0

    allocate( x(nx), y(ny) )
    do i=1, nx
       x(i) = (i-1) * dx + xbeg
    end do
    do j=1, ny
       y(j) = (j-1) * dy + ybeg
    end do

  end block

  include 'blk_initheight.f90'

  include 'blk_bathymetry.f90'

  include 'blk_station.f90'

  !! ----------------------------------------------------------------------- !!
  !>
  !! PML absorber settings
  !<
  !! --
  block

    real :: d1, d2, b1, b2
    integer :: i, j
    real :: d0, b0, R0
    real :: xxb, xxc
    real :: hx
    integer, parameter :: pd = 2
    integer, parameter :: pb = 2
    real,    parameter :: c0 = 500.0 !! assumed phase speed
    !! ----

    !! damping profile constants
    hx = na * dx
    R0 = 10**( - ( log10( real(na) ) - 1 ) / log10( 2.0 )  - 3.0 )
    d0 = - ( 1.0 / (2.0*hx) ) * ( pd +1 ) * c0 * log( R0 )
    b0 = 2.0

    !! initialize eta: assume initial amplitude wihtin PML layer is zero
    eta_x(1:nx,1:ny) = 0.0

    !! initialize
    do i=1, 2
       do j=1, nx
          g1x(i,j) = 1.0
          g2x(i,j) = 1.0
       end do
       do j=1, ny
          g1y(i,j) = 1.0
          g2y(i,j) = 1.0
       end do
    end do

    !! set absorber
    do i=1, na

       !! distance from PML boundary
       xxb = max(hx  - (i    ) * dx,0.)
       xxc = max(hx  - (i+0.5) * dx,0.)

       d1 = d0 * ( xxb / hx )**pd
       b1 = 1.0 + ( b0 - 1.0 ) * ( xxb / hx )**pb

       d2 = d0 * ( xxc / hx )**pd
       b2 = 1.0 + ( b0 - 1.0 ) * ( xxc / hx )**pb

       g1x(1,i) = (b1 - d1/2.0*dt) / (b1 + d1/2*dt)
       g1x(2,i) = (b1 - d2/2.0*dt) / (b1 + d2/2*dt)
       g2x(1,i) =  1.0             / (b1 + d1/2*dt)
       g2x(2,i) =  1.0             / (b1 + d2/2*dt)

       g1y(:,i) = g1x(:,i)
       g2y(:,i) = g2x(:,i)

       ! Opposite Side; Exchange Staggered Grid
       g1x(1,nx-i+1) = g1x(2,i)
       g1x(2,nx-i+1) = g1x(1,i)
       g2x(1,nx-i+1) = g2x(2,i)
       g2x(2,nx-i+1) = g2x(1,i)

       g1y(1,ny-i+1) = g1x(2,i)
       g1y(2,ny-i+1) = g1x(1,i)
       g2y(1,ny-i+1) = g2x(2,i)
       g2y(2,ny-i+1) = g2x(1,i)

    end do

  end block

  !! ----------------------------------------------------------------------- !!
  !>
  !! Use 10-m cut off for linear long wave
  !<
  block

    integer :: i, j
    real, parameter :: CUTOFF_DEPTH = 10.0

    do j=1, ny
       do i=1, nx
          if( hh(i,j) < 0.0 ) then
             eta(i,j) = 0.0
          else if ( hh(i,j) < CUTOFF_DEPTH ) then
             hh(i,j) = 10.0
          end if
       end do
    end do

  end block

  !! ----------------------------------------------------------------------- !!
  !>
  !! bathymetry averaging for staggered grid calculation
  !<
  block

    integer :: i, j

    ! x-averaged bathymetry
    do j=1, ny
       hm(1,j) = max( hh(1,j), 0.0 )
       do i=2, nx
          hm(i,j) = ( hh(i,j)+hh(i-1,j) ) / 2.0
          if( hh(i,j) <= 0.0  .or.  hh(i-1,j) <= 0.0 ) hm(i,j) = 0.0
       end do
    end do

    ! y-averaged bathymetry
    do i=1, nx
       do j=2, ny
          hn(i,j) = ( hh(i,j) + hh(i,j-1) ) / 2.0
          if( hh(i,j) <= 0.0  .or.  hh(i,j-1) <= 0.0 ) hn(i,j) = 0.0
       end do
       hn(i,1) = max( hh(i,1), 0.0 )
    end do

  end block

  !! ----------------------------------------------------------------------- !!
  !>
  !! define land-filter matrix for reflection boundaries
  !<
  block

    integer :: i, j

    fm(:,:) = 1.0
    fn(:,:) = 1.0
    fh(:,:) = 1.0

    do j=1, ny
       do i=1, nx
          if( hm(i,j) < 0.0 ) fm(i,j) = 0.0
          if( hn(i,j) < 0.0 ) fn(i,j) = 0.0
          if( hh(i,j) < 0.0 ) fh(i,j) = 0.0
       end do
    end do

  end block

  !! ----------------------------------------------------------------------- !!
  !>
  !! CFL stability condition check
  !<
  block

    if( min(dx,dy)/dt < sqrt( 2*g0*maxval(hh(:,:)) ) ) then
       write(STDERR,*) "ERROR: stability condition is violated: ",  &
            min(dx,dy)/dt,  sqrt( 2*g0*maxval(hh(:,:)) )
       stop
    end if

  end block

  !! ----------------------------------------------------------------------- !!
  !>
  !! Dispersive tsunami coefficieints
  !<
  !! --
  block
    real :: xdiv, ydiv
    integer :: i, j

    do j=1, ny
       do i=1, nx
          xdiv = 3*dx*dx / (3*dx*dx + 2*hm(i,j)*hm(i,j) )
          ydiv = 3*dy*dy / (3*dy*dy + 2*hn(i,j)*hn(i,j) )
          c1x(i,j) = - xdiv  * g0 * hm(i,j)
          c2x(i,j) = xdiv * hm(i,j)*hm(i,j) / ( 3 * dx*dx )
          c3x(i,j) = xdiv * hm(i,j)*hm(i,j) / ( 3 * dx*dy )
          c1y(i,j) = - ydiv  * g0 * hn(i,j)
          c2y(i,j) = ydiv * hn(i,j)*hn(i,j) / ( 3 * dy*dy )
          c3y(i,j) = ydiv * hn(i,j)*hn(i,j) / ( 3 * dx*dy )
       end do
    end do

    ! PML weight
    xw(:) = 1.0
    yw(:) = 1.0
    do i=1, na
       xw(i) = sin( PI * i / na / 2 )

       xw(nx-i+1) = xw(i)
       yw(i)      = xw(i)
       yw(ny-i+1) = yw(i)
    end do

  end block

  !! ----------------------------------------------------------------------- !!
  !>
  !! time stepping
  !<
  block

    integer :: it
    real :: dxeta(nx,ny), dyeta(nx,ny)
    real :: dxm(nx,ny), dyn(nx,ny)
    !! --

    do it= 1, nt

       if( mod(it,100) == 0 ) then
          write(STDERR,'(A,I4.4,A,I4.4)') trim(title) // &
               ": cycle = ", it, " / ", nt
       end if

       !! ------------------------------------------------------------------ !!
       !>
       !! difference of eta with respect to x, y
       !<
       !! --
       block

         integer :: i, j
         !! --

         do j=1, ny
            do i=2, nx
               dxeta(i,j) = ( eta(i,j) - eta(i-1,j) ) * dxi
            end do
            dxeta(1,j) = ( eta(1,j) -        0.0 ) * dxi
         end do

         do i=1, nx
            do j=2, ny
               dyeta(i,j) = ( eta(i,j) - eta(i,j-1) ) * dyi
            end do
            dyeta(i,1) = ( eta(i,1) -        0.0 ) * dyi
         end do

       end block

       !! ------------------------------------------------------------------ !!
       block

         integer, parameter :: ITER_MAX = 1000
         real,    parameter :: TOR = 1e-5
         integer :: k
         integer :: i, j
         real :: err, amax
         real :: mm1, nn1, div
         integer :: icol, jcol
         real :: tmp
         !! --

         !! initial values of time-derivative of vel are
         !! approximated by long-wave theory
         if( it == 1 ) then
            do j=1, ny
               do i=1, nx

                  dtm(i,j) = - g0*hm(i,j)*dxeta(i,j)*dt*fm(i,j)
                  dtn(i,j) = - g0*hn(i,j)*dyeta(i,j)*dt*fn(i,j)

               end do
            end do
         end if

         !! maximum value for conversion check
         amax =max( maxval( abs(dtm(2:nx-1,2:ny-1)) ), &
                    maxval( abs(dtn(2:nx-1,2:ny-1)) ) )
         if( amax < epsilon(1.0 ) ) amax = 1.0

         !! iteration for time-derivative of vel
         do k=1, ITER_MAX

            !! copy previous vel velocities
            do j=1, ny
               do i=1, nx
                  dtm0(i,j) = dtm(i,j)
                  dtn0(i,j) = dtn(i,j)
               end do
            end do

            err = -1e30
            do j=2, ny-1
               do i=2, nx-1
                  dtm(i,j) = c1x(i,j) * dxeta(i,j) &
                           + c2x(i,j) * ( dtm(i+1,j)+dtm(i-1,j) ) &
                           + c3x(i,j) * ( dtn(i,j+1)-dtn(i,j)-dtn(i-1,j+1)+dtn(i-1,j) )
                  err = max( err,  abs( dtm(i,j) - dtm0(i,j) ) / amax )
               end do
            end do

            do j=2, ny-1
               do i=2, nx-1
                  dtn(i,j) = c1y(i,j) *  dyeta(i,j) &
                           + c2y(i,j) * ( dtn(i,j+1)+dtn(i,j-1) ) &
                           + c3y(i,j) * ( dtm(i+1,j)-dtm(i,j)-dtm(i+1,j-1)+dtm(i,j-1))
                  err = max( err,  abs( dtn(i,j) - dtn0(i,j) ) / amax )
               end do
            end do

            if( err < TOR ) exit

         end do

         !! update tsunami velocity using dispersive terms
         do j=2, ny-1
            do i=2, nx-1
               mm(i,j) = g1x(1,i) * mm(i,j) &
                    + g2x(1,i) * ( ( xw(i) - 1 ) * g0 * hm(i,j) * dxeta(i,j) &
                                 + xw(i)   * dtm(i,j)  ) * dt * fm(i,j)
               nn(i,j) = g1y(1,j) * nn(i,j) &
                    + g2y(1,j) * ( ( yw(j) - 1 ) * g0 * hn(i,j) * dyeta(i,j) &
                                 + yw(j)   * dtn(i,j) ) * dt * fn(i,j)
            end do
         end do

       end block

       !! ------------------------------------------------------------------ !!
       !>
       !! difference of velocities with respect to x, y
       !<
       !! --
       block

         integer :: i, j
         !! --

         do j=1, ny
            dxm(nx,j) = ( 0.0 - mm(nx,j)  ) * dxi
            do i=1, nx-1
               dxm(i,j) = ( mm(i+1,j) - mm(i,j) ) * dxi
            end do
         end do

         do i=1, nx
            dyn(i,ny) = ( 0.0 - nn(i,ny) ) * dyi
            do j=1, ny-1
               dyn(i,j) = ( nn(i,j+1) - nn(i,j) ) * dyi
            end do
         end do

       end block

       !! ------------------------------------------------------------------ !!
       block

         integer i, j
         real :: dtxi, dtyi
         !! --

         dtxi = dt * dxi
         dtyi = dt * dyi

         do j=na+1, ny-na
            do i=na+1, nx-na
               eta(i,j) =  eta(i,j) - ( dxm(i,j) +  dyn(i,j) )*dt * fh(i,j)
            end do
         end do

       end block

       !! ------------------------------------------------------------------ !!
       block

         integer :: i, j
         real :: eta_y
         !! --

         do j=1, na
            do i=1, nx
               eta_y = eta(i,j) - eta_x(i,j)

               eta_x(i,j) = g1x(2,i) * eta_x(i,j) - g2x(2,i) * dxm(i,j) * dt
               eta_y      = g1y(2,j) * eta_y      - g2y(2,j) * dyn(i,j) * dt

               eta(i,j) = ( eta_x(i,j) + eta_y ) * fh(i,j)

            end do
         end do

         do j=ny-na+1,ny
            do i=1, nx
               eta_y = eta(i,j) - eta_x(i,j)

               eta_x(i,j) = g1x(2,i) * eta_x(i,j) - g2x(2,i) * dxm(i,j) * dt
               eta_y      = g1y(2,j) * eta_y      - g2y(2,j) * dyn(i,j) * dt

               eta(i,j) = ( eta_x(i,j) + eta_y ) * fh(i,j)
            end do
         end do

         do j=na+1, ny-na
            do i=1, na
               eta_y = eta(i,j) - eta_x(i,j)

               eta_x(i,j) = g1x(2,i) * eta_x(i,j) - g2x(2,i) * dxm(i,j) * dt
               eta_y      = g1y(2,j) * eta_y      - g2y(2,j) * dyn(i,j) * dt

               eta(i,j) = ( eta_x(i,j) + eta_y ) * fh(i,j)
            end do
         end do

         do j=na+1, ny-na
            do i=nx-na+1,nx
               eta_y = eta(i,j) - eta_x(i,j)

               eta_x(i,j) = g1x(2,i) * eta_x(i,j) - g2x(2,i) * dxm(i,j) * dt
               eta_y      = g1y(2,j) * eta_y      - g2y(2,j) * dyn(i,j) * dt

               eta(i,j) = ( eta_x(i,j) + eta_y ) * fh(i,j)
            end do
         end do

       end block

       !! ------------------------------------------------------------------ !!
       block

         integer :: i

         do i=1, nst
            wav(i,it) = eta( ist(i), jst(i) )
         end do

       end block

    end do

  end block

  !! ----------------------------------------------------------------------- !!
  block

    integer :: i, it
    character(256) :: fn
    integer, parameter :: io = 100
    !! --

    do i=1, nst
       write(fn,'(A,I3.3,A)') trim(adjustl(title))//'_', i, '.wav'
       open(io, file=fn, action='write', status='unknown' )
       do it=0, nt
          write(io,*) it*dt, wav(i,it)
       end do
       close(io)
    end do

  end block

  !! ----------------------------------------------------------------------- !!
  block

    deallocate( eta )
    deallocate( mm  )
    deallocate( nn  )
    deallocate( hh  )
    deallocate( hm  )
    deallocate( hn  )
    deallocate( fh  )
    deallocate( fm  )
    deallocate( fn  )
    deallocate( x, y )
    deallocate( ist, jst, xst, yst )
    deallocate( wav )
    deallocate( g1x, g2x, g1y, g2y )
    deallocate( eta_x )
    deallocate( c1x, c2x, c3x )
    deallocate( c1y, c2y, c3y )
  end block

end program ldw
!! ------------------------------------------------------------------------- !!


CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC
C
C   FFTPACK 5.0
C   Copyright (C) 1995-2004, Scientific Computing Division,
C   University Corporation for Atmospheric Research
C   Licensed under the GNU General Public License (GPL)
C
C   Authors:  Paul N. Swarztrauber and Richard A. Valent
C
C   $Id: costb1.f,v 1.2 2004/06/15 21:14:57 rodney Exp $
C
CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC

      SUBROUTINE COSTB1(N,INC,X,WSAVE,WORK,IER)
      REAL       X(INC,*)       ,WSAVE(*)
      DOUBLE PRECISION           DSUM
      IER = 0
      NM1 = N-1
      NP1 = N+1
      NS2 = N/2
      IF (N-2) 106,101,102
  101 X1H = X(1,1)+X(1,2)
      X(1,2) = X(1,1)-X(1,2)
      X(1,1) = X1H
      RETURN
  102 IF (N .GT. 3) GO TO 103
      X1P3 = X(1,1)+X(1,3)
      X2 = X(1,2)
      X(1,2) = X(1,1)-X(1,3)
      X(1,1) = X1P3+X2
      X(1,3) = X1P3-X2
      RETURN
  103 X(1,1) = X(1,1)+X(1,1)
      X(1,N) = X(1,N)+X(1,N)
      DSUM = X(1,1)-X(1,N)
      X(1,1) = X(1,1)+X(1,N)
      DO 104 K=2,NS2
         KC = NP1-K
         T1 = X(1,K)+X(1,KC)
         T2 = X(1,K)-X(1,KC)
         DSUM = DSUM+WSAVE(KC)*T2
         T2 = WSAVE(K)*T2
         X(1,K) = T1-T2
         X(1,KC) = T1+T2
  104 CONTINUE
      MODN = MOD(N,2)
      IF (MODN .EQ. 0) GO TO 124
      X(1,NS2+1) = X(1,NS2+1)+X(1,NS2+1)
  124 LENX = INC*(NM1-1)  + 1
      LNSV = NM1 + INT(LOG(REAL(NM1))) + 4
      LNWK = NM1
C
      CALL RFFT1F(NM1,INC,X,LENX,WSAVE(N+1),LNSV,WORK,
     1            LNWK,IER1)
      IF (IER1 .NE. 0) THEN
        IER = 20
        CALL XERFFT ('COSTB1',-5)
        RETURN
      ENDIF
C
      FNM1S2 = FLOAT(NM1)/2.
      DSUM = .5*DSUM
      X(1,1) = FNM1S2*X(1,1)
      IF(MOD(NM1,2) .NE. 0) GO TO 30
      X(1,NM1) = X(1,NM1)+X(1,NM1)
   30 FNM1S4 = FLOAT(NM1)/4.
      DO 105 I=3,N,2
         XI = FNM1S4*X(1,I)
         X(1,I) = FNM1S4*X(1,I-1)
         X(1,I-1) = DSUM
         DSUM = DSUM+XI
  105 CONTINUE
      IF (MODN .NE. 0) RETURN
         X(1,N) = DSUM
  106 RETURN
      END


! DART software - Copyright UCAR. This open source software is provided
! by UCAR, "as is", without charge, subject to all terms of use at
! http://www.image.ucar.edu/DAReS/DART/DART_download
!
! DART $Id$

program test_quad_irreg_interp

! intended to show how the state structure and quad code can be used
! together.  start with a simple regional grid and work out from there.

! Modules that are absolutely required for use are listed
use        types_mod, only : r8, i8, MISSING_R8, deg2rad, rad2deg
use    utilities_mod, only : error_handler, initialize_utilities, finalize_utilities
use   random_seq_mod, only : init_random_seq, random_seq_type, &
                             random_uniform, random_gaussian

use quad_utils_mod, only : quad_interp_handle, init_quad_interp, finalize_quad_interp, set_quad_coords,       &
                           quad_lon_lat_locate, quad_lon_lat_evaluate, GRID_QUAD_FULLY_REGULAR,               &
                           GRID_QUAD_IRREG_SPACED_REGULAR, GRID_QUAD_FULLY_IRREGULAR, GRID_QUAD_UNKNOWN_TYPE, &
                           QUAD_LOCATED_UNKNOWN, QUAD_LOCATED_CELL_CENTERS, QUAD_LOCATED_LON_EDGES,           &
                           QUAD_LOCATED_LAT_EDGES, QUAD_LOCATED_CELL_CORNERS

implicit none

integer :: debug = 0

type(quad_interp_handle) :: h

! data grid size
integer, parameter :: nx = 9
integer, parameter :: ny = 5

! locations of data grid corners
real(r8) :: data_lons(nx, ny) = MISSING_R8
real(r8) :: data_lats(nx, ny) = MISSING_R8

! extents of the data grid (these mimic a regional model's grid)
real(r8) :: start_lon = 100.0_r8
real(r8) :: end_lon   = 150.5_r8
real(r8) :: start_lat = -11.4_r8
real(r8) :: end_lat   =  34.1_r8

! angle to rotate data grid in degrees
! positive is counterclockwise; will rotate
! around lower left grid point (start lon/lat).
 real(r8) :: angle = 10.0_r8
!real(r8) :: angle = 45.0_r8
!real(r8) :: angle = 30.0_r8
!real(r8) :: angle =  90.0_r8
!real(r8) :: angle = -30.0_r8
!real(r8) :: angle = -10.0_r8
!real(r8) :: angle = 0.0_r8

! deform grid by this fraction of the deltas
real(r8) :: lon_def = 0.25_r8
real(r8) :: lat_def = 0.25_r8

! data values on the grid
real(r8) :: grid_data(nx, ny) = MISSING_R8
integer  :: data_choice = 0   ! see code for selection values

! percent of data values that should be marked 'missing data'
!real(r8) :: miss_percent =   0.0_r8    ! none
 real(r8) :: miss_percent =   3.0_r8    ! 3%
!real(r8) :: miss_percent = 100.0_r8    ! all

! sampling grid size
integer, parameter :: nrx = 210
integer, parameter :: nry = 150

! locations of sampling grid
real(r8) :: sample_lons(nrx) = MISSING_R8
real(r8) :: sample_lats(nry) = MISSING_R8

! extents of the sampling grid
real(r8) :: sample_start_lon = 110.0_r8
real(r8) :: sample_end_lon   = 140.0_r8
real(r8) :: sample_start_lat = -20.0_r8
real(r8) :: sample_end_lat   =  30.0_r8

! where interpolated values are stored on reg grid
real(r8) :: interp_data(nrx, nry) = MISSING_R8

type(random_seq_type) :: ran

integer  :: i, j, k
real(r8) :: lon_del, lat_del, sample_lon_del, sample_lat_del
integer  :: lon_bot, lat_bot, lon_top, lat_top
integer  :: four_lons(4), four_lats(4)
integer  :: istatus
real(r8) :: invals(4), outval
integer  :: iunit_orig, iunit_interp

call initialize_utilities('test_quad_irreg_interp')
call init_random_seq(ran)

lon_del = (end_lon - start_lon) / (nx-1)
lat_del = (end_lat - start_lat) / (ny-1)

! "data grid" corners and data vals
do i=1, nx
   do j=1, ny
      ! generate locations of the corners of all the quads
      data_lons(i, j) = start_lon + (i-1)*lon_del + deform(lon_del, lon_def, ran)
      data_lats(i, j) = start_lat + (j-1)*lat_del + deform(lat_del, lat_def, ran)
      if (angle /= 0.0_r8) &
         call rotate(data_lons(i, j), data_lats(i, j), angle, start_lon, start_lat)

      ! generate the data values on the corners.  pick one:
      select case (data_choice)
      case (1)
         ! increasing monotonically
         grid_data(i, j) = (j-1)*nx + i
      case (2)
         ! constant by row
         grid_data(i, j) = j
      case (3)
         ! constant by column
         grid_data(i, j) = i
      case (4)
         ! based on lon only
         grid_data(i, j) = data_lons(i, j)
      case (5)
         ! based on lat only
         grid_data(i, j) = data_lats(i, j)
      case (6)
         ! random between (0-10)
         grid_data(i, j) = random_uniform(ran) * 10.0_r8
      case default
         ! gaussian with mean 0 and stddev 1
         grid_data(i, j) = random_gaussian(ran, 0.0_r8, 1.0_r8)
      end select

      if (miss_percent > 0.0_r8) then
        if (random_uniform(ran) * 100.0_r8 < miss_percent) grid_data(i, j) = MISSING_R8
      endif

   enddo
enddo

sample_lon_del = (sample_end_lon - sample_start_lon) / nrx
sample_lat_del = (sample_end_lat - sample_start_lat) / nry

! "sampled grid" spacing along each axis
do i=1, nrx
   sample_lons(i) = sample_start_lon + (i-1)*sample_lon_del
enddo
do j=1, nry
   sample_lats(j) = sample_start_lat + (j-1)*sample_lat_del
enddo

! end of data setup - now call interp routines

call init_quad_interp(GRID_QUAD_FULLY_IRREGULAR, nx, ny, QUAD_LOCATED_CELL_CENTERS, .false., .false., .false., h)
call set_quad_coords(h, data_lons, data_lats)

! for each location in the sampling grid, interpolate a data value
do i=1, nrx
   do j=1, nry
      call quad_lon_lat_locate(h, sample_lons(i), sample_lats(j), four_lons, four_lats, istatus)
      if (istatus /= 0) then
         !print *, 'location outside of grid: ', sample_lons(i), sample_lats(j)
         interp_data(i, j) = MISSING_R8
         cycle
      endif
      if (debug > 0) print *, i, j, four_lons(1), four_lons(3), four_lats(1), four_lats(3), sample_lons(i), sample_lats(j)

      ! get values of data at lon/lat bot/top indices, counterclockwise around quad
      do k=1, 4
         invals(k) = grid_data(four_lons(k), four_lats(k))
      enddo

      call quad_lon_lat_evaluate(h, sample_lons(i), sample_lats(j), four_lons, four_lats, &
                                 invals, outval, istatus)

      interp_data(i, j) = outval

   enddo
enddo

! this program doesn't currently have any missing locations - but i'll test that next.
if (debug > 0) print *, 'number of missing values in  input data: ', count(grid_data(:,:) == MISSING_R8)
if (debug > 0) print *, 'number of missing values in output data: ', count(interp_data(:,:) == MISSING_R8)

call writeit_2d('data_lons_2d_irreg_test.txt', nx, ny, data_lons)
call writeit_2d('data_lats_2d_irreg_test.txt', nx, ny, data_lats)
call writeit_2d('data_data_2d_irreg_test.txt', nx, ny, grid_data)

call writeit_1d('sample_lons_1d_irreg_test.txt', nrx, sample_lons)
call writeit_1d('sample_lats_1d_irreg_test.txt', nry, sample_lats)
call writeit_2d('sample_data_2d_irreg_test.txt', nrx, nry, interp_data)

call finalize_quad_interp(h)
call finalize_utilities('test_quad_irreg_interp')

if (debug > 0) print *, 'closed files and finalized interp handle'

contains

!------------------------------------------------------------
! rotate vector a counterclockwise by angle theta, relative
! to the given origin point.

subroutine rotate(x, y, theta, x0, y0)
 real(r8), intent(inout) :: x, y
 real(r8), intent(in)    :: theta
 real(r8), intent(in)    :: x0, y0

real(r8) :: a(2), b(2)
real(r8) :: r(2,2)
real(r8) :: rads

a(1) = x - x0
a(2) = y - y0

rads = theta * deg2rad

r(1,1) = cos(rads)
r(1,2) = sin(rads)
r(2,1) = sin(-rads)
r(2,2) = cos(rads)

b(1) = r(1,1)*a(1) + r(1,2)*a(2)
b(2) = r(2,1)*a(1) + r(2,2)*a(2)

x = b(1) + x0
y = b(2) + y0

end subroutine rotate

!------------------------------------------------------------
! compute +/- a random value based on a width and percentage
! of that width

function deform(width, fraction, seq)

use random_seq_mod

 real(r8), intent(in) :: width
 real(r8), intent(in) :: fraction
 type(random_seq_type), intent(inout) :: seq
 real(r8)             :: deform

real(r8) :: val

! random val between -1 and 1
val = (random_uniform(seq) * 2.0_r8) - 1.0_r8

deform = val * width * fraction

end function deform

!------------------------------------------------------------

subroutine writeit_1d(fname, nx, dataarray)

use    utilities_mod

 character(len=*), intent(in) :: fname
 integer, intent(in) :: nx
 real(r8), intent(in) :: dataarray(:)

integer :: i, j, iunit

iunit = open_file(fname, action='write')

do i=1, nx
   write(iunit, *) dataarray(i)
enddo

call close_file(iunit)

end subroutine writeit_1d

!------------------------------------------------------------

subroutine writeit_2d(fname, nx, ny, dataarray)

use    utilities_mod

 character(len=*), intent(in) :: fname
 integer, intent(in) :: nx, ny
 real(r8), intent(in) :: dataarray(nx, ny)

integer :: i, j, iunit

iunit = open_file(fname, action='write')

do i=1, nx
   do j=1, ny
      write(iunit, *) dataarray(i, j)
   enddo
enddo

call close_file(iunit)

end subroutine writeit_2d

!------------------------------------------------------------

end program test_quad_irreg_interp

! <next few lines under version control, do not edit>
! $URL$
! $Id$
! $Revision$
! $Date$


C $Id$
************************************************************************
*                                                                      *
      subroutine ecp_angint (angint,lambda,k,l,G_kq)
*                                                                      *
*   Set up array of 3j products, contract over spherical tensors.      *
*   This is (in TeX) \sum_q [(1+\delta_{mu,0})/(1+\delta_{\rho,0})]    *
*      \times {\cal B}^{\lambda k \ell}_{\mu q m}  G_{kq}              *
*                                                                      *
*   Written by K. G. Dyall                                             *
*                                                                      *
************************************************************************
      implicit none
      integer lambda,k,l,mu,q,m
      double precision angint(-l:l,-lambda:lambda),G_kq(-k:k),wa
*
C      write (6,*) 'ecp_angint'
C      write (6,*) loc(angint(-l,-lambda))
C      write (0,*) lambda,k,l
      do m = 1,l
        do mu = 1,lambda
          angint(m,mu) = 0.0d00
          angint(m,-mu) = 0.0d00
          angint(-m,mu) = 0.0d00
          angint(-m,-mu) = 0.0d00
          q = m+mu
          if (q .le. k) then
            call ecp_3j_prod (lambda,l,k,mu,m,wa)
C            write (6,'(1PE15.7)') wa
C            write (6,'(1P2E15.7)') G_kq(q),G_kq(-q)
            angint(m,mu) = angint(m,mu)+wa*G_kq(q)
            angint(-m,-mu) = angint(-m,-mu)-wa*G_kq(q)
            if (q .ne. 0) then
              angint(m,-mu) = angint(m,-mu)+wa*G_kq(-q)
              angint(-m,mu) = angint(-m,mu)+wa*G_kq(-q)
            end if
C            write (6,'(1P2E15.7)') angint(m,mu),angint(m,-mu)
C            write (6,'(1P2E15.7)') angint(-m,mu),angint(-m,-mu)
          end if
          q = abs(mu-m)
          if (q .le. k) then
            call ecp_3j_prod (lambda,l,k,mu,-m,wa)
C            write (6,'(1PE15.7)') wa
C            write (6,'(1P2E15.7)') G_kq(q),G_kq(-q)
            angint(m,mu) = angint(m,mu)+wa*G_kq(q)
            angint(-m,-mu) = angint(-m,-mu)+wa*G_kq(q)
            if (q .ne. 0) then
              if (mu .gt. m) wa = -wa
              angint(m,-mu) = angint(m,-mu)-wa*G_kq(-q)
              angint(-m,mu) = angint(-m,mu)+wa*G_kq(-q)
            end if
C            write (6,'(1P2E15.7)') angint(m,mu),angint(m,-mu)
C            write (6,'(1P2E15.7)') angint(-m,mu),angint(-m,-mu)
          end if
        end do
        angint(m,0) = 0.0d00
        angint(-m,0) = 0.0d00
        q = m
        if (q .le. k) then
          call ecp_3j_prod (lambda,l,k,0,m,wa)
C          write (6,'(1PE15.7)') wa
          wa = wa+wa
C          write (6,'(1P2E15.7)') G_kq(q),G_kq(-q)
          angint(m,0) = wa*G_kq(q)
          angint(-m,0) = wa*G_kq(-q)
C          write (6,'(1PE15.7)') angint(m,0)
C          write (6,'(1PE15.7)') angint(-m,0)
        end if
      end do
      do mu = 1,lambda
        angint(0,mu) = 0.0d00
        angint(0,-mu) = 0.0d00
        q = mu
        if (q .le. k) then
          call ecp_3j_prod (lambda,l,k,mu,0,wa)
C          write (6,'(1PE15.7)') wa
C          write (6,'(1P2E15.7)') G_kq(q),G_kq(-q)
          angint(0,mu) = wa*G_kq(q)
          angint(0,-mu) = wa*G_kq(-q)
C          write (6,'(1P2E15.7)') angint(0,mu),angint(0,-mu)
        end if
      end do
      call ecp_3j_prod (lambda,l,k,0,0,wa)
C      write (6,'(1PE15.7)') wa
C      write (6,'(1PE15.7)') G_kq(0)
      angint(0,0) = wa*G_kq(0)
C      write (6,'(1PE15.7)') angint(0,0)
*
      return
      end