!
!    Copyright 2011 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 orthodrome

  ! This module provides methods to calculate azimuths and distances between
  ! points on a sphere or ellipsoid of rotation.

  ! It also provides a method to calculate relative azimuths and distances for
  ! small perturbations of the point coordinates.

  ! All angles and coordinates in the subroutines are required in radians.

    use constants
    use util

    implicit none

   type, public :: t_geo_coords
        real*8 :: lat, lon
    end type

    private
    public approx_differential_azidist
    public arcdistance
    public distance
    public distance_accurate50m
    public azimuth
    public azibazi
    public azidist

    public r2d
    public d2r

  ! degrees to radians
    interface d2r
        module procedure d2r_tgc
        module procedure d2r_r
        module procedure d2r_d
    end interface

  ! radians to degrees
    interface r2d
        module procedure r2d_tgc
        module procedure r2d_r
        module procedure r2d_d
    end interface

  ! these parameters control what approximation is used when calculating relative
  ! azimuths and distances:
  !
  ! flat geometry approximation is used for distances up to this:
  !  real*8, parameter, public :: max_distance_flat_approx = 250000. ! [m]
    real*8, parameter, public :: max_distance_flat_approx = -1. ! [m] ! turned off.

  ! constant azimuth approximation is used when distance/dr ratio is greater than this:
  !  real*8, parameter, public :: min_ratio_const_azimuth_approx = 50.
    real*8, parameter, public :: min_ratio_const_azimuth_approx = huge(min_ratio_const_azimuth_approx) ! turned off.

  contains

    subroutine approx_differential_azidist( delta_x, delta_y, azimuth, backazimuth, dist,&
                                      new_azimuth, new_backazimuth, new_dist )

        real, intent(in) :: delta_x, delta_y ! north, east
        real*8, intent(in) :: azimuth, backazimuth, dist
        real*8, intent(out) :: new_azimuth, new_backazimuth, new_dist

      ! given two points a and b connected with distance 'dist' in [m]
      ! and azimuth 'azimuth', calculate azimuth and arc-distance to b
      ! from new point with relative cartesian coordinates r=(delta_x,delta_y)
      ! the new azimuth and distance are put to 'new_dist'
      ! and 'new_azimuth'

      ! the following two approximations are tried in this order:

      ! 1) if dist < max_distance_flat_approx
      !    the calculation is performed assuming a flat geometry

      ! 2) if r/dist > min_ratio_const_azimuth_approx
      !    it is assumed that the azimuth does not change

      ! 3) exact formula for spherical earth are used

        real*8 :: new_dist_x, new_dist_y
        real*8 :: r, a,b,c
        real*8 :: alpha, beta, gamma, lambda

        if (dist < max_distance_flat_approx) then
          ! flat approximation for local distances
            new_dist_x = dist*cos(azimuth)-delta_x
            new_dist_y = dist*sin(azimuth)-delta_y
            new_azimuth = atan2(new_dist_y, new_dist_x)
            new_backazimuth = backazimuth + (new_azimuth-azimuth)
            new_dist = sqrt(new_dist_x**2 + new_dist_y**2)
        else

            r = sqrt(delta_x**2 + delta_y**2)

            if (dist/r > min_ratio_const_azimuth_approx) then
              ! 'const-azimuth'-approximation for larger distances
                new_azimuth = azimuth
                new_backazimuth = backazimuth
                new_dist = dist - (delta_x*cos(azimuth) + delta_y*sin(azimuth))
            else
              ! "exact" formulas assuming spherical earth
                a = r/earthradius
                b = dist/earthradius
                lambda = atan2(delta_y,delta_x)
                gamma = azimuth-lambda

                c = acos( clip(cos(a)*cos(b)+sin(a)*sin(b)*cos(gamma),-1D00,1D00) )
                alpha = asin( clip(sin(a)*sin(gamma)/sin(c),-1D00,1D00) )
                beta = asin( clip(sin(b)*sin(gamma)/sin(c),-1D00,1D00) )

                ! put alpha and beta to the correct quandrant...
                if (cos(a)-cos(b)*cos(c) < 0) then
                    if (alpha > 0) then
                        alpha = pi_-alpha
                    else
                        alpha = -pi_-alpha
                    end if
                end if
                if (cos(b)-cos(a)*cos(c) < 0) then
                    if (beta > 0) then
                        beta = pi_-beta
                    else
                        beta = -pi_-beta
                    end if
                end if

                new_dist = c*earthradius
                new_backazimuth = wrap(backazimuth + alpha,-pi_,pi_)
                new_azimuth = wrap(lambda - pi_ - beta,-pi_,pi_)

            end if

        end if

    end subroutine

    elemental function clip( x, mi, ma )
        real*8, intent(in) :: x, mi, ma
        real*8 :: clip

        clip = min(max(mi,x),ma)

    end function

    elemental function wrap( x, mi, ma )
        real*8, intent(in) :: x, mi, ma
        real*8 :: wrap
        wrap = x - floor((x-mi)/(ma-mi)) * (ma-mi)
    end function

    elemental function arcdistance( a,b )

        type(t_geo_coords), intent(in) :: a,b
        real*8 :: arcdistance

      ! returns arc-distance in radians between points a and b.
      ! coordinates must be given in radians.

        arcdistance = acos(cosdelta(a,b))

    end function

    elemental function distance( a, b )

       type(t_geo_coords), intent(in) :: a,b
       real*8 :: distance

        distance = arcdistance(a, b)*earthradius

    end function

    elemental function distance_accurate50m( a, b )

      ! more accurate distance calculation based on a spheroid of rotation

      ! returns distance in [m] between points a and b
      ! coordinates must be given in radians.

      ! should be accurate to 50 m using WGS84
      !     earth_oblateness = 1./298.257223563
      !     earthradius_equator = 6378.14 * 1000.

      ! from wikipedia :  http://de.wikipedia.org/wiki/Orthodrome
      ! based on: Meeus, J.: Astronomical Algorithms, S 85, Willmann-Bell,
      !           Richmond 2000 (2nd ed., 2nd printing), ISBN 0-943396-61-1

        type(t_geo_coords), intent(in) :: a,b
        real*8 :: distance_accurate50m

        real*8 :: f,g,l,s,c,w,r,d,h1,h2

        f = (a%lat + b%lat) / 2.
        g = (a%lat - b%lat) / 2.
        l = (a%lon - b%lon) / 2.

        s = sin(g)**2 * cos(l)**2 + cos(f)**2 * sin(l)**2
        c = cos(g)**2 * cos(l)**2 + sin(f)**2 * sin(l)**2

        w = atan( sqrt( s/c ) )
        r = sqrt(s*c)/w
        d = 2.*w*earthradius_equator
        h1 = (3.*r-1.)/(2.*c)
        h2 = (3.*r+1.)/(2.*s)

        distance_accurate50m = d * (1.+ earth_oblateness * h1 * sin(f)**2 * cos(g)**2 - &
                                        earth_oblateness * h2 * cos(f)**2 * sin(g)**2)

    end function

    elemental function azimuth( a, b )

        type(t_geo_coords), intent(in) :: a,b
        real*8 :: azimuth

      ! get azimuth of point b as seen at a.
      ! coordinates must be given in radians.
      ! result is in the range ]-pi,pi]

        azimuth = atan2( cos(a%lat) * cos(b%lat) * sin( b%lon - a%lon ), &
                         sin(b%lat) - sin(a%lat) * cosdelta(a,b) )

    end function

    elemental subroutine azibazi( a, b, azimuth, backazimuth )

        type(t_geo_coords), intent(in) :: a,b
        real*8, intent(out) :: azimuth, backazimuth

        real*8 :: t, sb, sa, cd

      ! get azimuth of point b as seen at a.
      ! and azimuth of point a as seen at b (backazimuth).
      ! coordinates must be given in radians.
      ! result is in the range ]-pi,pi]

        t = cos(a%lat) * cos(b%lat) * sin( b%lon - a%lon )
        sb = sin(b%lat)
        sa = sin(a%lat)
        cd = cosdelta(a,b)

        azimuth     = atan2(  t, sb - sa * cd )
        backazimuth = atan2( -t, sa - sb * cd )

    end subroutine

    elemental subroutine azidist( a,b, azimuth, arcdist )

        type(t_geo_coords), intent(in) :: a,b
        real*8,intent(out) :: azimuth, arcdist

      ! get arc-distance and azimuth in one shot
      ! coordinates must be given in radians.

        real*8 :: cd

        cd = cosdelta(a,b)

        azimuth = atan2( cos(a%lat) * cos(b%lat) * sin( b%lon - a%lon ), &
                         sin(b%lat) - sin(a%lat) * cd )
        arcdist = acos(cd)

    end subroutine

    elemental function cosdelta( a, b )

        type(t_geo_coords), intent(in) :: a,b
        real*8 :: cosdelta

      ! needed by azimuth and arcdistance

        cosdelta = sin(a%lat) * sin(b%lat) + cos(a%lat) * cos(b%lat) * cos(b%lon-a%lon)

    end function

    elemental function d2r_tgc( deg ) result(rad)

        type(t_geo_coords), intent(in) :: deg
        type(t_geo_coords) :: rad

        rad%lat = 2./360.*pi*deg%lat
        rad%lon = 2./360.*pi*deg%lon

    end function

    elemental function r2d_tgc( rad ) result(deg)

        type(t_geo_coords), intent(in) :: rad
        type(t_geo_coords) :: deg

        deg%lat = 360./2./pi*rad%lat
        deg%lon = 360./2./pi*rad%lon

    end function

    elemental function d2r_r( deg ) result(rad)

        real, intent(in) :: deg
        real :: rad

        rad = 2./360.*pi*deg

    end function

    elemental function r2d_r( rad ) result(deg)

        real, intent(in) :: rad
        real :: deg

        deg = 360./2./pi*rad

    end function

    elemental function d2r_d( deg ) result(rad)

        real*8, intent(in) :: deg
        real*8 :: rad

        rad = 2./360.*pi*deg

    end function

    elemental function r2d_d( rad ) result(deg)

        real*8, intent(in) :: rad
        real*8 :: deg

        deg = 360./2./pi*rad

    end function

end module


C FILE: AICD.F
       SUBROUTINE aicd(aic,aicder,data1,npts)
C
C      Calculate AIC for picker
C
       INTEGER npts
       REAL*8 data1(npts)
       REAL*8 aic(npts)
       REAL*8 aicder(npts)
       REAL*8 data2(npts)

       REAL*8 sum_data, sum_data2
       REAL*8 sum_data_k, mean_data_k1, mean_data_k2
       REAL*8 sum_data2_k, mean_data2_k1, mean_data2_k2

Cf2py  intent(in) npts
Cf2py  intent(in) data1
Cf2py  intent(out) aic
Cf2py  intent(out) aicder
Cf2py  depend(npts) aic
Cf2py  depend(npts) aicder
Cf2py  depend(npts) data1

       data2 = 0.d0
       sum_data = 0.d0
       sum_data2 = 0.d0

       DO i = 1, npts
          data2(i)  = data1(i)*data1(i)
          sum_data  = sum_data + data1(i)
          sum_data2 = sum_data2 + data2(i)
       ENDDO

       sum_data_k = sum_data - data1(npts)
       sum_data2_k = sum_data2 - data2(npts)
       x=0.0

       DO i = npts - 1, 2, -1
          sum_data_k = sum_data_k - data1(i)
          mean_data_k1 = (sum_data_k / i)*(sum_data_k / i)
          mean_data_k2 = ((sum_data-sum_data_k)/(npts-i))**2

          sum_data2_k = sum_data2_k - data2(i)
          mean_data2_k1 = sum_data2_k/i
          mean_data2_k2 = (sum_data2 - sum_data2_k)/(npts-i)

          a = i*log(mean_data2_k1-mean_data_k1)+(npts-i)*
     &          log(mean_data2_k2-mean_data_k2)

          IF ( a == -log(x) ) THEN
            a = aic(i+1)
          ENDIF

          aic(i) = a

       ENDDO

       aic(1) = aic(2)
       aic(npts) = aic(npts-1)

       DO i = 2, npts
          aicder(i) = ABS(aic(i)-aic(i-1))
       ENDDO
       aicder(1) = aicder(2)

       END
C END FILE AICD.F


!
!  Copyright 2018 S.A. Sato
!
!  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.
!
subroutine dt_evolve_macro_field
  use global_variables
  use ms_maxwell_ks_variables
  implicit none

  call dt_evolve_macro_field_explicit_multi_step
!  call dt_evolve_macro_field_explicit
!  call dt_evolve_macro_field_implicit

contains
!--------------------------------------------------------------
  subroutine dt_evolve_macro_field_explicit_multi_step
    implicit none
    real(8) :: clap0, clap1
    real(8) :: Lap_Ac(nx_s:nx_e)
    real(8) :: jt_tmp(Mx), af(Mx), bf(Mx), cf(Mx)
    integer :: ix
    integer :: iter
    real(8) :: xx

    clap0 = -2d0/dx_m**2
    clap1 = 1d0/dx_m**2

    af = (jt_m-2d0*jt_old_m+jt_old2_m)/dt**2
    bf = 0.5d0*(3d0*jt_m-4d0*jt_old_m+jt_old2_m)/dt
    cf = jt_m

    do iter = 0, nt_internal_m-1
      xx = dt_m*iter
      jt_tmp = 0.5d0*af*xx**2+bf*xx+cf

      Lap_Ac(nx_s) = clap0*Ac_m(nx_s) + clap1*Ac_m(nx_s+1)
      do ix = nx_s+1,nx_e-1
        Lap_Ac(ix) = clap0*Ac_m(ix) + clap1*(Ac_m(ix+1)+Ac_m(ix-1))
      end do
      Lap_Ac(nx_e) = clap0*Ac_m(nx_e) + clap1*Ac_m(nx_e-1)

      Ac_m_n = 2d0*Ac_m - Ac_m_o + (dt_m*clight)**2*Lap_Ac
      Ac_m_n(1:Mx) = Ac_m_n(1:Mx) -4d0*pi*dt_m**2*jt_tmp(1:Mx)

      Ac_m_o = Ac_m
      Ac_m = Ac_m_n

    end do

  end subroutine dt_evolve_macro_field_explicit_multi_step
!--------------------------------------------------------------
  subroutine dt_evolve_macro_field_explicit
    implicit none
    real(8) :: clap0, clap1
    real(8) :: Lap_Ac(nx_s:nx_e)
    integer :: ix

    clap0 = -2d0/dx_m**2
    clap1 = 1d0/dx_m**2

    Lap_Ac(nx_s) = clap0*Ac_m(nx_s) + clap1*Ac_m(nx_s+1)
    do ix = nx_s+1,nx_e-1
      Lap_Ac(ix) = clap0*Ac_m(ix) + clap1*(Ac_m(ix+1)+Ac_m(ix-1))
    end do
    Lap_Ac(nx_e) = clap0*Ac_m(nx_e) + clap1*Ac_m(nx_e-1)

    Ac_m_n = 2d0*Ac_m - Ac_m_o + (dt*clight)**2*Lap_Ac
    Ac_m_n(1:Mx) = Ac_m_n(1:Mx) -4d0*pi*dt**2*jt_m(1:Mx)
  end subroutine dt_evolve_macro_field_explicit
!--------------------------------------------------------------
  subroutine dt_evolve_macro_field_implicit
    implicit none
    real(8),parameter :: eps_error = 1d-28
    real(8) :: xvec(nx_s:nx_e), bvec(nx_s:nx_e)
    real(8) :: rvec(nx_s:nx_e), pvec(nx_s:nx_e)
    real(8) :: Axvec(nx_s:nx_e), Apvec(nx_s:nx_e)
    real(8) :: rr_p, rr_p_old, alpha, beta
    real(8) :: pAp_p
    real(8) :: clap0, clap1
    real(8) :: coeff0, coeff1
    real(8) :: bnorm, xnorm
    integer :: iter

    clap0 = -2d0/dx_m**2
    clap1 = 1d0/dx_m**2

    coeff0 = 1d0 -0.5d0*(dt*clight)**2*clap0
    coeff1 = -0.5d0*(dt*clight)**2*clap1

    call operate(Ac_m_o,bvec, coeff0, coeff1)
    bvec = -bvec + 2d0*Ac_m
    bvec(1:Mx) = bvec(1:Mx) - 4d0*pi*dt**2*jt_m(1:Mx)
    bnorm = sum(bvec**2)

! initial guess
    call dt_evolve_macro_field_explicit
    xvec = Ac_m_n
    xnorm = sum(xvec**2)

    call operate(xvec,Axvec, coeff0, coeff1)
    rvec = bvec-Axvec
    pvec = rvec

    rr_p = sum(rvec**2)
    rr_p_old = rr_p
    iter = 0
    if(sqrt(rr_p/xnorm) < eps_error)then
      return
    end if

    do iter = 1, 10
      call operate(pvec,Apvec, coeff0, coeff1)
      pAp_p = sum(pvec*Apvec)

      alpha = rr_p/pAp_p
      xvec = xvec + alpha*pvec
      xnorm = sum(xvec**2)
      rvec = rvec - alpha*Apvec
      rr_p_old = rr_p
      rr_p = sum(rvec**2)
      if(sqrt(rr_p/xnorm) < eps_error)exit

      beta = rr_p/rr_p_old
      pvec = rvec + beta*pvec

    end do
    Ac_m_n = xvec

  end subroutine dt_evolve_macro_field_implicit
!--------------------------------------------------------------
  subroutine operate(xvec, Axvec, coeff0, coeff1)
    implicit none
    real(8),intent(in)  :: xvec(nx_s:nx_e), coeff0, coeff1
    real(8),intent(out) :: Axvec(nx_s:nx_e)
    integer :: ix

    Axvec(nx_s) = coeff0*xvec(nx_s) + coeff1*xvec(nx_s+1)
    do ix = nx_s+1,nx_e-1
      Axvec(ix) = coeff0*xvec(ix) + coeff1*(xvec(ix+1)+xvec(ix-1))
    end do
    Axvec(nx_e) = coeff0*xvec(nx_e) + coeff1*xvec(nx_e-1)

  end subroutine operate

end subroutine dt_evolve_macro_field


      subroutine iniptz(ptz,iptz,modus)
        !KJ This routine rewrites the ptz-matrix.

      implicit none
c     which polarization tensor to create
      integer iptz
c     two ways of working
      integer modus
c     the polarization tensor
      complex*16 ptz(-1:1,-1:1)

      complex*16 zero,one,coni
      parameter (zero=(0,0),one=(1,0),coni=(0,1))
      integer i,j
      real*8 unity(3,3)

      if (iptz.lt.1.or.iptz.gt.10) then
          call wlog('Inieln sees weird iptz - returns without
     1      changing ptz - danger of calculating nonsense !!')
      endif

      do i=1,3
      do j=1,3
        unity(i,j)=dble(0)
      enddo
      unity(i,i)=dble(1)/dble(3)
      enddo
      do i=-1,1
      do j=-1,1
        ptz(i,j)=zero
      enddo
      enddo

      if (modus.eq.1) then
! work in spherical coordinates

         if(iptz.eq.10) then
        do i=-1,1
        do j=-1,1
           ptz(i,j)=unity(i+2,j+2)
        enddo
        enddo
         else
            i=(iptz-1)/3+1  ! row index
            j=iptz-3*(i-1)  ! column index
            i=i-2 !shift from 1..3 to -1..1
            j=j-2
            ptz(i,j)=one
         endif

      elseif(modus.eq.2) then
! work in carthesian coordinates

      if (iptz.eq.10) then ! orientation averaged spectrum
        do i=-1,1
        do j=-1,1
           ptz(i,j)=unity(i+2,j+2)
        enddo
        enddo

        elseif (iptz.eq.1) then   ! x x*
          ptz(1,1)=one/dble(2)
        ptz(-1,-1)=one/dble(2)
        ptz(-1,1)=-one/dble(2)
        ptz(1,-1)=-one/dble(2)
        elseif (iptz.eq.5) then ! y y*
          ptz(1,1)=one/dble(2)
        ptz(-1,-1)=one/dble(2)
        ptz(-1,1)=one/dble(2)
        ptz(1,-1)=one/dble(2)
         elseif (iptz.eq.9) then ! z z*
        ptz(0,0)=one
        elseif (iptz.eq.2) then ! x y*
          ptz(1,1)=one*coni/dble(2)
        ptz(-1,-1)=-one*coni/dble(2)
        ptz(-1,1)=-one*coni/dble(2)
        ptz(1,-1)=one*coni/dble(2)
        elseif (iptz.eq.4) then ! x* y
          ptz(1,1)=-one*coni/dble(2)
        ptz(-1,-1)=one*coni/dble(2)
        ptz(-1,1)=-one*coni/dble(2)
        ptz(1,-1)=one*coni/dble(2)
        elseif (iptz.eq.3) then ! x z*
          ptz(-1,0)=one/dsqrt(dble(2))
        ptz(1,0)=-one/dsqrt(dble(2))
        elseif (iptz.eq.7) then ! x* z
          ptz(0,-1)=one/dsqrt(dble(2))
        ptz(0,1)=-one/dsqrt(dble(2))
        elseif (iptz.eq.6) then ! y z*
          ptz(-1,0)=-one*coni/dsqrt(dble(2))
        ptz(1,0)=-one*coni/dsqrt(dble(2))
        elseif (iptz.eq.8) then ! y* z
          ptz(0,-1)=one*coni/dsqrt(dble(2))
        ptz(0,1)=one*coni/dsqrt(dble(2))
      endif

        else
          stop 'alien modus in inieln'
        endif

      return
      end


c
c
c =========================================================
      subroutine src1(maxmx,meqn,mbc,mx,xlower,dx,q,maux,aux,t,dt)
c =========================================================
      implicit real*8(a-h,o-z)
      dimension q(1-mbc:maxmx+mbc, meqn)
c
      common /comrp/ grav
      common /comsrc/ ndim
c
c     # source terms for radial symmetry in shallow water equations
c
c     # ndim should be set in setprob.f
c     # ndim = 2  for cylindrical symmetry
c     # ndim = 3  for spherical symmetry
c
c     # 2-stage Runge-Kutta method
c
      do 10 i=1,mx+mbc
	 xcell = xlower + (i-0.5d0)*dx
	 qstar1 = q(i,1) - 0.5d0*dt*(ndim-1)/xcell * q(i,2)
 	 qstar2 = q(i,2) - 0.5d0*dt*(ndim-1)/xcell * q(i,2)**2 / q(i,1)
c
	 q(i,1) = q(i,1) - dt*(ndim-1)/xcell * qstar2
 	 q(i,2) = q(i,2) - dt*(ndim-1)/xcell * qstar2**2 / qstar1
   10    continue
c
      return
      end


MODULE Controllers

    USE, INTRINSIC :: ISO_C_Binding
    USE Functions
    USE Filters

    IMPLICIT NONE

CONTAINS
    SUBROUTINE PitchControl(avrSWAP, CntrPar, LocalVar, objInst)

        USE DRC_Types, ONLY : ControlParameters, LocalVariables, ObjectInstances

        ! Local Variables:
        REAL(C_FLOAT), INTENT(INOUT)    :: avrSWAP(*)   ! The swap array, used to pass data to, and receive data from the DLL controller.
        INTEGER(4)                      :: K            ! Index used for looping through blades.

        TYPE(ControlParameters), INTENT(INOUT)  :: CntrPar
        TYPE(LocalVariables), INTENT(INOUT)     :: LocalVar
        TYPE(ObjectInstances), INTENT(INOUT)    :: objInst

        !..............................................................................................................................
        ! Pitch control
        !..............................................................................................................................
        IF (LocalVar%PC_State >= 1) THEN
            LocalVar%PC_MaxPitVar = CntrPar%PC_MaxPit
        ELSE
            LocalVar%PC_MaxPitVar = CntrPar%PC_FinePit
        END IF

        ! Compute the gain scheduling correction factor based on the previously
        ! commanded pitch angle for blade 1:
        LocalVar%PC_KP = interp1d(CntrPar%PC_GS_angles, CntrPar%PC_GS_KP, LocalVar%PC_PitComT)
        LocalVar%PC_KI = interp1d(CntrPar%PC_GS_angles, CntrPar%PC_GS_KI, LocalVar%PC_PitComT)
        LocalVar%PC_KD = interp1d(CntrPar%PC_GS_angles, CntrPar%PC_GS_KD, LocalVar%PC_PitComT)
        LocalVar%PC_TF = interp1d(CntrPar%PC_GS_angles, CntrPar%PC_GS_TF, LocalVar%PC_PitComT)

        ! Integrate the error signal w.r.t. time; saturate the integral term using the pitch angle limits:
        ! Compute the pitch commands associated with the proportional and integral
        ! gains:
        IF (LocalVar%iStatus == 0) THEN
            LocalVar%PC_PitComT = PIController(LocalVar%PC_SpdErr, LocalVar%PC_KP, LocalVar%PC_KI, CntrPar%PC_FinePit, LocalVar%PC_MaxPitVar, LocalVar%DT, LocalVar%PitCom(1), .TRUE., objInst%instPI)
        ELSE
            LocalVar%PC_PitComT = PIController(LocalVar%PC_SpdErr, LocalVar%PC_KP, LocalVar%PC_KI, CntrPar%PC_FinePit, LocalVar%PC_MaxPitVar, LocalVar%DT, CntrPar%PC_FinePit, .FALSE., objInst%instPI)
        END IF

        ! Individual pitch control
        IF ((CntrPar%IPC_ControlMode >= 1) .OR. (CntrPar%Y_ControlMode == 2)) THEN
            CALL IPC(CntrPar, LocalVar, objInst)
        ELSE
            LocalVar%IPC_PitComF = 0.0 ! THIS IS AN ARRAY!!
        END IF

        ! Fore-aft tower vibration damping control
        IF ((CntrPar%FA_KI > 0.0) .OR. (CntrPar%Y_ControlMode == 2)) THEN
            CALL ForeAftDamping(CntrPar, LocalVar, objInst)
        ELSE
            LocalVar%FA_PitCom = 0.0 ! THIS IS AN ARRAY!!
        END IF

        ! Sine excitation on pitch
        IF (CntrPar%Z_EnableSine == 1) THEN
            LocalVar%PC_SineExcitation = CntrPar%Z_PitchAmplitude*sin(CntrPar%Z_PitchFrequency*LocalVar%Time)
        ELSE
            LocalVar%PC_SineExcitation = 0
        END IF

        ! Combine and saturate all pitch commands:
        DO K = 1,LocalVar%NumBl ! Loop through all blades, add IPC contribution and limit pitch rate
            ! PitCom(K) = ratelimit(LocalVar%PC_PitComT_IPC(K), LocalVar%BlPitch(K), PC_MinRat, PC_MaxRat, LocalVar%DT) ! Saturate the overall command of blade K using the pitch rate limit
            LocalVar%PitCom(K) = saturate(LocalVar%PC_PitComT, CntrPar%PC_MinPit, CntrPar%PC_MaxPit)                    ! Saturate the overall command using the pitch angle limits
            LocalVar%PitCom(K) = LocalVar%PitCom(K) + LocalVar%IPC_PitComF(K) + LocalVar%FA_PitCom(K) + LocalVar%PC_SineExcitation
        END DO

        ! Command the pitch demanded from the last
        ! call to the controller (See Appendix A of Bladed User's Guide):
        avrSWAP(42) = LocalVar%PitCom(1)    ! Use the command angles of all blades if using individual pitch
        avrSWAP(43) = LocalVar%PitCom(2)    ! "
        avrSWAP(44) = LocalVar%PitCom(3)    ! "
        avrSWAP(45) = LocalVar%PitCom(1)    ! Use the command angle of blade 1 if using collective pitch
    END SUBROUTINE PitchControl

    SUBROUTINE VariableSpeedControl(avrSWAP, CntrPar, LocalVar, objInst)

        USE DRC_Types, ONLY : ControlParameters, LocalVariables, ObjectInstances

        REAL(C_FLOAT), INTENT(INOUT)            :: avrSWAP(*)    ! The swap array, used to pass data to, and receive data from, the DLL controller.

        TYPE(ControlParameters), INTENT(INOUT)  :: CntrPar
        TYPE(LocalVariables), INTENT(INOUT)     :: LocalVar
        TYPE(ObjectInstances), INTENT(INOUT)    :: objInst

        !..............................................................................................................................
        ! VARIABLE-SPEED TORQUE CONTROL:
        ! Compute the generator torque, which depends on which region we are in:
        !..............................................................................................................................
        IF (LocalVar%VS_State >= 4) THEN
            LocalVar%GenArTq = PIController(LocalVar%VS_SpdErrAr, CntrPar%VS_KP(1), CntrPar%VS_KI(1), CntrPar%VS_MaxOMTq, CntrPar%VS_ArSatTq, LocalVar%DT, CntrPar%VS_ArSatTq, .TRUE., objInst%instPI)
            LocalVar%GenBrTq = PIController(LocalVar%VS_SpdErrBr, CntrPar%VS_KP(1), CntrPar%VS_KI(1), CntrPar%VS_MinTq, CntrPar%VS_MinOMTq, LocalVar%DT, CntrPar%VS_MinOMTq, .TRUE., objInst%instPI)
            IF (LocalVar%VS_State == 4) THEN
                LocalVar%GenTq = CntrPar%VS_RtTq
            ELSEIF (LocalVar%VS_State == 5) THEN
                LocalVar%GenTq = (CntrPar%VS_RtPwr/CntrPar%VS_GenEff)/LocalVar%GenSpeedF
            END IF
        ELSE
            LocalVar%GenArTq = PIController(LocalVar%VS_SpdErrAr, CntrPar%VS_KP(1), CntrPar%VS_KI(1), CntrPar%VS_MaxOMTq, CntrPar%VS_ArSatTq, LocalVar%DT, CntrPar%VS_MaxOMTq, .FALSE., objInst%instPI)
            LocalVar%GenBrTq = PIController(LocalVar%VS_SpdErrBr, CntrPar%VS_KP(1), CntrPar%VS_KI(1), CntrPar%VS_MinTq, CntrPar%VS_MinOMTq, LocalVar%DT, CntrPar%VS_MinOMTq, .FALSE., objInst%instPI)
            IF (LocalVar%VS_State == 3) THEN
                LocalVar%GenTq = LocalVar%GenArTq
            ELSEIF (LocalVar%VS_State == 1) THEN
                LocalVar%GenTq = LocalVar%GenBrTq
            ELSEIF (LocalVar%VS_State == 2) THEN
                LocalVar%GenTq = CntrPar%VS_Rgn2K*LocalVar%GenSpeedF*LocalVar%GenSpeedF
            ELSE
                LocalVar%GenTq = CntrPar%VS_MaxOMTq
            END IF
        END IF

        ! Saturate the commanded torque using the maximum torque limit:
        LocalVar%GenTq = MIN(LocalVar%GenTq, CntrPar%VS_MaxTq)                    ! Saturate the command using the maximum torque limit

        ! Saturate the commanded torque using the torque rate limit:
        IF (LocalVar%iStatus == 0)  LocalVar%VS_LastGenTrq = LocalVar%GenTq       ! Initialize the value of LocalVar%VS_LastGenTrq on the first pass only
        LocalVar%GenTq = ratelimit(LocalVar%GenTq, LocalVar%VS_LastGenTrq, -CntrPar%VS_MaxRat, CntrPar%VS_MaxRat, LocalVar%DT)    ! Saturate the command using the torque rate limit

        ! Reset the value of LocalVar%VS_LastGenTrq to the current values:
        LocalVar%VS_LastGenTrq = LocalVar%GenTq

        ! Set the command generator torque (See Appendix A of Bladed User's Guide):
        avrSWAP(47) = LocalVar%VS_LastGenTrq   ! Demanded generator torque
    END SUBROUTINE VariableSpeedControl

    SUBROUTINE YawRateControl(avrSWAP, CntrPar, LocalVar, objInst)

        USE DRC_Types, ONLY : ControlParameters, LocalVariables, ObjectInstances

        REAL(C_FLOAT), INTENT(INOUT) :: avrSWAP(*) ! The swap array, used to pass data to, and receive data from, the DLL controller.

        TYPE(ControlParameters), INTENT(INOUT)    :: CntrPar
        TYPE(LocalVariables), INTENT(INOUT)       :: LocalVar
        TYPE(ObjectInstances), INTENT(INOUT)      :: objInst

        !..............................................................................................................................
        ! Yaw control
        !..............................................................................................................................

        IF (CntrPar%Y_ControlMode == 1) THEN
            avrSWAP(29) = 0                                      ! Yaw control parameter: 0 = yaw rate control
            IF (LocalVar%Time >= LocalVar%Y_YawEndT) THEN        ! Check if the turbine is currently yawing
                avrSWAP(48) = 0.0                                ! Set yaw rate to zero

                LocalVar%Y_ErrLPFFast = LPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_omegaLPFast, LocalVar%iStatus, .FALSE., objInst%instLPF)        ! Fast low pass filtered yaw error with a frequency of 1
                LocalVar%Y_ErrLPFSlow = LPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_omegaLPSlow, LocalVar%iStatus, .FALSE., objInst%instLPF)        ! Slow low pass filtered yaw error with a frequency of 1/60

                LocalVar%Y_AccErr = LocalVar%Y_AccErr + LocalVar%DT*SIGN(LocalVar%Y_ErrLPFFast**2, LocalVar%Y_ErrLPFFast)    ! Integral of the fast low pass filtered yaw error

                IF (ABS(LocalVar%Y_AccErr) >= CntrPar%Y_ErrThresh) THEN                                   ! Check if accumulated error surpasses the threshold
                    LocalVar%Y_YawEndT = ABS(LocalVar%Y_ErrLPFSlow/CntrPar%Y_Rate) + LocalVar%Time        ! Yaw to compensate for the slow low pass filtered error
                END IF
            ELSE
                avrSWAP(48) = SIGN(CntrPar%Y_Rate, LocalVar%Y_MErr)        ! Set yaw rate to predefined yaw rate, the sign of the error is copied to the rate
                LocalVar%Y_ErrLPFFast = LPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_omegaLPFast, LocalVar%iStatus, .TRUE., objInst%instLPF)        ! Fast low pass filtered yaw error with a frequency of 1
                LocalVar%Y_ErrLPFSlow = LPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_omegaLPSlow, LocalVar%iStatus, .TRUE., objInst%instLPF)        ! Slow low pass filtered yaw error with a frequency of 1/60
                LocalVar%Y_AccErr = 0.0    ! "
            END IF
        END IF
    END SUBROUTINE YawRateControl

    SUBROUTINE IPC(CntrPar, LocalVar, objInst)
        !-------------------------------------------------------------------------------------------------------------------------------
        ! Individual pitch control subroutine
        ! Calculates the commanded pitch angles for IPC employed for blade fatigue load reductions at 1P and 2P
        !
        ! Variable declaration and initialization
        !------------------------------------------------------------------------------------------------------------------------------
        USE DRC_Types, ONLY : ControlParameters, LocalVariables, ObjectInstances

        ! Local variables
        REAL(4)                  :: PitComIPC(3), PitComIPCF(3), PitComIPC_1P(3), PitComIPC_2P(3)
        INTEGER(4)               :: K                                       ! Integer used to loop through turbine blades
        REAL(4)                  :: axisTilt_1P, axisYaw_1P, axisYawF_1P    ! Direct axis and quadrature axis outputted by Coleman transform, 1P
        REAL(4), SAVE            :: IntAxisTilt_1P, IntAxisYaw_1P           ! Integral of the direct axis and quadrature axis, 1P
        REAL(4)                  :: axisTilt_2P, axisYaw_2P, axisYawF_2P    ! Direct axis and quadrature axis outputted by Coleman transform, 1P
        REAL(4), SAVE            :: IntAxisTilt_2P, IntAxisYaw_2P           ! Integral of the direct axis and quadrature axis, 1P
        REAL(4)                  :: IntAxisYawIPC_1P                        ! IPC contribution with yaw-by-IPC component
        REAL(4)                  :: Y_MErrF, Y_MErrF_IPC                    ! Unfiltered and filtered yaw alignment error [rad]

        TYPE(ControlParameters), INTENT(INOUT)  :: CntrPar
        TYPE(LocalVariables), INTENT(INOUT)     :: LocalVar
        TYPE(ObjectInstances), INTENT(INOUT)    :: objInst

        ! Body
        ! Initialization
        ! Set integrals to be 0 in the first time step
        IF (LocalVar%iStatus==0) THEN
            IntAxisTilt_1P = 0.0
            IntAxisYaw_1P = 0.0
            IntAxisTilt_2P = 0.0
            IntAxisYaw_2P = 0.0
        END IF

        ! Pass rootMOOPs through the Coleman transform to get the tilt and yaw moment axis
        CALL ColemanTransform(LocalVar%rootMOOP, LocalVar%Azimuth, NP_1, axisTilt_1P, axisYaw_1P)
        CALL ColemanTransform(LocalVar%rootMOOP, LocalVar%Azimuth, NP_2, axisTilt_2P, axisYaw_2P)

        ! High-pass filter the MBC yaw component and filter yaw alignment error, and compute the yaw-by-IPC contribution
        IF (CntrPar%Y_ControlMode == 2) THEN
            Y_MErrF = SecLPFilter(LocalVar%Y_MErr, LocalVar%DT, CntrPar%Y_IPC_omegaLP, CntrPar%Y_IPC_zetaLP, LocalVar%iStatus, .FALSE., objInst%instSecLPF)
            Y_MErrF_IPC = PIController(Y_MErrF, CntrPar%Y_IPC_KP(1), CntrPar%Y_IPC_KI(1), -CntrPar%Y_IPC_IntSat, CntrPar%Y_IPC_IntSat, LocalVar%DT, 0.0, .FALSE., objInst%instPI)
        ELSE
            axisYawF_1P = axisYaw_1P
            Y_MErrF = 0.0
            Y_MErrF_IPC = 0.0
        END IF

        ! Integrate the signal and multiply with the IPC gain
        IF ((CntrPar%IPC_ControlMode >= 1) .AND. (CntrPar%Y_ControlMode /= 2)) THEN
            IntAxisTilt_1P = IntAxisTilt_1P + LocalVar%DT * CntrPar%IPC_KI(1) * axisTilt_1P
            IntAxisYaw_1P = IntAxisYaw_1P + LocalVar%DT * CntrPar%IPC_KI(1) * axisYawF_1P
            IntAxisTilt_1P = saturate(IntAxisTilt_1P, -CntrPar%IPC_IntSat, CntrPar%IPC_IntSat)
            IntAxisYaw_1P = saturate(IntAxisYaw_1P, -CntrPar%IPC_IntSat, CntrPar%IPC_IntSat)

            IF (CntrPar%IPC_ControlMode >= 2) THEN
                IntAxisTilt_2P = IntAxisTilt_2P + LocalVar%DT * CntrPar%IPC_KI(2) * axisTilt_2P
                IntAxisYaw_2P = IntAxisYaw_2P + LocalVar%DT * CntrPar%IPC_KI(2) * axisYawF_2P
                IntAxisTilt_2P = saturate(IntAxisTilt_2P, -CntrPar%IPC_IntSat, CntrPar%IPC_IntSat)
                IntAxisYaw_2P = saturate(IntAxisYaw_2P, -CntrPar%IPC_IntSat, CntrPar%IPC_IntSat)
            END IF
        ELSE
            IntAxisTilt_1P = 0.0
            IntAxisYaw_1P = 0.0
            IntAxisTilt_2P = 0.0
            IntAxisYaw_2P = 0.0
        END IF

        ! Add the yaw-by-IPC contribution
        IntAxisYawIPC_1P = IntAxisYaw_1P + Y_MErrF_IPC

        ! Pass direct and quadrature axis through the inverse Coleman transform to get the commanded pitch angles
        CALL ColemanTransformInverse(IntAxisTilt_1P, IntAxisYawIPC_1P, LocalVar%Azimuth, NP_1, CntrPar%IPC_aziOffset(1), PitComIPC_1P)
        CALL ColemanTransformInverse(IntAxisTilt_2P, IntAxisYaw_2P, LocalVar%Azimuth, NP_2, CntrPar%IPC_aziOffset(2), PitComIPC_2P)

        ! Sum nP IPC contributions and store to LocalVar data type
        DO K = 1,LocalVar%NumBl
            PitComIPC(K) = PitComIPC_1P(K) + PitComIPC_2P(K)

            ! Optionally filter the resulting signal to induce a phase delay
            IF (CntrPar%IPC_CornerFreqAct > 0.0) THEN
                PitComIPCF(K) = LPFilter(PitComIPC(K), LocalVar%DT, CntrPar%IPC_CornerFreqAct, LocalVar%iStatus, .FALSE., objInst%instLPF)
            ELSE
                PitComIPCF(K) = PitComIPC(K)
            END IF

            LocalVar%IPC_PitComF(K) = PitComIPCF(K)
        END DO
    END SUBROUTINE IPC

    SUBROUTINE ForeAftDamping(CntrPar, LocalVar, objInst)
        !-------------------------------------------------------------------------------------------------------------------------------
        ! Fore-aft damping controller, reducing the tower fore-aft vibrations using pitch
        !
        ! Variable declaration and initialization
        !------------------------------------------------------------------------------------------------------------------------------
        USE DRC_Types, ONLY : ControlParameters, LocalVariables, ObjectInstances

        ! Local variables
        INTEGER(4) :: K    ! Integer used to loop through turbine blades

        TYPE(ControlParameters), INTENT(INOUT)  :: CntrPar
        TYPE(LocalVariables), INTENT(INOUT)     :: LocalVar
        TYPE(ObjectInstances), INTENT(INOUT)    :: objInst

        ! Body
        LocalVar%FA_AccHPFI = PIController(LocalVar%FA_AccHPF, 0.0, CntrPar%FA_KI, -CntrPar%FA_IntSat, CntrPar%FA_IntSat, LocalVar%DT, 0.0, .FALSE., objInst%instPI)

        ! Store the fore-aft pitch contribution to LocalVar data type
        DO K = 1,LocalVar%NumBl
            LocalVar%FA_PitCom(K) = LocalVar%FA_AccHPFI
        END DO

    END SUBROUTINE ForeAftDamping
END MODULE Controllers


      SUBROUTINE MB03LF( COMPQ, COMPU, ORTH, N, Z, LDZ, B, LDB, FG,
     $                   LDFG, NEIG, Q, LDQ, U, LDU, ALPHAR, ALPHAI,
     $                   BETA, IWORK, LIWORK, DWORK, LDWORK, BWORK,
     $                   IWARN, INFO )
C
C     SLICOT RELEASE 5.5.
C
C     Copyright (c) 2002-2012 NICONET e.V.
C
C     PURPOSE
C
C     To compute the relevant eigenvalues of a real N-by-N skew-
C     Hamiltonian/Hamiltonian pencil aS - bH, with
C
C                                   (  B  F  )      (  0  I  )
C       S = T Z = J Z' J' Z and H = (        ), J = (        ),      (1)
C                                   (  G -B' )      ( -I  0  )
C
C     where the notation M' denotes the transpose of the matrix M.
C     Optionally, if COMPQ = 'C', an orthogonal basis of the right
C     deflating subspace of aS - bH corresponding to the eigenvalues
C     with strictly negative real part is computed. Optionally, if
C     COMPU = 'C', an orthonormal basis of the companion subspace,
C     range(P_U) [1], which corresponds to the eigenvalues with strictly
C     negative real part, is computed.
C
C     ARGUMENTS
C
C     Mode Parameters
C
C     COMPQ   CHARACTER*1
C             Specifies whether to compute the right deflating subspace
C             corresponding to the eigenvalues of aS - bH with strictly
C             negative real part.
C             = 'N':  do not compute the deflating subspace;
C             = 'C':  compute the deflating subspace and store it in the
C                     leading subarray of Q.
C
C     COMPU   CHARACTER*1
C             Specifies whether to compute the companion subspace
C             corresponding to the eigenvalues of aS - bH with strictly
C             negative real part.
C             = 'N': do not compute the companion subspace;
C             = 'C': compute the companion subspace and store it in the
C                    leading subarray of U.
C
C     ORTH    CHARACTER*1
C             If COMPQ = 'C' and/or COMPU = 'C', specifies the technique
C             for computing the orthogonal basis of the deflating
C             subspace, and/or of the companion subspace, as follows:
C             = 'P':  QR factorization with column pivoting;
C             = 'S':  singular value decomposition.
C             If COMPQ = 'N' and COMPU = 'N', the ORTH value is not
C             used.
C
C     Input/Output Parameters
C
C     N       (input) INTEGER
C             The order of the pencil aS - bH.  N >= 0, even.
C
C     Z       (input/output) DOUBLE PRECISION array, dimension (LDZ, N)
C             On entry, the leading N-by-N part of this array must
C             contain the non-trivial factor Z in the factorization
C             S = J Z' J' Z of the skew-Hamiltonian matrix S.
C             On exit, if COMPQ = 'C' or COMPU = 'C', the leading
C             N-by-N part of this array contains the transformed upper
C                               ~
C             triangular matrix Z11 (see METHOD), after moving the
C             eigenvalues with strictly negative real part to the top
C             of the pencil (3). The strictly lower triangular part is
C             not zeroed.
C             If COMPQ = 'N' and COMPU = 'N', the leading N-by-N part of
C             this array contains the matrix Z obtained by the SLICOT
C             Library routine MB04AD just before the application of the
C             periodic QZ algorithm. The elements of the (2,1) block,
C             i.e., in the rows N/2+1 to N and in the columns 1 to N/2
C             are not set to zero, but are unchanged on exit.
C
C     LDZ     INTEGER
C             The leading dimension of the array Z.  LDZ >= MAX(1, N).
C
C     B       (input) DOUBLE PRECISION array, dimension (LDB, N/2)
C             On entry, the leading N/2-by-N/2 part of this array must
C             contain the matrix B.
C
C     LDB     INTEGER
C             The leading dimension of the array B.  LDB >= MAX(1, N/2).
C
C     FG      (input) DOUBLE PRECISION array, dimension (LDFG, N/2+1)
C             On entry, the leading N/2-by-N/2 lower triangular part of
C             this array must contain the lower triangular part of the
C             symmetric matrix G, and the N/2-by-N/2 upper triangular
C             part of the submatrix in the columns 2 to N/2+1 of this
C             array must contain the upper triangular part of the
C             symmetric matrix F.
C
C     LDFG    INTEGER
C             The leading dimension of the array FG.
C             LDFG >= MAX(1, N/2).
C
C     NEIG    (output) INTEGER
C             If COMPQ = 'C' or COMPU = 'C', the number of eigenvalues
C             in aS - bH with strictly negative real part.
C
C     Q       (output) DOUBLE PRECISION array, dimension (LDQ, 2*N)
C             On exit, if COMPQ = 'C', the leading N-by-NEIG part of
C             this array contains an orthogonal basis of the right
C             deflating subspace corresponding to the eigenvalues of
C             aS - bH with strictly negative real part. The remaining
C             part of this array is used as workspace.
C             If COMPQ = 'N', this array is not referenced.
C
C     LDQ     INTEGER
C             The leading dimension of the array Q.
C             LDQ >= 1,           if COMPQ = 'N';
C             LDQ >= MAX(1, 2*N), if COMPQ = 'C'.
C
C     U       (output) DOUBLE PRECISION array, dimension (LDU, 2*N)
C             On exit, if COMPU = 'C', the leading N-by-NEIG part of
C             this array contains an orthogonal basis of the companion
C             subspace corresponding to the eigenvalues of aS - bH with
C             strictly negative real part. The remaining part of this
C             array is used as workspace.
C             If COMPU = 'N', this array is not referenced.
C
C     LDU     INTEGER
C             The leading dimension of the array U.
C             LDU >= 1,         if COMPU = 'N';
C             LDU >= MAX(1, N), if COMPU = 'C'.
C
C     ALPHAR  (output) DOUBLE PRECISION array, dimension (N/2)
C             The real parts of each scalar alpha defining an eigenvalue
C             of the pencil aS - bH.
C
C     ALPHAI  (output) DOUBLE PRECISION array, dimension (N/2)
C             The imaginary parts of each scalar alpha defining an
C             eigenvalue of the pencil aS - bH.
C             If ALPHAI(j) is zero, then the j-th eigenvalue is real.
C
C     BETA    (output) DOUBLE PRECISION array, dimension (N/2)
C             The scalars beta that define the eigenvalues of the pencil
C             aS - bH.
C             If INFO = 0, the quantities alpha = (ALPHAR(j),ALPHAI(j)),
C             and beta = BETA(j) represent together the j-th eigenvalue
C             of the pencil aS - bH, in the form lambda = alpha/beta.
C             Since lambda may overflow, the ratios should not, in
C             general, be computed. Due to the skew-Hamiltonian/
C             Hamiltonian structure of the pencil, only half of the
C             spectrum is saved in ALPHAR, ALPHAI and BETA.
C             Specifically, only eigenvalues with imaginary parts
C             greater than or equal to zero are stored; their conjugate
C             eigenvalues are not stored. If imaginary parts are zero
C             (i.e., for real eigenvalues), only positive eigenvalues
C             are stored. The remaining eigenvalues have opposite signs.
C             If IWARN = 1, one or more BETA(j) is not representable,
C             and the eigenvalues are returned as described below (see
C             the description of the argument IWARN).
C
C     Workspace
C
C     IWORK   INTEGER array, dimension (LIWORK)
C             On exit, if INFO = -20, IWORK(1) returns the minimum value
C             of LIWORK.
C             On exit, if INFO = 0 and IWARN = 1, then IWORK(1), ...,
C             IWORK(N/2) return the scaling parameters for the
C             eigenvalues of the pencil aS - bH (see IWARN).
C
C     LIWORK  INTEGER
C             The dimension of the array IWORK.
C             LIWORK >= N/2 + 18,    if COMPQ = 'N' and COMPU = 'N';
C             LIWORK >= MAX( N + 48, 5*N/2 + 1 ), otherwise.
C
C     DWORK   DOUBLE PRECISION array, dimension (LDWORK)
C             On exit, if INFO = 0, DWORK(1) returns the optimal value
C             of LDWORK, and DWORK(2) returns the machine base, b.
C             On exit, if INFO = -22, DWORK(1) returns the minimum value
C             of LDWORK.
C
C     LDWORK  INTEGER
C             The dimension of the array DWORK.
C             LDWORK >= c*N**2 + max( N*N + MAX( N/2+252, 432 ),
C                                     MAX(8*N+48,171) ), where
C                       c = a,   if COMPU = 'N',
C                       c = a+1, if COMPU = 'C', and
C                       a = 6,   if COMPQ = 'N',
C                       a = 9,   if COMPQ = 'C'.
C             For good performance LDWORK should be generally larger.
C
C             If LDWORK = -1  a workspace query is assumed; the
C             routine only calculates the optimal size of the DWORK
C             array, returns this value as the first entry of the DWORK
C             array, and no error message is issued by XERBLA.
C
C     BWORK   LOGICAL array, dimension (N/2)
C
C     Warning Indicator
C
C     IWARN   INTEGER
C             = 0: no warning;
C             = 1: the eigenvalues will under- or overflow if evaluated;
C                  therefore, the j-th eigenvalue is represented by
C                  the quantities alpha = (ALPHAR(j),ALPHAI(j)),
C                  beta = BETA(j), and gamma = IWORK(j) in the form
C                  lambda = (alpha/beta) * b**gamma, where b is the
C                  machine base (often 2.0), returned in DWORK(2).
C
C     Error Indicator
C
C     INFO    INTEGER
C             = 0: succesful exit;
C             < 0: if INFO = -i, the i-th argument had an illegal value;
C             = 1: periodic QZ iteration failed in the SLICOT Library
C                  routines MB04AD, MB04CD or MB03BB (QZ iteration did
C                  not converge or computation of the shifts failed);
C             = 2: standard QZ iteration failed in the SLICOT Library
C                  routines MB04CD or MB03CD (called by MB03ID);
C             = 3: a numerically singular matrix was found in the SLICOT
C                  Library routine MB03GD (called by MB03ID);
C             = 4: the singular value decomposition failed in the LAPACK
C                  routine DGESVD (for ORTH = 'S').
C
C     METHOD
C
C     First, the decompositions of S and H are computed via orthogonal
C     matrices Q1 and Q2 and orthogonal symplectic matrices U1 and U2,
C     such that
C
C                                   ( T11  T12 )
C       Q1' T U1 = Q1' J Z' J' U1 = (          ),
C                                   (  0   T22 )
C
C                  ( Z11  Z12 )
C       U2' Z Q2 = (          ),                                     (2)
C                  (  0   Z22 )
C
C                  ( H11  H12 )
C       Q1' H Q2 = (          ),
C                  (  0   H22 )
C
C     where T11, T22', Z11, Z22', H11 are upper triangular and H22' is
C     upper quasi-triangular.
C
C     Then, orthogonal matrices Q3, Q4 and U3 are found, for the
C     matrices
C
C       ~     ( T22'  0  )  ~     ( T11'  0  )  ~   (   0   H11 )
C       Z11 = (          ), Z22 = (          ), H = (           ),
C             (  0   Z11 )        (  0   Z22 )      ( -H22'  0  )
C
C               ~          ~       ~          ~
C     such that Z11 := U3' Z11 Q4, Z22 := U3' Z22 Q3 are upper
C                    ~          ~
C     triangular and H11 := Q3' H Q4 is upper quasi-triangular. The
C     following matrices are computed:
C
C       ~          ( -T12'  0  )        ~          (  0   H12 )
C       Z12 := U3' (           ) Q3 and H12 := Q3' (          ) Q3.
C                  (  0    Z12 )                   ( H12'  0  )
C
C     Then, an orthogonal matrix Q and an orthogonal symplectic matrix U
C     are found such that the eigenvalues with strictly negative real
C     parts of the pencil
C
C             ~    ~          ~    ~           ~    ~
C           ( Z11  Z12 )'   ( Z11  Z12 )     ( H11  H12  )
C       a J (      ~   ) J' (      ~   ) - b (      ~    )           (3)
C           (  0   Z22 )    (  0   Z22 )     (  0  -H11' )
C
C     are moved to the top of this pencil.
C
C     Finally, an orthogonal basis of the right deflating subspace
C     and an orthogonal basis of the companion subspace corresponding to
C     the eigenvalues with strictly negative real part are computed.
C     See also page 11 in [1] for more details.
C
C     REFERENCES
C
C     [1] Benner, P., Byers, R., Losse, P., Mehrmann, V. and Xu, H.
C         Numerical Solution of Real Skew-Hamiltonian/Hamiltonian
C         Eigenproblems.
C         Tech. Rep., Technical University Chemnitz, Germany,
C         Nov. 2007.
C
C     NUMERICAL ASPECTS
C                                                               3
C     The algorithm is numerically backward stable and needs O(N )
C     floating point operations.
C
C     FURTHER COMMENTS
C
C     This routine does not perform any scaling of the matrices. Scaling
C     might sometimes be useful, and it should be done externally.
C
C     CONTRIBUTOR
C
C     V. Sima, Research Institute for Informatics, Bucharest, Romania,
C     Jan. 2011.
C
C     REVISIONS
C
C     V. Sima, Feb. 2011, Aug. 2011, Nov. 2011, Oct. 2012.
C     M. Voigt, Jan. 2012.
C
C     KEYWORDS
C
C     Deflating subspace, embedded pencil, skew-Hamiltonian/Hamiltonian
C     pencil, structured Schur form.
C
C     ******************************************************************
C
C     .. Parameters ..
      DOUBLE PRECISION   ZERO, ONE, TWO
      PARAMETER          ( ZERO = 0.0D+0, ONE = 1.0D+0, TWO = 2.0D+0 )
C
C     .. Scalar Arguments ..
      CHARACTER          COMPQ, COMPU, ORTH
      INTEGER            INFO, IWARN, LDB, LDFG, LDQ, LDU, LDWORK, LDZ,
     $                   LIWORK, N, NEIG
C
C     .. Array Arguments ..
      LOGICAL            BWORK( * )
      INTEGER            IWORK( * )
      DOUBLE PRECISION   ALPHAI( * ), ALPHAR( * ), B( LDB, * ),
     $                   BETA( * ), DWORK( * ), FG( LDFG, * ),
     $                   Q( LDQ, * ), U( LDU, * ), Z( LDZ, * )
C
C     .. Local Scalars ..
      LOGICAL            LCMP, LCMPQ, LCMPU, LQUERY, QR, QRP, SVD
      CHARACTER*14       CMPI, CMQI, CMUI, JOB
      INTEGER            I, IH, IH12, IQ1, IQ2, IQ3, IQ4, IS, IT, IU11,
     $                   IU12, IU21, IU22, IU3, IW, IWRK, IZ12, J, M,
     $                   MINDB, MINDW, MINIW, MM, MP1, N2, NM, NMM, NN,
     $                   NP1, OPTDW
      DOUBLE PRECISION   BASE
C
C     .. Local Arrays ..
      DOUBLE PRECISION   DUM( 5 )
C
C     .. External Functions ..
      LOGICAL            LSAME
      DOUBLE PRECISION   DLAMCH
      EXTERNAL           DLAMCH, LSAME
C
C     .. External Subroutines ..
      EXTERNAL           DAXPY, DCOPY, DGEMM, DGEQP3, DGEQRF, DGESVD,
     $                   DLACPY, DORGQR, DSWAP, MA02ED, MB03ID, MB04AD,
     $                   MB04CD, XERBLA
C
C     .. Intrinsic Functions ..
      INTRINSIC          INT, MAX, MOD, SQRT
C
C     .. Executable Statements ..
C
C     Decode the input arguments.
C
      M  = N/2
      N2 = N*2
      NN = N*N
C
      NEIG  = 0
      LCMPQ = LSAME( COMPQ, 'C' )
      LCMPU = LSAME( COMPU, 'C' )
      LCMP  = LCMPQ .OR. LCMPU
      IF( LCMP ) THEN
         QR    = LSAME( ORTH, 'Q' )
         QRP   = LSAME( ORTH, 'P' )
         SVD   = LSAME( ORTH, 'S' )
         MINIW = MAX( N + 48, 5*M + 1 )
      ELSE
         QR    = .FALSE.
         QRP   = .FALSE.
         SVD   = .FALSE.
         MINIW = M + 18
      END IF
      IF( N.EQ.0 ) THEN
         MINDW = 2
      ELSE
         IF( LCMPQ ) THEN
            I  = 4
            J  = 9
            IS = 8*N
         ELSE
            I  = 2
            J  = 6
            IS = 4*N
         END IF
         IF( LCMPU ) THEN
            I = I + 1
            J = J + 1
         END IF
         MINDB = I*NN
         MINDW = J*NN + MAX( NN + MAX( M + 252, 432 ),
     $                            MAX( IS + 48, 171 ) )
      END IF
      LQUERY = LDWORK.EQ.-1
C
C     Test the input arguments.
C
      INFO = 0
      IF( .NOT.( LSAME( COMPQ, 'N' ) .OR. LCMPQ ) ) THEN
         INFO = -1
      ELSE IF( .NOT.( LSAME( COMPU, 'N' ) .OR. LCMPU ) ) THEN
         INFO = -2
      ELSE IF( LCMP .AND. .NOT. ( QR .OR. QRP .OR. SVD ) ) THEN
         INFO = -3
      ELSE IF( N.LT.0 .OR. MOD( N, 2 ).NE.0 ) THEN
         INFO = -4
      ELSE IF(  LDZ.LT.MAX( 1, N ) ) THEN
         INFO = -6
      ELSE IF(  LDB.LT.MAX( 1, M ) ) THEN
         INFO = -8
      ELSE IF( LDFG.LT.MAX( 1, M ) ) THEN
         INFO = -10
      ELSE IF(  LDQ.LT.1 .OR. ( LCMPQ .AND. LDQ.LT.N2 ) ) THEN
         INFO = -13
      ELSE IF(  LDU.LT.1 .OR. ( LCMPU .AND. LDU.LT.N  ) ) THEN
         INFO = -15
      ELSE IF( LIWORK.LT.MINIW ) THEN
         IWORK( 1 ) = MINIW
         INFO = -20
      ELSE IF( .NOT.LQUERY ) THEN
         IF( LDWORK.LT.MINDW ) THEN
            DWORK( 1 ) = MINDW
            INFO = -22
         END IF
      END IF
C
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'MB03LF', -INFO )
         RETURN
      ELSE IF( N.GT.0 ) THEN
C
C        Compute optimal workspace.
C
         CMPI = 'Initialize'
         IF( LCMPQ ) THEN
            CMQI = CMPI
         ELSE
            CMQI = 'No Computation'
         END IF
C
         IF( LCMPU ) THEN
            CMUI = CMPI
         ELSE
            CMUI = 'No Computation'
         END IF
C
         IF( LCMP ) THEN
            JOB = 'Triangularize'
         ELSE
            JOB = 'Eigenvalues'
         END IF
C
         IF( LQUERY ) THEN
            CALL MB04AD( JOB, CMQI, CMQI, CMUI, CMUI, N, DWORK, N,
     $                   DWORK, N, DWORK, N, DWORK, N, DWORK, M, DWORK,
     $                   M, DWORK, M, DWORK, M, DWORK, N, DWORK, DWORK,
     $                   DWORK, IWORK, LIWORK, DUM, -1, INFO )
C
            IF( LCMP ) THEN
               IW    = MINDB
               MINDB = MINDB + 2*NN
               IF( LCMPQ ) THEN
                  IW    = IW    + NN
                  MINDB = MINDB + NN
               ELSE
                  IW    = 0
               END IF
               CALL MB04CD( CMQI, CMPI, CMPI, N, DWORK, N, DWORK, N,
     $                      DWORK, N, DWORK, N, DWORK, N, DWORK, N,
     $                      IWORK, LIWORK, DUM( 2 ), -1, BWORK, INFO )
               IF( SVD ) THEN
                  CALL DGESVD( 'O', 'N', N, N, Q, LDQ, DWORK, DWORK,
     $                         LDQ, DWORK, 1, DUM( 3 ), -1, INFO )
                  J = N + INT( DUM( 3 ) )
               ELSE
                  I = MAX( LDQ, LDU )
                  IF( QR ) THEN
                     CALL DGEQRF( N, M, Q, I, DWORK, DUM( 3 ), -1,
     $                            INFO )
                     J = M
                  ELSE
                     CALL DGEQP3( N, N, Q, I, IWORK, DWORK, DUM( 3 ),
     $                            -1, INFO )
                     J = N
                  END IF
                  CALL DORGQR( N, J, J, Q, I, DWORK, DUM( 4 ), -1,
     $                         INFO )
                  J = J + IW + MAX( INT( DUM( 3 ) ), INT( DUM( 4 ) ) )
               END IF
               OPTDW = MAX( MINDB + INT( DUM( 2 ) ), J )
            ELSE
               OPTDW = 0
            END IF
            OPTDW = MAX( MINDW, OPTDW, MINDB + INT( DUM( 1 ) ) )
            DWORK( 1 ) = OPTDW
            RETURN
         END IF
      END IF
C
C     Quick return if possible.
C
      IWARN = 0
      IF( N.EQ.0 ) THEN
         IWORK( 1 ) = 1
         DWORK( 1 ) = TWO
         DWORK( 2 ) = DLAMCH( 'Base' )
         RETURN
      END IF
C
C     STEP 1: Apply MB04AD to compute the generalized symplectic
C             URV decomposition.
C
C     Set the pointers for the inputs and outputs of MB04AD.
C
C     Real workspace:     need   w1 + w, where
C                         w1 = 2*N**2, if COMPQ = 'N' and COMPU = 'N';
C                         w1 = 3*N**2, if COMPQ = 'N' and COMPU = 'C';
C                         w1 = 4*N**2, if COMPQ = 'C' and COMPU = 'N';
C                         w1 = 5*N**2, if COMPQ = 'C' and COMPU = 'C';
C                         w  = 3/2*N**2 + MAX(N, 48), if  COMPQ = 'N'
C                                                     and COMPU = 'N';
C                         w  =   3*N**2 + MAX(N, 48), otherwise;
C                         prefer larger.
C     Integer workspace:  need   M + 18.
C
      MM  = M*M
      NM  = N*M
      NMM = NM + M
      NP1 = N  + 1
      MP1 = M  + 1
      IQ1 = 1
      IF( LCMPQ ) THEN
         IQ2  = IQ1 + NN
         IU11 = IQ2 + NN
         IH   = IU11
      ELSE
         IQ2  = 1
         IU11 = 1
         IH   = 1
      END IF
C
      IF( LCMPU ) THEN
         IU12 = IU11 + MM
         IU21 = IU12 + MM
         IU22 = IU21 + MM
         IH   = IU22 + MM
      ELSE
         IU12 = 1
         IU21 = 1
         IU22 = 1
      END IF
C
C     Build the matrix H.
C
      IW = IH
      IS = IH + M + N
      DO 10 J = 1, M
         CALL DCOPY( M, B( 1, J ), 1, DWORK( IW ), 1 )
         IW = IW + M + J - 1
         CALL DCOPY( M-J+1, FG( J, J ),  1, DWORK( IW ), 1 )
         CALL DCOPY( M-J, DWORK( IW+1 ), 1, DWORK( IS ), N )
         IW = IW + MP1 - J
         IS = IS + NP1
   10 CONTINUE
C
      IW = IH + NM
      IS = IW
      DO 30 J = 1, M
         CALL DCOPY( J, FG( 1, J+1 ),  1, DWORK( IW ), 1 )
         CALL DCOPY( J-1, DWORK( IW ), 1, DWORK( IS ), N )
         IW = IW + M
         IS = IS + 1
         DO 20 I = 1, M
            DWORK( IW ) = -B( J, I )
            IW = IW + 1
   20    CONTINUE
   30 CONTINUE
C
      IT   = IH + NN
      IWRK = IT + NN
C
      CALL MB04AD( JOB, CMQI, CMQI, CMUI, CMUI, N, Z, LDZ, DWORK( IH ),
     $             N, DWORK( IQ1 ), N, DWORK( IQ2 ), N, DWORK( IU11 ),
     $             M, DWORK( IU12 ), M, DWORK( IU21 ), M, DWORK( IU22 ),
     $             M, DWORK( IT ), N, ALPHAR, ALPHAI, BETA, IWORK,
     $             LIWORK, DWORK( IWRK ), LDWORK-IWRK+1, INFO )
C
      IF( INFO.EQ.3 ) THEN
         IWARN = 1
      ELSE IF( INFO.GT.0 ) THEN
         INFO = 1
         RETURN
      END IF
      OPTDW = MAX( MINDW, INT( DWORK( IWRK ) ) + IWRK - 1 )
C
      BASE = DWORK( IWRK+1 )
      IF( .NOT.LCMP ) THEN
         DWORK( 1 ) = OPTDW
         DWORK( 2 ) = BASE
         RETURN
      END IF
C                                                    ~    ~        ~
C     STEP 2: Build the needed parts of the matrices Z11, Z22' and H,
C     and compute the transformed matrices and the orthogonal matrices
C     Q3, Q4 and U3. (Q4 might not be required.)
C
C     Real workspace:     need   w1 + w2 + 3*N*N + MAX( M+252, 432 ),
C                                w2 = 2*N**2, if COMPQ = 'N';
C                                w2 = 3*N**2, if COMPQ = 'C';
C                         prefer larger.
C     Integer workspace:  need   N + 48.
C     Logical workspace:  need   M.
C
C     Save Z12, T12, and H12, since they are overwritten by MB04CD.
C
      IW = IT + NM
      IS = IH + NM
      IF( LCMPU ) THEN
         DO 40 J = 1, M
            CALL DCOPY( M, Z( 1, M+J ), 1, U(   1, J ), 1 )
            CALL DCOPY( M, DWORK( IW ), 1, U( MP1, J ), 1 )
            IW = IW + N
   40    CONTINUE
         DO 50 J = 1, M
            CALL DCOPY( M, DWORK( IS ), 1, U( 1, M+J ), 1 )
            IS = IS + N
   50    CONTINUE
      ELSE
         DO 60 J = 1, M
            CALL DCOPY( M, Z( 1, M+J ), 1, Q(   1, J ), 1 )
            CALL DCOPY( M, DWORK( IW ), 1, Q( MP1, J ), 1 )
            IW = IW + N
   60    CONTINUE
         DO 70 J = 1, M
            CALL DCOPY( M, DWORK( IS ), 1, Q( 1, M+J ), 1 )
            IS = IS + N
   70    CONTINUE
      END IF
C
      IU3 = IWRK
      IQ3 = IU3 + NN
      IQ4 = IQ3 + NN
      IF( LCMPQ ) THEN
         IWRK = IQ4 + NN
      ELSE
         IWRK = IQ4
      END IF
C
      IW = IH
      DO 90 J = 1, M
         IS = IW + NM
         CALL DCOPY( M, DWORK( IW ), 1, DWORK( IS ), 1 )
         IW = IW + M
         IS = IH + NMM + J - 1
         DO 80 I = 1, M
            DWORK( IW ) = -DWORK( IS )
            IW = IW + 1
            IS = IS + N
   80    CONTINUE
   90 CONTINUE
C
      IS = IT + NMM
      DO 100 J = 1, M
         CALL DSWAP( M, Z( 1, J ), 1, Z( MP1, M+J ), 1 )
         CALL DSWAP( M, Z( 1, J ), 1, DWORK( IS ), N )
         IS = IS + 1
  100 CONTINUE
C
      CALL MB04CD( CMQI, CMPI, CMPI, N, DWORK( IT ), N, Z, LDZ,
     $             DWORK( IH ), N, DWORK( IQ4 ), N, DWORK( IU3 ), N,
     $             DWORK( IQ3 ), N, IWORK( MP1 ), LIWORK-M,
     $             DWORK( IWRK ), LDWORK-IWRK+1, BWORK, INFO )
      IF( INFO.GT.0 ) THEN
         IF( INFO.GT.2 )
     $      INFO = 2
         RETURN
      END IF
      OPTDW = MAX( OPTDW, INT( DWORK( IWRK ) ) + IWRK - 1 )
C
C                     ~                                    ~
C     STEP 3: Compute Z12 and the upper triangular part of H12,
C     exploiting the structure.
C
C     Real workspace:     need   w1 + w2 + w3, where
C                                w3 = 2*N**2.
C
      IZ12 = IWRK
      IH12 = IZ12 + NN
      IWRK = IH12 + NN
C
C             ~           [ -T12'  0  ]                   [ Qa  Qc ]
C     Compute Z12 = U3' * [           ] * Q3, where Q3 =: [        ].
C                         [   0   Z12 ]                   [ Qb  Qd ]
C
C     Part of the arrays U or Q and DWORK(IH12) are used as workspace.
C
      IF( LCMPU ) THEN
         CALL DGEMM( 'Transpose', 'No Transpose', M, N, M, -ONE,
     $               U( MP1, 1 ), LDU, DWORK( IQ3 ), N, ZERO,
     $               DWORK( IH12 ), N )
         CALL DGEMM( 'No Transpose', 'No Transpose', M, N, M, ONE,
     $               U, LDU, DWORK( IQ3+M ), N, ZERO, DWORK( IH12+M ),
     $               N )
      ELSE
         CALL DGEMM( 'Transpose', 'No Transpose', M, N, M, -ONE,
     $               Q( MP1, 1 ), LDQ, DWORK( IQ3 ), N, ZERO,
     $               DWORK( IH12 ), N )
         CALL DGEMM( 'No Transpose', 'No Transpose', M, N, M, ONE,
     $               Q, LDQ, DWORK( IQ3+M ), N, ZERO, DWORK( IH12+M ),
     $               N )
      END IF
      CALL DGEMM( 'Transpose', 'No Transpose', N, N, N, ONE,
     $            DWORK( IU3 ), N, DWORK( IH12 ), N, ZERO,
     $            DWORK( IZ12 ), N )
C
C             ~           [  0   H12 ]
C     Compute H12 = Q3' * [          ] * Q3.
C                         [ H12'  0  ]
C
C     The (2,1) block of Z is used as workspace.
C
C     Compute Qb'*H12'*Qa + Qa'*H12*Qb.
C
      IF( LCMPU ) THEN
         CALL DGEMM( 'No Transpose', 'No Transpose', M, M, M, ONE,
     $               U( 1, MP1 ), LDU, DWORK( IQ3+M ), N, ZERO,
     $               Z( MP1, 1 ), LDZ )
      ELSE
         CALL DGEMM( 'No Transpose', 'No Transpose', M, M, M, ONE,
     $               Q( 1, MP1 ), LDQ, DWORK( IQ3+M ), N, ZERO,
     $               Z( MP1, 1 ), LDZ )
      END IF
      CALL DGEMM( 'Transpose', 'No Transpose', M, M, M, ONE,
     $            DWORK( IQ3 ), N, Z( MP1, 1 ), LDZ, ZERO,
     $            DWORK( IH12 ), N )
      IS = 0
      DO 110 J = 0, M - 1
         CALL DAXPY( J+1, ONE, DWORK( IH12+J ), N, DWORK( IH12+IS ), 1 )
         IS = IS + N
  110 CONTINUE
C
C     Compute Qb'*H12'*Qc + Qa'*H12*Qd.
C
      CALL DGEMM( 'Transpose', 'No Transpose', M, M, M, ONE,
     $            Z( MP1, 1 ), LDZ, DWORK( IQ3+NM ), N, ZERO,
     $            DWORK( IH12+NM ), N )
      IF( LCMPU ) THEN
         CALL DGEMM( 'No Transpose', 'No Transpose', M, M, M, ONE,
     $              U( 1, MP1 ), LDU, DWORK( IQ3+NMM ), N, ZERO,
     $              Z( MP1, 1 ), LDZ )
      ELSE
         CALL DGEMM( 'No Transpose', 'No Transpose', M, M, M, ONE,
     $              Q( 1, MP1 ), LDQ, DWORK( IQ3+NMM ), N, ZERO,
     $              Z( MP1, 1 ), LDZ )
      END IF
      CALL DGEMM( 'Transpose', 'No Transpose', M, M, M, ONE,
     $            DWORK( IQ3 ), N, Z( MP1, 1 ), LDZ, ONE,
     $            DWORK( IH12+NM ), N )
C
C     Compute Qd'*H12'*Qc + Qc'*H12*Qd.
C
      CALL DGEMM( 'Transpose', 'No Transpose', M, M, M, ONE,
     $            Z( MP1, 1 ), LDZ, DWORK( IQ3+NM ), N, ZERO,
     $            DWORK( IH12+NMM ), N )
      IS = 0
      DO 120 J = 0, M - 1
         CALL DAXPY( J+1, ONE, DWORK( IH12+NMM+J ), N,
     $               DWORK( IH12+NMM+IS ), 1 )
         IS = IS + N
  120 CONTINUE
C
C     STEP 4: Apply MB03ID to reorder the eigenvalues with strictly
C             negative real part to the top.
C
C     Real workspace:     need   w1 + w2 + w3 + w4;
C                                w4 = MAX(4*N+48,171), if COMPQ = 'N';
C                                w4 = MAX(8*N+48,171), if COMPQ = 'C'.
C     Integer workspace:  need   5*N/2 + 1.
C
      CALL MA02ED( 'Upper', N, DWORK( IT ), N )
C
      CALL MB03ID( CMQI, CMUI, N2, Z, LDZ, DWORK( IT ), N,
     $             DWORK( IZ12 ), N, DWORK( IH ), N, DWORK( IH12 ), N,
     $             Q, LDQ, U, LDU, U( 1, NP1 ), LDU, NEIG, IWORK( MP1 ),
     $             LIWORK-M, DWORK( IWRK ), LDWORK-IWRK+1, INFO )
      IF( INFO.GT.0 )
     $   RETURN
C
      IF( QR )
     $   NEIG = NEIG/2
C
      IWRK = IZ12
C
      IF( LCMPQ ) THEN
C
C        STEP 5: Compute the deflating subspace corresponding to the
C                eigenvalues with strictly negative real part.
C
C        Real workspace:     need   w1 + w2 +   N**2, if ORTH = 'QR'.
C                                   w1 + w2 + 2*N**2, otherwise.
C
C        The workspace used before for storing H and T is reused.
C
C        Compute [ J*Q1*J' Q2 ].
C
         CALL DLACPY( 'Full', M, M, DWORK( IQ1+NMM ), N, DWORK( IH ),
     $                N )
         IW = IH  + M
         IS = IQ1 + NM
         DO 140 J = 1, M
            DO 130 I = 1, M
               DWORK( IW ) = -DWORK( IS )
               IW = IW + 1
               IS = IS + 1
  130       CONTINUE
            IW = IW + M
            IS = IS + M
  140    CONTINUE
C
         IW = IH  + NM
         IS = IQ1 + M
         DO 160 J = 1, M
            DO 150 I = 1, M
               DWORK( IW ) = -DWORK( IS )
               IW = IW + 1
               IS = IS + 1
  150       CONTINUE
            IW = IW + M
            IS = IS + M
  160    CONTINUE
         CALL DLACPY( 'Full', M, M, DWORK( IQ1 ), N, DWORK( IH+NMM ),
     $                N )
C
         CALL DLACPY( 'Full', N, N, DWORK( IQ2 ), N, DWORK( IT ), N )
C
C        Compute the first NEIG columns of P*[ Q4  0; 0 Q3 ]*Q.
C
         CALL DGEMM( 'No Transpose', 'No Transpose', M, NEIG, N, ONE,
     $               DWORK( IQ4 ), N, Q, LDQ, ZERO, DWORK( IWRK ), N2 )
         CALL DGEMM( 'No Transpose', 'No Transpose', M, NEIG, N, ONE,
     $               DWORK( IQ3 ), N, Q( NP1, 1 ), LDQ, ZERO,
     $               DWORK( IWRK+M ), N2 )
         CALL DGEMM( 'No Transpose', 'No Transpose', M, NEIG, N, ONE,
     $               DWORK( IQ4+M ), N, Q, LDQ, ZERO, DWORK( IWRK+N ),
     $               N2 )
         CALL DGEMM( 'No Transpose', 'No Transpose', M, NEIG, N, ONE,
     $               DWORK( IQ3+M ), N, Q( NP1, 1 ), LDQ, ZERO,
     $               DWORK( IWRK+N+M ), N2 )
C
C        Compute the deflating subspace.
C
         CALL DGEMM( 'No Transpose', 'No Transpose', N, NEIG, N2,
     $               SQRT( TWO )/TWO, DWORK( IH ), N, DWORK( IWRK ), N2,
     $               ZERO, Q, LDQ )
C
C        Orthogonalize the basis given in Q(1:n,1:neig).
C
         IF( LCMPU ) THEN
            IWRK = IQ3
         ELSE
            IWRK = NEIG + 1
         END IF
         IF( SVD ) THEN
C
C           Real workspace:     need   w5 + N + MAX(1,5*N);
C                                      w5 = 0,        if COMPU = 'N';
C                                      w5 = w1 + N*N, if COMPU = 'C'.
C                               prefer larger.
C
            CALL DGESVD( 'Overwrite', 'No V', N, NEIG, Q, LDQ, DWORK,
     $                   DWORK, 1,  DWORK, 1, DWORK( IWRK ),
     $                   LDWORK-IWRK+1, INFO )
            IF( INFO.GT.0 ) THEN
               INFO = 4
               RETURN
            END IF
            OPTDW = MAX( OPTDW, INT( DWORK( IWRK ) ) + IWRK - 1 )
            IF( .NOT.LCMPU )
     $         NEIG = NEIG/2
C
         ELSE
            IF( QR ) THEN
C
C              Real workspace:     need   w5 + N;
C                                  prefer w5 + M + M*NB, where NB is the
C                                                  optimal blocksize.
C
               CALL DGEQRF( N, NEIG, Q, LDQ, DWORK, DWORK( IWRK ),
     $                      LDWORK-IWRK+1, INFO )
            ELSE
C
C              Real workspace:     need   w5 + 4*N + 1;
C                                  prefer w5 + 3*N + (N+1)*NB.
C
               DO 170 J = MP1, M + NEIG
                  IWORK( J ) = 0
  170          CONTINUE
               CALL DGEQP3( N, NEIG, Q, LDQ, IWORK( MP1 ), DWORK,
     $                      DWORK( IWRK ), LDWORK-IWRK+1, INFO )
            END IF
            OPTDW = MAX( OPTDW, INT( DWORK( IWRK ) ) + IWRK - 1 )
C
C           Real workspace:     need   w5 + 2*NEIG;
C                               prefer w5 + NEIG + NEIG*NB.
C
            CALL DORGQR( N, NEIG, NEIG, Q, LDQ, DWORK, DWORK( IWRK ),
     $                   LDWORK-IWRK+1, INFO )
            OPTDW = MAX( OPTDW, INT( DWORK( IWRK ) ) + IWRK - 1 )
            IF( QRP .AND. .NOT.LCMPU )
     $         NEIG = NEIG/2
         END IF
C
      END IF
C
      IF( LCMPU ) THEN
C
C        STEP 6: Compute the companion subspace corresponding to the
C                eigenvalues with strictly negative real part.
C
C        Real workspace:     need   w1 + w2 +   N**2, if ORTH = 'QR'.
C                                   w1 + w2 + 2*N**2, otherwise.
C
C        The workspace used before for storing H and T is reused.
C
C        Set [ U1 U2 ].
C
         CALL DLACPY( 'Full', M, N, DWORK( IU11 ), M, DWORK( IH ), N )
         IW = IH + M
         IS = IU12
         DO 190 J = 1, M
            DO 180 I = 1, M
               DWORK( IW ) = -DWORK( IS )
               IW = IW + 1
               IS = IS + 1
  180       CONTINUE
            IW = IW + M
  190    CONTINUE
         CALL DLACPY( 'Full', M, M, DWORK( IU11 ), M, DWORK( IH+NMM ),
     $                N )
         CALL DLACPY( 'Full', M, N, DWORK( IU21 ), M, DWORK( IT ), N )
         IW = IT + M
         IS = IU22
         DO 210 J = 1, M
            DO 200 I = 1, M
               DWORK( IW ) = -DWORK( IS )
               IW = IW + 1
               IS = IS + 1
  200       CONTINUE
            IW = IW + M
  210    CONTINUE
         CALL DLACPY( 'Full', M, M, DWORK( IU21 ), M, DWORK( IT+NMM ),
     $                N )
C
C        Compute the first NEIG columns of P*[ U3  0; 0 U3 ]*U.
C
         CALL DGEMM( 'No Transpose', 'No Transpose', M, NEIG, N, ONE,
     $               DWORK( IU3 ), N, U, LDU, ZERO, DWORK( IWRK ), N2 )
         CALL DGEMM( 'No Transpose', 'No Transpose', M, NEIG, N, -ONE,
     $               DWORK( IU3 ), N, U( 1, NP1 ), LDU, ZERO,
     $               DWORK( IWRK+M ), N2 )
         CALL DGEMM( 'No Transpose', 'No Transpose', M, NEIG, N, ONE,
     $               DWORK( IU3+M ), N, U, LDU, ZERO, DWORK( IWRK+N ),
     $               N2 )
         CALL DGEMM( 'No Transpose', 'No Transpose', M, NEIG, N, -ONE,
     $               DWORK( IU3+M ), N, U( 1, NP1 ), LDU, ZERO,
     $               DWORK( IWRK+N+M ), N2 )
C
C        Compute the companion subspace.
C
         CALL DGEMM( 'No Transpose', 'No Transpose', N, NEIG, N2,
     $               SQRT( TWO )/TWO, DWORK( IH ), N, DWORK( IWRK ), N2,
     $               ZERO, U, LDU )
C
C        Orthogonalize the basis given in U(1:n,1:neig).
C
         IWRK = NEIG + 1
         IF( SVD ) THEN
C
C           Real workspace:     need   N + MAX(1,5*N);
C                               prefer larger.
C
            CALL DGESVD( 'Overwrite', 'No V', N, NEIG, U, LDU, DWORK,
     $                   DWORK, 1,  DWORK, 1, DWORK( IWRK ),
     $                   LDWORK-IWRK+1, INFO )
            IF( INFO.GT.0 ) THEN
               INFO = 4
               RETURN
            END IF
            OPTDW = MAX( OPTDW, INT( DWORK( IWRK ) ) + IWRK - 1 )
            NEIG = NEIG/2
C
         ELSE
            IF( QR ) THEN
C
C              Real workspace:     need   N;
C                                  prefer M + M*NB, where NB is the
C                                                   optimal blocksize.
C
               CALL DGEQRF( N, NEIG, U, LDU, DWORK, DWORK( IWRK ),
     $                      LDWORK-IWRK+1, INFO )
            ELSE
C
C              Real workspace:     need   4*N + 1;
C                                  prefer 3*N + (N+1)*NB.
C
               DO 220 J = MP1, M + NEIG
                  IWORK( J ) = 0
  220          CONTINUE
               CALL DGEQP3( N, NEIG, U, LDU, IWORK( MP1 ), DWORK,
     $                      DWORK( IWRK ), LDWORK-IWRK+1, INFO )
            END IF
            OPTDW = MAX( OPTDW, INT( DWORK( IWRK ) ) + IWRK - 1 )
C
C           Real workspace:     need   2*NEIG;
C                               prefer NEIG + NEIG*NB.
C
            CALL DORGQR( N, NEIG, NEIG, U, LDU, DWORK, DWORK( IWRK ),
     $                   LDWORK-IWRK+1, INFO )
            OPTDW = MAX( OPTDW, INT( DWORK( IWRK ) ) + IWRK - 1 )
            IF( QRP )
     $         NEIG = NEIG/2
         END IF
C
      END IF
C
      DWORK( 1 ) = OPTDW
      DWORK( 2 ) = BASE
      RETURN
C *** Last line of MB03LF ***
      END


c=======================================================================
c
c     subroutine  NLSBX
c
c     Non-Linear Least-Square Solver (with lower/upper bounds)
c     Expert Version
c
c-----------------------------------------------------------------------
      SUBROUTINE nlsbx ( simext, mxopt, xini, dxmin, df1, epsabs,
     &                   mode, maxitr, maxsim, binf, bsup,
     &                   nfct, obs, spadim, grid,
     &                   liudat, iudata, ldudat, dudata, iwork, dwork,
     &                   xopt, fxopt, gxopt, totitr, totsim, info )
c-----------------------------------------------------------------------
c     INPUT
c            simect : entry point of an external subroutine
c                     provided by the user
c            mxopt  : number of parameters to find               integer
c            xini   : initial solution  vector(mxopt)             double
c            dxmin  : precision on the variables    vector(mxopt) double
c            df1    : estimation of the first decrease of the criter
c                     used if mode=1 computed if =<0
c                     must be nearly abs( f(xvar)-f(xopt) ) )     double
c            epsabs : precision stop test                         double
c            mode   : type of initialization                     integer
c                     = 1 : nothing to initialize
c            maxitr : maximum of iterations authorized           integer
c            maxsim : maximum of calls of simul authorized       integer
c                     3*maxitr if largely good
c            binf   : inferior bounds on parameters vector(mxopt) double
c            bsup   : superior bounds on parameters vector(mxopt) double
c            nfct   : number of functions (observations)         integer
c            obs    : observations  vector(nfct)                  double
c            spadim : space dimension                            integer
c            grid   : grid     vector(nfct*spadim)                double
c            liudat : size of integer user data for SIMEXT       integer
c            iudata : integer user data for SIMEXT               integer
c                     vector(liudat)
c            ldudat : size of double prec. user data for SIMEXT  integer
c            dudata : double precision user data for SIMEXT       double
c                     vector(ldudat)
c
c     WORKSPACE
c            iwork  : workspace                                  integer
c                     vector( 2*mxopt + 6 + liudat )
c            dwork  : double precision  workspace                 double
c                     vector( mxopt*(mxopt+11)/2
c                           + nfct*(mxopt+3+spadim) + ldudat )
c
c     OUTPUT
c            xopt   : optimal point (mxopt)                       double
c            fxopt  : function value at optimal point             double
c            gxopt  : gradient value at optimal point (mxopt)     double
c            totitr : total of iterations                        integer
c            totsim : total of calls of simul                    integer
c            info   : = 0 successful exit                        integer
c
c     CALL
c        YV       : copy a vector in a vector
c        YVI      : copy a vector of integers in a vector of integers
c        BFGSBOX  : quasi-Newton optimizer with BFGS method
c                   ( short call version )
c        SIMNLS   : subroutine used by BFGS to compute the value of the
c                   function and its gradient
c
c-----------------------------------------------------------------------
c
c     SIMEXT SIGNATURE
c
c     INPUT
c            nfct   : number of functions                        integer
c            mxopt  : size of the vector solution                integer
c            xopt   : solution  vector(mxopt)                     double
c            spadim : space dimension                            integer
c            grid   : grid     vector(nfct*spadim)                double
c            liudat : size of integers data provided by the user integer
c            iusdat : integers user data provided by the user
c                     vector(liudat)                             integer
c            ldudat : size of doubles data provided by the user  integer
c            dusdat : doubles user data provided by the user
c                     vector(ldudat)                              double
c
c     OUTPUT
c            funct  : functions   vector(nfct)                    double
c            jacob  : jacobian    matrix(nfct*mx)                 double
c            info   : = 0 successful exit                        integer
c
c-----------------------------------------------------------------------
c
      IMPLICIT NONE
c
c     external functions
      EXTERNAL simext, simnls, YV, YVI, bfgsbox
c
c     arguments i/o
      INTEGER mxopt, nfct, spadim, liudat, ldudat, info, mode, maxitr,
     &        maxsim, totitr, totsim
      INTEGER iudata(*)
      DOUBLE PRECISION df1, epsabs, fxopt
      DOUBLE PRECISION binf(*), bsup(*), obs(*), xini(*), grid(*),
     &                 dudata(*), dxmin(*), xopt(*), gxopt(*)
c
c     workspaces
      INTEGER iwork(*)
      DOUBLE PRECISION dwork(*)
c
c     local variables
      INTEGER piwbf, pnfct, pspad, pliu, pldu, pinfo, piwu,
     &        pgrad, pdwbf, pwsnls, pwsext, pobs, pgrid, pdwu
c
c-----------------------------------------------------------------------
c     initializations
      info = 0
      fxopt = 0
      CALL YV ( mxopt, xini, xopt )
      CALL YV ( mxopt, xini, gxopt )
c
c     pointers for integer work space  : iwork
c     ----------------------------------------
      piwbf  = 1
c     piwbf  : pointer for BFGSBOX who needs (2*mxopt + 1)
c              so (2*mxopt + 1) more
      pnfct  = piwbf + (2*mxopt + 1)
c     pnfct  : pointer for the number of functions, ( communication
c              with SIMEXT, user simulator )
      pspad  = pnfct + 1
c     pspad  : pointer for the space dimension ( communication
c              with SIMEXT, user simulator )
      pliu   = pspad + 1
c     pliu   : pointer for size of integers data provided by the user,
c              ( communication with SIMEXT, user simulator )
      pldu   = pliu + 1
c     pldu   : pointer for size of doubles data provided by the user,
c              ( communication with SIMEXT, user simulator )
      pinfo  = pldu + 1
c     pinfo  : pointer for info (errors of SIMNLS)
      piwu   = pinfo + 1
c     piwu   : pointer for user SIMEXT workspace, so (liudat) more
c
c     Total size of iwork array = (2*mxopt + 6) + liudat
c
c     pointers for double precision work space  : dwork
c     -------------------------------------------------
      pgrad = 1
c     pgrad : pointer for gradient at solution, so mxopt more
c
      pdwbf = pgrad + mxopt
c     pdwbf : pointer for BFGSBOX who needs ( mxopt*(mxopt+9)/2 )
c             so ( mxopt*(mxopt+9)/2 ) more
      pwsnls = pdwbf +  mxopt*(mxopt+9)/2
c     pwsnls: pointer for workspace of SIMNLS, so (nfct*(mxopt+2)) more
      pwsext = pwsnls + nfct*(mxopt+2)
c     pwsext: pointer for workspace ( communication with SIMNLS
c             and SIMEXT, BFGSBOX simulator ),
c             so ( nfct*(spadim+1) + ldudat ) more
      pobs = pwsext
c     pobs  : pointer for the vector of observations ( communication
c             with SIMNLS, BFGSBOX simulator ), so nfct  more
c
      pgrid = pobs +  nfct
c     pgrid : pointer for grid matrix ( communication with SIMEXT,
c             user simulator ), so (nfct*spadim) more
      pdwu = pgrid + (nfct*spadim)
c     pdwu   : pointer for user SIMEXT workspace, so (ldudat) more
c
c     Total size of dwork array = mxopt + mxopt*(mxopt+9)/2
c                                 + ( nfct*(mxopt+2) )
c                                 + ( nfct*(spadim+1) ) + ldudat
c          = mxopt*(mxopt+11)/2 + nfct*(mxopt+3+spadim) + ldudat
c
c-----------------------------------------------------------------------
c
c     communication with SIMNLS (BFGSBOXS simulator)
c                    and SIMEXT (user simulator)
      iwork(pnfct) = nfct
      iwork(pspad) = spadim
      iwork(pliu) = liudat
      iwork(pldu) = ldudat
      iwork(pinfo)= 0
      CALL YV ( nfct, obs, dwork(pobs) )
      CALL YV ( nfct*spadim, grid, dwork(pgrid) )
      CALL YV ( ldudat, dudata, dwork(pdwu) )
      CALL YVI ( liudat, iudata, iwork(piwu) )
c
c     optimization BFGS-box
      CALL bfgsbox ( simnls, simext, mxopt, xopt,
     &               dxmin, df1, epsabs, mode, maxitr, maxsim,
     &               binf, bsup,
     &               iwork(piwbf), dwork(pdwbf),
     &               xopt, fxopt, gxopt, totitr, totsim, info )
c
c     error code
      IF (iwork(pinfo) .NE. 0) THEN
         info = iwork(pinfo)
         RETURN
      ENDIF
c
      RETURN
      END


module flink_list_m

	use datatypes,		only: particles
	use globvar,		only: ntotal_loc,nhalo_loc,nvirt_loc,parts,scale_k,niac,pairs,maxinter
	use globvar_para,	only: procid,numprocs
	use param,			only: dim,hsml,f

	use error_msg_m,	only: error_msg
	use kernel_m,		only: kernel

	type particleincellarray
		type(particles),pointer:: p
	end type particleincellarray

	public:: flink_list
	private:: particleincellarray,check_if_interact

contains

	!==============================================================================================================================
	subroutine flink_list( )
	! subroutine to search for particle interactions using cell-linked list.

		implicit none
		integer,parameter:: maxpcell=12
		integer:: ngridx(dim),i,j,k,d,icell,jcell,xi,yi
		real(f):: maxgridx(dim),mingridx(dim),dcell
		integer,allocatable:: pincell(:,:)
		type(particleincellarray),allocatable:: cells(:,:,:)
		integer,parameter:: sweep(2,4) = reshape((/ 1,-1,&
													1, 0,&
													0, 1,&
													1, 1/),(/2,4/))

		!Determining bounding box extents
		mingridx(:) = parts(1)%x(:)
		maxgridx(:) = parts(1)%x(:)
		do i = 2,ntotal_loc+nhalo_loc+nvirt_loc
			do d = 1,dim
				if (parts(i)%x(d)<mingridx(d)) mingridx(d) = parts(i)%x(d)
				if (parts(i)%x(d)>maxgridx(d)) maxgridx(d) = parts(i)%x(d)
			end do
		end do

		!Determining number of grid cells in each direction
		dcell = scale_k*hsml
		maxgridx(:) = maxgridx(:) + 2d0*dcell
		mingridx(:) = mingridx(:) - 2d0*dcell
		ngridx(:) = int((maxgridx(:) - mingridx(:))/dcell) + 1
		maxgridx(:) = mingridx(:) + ngridx(:)*dcell

		allocate( pincell(ngridx(1),ngridx(2)),&
					cells(maxpcell,ngridx(1),ngridx(2)) )

		!Mapping particles to grid cells
		pincell(:,:) = 0

		do i=1,ntotal_loc+nhalo_loc+nvirt_loc
			icell = int((parts(i)%x(1) - mingridx(1))/dcell) + 1
			jcell = int((parts(i)%x(2) - mingridx(2))/dcell) + 1
			pincell(icell,jcell) = pincell(icell,jcell) + 1
			cells(pincell(icell,jcell),icell,jcell)%p => parts(i)
		enddo

		! Searching for interactions. Algorithm loops through every cell with indices icell,jcell.
		! First checks particles within cell are interacting, then checks particles interacting with
		! particles in adjacent cells.
		niac = 0
		do icell = 2,ngridx(1)-2
			do jcell = 2,ngridx(2)-2

				! finding pairs within cell icell,jcell
				do i = 1,pincell(icell,jcell)-1
					do j = i+1,pincell(icell,jcell)
						call check_if_interact(cells(i,icell,jcell)%p,cells(j,icell,jcell)%p)
					end do
				end do

				! finding pairs between particles in cell icell,jcell and particles in cell xi,yi
				do k = 1,4
					xi = icell + sweep(1,k)
					yi = jcell + sweep(2,k)
					do i = 1,pincell(icell,jcell)
						do j = 1,pincell(xi,yi)
							call check_if_interact(cells(i,icell,jcell)%p,cells(j,xi,yi)%p)
						end do
					end do
				end do

			end do
		end do

	end subroutine flink_list

	!==============================================================================================================================
	subroutine check_if_interact(p_i,p_j)
		! subroutine to chekc if two particles are interacting and consequently adding to pair list

		implicit none
		type(particles),intent(in),target:: p_i,p_j
		real(f):: dxiac(dim),r

		! only consider interactions when real-real are involved
		if ( p_i%itype.eq.1 .or. p_j%itype.eq.1 ) then
			dxiac(:) = p_i%x(:) - p_j%x(:)
			r = SQRT(SUM(dxiac*dxiac))
			if (r < hsml*scale_k) then
				niac = niac + 1
				if (niac < maxinter) then
					pairs(niac)%i => p_i
					pairs(niac)%j => p_j
					call kernel(r,dxiac,hsml,pairs(niac)%w,pairs(niac)%dwdx(:))
				else
					call error_msg(8,p_i%indloc)
				end if
			end if
		end if

	end subroutine check_if_interact

end module flink_list_m


      SUBROUTINE SANORM (*,A)
      DIMENSION  A(3)
C
C     VECTOR NORMALIZATION AND VECTOR LENGTH
C
      XL=A(1)*A(1) + A(2)*A(2) + A(3)*A(3)
      IF (XL .LE. 0.0) RETURN 1
      XL   = SQRT(XL)
      A(1) = A(1)/XL
      A(2) = A(2)/XL
      A(3) = A(3)/XL
      RETURN
      END


      IMPLICIT DOUBLE PRECISION(A-H,O-Z)
      PARAMETER (NX=3200)
C
C.....Declare arrays.
      DIMENSION XA(NX),AR(NX,NX),WCH(NX),ZR(NX,NX)
      DIMENSION FV1(NX),FV2(NX)
      DATA PI/3.141592653589793D0/
      DATA NPRIN/0/
      DATA NWRIT/20/
      INTEGER :: L
      LOGICAL :: MORLJ

      ZMA   = 242271.817297
      ZMB   = 242271.817297
      L     = 1000
      RMAX  = 100.0D0
      RMIN  = 5.0D0
      C6    = 6870.1
      C8    = 0.0D0
      C10   = 0.0D0
      C12   = 709538221.031
      XMIN  = 6.5
      XMAX  = 25.0
      DX    = (XMAX-XMIN)/DBLE(NX)
      ZMU=ZMA*ZMB/(ZMA+ZMB)
      DHART = C6**2/(4.0*C12)
      R0    = (C12/C6)**(1.0/6.0)*2.0**(1.0/6.0)
      BETA  = 6.0/R0

      DO I=1,NX
       X = XMIN + (I-1)*DX
       CALL VSUB_M(X,V1,R0,BETA,DHART,L,ZMU)
       CALL VSUB_LJ(X,V2,R0,C6,C8,C10,C12,L,ZMU)
       CALL VSUB_MLR3(X,V3,L,ZMU)
       WRITE(*,*)X,V2,V3
      ENDDO
      END
C
      SUBROUTINE VSUB_MLR3(X,V,L,MU)
C     **************************************************************
C.....SUBROUTINE TO COMPUTE POTENTIAL ENERGY AT SEPERATION X (IN
C.....ATOMIC UNITS). THE VALUE OF THE POTENTIAL IS RETURNED IN V,
C.....AGAIN IN ATOMIC UNITS.THE POTENTIAL ENERGY IS ASSUMED TO
C.....TO BE A MLR3 POTNETIAL AS DEFINDED IN JCP 132 094105 2010
C.....FOR GROUND STATE CEASIUM DIMERS
      IMPLICIT DOUBLE PRECISION(A-H,O-Z)
      DIMENSION PHI(19)

      BRtoAng = 0.52917721080000002
      R = X*BrtoAng
!     scipy.constants.physical_constants["Bohr radius"][0]*1.0E10
      CM_1toHar = 4.5563352527603877e-06
!     De = De*100.0/(scipy.constants.Rydberg*2.0)

      PHI(1)  = -2.48715027
      PHI(2)  = -0.7250527
      PHI(3)  = -1.988236
      PHI(4)  = -0.671755
      PHI(5)  = -1.202501
      PHI(6)  = -0.020151
      PHI(7)  = -0.5414
      PHI(8)  =  0.2298
      PHI(9)  =  1.3964
      PHI(10) =  0.687
      PHI(11) = -8.655
      PHI(12) =  1.73
      PHI(13) =  32.2
      PHI(14) = -2.66
      PHI(15) = -61.0
      PHI(16) =  6.1
      PHI(17) =  65.6
      PHI(18) = -2.0
      PHI(19) = -28.0

      Re      = 4.6479723
      De      = 3649.847
      C6      = 3.315E7
      C8      = 1.29962E9
      C12     = 5.136E10
      MCS     = 132.905451933
      RMIN    = 3.1
      RMAX    = 20.1
      DR      = 0.001
      RREF    = 5.47
      P       = 5.0
      M       = 6.0
      Q       = 4.0
      A       = 1.79

      ULRR    = C6/R**6 + C8/R**8 + C12/R**12
      ULRRe   = C6/Re**6 + C8/Re**8 + C12/Re**12
      YM      = (R**M-RREF**M)/(R**M+RREF**M)
      YQ      = (R**Q-RREF**Q)/(R**Q+RREF**Q)

      PHIMLR3 = 0.0
      DO I=1,19
       PHIMLR3 = PHIMLR3 + PHI(I)*YQ!**I
      ENDDO
      PHIMLR3 = PHIMLR3*(1.0-YM)
      PHIINF  = 0.5*((2.0*De*Re**6)/(C6))
      PHIMLR3 = PHIMLR3 + YM*PHIINF
      YPA     = (X**P-Re**P)/(X**P+A*Re**P)
      V       = De*(1.0-(ULRR/ULRRe)*EXP(-PHIMLR3*YPA))**2 - De
      V = V*4.5563352527603877e-06
      V = V + DBLE(L)*(DBLE(L)+1.0)/(2.0*MU*X**2)
      RETURN
      END

      SUBROUTINE VSUB_M(X,V,R0,BETA,DHART,L,MU)
C     **************************************************************
C.....SUBROUTINE TO COMPUTE POTENTIAL ENERGY AT SEPERATION X (IN
C.....ATOMIC UNITS). THE VALUE OF THE POTENTIAL IS RETURNED IN V,
C.....AGAIN IN ATOMIC UNITS.THE POTENTIAL ENERGY IS ASSUMED TO
C..... BE A MORSE CURVE:V(X) = DHART * (EXP(-BETA*(X-R0)) - 1)**2
C..... THE FOLLOWING DATA CONSTANTS REPRESENT :
C.....    DHART : THE DISSOCIATION ENERGY FOR THE GIVEN MOLECULE
C.....    BETA  : THE EXPONENTIAL PARAMETER
C.....    R0    : THE BOND LENGTH AT EQUILIBRIUM FOR GIVEN MOLECULE
C     **************************************************************
      IMPLICIT DOUBLE PRECISION(A-H,O-Z)
      DOUBLE PRECISION :: MU
      INTEGER :: L
C      DATA DHART/0.2250073497D0/
C      DATA BETA/1.1741D0/
C      DATA R0/1.7329D0/
C.....Set morse curve parameters.
      ARG=X-R0
      ARG=-BETA*ARG
      EF=DEXP(ARG)
      EF=1.0D0-EF
      EF=EF*EF
      V=DHART*EF-DHART
      V = V + DBLE(L)*(DBLE(L)+1.0)/(2.0*MU*X**2)
      RETURN
      END
CC
      SUBROUTINE VSUB_LJ(X,V,R0,C6,C8,C10,C12,L,MU)
C     **************************************************************
C.....SUBROUTINE TO COMPUTE POTENTIAL ENERGY AT SEPERATION X (IN
C.....ATOMIC UNITS). THE VALUE OF THE POTENTIAL IS RETURNED IN V,
C.....AGAIN IN ATOMIC UNITS.THE POTENTIAL ENERGY IS ASSUMED TO
C..... BE A MORSE CURVE:V(X) = DHART * (EXP(-BETA*(X-R0)) - 1)**2
C..... THE FOLLOWING DATA CONSTANTS REPRESENT :
C.....    DHART : THE DISSOCIATION ENERGY FOR THE GIVEN MOLECULE
C.....    BETA  : THE EXPONENTIAL PARAMETER
C.....    R0    : THE BOND LENGTH AT EQUILIBRIUM FOR GIVEN MOLECULE
C     **************************************************************
      IMPLICIT DOUBLE PRECISION(A-H,O-Z)
      DOUBLE PRECISION :: MU
      INTEGER :: L
C      DATA DHART/0.2250073497D0/
C      DATA BETA/1.1741D0/
C      DATA R0/1.7329D0/
C.....Set morse curve parameters.
      V =C12/X**12-C10/X**6-C8/X**6-C6/X**6
      V = V + DBLE(L)*(DBLE(L)+1.0)/(2.0*MU*X**2)
      RETURN
      END
C
C


      subroutine runCY_0000i1i2(k,l,i1,i2,Xtwiddle,Gtwiddle,Shat6zz,N0)
      implicit none
C---  Expression for Eq. 5.58c
C---  Calculates C0000i1i2, requires C0000li1,C0000li2
C---  Small terms of order Xtwiddle(0,k)*C00iii,Xtwiddle(0,0)*C00iiii
C---  Denominator Gtwiddle(k,l)
      include 'pvCnames.f'
      include 'pvCv.f'
      include 'Carraydef.f'
      include 'Carrays.f'
      integer ep,N0,k,l,i1,i2,np
      parameter(np=2)
      double precision Xtwiddle(0:np,0:np),Gtwiddle(np,np)
      double complex Shat6zz(np,z3max,-2:0)

      if (  (i1 .eq. l) .or. (i2 .eq. l)
     . .or. (i1 .eq. 0) .or. (i2 .eq. 0)) then
      return
      endif

      do ep=-2,0

      Cv(czzzzii(z2(i1,i2))+N0,ep)=
     .(-2d0*Gtwiddle(k,i1)*Cv(czzzzii(z2(l,i2))+N0,ep)
     . -2d0*Gtwiddle(k,i2)*Cv(czzzzii(z2(l,i1))+N0,ep)
     . +Gtwiddle(k,1)*Shat6zz(1,z3(l,i1,i2),ep)
     . +Gtwiddle(k,2)*Shat6zz(2,z3(l,i1,i2),ep)
     . +Xtwiddle(0,k)*Cv(czziii(z3(l,i1,i2))+N0,ep)
     . -Xtwiddle(0,0)*Cv(czziiii(z4(k,l,i1,i2))+N0,ep)
     . )/(2d0*Gtwiddle(k,l))
      enddo

      return
      end