!*************************************************************************
!  Example with one extra parameter T.
!  Can put parameter and constraint =0 when not required.
!- - - - - - - - - - - - - - -
!  Newton vector:
!    x(1)   = T
!    x(2:5) = state vector x
!
!  Extra constraint:
!    (F(x)-x). dx/dt = 0 .
!       no update along direction of trajectory.
!
!  Jacobian approximation:
!    dF(x_n)/dx . dx = (F(x_n+eps.dx)-F(x_n))/eps
!
!  File guess.in :
!     T		initial guess for period
!     ndts	number of timesteps taken in one period
!     
!*************************************************************************
 module orbit
   implicit none
   save
					!* Params.in:
   integer          :: mgmres, nits 	!m for gmres(m), max newton its
   double precision :: rel_err		!relative error
   double precision :: del, mndl, mxdl  !delta for hookstep-trust-region 
   double precision :: gtol, epsJ	!gmres tolerance, eps in eqn above
					!* Typical params.in:
					!    100  100
					!    1d-7
					!   -1d0  1d-7  1d+7
					!    1d-3  1d-6

   integer, parameter :: n = 5		!size of state + parameters
   double precision   :: tol	
   integer :: info, ndts

 end module orbit


!*************************************************************************
include 'NewtonHook.f90'
include 'GMRESm.f90'
 PROGRAM MAIN
!*************************************************************************
   use orbit
   use newton
   implicit none
   external :: getrhs, multJ, multJp, saveorbit 
   double precision, external :: dotprod
   double precision :: d
   integer :: i1, i2, i3
   character(4) :: cnum

   open(99,status='old',file='params.in')
   read(99,*) mgmres, nits
   read(99,*) rel_err
   read(99,*) del, mndl, mxdl
   read(99,*) gtol, epsJ
   close(99)

   allocate(new_x(n))
   allocate(new_fx(n))  
   new_x = 0d0

   open(99,status='old',file='guess.in')
   read(99,*) new_x(1)
   read(99,*) ndts
   close(99)
   if(ndts==-1) ndts = nint(new_x(1)/0.002d0)

*****
   LOAD STATE TO  new_x(2:)
*****
			! optionally scale params by norm of state
   d = dotprod(-1,new_x,new_x)
   tol  = rel_err * dsqrt(d)
   del  = del     * dsqrt(d)
   mndl = mndl    * dsqrt(d)
   mxdl = mxdl    * dsqrt(d)

   info = 1
   call newtonhook(getrhs, multJ, multJp, saveorbit, dotprod, &
                   mgmres, n, gtol, tol, del, mndl, mxdl, nits, info)
   stop

 contains

!*************************************************************************
 END PROGRAM MAIN
!*************************************************************************

!-------------------------------------------------------------------------
!  function to be minimised   
!-------------------------------------------------------------------------
 subroutine getrhs(n_,x, y)
   use orbit
   implicit none
   integer,          intent(in)  :: n_
   double precision, intent(in)  :: x(n)
   double precision, intent(out) :: y(n)
   double precision :: x_(n), y_(n)

   x_ = x
   call steporbit(ndts,x_, y_)
   y = y_ - x					! diff
   y(1) = 0d0					! constraints, rhs=0

 end subroutine getrhs


!-------------------------------------------------------------------------
!  Jacobian of function + lhs of constraints on update
!-------------------------------------------------------------------------
 subroutine multJ(n_,x, y)
   use newton
   use orbit,    only : n, epsJ, dt__
   implicit none
   integer,          intent(in)  :: n_
   double precision, intent(in)  :: x(n)
   double precision, intent(out) :: y(n)   
   double precision, external :: dotprod
   double precision :: eps, s(n)
    				! (F(x0+eps.x)-F(x0))/eps
   eps = dsqrt(dotprod(1,x,x))
   if(eps==0d0)  stop 'multJ: eps=0 (1)'
   eps = epsJ * dsqrt(dotprod(1,new_x,new_x)) / eps
   if(eps==0d0)  stop 'multJ: eps=0 (2)'
   y = new_x + eps*x
   call getrhs(n_,y, s)
   y = (s - new_fx) / eps
      				! contstraint, 
				! no update in trajectory direction
   call steporbit(1,new_x, s)
   s = (s - new_x) / dt__
   y(1) = dotprod(-1,s,x)

 end subroutine multJ
 

!-------------------------------------------------------------------------
!  preconditioner for multJ.  Empty - no preconditioner required
!-------------------------------------------------------------------------
 subroutine multJp(n, x)
   implicit none
   integer,          intent(in)    :: n
   double precision, intent(inout) :: x(n)
 end subroutine multJp


!-------------------------------------------------------------------------
!  called at each newton iteration   
!-------------------------------------------------------------------------
 subroutine saveorbit()
   use newton
   use orbit
   use io
   implicit none
   double precision :: norm_x, p
   double precision, external :: dotprod

   norm_x = dsqrt(dotprod(-1,new_x,new_x))
   
   open(99,status='unknown',access='append',file='newton.dat')
   if(new_nits==0)  write(99,*) ndts, mgmres, n
   write(99,'(2I6,4e13.5)')  &
         new_nits, new_gits, new_tol, new_del, new_tol/norm_x, norm_x
   close(99)
!  newton its completed
!  num gmres its for last newton it
!  current error
!  current trust region
!  relative error
!  size of state

   open(99,status='unknown',access='append',file='guesses.dat')
   if(new_nits==0)  write(99,*) ndts
   write(99,'(1I6,1e26.18)')  new_nits, new_x(1)
   close(99)
!  newton its completed
!  current guess for T

*****
   SAVE MOST RECENT STATE  new_x(2:)
*****
   
 end subroutine saveorbit
 
 
!-------------------------------------------------------------------------
! dot product.  can flag to exclude parameter T.  Could include weights
!-------------------------------------------------------------------------
 double precision function dotprod(n_,a,b)
   use orbit
   implicit none
   integer,          intent(in) :: n_
   double precision, intent(in) :: a(n), b(n)
   double precision :: d,d_
   integer :: n1
   n1 = 1
   if(n_==-1) n1 = 2
   dotprod = dot_product(a(n1:n),b(n1:n))
 end function dotprod


!-------------------------------------------------------------------------
!  timestep
!-------------------------------------------------------------------------
 subroutine steporbit(ndts_,x, y)
   use orbit
   use io
   implicit none
   integer,          intent(in)  :: ndts_
   double precision, intent(in)  :: x(n)
   double precision, intent(out) :: y(n)
   double precision, save :: dt
  
   if(ndts_/=1) then
      dt = x(1) / dble(ndts_)
   end if

*****   
   STATE = x(2:)
   TIMESTEP ndts_ STEPS OF SIZE dt
   PUT RESULT IN y(2:)
*****

 end subroutine steporbit