***********************************************************************
*   function subprograms
************************************************************************

************************************************************************
* calculate Kronecher's Delta
*     krodelta(i,j)
*
*     krodelta : value of delta
*     i,j      : delta=1 only when i=j
************************************************************************
      double precision function krodelta(i,j)
      implicit double precision(a-z)
      integer i,j
      
      krodelta= 0.0d0
      if (i.eq.j) then 
      krodelta= 1.0d0
      end if

      return
      end

**********************************************************
* calculate inner product
*     inpro(a1,a2,a3,b1,b2,b3)  
*
*     inpro : value of inner product            
*     a1    : x component of vector a
*     a2    : y component of vector a
*     a3    : z component of vector a
*     b1    : x component of vector b
*     b2    : y component of vector b
*     b3    : z component of vector b
**********************************************************
      double precision function inpro(a1,a2,a3,b1,b2,b3)  
      implicit double precision(a-z)

      inpro= a1*b1 + a2*b2 + a3*b3
      return
      end

************************************************************
* calculate x component of outer product (axb)
*     exprox(a,b)  
*
*     exprox : x component of outer product
*     a      : vector a (array: dimension=3)
*     b      : vector b (array: dimension=3)
************************************************************
      double precision function exprox(a,b)  
      implicit double precision(a-z)
      double precision a(3),b(3)

      exprox= a(2)*b(3) - a(3)*b(2)
      return
      end

************************************************************
* calculate y component of outer product (axb)
*     exproy(a,b)  
*
*     exproy : y component of outer product
*     a      : vector a (array: dimension=3)
*     b      : vector b (array: dimension=3)
************************************************************
      double precision function exproy(a,b)  
      implicit double precision(a-z)
      double precision a(3),b(3)

      exproy= a(3)*b(1) - a(1)*b(3)
      return
      end

************************************************************
* calculate z component of outer product (axb)
*     exproz(a,b)  
*
*     exproz : z component of outer product
*     a      : vector a (array: dimension=3)
*     b      : vector b (array: dimension=3)
************************************************************
      double precision function exproz(a,b)  
      implicit double precision(a-z)
      double precision a(3),b(3)

      exproz= a(1)*b(2) - a(2)*b(1)
      return
      end

************************************************************
* calculate absolute value (|v|) of vector v
*     vecabs(x,y,z)  
*
*     vecabs : absolute value
*     x,y,z  : vector component
************************************************************
      double precision function vecabs(x,y,z)  
      implicit double precision(a-z)

      vecabs= dsqrt( x*x + y*y + z*z )
      return
      end

************************************************************
* normalize vector
*     vecnor(x,y,z)
*
*     x,y,z : vector component
************************************************************
      subroutine vecnor(x,y,z)
      implicit double precision(a-z)

      vabs= dsqrt( x*x + y*y + z*z )
      x = x / vabs
      y = y / vabs
      z = z / vabs
      return
      end

************************************************************
* substitute zero into all terms of 3x3 matrix
*     matset0(a)
*
*     a : target matrix
************************************************************
      subroutine matset0(a)
      implicit double precision(a-z)
      double precision a(3,3)
      integer i,j
 
      do 100 i=1,3
      do 100 j=1,3
      a(i,j) = 0.0d0
  100 continue

      return
      end

************************************************************
* calculate product of two 3x3 matrices
*     matpro(a,b,c)
*
*     a,b : two matricies
*     c   : resultant matrix
************************************************************
      subroutine matpro(a,b,c)
      implicit double precision(a-z)
      double precision a(3,3),b(3,3),c(3,3)
      integer i,j,k

      
      do 100 i=1,3
      do 100 j=1,3

      c(i,j) = 0.0d0
      do 100 k=1,3
      c(i,j) = c(i,j) + a(i,k) * b(k,j)
  100 continue

      return
      end

************************************************************
* add 3x3 matrix to another matrix
*     matadd(a,b)
*
*     a : original matrix and also resultant matrix
*     b : adding matrix
************************************************************
      subroutine matadd(a,b)
      implicit double precision(a-z)
      double precision a(3,3),b(3,3)
      integer i,j

      do 100 i=1,3
      do 100 j=1,3
      a(i,j) = a(i,j) + b(i,j)
  100 continue

      return
      end

************************************************************
* make 3x3 double precision matrix be single precision
*     matsubs(a,b)
*
*     a : original double precision matrix
*     b : resultant single precision matrix
************************************************************
      subroutine matsubs(a,b)
      implicit double precision(a-z)
      double precision a(3,3)
      real b(3,3)
      integer i,j

      do 100 i=1,3
      do 100 j=1,3
      b(i,j) = real(a(i,j))
  100 continue

      return
      end

************************************************************
* make 3x3 complex matrix be double precision matrix
*     matdble(a,b)
*
*     a : original complex matrix
*     b : resultant double precision matrix
************************************************************
      subroutine matdble(a,b)
      implicit double precision(a-z)
      complex a(3,3)
      double precision b(3,3)
      integer i,j

      do 100 i=1,3
      do 100 j=1,3
      b(i,j) = dble(a(i,j))
  100 continue

      do 200 i=1,3
      abso = dsqrt(b(1,i)**2+b(2,i)**2+b(3,i)**2)
      b(1,i)=b(1,i)/abso
      b(2,i)=b(2,i)/abso
      b(3,i)=b(3,i)/abso
  200 continue

      return
      end

************************************************************
* return transposed 3x3 matrix 
*     mattrans(a,b)
*
*     a : original matrix
*     b : resultant transposed matrix
************************************************************
      subroutine mattrans(a,b)
      implicit double precision(a-z)
      double precision a(3,3),b(3,3)
      integer i,j

      do 100 i=1,3
      do 100 j=1,3
      b(i,j) = a(j,i)
  100 continue

      return
      end

************************************************************
* rotate vector about one axis
*     vecrot(x,y,z,rx,ry,rz)
*
*     x,y,z : original and resultant vector
*     rx,ry,rz : rotating angle about each axis (radian)
************************************************************
      subroutine vecrot(x,y,z,rx,ry,rz)
      implicit double precision(a-z)

* rotate about x
      x1 =  x
      y1 =  y * dcos(rx) - z * dsin(rx)
      z1 =  y * dsin(rx) + z * dcos(rx)

* rotate about y
      x2 =  x1 * dcos(ry) + z1 * dsin(ry)
      y2 =  y1
      z2 = -x1 * dsin(ry) + z1 * dcos(ry)

* rotate about z
      x3 =  x2 * dcos(rz) - y2 * dsin(rz)
      y3 =  x2 * dsin(rz) + y2 * dcos(rz)
      z3 =  z2

      x = x3
      y = y3
      z = z3

      return
      end

************************************************************
* spline curve interpolation
*     spline(n,x,u,k,numl,up)
*
*         n : number of division
*         x : abscissa
*         u : ordinate
*         k : time step
*      numl : number of time step
*        up : results of interpolation
************************************************************
      subroutine spline(n,x,u,k,numl,up)
      implicit double precision(a-z)
      integer i,j,k,n,numl
      double precision x(20),u(20),udd(n),a(n,n),b(n)
     & ,ipvt(n),zmat(n)

***************
* make matrix *
***************

* first, substitute zero into all terms
      do 100 i=1,n
      do 100 j=1,n
      a(i,j) = 0.0d0
  100 continue

* next, substitute based on definition of spline curve
      do 200 i=2,n-1
      a(i,i-1) = (x(i)-x(i-1))/6.0d0
      a(i,i)   = ( (x(i)-x(i-1))+(x(i+1)-x(i)) )/3.0d0
      a(i,i+1) = (x(i+1)-x(i))/6.0d0
      
      b(i) = (u(i+1)-u(i))/(x(i+1)-x(i))
     &      -(u(i)-u(i-1))/(x(i)-x(i-1))
  200 continue

* give manually in case i=1
      i=1
      a(i,n) = (1.0d0-x(n))/6.0d0
      a(i,i)   = ( (1.0d0-x(n))+(x(i+1)-x(i)) )/3.0d0
      a(i,i+1) = (x(i+1)-x(i))/6.0d0
      
      b(i) = (u(i+1)-u(i))/(x(i+1)-x(i))
     &      -(u(1)-u(n))/(1.0d0-x(n))

* also give manually in case i=n
      i=n
      a(i,i-1) = (x(i)-x(i-1))/6.0d0
      a(i,i)   = ( (x(i)-x(i-1))+(1.0d0-x(i)) )/3.0d0
      a(i,1) = (1.0d0-x(i))/6.0d0
      
      b(i) = (u(1)-u(i))/(1.0d0-x(i))
     &      -(u(i)-u(i-1))/(x(i)-x(i-1))

****************************************
* obtain udd by solving inverse matrix *
* (returned b is udd)                  *
****************************************
      call dgeco(a,n,n,ipvt,rcond,zmat)
      call dgesl(a,n,n,ipvt,b,0)

      do 250 i=1,n
      udd(i)=b(i)
  250 continue

****************
* plot results *
****************
      if (k.eq.0) then
          xp = dble(numl-1)/dble(numl)
      else
          xp = dble(k-1)/dble(numl)
      end if

      do 400 i=1,n-1
      if((xp.gt.x(i)).and.(xp.le.x(i+1)).or.(xp.le.x(1))) then
          h = x(i+1)-x(i)
          t = (xp-x(i))/h
          up = h**2/6.0d0* ( (1.0d0-t)**3 * udd(i) + t**3 * udd(i+1) )
     &         + ( u(i)-h**2/6.0d0*udd(i) )*(1.0d0-t)
     &         + ( u(i+1)-h**2/6.0d0*udd(i+1) ) * t
c         write(*,*) xp,up
          goto 410
      end if
  400 continue

*     in case xp > x(n)
          h = 1.0d0-x(i)
          t = (xp-x(i))/h
          up = h**2/6.0d0* ( (1.0d0-t)**3 * udd(i) + t**3 * udd(1) )
     &         + ( u(i)-h**2/6.0d0*udd(i) )*(1.0d0-t)
     &         + ( u(1)-h**2/6.0d0*udd(1) ) * t
c         write(*,*) xp,up
 
  410 continue

c     if (wflag.eq.1) then
c         do 500 i=1,n
c         write(11,*) x(i),u(i)
c 500     continue

c         write(12,*) xp,up
c     end if

      return
      end

************************************************************
* spline curve interpolation (data output version)
*     splinew(n,x,u,k,numl,up,wflag)
*
*         n : number of division
*         x : abscissa
*         u : ordinate
*         k : time step
*      numl : number of time step
*        up : results of interpolation
************************************************************
      subroutine splinew(n,x,u,k,numl,up)
      implicit double precision(a-z)
      integer i,j,k,n,numl
      double precision x(20),u(20),udd(n),a(n,n),b(n)
     & ,ipvt(n),zmat(n)

***************
* make matrix *
***************

* first, substitute zero into all terms
      do 100 i=1,n
      do 100 j=1,n
      a(i,j) = 0.0d0
  100 continue

* next, substitute based on definition of spline curve
      do 200 i=2,n-1
      a(i,i-1) = (x(i)-x(i-1))/6.0d0
      a(i,i)   = ( (x(i)-x(i-1))+(x(i+1)-x(i)) )/3.0d0
      a(i,i+1) = (x(i+1)-x(i))/6.0d0
      
      b(i) = (u(i+1)-u(i))/(x(i+1)-x(i))
     &      -(u(i)-u(i-1))/(x(i)-x(i-1))
  200 continue

* give manually in case i=1
      i=1
      a(i,n) = (1.0d0-x(n))/6.0d0
      a(i,i)   = ( (1.0d0-x(n))+(x(i+1)-x(i)) )/3.0d0
      a(i,i+1) = (x(i+1)-x(i))/6.0d0
      
      b(i) = (u(i+1)-u(i))/(x(i+1)-x(i))
     &      -(u(1)-u(n))/(1.0d0-x(n))

* also give manually in case i=n
      i=n
      a(i,i-1) = (x(i)-x(i-1))/6.0d0
      a(i,i)   = ( (x(i)-x(i-1))+(1.0d0-x(i)) )/3.0d0
      a(i,1) = (1.0d0-x(i))/6.0d0
      
      b(i) = (u(1)-u(i))/(1.0d0-x(i))
     &      -(u(i)-u(i-1))/(x(i)-x(i-1))

****************************************
* obtain udd by solving inverse matrix *
* (returned b is udd)                  *
****************************************
      call dgeco(a,n,n,ipvt,rcond,zmat)
      call dgesl(a,n,n,ipvt,b,0)

      do 250 i=1,n
      udd(i)=b(i)
  250 continue

****************
* plot results *
****************
      if (k.eq.0) then
          xp = dble(numl-1)/dble(numl)
      else
          xp = dble(k-1)/dble(numl)
      end if

      do 400 i=1,n-1
      if((xp.gt.x(i)).and.(xp.le.x(i+1)).or.(xp.le.x(1))) then
          h = x(i+1)-x(i)
          t = (xp-x(i))/h
          up = h**2/6.0d0* ( (1.0d0-t)**3 * udd(i) + t**3 * udd(i+1) )
     &         + ( u(i)-h**2/6.0d0*udd(i) )*(1.0d0-t)
     &         + ( u(i+1)-h**2/6.0d0*udd(i+1) ) * t
c         write(*,*) xp,up
          goto 410
      end if
  400 continue

*     in case xp > x(n)
          h = 1.0d0-x(i)
          t = (xp-x(i))/h
          up = h**2/6.0d0* ( (1.0d0-t)**3 * udd(i) + t**3 * udd(1) )
     &         + ( u(i)-h**2/6.0d0*udd(i) )*(1.0d0-t)
     &         + ( u(1)-h**2/6.0d0*udd(1) ) * t
c         write(*,*) xp,up
 
  410 continue

      do 500 i=1,n
      write(11,*) x(i),u(i)
  500 continue

      write(12,*) xp,up

      return
      end

************************************************************
* make vector be orithonormal
*     vecnoror(x1,y1,z1,x2,y2,z2,x3,y3,z3)
*
*     x1,y1,z1 : compoents of vector 1
*     x2,y2,z2 : compoents of vector 2
*     x3,y3,z3 : compoents of vector 3
************************************************************
      subroutine vecnoror(x1,y1,z1,x2,y2,z2,x3,y3,z3)
      implicit double precision(a-z)
      double precision a(3),b(3)

* first, orthonormalize each one
      call vecnor(x1,y1,z1)
      call vecnor(x2,y2,z2)
      call vecnor(x3,y3,z3)

* next, derive reasonable direction
* from outer product of other two vectors

* vector 1
      a(1) = x2
      a(2) = y2
      a(3) = z2
      b(1) = x3
      b(2) = y3
      b(3) = z3

      x1d = exprox(a,b)
      y1d = exproy(a,b)
      z1d = exproz(a,b)

* vector 2
      a(1) = x3
      a(2) = y3
      a(3) = z3
      b(1) = x1
      b(2) = y1
      b(3) = z1

      x2d = exprox(a,b)
      y2d = exproy(a,b)
      z2d = exproz(a,b)

* vector 3
      a(1) = x1
      a(2) = y1
      a(3) = z1
      b(1) = x2
      b(2) = y2
      b(3) = z2

      x3d = exprox(a,b)
      y3d = exproy(a,b)
      z3d = exproz(a,b)

* orthonormalize each one
      call vecnor(x1d,y1d,z1d)
      call vecnor(x2d,y2d,z2d)
      call vecnor(x3d,y3d,z3d)

* take average of reasonable direction and present direction of vector
      x1 = (x1 + x1d) *0.5d0
      y1 = (y1 + y1d) *0.5d0
      z1 = (z1 + z1d) *0.5d0

      x2 = (x2 + x2d) *0.5d0
      y2 = (y2 + y2d) *0.5d0
      z2 = (z2 + z2d) *0.5d0

      x3 = (x3 + x3d) *0.5d0
      y3 = (y3 + y3d) *0.5d0
      z3 = (z3 + z3d) *0.5d0

* again, orthonormalize each one
      call vecnor(x1,y1,z1)
      call vecnor(x2,y2,z2)
      call vecnor(x3,y3,z3)

      return
      end

