      program surfaces
! Curvilinear motion along a curve
      implicit double precision (a-h,o-z)
      common /par/dt,dt2,dtz,g,stoll,xm,aa,a2,cc,c2,nm,nl,shape
      common /pos/x,y,z,xo,yo,zo,xn,yn,zn,vx,vy,vz
      character*6 shape
!
      open(unit=7,file='surfaces.out',form='formatted'
     1 ,status='unknown')
      open(unit=77,file='surfaces.dat',form='formatted'
     1 ,status='unknown') ! for gnuplot
! Input deck:
      nt=40000;    ! no of time steps 
      nl=80;      ! max no of Lagrange iterations
      dt=0.01d0; dth=0.5d0*dt; dtz=2.d0*dt;
      dt2=dt*dt; dtsh=0.5d0*dt2;
      g=-9.81;
      stol=1.d-12; ! sigma tolerance
      xm=1.d0;
! Choose a shape:
!     shape='sphere'
!      shape='parbld'
!      shape='hypbl1'  ! "true" hyperellipsoid; less useful as model grav well
      shape='hypbl2'   ! pseudeo-hyperellipsoid; produces circles and ellipses
!
      call startconf
!
      vx=(xn-xo)/dtz; vy=(yn-yo)/dtz; vz=(zn-zo)/dtz; 
      ek=0.5d0*xm*(vx*vx+vy*vy+vz*vz); 
      ep=-xm*g*z; et=ek+ep;
      write(*,100) it,xn,yn,ek,ep,et,am
!
      do 1 it=1,nt
!  Verlet move:
        xn=2.d0*x-xo; yn=2.d0*y-yo; zn=2.d0*z-zo+g*dt2;
!  Lagrange-1 procedure:
        call guiseppe 
!        write(*,199) x,y,sig,xn,yn
199     format(1x,'surfaces: x_y,xn_yn = ',5e14.5)
        vx=0.5d0*(xn-xo)/dt; vy=0.5d0*(yn-yo)/dt; vz=0.5d0*(zn-zo)/dt;
        ax=(xn-2.d0*x+xo)/dt2; 
        ay=(yn-2.d0*y+yo)/dt2; 
        az=(zn-2.d0*z+zo)/dt2;
        am=dsqrt(ax*ax+ay*ay+az*az);
        ek=0.5d0*xm*(vx*vx+vy*vy+vz*vz); 
        ep=-xm*g*z; et=ek+ep;
        if(it.gt.4) then 
          sg=(xn*xn+yn*yn+zn*zn)/a2-1.d0
          write(*,100) it,xn,yn,zn,ek,ep,et,am
          write(7,100) it,xn,yn,zn,ek,ep,et,am
          write(77,100) it,xn,yn,zn,ek,ep,et,am
100       format(1x,'it,xyzn,ekpt,am= ',i6,7e11.3)
        endif
        xo=x; yo=y; zo=z;
        x=xn; y=yn; z=zn;
!
1     continue   ! end time step
!
      stop
      end
!
!------------------------------------------------------
      subroutine startconf
      implicit double precision (a-h,o-z)
      common /par/dt,dt2,dtz,g,stoll,xm,aa,a2,cc,c2,nm,nl,shape
      common /pos/x,y,z,xo,yo,zo,xn,yn,zn,vx,vy,vz
      character*6 shape
!
      if(shape.eq.'sphere') then      ! sphere
        aa=1.d0; a2=aa*aa; ! 
        x=aa; y=0.d0; z=0.d0;
        vx=1.d0; vy=1.d0; vz=1.d0;
        call sigma(x,y,z,sig)
        call dsigma(x,y,z,dsx,dsy,dsz)
        term=dsqrt(dsx*dsx+dsy*dsy+dsz*dsz); ! | grad sigma |
        dsx=dsx/term; dsy=dsy/term; dsz=dsz/term; ! unit v. along grad sigma
        term=vx*dsx+vy*dsy+vz*dsz; ! v projected on grad sigma
        vx=vx-dsx*term; vy=vy-dsy*term; vz=vz-dsz*term; ! should be perp grad s. 
        print *, vx*dsx+vy*dsy+vz*dsz;        
      else if(shape.eq.'parbld') then
        aa=1.d0; a2=aa*aa; ! 
        x=aa; y=0.d0; z=0.2d0;
        vx=1.d0; vy=1.d0; vz=1.d0;
        call sigma(x,y,z,sig)
        call dsigma(x,y,z,dsx,dsy,dsz)
      else if(shape.eq.'hypbl1') then
        aa=0.01d0; a2=aa*aa; ! 
        cc=0.01d0; c2=cc*cc; !  
        x=2000.d0*aa; y=0.d0;
        z=cc*dsqrt((x*x+y*y)/a2-1.d0);
        vx=0.d0; vy=12.d0; vz=0.d0; 
        call sigma(x,y,z,sig)
        call dsigma(x,y,z,dsx,dsy,dsz)
      else if(shape.eq.'hypbl2') then
        aa=1.d0; a2=aa*aa; ! 
 !       z=-.1d0; y=0.d0; x=-aa/z; vx=0.d0; vy=0.99d0; vz=0.d0; ! perfect circle
 !       z=-.1d0; y=0.d0; x=-aa/z; vx=0.d0; vy=1.1d0; vz=0.d0; ! ellipse, stable
 !       z=-.1d0; y=0.d0; x=-aa/z; vx=0.d0; vy=1.2d0; vz=0.d0; ! ellipse, stable
 !       z=-.1d0; y=0.d0; x=-aa/z; vx=0.d0; vy=1.3d0; vz=0.d0; ! ellipse, stable
 !      z=-.1d0; y=0.d0; x=-aa/z; vx=0.d0; vy=0.6d0; vz=0.d0; ! ellipse, slwly prcssng
 !       z=-.05d0; y=0.d0; x=-aa/z; vx=0.d0; vy=0.2d0; vz=0.d0; ! ellipse, precessing
         z=-.1d0; y=0.d0; x=-aa/z; vx=-.9d0; vy=0.5d0; vz=0.d0; ! ellipse, processing  
        call sigma(x,y,z,sig)
        call dsigma(x,y,z,dsx,dsy,dsz)
      endif
! Determine r(-1):
      xn=x-dt*vx; yn=y-dt*vy; zn=z-dt*vz+0.5d0*g*dt2; 
      call guiseppe;
      print *,'xyzn,sig=',xn,yn,zn,sig;
      xo=xn; yo=yn; zo=zn; ! r(-1) for Verlet
      return
      end
!
!------------------------------------------------------
      subroutine guiseppe
      implicit double precision (a-h,o-z)
      common /par/dt,dt2,dtz,g,stoll,xm,aa,a2,cc,c2,nm,nl,shape
      common /pos/x,y,z,xo,yo,zo,xn,yn,zn,vx,vy,vz
      character*6 shape
      do is=1,nl   ! Loop over Lagrange iterations
        call sigma(xn,yn,zn,sig) ! 
        call dsigma(xn,yn,zn,dsxn,dsyn,dszn);
        call dsigma(x,y,z,dsx,dsy,dsz);
        sr=dsxn*dsx+dsyn*dsy+dszn*dsz;
        cg=sig/sr/4.d0;
        xn  =xn  -cg*dsx;
        yn  =yn  -cg*dsy;
        zn  =zn  -cg*dsz;
        if(dabs(sig).lt.stol) exit
      enddo       ! end Lagrange loop
!
      return
      end
!
!------------------------------------------------------
!
      subroutine sigma(xs,ys,zs,sig)
      implicit double precision (a-h,o-z)
      common /par/dt,dt2,dtz,g,stoll,xm,aa,a2,cc,c2,nm,nl,shape
      common /pos/x,y,z,xo,yo,zo,xn,yn,zn,vx,vy,vz
      character*6 shape
 !     print *,shape
      if(shape.eq.'sphere') then      ! sphere
        sig=(xs*xs+ys*ys+zs*zs)/a2-1.d0;
      else if(shape.eq.'parbld') then ! paraboloid
        sig=aa*(xs*xs+ys*ys)-zs;
      else if(shape.eq.'hypbl1') then ! hyperboloid
        sig=(xs*xs+ys*ys)/a2-zs*zs/c2-1.d0;
      else if(shape.eq.'hypbl2') then ! hyperboloid
        sig=-aa/dsqrt(xs*xs+ys*ys)-zs;
      endif
!
      return
      end
!
!------------------------------------------------------
      subroutine dsigma(xs,ys,zs,dsxs,dsys,dszs)
      implicit double precision (a-h,o-z)
      common /par/dt,dt2,dtz,g,stoll,xm,aa,a2,cc,c2,nm,nl,shape
      common /pos/x,y,z,xo,yo,zo,xn,yn,zn,vx,vy,vz
      character*6 shape
 !     print *,shape
      if(shape.eq.'sphere') then      ! sphere
        dsxs=2.d0*xs/a2; dsys=2.d0*ys/a2; dszs=2.d0*zs/a2;
      else if(shape.eq.'parbld') then ! paraboloid
        dsxs=2.d0*aa*xs; dsys=2.d0*aa*ys; dszs=-1.d0;
      else if(shape.eq.'hypbl1') then ! hyperboloid
        dsxs=2.d0*xs/a2; dsys=2.d0*ys/a2; dszs=-2.d0*zs/c2;
      else if(shape.eq.'hypbl2') then ! pseudo-hyperboloid
        rs=dsqrt(xs*xs+ys*ys); rs3=rs*rs*rs;
        dsxs=aa*xs/rs3; dsys=aa*ys/rs3;  dszs=-1.d0;
      endif
!
      return
      end
!
!--------------------------------------------------------

