fun01 <- function(x1,x2){
  return(x1^2 - x1*x2 -4*x1 +x2^2 -x2 )
}

x1 = seq(-10,10,0.05); x2 = seq(-10,10,0.05)
v <- outer(x1,x2,function(x1,x2) fun01(x1,x2))
contour(x1,x2,v,nlevels= 100, drawlabels = F,col='blue')

a = runif(2,0,10); b = runif(2,0,10);c = runif(2,0,10)

sortABC <-  function(a,b,c,func){
  fA = do.call(func,list(a[1],a[2]))
  fB = do.call(func,list(b[1],b[2]))
  fC = do.call(func,list(c[1],c[2]))
  sort.F = sort(c(fA,fB,fC),index.return=T)
  temp.set = rbind(a,b,c)
  a = temp.set[sort.F$ix[1],]; fA = sort.F$x[1]
  b = temp.set[sort.F$ix[2],]; fB = sort.F$x[2]
  c = temp.set[sort.F$ix[3],]; fC = sort.F$x[3]
  return(list(abc = rbind(a,b,c), fs = c(fA,fB,fC)))
}

nelder.mead01 <- function(a,b,c, func, tol){
# not in the original form
# expansion -> reflection -> contraction -> shrink
# minimizes f with 2 variables 
  repeat {
    sort.abc = sortABC(a,b,c,func)
    a = sort.abc$abc[1,];b = sort.abc$abc[2,];c = sort.abc$abc[3,]
    fA = sort.abc$fs[1];fB = sort.abc$fs[2];fC = sort.abc$fs[3];
    max.diffs = max(abs(outer(c(fA,fB,fC),c(fA,fB,fC),'-')))
    if (max.diffs < tol){
      return(list(abc=rbind(a,b,c), fs = c(fA,fB,fC)));
      break
    }
    points(a[1],a[2],pch = 20, col ='red')
    points(b[1],b[2],pch = 20, col ='cyan')
    points(c[1],c[2],pch = 20, col ='green')
    m = (a + b)/2
    e = m + 2*(m - c)
    fE = do.call(func,list(e[1],e[2]))
    if (fE < fB){
      c = e; fC = fE
    } else {
      r = 2*m -c
      fR = do.call(func,list(r[1],r[2]))
      if (fR < fC){
        c = r; fC = fR
      }
      if (fR >= fB){
        s = (c + m)/2; fS = do.call(func,list(s[1],s[2]))
        if (fS < fC){
          c = s; fC =fS
        } else {
          b = m; fB = do.call(func,list(m[1],m[2]))
          c = (a + c)/2; fC = do.call(func,list(c[1],c[2]))
        }
      }
    }
  }
}

# simulated annealing
# version 1
SimAneal01<-function(func, initXY, maxItr=1000, C=1, eta=0.99, width=10) {
  x=initXY
  opt.value = do.call(func,list(x))
  n.var = length(x)
  opt.hist=matrix(0, nrow=maxItr, ncol=5)
  opt.hist[1,]=c(x,x,opt.value,opt.value,0)
  for (i_loop in 2:maxItr) {
    accept = 0
    temp.x = x + rnorm(n.var, mean = 0, sd=width)
    temp.value= do.call(func,list(temp.x))
    delta=temp.value-opt.value;
    prob=1/(1+exp(delta/(C*width)))
    if (runif(1) < prob) {
      x = temp.x;
      opt.value = temp.value;
      accept = 1
    } 
    opt.hist[i_loop,]=c(x, temp.x, opt.value, temp.value, accept);
    width=width*eta
  }
  return(data.frame(opt.hist))
}

set.seed(50)
n.iter =500
fun01<-function(x){x^2+2*x}
res <- SimAneal01(fun01, 3, n.iter, 1, 0.985, 1)

# version 2
funZ<-function(x,y) {x^4-16*x^2-5*x+y^4-16*y^2-5*y}
minSA<-function(initXY,maxItr=1000,C=1,eta=0.99,width) {
  x=initXY[1];y=initXY[2];z=funZ(x,y); 
  opHist=matrix(0,nrow=maxItr,ncol=3)
  opHist[1,]=c(x,y,z)
  for (i_loop in 2:maxItr) {
    xTemp=x+rnorm(1,mean=0,sd=width)
    yTemp=y+rnorm(1,mean=0,sd=width)
    zTemp=funZ(xTemp, yTemp);
    delta=zTemp-z;
    prob=1/(1+exp(delta/(C*width)))
    if (runif(1) < prob) {
      x=xTemp;y=yTemp;z=zTemp;
    } 
    opHist[i_loop,]=c(x,y,z);
    width=width*eta
  }
  return(opHist)
}

funZ<-function(x,y) {x^4-16*x^2-5*x+y^4-16*y^2-5*y}
xmin=-5;xmax=5;n_len=100;
x<-seq(xmin, xmax,length.out=n_len);
y<-x;z<-outer(x,y,funZ)

res<-minSA(c(0,0),1000,1,0.99,10)
lines(res[,1:2],type='o',lwd=2,col='green',pch=20)