program wemva_steep

use sep
use OW_type
use OW_parms_areal
use OW_wemva_feval
use OW_grabvel_areal
use lloyd_vel

implicit none
character*80                 :: hor1
character*80                 :: log,listagem
double precision             :: alp, tol, fc, fgoal, q0, qp0, lamc, qc, ft, qm, lamm,a,b
double precision,allocatable :: gc(:),gt(:)
integer                      :: itc,numf,numg,numh,iarm,maxit
double precision,allocatable :: x0(:),xc(:),xt(:), x(:)
real                         :: bhigh, blow, lambda
integer, allocatable         :: ithist(:,:)
integer                      :: costdata(3),i
real, allocatable            :: vel0(:),pick1(:),velint(:)

!
! C. T. Kelley, Dec 20, 1996
!
! This code comes with no guarantee or warranty of any kind.
!
! function [x,histout,costdata] = steep(x0,f,tol,maxit)
!
! steepest descent with Armijo rule, polynomial linesearch 
! 
!
! Input: x0 = initial iterate
!        f = objective function,
!            the calling sequence for f should be
!            [fout,gout]=f(x) where fout=f(x) is a scalar
!              and gout = grad f(x) is a COLUMN vector
!        tol = termination criterion norm(grad) < tol
!              optional, default = 1.d-6
!        maxit = maximum iterations (optional) default = 1000
!
! Output: x = solution
!         histout = iteration history   
!             Each row of histout is
!            [norm(grad), f, number of step length reductions, iteration count]
!         costdata = [num f, num grad, num hess] (for steep, num hess=0)
!
! Requires: polymod.m
!
! linesearch parms
! 
 call sep_init()
 call sep_begin_prog()

 call owparam()
!----------------------------------------------
! Horizon with no velocity update
if (exist_file("horv0")) then
   call from_param('horv0',hor1)
   open(51,file=hor1,status='old')
   allocate(pick1(img%xm%n));pick1=9999.
   do i=1,img%xm%n
      read(51,*,end=1) pick1(i)
   end do
1 close(51)
end if
call from_param("maxit",maxit,30)
 call from_param("log",log,"log")
 open(unit=2,file=log)
 call from_param("list",listagem,"listagem")
 open(unit=3,file=listagem)


!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
!
! Read velocities
!
 allocate(xc(img%xm%n*img%z%n),xt(img%xm%n*img%z%n))
 allocate(gc(img%xm%n*img%z%n),gt(img%xm%n*img%z%n))
 allocate(velint(v%z%n*v%xm%n))
 allocate(vel0(img%z%n*img%xm%n))
 call sreed("vel",velint,v%z%n*v%xm%n*4)
 call owgrab_vel(velint,vel0,.true.)
 xc=dble(vel0)
 deallocate(velint,vel0)

 bhigh=.5; blow=.1;
 alp=1.d-4
 tol=1.d-6
 itc=1
 call feval_init(pick1)
 numf=1
 numg=1
 numh=0
 fc=0;gc=0
 call feval(vel0=xc,fun=fc,g=gc,numg=numg)
! gc=gc/maxval(abs(gc))
 call to_history("n4",numg,"Velout")
 call srite("Velout",sngl(1/xc),4*img%z%n*img%xm%n)
 allocate(ithist(maxit,4));ithist=0
 ithist(1,1)=sqrt(dot_product(gc,gc)); ithist(1,2) = fc; ithist(1,4)=itc-1; ithist(1,3)=0; 
 do while(sqrt(dot_product(gc,gc)) > tol .and. itc <= maxit)
!
!       fixup for very long steps, see (3.50) in the book
!
   lambda=sngl(min(1.d+0,0.01/(1.+sqrt(dot_product(gc,gc)))))
   write(0,*) "LAMBDA = ",lambda,sqrt(dot_product(gc,gc))
!!! ADDED TO CONTROL THE UPDATES
   a=sum(gc)
   where((xc-lambda*gc)/xc > 1.05) 
     gc=gc*0.95
   end where
   if (a.ne.sum(gc)) write(0,*) "Local decrease of the gradient"
   a=sum(gc)
   where((xc-lambda*gc)/xc < 0.95)
     gc=gc*1.05
   end where
   if (a.ne.sum(gc)) write(0,*) "Local increase of the gradient"
!   if (a==1.or.b==1) then
!      lambda=sngl(min(1.d+0,0.01/(1.+sqrt(dot_product(gc,gc)))))
!      write(0,*) "New LAMBDA = ",lambda,sqrt(dot_product(gc,gc))
!      a=0;b=0
!   end if
!!!
   xt=xc-lambda*gc
   call feval(vel0=xt,fun=ft)
   numf=numf+1;
   iarm=0; itc=itc+1;
   fgoal=fc-alp*lambda*dot_product(gc,gc)
!
!       polynomial line search
!
   q0=fc
   qp0=-dot_product(gc,gc)
   lamc=lambda
   qc=ft
   write(0,*) ft,fgoal
   do while(ft > fgoal)
      iarm=iarm+1;
      if (iarm==1) then
         call polymod(q0, qp0, lamc, qc, blow, bhigh, lambda)
      else
         call polymod(q0, qp0, lamc, qc, blow, bhigh, lambda, lamm, qm)
      end if
      qm=qc
      lamm=lamc
      lamc=lambda
      xt=xc-lambda*gc
      ft=0
      call feval(vel0=xt,fun=ft)
      numf = numf+1
      qc=ft

      write(0,2) "Grad eval # ",numg," Fct eval # ",numf,"  Obj fct = ", ft, "  Fgoal  =  ",fgoal
2     format(2(a12,i4),2(a12,f12.8))
      if(iarm > 10) then
        write(0,*)' Armijo error in steepest descent '
!        histout=ithist(1:itc,:); costdata=[numf, numg, numh];
        stop
      end if
      fgoal=fc-alp*lambda*dot_product(gc,gc)

   end do
   xc=xt
   call to_history("n4",numg,"Velout")
   call srite("Velout",sngl(1/xc),4*img%z%n*img%xm%n)
   numf=numf+1
   numg=numg+1
   fc=0;gc=0
   call feval(vel0=xc,fun=fc,g=gc,numg=numg)
!   gc=gc/maxval(abs(gc))
   ithist(itc,1)=sqrt(dot_product(gc,gc))
   ithist(itc,2) = fc
   ithist(itc,4)=itc-1
   ithist(itc,3)=iarm
end do
!x=xc 
!histout=ithist(1:itc,:)
costdata(1)=numf
costdata(2)=numg
costdata(3)=numh
call feval_close()
call sep_end_prog()
end program
!--------------------------------------------------------------

subroutine polymod(q0, qp0, lamc, qc, blow, bhigh, lplus, lamm, qm)
!
! C. T. Kelley, Dec 29, 1997
!
! This code comes with no guarantee or warranty of any kind.
!
! function [lambda]=polymod(q0, qp0, qc, blow, bhigh, qm)
!
! Cubic/quadratic polynomial linesearch
!
! Finds minimizer lambda of the cubic polynomial q on the interval
! [blow * lamc, bhigh * lamc] such that
!
! q(0) = q0, q'(0) = qp0, q(lamc) = qc, q(lamm) = qm
! 
! if data for a cubic is not available (first stepsize reduction) then
! q is the quadratic such that
! 
! q(0) = q0, q'(0) = qp0, q(lamc) = qc
!
double precision, optional   :: lamm,qm
double precision             :: q0, qp0, lamc, qc, lleft, lright
double precision             :: a(2,2), b(2), c(2), a1(2,2)
real                         :: blow, bhigh, lplus

lleft=lamc*dble(blow); lright=lamc*dble(bhigh) 
if (present(lamm)) then
!
! quadratic model (temp hedge in case lamc is not 1)
!
    lplus = - qp0/(2 * lamc*(qc - q0 - qp0) );
    if (lplus < lleft) lplus = lleft
    if (lplus > lright) lplus = lright
else
!
! cubic model
!
    a(1,1)=lamc**2;a(1,2)=lamc**3;a(2,1)=lamm**2;a(2,2)=lamm**3
    a1(1,1)=a(2,2);a1(1,2)=-a(1,2);a1(2,1)=a(2,1);a1(2,2)=a(1,1)
    a1=a1/(a(1,1)*a(2,2)-a(1,2)*a(2,1))
    b=(/qc, qm/)-(/q0 + qp0*lamc, q0 + qp0*lamm/)
    c=matmul(a1,b)
    lplus=(-c(1)+sqrt(c(1)*c(1) - 3 *c(2) *qp0))/(3*c(2));
    if (lplus < lleft) lplus = lleft
    if (lplus > lright) lplus = lright
end if

end subroutine

