%
%                    Example 12_12.m 
%
%
% This code illustrates the implementation of the Metropolis algorithm
% in Example 12.12. Here we are sampling the stiffness and damping parameters
% q = [C,K] as well as the variance sigma^2 for the measurement error. 
%

  clear all

%
% Input the data generated in Example 6_5 and provide the stiffness and damping 
% parameters estimated in Example 11.21.
%

  load spring_data.txt
  obs = spring_data(:,2)';
 
  t = 0:.01:5;
  n = length(t);

  K = 22.191420569879639;
  C = 1.318132953020852;
  p = 2;
  N = 10000;

  params = [C, K];
  modelfun = @(params) spring_function_2params(params,obs,t);
  [params_opt,fval] = fminsearch(modelfun,params);

  C_opt = params_opt(1);
  K_opt = params_opt(2);

  factor = sqrt(K - C^2/4);
  denom = sqrt(4*K - C^2);

%
% Construct solution on [0,5] and the analytic sensitivity relations.
%

  f = 2*exp(-C*t/2).*cos(factor*t);
  dfdc = exp(-C*t/2).*((C*t/denom).*sin(factor*t) - t.*cos(factor*t));
  dfdk = (-2*t/denom).*exp(-C*t/2).*sin(factor*t);


%  error = sigma*randn(size(t));
%  obs = y + error;

%
% Construct the sensitivity matrix S and covariance matrix V.
%

  S = [dfdc' dfdk'];
  Res = obs - f;
  var_est = (1/(n-p))*Res*Res';
  V = var_est*inv(S'*S);
  R = chol(V);

  q_old = [C;K];
  SS_old = (obs-f)*(obs-f)';

%
% Construct the parameters used when sampling the error variance in the Metropolis algorithm.
%

  n0 = .001;
  sigma02 = var_est;
  aval = 0.5*(n0 + n);
  bval = 0.5*(n0*sigma02 + SS_old);
  sigma2 = 1/gamrnd(aval,1/bval);

%
% Construct a Metropolis chain of length N using the anisotropic proposal
% distribution J(\theta^* | \theta^{k-1}) = N(\theta^{k-1},V) 
%

  for i = 1:N
    z = randn(2,1); 
    q_new = q_old + R'*z;
    C_new = q_new(1,1);
    K_new = q_new(2,1);
    factor = sqrt(K_new - C_new^2/4);
    f_new = 2*exp(-C_new*t/2).*cos(factor*t);
    u_alpha = rand(1);
    SS_new = (obs-f_new)*(obs-f_new)';
    term = exp(-.5*(SS_new-SS_old)/sigma2);
    %alpha = min(1,term);
    alpha = term;
    if u_alpha < alpha
      Q_MCMC(:,i) = [C_new; K_new];
      q_old = q_new;
      SS_old = SS_new;
    else
      Q_MCMC(:,i) = q_old;
    end
    Sigma2(i) = sigma2;
    bval = 0.5*(n0*sigma02 + SS_old);
    sigma2 = 1/gamrnd(aval,1/bval);
  end

  Cvals = Q_MCMC(1,:);
  Kvals = Q_MCMC(2,:);
  
%
% Use kde to construct densities for C and K
%

  range_C = max(Cvals) - min(Cvals);
  range_K = max(Kvals) - min(Kvals);
  C_min = min(Cvals)-range_C/10;
  C_max = max(Cvals)+range_C/10;
  K_min = min(Kvals)-range_K/10;
  K_max = max(Kvals)+range_K/10;
  [bandwidth_C,density_C,Cmesh,cdf_C]=kde(Cvals);
  [bandwidth_K,density_K,Kmesh,cdf_K]=kde(Kvals);

  [density_C,Cmesh] = ksdensity(Cvals);
  [density_K,Kmesh] = ksdensity(Kvals);

%
% Plot results.  Note that the axis limits may change as a function of
% each new generated data set.
%

  figure(1)
  plot(Cvals,'-','linewidth',1)
  set(gca,'Fontsize',[22]);
  axis([0 N 1.26 1.38])
  xlabel('Chain Iteration')
  ylabel('Damping Parameter C')

  figure(2)
  plot(Kvals,'-','linewidth',1)
  set(gca,'Fontsize',[22]);
  axis([0 N 21.9 22.5])
  xlabel('Chain Iteration')
  ylabel('Stiffness Parameter K')

  figure(3)
  plot(Sigma2)
  set(gca,'Fontsize',[22]);
  title('Measurement Error Variance \sigma^2')

  figure(4)
  plot(Cmesh,density_C,'k-','linewidth',3)
  axis([1.25 1.38 0 30])
  set(gca,'Fontsize',[22]);
  xlabel('Damping Parameter C')

  figure(5)
  plot(Kmesh,density_K,'k-','linewidth',3)
  axis([21.9 22.5 0 6])
  set(gca,'Fontsize',[22]);
  xlabel('Stiffness Parameter K')

  figure(6)
  scatter(Cvals,Kvals)
  box on
 % axis([-19.2 -17.6 1.83e-3 2e-3])
  set(gca,'Fontsize',[22]);
  xlabel('Damping Parameter C')
  ylabel('Stiffness Parameter K')

%
% Construct a Metropolis chain of length N using the isotropic proposal distribution
% J(\theta^* | \theta^{k-1}) = N(\theta^{k-1},sI) 
% with s1 = 5e-3, s2 = 5e-6, s3 = 1e-1
%

  s(1) = sqrt(5e-3);
  s(2) = sqrt(5e-6);
  s(3) = sqrt(1e-1);
  for j=1:3
    for i = 1:N
      z = randn(2,1);
      q_new = q_old + s(j)*z;
      C_new = q_new(1,1);
      K_new = q_new(2,1);
      factor = sqrt(K_new - C_new^2/4);
      f_new = 2*exp(-C_new*t/2).*cos(factor*t);
      u_alpha = rand(1);
      SS_new = (obs-f_new)*(obs-f_new)';
      term = exp(-.5*(SS_new-SS_old)/sigma2);
      alpha = min(1,term);
      if u_alpha < alpha
        Q_MCMC(:,i) = [C_new; K_new];
        q_old = q_new;
        SS_old = SS_new;
      else
        Q_MCMC(:,i) = q_old;
      end
      Sigma2(i) = sigma2;
      bval = 0.5*(n0*sigma02 + SS_old);
      sigma2 = 1/gamrnd(aval,1/bval);
    end
    Cvals_iso(j,:)  = Q_MCMC(1,:);
    Kvals_iso(j,:) = Q_MCMC(2,:);
  end

%
% Plot results for anisotropic and isotropic proposal distributions
%

  c1 = 1.25;
  c2 = 1.38;
  k1 = 21.9;
  k2 = 22.5;

  figure(7)
  subplot(2,1,1)
  plot(Cvals,'-b','linewidth',2)
  axis([0 N c1 c2])
  set(gca,'Fontsize',[22]);
  ylabel('Damping C')
  subplot(2,1,2)
  plot(Kvals,'-b','linewidth',2)
  axis([0 N k1 k2])
  set(gca,'Fontsize',[22]);
  ylabel('Stiffness K')
  xlabel('Chain Iteration')
  %print -depsc fig12_6a

  figure(8)
  subplot(2,1,1)
  plot(Cvals_iso(1,:),'-b','linewidth',2)
  axis([0 N c1 c2])
  set(gca,'Fontsize',[22]);
  ylabel('Damping C')
  subplot(2,1,2)
  plot(Kvals_iso(1,:),'-b','linewidth',2)
  axis([0 N k1 k2])
  set(gca,'Fontsize',[22]);
  ylabel('Stiffness K')
  xlabel('Chain Iteration')
  %print -depsc fig12_6b

  figure(9)
  subplot(2,1,1)
  plot(Cvals_iso(2,:),'-b','linewidth',2)
  axis([0 N c1 c2])
  set(gca,'Fontsize',[22]);
  ylabel('Damping C')
  subplot(2,1,2)
  plot(Kvals_iso(2,:),'-b','linewidth',2)
  axis([0 N k1 k2])
  set(gca,'Fontsize',[22]);
  ylabel('Stiffness K')
  xlabel('Chain Iteration')
  %print -depsc fig12_6c

  figure(10)
  subplot(2,1,1)
  plot(Cvals_iso(3,:),'-b','linewidth',2)
  axis([0 N c1 c2])
  set(gca,'Fontsize',[22]);
  ylabel('Damping C')
  subplot(2,1,2)
  plot(Kvals_iso(3,:),'-b','linewidth',2)
  axis([0 N k1 k2])
  set(gca,'Fontsize',[22]);
  ylabel('Stiffness K')
  xlabel('Chain Iteration')
  %print -depsc fig12_6d