source("gradientDescents.R")

parabola <- function(x,xmin,s) {
  d <- x - xmin
  t(d) %*% s %*% d 
}
parabolaGrad <- function(x,xmin,s) {
  d <- x - xmin
  2 * s %*% d 
}

f <- parabola
df <- parabolaGrad
center <- c(5,5)
S <- matrix(c(5,4,4,5),2,2)

xs <- seq(0,10,len=10)
ys <- seq(0,10,len=10)
p <- expand.grid(xs,ys)
fx <- NULL
for (i in 1:nrow(p)) {
  fx <- c(fx,f(t(p[i,]),center,S))
}
fx <- matrix(fx,length(xs),length(ys))
contour(xs,ys,fx,nlevels=20)

loc <- 1
while (!is.null(loc)) {

  cat("Click left button to start search. To stop, click right.\n");
  loc <- locator(1)
  if (!is.null(loc)) {
    firstx <- matrix(c(loc$x, loc$y))

    resultSCG <- scg(firstx, f, df, center, S, xPrecision=0.001, xtracep=TRUE)
    resultSteepest <- steepest(firstx,f,df,center,S,stepsize=0.1,
                               xPrecision=0.001, xtracep=TRUE)

    contour(xs,ys,fx,nlevels=20)
    
    fsearch <- c()
    r <- resultSteepest
    for (i in ncol(r$xtrace)) {
      x <- r$xtrace[,i]
      fsearch <- c(fsearch,f(x,center,S))
    }
    lines(r$xtrace[1,],r$xtrace[2,],col=2)
    points(r$xtrace[1,],r$xtrace[2,],col=2)

    fsearch <- c()
    r <- resultSCG
    for (i in ncol(r$xtrace)) {
      x <- r$xtrace[,i]
      fsearch <- c(fsearch,f(x,center,S))
    }
    lines(r$xtrace[1,],r$xtrace[2,],col=4)
    points(r$xtrace[1,],r$xtrace[2,],col=4)

    text(2,3,paste(ncol(resultSteepest$xtrace),"steps for Steepest Descent"),pos=4,col=2)
    text(2,2.5,paste(ncol(resultSCG$xtrace),"steps for SCG"),pos=4,col=4)
    
  }

}