% Used to make the plots for the conjugate gradient example

% Example 4.9
function []=main()

steepmeth = 'steep';
cgmeth =    'cg   ';
gradmeth =  'grad ';

J1=optcg(gradmeth);
J2=optcg(steepmeth);
figure(1); clf;
loglog(J1); hold on
loglog(J2,'--');
legend('Conjugate Gradient','Gradient');
xlabel('k');

function [J] = optcg(method)
steepmeth = 'steep';
cgmeth =    'cg   ';
gradmeth =  'grad ';
% plot the test function
x=-2:0.001:2;
y=testfcn(x);
%figure(1); hold off
%plot(x,y);

% define the samples
randn('seed', 1);  % make sure its repeatable
M = 100;
samp = randn(M, 1);
samp(find(samp>2)) = 2;
samp(find(samp<-2)) = -2; 
yi = testfcn(samp);

%hold on
%plot(samp, ones(length(samp),1), 'o');


% initialize the parameter estimates
p = 25;
A = zeros(p, 1);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

tol = 1e-10;
done = 0;
its = 1;
maxits = 100;
J=zeros(maxits, 1);

% 1) find initial direction d = -zeta(k) along the gradient
zeta = zeros(p, 1);
for i=1:M
  zetai = makezeta(samp(i), A);
  e = yi(i) - approx(samp(i), A);
  zeta = zeta - zetai*e;
end
dir = -zeta;

% 2) set A(k+1) which minimizes J along k (this is the main loop)
while (~done)

a = 0;
rat = 2;
Ja = cost(A, samp, yi)
J(its) = Ja;
b = 0;

if ((method == cgmeth)|(method == steepmeth))
  for i=-20:10
    c = rat^i;
    Jc = cost(A+c*dir, samp, yi);
    if (Jc > Ja) break; end;
    b = c;
  end

  if (b==0)
    b = (a+c)/2;
    Jb = cost(A+b*dir, samp, yi);
    if (Jb > Ja) error('need to change bracket'); end;
    if (Jb > Jc) error('need to change bracket'); end;
  end;

  Aprev = A;
  A = A + myfmin('minfcn', a, b, c, A, samp, yi, dir)*dir;
else
  A = A + 0.01*dir;  % back-prop
end;

% 3) calculate zeta(k+1)

zeta2 = zeros(p, 1);
for i=1:M
  zetai = makezeta(samp(i), A);
  e = yi(i) - approx(samp(i), A);
  zeta2 = zeta2 - zetai*e;
end

% 4) check for stop condition
%if (norm(A - Aprev) < tol) break; end;
if (its >= maxits) break; end;

% 5) set new search direction

dir = -zeta2;    % steepest descent
if (method == cgmeth)
  % eta = zeta2'*zeta2 / (zeta'*zeta);
  eta = (zeta2 - zeta)'*zeta2 / (zeta'*zeta);
  dir = -zeta2 + eta*dir;
else
  dir = -zeta2;    % steepest descent/gradient
end

its = its+1
end;  % while(~done)

if (method==gradmeth)
 figure(2); hold off;
 plot(samp, yi, 'o'); hold on;
 l = length(x);
 ytest=zeros(l,1);
 for j=1:l
   ytest(j) = approx(x(j), A);
 end
 plot(x, ytest);
 hold off;
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% find xmin which minimized J(x) given a<b<c such that J(b)<J(a)
% and J(b)<J(c) based on golden section search.
function [xmin, z] = myfmin(J, a, b, c, varargin)

R = 0.38197;  % golden ratio
tol = 1e-4;
maxit = 40;
n = 0;

if (abs(c-b) > abs(b-a))
  t1 = b;
  t2 = b + R*(c-b);
else
  t1 = b - R*(b-a);
  t2 = b;
end

J1 = feval(J, t1, varargin{:});
J2 = feval(J, t2, varargin{:});

z = [a, t1, t2, c];

while (abs(c-a) > tol)
  if (J2 < J1)
    a = t1;
    t1 = t2;
    t2 = t2 + R*(c - t2);
    J1 = J2;
    J2 = feval(J, t2, varargin{:});
  else
    c = t2;
    t2 = t1;
    t1 = t1 - R*(t1 - a);
    J2 = J1;
    J1 = feval(J, t1, varargin{:});
  end

  n = n + 1;
  if (n >= maxit) 
    warning('Maximum numer of iterations reached');
    break;
  end;

  z = [z; [a, t1, t2, c]];
end %while

if (J1 < J2)
  xmin = t1;
else
  xmin = t2;
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Evaluate the approximator (assumes x is a scalar)
function [y] = approx(x, A)

p = length(A);  % number of linear coef.
sigma = 0.1;
c = 1:p;
c = 4*(c(:)-1)/(p-1) - 2;

tmp1 = (x - c)./sigma;
tmp2 = exp(-(tmp1.^2));
y = (A(:))'*tmp2;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Define the basis functions (assumes x is a scalar)
function [zeta] = makezeta(x, A)

p = length(A);  % number of linear coef.
sigma = 0.1;
c = 1:p;
c = 4*(c(:)-1)/(p-1) - 2;

% define zeta for linear coef.
zeta = zeros(p,1);
for i=1:p
  tmp = (x-c(i))/sigma;
  zeta(i) = exp(-(tmp^2));    
end
zeta;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Define the test function
function [y] = testfcn(x)

y = cos(pi./(0.5+abs(x)));


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Define the 1-dimensional function to minimize
function [J] = minfcn(gamma, A, xi, yi, d)

J = cost(A + gamma*d, xi, yi);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Define the cost function
function [J] = cost(A, xi, yi)

M = length(yi);

J = 0;
for i=1:M
  J = J + (yi(i) - approx(xi(i), A))^2;
end