%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Metropolis algorithm
%
% M Nofrarias 15-06-09
%
% $Id: mhsample.m,v 1.19 2011/11/16 08:52:50 nikos Exp $
% prior
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function [smpl smplr] = mhsample(model,in,out,nse,cov,number,limit,param,Tc,xi,xo,search,jumps,parplot,dbg_info,inNames,outNames,fpars,anneal,SNR0,DeltaL,inModel,outModel)
  
  import utils.const.*
  
  % compute mean and range
  mn = mean(limit);
  range = diff(limit);
  % initialize
  acc = [];
  nacc = 1;
  nrej = 1;
  loop = 0;
  oldacc = 0;
  oldrej = 0;
  chgsc = 0;
  nparam = length(param);
  delta = 1;
  SNR = 0;
  heat = xi;
  
  % plist to pass in to the sampler for use in bode
  spl = plist('outputs', outNames, ...
    'inputs', inNames, ...
    'reorganize', false,...
    'numeric output',true);

  % check if using priors
  nparams = size(xo);
  if isempty(fpars)
      fpars = zeros(nparams(2),3);
  end

  
  
  % switch depending on model class (initial likelihood)
  switch class(model)
    case 'matrix'
      [loglk1 SNR] = utils.math.loglikelihood_matrix(xo,in,out,nse,model,param,inModel,outModel);
    case 'smodel'
      loglk1 = utils.math.loglikelihood(xo,in,out,nse,model,param);
    case 'ssm'
      Amats = model.amats;
      Bmats = model.bmats;
      Cmats = model.cmats;
      Dmats = model.dmats;
      loglk1 = utils.math.loglikelihood_ssm(xo,in,out,nse,model,param,inNames,outNames, spl, Amats, Bmats, Cmats, Dmats);
    otherwise
      error('### Model must be either from the ''smodel'' or the ''ssm'' class. Please check the inputs')
  end
  
  % accept the first sample if no search active
  if ~search
    smpl(1,:) = xo;
    nacc = 1;
  else
    smpl = [];
    nacc = 0;
  end
  nrej = 0;
  
  utils.helper.msg(msg.IMPORTANT, 'Starting Monte Carlo sampling', mfilename('class'), mfilename);
  
  % compute prior for the initial sample of the chain
  logprior1 = logPriors(xo,fpars);
  % if initial guess is far away, this contidion sets our logprior
  % to zero and takes account only the loglikelihood for the 
  % computation of logratio. From this initial point it starts
  % searching for the small area of acceptance.
  if logprior1 == -Inf;
     logprior1 = 0; 
  end
  
  T = Tc(1);
  hjump = 0;
  % main loop
  while(nacc<number)
    %    jumping criterion during search phase
    %    - 2-sigma  by default
    %    - 10-sigma  jumps at mod(10) samples
    %    - 100-sigma jumps at mod(25) samples
    %    - 1000-sigma jumps at mod(100) samples

    
    if search
      if nacc <= Tc(1)
        if(mod(nacc,10) == 0 && mod(nacc,25) ~= 0 && mod(nacc,100) ~= 0 && hjump  ~= 1)
          hjump = 1;
          xn = mvnrnd(xo,jumps(2)^2*cov);
        elseif(mod(nacc,20) == 0 && mod(nacc,100) ~= 0 && hjump  ~= 1)
          hjump = 1;
          xn = mvnrnd(xo,jumps(3)^2*cov);
        elseif(mod(nacc,50) == 0 && hjump  ~= 1)
          hjump = 1;
          xn = mvnrnd(xo,jumps(4)^2*cov);
          %           xn = xo + range/2+rand(size(xo));
        else
          hjump = 0;
          xn = mvnrnd(xo,jumps(1)^2*cov);
        end
      else
        xn = mvnrnd(xo,cov);  % changed that too
      end
    else
      xn = mvnrnd(xo,cov);
    end
    
    
    % compute prior probability 
    logprior2 = logPriors(xn,fpars);
    
    %check if out of limits
    if( any(xn < limit(1,:)) || any(xn > limit(2,:)))
      logprior2 =  inf;
      loglk2 = inf;
      betta = inf;
      %heat = inf;
      
    % This condition save us computation time.
    elseif logprior2 == -Inf 
        loglk2 = inf;
        betta = inf; 
    else
      % switch depending on model class (new likelihood)
      switch class(model)
        case 'matrix'
          [loglk2 SNR] = utils.math.loglikelihood_matrix(xn,in,out,nse,model,param,inModel,outModel);
        case 'smodel'
          loglk2 = utils.math.loglikelihood(xn,in,out,nse,model,param);
        case 'ssm'
          [loglk2 SNR] = utils.math.loglikelihood_ssm(xn,in,out,nse,model,param,inNames,outNames, spl, Amats, Bmats, Cmats, Dmats);
        otherwise
          error('### Model must be either from the ''smodel'' or the ''ssm'' class. Please check the inputs')
      end
      
      
      % compute annealing
      if ~isempty(Tc)
        if nacc <= Tc(1)
            
          % compute heat factor
          switch anneal
              case 'simul'
                delta = 1;
              case 'thermo'
                if (0 <= SNR(1) && SNR(1) <= SNR0)
                delta = 1;
                elseif (SNR(1) > SNR0)
                delta = (SNR(1)/SNR0)^2;              
                end
              case 'simple'
                if (nacc > 10 && mod(nacc,10) == 0)
                deltalogp = std(smpl(nacc-10:nacc,1));          
                    if deltalogp <= DeltaL(1) 
                    T = T + DeltaL(3);
                    elseif deltalogp >= DeltaL(2)
                    T = T - DeltaL(4);
                    end 
                    delta = Tc(1)/T;
                end
                heat = xi*delta;
          end
            
          betta = 1/2 * 10^(-heat*(1-Tc(1)/Tc(2)));
        elseif Tc(1) < nacc  && nacc <= Tc(2)
          betta = 1/2 * 10^(-heat*(1-nacc/Tc(2)));
        else
          betta = 1/2;
        end
      else
        betta = 1/2;
      end
      
    end % here we are 
    
    % likelihood ratio
    logr = betta*(logprior2 + loglk2 - loglk1 - logprior1) ;    
    % decide if sample is accepted or not
    if logr < 0
      xo = xn;
      nacc = nacc+1;
      sumsamples = nacc + nrej;
      smpl(nacc,1) = loglk2;
      smpl(nacc,2:size(xn,2)+1) = xn;
      smplr(sumsamples,1:size(xn,2)) = xn;
      smplr(sumsamples,size(xn,2)+1) = 1;
      smplr(sumsamples,size(xn,2)+2) = loglk2;
      loglk1 =loglk2;
      logprior1=logprior2;
      if dbg_info
       %utils.helper.msg(msg.IMPORTANT, sprintf('acc.\t loglik: %d -> %d priors: %d -> %d betta: %d  ratio: %d',loglk1,loglk2,logprior1,logprior2,betta,logr), mfilename('class'), mfilename);
       utils.helper.msg(msg.IMPORTANT, sprintf('acccount: %d SNR: %d %d Heat: %d',nacc,SNR(1),SNR(2),heat));
      end
    elseif rand(1) > (1 - exp(-logr))
      xo = xn;
      nacc = nacc+1;
      sumsamples = nacc + nrej;
      smpl(nacc,1) = loglk2;
      smpl(nacc,2:size(xn,2)+1) = xn;
      smplr(sumsamples,1:size(xn,2)) = xn;
      smplr(sumsamples,size(xn,2)+1) = 1;
      smplr(sumsamples,size(xn,2)+2) = loglk2;
      loglk1 =loglk2;
      logprior1=logprior2;
      if dbg_info
        %utils.helper.msg(msg.IMPORTANT, sprintf('acc.\t loglik: %d -> %d priors: %d -> %d betta: %d  ratio: %d',loglk1,loglk2,logprior1,logprior2,betta,logr), mfilename('class'), mfilename);
        utils.helper.msg(msg.IMPORTANT, sprintf('acccount: %d SNR: %d %d Heat: %d',nacc,SNR(1),SNR(2),heat));
      end
    else
      nrej = nrej+1;
      sumsamples = nacc + nrej;      
      smplr(sumsamples,1:size(xn,2)) = xn;
      smplr(sumsamples,size(xn,2)+1) = 0;
      smplr(sumsamples,size(xn,2)+2) = loglk2;
      if dbg_info
        %utils.helper.msg(msg.IMPORTANT, sprintf('rej.\t loglik: %d -> %d  priors: %d -> %d betta: %d  ratio: %d',loglk1,loglk2,logprior1,logprior2,betta,logr), mfilename('class'), mfilename);
        utils.helper.msg(msg.IMPORTANT, sprintf('rejcount: %d SNR: %d %d Heat: %d',nrej,SNR(1),SNR(2),heat));
      end
      
    end
    
    % display and save
    if(mod(nacc,100) == 0 && nacc ~= oldacc)
      updacc = nacc-oldacc;
      updrej = nrej-oldrej;
      ratio(nacc/10,:) = updacc/(updacc+updrej);
      utils.helper.msg(msg.IMPORTANT, sprintf('accepted: %d   rejected : %d   acc. rate: %4.2f',nacc,updrej,ratio(end)), mfilename('class'), mfilename);
      for i = 1:numel(param)
        fprintf('###  Parameters: %s = %d \n',param{i},xn(i))        
      end
      oldacc = nacc;
      oldrej = nrej;
      save('chain.txt','smpl','-ASCII')
      save('acceptance.txt','ratio','-ASCII')
    end
    
    % plot 
    if ((mod(sumsamples,100) == 0) && (nacc ~= 0) && ~isempty(parplot))
      for i = 1:numel(parplot)
        figure(parplot(i))
        plot(smpl(:,parplot(i)))
      end
      figure(i+1)
      plot(smplr(:,2))
    end
    
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%      compute logpriors
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function logprior = logPriors(parVect,fitparams)

D = size(parVect);

for ii=1:D(2)
   prior(ii) = normpdf(parVect(ii),fitparams(ii,1),fitparams(ii,2))/fitparams(ii,3);
   % checking if priors are used for this run 
   if isnan(prior(ii))
        prior(ii) = 1;
    end
end
prior = log(prior);
logprior = sum(prior);

% assuming that priors are independent, then
% p(x|8n)=p(x|81)p(x|82)p(x|83)...p(x|8n)=p1xp2xp3...xpn
% and log(p(x|8n)) = logp1+logp2+...+logpn

end