library("numDeriv")
#####
##  example
f3 <- function(X){
  x1 <- X[1]; x2 <- X[2];
  y  <- (cos(x1))^2+sin(x2);
  return(y)
}
optim(fn = f3,par = c(3,-1),method = "CG")
##  min is -1 at (1.570796, -1.570795).

#####  My CG
##  Input
fn = f3; x_start = c(3,-1); CGmaxit = 200; CGtol = 10^-8; NRmaxit = 200; NRtol = 10^-8;

##  initial set
x = x_start;
i = 0; k = 0; r = -grad(func=fn,x=x); d = r; n = length(x);
delta_new = t(r)%*%r; delta_0 = delta_new;

##  iteration
while( i < CGmaxit & delta_new > CGtol^2*delta_0 ){
  j = 0; delta_d = d%*%d;
  alpha = - (t(grad(func=fn,x=x))%*%d)/(t(d)%*%hessian(func=fn,x=x)%*%d);
  
  ##  Newton Raphson
  while( j < NRmaxit & alpha^2*delta_d > NRtol^2 ){
    alpha = - (t(grad(func=fn,x=x))%*%d)/(t(d)%*%hessian(func=fn,x=x)%*%d);
    x = x + alpha*d;
    j = j + 1;
  }
  
  ##  update
  r = -grad(func=fn,x=x);
  delta_old = delta_new;
  delta_new = t(r)%*%r;
  beta      = delta_new/delta_old;
  d         = r + beta*d;
  k         = k + 1;
  if( k==n || t(r)%*%d<=0 ){
    d = r;
    k = 0;
  }
}
f3(x)
##  my min is 2 at (-3.141593, -4.712389)