include 'specialFunctions.f95'

!This function traces to a Wolter I primary mirror
!Defined by Van Speybroeck prescription
!For WFS test, use flat to get rays close so they find correct intersection
!Surface should be placed at common focus with z+ pointing toward mirrors
subroutine wolterprimary(x,y,z,l,m,n,ux,uy,uz,num,r0,z0,psi)
  !Declarations
  implicit none
  integer, intent(in) :: num
  real*8 , intent(inout) :: x(num),y(num),z(num),l(num),m(num),n(num),ux(num),uy(num),uz(num)
  real*8, intent(in) :: r0,z0,psi
  real*8 :: alpha,thetah,thetap,p,d,e
  real*8 :: F,Fx,Fy,Fz,Fp,delt,dum
  integer :: i

  !Compute Van Speybroeck parameters
  alpha = .25*atan(r0/z0)
  thetah = 2*(1+2*psi)/(1+psi) * alpha
  thetap = 2*psi/(1+psi) * alpha
  p = z0*tan(4*alpha)*tan(thetap)
  d = z0*tan(4*alpha)*tan(4*alpha-thetah)
  e = cos(4*alpha)*(1+tan(4*alpha)*tan(thetah))

  Fz = 2*p
  !Loop through rays and trace to mirror
  !$omp parallel do private(delt,F,Fx,Fy,Fp)
  do i=1,num
    delt = 100.
    do while(abs(delt)>1.e-8)
      F = 2*p*z(i) + p**2 + 4*e**2*p*d/(e**2-1) - x(i)**2 - y(i)**2
      Fx = -2.*x(i)
      Fy = -2.*y(i)
      Fp = Fx*l(i) + Fy*m(i) + Fz*n(i)
      delt = -F/Fp
      x(i) = x(i) + l(i)*delt
      y(i) = y(i) + m(i)*delt
      z(i) = z(i) + n(i)*delt
      !print *, x(i),y(i),z(i)
      !print *, F
      !print * ,delt
      !read *, dum
    end do
    Fp = sqrt(Fx*Fx+Fy*Fy+Fz*Fz)
    ux(i) = Fx/Fp
    uy(i) = Fy/Fp
    uz(i) = Fz/Fp
    !print *, x(i),y(i),z(i)
    !print *, ux(i),uy(i),uz(i)
    !read *, dum
  end do
  !$omp end parallel do

end subroutine wolterprimary

!This function traces to a Wolter I primary mirror
!Defined by Van Speybroeck prescription
!For WFS test, use flat to get rays close so they find correct intersection
!Surface should be placed at common focus with z+ pointing toward mirrors
subroutine wolterprimaryopd(opd,x,y,z,l,m,n,ux,uy,uz,num,r0,z0,psi,nr)
  !Declarations
  implicit none
  integer, intent(in) :: num
  real*8 , intent(inout) :: opd(num),x(num),y(num),z(num),l(num),m(num),n(num),ux(num),uy(num),uz(num)
  real*8, intent(in) :: r0,z0,psi,nr
  real*8 :: alpha,thetah,thetap,p,d,e
  real*8 :: F,Fx,Fy,Fz,Fp,delt,dum
  integer :: i

  !Compute Van Speybroeck parameters
  alpha = .25*atan(r0/z0)
  thetah = 2*(1+2*psi)/(1+psi) * alpha
  thetap = 2*psi/(1+psi) * alpha
  p = z0*tan(4*alpha)*tan(thetap)
  d = z0*tan(4*alpha)*tan(4*alpha-thetah)
  e = cos(4*alpha)*(1+tan(4*alpha)*tan(thetah))

  Fz = 2*p
  !Loop through rays and trace to mirror
  !$omp parallel do private(delt,F,Fx,Fy,Fp)
  do i=1,num
    delt = 100.
    do while(abs(delt)>1.e-10)
      F = 2*p*z(i) + p**2 + 4*e**2*p*d/(e**2-1) - x(i)**2 - y(i)**2
      Fx = -2.*x(i)
      Fy = -2.*y(i)
      Fp = Fx*l(i) + Fy*m(i) + Fz*n(i)
      delt = -F/Fp
      x(i) = x(i) + l(i)*delt
      y(i) = y(i) + m(i)*delt
      z(i) = z(i) + n(i)*delt
      opd(i) = opd(i) + nr*delt
      !print *, x(i),y(i),z(i)
      !print *, F
      !print * ,delt
      !read *, dum
    end do
    Fp = sqrt(Fx*Fx+Fy*Fy+Fz*Fz)
    ux(i) = Fx/Fp
    uy(i) = Fy/Fp
    uz(i) = Fz/Fp
    !print *, x(i),y(i),z(i)
    !print *, ux(i),uy(i),uz(i)
    !read *, dum
  end do
  !$omp end parallel do

end subroutine wolterprimaryopd

!This function traces to a Wolter I secondary mirror
!Defined by Van Speybroeck prescription
!For WFS test, use flat to get rays close so they find correct intersection
!Surface should be placed at common focus with z+ pointing toward mirrors
subroutine woltersecondary(x,y,z,l,m,n,ux,uy,uz,num,r0,z0,psi)
  !Declarations
  implicit none
  integer, intent(in) :: num
  real*8 , intent(inout) :: x(num),y(num),z(num),l(num),m(num),n(num),ux(num),uy(num),uz(num)
  real*8, intent(in) :: r0,z0,psi
  real*8 :: alpha,thetah,thetap,p,d,e
  real*8 :: F,Fx,Fy,Fz,Fp,delt,dum
  integer :: i

  !Compute Van Speybroeck parameters
  alpha = .25*atan(r0/z0)
  thetah = 2*(1+2*psi)/(1+psi) * alpha
  thetap = 2*psi/(1+psi) * alpha
  p = z0*tan(4*alpha)*tan(thetap)
  d = z0*tan(4*alpha)*tan(4*alpha-thetah)
  e = cos(4*alpha)*(1+tan(4*alpha)*tan(thetah))

  !Loop through rays and trace to mirror
  !$omp parallel do private(delt,F,Fx,Fy,Fz,Fp)
  do i=1,num
    delt = 100.
    do while(abs(delt)>1.e-8)
      F = e**2*(d+z(i))**2 - z(i)**2 - x(i)**2 - y(i)**2
      Fx = -2.*x(i)
      Fy = -2.*y(i)
      Fz = 2*e**2*(d+z(i)) - 2*z(i)
      Fp = Fx*l(i) + Fy*m(i) + Fz*n(i)
      delt = -F/Fp
      x(i) = x(i) + l(i)*delt
      y(i) = y(i) + m(i)*delt
      z(i) = z(i) + n(i)*delt
      !print *, x(i),y(i),z(i)
      !print *, F, Fx, Fy, Fz
      !print * ,delt
      !read *, dum
    end do
    Fp = sqrt(Fx*Fx+Fy*Fy+Fz*Fz)
    ux(i) = Fx/Fp
    uy(i) = Fy/Fp
    uz(i) = Fz/Fp
    !print *, x(i),y(i),z(i)
    !print *, ux(i),uy(i),uz(i)
    !read *, dum
  end do
  !$omp end parallel do

end subroutine woltersecondary

!This function traces to a Wolter I primary mirror with sinusoidal perturbation
!Defined by Van Speybroeck prescription
!For WFS test, use flat to get rays close so they find correct intersection
!Surface should be placed at common focus with z+ pointing toward mirrors
subroutine woltersine(x,y,z,l,m,n,ux,uy,uz,num,r0,z0,amp,freq)
  !Declarations
  implicit none
  integer, intent(in) :: num
  real*8 , intent(inout) :: x(num),y(num),z(num),l(num),m(num),n(num),ux(num),uy(num),uz(num)
  real*8, intent(in) :: r0,z0,amp,freq
  real*8 :: alpha,thetah,thetap,p,d,e
  real*8 :: F,Fx,Fy,Fz,Fp,delt,dum,rad
  integer :: i

  !Compute Van Speybroeck parameters
  alpha = .25*atan(r0/z0)
  thetah = 3.*alpha
  thetap = alpha
  p = z0*tan(4*alpha)*tan(thetap)
  d = z0*tan(4*alpha)*tan(4*alpha-thetah)
  e = cos(4*alpha)*(1+tan(4*alpha)*tan(thetah))

  !Loop through rays and trace to mirror
  !$omp parallel do private(delt,F,Fx,Fy,Fz,Fp,rad)
  do i=1,num
    delt = 100.
    do while(abs(delt)>1.e-10)
      rad = sqrt(x(i)**2+y(i)**2) + amp*sin(2*acos(-1.)*freq*z(i))
      F = 2*p*z(i) + p**2 + 4*e**2*p*d/(e**2-1) - rad**2
      Fx = -2.*x(i)
      Fy = -2.*y(i)
      Fz = 2.*p - 2*rad*amp*2*acos(-1.)*freq*cos(2*acos(-1.)*freq*z(i))
      Fp = Fx*l(i) + Fy*m(i) + Fz*n(i)
      delt = -F/Fp
      x(i) = x(i) + l(i)*delt
      y(i) = y(i) + m(i)*delt
      z(i) = z(i) + n(i)*delt
      !print *, x(i),y(i),z(i)
      !print *, F
      !print * ,delt
      !read *, dum
    end do
    Fp = sqrt(Fx*Fx+Fy*Fy+Fz*Fz)
    ux(i) = Fx/Fp
    uy(i) = Fy/Fp
    uz(i) = Fz/Fp
    !print *, x(i),y(i),z(i)
    !print *, ux(i),uy(i),uz(i)
    !read *, dum
  end do
  !$omp end parallel do

end subroutine woltersine

!Construct a paraboloid as in Wolter but with Legendre-Legendre
!deformations. Define Legendre and Legendre derivative functions.
!Pass in coeff, axial order, and azimuthal order as in Zemax implementation
subroutine wolterprimLL(x,y,z,l,m,n,ux,uy,uz,num,r0,z0,zmax,zmin,dphi,coeff,axial,az,cnum)
  !Declarations
  implicit none
  integer, intent(in) :: num,cnum
  real*8 , intent(inout) :: x(num),y(num),z(num),l(num),m(num),n(num),ux(num),uy(num),uz(num)
  real*8, intent(in) :: r0,z0,zmax,zmin,dphi,coeff(cnum)
  integer, intent(in) :: axial(cnum),az(cnum)
  real*8 :: alpha,thetah,thetap,p,d,e
  real*8 :: F,Fx,Fy,Fz,Fp,delt,dum,rad,add,addx,addy,addz,G
  integer :: i,a
  real*8 :: pi,legendre,legendrep,zarg,targ,ang

  !Compute Van Speybroeck parameters
  pi = acos(-1.)
  alpha = .25*atan(r0/z0)
  thetah = 3.*alpha
  thetap = alpha
  p = z0*tan(4*alpha)*tan(thetap)
  d = z0*tan(4*alpha)*tan(4*alpha-thetah)
  e = cos(4*alpha)*(1+tan(4*alpha)*tan(thetah))

  !Loop through rays and trace to mirror
  !$omp parallel do private(delt,F,Fx,Fy,Fz,Fp,rad,ang,zarg,targ,add,addx,addy,addz,a,G)
  do i=1,num
    delt = 100.
    do while(abs(delt)>1.e-10)
      ang = atan2(y(i),x(i))
      zarg = (z(i)-((zmax+zmin)/2.)) / ((zmax-zmin)/2.)
      targ = 2*ang/dphi

      !Compute Legendre additive terms
      add = 0.
      addx = 0.
      addy = 0.
      addz = 0.
      do a=1,cnum
        add = add + coeff(a)*legendre(zarg,axial(a))*legendre(targ,az(a))
        addx = addx - coeff(a)*legendre(zarg,axial(a))*legendrep(targ,az(a))*(2/dphi)*(y(i)/(y(i)**2+x(i)**2))
        addy = addy + coeff(a)*legendre(zarg,axial(a))*legendrep(targ,az(a))*(2/dphi)*(x(i)/(y(i)**2+x(i)**2))
        addz = addz + coeff(a)*legendrep(zarg,axial(a))*legendre(targ,az(a))*2/(zmax-zmin)
      end do
      G = sqrt(x(i)**2+y(i)**2) + add
      F = -(G**2 - p**2 - 2*p*z(i) - 4*e**2*p*d/(e**2-1))
      Fx = -2*G*(x(i)/sqrt(x(i)**2+y(i)**2)+addx)
      Fy = -2*G*(y(i)/sqrt(x(i)**2+y(i)**2)+addy)
      Fz = 2*p - 2*G*addz
      Fp = Fx*l(i) + Fy*m(i) + Fz*n(i)
      delt = -F/Fp
      x(i) = x(i) + l(i)*delt
      y(i) = y(i) + m(i)*delt
      z(i) = z(i) + n(i)*delt
    end do
    Fp = sqrt(Fx*Fx+Fy*Fy+Fz*Fz)
    ux(i) = Fx/Fp
    uy(i) = Fy/Fp
    uz(i) = Fz/Fp
    !Set rays outside mirror definition to NaN
    !if (abs(zarg)>1 .or. abs(targ) > 1) then
    !  x(i) = 0.
    !  y(i) = 0.
    !  z(i) = 0.
    !end if
    !print *, x(i),y(i),z(i)
    !print *, ux(i),uy(i),uz(i)
    !read *, dum
  end do
  !$omp end parallel do

end subroutine wolterprimLL

!Construct a hyperboloid as in Wolter but with Legendre-Legendre
!deformations. Define Legendre and Legendre derivative functions.
!Pass in coeff, axial order, and azimuthal order as in Zemax implementation
subroutine woltersecLL(x,y,z,l,m,n,ux,uy,uz,num,r0,z0,psi,zmax,zmin,dphi,coeff,axial,az,cnum)
  !Declarations
  implicit none
  integer, intent(in) :: num,cnum
  real*8 , intent(inout) :: x(num),y(num),z(num),l(num),m(num),n(num),ux(num),uy(num),uz(num)
  real*8, intent(in) :: r0,z0,psi,zmax,zmin,dphi,coeff(cnum)
  integer, intent(in) :: axial(cnum),az(cnum)
  real*8 :: alpha,thetah,thetap,p,d,e
  real*8 :: F,Fx,Fy,Fz,Fp,delt,dum,rad,add,addx,addy,addz,G
  integer :: i,a
  real*8 :: pi,legendre,legendrep,zarg,targ,ang

  !Compute Van Speybroeck parameters
  alpha = .25*atan(r0/z0)
  thetah = 2*(1+2*psi)/(1+psi) * alpha
  thetap = 2*psi/(1+psi) * alpha
  p = z0*tan(4*alpha)*tan(thetap)
  d = z0*tan(4*alpha)*tan(4*alpha-thetah)
  e = cos(4*alpha)*(1+tan(4*alpha)*tan(thetah))
  !print *, “Parameters ok”,p
  !read *, dum

  !Loop through rays and trace to mirror
  !$omp parallel do private(delt,F,Fx,Fy,Fz,Fp,rad,ang,zarg,targ,add,addx,addy,addz,a,G)
  do i=1,num
    delt = 100.
    do while(abs(delt)>1.e-7)
      ang = atan2(y(i),x(i))
      zarg = (z(i)-((zmax+zmin)/2.)) / ((zmax-zmin)/2.)
      targ = 2*ang/dphi

      !print *, x(i),y(i),z(i)
      !read *, dum

      !Compute Legendre additive terms
      add = 0.
      addx = 0.
      addy = 0.
      addz = 0.
      do a=1,cnum
        add = add + coeff(a)*legendre(zarg,axial(a))*legendre(targ,az(a))
        !print *, add
        !read *, dum
        addx = addx - coeff(a)*legendre(zarg,axial(a))*legendrep(targ,az(a))*(2/dphi)*(y(i)/(y(i)**2+x(i)**2))
        !print *, addx
        addy = addy + coeff(a)*legendre(zarg,axial(a))*legendrep(targ,az(a))*(2/dphi)*(x(i)/(y(i)**2+x(i)**2))
        !print *, addy
        addz = addz + coeff(a)*legendrep(zarg,axial(a))*legendre(targ,az(a))*2/(zmax-zmin)
        !print *, addz
        !read *, dum
      end do
      G = sqrt(x(i)**2+y(i)**2) + add
      F = -(G**2 - e**2*(d+z(i))**2 + z(i)**2)
      Fx = -2*G*(x(i)/sqrt(x(i)**2+y(i)**2)+addx)
      Fy = -2*G*(y(i)/sqrt(x(i)**2+y(i)**2)+addy)
      Fz = 2*e**2*(d+z(i)) - 2*z(i) - 2*G*addz
      Fp = Fx*l(i) + Fy*m(i) + Fz*n(i)
      delt = -F/Fp
      !print *, G, F, Fx, Fy, Fz
      !print *, l(i), m(i), n(i), delt
      !print *, i
      !read *, dum
      x(i) = x(i) + l(i)*delt
      y(i) = y(i) + m(i)*delt
      z(i) = z(i) + n(i)*delt
      !print *, x(i),y(i),z(i)
      !print *, F
      !print * ,delt
      !read *, dum
    end do
    Fp = sqrt(Fx*Fx+Fy*Fy+Fz*Fz)
    ux(i) = Fx/Fp
    uy(i) = Fy/Fp
    uz(i) = Fz/Fp
    !Set rays outside mirror definition to NaN
    !if (abs(zarg)>1 .or. abs(targ) > 1) then
    !  x(i) = 0.
    !  y(i) = 0.
    !  z(i) = 0.
    !end if
    !print *, x(i),y(i),z(i)
    !print *, ux(i),uy(i),uz(i)
    !read *, dum
  end do
  !$omp end parallel do

end subroutine woltersecLL

!This function traces to a Wolter-Schwarzschild primary mirror
!Defined by Van Speybroeck prescription
!Surface should be placed at common focus with z+ pointing toward mirrors
!If ray is within inner radius of mirror (defined by betas), it will be
!traced to minimum z position
!Code in Python wrapper must vignette such rays
subroutine wsprimary(x,y,z,l,m,n,ux,uy,uz,num,alpha,z0,psi)
  !Declarations
  implicit none
  integer, intent(in) :: num
  real*8 , intent(inout) :: x(num),y(num),z(num),l(num),m(num),n(num),ux(num),uy(num),uz(num)
  real*8, intent(in) :: alpha,z0,psi
  real*8 :: k,kterm,dbdx,dbdy,beta,betas,ff,g,r
  real*8 :: F,Fx,Fy,Fz,Fp,delt,dum,Fb,xi,yi,zi
  integer :: i, flag, c

  !Compute Chase parameters
  betas = 4*alpha
  ff = z0/cos(betas)
  g = ff / psi
  k = tan(betas/2)**2

  !Loop through rays and trace to mirror
  !$omp parallel do private(i,delt,F,Fx,Fy,Fz,Fp,Fb,kterm,beta,dbdx,dbdy,flag,r,xi,yi,zi)
  do i=1,num
    delt = 100.
    c = 0
    !Initial ray position
    xi = x(i)
    yi = y(i)
    zi = z(i)
    do while(abs(delt)>1.e-8)
      beta = asin(sqrt(x(i)**2 + y(i)**2)/ff)
      flag = 0
      if (beta<=betas) then
        beta = betas
        flag = 1
        kterm = 0.
      else
        kterm = (1/k)*tan(beta/2)**2 - 1
      end if
      F = -z(i) - ff*sin(betas/2)**2 + &
          ff**2*sin(beta)**2/(4*ff*sin(betas/2)**2) + &
          g*cos(beta/2)**4*(kterm)**(1-k)
      Fb = ff**2*sin(beta)*cos(beta)/(2*ff*sin(betas/2)**2) - &
           2*g*cos(beta/2)**3*sin(beta/2)*(kterm)**(1-k) + &
           g*(1-k)*cos(beta/2)*sin(beta/2)*(kterm)**(-k)*(1/k)
      Fz = -1.
      if (flag==1) then
        r = sqrt(x(i)**2 + y(i)**2)
        Fb = ff**2*sin(betas)*cos(betas)/(2*ff*sin(betas/2)**2) + &
              g*(1-k)*cos(betas/2)*sin(betas/2)*(1/k)
        F = F + (r - ff*sin(betas))*z(i)/(r**2+z(i)**2)*Fb
        Fz = Fz + (r-ff*sin(betas))*(r**2-z(i)**2)/(r**2+z(i)**2)**2*Fb
        !print *, Fb, F, Fz
        !read *, dum
      end if
      dbdx = x(i)/sqrt(1-(x(i)**2+y(i)**2)/ff**2)/ff/sqrt(x(i)**2+y(i)**2)
      dbdy = y(i)/sqrt(1-(x(i)**2+y(i)**2)/ff**2)/ff/sqrt(x(i)**2+y(i)**2)
      Fx = Fb * dbdx
      Fy = Fb * dbdy
      Fp = Fx*l(i) + Fy*m(i) + Fz*n(i)
      delt = -F/Fp
      !if (isnan(delt)) then
      !  print *, c,x(i),y(i),z(i)
      !  print *, F, Fx, Fy, Fz
      !  print *, kterm, Fb, flag,k,tan(beta/2)**2
      !  print *, betas,ff
      !  read *, dum
      !end if
      x(i) = x(i) + l(i)*delt
      y(i) = y(i) + m(i)*delt
      z(i) = z(i) + n(i)*delt
      if (c > 25 .or. isnan(delt)) then
        delt = 0.
        x(i) = xi
        y(i) = yi
        z(i) = zi
        c = 1000
      end if
      c = c + 1
      !read *, dum
    end do
    if (c < 26) then
      Fp = sqrt(Fx*Fx+Fy*Fy+Fz*Fz)
      ux(i) = -Fx/Fp
      uy(i) = -Fy/Fp
      uz(i) = -Fz/Fp
    end if
    !print *, x(i),y(i),z(i)
    !print *, ux(i),uy(i),uz(i)
    !read *, dum
  end do
  !$omp end parallel do

end subroutine wsprimary

!This function traces to a Wolter-Schwarzschild secondary mirror
!Defined by Van Speybroeck prescription
!Surface should be placed at common focus with z+ pointing toward mirrors
!If ray is within inner radius of mirror (defined by betas), it will be
!traced to minimum z position
!Code in Python wrapper must vignette such rays
subroutine wssecondary(x,y,z,l,m,n,ux,uy,uz,num,alpha,z0,psi)
  !Declarations
  implicit none
  integer, intent(in) :: num
  real*8 , intent(inout) :: x(num),y(num),z(num),l(num),m(num),n(num),ux(num),uy(num),uz(num)
  real*8, intent(in) :: alpha,z0,psi
  real*8 :: k,kterm,dbdx,dbdy,dbdz,dadb,beta,betas,ff,g,d,a
  real*8 :: gam,dbdzs,dadbs
  real*8 :: F,Fx,Fy,Fz,Fp,delt,dum,Fb,xi,yi,zi
  integer :: i, flag, c

  !Compute Chase parameters
  betas = 4*alpha
  ff = z0/cos(betas)
  g = ff / psi
  k = tan(betas/2)**2

  !Loop through rays and trace to mirror
  !$omp parallel do private(i,delt,F,Fx,Fy,Fz,Fp,Fb,kterm,beta,dbdx,dbdy,dbdz,flag,c,a,dadbs,dbdzs,gam,dadb,xi,yi,zi)
  do i=1,num
    delt = 100.
    c = 0
    !Initial ray position
    xi = x(i)
    yi = y(i)
    zi = z(i)
    do while(abs(delt)>1.e-8)
      beta = atan2(sqrt(x(i)**2 + y(i)**2),z(i))
      flag = 0
      if (beta<=betas) then
        beta = betas
        kterm = 0
        a = 1/ff
        flag = 1
      else
        kterm = (1/k)*tan(beta/2)**2 - 1
        a = (1-cos(beta))/(1-cos(betas))/ff + &
          (1+cos(beta))/(2*g)*(kterm)**(1+k)
      end if
      F = -z(i) + cos(beta)/a
      !Add corrective term to F if beta was < betas
      if (flag==1) then
        Fb = 0.
        dadbs = sin(betas)/ff/(1-cos(betas)) + &
                (k+1)*(cos(betas)+1)*tan(betas/2)/cos(betas/2)**2/2/g/k
        dbdzs = -sin(betas)**2/sqrt(x(i)**2+y(i)**2)
        gam = (-ff*sin(betas)-ff**2*cos(betas)*dadbs)*dbdzs
        F = F + gam*(z(i)-sqrt(x(i)**2+y(i)**2)/tan(betas))
        Fx = -2./tan(betas)*x(i)/sqrt(x(i)**2+y(i)**2)
        Fy = -2./tan(betas)*y(i)/sqrt(x(i)**2+y(i)**2)
        Fz = gam - 1.
        !print *, x(i), y(i), z(i)
        !print *, F, Fx, Fy, Fz
        !print *, delt
        !read *, dum
      !Otherwise, business as usual
      else
        dadb = sin(beta)/ff/(1-cos(betas)) - &
               sin(beta)/(2*g)*(kterm)**(1+k) + &
               (k+1)*(cos(beta)+1)*tan(beta/2)*kterm**k/2/g/k/(cos(beta/2)**2)
        Fb = -sin(beta)/a - cos(beta)/a**2*dadb
        dbdx = x(i)*z(i)/(x(i)**2+y(i)**2+z(i)**2)/sqrt(x(i)**2+y(i)**2)
        dbdy = y(i)*z(i)/(x(i)**2+y(i)**2+z(i)**2)/sqrt(x(i)**2+y(i)**2)
        dbdz = -sqrt(x(i)**2+y(i)**2)/(x(i)**2+y(i)**2+z(i)**2)
        Fx = Fb * dbdx
        Fy = Fb * dbdy
        Fz = -1. + Fb*dbdz
      end if
      !We have derivatives, now compute the iteration
      Fp = Fx*l(i) + Fy*m(i) + Fz*n(i)
      delt = -F/Fp
      !print *, x(i), y(i), z(i)
      !print *, F, Fx, Fy, Fz
      !print *, delt
      !read *, dum
      x(i) = x(i) + l(i)*delt
      y(i) = y(i) + m(i)*delt
      z(i) = z(i) + n(i)*delt
      if (c > 25 .or. isnan(delt)) then
        delt = 0.
        x(i) = xi
        y(i) = yi
        z(i) = zi
        c = 1000
      end if
      !print *, x(i),y(i),z(i)
      !print *, F
      !print * ,delt
      !read *, dum
      c = c+1
    end do
    if (c < 26) then
      Fp = sqrt(Fx*Fx+Fy*Fy+Fz*Fz)
      ux(i) = Fx/Fp
      uy(i) = Fy/Fp
      uz(i) = Fz/Fp
    end if
    !print *, x(i),y(i),z(i)
    !print *, ux(i),uy(i),uz(i)
    !print *, F, Fx, Fy, Fz
    !read *, dum
  end do
  !$omp end parallel do

end subroutine wssecondary

!Intersection with SPO Cone
subroutine spoCone(x,y,z,l,m,n,ux,uy,uz,num,R0,tg)
  !Declarations
  implicit none
  integer, intent(in) :: num
  real*8 , intent(inout) :: x(num),y(num),z(num),l(num),m(num),n(num),ux(num),uy(num),uz(num)
  real*8, intent(in) :: R0,tg
!  real*8 :: k,kterm,dbdx,dbdy,dbdz,dadb,beta,betas,ff,g,d,a
!  real*8 :: gam,dbdzs,dadbs
!  real*8 :: F,Fx,Fy,Fz,Fp,delt,dum,Fb
  integer :: i
  real*8 :: A,B,C,sl,det,t1,t2

  !Loop through rays and trace to mirror
  sl = tan(tg)
  !$omp parallel do private(i,A,B,C,det,t1,t2)
  do i=1,num
    !Solve quadratic equation for ray advancement distance
    A = n(i)**2*sl**2 - m(i)**2 - l(i)**2
    B = 2*n(i)*sl*R0 + 2*z(i)*sl**2*n(i) - 2*x(i)*l(i) - 2*y(i)*m(i)
    C = R0**2 + 2*sl*R0*z(i) + z(i)**2*sl**2 - x(i)**2 - y(i)**2
    det = B**2 - 4*A*C
    if (det .ge. 0) then
      t1 = (-B + sqrt(det))/(2*A)
      t2 = (-B - sqrt(det))/(2*A)
      if (abs(t2) < abs(t1)) then
        t1 = t2
      end if
      !Set new ray position
      x(i) = x(i) + t1*l(i)
      y(i) = y(i) + t1*m(i)
      z(i) = z(i) + t1*n(i)
      !Set up surface normal
      ux(i) = -x(i)/sqrt(x(i)**2+y(i)**2)*cos(tg)
      uy(i) = -y(i)/sqrt(x(i)**2+y(i)**2)*cos(tg)
      uz(i) = sin(tg)!*abs(z(i))/z(i)
    else
      l(i) = 0.
      m(i) = 0.
      n(i) = 0.
    end if
    !print *, x(i),y(i),z(i)
    !print *, ux(i),uy(i),uz(i)
    !print *, F, Fx, Fy, Fz
    !read *, dum
  end do
  !$omp end parallel do

end subroutine spoCone

!Construct an ellipsoid primary but with Legendre-Legendre
!deformations. Define Legendre and Legendre derivative functions.
!Pass in coeff, axial order, and azimuthal order as in Zemax implementation
subroutine ellipsoidWoltLL(x,y,z,l,m,n,ux,uy,uz,num,r0,z0,psi,S,zmax,zmin,dphi,coeff,axial,az,cnum)
  !Declarations
  implicit none
  integer, intent(in) :: num,cnum
  real*8 , intent(inout) :: x(num),y(num),z(num),l(num),m(num),n(num),ux(num),uy(num),uz(num)
  real*8, intent(in) :: r0,z0,psi,S,zmax,zmin,dphi,coeff(cnum)
  integer, intent(in) :: axial(cnum),az(cnum)
  real*8 :: P,ff,bq,cq,aa,bb,e
  real*8 :: F,Fx,Fy,Fz,Fp,delt,dum,rad,add,addx,addy,addz,G
  integer :: i,a
  real*8 :: pi,legendre,legendrep,zarg,targ,ang,dummy,zfoc

  !Compute telescope parameters
  pi = acos(-1.)
  P = R0/sin((psi*asin(R0/z0)-asin(R0/S))/(1+psi))
  ff = (S+P)/2.
  bq = -(R0**2+(ff-P)**2+ff**2)
  cq = ff**2*(ff-P)**2
  aa = sqrt((-bq+sqrt(bq**2-4*cq))/2.)
  bb = sqrt(aa**2-ff**2)
  e = ff/aa
  zfoc = ff-P+z0

  !Loop through rays and trace to mirror
  !$omp parallel do private(delt,F,Fx,Fy,Fz,Fp,rad,ang,zarg,targ,add,addx,addy,addz,a,G)
  do i=1,num
    delt = 100.
    do while(abs(delt)>1.e-10)
      ang = atan2(y(i),x(i))
      zarg = (z(i)-((zmax+zmin)/2.)) / ((zmax-zmin)/2.)
      targ = 2*ang/dphi

      !Compute Legendre additive terms
      add = 0.
      addx = 0.
      addy = 0.
      addz = 0.
      do a=1,cnum
        add = add + coeff(a)*legendre(zarg,axial(a))*legendre(targ,az(a))
        addx = addx - coeff(a)*legendre(zarg,axial(a))*legendrep(targ,az(a))*(2/dphi)*(y(i)/(y(i)**2+x(i)**2))
        addy = addy + coeff(a)*legendre(zarg,axial(a))*legendrep(targ,az(a))*(2/dphi)*(x(i)/(y(i)**2+x(i)**2))
        addz = addz + coeff(a)*legendrep(zarg,axial(a))*legendre(targ,az(a))*2/(zmax-zmin)
      end do
      G = sqrt(x(i)**2+y(i)**2) + add
      F = (z(i)-zfoc)**2/aa**2 + G**2/bb**2 - 1.
      Fx = 2*G/bb**2*(x(i)/sqrt(x(i)**2+y(i)**2)+addx)
      Fy = 2*G/bb**2*(y(i)/sqrt(x(i)**2+y(i)**2)+addy)
      Fz = 2*(z(i)-zfoc)/aa**2 + (2*G/bb**2)*(addz)
      Fp = Fx*l(i) + Fy*m(i) + Fz*n(i)
      delt = -F/Fp
      !print *, 'Position: ',x(i), y(i), z(i)
      !print *, 'Function: ', F, Fx, Fy, Fz
      !print *, 'Change: ',delt, z0,aa
      !print *, z(i)-zfoc
      !read *, dummy
      x(i) = x(i) + l(i)*delt
      y(i) = y(i) + m(i)*delt
      z(i) = z(i) + n(i)*delt
    end do
    Fp = sqrt(Fx*Fx+Fy*Fy+Fz*Fz)
    ux(i) = Fx/Fp
    uy(i) = Fy/Fp
    uz(i) = Fz/Fp
    !Set rays outside mirror definition to NaN
    !if (abs(zarg)>1 .or. abs(targ) > 1) then
    !  x(i) = 0.
    !  y(i) = 0.
    !  z(i) = 0.
    !end if
    !print *, x(i),y(i),z(i)
    !print *, ux(i),uy(i),uz(i)
    !read *, dum
  end do
  !$omp end parallel do

end subroutine ellipsoidWoltLL

!This function traces to a Wolter-Schwarzschild primary mirror
!Defined by Van Speybroeck prescription
!Surface should be placed at common focus with z+ pointing toward mirrors
!If ray is within inner radius of mirror (defined by betas), it will be
!traced to minimum z position
!Code in Python wrapper must vignette such rays
subroutine wsprimaryBack(x,y,z,l,m,n,ux,uy,uz,num,alpha,z0,psi,thick)
  !Declarations
  implicit none
  integer, intent(in) :: num
  real*8 , intent(inout) :: x(num),y(num),z(num),l(num),m(num),n(num),ux(num),uy(num),uz(num)
  real*8, intent(in) :: alpha,z0,psi,thick
  real*8 :: k,kterm,dbdx,dbdy,beta,betas,ff,g,r,theta,x2,y2
  real*8 :: F,Fx,Fy,Fz,Fp,delt,dum,Fb,xi,yi,zi
  integer :: i, flag, c

  !Compute Chase parameters
  betas = 4*alpha
  ff = z0/cos(betas)
  g = ff / psi
  k = tan(betas/2)**2

  !Loop through rays and trace to mirror
  !$omp parallel do private(i,delt,F,Fx,Fy,Fz,Fp,Fb,kterm,beta,dbdx,dbdy,flag,r,x2,y2,theta,xi,yi,zi)
  do i=1,num
    delt = 100.
    c = 0
    !Initial ray position
    xi = x(i)
    yi = y(i)
    zi = z(i)
    do while(abs(delt)>1.e-8)
      !Adjust x and y positions
      r = sqrt(x(i)**2+y(i)**2)
      theta = atan2(y(i),x(i))
      x2 = (r-thick)*cos(theta)
      y2 = (r-thick)*sin(theta)
      beta = asin(sqrt(x2**2 + y2**2)/ff)
      flag = 0
      if (beta<=betas) then
        beta = betas
        flag = 1
        kterm = 0.
      else
        kterm = (1/k)*tan(beta/2)**2 - 1
      end if
      F = -z(i) - ff*sin(betas/2)**2 + &
          ff**2*sin(beta)**2/(4*ff*sin(betas/2)**2) + &
          g*cos(beta/2)**4*(kterm)**(1-k)
      Fb = ff**2*sin(beta)*cos(beta)/(2*ff*sin(betas/2)**2) - &
           2*g*cos(beta/2)**3*sin(beta/2)*(kterm)**(1-k) + &
           g*(1-k)*cos(beta/2)*sin(beta/2)*(kterm)**(-k)*(1/k)
      Fz = -1.
      if (flag==1) then
        r = sqrt(x2**2 + y2**2)
        Fb = ff**2*sin(betas)*cos(betas)/(2*ff*sin(betas/2)**2) + &
              g*(1-k)*cos(betas/2)*sin(betas/2)*(1/k)
        F = F + (r - ff*sin(betas))*z(i)/(r**2+z(i)**2)*Fb
        Fz = Fz + (r-ff*sin(betas))*(r**2-z(i)**2)/(r**2+z(i)**2)**2*Fb
        !print *, Fb, F, Fz
        !read *, dum
      end if
      dbdx = x2/sqrt(1-(x2**2+y2**2)/ff**2)/ff/sqrt(x2**2+y2**2)
      dbdy = y2/sqrt(1-(x2**2+y2**2)/ff**2)/ff/sqrt(x2**2+y2**2)
      Fx = Fb * dbdx
      Fy = Fb * dbdy
      Fp = Fx*l(i) + Fy*m(i) + Fz*n(i)
      delt = -F/Fp
      !print *, x(i),y(i),z(i)
      !print *, F, Fx, Fy, Fz
      !print *, kterm, Fb, flag,k,tan(beta/2)**2
      x(i) = x(i) + l(i)*delt
      y(i) = y(i) + m(i)*delt
      z(i) = z(i) + n(i)*delt
      if (c > 25 .or. isnan(delt)) then
        delt = 0.
        x(i) = xi
        y(i) = yi
        z(i) = zi
        c = 1000
      end if
      c = c + 1
      !read *, dum
    end do
    if (c < 26) then
      Fp = sqrt(Fx*Fx+Fy*Fy+Fz*Fz)
      ux(i) = -Fx/Fp
      uy(i) = -Fy/Fp
      uz(i) = -Fz/Fp
    end if
    !print *, x(i),y(i),z(i)
    !print *, ux(i),uy(i),uz(i)
    !read *, dum
  end do
  !$omp end parallel do

end subroutine wsprimaryBack

!This function traces to a Wolter-Schwarzschild secondary mirror
!Defined by Van Speybroeck prescription
!Surface should be placed at common focus with z+ pointing toward mirrors
!If ray is within inner radius of mirror (defined by betas), it will be
!traced to minimum z position
!Code in Python wrapper must vignette such rays
subroutine wssecondaryBack(x,y,z,l,m,n,ux,uy,uz,num,alpha,z0,psi,thick)
  !Declarations
  implicit none
  integer, intent(in) :: num
  real*8 , intent(inout) :: x(num),y(num),z(num),l(num),m(num),n(num),ux(num),uy(num),uz(num)
  real*8, intent(in) :: alpha,z0,psi,thick
  real*8 :: k,kterm,dbdx,dbdy,dbdz,dadb,beta,betas,ff,g,d,a,r,theta,x2,y2
  real*8 :: gam,dbdzs,dadbs
  real*8 :: F,Fx,Fy,Fz,Fp,delt,dum,Fb,xi,yi,zi
  integer :: i, flag, c

  !Compute Chase parameters
  betas = 4*alpha
  ff = z0/cos(betas)
  g = ff / psi
  k = tan(betas/2)**2

  !Loop through rays and trace to mirror
  !$omp parallel do private(i,delt,F,Fx,Fy,Fz,Fp,Fb,kterm,beta,dbdx,dbdy,dbdz,flag,c,a,dadbs,dbdzs,gam,dadb,r,theta,x2,y2,xi,yi,zi)
  do i=1,num
    delt = 100.
    c = 0
    !Initial ray position
    xi = x(i)
    yi = y(i)
    zi = z(i)
    do while(abs(delt)>1.e-8)
      !Adjust x and y positions
      r = sqrt(x(i)**2 + y(i)**2)
      theta = atan2(y(i),x(i))
      x2 = (r-thick)*cos(theta)
      y2 = (r-thick)*sin(theta)
      beta = atan2(sqrt(x2**2 + y2**2),z(i))
      flag = 0
      if (beta<=betas) then
        beta = betas
        kterm = 0
        a = 1/ff
        flag = 1
      else
        kterm = (1/k)*tan(beta/2)**2 - 1
        a = (1-cos(beta))/(1-cos(betas))/ff + &
          (1+cos(beta))/(2*g)*(kterm)**(1+k)
      end if
      F = -z(i) + cos(beta)/a
      !Add corrective term to F if beta was < betas
      if (flag==1) then
        Fb = 0.
        dadbs = sin(betas)/ff/(1-cos(betas)) + &
                (k+1)*(cos(betas)+1)*tan(betas/2)/cos(betas/2)**2/2/g/k
        dbdzs = -sin(betas)**2/sqrt(x2**2+y2**2)
        gam = (-ff*sin(betas)-ff**2*cos(betas)*dadbs)*dbdzs
        F = F + gam*(z(i)-sqrt(x2**2+y2**2)/tan(betas))
        Fx = -2./tan(betas)*x2/sqrt(x2**2+y2**2)
        Fy = -2./tan(betas)*y2/sqrt(x2**2+y2**2)
        Fz = gam - 1.
        !print *, x(i), y(i), z(i)
        !print *, F, Fx, Fy, Fz
        !print *, delt
        !read *, dum
      !Otherwise, business as usual
      else
        dadb = sin(beta)/ff/(1-cos(betas)) - &
               sin(beta)/(2*g)*(kterm)**(1+k) + &
               (k+1)*(cos(beta)+1)*tan(beta/2)*kterm**k/2/g/k/(cos(beta/2)**2)
        Fb = -sin(beta)/a - cos(beta)/a**2*dadb
        dbdx = x2*z(i)/(x2**2+y2**2+z(i)**2)/sqrt(x2**2+y2**2)
        dbdy = y2*z(i)/(x2**2+y2**2+z(i)**2)/sqrt(x2**2+y2**2)
        dbdz = -sqrt(x2**2+y2**2)/(x2**2+y2**2+z(i)**2)
        Fx = Fb * dbdx
        Fy = Fb * dbdy
        Fz = -1. + Fb*dbdz
      end if
      !We have derivatives, now compute the iteration
      Fp = Fx*l(i) + Fy*m(i) + Fz*n(i)
      delt = -F/Fp
      !print *, x(i), y(i), z(i)
      !print *, F, Fx, Fy, Fz
      !print *, delt
      !read *, dum
      x(i) = x(i) + l(i)*delt
      y(i) = y(i) + m(i)*delt
      z(i) = z(i) + n(i)*delt
      if (c > 25 .or. isnan(delt)) then
        delt = 0.
        x(i) = xi
        y(i) = yi
        z(i) = zi
        c = 1000
      end if
      !print *, x(i),y(i),z(i)
      !print *, F
      !print * ,delt
      !read *, dum
      c = c+1
    end do
    if (c < 26) then
      Fp = sqrt(Fx*Fx+Fy*Fy+Fz*Fz)
      ux(i) = Fx/Fp
      uy(i) = Fy/Fp
      uz(i) = Fz/Fp
    end if
    !print *, x(i),y(i),z(i)
    !print *, ux(i),uy(i),uz(i)
    !print *, F, Fx, Fy, Fz
    !read *, dum
  end do
  !$omp end parallel do

end subroutine wssecondaryBack


      program correl2
c
c***********************************************************************
c
c     dl_poly utility program for calculating correlation functions
c     using the fast fourier transform strategy
c
c     copyright - daresbury laboratory 1992
c     author    - w. smith may 1992.
c
c     note - this program includes a naff fft routine purely for
c     portability and verification reasons. users are strongly
c     recommended to substitute a machine specific fft, which
c     should vastly improve performance.
c
c     note - you will probably need to change some of the array
c     dimensions using the following parameters.
c
c     note - ndiv must be a power of 2. ndiv3 will change according to
c     the fft routine you use. leave ndiv2 as it is.
c
c***********************************************************************
c

      implicit real*8(a-h,o-z)

      parameter (ndiv=8192,ndiv2=2*ndiv,ndiv3=2*ndiv)
      parameter (mxcols=15,mxcorr=10)

      character*4 name

      complex*16 g(ndiv2),c(ndiv2),w(ndiv3),ave,cmplx
      dimension sample(mxcols),u(ndiv),t(ndiv)
      dimension icol(mxcorr),jcol(mxcorr),key(ndiv2)

c
c     declare how many correlation functions are required
c     and how many columns of data there in the input data
c     (excluding the time column)

      write(*,'(a)')'enter number of required correlation functions'
      read(*,*)npairs
      write(*,'(a)')'enter number of data columns in input file'
      read(*,*)ncols
      ncols=min(mxcols,ncols)
      npairs=min(mxcorr,npairs)

c
c     specify which columns in the input data are to be correlated

      write(*,'(a)')'enter ids of column pairs to be correlated'
      read(*,*)(icol(i),jcol(i),i=1,npairs)

c
c     initialise the fft work arrays

      call fft(1,1,ndiv2,key,g,w,g)

c
c     now process each correlation function in turn

      do ipairs=1,npairs

c
c     zero the complex data array

         do i=1,ndiv2
            g(i)=(0.d0,0.d0)
         enddo

c
c     open the file containing the columns of data

         open(7,file='input.data')

c
c     now read the required columns (note first column is time)

         do i=1,ndiv
            read(7,*,end=100)t(i),(sample(j),j=1,ncols)
            g(i)=cmplx(sample(icol(ipairs)),sample(jcol(ipairs)))
         enddo

  100    m=i-1

c
c     close data file

         close(7)

         dtim=t(2)-t(1)

c
c     calculate column  average

         ave=(0.d0,0.d0)

         do i=1,m
            ave=ave+g(i)
         enddo

         ave=ave/dble(m)

c
c     subtract average from columns

         do i=1,m
            g(i)=g(i)-ave
         enddo

c
c     fourier transform the data

         call fft(0,-1,ndiv2,key,g,w,g)

c
c     calculate correlation as product in frequency domain

         ur=real(g(1))
         ui=0.d0
         vr=aimag(g(1))
         vi=0.d0
         c(1)=cmplx(ur*vr+ui*vi,ur*vi-ui*vr)

         do i=2,ndiv2
            ur=0.5d0*(real(g(i))+real(g(ndiv2+2-i)))
            ui=0.5d0*(aimag(g(i))-aimag(g(ndiv2+2-i)))
            vr=0.5d0*(aimag(g(i))+aimag(g(ndiv2+2-i)))
            vi=0.5d0*(-real(g(i))+real(g(ndiv2+2-i)))
            c(i)=cmplx(ur*vr+ui*vi,ur*vi-ui*vr)
         enddo

c
c     inverse fourier transform

         call fft(0,1,ndiv2,key,c,w,c)

c
c     normalise the correlation function

         rnorm=1.d0/dble(ndiv2)
         do i=1,m
            u(i)=rnorm*real(c(i))/dble(m+1-i)
         enddo

c
c     open output file to receive correlation function

         namfil=100*icol(ipairs)+jcol(ipairs)
         write(name,'(i4.4)')namfil
         open(8,file='c'//name)

c
c     write out correlation function

         do i=1,m
            tim=dble(i-1)*dtim
            write(8,'(2f10.6)')tim,u(i)
         enddo

c
c     close correlation function file

         close(8)

      enddo

      stop
      end

      subroutine fft(ind,isw,ndiv,key,aaa,wfft,bbb)

c***********************************************************************
c
c     fast fourier transform routine
c
c     copyright daresbury laboratory
c     author - w. smith
c
c***********************************************************************

      implicit real*8(a-h,o-z)

      logical check
      complex*16 aaa(ndiv),bbb(ndiv),wfft(ndiv),ttt
      dimension key(ndiv)
      data tpi/6.2831853072d0/
   10 format(1h0,'error - number of points not a power of two')

c
c     check that array is of suitable length
      nt=1
      check=.true.
      do i=1,20
         nt=2*nt
         if(nt.eq.ndiv)then
            check=.false.
            nu=i
         endif
      enddo
      if(check)then
         write(*,10)
         stop
      endif

      if(ind.gt.0)then
c
c     set reverse bit address array

         do kkk=1,ndiv
            iii=0
            jjj=kkk-1
            do j=1,nu
               jj2=jjj/2
               iii=2*(iii-jj2)+jjj
               jjj=jj2
            enddo
            key(kkk)=iii+1
         enddo
c
c     initialise complex exponential factors

         tpn=tpi/dble(ndiv)
         arg=0.d0
         np1=ndiv+1
         np2=ndiv/2
         wfft(1)=(1.d0,0.d0)
         do i=1,np2
            arg=tpn*dble(i)
            wfft(i+1)=cmplx(cos(arg),sin(arg))
            wfft(np1-i)=conjg(wfft(i+1))
         enddo

         return
      endif

c
c     take conjugate of exponentials if required

      if(isw.lt.0)then

         do i=1,ndiv
            wfft(i)=conjg(wfft(i))
         enddo

      endif

c
c     take copy input array

      do i=1,ndiv
         bbb(i)=aaa(i)
      enddo
c
c     perform fourier transform

      kkk=0
      nu1=nu-1
      np2=ndiv/2
      do l=1,nu

  100    do i=1,np2
            iii=key(kkk/2**nu1+1)
            kk1=kkk+1
            k12=kk1+np2
            ttt=bbb(k12)*wfft(iii)
            bbb(k12)=bbb(kk1)-ttt
            bbb(kk1)=bbb(kk1)+ttt
            kkk=kkk+1
         enddo
         kkk=kkk+np2
         if(kkk.lt.ndiv)go to 100
         kkk=0
         nu1=nu1-1
         np2=np2/2

      enddo
c
c     unscramble the fft using bit address array

      do kkk=1,ndiv
         iii=key(kkk)
         if(iii.gt.kkk)then
            ttt=bbb(kkk)
            bbb(kkk)=bbb(iii)
            bbb(iii)=ttt
         endif
      enddo
c
c     restore exponentials to unconjugated values if necessary

      if(isw.lt.0)then

         do i=1,ndiv
            wfft(i)=conjg(wfft(i))
         enddo

      endif

      return
      end


      SUBROUTINE DHSEIN( SIDE, EIGSRC, INITV, SELECT, N, H, LDH, WR, WI,
     $                   VL, LDVL, VR, LDVR, MM, M, WORK, IFAILL,
     $                   IFAILR, INFO )
*
*  -- LAPACK routine (version 3.2) --
*     Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd..
*     November 2006
*
*     .. Scalar Arguments ..
      CHARACTER          EIGSRC, INITV, SIDE
      INTEGER            INFO, LDH, LDVL, LDVR, M, MM, N
*     ..
*     .. Array Arguments ..
      LOGICAL            SELECT( * )
      INTEGER            IFAILL( * ), IFAILR( * )
      DOUBLE PRECISION   H( LDH, * ), VL( LDVL, * ), VR( LDVR, * ),
     $                   WI( * ), WORK( * ), WR( * )
*     ..
*
*  Purpose
*  =======
*
*  DHSEIN uses inverse iteration to find specified right and/or left
*  eigenvectors of a real upper Hessenberg matrix H.
*
*  The right eigenvector x and the left eigenvector y of the matrix H
*  corresponding to an eigenvalue w are defined by:
*
*               H * x = w * x,     y**h * H = w * y**h
*
*  where y**h denotes the conjugate transpose of the vector y.
*
*  Arguments
*  =========
*
*  SIDE    (input) CHARACTER*1
*          = 'R': compute right eigenvectors only;
*          = 'L': compute left eigenvectors only;
*          = 'B': compute both right and left eigenvectors.
*
*  EIGSRC  (input) CHARACTER*1
*          Specifies the source of eigenvalues supplied in (WR,WI):
*          = 'Q': the eigenvalues were found using DHSEQR; thus, if
*                 H has zero subdiagonal elements, and so is
*                 block-triangular, then the j-th eigenvalue can be
*                 assumed to be an eigenvalue of the block containing
*                 the j-th row/column.  This property allows DHSEIN to
*                 perform inverse iteration on just one diagonal block.
*          = 'N': no assumptions are made on the correspondence
*                 between eigenvalues and diagonal blocks.  In this
*                 case, DHSEIN must always perform inverse iteration
*                 using the whole matrix H.
*
*  INITV   (input) CHARACTER*1
*          = 'N': no initial vectors are supplied;
*          = 'U': user-supplied initial vectors are stored in the arrays
*                 VL and/or VR.
*
*  SELECT  (input/output) LOGICAL array, dimension (N)
*          Specifies the eigenvectors to be computed. To select the
*          real eigenvector corresponding to a real eigenvalue WR(j),
*          SELECT(j) must be set to .TRUE.. To select the complex
*          eigenvector corresponding to a complex eigenvalue
*          (WR(j),WI(j)), with complex conjugate (WR(j+1),WI(j+1)),
*          either SELECT(j) or SELECT(j+1) or both must be set to
*          .TRUE.; then on exit SELECT(j) is .TRUE. and SELECT(j+1) is
*          .FALSE..
*
*  N       (input) INTEGER
*          The order of the matrix H.  N >= 0.
*
*  H       (input) DOUBLE PRECISION array, dimension (LDH,N)
*          The upper Hessenberg matrix H.
*
*  LDH     (input) INTEGER
*          The leading dimension of the array H.  LDH >= max(1,N).
*
*  WR      (input/output) DOUBLE PRECISION array, dimension (N)
*  WI      (input) DOUBLE PRECISION array, dimension (N)
*          On entry, the real and imaginary parts of the eigenvalues of
*          H; a complex conjugate pair of eigenvalues must be stored in
*          consecutive elements of WR and WI.
*          On exit, WR may have been altered since close eigenvalues
*          are perturbed slightly in searching for independent
*          eigenvectors.
*
*  VL      (input/output) DOUBLE PRECISION array, dimension (LDVL,MM)
*          On entry, if INITV = 'U' and SIDE = 'L' or 'B', VL must
*          contain starting vectors for the inverse iteration for the
*          left eigenvectors; the starting vector for each eigenvector
*          must be in the same column(s) in which the eigenvector will
*          be stored.
*          On exit, if SIDE = 'L' or 'B', the left eigenvectors
*          specified by SELECT will be stored consecutively in the
*          columns of VL, in the same order as their eigenvalues. A
*          complex eigenvector corresponding to a complex eigenvalue is
*          stored in two consecutive columns, the first holding the real
*          part and the second the imaginary part.
*          If SIDE = 'R', VL is not referenced.
*
*  LDVL    (input) INTEGER
*          The leading dimension of the array VL.
*          LDVL >= max(1,N) if SIDE = 'L' or 'B'; LDVL >= 1 otherwise.
*
*  VR      (input/output) DOUBLE PRECISION array, dimension (LDVR,MM)
*          On entry, if INITV = 'U' and SIDE = 'R' or 'B', VR must
*          contain starting vectors for the inverse iteration for the
*          right eigenvectors; the starting vector for each eigenvector
*          must be in the same column(s) in which the eigenvector will
*          be stored.
*          On exit, if SIDE = 'R' or 'B', the right eigenvectors
*          specified by SELECT will be stored consecutively in the
*          columns of VR, in the same order as their eigenvalues. A
*          complex eigenvector corresponding to a complex eigenvalue is
*          stored in two consecutive columns, the first holding the real
*          part and the second the imaginary part.
*          If SIDE = 'L', VR is not referenced.
*
*  LDVR    (input) INTEGER
*          The leading dimension of the array VR.
*          LDVR >= max(1,N) if SIDE = 'R' or 'B'; LDVR >= 1 otherwise.
*
*  MM      (input) INTEGER
*          The number of columns in the arrays VL and/or VR. MM >= M.
*
*  M       (output) INTEGER
*          The number of columns in the arrays VL and/or VR required to
*          store the eigenvectors; each selected real eigenvector
*          occupies one column and each selected complex eigenvector
*          occupies two columns.
*
*  WORK    (workspace) DOUBLE PRECISION array, dimension ((N+2)*N)
*
*  IFAILL  (output) INTEGER array, dimension (MM)
*          If SIDE = 'L' or 'B', IFAILL(i) = j > 0 if the left
*          eigenvector in the i-th column of VL (corresponding to the
*          eigenvalue w(j)) failed to converge; IFAILL(i) = 0 if the
*          eigenvector converged satisfactorily. If the i-th and (i+1)th
*          columns of VL hold a complex eigenvector, then IFAILL(i) and
*          IFAILL(i+1) are set to the same value.
*          If SIDE = 'R', IFAILL is not referenced.
*
*  IFAILR  (output) INTEGER array, dimension (MM)
*          If SIDE = 'R' or 'B', IFAILR(i) = j > 0 if the right
*          eigenvector in the i-th column of VR (corresponding to the
*          eigenvalue w(j)) failed to converge; IFAILR(i) = 0 if the
*          eigenvector converged satisfactorily. If the i-th and (i+1)th
*          columns of VR hold a complex eigenvector, then IFAILR(i) and
*          IFAILR(i+1) are set to the same value.
*          If SIDE = 'L', IFAILR is not referenced.
*
*  INFO    (output) INTEGER
*          = 0:  successful exit
*          < 0:  if INFO = -i, the i-th argument had an illegal value
*          > 0:  if INFO = i, i is the number of eigenvectors which
*                failed to converge; see IFAILL and IFAILR for further
*                details.
*
*  Further Details
*  ===============
*
*  Each eigenvector is normalized so that the element of largest
*  magnitude has magnitude 1; here the magnitude of a complex number
*  (x,y) is taken to be |x|+|y|.
*
*  =====================================================================
*
*     .. Parameters ..
      DOUBLE PRECISION   ZERO, ONE
      PARAMETER          ( ZERO = 0.0D+0, ONE = 1.0D+0 )
*     ..
*     .. Local Scalars ..
      LOGICAL            BOTHV, FROMQR, LEFTV, NOINIT, PAIR, RIGHTV
      INTEGER            I, IINFO, K, KL, KLN, KR, KSI, KSR, LDWORK
      DOUBLE PRECISION   BIGNUM, EPS3, HNORM, SMLNUM, ULP, UNFL, WKI,
     $                   WKR
*     ..
*     .. External Functions ..
      LOGICAL            LSAME
      DOUBLE PRECISION   DLAMCH, DLANHS
      EXTERNAL           LSAME, DLAMCH, DLANHS
*     ..
*     .. External Subroutines ..
      EXTERNAL           DLAEIN, XERBLA
*     ..
*     .. Intrinsic Functions ..
      INTRINSIC          ABS, MAX
*     ..
*     .. Executable Statements ..
*
*     Decode and test the input parameters.
*
      BOTHV = LSAME( SIDE, 'B' )
      RIGHTV = LSAME( SIDE, 'R' ) .OR. BOTHV
      LEFTV = LSAME( SIDE, 'L' ) .OR. BOTHV
*
      FROMQR = LSAME( EIGSRC, 'Q' )
*
      NOINIT = LSAME( INITV, 'N' )
*
*     Set M to the number of columns required to store the selected
*     eigenvectors, and standardize the array SELECT.
*
      M = 0
      PAIR = .FALSE.
      DO 10 K = 1, N
         IF( PAIR ) THEN
            PAIR = .FALSE.
            SELECT( K ) = .FALSE.
         ELSE
            IF( WI( K ).EQ.ZERO ) THEN
               IF( SELECT( K ) )
     $            M = M + 1
            ELSE
               PAIR = .TRUE.
               IF( SELECT( K ) .OR. SELECT( K+1 ) ) THEN
                  SELECT( K ) = .TRUE.
                  M = M + 2
               END IF
            END IF
         END IF
   10 CONTINUE
*
      INFO = 0
      IF( .NOT.RIGHTV .AND. .NOT.LEFTV ) THEN
         INFO = -1
      ELSE IF( .NOT.FROMQR .AND. .NOT.LSAME( EIGSRC, 'N' ) ) THEN
         INFO = -2
      ELSE IF( .NOT.NOINIT .AND. .NOT.LSAME( INITV, 'U' ) ) THEN
         INFO = -3
      ELSE IF( N.LT.0 ) THEN
         INFO = -5
      ELSE IF( LDH.LT.MAX( 1, N ) ) THEN
         INFO = -7
      ELSE IF( LDVL.LT.1 .OR. ( LEFTV .AND. LDVL.LT.N ) ) THEN
         INFO = -11
      ELSE IF( LDVR.LT.1 .OR. ( RIGHTV .AND. LDVR.LT.N ) ) THEN
         INFO = -13
      ELSE IF( MM.LT.M ) THEN
         INFO = -14
      END IF
      IF( INFO.NE.0 ) THEN
         CALL XERBLA( 'DHSEIN', -INFO )
         RETURN
      END IF
*
*     Quick return if possible.
*
      IF( N.EQ.0 )
     $   RETURN
*
*     Set machine-dependent constants.
*
      UNFL = DLAMCH( 'Safe minimum' )
      ULP = DLAMCH( 'Precision' )
      SMLNUM = UNFL*( N / ULP )
      BIGNUM = ( ONE-ULP ) / SMLNUM
*
      LDWORK = N + 1
*
      KL = 1
      KLN = 0
      IF( FROMQR ) THEN
         KR = 0
      ELSE
         KR = N
      END IF
      KSR = 1
*
      DO 120 K = 1, N
         IF( SELECT( K ) ) THEN
*
*           Compute eigenvector(s) corresponding to W(K).
*
            IF( FROMQR ) THEN
*
*              If affiliation of eigenvalues is known, check whether
*              the matrix splits.
*
*              Determine KL and KR such that 1 <= KL <= K <= KR <= N
*              and H(KL,KL-1) and H(KR+1,KR) are zero (or KL = 1 or
*              KR = N).
*
*              Then inverse iteration can be performed with the
*              submatrix H(KL:N,KL:N) for a left eigenvector, and with
*              the submatrix H(1:KR,1:KR) for a right eigenvector.
*
               DO 20 I = K, KL + 1, -1
                  IF( H( I, I-1 ).EQ.ZERO )
     $               GO TO 30
   20          CONTINUE
   30          CONTINUE
               KL = I
               IF( K.GT.KR ) THEN
                  DO 40 I = K, N - 1
                     IF( H( I+1, I ).EQ.ZERO )
     $                  GO TO 50
   40             CONTINUE
   50             CONTINUE
                  KR = I
               END IF
            END IF
*
            IF( KL.NE.KLN ) THEN
               KLN = KL
*
*              Compute infinity-norm of submatrix H(KL:KR,KL:KR) if it
*              has not ben computed before.
*
               HNORM = DLANHS( 'I', KR-KL+1, H( KL, KL ), LDH, WORK )
               IF( HNORM.GT.ZERO ) THEN
                  EPS3 = HNORM*ULP
               ELSE
                  EPS3 = SMLNUM
               END IF
            END IF
*
*           Perturb eigenvalue if it is close to any previous
*           selected eigenvalues affiliated to the submatrix
*           H(KL:KR,KL:KR). Close roots are modified by EPS3.
*
            WKR = WR( K )
            WKI = WI( K )
   60       CONTINUE
            DO 70 I = K - 1, KL, -1
               IF( SELECT( I ) .AND. ABS( WR( I )-WKR )+
     $             ABS( WI( I )-WKI ).LT.EPS3 ) THEN
                  WKR = WKR + EPS3
                  GO TO 60
               END IF
   70       CONTINUE
            WR( K ) = WKR
*
            PAIR = WKI.NE.ZERO
            IF( PAIR ) THEN
               KSI = KSR + 1
            ELSE
               KSI = KSR
            END IF
            IF( LEFTV ) THEN
*
*              Compute left eigenvector.
*
               CALL DLAEIN( .FALSE., NOINIT, N-KL+1, H( KL, KL ), LDH,
     $                      WKR, WKI, VL( KL, KSR ), VL( KL, KSI ),
     $                      WORK, LDWORK, WORK( N*N+N+1 ), EPS3, SMLNUM,
     $                      BIGNUM, IINFO )
               IF( IINFO.GT.0 ) THEN
                  IF( PAIR ) THEN
                     INFO = INFO + 2
                  ELSE
                     INFO = INFO + 1
                  END IF
                  IFAILL( KSR ) = K
                  IFAILL( KSI ) = K
               ELSE
                  IFAILL( KSR ) = 0
                  IFAILL( KSI ) = 0
               END IF
               DO 80 I = 1, KL - 1
                  VL( I, KSR ) = ZERO
   80          CONTINUE
               IF( PAIR ) THEN
                  DO 90 I = 1, KL - 1
                     VL( I, KSI ) = ZERO
   90             CONTINUE
               END IF
            END IF
            IF( RIGHTV ) THEN
*
*              Compute right eigenvector.
*
               CALL DLAEIN( .TRUE., NOINIT, KR, H, LDH, WKR, WKI,
     $                      VR( 1, KSR ), VR( 1, KSI ), WORK, LDWORK,
     $                      WORK( N*N+N+1 ), EPS3, SMLNUM, BIGNUM,
     $                      IINFO )
               IF( IINFO.GT.0 ) THEN
                  IF( PAIR ) THEN
                     INFO = INFO + 2
                  ELSE
                     INFO = INFO + 1
                  END IF
                  IFAILR( KSR ) = K
                  IFAILR( KSI ) = K
               ELSE
                  IFAILR( KSR ) = 0
                  IFAILR( KSI ) = 0
               END IF
               DO 100 I = KR + 1, N
                  VR( I, KSR ) = ZERO
  100          CONTINUE
               IF( PAIR ) THEN
                  DO 110 I = KR + 1, N
                     VR( I, KSI ) = ZERO
  110             CONTINUE
               END IF
            END IF
*
            IF( PAIR ) THEN
               KSR = KSR + 2
            ELSE
               KSR = KSR + 1
            END IF
         END IF
  120 CONTINUE
*
      RETURN
*
*     End of DHSEIN
*
      END


! program to use the Euler's method in fortran

program euler
    implicit none

    real, DIMENSION(6) :: x, y, df
    integer :: i,j
    real, parameter :: h = 0.1

    ! define the input values
    do i = 1,6
        x(i) = 0 + (i * h)
    end do

    y(1) = 1 ! initialised value of y

    ! make the algorithm
    do i = 2,6
        y(i) = df(x(i-1),y(i-1))
    end do

    WRITE(*,*) x,y

end program euler

function df(x,y)
    implicit none
    real, INTENT(IN) :: x(:), y(:)
    real :: df(size(x))

    df = y - x
end function df


c-------------------------------------------------------------------------
      subroutine qthermal

C     Compute the thermal divergence QTL
C
C     QTL := div(v) = -1/rho * Drho/Dt
c
c     If we use the ideal gas law and assume
c     that p,R is const we end up with
c     QTL = 1/(rho*cp) rho*cp*DT/Dt
C
C     where rho*cp*DT/Dt represents the RHS of the
C     energy equation expressed in terms of temperature.

      include 'SIZE'
      include 'TOTAL'

      COMMON /SCRNS/ w1(LX1,LY1,LZ1,LELT)
     $              ,w2(LX1,LY1,LZ1,LELT)
     $              ,tx(LX1,LY1,LZ1,LELT)
     $              ,ty(LX1,LY1,LZ1,LELT)
     $              ,tz(LX1,LY1,LZ1,LELT)

      ntot = nx1*ny1*nz1*nelv

      if (.not.iflomach) then
         call rzero(qtl,ntot)
         return
      endif

      ifld_save = ifield

c - - Assemble RHS of T-eqn
      ifield=2
      call setqvol (QTL) ! volumetric heating source
      call col2    (QTL,BM1,ntot)

      ifield=1     !set right gs handle (QTL is only defined on the velocity mesh)
      call opgrad  (tx,ty,tz,T)
      call opdssum (tx,ty,tz)
      call opcolv  (tx,ty,tz,binvm1)
      call opcolv  (tx,ty,tz,vdiff(1,1,1,1,2))
      call opdiv   (w2,tx,ty,tz)

      call add2    (QTL,w2,ntot)

      call col3    (w2,vtrans(1,1,1,1,2),T,ntot)
      call invcol2 (QTL,w2,ntot)

      call dssum   (QTL,nx1,ny1,nz1)
      call col2    (QTL,binvm1,ntot)

      ifield = ifld_save

      return
      end


c GENINIT_J3 reads Jacobi orbital elements of nbod planets and generates
c initial position and velocity in Jacobi coords.
c This version outputs rpl and rhill.

c Last modified by Man Hoi Lee, Aug 16, 2003.

      include 'swift.inc'

      real*8 SMASSYR,MSUN,AU
      parameter (SMASSYR=TWOPI*TWOPI)
      parameter (MSUN=1.989d33, AU=1.49597892d13)

      integer nbod
      real*8 mass(NTPMAX),j2rp2,j4rp4
      real*8 xj(NTPMAX),yj(NTPMAX),zj(NTPMAX)
      real*8 vxj(NTPMAX),vyj(NTPMAX),vzj(NTPMAX)
      real*8 rpl(NTPMAX),rhill(NTPMAX)
      integer iflgchk
      logical*2 lclose

      real*8 mstar0,mpl0,frho3

      real*8 gm,a,ecc,inc,capom,omega,capm

      integer ialpha,i,iuflg

      character*80 outfile

 100  write (*,*) ' Units Menu:'
      write (*,*) '       0 ==> Solar masses, and AU'
      write (*,*) '       1 ==> AU, and Years'
      read (*,*) iuflg
      if ((iuflg.ne.0).and.(iuflg.ne.1)) goto 100
      write (*,*) ' Mass of central star (in solar mass):'
      read (*,*) mstar0
      write (*,*) ' Number of planets:'
      read (*,*) nbod
      write (*,*) ' f/rho^(1/3) for radius (rho in g/cm^3):'
      read (*,*) frho3
      write (*,*) ' Enter name of data file for symba5_j:'
      read (*,201) outfile
 201  format (a)

      lclose = .true.
      iflgchk = 0
      nbod = nbod + 1
      if (iuflg.eq.0) then
          mass(1) = mstar0
      else
          mass(1) = mstar0*SMASSYR/(365.25*365.25)
      endif
      j2rp2 = 0.d0
      j4rp4 = 0.d0
      xj(1) = 0.d0
      yj(1) = 0.d0
      zj(1) = 0.d0
      vxj(1) = 0.d0
      vyj(1) = 0.d0
      vzj(1) = 0.d0

      open (unit=12,file='geninit_j.out')
      write (12,*) ' m     a     e     i     omega capom M     rpl   rh'

      gm = mass(1)

      do 300 i=2,nbod
          write (*,*) ' Mass of planet',i-1,' (in solar mass):'
          read (*,*) mpl0
          if (iuflg.eq.0) then
              mass(i) = mpl0
          else
              mass(i) = mpl0*SMASSYR/(365.25*365.25)
          endif
          rpl(i) = frho3*(1.5d0*mpl0*MSUN/TWOPI)**0.3333333333d0/AU

          ialpha = -1
          gm = gm + mass(i)

          write (*,*)
     &        ' a (AU), e, i (deg), omega (deg), capom (deg), M (deg):'
          read (*,*) a,ecc,inc,omega,capom,capm

          inc = inc/DEGRAD
          capom = capom/DEGRAD
          omega = omega/DEGRAD
          capm = capm/DEGRAD

          rhill(i) = a*(mass(i)/(3.d0*mass(1)))**0.3333333333d0

          write (12,251) mass(i),a,ecc,inc,omega,capom,capm,
     &                   rpl(i),rhill(i)
 251      format (1p9e12.4)

          call ORBEL_EL2XV (gm,ialpha,a,ecc,inc,capom,omega,capm,
     &                      xj(i),yj(i),zj(i),vxj(i),vyj(i),vzj(i))
 300  continue

      close (unit=12)

      call IO_DUMP_PL_SYMBA (outfile,nbod,mass,xj,yj,zj,vxj,vyj,vzj,
     &                       lclose,iflgchk,rpl,rhill,j2rp2,j4rp4)

      stop
      end


      SUBROUTINE PLOAD
C
      INTEGER         NAME(2),GRIDP,PONT
      DIMENSION       GRIDP(5),IGPCO(4,4),GPCO1(3),GPCO2(3),GPCO3(3),
     1                PONT(4),IORD(4),VECT(3),VECT1(3),VECT2(3),
     2                PLOADS(3,4),GPCO4(3),VECT3(3)
      COMMON /LOADX / LCORE,SLT,BGPDT,OLD,CSTM,NN(11),NOBLD
      COMMON /SYSTEM/ KSYS(87),KSYS88
      COMMON /ZZZZZZ/ CORE(1)
      EQUIVALENCE     (PMAG,GRIDP(1)),
     1                (IGPCO(2,1),GPCO1(1)),(IGPCO(2,2),GPCO2(1)),
     2                (IGPCO(2,3),GPCO3(1)),(IGPCO(2,4),GPCO4(1))
      DATA    NAME  / 4HPLOA,4HD   /, PI / 3.141592654 /
C
C
      DO 10 I = 1,3
   10 PLOADS(I,4) = 0.0
      CALL READ (*150,*150,SLT,GRIDP(1),5,0,FLAG)
      PONT(1) = GRIDP(2)
      PONT(2) = GRIDP(3)
      PONT(3) = GRIDP(4)
      PONT(4) = GRIDP(5)
      N1 = 4
      IF (GRIDP(5) .EQ. 0) N1 = 3
      CALL PERMUT (PONT(1),IORD(1),N1,OLD)
      DO 20 I = 1,N1
      L = IORD(I)
   20 CALL FNDPNT (IGPCO(1,L),PONT(L))
      IF (N1 .EQ. 4) GO TO 160
C
C     THREE  POINTS
C
      DO 30 I = 1,3
      VECT3(I) = GPCO1(I) - GPCO2(I)
      VECT2(I) = GPCO3(I) - GPCO1(I)
   30 VECT1(I) = GPCO2(I) - GPCO3(I)
      CALL CROSS (VECT3(1),VECT1(1),VECT(1))
C
      DO 40 I = 1,3
      DO 40 J = 1,3
   40 PLOADS(J,I) = -VECT(J)
C
      IF (KSYS88 .EQ. 1) GO TO 50
C
C     KSYS88 = 0, PRESSURE LOAD IS DISTRIBUTED EVENLY (ONE-THIRD) TO
C     EACH OF THE 3 GRID POINTS. TRIANGULAR GEOMETRY IS NOT CONSIDERED.
C
      PMAG    = PMAG/6
      VECT(1) = PMAG
      VECT(2) = PMAG
      VECT(3) = PMAG
      GO TO 80
C
C     IMPLEMENTED BY G.CHAN/UNISYS   3/1990
C     KSYS88 = 1, PRESSURE LOAD IS DISTRIBUTED PROPORTIONALLY TO THE
C     THREE ANGLE SIZES.
C     E.G. A 45-90-45 DEGREE TRIANGLE ELEMENT WILL HAVE TWICE THE LOAD
C     AT THE 90 DEGREE ANGLE TO THAT OF THE 45 DEGREE ANGLE.
C     RECTANGULAR ELEMENT (4 POINTS) IS NOT AFFECTED
C
C     GET AREA(2X), SIDES (VI) AND ANGLES (AI) OF THE TRIANGLE
C
   50 CONTINUE
      AREA = SQRT(VECT (1)**2 + VECT (2)**2 + VECT (3)**2)
      V1   = SQRT(VECT1(1)**2 + VECT1(2)**2 + VECT1(3)**2)
      V2   = SQRT(VECT2(1)**2 + VECT2(2)**2 + VECT2(3)**2)
      V3   = SQRT(VECT3(1)**2 + VECT3(2)**2 + VECT3(3)**2)
C
C     CHOOSE AN ANGLE, WHICH IS NOT THE LARGEST, TO START COMPUTING
C     THE THREE ANGLES
C
      IF (V2.GT.V1 .AND. V2.GT.V3) GO TO 60
      SIN2 = AREA/(V3*V1)
      SIN1 = V1*SIN2/V2
      SIN3 = V3*SIN2/V2
      A2   = ASIN(SIN2)
      IF (SIN1 .GE. 0.0) A1 = ASIN(SIN1)
      IF (SIN3 .GE. 0.0) A3 = ASIN(SIN3)
      IF (V1 .GT. V3) A1 = PI - A2 - A3
      IF (V3 .GT. V1) A3 = PI - A2 - A1
      GO TO 70
C
   60 SIN3 = AREA/(V2*V1)
      SIN2 = V2*SIN3/V3
      SIN1 = V1*SIN3/V3
      A3   = ASIN(SIN3)
      IF (SIN2 .GE. 0.0) A2 = ASIN(SIN2)
      IF (SIN1 .GE. 0.0) A1 = ASIN(SIN1)
      IF (V1 .GT. V2) A1 = PI - A3 - A2
      IF (V2 .GT. V1) A2 = PI - A3 - A1
   70 PMAG    = 0.5*PMAG/PI
      VECT(1) = PMAG*A1
      VECT(2) = PMAG*A2
      VECT(3) = PMAG*A3
C
C     TRANSFORM TO GLOBAL AND ADD CONTRIBUTIONS
C
   80 DO 130 I = 1,N1
      DO 90  J = 1,3
      IF (N1 .EQ. 4) PLOADS(J,I) = -PLOADS(J,I)*PMAG
      IF (N1 .EQ. 3) PLOADS(J,I) = -PLOADS(J,I)*VECT(I)
   90 CONTINUE
      IF (IGPCO(1,I) .NE. 0) CALL BASGLB (PLOADS(1,I),PLOADS(1,I),
     1                                    IGPCO(2,I),IGPCO(1,I))
      CALL FNDSIL (PONT(I))
      DO 120 J = 1,3
      IN = PONT(I) + J - 1
      CORE(IN) = PLOADS(J,I) + CORE(IN)
  120 CONTINUE
  130 CONTINUE
  140 RETURN
C
  150 CALL MESAGE (-1,SLT,NAME)
      GO TO 140
C
C     FOUR  POINTS
C
C
C     TRIANGLE  1,2,3
C
  160 DO 170 I = 1,3
      VECT1(I) = GPCO1(I) - GPCO2(I)
  170 VECT2(I) = GPCO3(I) - GPCO2(I)
      CALL CROSS (VECT1(1),VECT2(1),VECT(1))
      DO 180 I = 1,3
      DO 180 J = 1,3
  180 PLOADS(J,I) = VECT(J)
C
C     TRIANGLE  2,3,4
C
      DO 190 I  =1,3
      VECT1(I) = GPCO2(I) - GPCO3(I)
  190 VECT2(I) = GPCO4(I) - GPCO3(I)
      CALL CROSS (VECT1(1),VECT2(1),VECT(1))
      DO 200 I = 2,4
      DO 200 J = 1,3
  200 PLOADS(J,I) = PLOADS(J,I) + VECT(J)
C
C     TRIANGLE  3,1,4
C
      DO 210 I = 1,3
      VECT1(I) = GPCO4(I) - GPCO1(I)
  210 VECT2(I) = GPCO3(I) - GPCO1(I)
      CALL CROSS (VECT1(1),VECT2(1),VECT(1))
      DO 230 I = 1,4
      IF (I .EQ. 2) GO TO 230
      DO 220 J = 1,3
  220 PLOADS(J,I) = PLOADS(J,I)+VECT(J)
  230 CONTINUE
C
C     TRIANGLE (4,1,2)
C
      DO 240 I = 1,3
      VECT1(I) = GPCO4(I) - GPCO1(I)
  240 VECT2(I) = GPCO2(I) - GPCO1(I)
      CALL CROSS (VECT1(1),VECT2(1),VECT(1))
      DO 260 I = 1,4
      IF (I .EQ. 3) GO TO 260
      DO 250 J = 1,3
  250 PLOADS(J,I) = PLOADS(J,I) + VECT(J)
  260 CONTINUE
      PMAG = PMAG/12.0
      GO TO 80
      END


SUBROUTINE caldat(ijulian,dd,mm,yyyy)
!
IMPLICIT NONE
!
INCLUDE 'm1sim.inc'
!
! PARAMETER definitions
!
INTEGER,PARAMETER :: igreg = 2299161
!
! Subroutine arguments
!
INTEGER :: dd,ijulian,mm,yyyy
!
! Local variables
!
REAL :: alpha,c,e
REAL*8 :: dble
INTEGER :: int
INTEGER :: ja,jalpha,jb,jc,jd,je,julian
!
!     + + + purpose + + +
!     inverse of the function julday. here 'julian' is input as a julian day
!     number, and the routine outputs the dd, mm, and yyyy on which the
!     specified julian day started at noon.
!
!     + + + keywords + + +
!     date, utility
!
!     + + + argument declarations + + +
!

!     + + + argument definitions + + +
!     mm     - integer value of mm in the range 1-12
!     dd     -                  dd in the range 1-31
!     yyyy   -                  yyyy (negative a.d., positive b.c.)
!     ijulian - julian day value (This is the value passed into Caldat.
!               Definition taken from Lstday)
!     julian - integer value equal to julian day number
!
!     + + + parameters + + +
!     gregorian calendar was adopted on oct. 15, 1582.
!     igreg - parameter

!     + + + parameter definitions + + +

!     + + + local variables + + +

!     + + + local variable definitions + + +
!     alpha -
!     c -
!     dble - double
!     e -
!     int -
!     ja -
!     jalpha -
!     jb -
!     jc -
!     jd -
!     je -

!     + + + common block variable definitions + + +
!     am0jd - current julian day of simulation

!     + + + end specifications + + +
!

!     if the date is -1 then use the simulation date.
julian = ijulian
IF (julian.EQ.-1) julian = am0jd

IF (julian.GE.igreg) THEN
  alpha = (dble(julian-1867216)-dble(0.25))/dble(36524.25)
  jalpha = int(alpha)
  ja = julian + 1 + jalpha - int(dble(0.25)*jalpha)
ELSE
  ja = julian
END IF
jb = ja + 1524
c = dble(6680.0) + ((jb-2439870)-dble(122.1))/dble(365.25)
!c = dble(6680.0) + ((jb-2439870)-dble(122.1))/dble(365.00)!de changed
jc = int(c)
jd = 365*jc + int(dble(0.25)*jc)
e = (jb-jd)/dble(30.6001)
je = int(e)
dd = jb - jd - int(dble(30.6001)*je)
mm = je - 1
IF (mm.GT.12) mm = mm - 12
yyyy = jc - 4715
IF (mm.GT.2) yyyy = yyyy - 1
IF (yyyy.LE.0) yyyy = yyyy - 1
!
END SUBROUTINE caldat

Project Documents/Source Code Original/Caldat.f90


c
c
c =========================================================
      subroutine rp1(maxmx,meqn,mwaves,mbc,mx,ql,qr,auxl,auxr,
     &		 fwave,s,amdq,apdq)
c =========================================================
c
c     # solve Riemann problems for the traffic equation.
c     # with variable speed limit umax stored in aux(i,1)
c
c     # returns fwave's instead of waves
c
c     # On input, ql contains the state vector at the left edge of each cell
c     #           qr contains the state vector at the right edge of each cell
c     # On output, wave contains the waves,
c     #            s the speeds,
c     #            amdq the  left-going flux difference  A^- \Delta q
c     #            apdq the right-going flux difference  A^+ \Delta q
c
c     # Note that the i'th Riemann problem has left state qr(i-1,:)
c     #                                    and right state ql(i,:)
c     # From the basic clawpack routine step1, rp is called with ql = qr = q.
c
c
      implicit double precision (a-h,o-z)
      dimension   ql(1-mbc:maxmx+mbc, meqn)
      dimension   qr(1-mbc:maxmx+mbc, meqn)
      dimension    s(1-mbc:maxmx+mbc, mwaves)
      dimension fwave(1-mbc:maxmx+mbc, meqn, mwaves)
      dimension amdq(1-mbc:maxmx+mbc, meqn)
      dimension apdq(1-mbc:maxmx+mbc, meqn)
      dimension  auxl(1-mbc:maxmx+mbc, 1)
      dimension  auxr(1-mbc:maxmx+mbc, 1)
      common /comlxf/ alxf
c
c
c
      do 30 i=2-mbc,mx+mbc
c
c        # Compute the fwave and speed, and fluctuations
c
c        # compute flux in each cell and flux difference:
         fim1 = auxl(i-1,1)*qr(i-1,1)*(1.d0 - qr(i-1,1))
         fi = auxl(i,1)*ql(i,1)*(1.d0 - ql(i,1))
         fwave(i,1,1) = fi - fim1

c        # compute characteristic speed in each cell:
	 sim1 = auxl(i-1,1)*(1.d0 - 2.d0*ql(i-1,1))
	 si = auxl(i,1)*(1.d0 - 2.d0*ql(i,1))

         if (sim1 .lt. 0.d0 .and. si .le. 0.d0) then
c             # left-going
              s(i,1) = sim1
	      amdq(i,1) = fwave(i,1,1)
	      apdq(i,1) = 0.d0
            else if (sim1 .ge. 0.d0 .and. si .gt. 0.d0) then
c             # right-going
              s(i,1) = si
	      amdq(i,1) = 0.d0
	      apdq(i,1) = fwave(i,1,1)
            else if (sim1 .lt. 0.d0 .and. si .gt. 0.d0) then
c             # transonic rarefaction
c             # split fwave between amdq and apdq:
              s(i,1) = 0.5d0*(sim1 + si)
              dq = ql(i,1) - qr(i-1,1)

c             # entropy fix:  (perhaps doesn't work for all cases!!!)
c             # This assumes the flux in the transonic case should
c             # correspond to q=0.5 on the side with the smaller umax value.
              f0 = dmin1(auxl(i-1,1),auxl(i,1))*0.25d0
              amdq(i,1) = f0 - fim1
              apdq(i,1) = fi - f0

            else
c             # transonic shock
              s(i,1) = 0.5d0*(sim1 + si)
              if (s(i,1) .lt. 0.d0) then
                   amdq(i,1) = fwave(i,1,1)
                   apdq(i,1) = 0.d0
                else if (s(i,1) .gt. 0.d0) then
                   amdq(i,1) = 0.d0
                   apdq(i,1) = fwave(i,1,1)
                else
	           amdq(i,1) = 0.5d0 * fwave(i,1,1)
	           apdq(i,1) = 0.5d0 * fwave(i,1,1)
                endif
            endif
c
   30    continue
c
      return
      end


c
C
C
      SUBROUTINE SIGINI(SIGMA,COORDS,NTENS,NCRDS,NOEL,NPT,LAYER,
     1 KSPT,LREBAR,NAMES)
C
      INCLUDE 'ABA_PARAM.INC'
C
      DIMENSION SIGMA(NTENS),COORDS(NCRDS)
      CHARACTER NAMES(2)*80
C
      D=coords(2)
      h=-D
c
      If (NAMES(2) =='CAX8P') then
        ratio=0.440116284
        porep = ((1000*h*9.81)/1000)+10100
        Total=((1750*h*10)/1000)+10100
            sigma(2) = -(Total - porep)
            sigma(1) = sigma(2) * ratio
            sigma(3) = sigma(2) * ratio
      end if
      If (NAMES(2) =='CAX8') then
      ratio = 1.0
      IF(COORDS(1)>0.1122.and.COORDS(1)<0.122238.and.
     1 COORDS(2)>-97)THEN
                 Total=((8000*h*9.81)/1000)+10100
            sigma(2) =-(Total)
            sigma(1) = sigma(2)*ratio
            sigma(3) = sigma(2)*ratio
       else
            Total=((1240*h*9.81)/1000)+10100
            sigma(2) = -(Total)
            sigma(1) = sigma(2)*ratio
            sigma(3) = sigma(2)*ratio
      End if
      End if
C
      return
      end
C
C
       SUBROUTINE UFIELD(FIELD,KFIELD,NSECPT,KSTEP,KINC,TIME,NODE,
     1  COORDS,TEMP,DTEMP)
        INCLUDE 'ABA_PARAM.INC'
        DIMENSION FIELD(NSECPT),TIME(2),COORDS(3),TEMP(NSECPT),
     1  DTEMP(NSECPT)
        KFIELD=1
        FIELD(1)=-COORDS(2)
        RETURN
        END
C
      subroutine uporep(uw0,coords,node)
C
      include 'aba_param.inc'
C
      dimension coords(3)
      COMMON /READ/ IRDR7
      COMMON /GRID/ XR7(897),YR7(897),SATR7(897)
C
      D=coords(2)
      h=-D
      uw0 =((1000* h*9.81)/1000)+10100
C
      return
      end
C
C
      subroutine disp(u,kstep,kinc,time,node,noel,jdof,coords)
c
      include 'aba_param.inc'
c
C      common/kycommonT/HWT
      common/Kycommon/HW8,HWP
      dimension u(3),time(2),coords(3)
           IF (kstep<7)then
      D=-coords(2)
      U(1) =((1000* D*9.81)/1000)+10100
      END IF
      h=-coords(2)
      IF (kstep==7)then
      IF(COORDS(2)>-150.and.COORDS(2)<-99.9)THEN
         if (COORDS(1)<3) then
          U(1) =((1000* h*9.81)/1000)+10000-1000*time(1)
                         else
         U(1) =((1000* h*9.81)/1000)+10100
         end if
               else
         U(1) =((1000* h*9.81)/1000)+10100
      end if
      end if
      IF (kstep==8)then
      IF(COORDS(2)>-150.and.COORDS(2)<-99.9)THEN
         if (COORDS(1)<3) then
          U(1) =((1000* h*9.81)/1000)+9000-1000*time(1)
         else if (COORDS(1)>3.and.COORDS(1)<6) then
          U(1) =((1000* h*9.81)/1000)+10000-1000*time(1)
          else
         U(1) =((1000* h*9.81)/1000)+10100
         end if
                  else
         U(1) =((1000* h*9.81)/1000)+10100
      end if
      end if
      IF (kstep==9)then
      IF(COORDS(2)>-150.and.COORDS(2)<-99.9)THEN
         if (COORDS(1)<3) then
          U(1) =((1000* h*9.81)/1000)+8000-1000*time(1)
          else if(COORDS(1)>3.and.COORDS(1)<6) then
          U(1) =((1000* h*9.81)/1000)+9000-1000*time(1)
          else if (COORDS(1)>6.and.COORDS(1)<9) then
          U(1) =((1000* h*9.81)/1000)+10000-1000*time(1)
          else
         U(1) =((1000* h*9.81)/1000)+10100
         end if
                  else
         U(1) =((1000* h*9.81)/1000)+10100
      end if
      end if
      IF (kstep==10)then
      IF(COORDS(2)>-150.and.COORDS(2)<-99.9)THEN
         if (COORDS(1)<3) then
          U(1) =((1000* h*9.81)/1000)+7000-1000*time(1)
          else if (COORDS(1)>3.and.COORDS(1)<6) then
          U(1) =((1000* h*9.81)/1000)+8000-1000*time(1)
          else if (COORDS(1)>6.and.COORDS(1)<9) then
          U(1) =((1000* h*9.81)/1000)+9000-1000*time(1)
          else if (COORDS(1)>9.and.COORDS(1)<12) then
          U(1) =((1000* h*9.81)/1000)+10000-1000*time(1)
          else
         U(1) =((1000* h*9.81)/1000)+10100
         end if
                  else
         U(1) =((1000* h*9.81)/1000)+10100
      end if
      end if
      IF (kstep==11)then
      IF(COORDS(2)>-150.and.COORDS(2)<-99.9)THEN
         if (COORDS(1)<3) then
          U(1) =((1000* h*9.81)/1000)+6000-500*time(1)
         else if (COORDS(1)>3.and.COORDS(1)<6) then
          U(1) =((1000* h*9.81)/1000)+7000-500*time(1)
         else if(COORDS(1)>6.and.COORDS(1)<9) then
          U(1) =((1000* h*9.81)/1000)+8000-500*time(1)
         else if(COORDS(1)>9.and.COORDS(1)<12) then
          U(1) =((1000* h*9.81)/1000)+9000-500*time(1)
         else if(COORDS(1)>12.and.COORDS(1)<15) then
          U(1) =((1000* h*9.81)/1000)+10000-500*time(1)
         else
         U(1) =((1000* h*9.81)/1000)+10100
      end if
                        else
         U(1) =((1000* h*9.81)/1000)+10100
      end if
      end if
      IF (kstep==12)then
      IF(COORDS(2)>-150.and.COORDS(2)<-99.9)THEN
         if (COORDS(1)<3) then
          U(1) =((1000* h*9.81)/1000)+5500-500*time(1)
         else if (COORDS(1)>3.and.COORDS(1)<6) then
          U(1) =((1000* h*9.81)/1000)+6500-500*time(1)
         else if(COORDS(1)>6.and.COORDS(1)<9) then
          U(1) =((1000* h*9.81)/1000)+7500-500*time(1)
         else if(COORDS(1)>9.and.COORDS(1)<12) then
          U(1) =((1000* h*9.81)/1000)+8500-500*time(1)
         else if(COORDS(1)>12.and.COORDS(1)<15) then
          U(1) =((1000* h*9.81)/1000)+9500-500*time(1)
         else
         U(1) =((1000* h*9.81)/1000)+10100
      end if
                        else
         U(1) =((1000* h*9.81)/1000)+10100
      end if
      end if
       IF (kstep==13)then
       IF(COORDS(2)>-150.and.COORDS(2)<-99.9)THEN
         if (COORDS(1)<3) then
          U(1) =((1000* h*9.81)/1000)+5000-500*time(1)
         else if (COORDS(1)>3.and.COORDS(1)<6) then
          U(1) =((1000* h*9.81)/1000)+6000-500*time(1)
         else if (COORDS(1)>6.and.COORDS(1)<9) then
          U(1) =((1000* h*9.81)/1000)+7000-500*time(1)
         else if (COORDS(1)>9.and.COORDS(1)<12) then
          U(1) =((1000* h*9.81)/1000)+8000-500*time(1)
         else if (COORDS(1)>12.and.COORDS(1)<15) then
          U(1) =((1000* h*9.81)/1000)+9000-500*time(1)
         else if (COORDS(1)>15.and.COORDS(1)<18) then
          U(1) =((1000* h*9.81)/1000)+10000-500*time(1)
         else
         U(1) =((1000* h*9.81)/1000)+10100
         end if
                  else
         U(1) =((1000* h*9.81)/1000)+10100
      end if
      end if
      IF (kstep==14)then
      IF(COORDS(2)>-150.and.COORDS(2)<-99.9)THEN
         if (COORDS(1)<3) then
          U(1) =((1000* h*9.81)/1000)+4500-500*time(1)
         else if (COORDS(1)>3.and.COORDS(1)<6) then
          U(1) =((1000* h*9.81)/1000)+5500-500*time(1)
         else if (COORDS(1)>6.and.COORDS(1)<9) then
          U(1) =((1000* h*9.81)/1000)+6500-500*time(1)
         else if (COORDS(1)>9.and.COORDS(1)<12) then
          U(1) =((1000* h*9.81)/1000)+7500-500*time(1)
         else if (COORDS(1)>12.and.COORDS(1)<15) then
          U(1) =((1000* h*9.81)/1000)+8500-500*time(1)
         else if (COORDS(1)>15.and.COORDS(1)<18) then
          U(1) =((1000* h*9.81)/1000)+9500-500*time(1)
         else
         U(1) =((1000* h*9.81)/1000)+10100
         end if
                  else
         U(1) =((1000* h*9.81)/1000)+10100
      end if
      end if
      IF (kstep==15)then
      IF(COORDS(2)>-150.and.COORDS(2)<-99.9)THEN
         if (COORDS(1)<3) then
          U(1) =((1000* h*9.81)/1000)+4000-500*time(1)
         else if (COORDS(1)>3.and.COORDS(1)<6) then
          U(1) =((1000* h*9.81)/1000)+5000-500*time(1)
         else if (COORDS(1)>6.and.COORDS(1)<9) then
          U(1) =((1000* h*9.81)/1000)+6000-500*time(1)
          else if (COORDS(1)>9.and.COORDS(1)<12) then
          U(1) =((1000* h*9.81)/1000)+7000-500*time(1)
         else if (COORDS(1)>12.and.COORDS(1)<15) then
          U(1) =((1000* h*9.81)/1000)+8000-500*time(1)
        else if (COORDS(1)>15.and.COORDS(1)<18) then
          U(1) =((1000* h*9.81)/1000)+9000-500*time(1)
         else if (COORDS(1)>18.and.COORDS(1)<21) then
          U(1) =((1000* h*9.81)/1000)+10000-500*time(1)
         else
         U(1) =((1000* h*9.81)/1000)+10100
         end if
                  else
         U(1) =((1000* h*9.81)/1000)+10100
      end if
      end if
      IF (kstep==16)then
      IF(COORDS(2)>-150.and.COORDS(2)<-99.9)THEN
         if (COORDS(1)<3) then
          U(1) =((1000* h*9.81)/1000)+3500-500*time(1)
         else if (COORDS(1)>3.and.COORDS(1)<6) then
          U(1) =((1000* h*9.81)/1000)+4500-500*time(1)
         else if (COORDS(1)>6.and.COORDS(1)<9) then
          U(1) =((1000* h*9.81)/1000)+5500-500*time(1)
          else if (COORDS(1)>9.and.COORDS(1)<12) then
          U(1) =((1000* h*9.81)/1000)+6500-500*time(1)
         else if (COORDS(1)>12.and.COORDS(1)<15) then
          U(1) =((1000* h*9.81)/1000)+7500-500*time(1)
        else if (COORDS(1)>15.and.COORDS(1)<18) then
          U(1) =((1000* h*9.81)/1000)+8500-500*time(1)
         else if (COORDS(1)>18.and.COORDS(1)<21) then
          U(1) =((1000* h*9.81)/1000)+9500-500*time(1)
          else
         U(1) =((1000* h*9.81)/1000)+10100
         end if
                  else
         U(1) =((1000* h*9.81)/1000)+10100
         end if
      end if
            IF (kstep==17)then
      IF(COORDS(2)>-150.and.COORDS(2)<-99.9)THEN
         if (COORDS(1)<3) then
          U(1) =((1000* h*9.81)/1000)+3000-500*time(1)
         else if (COORDS(1)>3.and.COORDS(1)<6) then
          U(1) =((1000* h*9.81)/1000)+4000-500*time(1)
        else if (COORDS(1)>6.and.COORDS(1)<9) then
          U(1) =((1000* h*9.81)/1000)+5000-500*time(1)
         else if (COORDS(1)>9.and.COORDS(1)<12) then
          U(1) =((1000* h*9.81)/1000)+6000-500*time(1)
         else if (COORDS(1)>12.and.COORDS(1)<15) then
          U(1) =((1000* h*9.81)/1000)+7000-500*time(1)
         else if (COORDS(1)>15.and.COORDS(1)<18) then
          U(1) =((1000* h*9.81)/1000)+8000-500*time(1)
         else if (COORDS(1)>18.and.COORDS(1)<21) then
          U(1) =((1000* h*9.81)/1000)+9000-500*time(1)
         else if (COORDS(1)>21.and.COORDS(1)<24) then
          U(1) =((1000* h*9.81)/1000)+10000-500*time(1)
          else
         U(1) =((1000* h*9.81)/1000)+10100
         end if
                  else
         U(1) =((1000* h*9.81)/1000)+10100
         end if
      end if
                  IF (kstep==18)then
      IF(COORDS(2)>-150.and.COORDS(2)<-99.9)THEN
         if (COORDS(1)<3) then
          U(1) =((1000* h*9.81)/1000)+2500-500*time(1)
         else if (COORDS(1)>3.and.COORDS(1)<6) then
          U(1) =((1000* h*9.81)/1000)+3500-500*time(1)
        else if (COORDS(1)>6.and.COORDS(1)<9) then
          U(1) =((1000* h*9.81)/1000)+4500-500*time(1)
         else if (COORDS(1)>9.and.COORDS(1)<12) then
          U(1) =((1000* h*9.81)/1000)+5500-500*time(1)
         else if (COORDS(1)>12.and.COORDS(1)<15) then
          U(1) =((1000* h*9.81)/1000)+6500-500*time(1)
         else if (COORDS(1)>15.and.COORDS(1)<18) then
          U(1) =((1000* h*9.81)/1000)+7500-500*time(1)
         else if (COORDS(1)>18.and.COORDS(1)<21) then
          U(1) =((1000* h*9.81)/1000)+8500-500*time(1)
         else if (COORDS(1)>21.and.COORDS(1)<24) then
          U(1) =((1000* h*9.81)/1000)+9500-500*time(1)
          else
         U(1) =((1000* h*9.81)/1000)+10100
         end if
                  else
         U(1) =((1000* h*9.81)/1000)+10100
         end if
      end if
      IF (kstep==19)then
      IF(COORDS(2)>-150.and.COORDS(2)<-99.9)THEN
         if (COORDS(1)<3) then
          U(1) =((1000* h*9.81)/1000)+2000-500*time(1)
         else if (COORDS(1)>3.and.COORDS(1)<6) then
          U(1) =((1000* h*9.81)/1000)+3000-500*time(1)
        else if (COORDS(1)>6.and.COORDS(1)<9) then
          U(1) =((1000* h*9.81)/1000)+4000-500*time(1)
         else if (COORDS(1)>9.and.COORDS(1)<12) then
          U(1) =((1000* h*9.81)/1000)+5000-500*time(1)
         else if (COORDS(1)>12.and.COORDS(1)<15) then
          U(1) =((1000* h*9.81)/1000)+6000-500*time(1)
         else if (COORDS(1)>15.and.COORDS(1)<18) then
          U(1) =((1000* h*9.81)/1000)+7000-500*time(1)
         else if (COORDS(1)>18.and.COORDS(1)<21) then
          U(1) =((1000* h*9.81)/1000)+8000-500*time(1)
         else if (COORDS(1)>21.and.COORDS(1)<24) then
          U(1) =((1000* h*9.81)/1000)+9000-500*time(1)
          else if (COORDS(1)>24.and.COORDS(1)<27) then
          U(1) =((1000* h*9.81)/1000)+10000-500*time(1)
          else
         U(1) =((1000* h*9.81)/1000)+10100
         end if
                  else
         U(1) =((1000* h*9.81)/1000)+10100
         end if
      end if
            IF (kstep==20)then
      IF(COORDS(2)>-150.and.COORDS(2)<-99.9)THEN
         if (COORDS(1)<3) then
          U(1) =((1000* h*9.81)/1000)+1500-500*time(1)
         else if (COORDS(1)>3.and.COORDS(1)<6) then
          U(1) =((1000* h*9.81)/1000)+2500-500*time(1)
        else if (COORDS(1)>6.and.COORDS(1)<9) then
          U(1) =((1000* h*9.81)/1000)+3500-500*time(1)
         else if (COORDS(1)>9.and.COORDS(1)<12) then
          U(1) =((1000* h*9.81)/1000)+4500-500*time(1)
         else if (COORDS(1)>12.and.COORDS(1)<15) then
          U(1) =((1000* h*9.81)/1000)+5500-500*time(1)
         else if (COORDS(1)>15.and.COORDS(1)<18) then
          U(1) =((1000* h*9.81)/1000)+6500-500*time(1)
         else if (COORDS(1)>18.and.COORDS(1)<21) then
          U(1) =((1000* h*9.81)/1000)+7500-500*time(1)
         else if (COORDS(1)>21.and.COORDS(1)<24) then
          U(1) =((1000* h*9.81)/1000)+8500-500*time(1)
          else if (COORDS(1)>24.and.COORDS(1)<27) then
          U(1) =((1000* h*9.81)/1000)+9500-500*time(1)
          else
         U(1) =((1000* h*9.81)/1000)+10100
         end if
                  else
         U(1) =((1000* h*9.81)/1000)+10100
         end if
      end if
      	RETURN
	END
c
c
	SUBROUTINE UVARM(UVAR,DIRECT,T,TIME,DTIME,
     +CMNAME,ORNAME,NUVARM,NOEL,NPT,LAYER,KSPT,
     +KSTEP,KINC,NDI,NSHR,COORD,JMAC,JMATYP,
     +MATLAYO,LACCFLA)
      INCLUDE 'ABA_PARAM.INC'
C
      CHARACTER*80 CMNAME,ORNAME
      CHARACTER*3 FLGRAY(15)
      DIMENSION UVAR(NUVARM),DIRECT(3,3),
     +T(3,3),TIME(2)
      DIMENSION ARRAY(15),JARRAY(15),
     +JMAC(*),JMATYP(*),COORD(*)
C
	CALL GETVRM('E',ARRAY,JARRAY,FLGRAY,JRCD,
     +JMAC,JMATYP,MATLAYO,LACCFLA)
C
C
C
      E11 = ARRAY(1)
      E22 = ARRAY(2)
      E33 = ARRAY(3)
C
      UVAR(1)=E11
      UVAR(2)=E22
      UVAR(3)=E33
      UVAR(4)=E11+E22+E33
C
	RETURN
	END
C
c     SDVINI code for Modified Cam Clay
      subroutine sdvini(statev,coords,nstatv,ncrds,noel,npt,layer,kspt)
      include 'aba_param.inc'
      dimension statev(nstatv),coords(ncrds),YR(336), SAT(336)
c
            YO=-COORDS(2)
      statev(1)=0.0       ! initial plastic volumetric strain
      statev(2)=0.54   ! initial void ratio
      If (YO<-10) then
      statev(9)=750
      else
      statev(9)=75*YO
      end if
      statev(3)=1         ! degragation factor
      statev(4)=0         ! initial plastic deviator strain
      statev(89)=0
      statev(90)=0
      statev(91)=0
      statev(92)=0
      statev(93)=0
      statev(94)=0
      statev(112)=0
c
      return
      end
c
cc     UMAT code for Cambridge Methane Hydrate Critical Model
      subroutine umat(stress,statev,DDSDDE,sse,spd,scd,
     1 rpl,ddsddt,drplde,drpldt,
     2 stran,dstran,time,dtime,temp,dtemp,predef,dpred,cmname,
     3 ndi,nshr,ntens,nstatev,props,nprops,coords,drot,pnewdt,
     4 celent,dfgrd0,dfgrd1,noel,npt,layer,kspt,kstep,kinc)
      include 'aba_param.inc'
      character*80 cmname
      dimension stress(ntens),statev(nstatev),
     1 DDSDDE(ntens,ntens),ddsddt(ntens),drplde(ntens),
     2 stran(ntens),dstran(ntens),time(2),predef(1),dpred(1),
     3 props(nprops),coords(3),drot(3,3),dfgrd0(3,3),dfgrd1(3,3)
c
c     define internal variables as double precision
      double precision stress1(4,1),strain1(4,1),dstress(4),DeINV(4,4),
     1 dstress1(4,1),dstrain1(4,1),Dsp1(4,1),DShe(4,1),Dsp2(4,1),
     2 dFds(4,1),dFdst(1,4),dlamda0(1,4),dlamda1(1,4),Dp11(4,1),
     3 DHSDDE(4,1),Dp12(4,4),Dp1(4,4),Dp(4,4),ee(4,1),Dhe(4,4),
     4 De(4,4),Dsp(4,1),dstress2(4,1),Dsp22(4,1),DShe1(4,1),Dsp3(4,1),
     5 stress2(4,1)
c
c
c     Initialise local variables
      dlamda5=0.0
      dlamda2=0.0
      dlamda3=0.0
      dlamda4=0.0
      do i=1,4
          stress1(i,1)=0.0
          strain1(i,1)=0.0
          dstress(i)=0.0
          dstress1(i,1)=0.0
          dstress2(i,1)=0.0
          dstrain1(i,1)=0.0
          dFds(i,1)=0.0
          dFdst(1,i)=0.0
          dlamda0(1,i)=0.0
          dlamda1(1,i)=0.0
          Dp11(i,1)=0.0
          ee(i,1)=0.0
          DShe(i,1)=0.0
          DShe1(i,1)=0.0
          DHSDDE(i,1)=0.0
          Dsp(i,1)=0.0
          Dsp1(i,1)=0.0
          Dsp2(i,1)=0.0
          Dsp3(i,1)=0.0
          Dsp22(i,1)=0.0
          stress2(i,1)=0.0
      end do
      do i=1,4
          do j=1,4
              De(i,j)=0.0
              Dp12(i,j)=0.0
              Dp1(i,j)=0.0
              Dp(i,j)=0.0
              Dhe(i,j)=0.0
              DeINV(i,j)=0.0
          end do
      end do
c
      ndim=3
      if (ntens==4) then
          ndim=2
      end if
c
c     Get stress tensor from abaqus (change sign)
      D=coords(2)
      h=-D
       ratio=0.440116284
      porep = ((1000*h*9.81)/1000)+10100
      Total=((1750*h*10)/1000)+10100
      VstressO = (Total - porep)
      HstressO = VstressO * ratio
      do i=1,ntens
          stress1(i,1)=-stress(i)
      end do
          stress2(1,1)=stress1(1,1)-VstressO
          stress2(2,1)=stress1(2,1)-HstressO
          stress2(3,1)=stress1(3,1)-HstressO
          stress2(4,1)=stress1(4,1)
c
c     Get strain tensor from abaqus (change sign)
      do i=1,ntens
          strain1(i,1)=-stran(i)
      end do
c
c     Get strain increment tensor from abaqus (change sign)
      do i=1,ntens
          dstrain1(i,1)=-dstran(i)
      end do
c
c     Calculate volmetric strain and deviatoric strain
      ev=strain1(1,1)+strain1(2,1)+strain1(3,1)
      ed=sqrt(2.0/3.0)*sqrt((strain1(1,1)-ev/3.0)**2+
     1 (strain1(2,1)-ev/3.0)**2+(strain1(3,1)-ev/3.0)**2+
     2 strain1(4,1)**2)
c
      IDBG=1
      KST=1
      IF(IDBG.EQ.1)WRITE(6,900)KST
900   FORMAT(1X,'***KST',i5)
c
c     Define material constants
      cil     =   props(1)    ! compression index
      sik     =   props(2)    ! swelling index
      rM      =   props(3)    ! stress ratio eta at the critical state
       u      =   props(4)    ! parameter represents the development of plastic strain
      pnyu    =   props(5)    ! Poisson's ratio of soil
      rM2     =   props(6)    ! parametre give the degree of increase in shear modulus
      eM      =   props(7)    ! mechanical degradation factor upon shearing
       a      =   props(8)    ! parameter for dilation
       b      =   props(9)    ! parameter for dilation
       c      =   props(10)   ! parameter for cohesion
       d      =   props(11)   ! parameter for cohesion
c       Sh     =   props(12)
c
c     Get plastic volumetric strain
      epv=statev(1)
c
c     Get plastic deviator strain
      epd=statev(4)
c
c     Get void ratio
      void=statev(2)
c
c     Get preconsolidation pressure
      p1=statev(9)
c
c     Get degradation factor
      X=statev(3)
c
c     Get plastic strain
      ep1=statev(89)
      ep2=statev(90)
      ep3=statev(91)
      ep4=statev(92)
      ep5=statev(93)
      ep6=statev(94)
c
c
c
c     Get hydrate saturation
      Sh=statev(112)
c     Get the enhancement of strength due to hydrate
      Pcc=c*(X*Sh)**d
c
c     Get the enhancement of dilatancy  due to hydrate
      Pcd=a*(X*Sh)**b
c
c     Calculate mean and deviator stresses
      pc=(stress1(1,1)+stress1(2,1)+stress1(3,1))/3.0
      if (pc<0.and.h<269.905) pc=0.0001
c
      qc=sqrt(3.0/2.0)*sqrt((stress1(1,1)-pc)**2+
     1 (stress1(2,1)-pc)**2+(stress1(3,1)-pc)**2+
     2 stress1(4,1)**2)
c
c
c
c     Calculate elastic stifness
 	  BKS=(1.0+void)/sik*pc
 	  GS0=3.0*BKS/2.0*(1.0-2.0*pnyu)/(1.0+pnyu)
 	  GS=GS0+rM2*X*Sh
c
       do i=1,3
           do j=1,3
               if (i==j) then
                   De(i,j)=BKS+4./3.*GS
               else
                   De(i,j)=BKS-2./3.*GS
               end if
           end do
       end do
                   De(4,4)=GS
c
      call MATINV(1,4,De,DeINV)
      do i=1,4
           do j=1,4
               ee(i,1)=ee(i,1)+DeINV(i,j)*stress2(j,1)
           end do
      end do
c     Calculate elastic stifness changes due to hydrate dissociation
       do i=1,3
           do j=1,3
               if (i==j) then
                   Dhe(i,j)=4./3.*rM2
               else
                   Dhe(i,j)=-2./3.*rM2
               end if
           end do
       end do
            Dhe(4,4)=rM2
       do i=1,4
          do j=1,4
                  DShe1(i,1)=DShe1(i,1)+Dhe(i,j)*ee(j,1)
          end do
       end do
c
         do i=1,4
                  DShe(i,1)=X*DShe1(i,1)
         end do
C     Calulate the dFdq
       dFdq=2*qc
c
c     Calculate the subloading ratio of R
       R=(qc**2+rM**2*pc*(pc+pcc))/(rM**2*(pc+pcc)*(p1+pcd+pcc))
       if (R>1) R=1
c
      KST=2
      IF(IDBG.EQ.1)WRITE(6,901)KST
901   FORMAT(1X,'***KST',i5)
c
c     Calculate the depddep
c      IF (ABS(epd)<1.0E-10)then
c          depddep1=1
c          depddep2=1
c          depddep3=1
c          depddep4=1
c          depddep5=1
c          depddep6=1
c      Else
c          depddep1=(2/3)/epd*(ep1-epv/3)
c          depddep2=(2/3)/epd*(ep2-epv/3)
c          depddep3=(2/3)/epd*(ep3-epv/3)
c          depddep4=(2/3)/(2*epd)*ep4
c          depddep5=(2/3)/(2*epd)*ep5
c          depddep6=(2/3)/(2*epd)*ep6
c      END IF
c
       IF (ABS(epd)<1.0E-10)then
           depddep1=0
           depddep2=0
           depddep3=0
           depddep4=0
           depddep5=0
           depddep6=0
       Else
           depddep1=(2.0/3.0)/epd*(ep1-epv/3.0)
           depddep2=(2.0/3.0)/epd*(ep2-epv/3.0)
           depddep3=(2.0/3.0)/epd*(ep3-epv/3.0)
           depddep4=(2.0/3.0)/(2*epd)*ep4
           depddep5=(2.0/3.0)/(2*epd)*ep5
           depddep6=(2.0/3.0)/(2*epd)*ep6
       END IF
C
c     Calculate the dFdS
      do i=1,3
          dFds(i,1)=(rM**2)*(2*pc-R*(pcd+pcc+p1)+pcc)/3
     1    +3*(stress1(i,1)-pc)
      end do
          dFds(4,1)=6.0*stress1(4,1)
c
c      Calculate the dFdPcc and dFdPcd
       dFdPcc=(rM**2)*(pc-R*(pc+pcc+pcd+p1+pcc))
       dFdPcd=-R*(rM**2)*(pc+pcc)
c
c      Calculate the dlamdah
       dlamdah=dFdPcd*a*b*(X*Sh)**b+dFdPcc*c*d*(X*Sh)**d
c
c
c     Calculate plastic stiffness matrix
      dFdsii=dFds(1,1)+dFds(2,1)+dFds(3,1)
c
      do i=1,4
          dFdst(1,i)=dFds(i,1)
      end do
      do i=1,4
          do j=1,4
              dlamda1(1,i)=dlamda1(1,i)+dFdst(1,j)*De(j,i)
          end do
      end do
c
C     abs(df/dsigma)
      rMODU=sqrt(dFds(1,1)**2+dFds(2,1)**2+dFds(3,1)**2
     1      +dFds(4,1)**2)
!      write(6,500) rMODU,dFds(1,1)
! 500  FORMAT(1X, 'rMODU=',E15.5, 'dFds(1,1)=',E15.5)
c     depd/dep*df/dsig
      rMODI=depddep1*dFds(1,1)+depddep2*dFds(2,1)
     1     +depddep3*dFds(3,1)+depddep4*dFds(4,1)
      dlamda2=-R*(rM**2)*(pc+pcc)*(1+void)*p1/(cil-sik)*dFdsii
     1     +rM**2*(pc+pcc)*(p1+pcd+pcc)*u*(1+(pcc+pcd)/p1)
     2     *log(R)*rMODU-eM*dlamdah*rMODI
c
      KST=3
      IF(IDBG.EQ.1)WRITE(6,902)KST
902   FORMAT(1X,'***KST',i5)
      do i=1,4
          dlamda3=dlamda3+dlamda1(1,i)*dFds(i,1)
      end do
c
      do i=1,4
          dlamda0(1,i)=dlamda1(1,i)/(-dlamda2+dlamda3)
      end do
c
c
      do i=1,4
          dlamda4=dlamda4+dlamda0(1,i)*dstrain1(i,1)
      end do
c
c     Calculate the dfdsh
      dpcddsh=X*a*b*(X*Sh)**(b-1)
      dpccdsh=X*c*d*(X*Sh)**(d-1)
      dfdsh=dfdpcd*dpcddsh+dfdpcc*dpccdsh
      do i=1,4
          do j=1,4
               Dsp22(i,1)=Dsp22(i,1)+De(i,j)*dFds(j,1)
          end do
      end do
c
      do i=1,4
              Dsp2(i,1)=dfdsh*Dsp22(i,1)
      end do
c
      dSh=0
            KST=4
      IF(IDBG.EQ.1)WRITE(6,903)KST
903   FORMAT(1X,'***KST',i5)
      do i=1,4
          dlamda5=dlamda5+dlamda0(1,i)*dstrain1(i,1)
      end do
          dlamda6=dSh*dfdsh/(-dlamda2+dlamda3)
          dlamda7=dSh*dlamda4/(-dlamda2+dlamda3)
          dlamda=dlamda5+dlamda6+dlamda7
c
      do i=1,4
          do j=1,4
              Dp11(i,1)=Dp11(i,1)+De(i,j)*dFds(j,1)
          end do
      end do
c
      do i=1,4
          do j=1,4
              Dp12(i,j)=Dp12(i,j)+Dp11(i,1)*dFdst(1,j)
          end do
      end do
c
      do i=1,4
          do j=1,4
              do k=1,4
                  Dp1(i,j)=Dp1(i,j)+Dp12(i,k)*De(k,j)
              end do
          end do
      end do
c
      do i=1,4
              do k=1,4
                  Dsp1(i,1)=Dsp1(i,1)+Dp12(i,k)*Dshe(k,1)
              end do
      end do
c
c
      do i=1,4
          do j=1,4
              Dp(i,j)=Dp1(i,j)/(-dlamda2+dlamda3)
          end do
      end do
c
c
      do i=1,4
              Dsp3(i,1)=Dsp1(i,1)+Dsp2(i,1)
      end do
c
c
      do i=1,4
              Dsp(i,1)=Dsp3(i,1)/(-dlamda2+dlamda3)
      end do
c
c     Define yield function
      fs=qc**2+(rM**2)*(pc+pcc)*(pc-R*(p1+pcc+pcd))
c
c     Judging yielding
      do i=1,4
          do j=1,4
              if (dlamda>=0.0) then
                  DDSDDE(i,j)=De(i,j)-Dp(i,j)
                  DHSDDE(i,1)=DShe(i,1)-Dsp(i,1)
                  depv=dlamda*dFdsii
                  depd=dlamda*dFDq
                  yjudge=1.0
              else
                  DDSDDE(i,j)=De(i,j)
                  DHSDDE(i,1)=DShe(i,1)
                  depv=0.0
                  yjudge=0.0
              end if
          end do
      end do
c
c     Update new stresses
      do i=1,4
          do j=1,4
              dstress2(i,1)=dstress2(i,1)+DDSDDE(i,j)*dstrain1(j,1)
          end do
      end do
c
      do i=1,4
              dstress1(i,1)=dstress2(i,1)+DHSDDE(i,1)*dSh
      end do
c
      do i=1,4
          stress1(i,1)=stress1(i,1)+dstress1(i,1)
          stress(i)=-stress1(i,1)
      end do
c
c     Update plastic volumetric strain
      epv=epv+depv
c
c     Update plastic deviatoric strain
      epd=epd+depd
c
c     Update plastic strain
      dep1=dlamda*dFds(1,1)
      dep2=dlamda*dFds(2,1)
      dep3=dlamda*dFds(3,1)
      dep4=dlamda*dFds(4,1)
      dep5=dlamda*dFds(5,1)
      dep6=dlamda*dFds(6,1)
      ep1=ep1+dep1
      ep2=ep2+dep2
      ep3=ep3+dep3
      ep4=ep4+dep4
      ep5=ep5+dep5
      ep6=ep6+dep6
c
c     Update the void ratio
      dvols=dstran(1)+dstran(2)+dstran(3)
      dv=dvols*(1.0+void)
      void=void+dv
c
c     Update preconsolidation pressure
      p1=p1+(1+void)*(pcc+pcd+p1)/(cil-sik)*depv
      if (p1<0) then
      p1=0
      end if
C
C
c     Update degradation factor
      dX=-eM*X*depd
      X=X+dX
c
c     Update pcc and pcd
      dpcc=c*d*(X*Sh)**(d-1)*(dX*Sh+X*dSh)
      dpcd=a*b*(X*Sh)**(b-1)*(dX*Sh+X*dSh)
c      pcc=pcc+dpcc
c      pcd=pcd+dpcd
c
c     Update degradation factor
      pc1=p1+pcc+pcd
c
c
c     Update Hydrate Saturation
      Sh=Sh+dSh
c
c     Update solution dependent state variable (SDV)
      statev(1)=epv
      statev(2)=void
      statev(3)=X
      statev(4)=epd
      statev(5)=dX
      statev(6)=pc
      statev(7)=qc
      statev(8)=GS
      statev(9)=p1
      statev(10)=yjudge
      statev(11)=dlamda
      statev(12)=fs
      statev(13)=dFdsii
      statev(14)=dFDq
      statev(15)=De(1,1)
      statev(16)=De(2,2)
      statev(17)=De(3,3)
      statev(18)=De(1,2)
      statev(19)=De(1,3)
      statev(20)=De(2,3)
      statev(21)=De(4,4)
      statev(22)=De(5,5)
      statev(23)=De(6,6)
      statev(24)=Dp(1,1)
      statev(25)=Dp(2,2)
      statev(26)=Dp(3,3)
      statev(27)=Dp(1,2)
      statev(28)=Dp(1,3)
      statev(29)=Dp(2,3)
      statev(30)=Dp(4,4)
      statev(31)=Dp(5,5)
      statev(32)=Dp(6,6)
      statev(33)=DDSDDE(1,1)
      statev(34)=DDSDDE(2,2)
      statev(35)=DDSDDE(3,3)
      statev(36)=DDSDDE(1,2)
      statev(37)=DDSDDE(1,3)
      statev(38)=DDSDDE(2,3)
      statev(39)=DDSDDE(4,4)
      statev(42)=Dp1(1,1)
      statev(43)=Dp1(2,2)
      statev(44)=Dp1(3,3)
      statev(45)=Dp1(1,2)
      statev(46)=Dp1(1,3)
      statev(47)=Dp1(2,3)
      statev(48)=Dp1(4,4)
      statev(49)=Dp1(5,5)
      statev(50)=Dp1(6,6)
      statev(51)=Dp11(1,1)
      statev(52)=Dp11(2,2)
      statev(53)=Dp11(3,3)
      statev(54)=Dp11(1,2)
      statev(55)=Dp11(1,3)
      statev(56)=Dp11(2,3)
      statev(57)=Dp11(4,4)
      statev(58)=Dp11(5,5)
      statev(59)=Dp11(6,6)
      statev(60)=Dp12(1,1)
      statev(61)=Dp12(2,2)
      statev(62)=Dp12(3,3)
      statev(63)=Dp12(1,2)
      statev(64)=Dp12(1,3)
      statev(65)=Dp12(2,3)
      statev(66)=Dp12(4,4)
      statev(67)=Dp12(5,5)
      statev(68)=Dp12(6,6)
      statev(69)=dstran(1)
      statev(70)=dstran(2)
      statev(71)=dstran(3)
      statev(72)=dstran(4)
      statev(75)=R
      statev(76)=depv
      statev(77)=depd
      statev(78)=dFds(1,1)
      statev(79)=dFds(2,1)
      statev(80)=dFds(3,1)
      statev(81)=pcc
      statev(82)=pcd
      statev(83)=dlamda2
      statev(84)=dlamda3
      statev(85)=dlamda4
      statev(86)=dlamdah
      statev(87)=dFdPcc
      statev(88)=dFdPcd
      statev(89)=ep1
      statev(90)=ep2
      statev(91)=ep3
      statev(92)=ep4
      statev(93)=ep5
      statev(94)=ep6
      statev(95)=pc1
      statev(96)=MODI
      statev(97)=dFds(4,1)
      statev(98)=dFds(5,1)
      statev(99)=dFds(6,1)
      statev(100)=depddep1
      statev(101)=depddep2
      statev(102)=depddep3
      statev(103)=depddep4
      statev(104)=depddep5
      statev(105)=depddep6
      statev(106)=ee(1,1)
      statev(107)=ee(2,1)
      statev(108)=ee(3,1)
      statev(109)=ee(4,1)
      statev(110)=ee(5,1)
      statev(111)=ee(6,1)
      statev(112)=Sh
      statev(113)=dSh
c
      return
      end
c
      SUBROUTINE MATINV(N1,N2,A,AINV)
C A general purpose matrix inverter by augmenting-pivoting technique:

C     A B C | 1 0 0        1 0 0 | J K L
C     D E F | 0 1 0   =>   0 1 0 | M N O
C     G H I | 0 0 1        0 0 1 | P Q R

C Explanation of passed parameters:
C        N1: lower dimension of square matrix
C        N2: upper dimension of square matrix
C         A: square matrix of dimension N1:N2,N1:N2 to be inverted
C      AINV: the inverted matrix

      IMPLICIT NONE              !N2-N1+1
      INTEGER I,J,K,N1,N2
      REAL*8 A(N1:N2,N1:N2),AINV(N1:N2,N1:N2),B(N1:N2,N1:2*N2-N1+1)
     &,PIVOT,XNUM

C     INITIALIZATION
      DO I=N1,N2
      DO J=N1,N2
      AINV(I,J)=0.0D0
      END DO
      END DO

C MAKE AUGMENTED MATRIX
      DO I=N1,N2
      DO J=N1,N2
      B(I,J)=0.0D0
      B(I,J+N2-N1+1)=0.0D0

      B(I,J)=A(I,J)
       IF(I.EQ.J) THEN
       B(I,J+N2-N1+1)=1.0D0
       END IF
      END DO
      END DO

      DO I=N1,N2

C CHOOSE THE LEFTMOST NON-ZERO ELEMENT AS PIVOT
      DO J=N1,N2
      IF(DABS(B(I,J)).GT.0)THEN
      PIVOT=B(I,J)
      EXIT
      END IF
      END DO

C STEP 1: Change the chosen pivot into "1" by dividing
C the pivot's row by the pivot number
      DO J=N1,2*N2-N1+1
      B(I,J)=B(I,J)/PIVOT
      END DO
      PIVOT=B(I,I) !UPDATE PIVOT VALUE

C STEP 2: Change the remainder of the pivot's COLUMN into 0's
C by adding to each row a suitable multiple of the PIVOT ROW
      DO K=N1,N2 !ROW
       IF(K.NE.I) THEN
       XNUM=B(K,I)/PIVOT !SAME COLUMN WITH THE CURRENT PIVOT
        DO J=N1,2*N2-N1+1 !COL
        B(K,J)=B(K,J)-XNUM*B(I,J)
        END DO
       END IF
      END DO

      END DO

C PREPARE THE FINAL INVERTED MATRIX
      DO I=N1,N2
      DO J=N1,N2
      AINV(I,J)=B(I,J+N2-N1+1)
      END DO
      END DO

      RETURN
      END
C