##
## Utilities
##

## Record function call arguments 
trackxvals <- function(f) {
    function(x) {
        xvals <<- rbind(xvals, x)
        f(x)
    }
}

## Show the path of function call arguments
showpath <- function(xvals, sleep = 0.2) {
    for (i in seq_len(nrow(xvals)))
        if (i == 1)
            points(xvals[i, 1], xvals[i,2])
        else {
            segments(xvals[i - 1, 1], xvals[i - 1, 2], xvals[i, 1], xvals[i,2])
            points(xvals[i, 1], xvals[i,2])
            Sys.sleep(sleep)
        }
    points(xvals[i, 1], xvals[i, 2], col = "red", pch = 19)
}


##
## Quadratic Function
##

B <- matrix(c(1, .5, .5, 1), 2)
b <- c(1, 1)

fn <- function(x) {
    y <- x - b
    sum(0.5 * y * (B %*% y))
}

gr <- function(x) B %*% (x - b)

x1 <- seq(0, 2, len = 101)
x2 <- x1
g <- expand.grid(x1 = x1, x2 = x2)
z <- apply(cbind(g$x1, g$x2), 1, fn)

xinit <- c(2, 1.8)

## Nelder-Mead
xvals <- NULL
optim(xinit, trackxvals(fn))
contour(x1, x2, matrix(z, length(x1), length(x2)))
showpath(xvals)

## BFGS
xvals <- NULL
optim(xinit, trackxvals(fn), gr, method = "BFGS")
contour(x1, x2, matrix(z, length(x1), length(x2)))
showpath(xvals)

## Constrained
c1 <- 1.1
c2 <- 1.2
xvals <- NULL
constrOptim(xinit, trackxvals(fn), gr, ui = diag(2), ci = c(c1, c2))
contour(x1, x2, matrix(z, length(x1), length(x2)))
abline(v = c1, lty = 2)
abline(h = c2, lty = 2)
showpath(xvals)


##
## Rosenbrock Banana function
##

a <- 5
fr <- function(x) {
         x1 <- x[1]
         x2 <- x[2]
         a * (x2 - x1 ^2)^2 + (1 - x1)^2
     }

grr <- function(x) { ## Gradient of 'fr'
         x1 <- x[1]
         x2 <- x[2]
         c(-4 * a * x1 * (x2 - x1 * x1) - 2 * (1 - x1),
            2 * a *      (x2 - x1 * x1))
     }

x1 <- seq(0, 2, len = 101)
x2 <- x1
g <- expand.grid(x1 = x1, x2 = x2)
z <- apply(cbind(g$x1, g$x2), 1, fr)

xinit <- c(1.5, 1,5)

## Nelder-Mead
xvals <- NULL
optim(c(1.5, 1.5), trackxvals(fr))
contour(x1, x2, matrix(z, length(x1), length(x2)), levels = 0.1 * (1:10))
showpath(xvals)

## BFGS
xvals <- NULL
optim(c(1.5, 1.5), trackxvals(fr), grr, method = "BFGS")
contour(x1, x2, matrix(z, length(x1), length(x2)), levels = 0.1 * (1:10))
showpath(xvals)

## Constrained
c1 <- 1.0
c2 <- 1.2
xvals <- NULL
constrOptim(c(1.5, 1.5), trackxvals(fr), grr, ui = diag(2), ci = c(c1, c2))
contour(x1, x2, matrix(z, length(x1), length(x2)), levels = 0.1 * (1:10),
        xlim = c(1, 2), ylim = c(1, 2))
abline(v = c1, lty = 2)
abline(h = c2, lty = 2)
showpath(xvals)