function [results] = vdpgm(given_data, opts)
%
% function [results] = vdpgm(given_data, opts)
%

start_time = clock;
if nargin == 1
  opts = struct();
end
if issparse(given_data)
  given_data = full(given_data);
end
if ~ isfield(opts, 'algorithm')
  % algorithm can be one of 'vdp', 'bj', 'cdp', 'csb' and 'non_dp'
  % vdp : variational DP
  % bj : Blei and Jordan
  % cdp : collapsed Dirichlet prior
  % csb : collapsed stick-breaking
  % non_dp : variational Bayes for Gaussian mixtures
  opts.algorithm = 'vdp';
end
if ~ isfield(opts, 'collapsed_means')
  opts.collapsed_means = 0;
end
if ~ isfield(opts, 'do_sort')
  opts.do_sort = '0';
end
if ~ isfield(opts, 'get_q_of_z')
  opts.get_q_of_z = 0;
end
if ~ isfield(opts, 'weight')
  opts.weight = 1;
end
if ~ isfield(opts, 'get_log_likelihood')
  opts.get_log_likelihood = 0;
end
if ~ isfield(opts, 'use_kd_tree')
  opts.use_kd_tree = 1;
end
if ~ isfield(opts, 'threshold')
  opts.threshold = 1.0e-5;
end
if ~ isfield(opts, 'sis')
  opts.sis = 0;
end
if ~ isfield(opts, 'initial_depth')
  opts.initial_depth = 3;
end
if ~ isfield(opts, 'initial_K')
  opts.initial_K = 1;
end
if ~ isfield(opts, 'ite')
  opts.ite = inf;
end
if ~ isfield(opts, 'do_split')
  opts.do_split = 0;
end
if ~ isfield(opts, 'do_merge')
  opts.do_merge = 0;
end
if ~ isfield(opts, 'do_greedy')
  opts.do_greedy = 1;
end
if ~ isfield(opts, 'max_target_ratio')
  opts.max_target_ratio = 0.5;
end
if ~ isfield(opts, 'init_of_split')
  % 'pc', 'rnd', 'rnd_close' or 'close_f'
  opts.init_of_split = 'pc';
end
if ~ isfield(opts, 'recursive_expanding_depth')
  opts.recursive_expanding_depth = 2;
end
if ~ isfield(opts, 'recursive_expanding_threshold')
  opts.recursive_expanding_threshold = 1.0e-1;
end
if ~ isfield(opts, 'recursive_expanding_frequency')
  opts.recursive_expanding_frequency = 3;
end
if isfield(opts, 'seed')
  rand('state', opts.seed);
else
  seed = rand('state');
  results.seed = seed;
end

data.given_data = given_data;
if opts.use_kd_tree
  partitions = init_kdtree_partitions(given_data, opts.initial_depth);
  data.kdtree = partitions;
end

% the hyperparameters of priors
hp_prior = mk_hp_prior(data, opts);

if isfield(opts, 'hp_posterior')
  opts.use_kd_tree = 0;
  if opts.get_q_of_z
    results.q_of_z = mk_q_of_z(data, opts.hp_posterior, opts.hp_prior, opts);
  end
  if opts.get_log_likelihood
    results.log_likelihood = mk_log_likelihood(data, opts.hp_posterior, opts.hp_prior, opts);
  end
  if isfield(opts, 'test_data')
    results.predictive_posterior = log_predictive_dist(data, opts.q_of_z, opts.hp_posterior, ...
                                                      opts.hp_prior, opts);
  end
  return
end

if opts.sis > 0
  q_of_z = sequential_importance_sampling(data, hp_prior, opts);
elseif isfield(opts, 'q_of_z')
  q_of_z = opts.q_of_z;
else
  q_of_z = rand_q_of_z(data, opts.initial_K, opts);
end

hp_posterior = mk_hp_posterior(data, q_of_z, hp_prior, opts);
if opts.do_greedy
  [free_energy, hp_posterior, data] = greedy(data, hp_posterior, hp_prior, opts);
else
  [free_energy, hp_posterior, data] = split_merge(data, hp_posterior, hp_prior, opts);
end

results.algorithm = opts.algorithm;
results.elapsed_time = etime(clock, start_time);
results.free_energy = free_energy;
results.hp_prior = hp_prior;
results.hp_posterior = hp_posterior;
results.K = length(hp_posterior.eta);
results.opts = opts;
if opts.get_q_of_z
  results.q_of_z = mk_q_of_z(data, hp_posterior, hp_prior, opts);
  if opts.use_kd_tree
    results.q_of_z = mk_non_kdtree_q_of_z(data, results.q_of_z);
  end
end
if opts.get_log_likelihood
  results.log_likelihood = mk_log_likelihood(data, hp_posterior, hp_prior, opts);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function log_likelihood = mk_log_likelihood(data, hp_posterior, hp_prior, opts);
[D,N] = size(data.given_data);
K = size(hp_posterior.m, 2);
log_likelihood = zeros(K,N);
E_pi = mk_E_pi(hp_posterior, hp_prior, opts);
for c=1:K
  mu = hp_posterior.m(:,c);
  f = hp_posterior.eta(c) + 1 - D;
  Sigma = (hp_posterior.xi(c)+1) / hp_posterior.xi(c) / f * hp_posterior.B{c};
  log_likelihood(c,:) = log_no_w(E_pi(c)) + logmvtpdf(data.given_data, mu, f, Sigma);
end
log_likelihood = log_sum_exp(log_likelihood, 1); % 1 by N
log_likelihood = sum(log_likelihood, 2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function log_prob = log_predictive_dist(data, q_of_z, hp_posterior, hp_prior, opts);
if opts.use_kd_tree
  N = length(data.kdtree);
  D = size(data.kdtree(1).sum_x, 1);
  Na = [data.kdtree(:).N];
  if isequal(opts.algorithm, 'vdp')
    true_Nc = Na*q_of_z; % 1*K
    q_of_z(:,end) = 0;
  end
  Nc = Na*q_of_z; % 1*K
  sum_x = [data.kdtree(:).sum_x] * q_of_z;
else
  [D,N] = size(data.given_data);
  if isequal(opts.algorithm, 'vdp')
    true_Nc = sum(q_of_z, 1); % 1*K
    q_of_z(:,end) = 0;
  end
  Nc = sum(q_of_z, 1); % 1*K
  sum_x = data.given_data * q_of_z;
end
E_pi = mk_E_pi(hp_posterior, hp_prior, opts);
m = (sum_x + repmat(hp_prior.xi0*hp_prior.m0, 1, size(sum_x,2))) ./ repmat(Nc, D, 1);
E_pi(find(Nc==0)) = 0;
n = size(opts.test_data, 2);
log_prob = zeros(size(m,2), n);
for c=1:size(m,2)
  f = hp_posterior.eta(c) + 1 - D;
  sigma = hp_posterior.B{c} * (hp_posterior.xi(c)+1) / hp_posterior.xi(c) / f;
  if E_pi(c) > 0
    log_prob(c,:) = log(E_pi(c)) + log_T_dist(opts.test_data, m(:,c), ...
                                              sigma, f);
  else
    log_prob(c,:) = -inf;
  end
end
log_prob = log_sum_exp(log_prob,1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function log_prob = log_T_dist(X, m, sigma, f);
% X is a D by n matrix
[D,n] = size(X);
diff = X-repmat(m,1,n);
log_prob = gammaln((f+D)/2) ...
    - D/2*log(f*pi) ...
    - gammaln(f/2) ...
    - 0.5*detln(sigma) ...
    - (f+D)/2*log(1 + sum( diff .* ((f*sigma) \ diff), 1));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function quad_term = mk_quad_term(data, q_of_z, hp_prior, opts)
% calculate the quad_term in marginalizing out means
% quad_term : D by D by K
%
[N, K] = size(q_of_z);
D = size(data.given_data, 1);
Nc = sum(q_of_z, 1); % 1 by K
xi = Nc + hp_prior.xi0;
f0 = 1 ./ (xi.^2);
f1 = - 2 ./ (xi.^3);
f2 = 3 ./ (xi.^4);
v = q_of_z.*(1 - q_of_z); % N by K
c = v.*((1-q_of_z).^2 + q_of_z.^2);
q = v.*((1-q_of_z).^3 + q_of_z.^3);
p_x = data.given_data * q_of_z; % D by K
v_x = data.given_data * v;      % D by K
c_x = data.given_data * c;      % D by K
v_p_x = data.given_data * (q_of_z + v); % D by K
c_p_x = data.given_data * (q_of_z + c); % D by K
for t=1:K
  first_term = f0(t) ...
      *((repmat(v(:,t)', D, 1).*data.given_data)*data.given_data' ...
        + p_x(:,t)*p_x(:,t)');
  second_term = f1(t) ...
      *((repmat(c(:,t)', D, 1).*data.given_data)*data.given_data' ...
        + v_p_x(:,t)*v_p_x(:,t)' - v_x(:,t)*v_x(:,t)' - p_x(:,t)*p_x(:,t)');
  tmp = q(:,t) - 3*v(:,t).^2 + v(:,t)*sum(v(:,t));
  third_term = f2(t) ...
      *((repmat(tmp', D, 1).*data.given_data)*data.given_data' ...
        + c_p_x(:,t)*c_p_x(:,t)' - c_x(:,t)*c_x(:,t)' - p_x(:,t)*p_x(:,t)' ...
        + 2*v_x(:,t)*v_x(:,t)' ...
        + sum(v(:,t))*p_x(:,t)*p_x(:,t)');
  quad_term(:,:,t) = first_term + second_term + third_term;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function E_log_p_of_z_given_other_z = mk_E_log_p_of_z_given_other_z(hp_posterior, hp_prior, opts);
% returns E[log p(z_n|Z^-n)]_q(Z^-n)
% E_log_p_of_z_given_other_z : N by K
q_of_z = hp_posterior.q_of_z;
[N,K] = size(q_of_z);
[E_Nc_minus_n, V_Nc_minus_n] = mk_E_Nc_minus_n(q_of_z); % N by K
use_variance = 1;
if isequal(opts.algorithm, 'cdp')
  if use_variance
    E_log_p_of_z_given_other_z = ...
        log(E_Nc_minus_n + hp_prior.alpha/K) ...
        - 0.5*V_Nc_minus_n./((E_Nc_minus_n+hp_prior.alpha/K).^2) ...
        - log(N - 1 + hp_prior.alpha);
  else
    E_log_p_of_z_given_other_z = ...
        log(E_Nc_minus_n + hp_prior.alpha/K) ...
        - log(N - 1 + hp_prior.alpha);
  end
elseif isequal(opts.algorithm, 'csb')
  E_Nc_minus_n_cumsum_geq = fliplr(cumsum(fliplr(E_Nc_minus_n), 2));
  q_of_z_cumsum_geq = fliplr(cumsum(fliplr(q_of_z), 2));
  dummy = q_of_z_cumsum_geq.*(1-q_of_z_cumsum_geq);
  v_Nc_cumsum_geq = repmat(sum(dummy, 1), N, 1) - dummy;

  E_Nc_minus_n_cumsum = E_Nc_minus_n_cumsum_geq - E_Nc_minus_n;
  q_of_z_cumsum = q_of_z_cumsum_geq - q_of_z;
  dummy = q_of_z_cumsum.*(1-q_of_z_cumsum);
  v_Nc_cumsum = repmat(sum(dummy, 1), N, 1) - dummy;

  if use_variance
    first_term = ...
        (log(1+E_Nc_minus_n) ...
         - 0.5*V_Nc_minus_n./((1+E_Nc_minus_n).^2) ...
         - log(1+hp_prior.alpha+E_Nc_minus_n_cumsum_geq) ...
         + 0.5*v_Nc_cumsum_geq./((1+hp_prior.alpha+E_Nc_minus_n_cumsum_geq).^2));
    first_term(:,end) = 0;
    dummy = log(hp_prior.alpha+E_Nc_minus_n_cumsum) ...
            - 0.5*v_Nc_cumsum./((hp_prior.alpha+E_Nc_minus_n_cumsum).^2) ...
            - log(1+hp_prior.alpha+E_Nc_minus_n_cumsum_geq) ...
            + 0.5*v_Nc_cumsum_geq./((1+hp_prior.alpha+E_Nc_minus_n_cumsum_geq).^2);
    second_term = cumsum(dummy, 2) - dummy;
  else
    first_term = log(1+E_Nc_minus_n) - log(1+hp_prior.alpha+E_Nc_minus_n_cumsum_geq);
    first_term(:,end) = 0;
    dummy = log(hp_prior.alpha+E_Nc_minus_n_cumsum) ...
            - log(1+hp_prior.alpha+E_Nc_minus_n_cumsum_geq);
    second_term = cumsum(dummy, 2) - dummy;
  end
  E_log_p_of_z_given_other_z = first_term + second_term;
else
  error('unsupported algorithm');
end
% Gaussian approximation may not give a proper distribution.
% note. E[log p] is not need to be proper

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [E_Nc_minus_n, V_Nc_minus_n] = mk_E_Nc_minus_n(q_of_z)
% returns E[Nc^-n]; the expected value of Nc-n; N by K
%         V[Nc^-n]; the variance of Nc-n;       N by K
% q_of_z : N by K
N = size(q_of_z, 1);
E_Nc_minus_n = repmat(sum(q_of_z, 1), N, 1) - q_of_z;
tmp = q_of_z.*(1-q_of_z);
V_Nc = sum(tmp, 1);
V_Nc_minus_n = repmat(V_Nc, N, 1) - tmp;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function E_pi = mk_E_pi(hp_posterior, hp_prior, opts);
if isequal(opts.algorithm, 'cdp') | isequal(opts.algorithm, 'non_dp')
  E_pi = hp_posterior.tilde_alpha / sum(hp_posterior.tilde_alpha);
elseif isequal(opts.algorithm, 'bj')
  second_term = [0 cumsum(log(hp_posterior.gamma(2,:)) - log(sum(hp_posterior.gamma)),2)]; % 1 by K
  E_pi = exp([(log(hp_posterior.gamma(1,:)) ...
               - log(sum(hp_posterior.gamma,1)) ...
               + second_term(1:end-1)) ...
              , second_term(end)]); % 1 by K
elseif isequal(opts.algorithm, 'vdp')
  opts_tmp = opts;
  opts_tmp.algorithm = 'bj';
  E_pi = mk_E_pi(hp_posterior, hp_prior, opts_tmp);
  E_pi(end) = max(1 - sum(E_pi(1:end-1)), 0);
elseif isequal(opts.algorithm, 'csb')
  [N,K] = size(hp_posterior.q_of_z);
  a = 1 + hp_posterior.Nc; % 1 by K
  b = hp_prior.alpha + N - cumsum(hp_posterior.Nc, 2); % 1 by K
  first_term = a ./ (a+b);
  first_term(end) = 1;
  second_term = cumprod(b./(a+b)) ./ (b./(a+b));
  E_pi = first_term .* second_term;
else
  error('not supported algorithm')
end

%
% p_TSB(z_test|Z)
%
% \begin{align*}
% p_{\text{TSB}}(Z) =&
% \alpha^{T-1}
% \prod_{i<T}
% \frac
% {\Gamma(1+N_i) \Gamma(\alpha + N_{>i})}
% {\Gamma(1+\alpha+N_{\geq i})}
% \\
% p_{\text{TSB}}(z_{test}=t,Z) = &
% \left(
% \frac{1+N_t}{1+\alpha+N_{\geq t}}
% \right)^{\mathbb{I}(t<T)}
% \left\{
% \prod_{j<t}\frac
% {\alpha + N_{>j}}
% {1+\alpha+N_{\geq j}}
% \right\}
% \alpha^{T-1}
% \prod_{i<T}
% \frac
% {\Gamma(1+N_i) \Gamma(\alpha + N_{>i})}
% {\Gamma(1+\alpha+N_{\geq i})}
% \\
% p_{\text{TSB}}(z_{test}=t|Z) = &
% \left(
% \frac{1+N_t}{1+\alpha+N_{\geq t}}
% \right)^{\mathbb{I}(t<T)}
% \prod_{j<t}\frac
% {\alpha + N_{>j}}
% {1+\alpha+N_{\geq j}}
% \end{align*}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q_of_z = sequential_importance_sampling(data, hp_prior, opts);
disp('start SIS.')
N = size(data.given_data, 2);
for r = 1:opts.sis
  I = randperm(N);
%   I = I(1:ceil(N*0.1));
  data_r.given_data = data.given_data(:,I);
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   data_r_sub.given_data = data_r.given_data(:,1);
%   q_of_z = ones(1,opts.initial_K) / opts.initial_K;
%   hp_posterior = mk_hp_posterior(data_r_sub, q_of_z, hp_prior, opts);
%   for n = 1:N
%     data_r_sub.given_data = data_r.given_data(:,1:n);
%     q_of_z = mk_q_of_z(data_r_sub, hp_posterior, hp_prior, opts);
%     q_of_z = sort_q_of_z(data, q_of_z, opts);
%     hp_posterior = mk_hp_posterior(data_r_sub, q_of_z, hp_prior, opts);
%   end
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  q_of_z = [];
  for n = 1:size(data_r.given_data,2)
    data_r_sub.given_data = data_r.given_data(:,1:n);
    q_of_z(n,:) = ones(1, opts.initial_K) / opts.initial_K;
    hp_posterior = mk_hp_posterior(data_r_sub, q_of_z, hp_prior, opts);
    q_of_z = mk_q_of_z(data_r_sub, hp_posterior, hp_prior, opts);
%     q_of_z = sort_q_of_z(data, q_of_z, opts);
  end
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  hist(r).hp_posterior = hp_posterior;
  hist(r).free_energy = mk_free_energy(data, hp_posterior, hp_prior, opts);
  [min_f, best_r] = min([hist(:).free_energy]);
  hp_posterior = hist(best_r).hp_posterior;
  disp(['SIS: ' num2str(r) ';  best f = ' num2str(min_f) ';  best Nc = ' num2str(hp_posterior.Nc)])
end
disp('SIS done.')
q_of_z = mk_q_of_z(data, hp_posterior, hp_prior, opts);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function nonkdtree_q_of_z = mk_non_kdtree_q_of_z(data, q_of_z);
nonkdtree_q_of_z = zeros(size(data.given_data,2), size(q_of_z, 2)); % N*K
for a=1:length(data.kdtree);
  nonkdtree_q_of_z(data.kdtree(a).indices,:) = repmat(q_of_z(a,:), length(data.kdtree(a).indices), 1);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [free_energy, hp_posterior, data] = greedy(data, hp_posterior, hp_prior, opts);
free_energy = mk_free_energy(data, hp_posterior, hp_prior, opts);
disp_status(free_energy, hp_posterior, opts);
while 1
  disp('finding the best one....')
  [new_free_energy, new_hp_posterior, new_data, c] = find_best_splitting(data, ...
                                                    hp_posterior, ...
                                                    hp_prior, opts);
  if c == -1
    break
  end
  disp(['finding the best one.... done.  component ' num2str(c) ' was split.'])
  disp_status(new_free_energy, new_hp_posterior, opts);
  [new_free_energy, new_hp_posterior, new_data] = update_posterior2(new_data, ...
                                                    new_hp_posterior, ...
                                                    hp_prior, opts, 1, opts.ite);
  if free_energy_improved(free_energy, new_free_energy, 0, opts) == 0
    break
  end
  free_energy = new_free_energy;
  hp_posterior = new_hp_posterior;
  data = new_data;
end
disp_status(free_energy, hp_posterior, opts);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [free_energy, hp_posterior, data, c] = find_best_splitting(data, ...
                                                  hp_posterior, ...
                                                  hp_prior, opts);
c_max = 10;
K = size(hp_posterior.m, 2);
candidates = find(hp_posterior.Nc>2);
if isempty(candidates)
  free_energy = 0;
  c = -1;
  return
end
q_of_z = mk_q_of_z(data, hp_posterior, hp_prior, opts);
new_free_energy = ones(1,max(candidates))*inf;
%%%%%%%%%%%%%%%%%%%%%
fc = mk_E_log_q_p_eta(data, hp_posterior, hp_prior, opts);
log_lambda = mk_log_lambda(data, hp_posterior, hp_prior, opts);
%%%%%%%%%%%%%%%%%%%%%
for c = candidates(1:min(c_max, length(candidates)))
  disp(['splitting ' num2str(c) '...'])
  [new_data(c), new_q_of_z, info] = split(c, data, q_of_z, hp_posterior, hp_prior, opts, 1);
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  new_c = info.new_c;
  relating_n = find(sum(new_q_of_z(:,[c new_c]),2) > 0.5);
  if isempty(relating_n)
    continue
  end
  new_K = size(new_q_of_z, 2);
  sub_q_of_z = new_q_of_z(relating_n, [c new_c new_K]);
  if opts.use_kd_tree
    sub_data.kdtree = new_data(c).kdtree(relating_n);
  else
    sub_data.given_data = new_data(c).given_data(:,relating_n);
  end
  sub_hp_posteior = mk_hp_posterior(sub_data, sub_q_of_z, hp_prior, opts);
  [sub_f, sub_hp_posteior, dummy, sub_q_of_z] = update_posterior2(sub_data, ...
                                                    sub_hp_posteior, ...
                                                    hp_prior, opts, 0, 10, 0);
  if size(sub_q_of_z,2) < 3
    continue
  end
  if length(find(sum(sub_q_of_z,1)<1.0e-10)) > 1
    continue
  end
  new_log_lambda = log_lambda;
  if opts.use_kd_tree
    updated_data.kdtree = new_data(c).kdtree(info.updated_I);
    new_log_lambda(info.updated_I,:) = mk_log_lambda(updated_data, hp_posterior, hp_prior, opts);
  end
  sub_log_lambda = mk_log_lambda(new_data(c), sub_hp_posteior, hp_prior, opts);
  insert_indices = [c new_c new_K:(new_K+size(sub_q_of_z,2)-3)];
  new_log_lambda(:,insert_indices) = sub_log_lambda;
  new_fc = fc;
  new_fc(insert_indices) = mk_E_log_q_p_eta(sub_data, sub_hp_posteior, hp_prior, opts);
  new_free_energy(c) = mk_free_energy(new_data(c), sub_hp_posteior, hp_prior, opts, new_fc, new_log_lambda);
  new_q_of_z(relating_n,:) = 0;
  new_q_of_z(relating_n,insert_indices) = sub_q_of_z;
  new_q_of_z_cell{c} = new_q_of_z;
end
[free_energy, c] = min(new_free_energy);
if isinf(free_energy)
  c = -1;
  return
end
data = new_data(c);
hp_posterior = mk_hp_posterior(data, new_q_of_z_cell{c}, hp_prior, opts);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [free_energy, hp_posterior, data] = split_merge(data, hp_posterior, hp_prior, opts);
%
% split:
% tdp: if an empty cluster exists, split is available.
%      otherwise, make a new empty cluster. =>  K<-K+1.
%
% non-dp: if
%
c_max = 3;
[free_energy, hp_posterior] = update_posterior2(data, hp_posterior, hp_prior, opts, 0, opts.ite);
while 1
  K = size(hp_posterior.m, 2);
  new_free_energy = inf;
  %%% split
  if ~ opts.do_split
    break
  end
  disp(['### start splitting'])
  [dummy, candidates_for_splliting] = sort(hp_posterior.Nc, 2, 'descend');
  candidates_for_splliting(find(hp_posterior.Nc(candidates_for_splliting)<1)) = [];
  q_of_z = mk_q_of_z(data, hp_posterior, hp_prior, opts); % N*K
  for c=candidates_for_splliting(1:min(c_max, length(candidates_for_splliting)))
    disp(['### start splitting k=' num2str(c) '...'])
    [new_data, new_q_of_z] = split(c, data, q_of_z, hp_posterior, hp_prior, opts);
    new_hp_posterior = mk_hp_posterior(new_data, new_q_of_z, hp_prior, opts);
    [new_free_energy, new_hp_posterior, new_data] = update_posterior2(new_data, ...
                                                      new_hp_posterior, ...
                                                      hp_prior, ...
                                                      opts, 1);
    if free_energy_improved(free_energy, new_free_energy, 0, opts)
      disp(['### splitting k=' num2str(c) ' improved the free energy.  ' ...
           num2str(free_energy) ' -> ' num2str(new_free_energy)])
      break
    else
      disp(['### splitting k=' num2str(c) ' did not improve the free energy.'])
    end
  end
  disp('### end splitting')
  if free_energy_improved(free_energy, new_free_energy, 0, opts)
    free_energy = new_free_energy;
    hp_posterior = new_hp_posterior;
    data = new_data;
    continue
  end
  if ~ opts.do_merge
    break
  end
  %%% merge
  disp(['### start merging'])
  q_of_z = mk_q_of_z(data, hp_posterior, hp_prior, opts); % N*K
  candidates_for_merging = mkcandidates_to_merge(q_of_z'); % 2*#pairs
  for i=1:min(c_max, size(candidates_for_merging,2))
    pair = candidates_for_merging(:,i);
    c1 = pair(1); c2 = pair(2);
    disp(sprintf('### start merging c1=%d, c2=%d ...', c1, c2))
    new_q_of_z = q_of_z;
    new_q_of_z(:,c1) = new_q_of_z(:,c1) + new_q_of_z(:,c2);
    new_q_of_z(:,c2) = zeros(size(new_q_of_z,1),1);
    new_hp_posterior = mk_hp_posterior(data, new_q_of_z, hp_prior, opts);
    [new_free_energy, new_hp_posterior] = update_posterior2(data, ...
                                                      new_hp_posterior, ...
                                                      hp_prior, opts);
    if free_energy_improved(free_energy, new_free_energy, 0, opts)
      disp(sprintf('### merging c1=%d, c2=%d improved the free energy.  %0.5g -> %0.5g', ...
                   c1, c2, free_energy, new_free_energy))
      break
    else
      disp(sprintf('### merging c1=%d, c2=%d did not improve the free energy.', c1, c2))
    end
  end
  disp(['### end merging'])
  if free_energy_improved(free_energy, new_free_energy, 0, opts)
    free_energy = new_free_energy;
    hp_posterior = new_hp_posterior;
    continue
  end
  break
end % while 1
disp_status(free_energy, hp_posterior, opts);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function log_p_of_x_given_c = mk_map_log_p_of_x_given_c(data, clusters, hp_posterior, opts);
% clusters: e.g [1:K]
[D,N] = size(data);
K = length(clusters);
log_p_of_x_given_c = zeros(K,N);
for i = 1:K
  c = clusters(i);
  m = hp_posterior.m(:,c);
  precision = hp_posterior.inv_B(:,:,c)*hp_posterior.eta(c);
  d = data - repmat(m, 1, N);
  log_p_of_x_given_c(i,:) = (-D*0.5)*log(2*pi)+0.5*detln(precision)-0.5*sum(d.*(precision*d),1);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [new_data, new_q_of_z, info] = split(c, data, q_of_z, ...
                                              hp_posterior, hp_prior, opts, update_kdtree)
% q_of_z: N*K
if nargin < 7
  update_kdtree = 1;
end
new_data = data;
if opts.use_kd_tree & update_kdtree
  [dummy, indices] = max(q_of_z,[],2);
  target_partitions = find(indices==c)';
  if ~ isempty(target_partitions)
    m = [data.kdtree(target_partitions).mean];
    log_p_of_m_given_c = mk_map_log_p_of_x_given_c(m, c, hp_posterior, opts); % 1*|target_partitions|
    [dummy, I] = sort(log_p_of_m_given_c, 2, 'descend');
    target_partitions = target_partitions(I(1:ceil(length(I)*opts.max_target_ratio)));
    %   target_partitions = find(q_of_z(:,c)>0.01)';
    [new_data, q_of_z, updated_I] = expand_all_nodes(new_data, q_of_z, hp_posterior, ...
                                                     hp_prior, target_partitions, opts);
    info.updated_I = updated_I;
  else
    info.updated_I = [];
  end
end
if isequal(opts.init_of_split, 'pc')  % principal eigenvector
  if opts.use_kd_tree
    arg1_data = [new_data.kdtree(:).mean];
  else
    arg1_data = new_data.given_data;
  end
  dir = divide_by_principal_component(arg1_data, ...
                                      hp_posterior.B{c}/hp_posterior.eta(c), ...
                                      hp_posterior.m(:,c));
  q_of_z_c1 = zeros(size(q_of_z,1),1);
  q_of_z_c2 = q_of_z(:,c);
  I = find(dir>=0);
  q_of_z_c1(I) = q_of_z(I,c);
  q_of_z_c2(I) = 0;
else
  q_of_z_c = q_of_z(:,c);
  if isequal(opts.init_of_split, 'rnd')  % random
    r = rand(size(q_of_z,1),1);
  elseif isequal(opts.init_of_split, 'rnd_close')  % make close clusters
    r = 0.5 + (rand(size(q_of_z,1),1)-0.5)*0.01;
  elseif isequal(opts.init_of_split, 'close_f')  % one is almost zero.
    r = 0.98 + rand(size(q_of_z,1),1)*0.01;
  else
    init_of_split = opts.init_of_split
    error('unknown option')
  end
  q_of_z_c1 = q_of_z_c.*r;
  q_of_z_c2 = q_of_z_c.*(1-r);
end
new_q_of_z = zeros(size(q_of_z,1), size(q_of_z,2)+1);
new_q_of_z(:,[1:end-2 end]) = q_of_z;
new_q_of_z(:,c) = q_of_z_c1;
new_c = size(new_q_of_z, 2) - 1;
new_q_of_z(:,new_c) = q_of_z_c2;
info.new_c = new_c;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [new_data, new_q_of_z, updated_I] = expand_all_nodes(data, ...
                                                  q_of_z, hp_posterior, ...
                                                  hp_prior, ...
                                                  target_partitions, opts);
if size(target_partitions, 1) ~= 1
  error('target_partitions must be a row vector.')
end
new_data = data;
for a=target_partitions
  if isempty(data.kdtree(a).children)
    children = mk_children_of_partition(data.kdtree(a));
    data.kdtree(a).children = children;
  else
    children = data.kdtree(a).children;
  end
  if length(children) == 1
    continue
  end
  new_data.kdtree(a) = children(1);
  new_data.kdtree(end+1) = children(2);
end % while a <= length(data.kdtree)
updated_I = [target_partitions length(data.kdtree)+1:length(new_data.kdtree)];
sub_data.given_data = data.given_data;
sub_data.kdtree = new_data.kdtree(updated_I);
sub_q_of_z = mk_q_of_z(sub_data, hp_posterior, hp_prior, opts);
new_q_of_z = zeros(length(new_data.kdtree),size(q_of_z,2));
new_q_of_z(1:size(q_of_z,1),:) = q_of_z;
new_q_of_z(updated_I,:) = sub_q_of_z;
disp(['### building kd-tree done; #partition ' num2str(length(data.kdtree)) ...
      ' -> ' num2str(length(new_data.kdtree))])

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [data, q_of_z] = expand_recursively_until_convergence(data, ...
                                                  q_of_z, hp_posterior, ...
                                                  hp_prior, opts);
kdtree_size = length(data.kdtree);
for a=1:length(data.kdtree)
  [data, q_of_z] = expand_recursively_until_convergence2(data, a, ...
                                                    q_of_z, hp_posterior, ...
                                                    hp_prior, opts, ...
                                                    opts.recursive_expanding_depth);
end
[prob, best_c] = max(q_of_z, [], 2);
% for c=1:size(q_of_z,2);
%   I_n =
% end
disp(['### building kd-tree done; #partition ' num2str(kdtree_size) ...
      ' -> ' num2str(length(data.kdtree))])

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [data, q_of_z] = expand_recursively_until_convergence2(data, a, ...
                                                  q_of_z, hp_posterior, ...
                                                  hp_prior, opts, depth);
% q_of_z : N*K
if depth == 0
  return
end
if isempty(data.kdtree(a).children)
  children = mk_children_of_partition(data.kdtree(a));
  data.kdtree(a).children = children;
else
  children = data.kdtree(a).children;
end
if length(children) == 1
  return
end
sub_data.given_data = data.given_data;
sub_data.kdtree = children;
sub_q_of_z = mk_q_of_z(sub_data, hp_posterior, hp_prior, opts); % 2*K
diff = sub_q_of_z - repmat(q_of_z(a,:), 2, 1);
if sum(sum(diff.*diff, 1), 2)/prod(size(sub_q_of_z)) < opts.recursive_expanding_threshold
  return
end
b = length(data.kdtree)+1;
data.kdtree(a) = children(1);
data.kdtree(b) = children(2);
q_of_z([a b],:) = sub_q_of_z;
[data, q_of_z] =  expand_recursively_until_convergence2(data, a, ...
                                                  q_of_z, hp_posterior, ...
                                                  hp_prior, opts, depth-1);
[data, q_of_z] =  expand_recursively_until_convergence2(data, b, ...
                                                  q_of_z, hp_posterior, ...
                                                  hp_prior, opts, depth-1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [free_energy, hp_posterior, data, q_of_z] = update_posterior2(data, hp_posterior, hp_prior, opts, upkdtree, ite, do_sort);
% update q_of_z: N*K
disp(['### updating posterior ...'])
free_energy = inf;
if nargin == 4
  upkdtree = 0;
end
if nargin < 6
  ite = inf;
end
if nargin < 7
  do_sort = 1;
end
i = 0;
last_Nc = 0;
start_sort = 0;
while 1
  i = i+1;
  [new_free_energy, log_lambda] = mk_free_energy(data, hp_posterior, hp_prior, opts);
  disp_status(new_free_energy, hp_posterior, opts);
  if (~isinf(ite) && i>=ite) || ...
        (isinf(ite) && free_energy_improved(free_energy, new_free_energy, 0, opts) == 0)
    free_energy = new_free_energy;
    if do_sort && opts.do_sort && ~ start_sort
      start_sort = 1;
    else
      break
    end
  end
  last_Nc = hp_posterior.Nc;
  free_energy = new_free_energy;
  [q_of_z, data] = mk_q_of_z(data, hp_posterior, hp_prior, opts, log_lambda);
  freq = opts.recursive_expanding_frequency;
  if opts.use_kd_tree & upkdtree & (freq==1 | mod(i,freq)==1)
    [data, q_of_z] = expand_recursively_until_convergence(data, ...
                                                      q_of_z, ...
                                                      hp_posterior, ...
                                                      hp_prior, opts);
  end
  % check if the last component is small enough
  if isequal(opts.algorithm, 'vdp') & sum(q_of_z(:,end)) > 1.0e-20
    q_of_z(:,end+1) = 0;
  end
  if start_sort
    q_of_z = sort_q_of_z(data, q_of_z, opts);
  end
  if isequal(opts.algorithm, 'vdp') & sum(q_of_z(:,end-1)) < 1.0e-10
    q_of_z(:,end-1) = [];
  end
  hp_posterior = mk_hp_posterior(data, q_of_z, hp_prior, opts);
end
% disp_status(free_energy, hp_posterior, opts);
disp(['### updating posterior ... done.'])

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [q_of_z, I] = sort_q_of_z(data, q_of_z, opts);
disp('sorting...')
if opts.use_kd_tree
  Nc = [data.kdtree(:).N]*q_of_z; % 1*K
else
  Nc = sum(q_of_z, 1); % 1*K
end
if isequal(opts.algorithm, 'vdp')
  [dummy,I] = sort(Nc(1:end-1), 2, 'descend');
  I(end+1) = length(Nc);
else
  [dummy,I] = sort(Nc, 2, 'descend');
end
q_of_z = q_of_z(:,I);
disp('sorting... done.')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function disp_status(free_energy, hp_posterior, opts);
if isequal(opts.algorithm, 'vdp')
  Nc = hp_posterior.true_Nc;
else
  Nc = hp_posterior.Nc;
end
disp(['F=' num2str(free_energy) ...
      ';    Nc=[' num2str(Nc, ' %0.5g ') ...
      '];'])

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function bool = free_energy_improved(free_energy, new_free_energy, warn_when_increasing, opts);
diff = new_free_energy - free_energy;
if abs(diff/free_energy) < opts.threshold
  bool = 0;
elseif diff > 0
  if warn_when_increasing
    if abs(diff/free_energy) > 1.0e-3
      error(['the free energy increased.  the diff is ' num2str(new_free_energy-free_energy)])
    else
      warning(['the free energy increased.  the diff is ' num2str(new_free_energy-free_energy)])
    end
  end
  bool = 0;
elseif diff == 0
  bool = 0
else
  bool = 1;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function partitions = init_kdtree_partitions(given_data, depth);
[D,N] = size(given_data);
root = mk_kdtree_partition(given_data, [1:N]);
partitions = expand_recursively(root, depth);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function partitions = expand_recursively(partition, depth);
children = mk_children_of_partition(partition);
if depth == 1 | length(children) == 1
  partitions = children;
else
  partitions1 = expand_recursively(children(1), depth-1);
  partitions2 = expand_recursively(children(2), depth-1);
  partitions = [partitions1 partitions2];
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function children = mk_children_of_partition(partition);
if partition.N == 1
  children = partition;
  return
end
dir = divide_by_principal_component(partition.given_data(:,partition.indices), ...
                                    partition.sigma, ...
                                    partition.mean);
positive_I = find(dir>=0);
negative_I = find(dir<0);
if length(positive_I) == 0 | length(negative_I) == 0
  children = partition;
  return
end
children(1) = mk_kdtree_partition(partition.given_data, partition.indices(positive_I));
children(2) = mk_kdtree_partition(partition.given_data, partition.indices(negative_I));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function direction = divide_by_principal_component(data, covariance, mean);
N = size(data, 2);
if size(data,1) <= 16
  [V,D] = eig(covariance);
  [eig_val, principal_component_i] = max(diag(D));
  principal_component = V(:,principal_component_i);
else
  [principal_component,eig_val] = power_method(covariance);
end
direction = sum((data - repmat(mean, 1, N)).*repmat(principal_component, 1, N), 1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function partition = mk_kdtree_partition(given_data, indices);
partition.N = length(indices);
if partition.N == 0
  error('indices must have at least one index.')
end
data_a = given_data(:,indices);
partition.given_data = given_data;
partition.indices = indices;
partition.sum_x = sum(data_a, 2); % D*1
mean = partition.sum_x / partition.N; % D*1
partition.mean = mean;
partition.sum_xx = data_a*data_a'; % D*D
partition.sigma = partition.sum_xx / partition.N - mean*mean';
partition.children = [];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q_of_z = rand_q_of_z(data, K, opts);
% q_of_z: N*K
if opts.use_kd_tree
  N = length(data.kdtree);
else
  N = size(data.given_data, 2);
end
if isequal(opts.algorithm, 'vdp')
  q_of_z = zeros(N, K+1);
else
  q_of_z = zeros(N, K);
end
q_of_z(:,1:K) = rand(N, K);
q_of_z = normalize(q_of_z, 2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function hp_posterior = mk_hp_posterior(data, q_of_z, hp_prior, opts);
% the last component of q_of_z represents the infinite rest of components
% the last component is the prior.
% q_of_z: N*K
% q_of_z(:,end) is the rest of responsibilities.
threshold_for_N = 1.0e-200;
K = size(q_of_z, 2);
if opts.use_kd_tree
  N = length(data.kdtree);
  D = size(data.kdtree(1).sum_x, 1);
  Na = [data.kdtree(:).N];
  if isequal(opts.algorithm, 'vdp')
    true_Nc = Na*q_of_z; % 1*K
    q_of_z(:,end) = 0;
  end
  Nc = Na*q_of_z; % 1*K
  sum_x = [data.kdtree(:).sum_x] * q_of_z;
else
  [D,N] = size(data.given_data);
  if isequal(opts.algorithm, 'vdp')
    true_Nc = sum(q_of_z, 1); % 1*K
    q_of_z(:,end) = 0;
  end
  Nc = sum(q_of_z, 1); % 1*K
  sum_x = data.given_data * q_of_z;
end
I = find(Nc>threshold_for_N);
inv_Nc = zeros(1,K);
inv_Nc(I) = 1./Nc(I);
hp_posterior.eta = hp_prior.eta0 + Nc;
hp_posterior.xi = hp_prior.xi0 + Nc;
means = sum_x .* repmat(inv_Nc, D, 1); % D*K
hp_posterior.inv_B = zeros(D,D,K);
if opts.use_kd_tree
  t1 = reshape([data.kdtree(:).sum_xx],D,D,N);
  for c=1:K
    v0 = means(:,c) - hp_prior.m0;
    q_of_z_c = reshape(q_of_z(:,c), 1, 1, N);
    S = sum(repmat(q_of_z_c,[D,D,1]).*t1, 3) - Nc(c)*means(:,c)*means(:,c)';
    hp_posterior.B{c} = hp_prior.B0 ...
        + S ...
        + Nc(c)*hp_prior.xi0*v0*v0'/(hp_posterior.xi(c)); % D*D
  end
else
  if opts.collapsed_means
    quad_term = mk_quad_term(data, q_of_z, hp_prior, opts);
  end
  for c=1:K
    if opts.collapsed_means
      hp_posterior.B{c} = hp_prior.B0 ...
          + (repmat(q_of_z(:,c),1,D)'.*data.given_data)*data.given_data' ...
          + quad_term(:,:,c);
    else
      v = data.given_data - repmat(means(:,c),1,N); % D*N
      v0 = means(:,c) - hp_prior.m0;
      hp_posterior.B{c} = hp_prior.B0 ...
          + (repmat(q_of_z(:,c),1,D)'.*v)*v' ...
          + Nc(c)*hp_prior.xi0*v0*v0'/(hp_posterior.xi(c)); % D*D
    end
  end
end
for c=1:K
  hp_posterior.inv_B(:,:,c) = inv(hp_posterior.B{c});
end
hp_posterior.m = (sum_x + repmat(hp_prior.xi0*hp_prior.m0,1,K)) ...
    ./ repmat(Nc+hp_prior.xi0,D,1);
if isequal(opts.algorithm, 'vdp')
  % gamma: 2*K
  hp_posterior.gamma = zeros(2,K);
  hp_posterior.gamma(1,:) = 1 + true_Nc;
  hp_posterior.gamma(2,:) = hp_prior.alpha + sum(true_Nc) - cumsum(true_Nc,2);
elseif isequal(opts.algorithm, 'bj')
  hp_posterior.gamma = zeros(2,K-1);
  hp_posterior.gamma(1,:) = 1 + Nc(1:K-1);
  hp_posterior.gamma(2,:) = hp_prior.alpha + sum(Nc) - cumsum(Nc(1:K-1),2);
elseif isequal(opts.algorithm, 'non_dp') | isequal(opts.algorithm, 'cdp')
  hp_posterior.tilde_alpha = hp_prior.alpha/K + Nc;
elseif isequal(opts.algorithm, 'csb')
  1;
else
  error('unknown algorithm')
end

hp_posterior.Nc = Nc;
if isequal(opts.algorithm, 'vdp')
  hp_posterior.true_Nc = true_Nc;
end
hp_posterior.q_of_z = q_of_z; % q_of_z is a N by K matrix where N is
                              % #given_data or #nodes in a kd-tree.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function fc = mk_E_log_q_p_eta(data, hp_posterior, hp_prior, opts);
% returns E[ log q(eta)/p(eta) ]_q
% fc : 1 by K
D = size(hp_posterior.m, 1);
K = size(hp_posterior.eta, 2);
log_det_B = zeros(1,K);
for c=1:K
  log_det_B(c) = detln(hp_posterior.B{c});
  d = hp_posterior.m(:,c)-hp_prior.m0; % D*1
  term_eta(1,c) = sum(sum(hp_posterior.inv_B(:,:,c).*(hp_prior.xi0*d*d'),1),2);
  term_eta(2,c) = sum(sum(hp_posterior.inv_B(:,:,c).*hp_prior.B0,1),2) - D;
end
E_log_q_p_mean = ...
    + 0.5*D*(hp_prior.xi0./hp_posterior.xi ...
             - log(hp_prior.xi0./hp_posterior.xi) ...
             - 1) ...
    + 0.5*(hp_posterior.eta).* term_eta(1,:);

psi_sum = sum(psi( (repmat(hp_posterior.eta+1,D,1) - repmat([1:D]',1,K))*0.5 ), 1); % 1*K
E_log_q_p_cov = ...
    0.5*hp_prior.eta0*(log_det_B-detln(hp_prior.B0)) ...
    + 0.5*hp_posterior.Nc.*psi_sum ...
    + 0.5*(hp_posterior.eta).* term_eta(2,:) ...
    + gamma_multivariate_ln(hp_prior.eta0*0.5,D) ...
    - gamma_multivariate_ln(hp_posterior.eta*0.5,D);

%debug
if length(find(E_log_q_p_mean<-1.0e-8,1)) > 0
  E_log_q_p_mean
  error('E_log_q_p_mean is negative.')
end
if length(find(E_log_q_p_cov<-1.0e-8,1)) > 0
  E_log_q_p_cov
  error('E_log_q_p_mean is negative.')
end

fc = E_log_q_p_mean + E_log_q_p_cov;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [free_energy, log_lambda] = mk_free_energy(data, hp_posterior, ...
                                                  hp_prior, opts, ...
                                                  fc, log_lambda);
if nargin == 4
  fc = mk_E_log_q_p_eta(data, hp_posterior, hp_prior, opts); % 1*K
  log_lambda = mk_log_lambda(data, hp_posterior, hp_prior, opts); % N*K
end
[N,K] = size(log_lambda);
if isequal(opts.algorithm, 'vdp') || isequal(opts.algorithm, 'bj')
  % note when bj,  if full hp_posterior is given, len_gamma = K - 1.
  len_gamma = size(hp_posterior.gamma, 2);
  if isequal(opts.algorithm, 'bj') && len_gamma ~= K - 1
    error('invalid length')
  end
  E_log_p_of_V = ...
      gammaln(sum(hp_posterior.gamma, 1)) ...
      - gammaln(1+hp_prior.alpha) ...
      - sum(gammaln(hp_posterior.gamma), 1) ...
      + gammaln(hp_prior.alpha) ...
      + ((hp_posterior.gamma(1,:)-1) ...
         .*(psi(hp_posterior.gamma(1,:))-psi(sum(hp_posterior.gamma,1)))) ...
      + ((hp_posterior.gamma(2,:)-hp_prior.alpha) ...
         .*(psi(hp_posterior.gamma(2,:))-psi(sum(hp_posterior.gamma,1))));
  extra_term = sum(E_log_p_of_V);
elseif isequal(opts.algorithm, 'non_dp')
  E_log_p_of_pi = ...
      sum(gammaln(hp_prior.alpha/K) ...
          - gammaln(hp_posterior.tilde_alpha) ...
          + hp_posterior.Nc.*(psi(hp_posterior.tilde_alpha) ...
                              - psi(sum(hp_posterior.tilde_alpha)))) ...
      + gammaln(sum(hp_prior.alpha+size(data,2))) - gammaln(hp_prior.alpha);
  extra_term = E_log_p_of_pi;
elseif isequal(opts.algorithm, 'cdp') || isequal(opts.algorithm, 'csb')
  E_log_p_of_z_given_other_z = ...
      mk_E_log_p_of_z_given_other_z(hp_posterior, hp_prior, opts); % N by K
  q_of_z = mk_q_of_z(data,hp_posterior,hp_prior,opts,log_lambda);
%   q_of_z = hp_posterior.q_of_z;
  E_Nc = hp_posterior.Nc;
  V_Nc = sum(q_of_z.*(1-q_of_z), 1); % 1 by K
  if isequal(opts.algorithm, 'cdp')
    E_log_p_of_z = gammaln(hp_prior.alpha) - gammaln(N+hp_prior.alpha) ...
        + sum(gammaln(hp_prior.alpha/K + E_Nc) ...
              - 0.5*psi(1, hp_prior.alpha/K + E_Nc).*V_Nc - gammaln(hp_prior.alpha/K));
  else % csb
    E_Nc_geq_to_i = cumsum(E_Nc, 2);
    q_of_z_geq_to_i = cumsum(q_of_z, 2);
    V_Nc_geq_to_i = sum(q_of_z_geq_to_i.*(1-q_of_z_geq_to_i), 1);

    E_Nc_greater_than_i = cumsum(E_Nc, 2) - E_Nc;
    q_of_z_greater_than_i = q_of_z_geq_to_i - q_of_z;
    V_Nc_greater_than_i = sum(q_of_z_greater_than_i.*(1-q_of_z_greater_than_i), 1);

    tmp = gammaln(1 + E_Nc) - 0.5*psi(1, 1 + E_Nc).*V_Nc ...  % E[log p(1+Nc)]
          + (gammaln(1 + E_Nc_greater_than_i) ...             % E[log p(1+N_{>c})]
             - 0.5*psi(1, 1 + E_Nc_greater_than_i).*V_Nc_greater_than_i) ...
          - (gammaln(1 + hp_prior.alpha + E_Nc_geq_to_i) ...  % E[log p(1+alpha+N_{>=c})]
             - 0.5*psi(1, 1 + hp_prior.alpha + E_Nc_geq_to_i).*V_Nc_geq_to_i);
    E_log_p_of_z = sum(tmp(1:end-1));
  end
  extra_term = sum(sum(E_log_p_of_z_given_other_z.*q_of_z, 2), 1) - E_log_p_of_z;
else
  error('unknown algorithm')
end
if opts.use_kd_tree
  free_energy = extra_term + sum(fc) - [data.kdtree(:).N]*log_sum_exp(log_lambda, 2);
else
  free_energy = extra_term + sum(fc) - sum(log_sum_exp(log_lambda, 2), 1);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function log_lambda = mk_log_lambda(data, hp_posterior, hp_prior, opts);
% log_lambda: N*K
% q(z_n=c|x_n) = lambda_n_c / sum_c lambda_n_c

if isequal(opts.algorithm, 'vdp')
  if abs(hp_posterior.gamma(2,end) - hp_prior.alpha) > 1.0e-5
    hp_posterior.gamma(2,end)
    hp_prior.alpha
    diff = hp_prior.alpha - hp_posterior.gamma(2,end)
    error('must be alpha')
  end
end

if opts.use_kd_tree
  N = length(data.kdtree);
  D = size(data.kdtree(1).sum_x, 1);
else
  [D,N] = size(data.given_data);
end
K = size(hp_posterior.eta, 2);

psi_sum = sum(psi( (repmat(hp_posterior.eta+1,D,1) - repmat([1:D]',1,K))*0.5 ), 1); % 1*K
log_lambda = zeros(N,K);
if opts.use_kd_tree
  t1 = reshape([data.kdtree(:).sum_xx],D,D,N);
  sum_x = repmat(reshape([data.kdtree(:).sum_x],D,1,N),[1,D,1]);
  Na = repmat(reshape([data.kdtree(:).N],1,1,N),[D,D,1]);
end
if isequal(opts.algorithm, 'csb') | isequal(opts.algorithm, 'cdp')
  E_log_p_of_z_given_other_z = mk_E_log_p_of_z_given_other_z(hp_posterior, hp_prior, opts);
end
for c=1:K
%   Precision = 0.5*hp_posterior.inv_B(:,:,c)*hp_posterior.eta(c);
%   if isequal(opts.algorithm, 'vdp')
%     constant = psi(hp_posterior.gamma(1,c)) - psi(sum(hp_posterior.gamma(:,c),1)) ...
%         + sum(psi(hp_posterior.gamma(2,[1:c-1])) - psi(sum(hp_posterior.gamma(:,[1:c-1]),1)), 2);
%   elseif isequal(opts.algorithm, 'bj')
%     if c < K
%       constant = psi(hp_posterior.gamma(1,c)) - psi(sum(hp_posterior.gamma(:,c),1)) ...
%           + sum(psi(hp_posterior.gamma(2,[1:c-1])) - psi(sum(hp_posterior.gamma(:,[1:c-1]),1)), 2);
%     else
%       constant = sum(psi(hp_posterior.gamma(2,[1:c-1])) - psi(sum(hp_posterior.gamma(:,[1:c-1]),1)), 2);
%     end
%   elseif isequal(opts.algorithm, 'non_dp')
%     % E[log pi] ; pi is the weight of mixtures.
%     constant = psi(hp_posterior.tilde_alpha(c)) - psi(sum(hp_posterior.tilde_alpha));
%   elseif isequal(opts.algorithm, 'cdp')
%     constant = 0;
%   elseif isequal(opts.algorithm, 'csb')
%     constant = 0;
%   else
%     error('unknown algorithm')
%   end
%   constant = constant - 0.5*D*log(pi) - 0.5*detln(hp_posterior.B{c}) ...
%       + 0.5*psi_sum(c) - 0.5*D/(hp_posterior.xi(c));
%   if opts.use_kd_tree
%     t2 = sum_x.*repmat(hp_posterior.m(:,c)',[D,1,N]);
%     term_dependent_on_n = (t1 - t2 - permute(t2,[2,1,3]))./Na + ...
%         repmat(hp_posterior.m(:,c)*hp_posterior.m(:,c)',[1,1,N]);
%     column_c = - sum(sum(repmat(Precision,[1,1,N]).*term_dependent_on_n,2),1) + constant;
%   else
%     d = data.given_data - repmat(hp_posterior.m(:,c),1,N);
%     column_c = - sum(d.*(Precision*d),1) + constant; % 1*N
%   end
%   if isequal(opts.algorithm, 'csb') | isequal(opts.algorithm, 'cdp')
%     column_c = column_c + E_log_p_of_z_given_other_z(:,c)';
%   end
%   log_lambda(:,c) = column_c;
  if isequal(opts.algorithm, 'vdp')
    E_log_p_of_z_given_other_z_c = ...
        psi(hp_posterior.gamma(1,c)) ...
        - psi(sum(hp_posterior.gamma(:,c),1)) ...
        + sum(psi(hp_posterior.gamma(2,[1:c-1])) - psi(sum(hp_posterior.gamma(:,[1:c-1]),1)), 2);
  elseif isequal(opts.algorithm, 'bj')
    if c < K
      E_log_p_of_z_given_other_z_c = ...
          psi(hp_posterior.gamma(1,c)) ...
          - psi(sum(hp_posterior.gamma(:,c),1)) ...
          + sum(psi(hp_posterior.gamma(2,[1:c-1])) - psi(sum(hp_posterior.gamma(:,[1:c-1]),1)), 2);
    else
      E_log_p_of_z_given_other_z_c = sum(psi(hp_posterior.gamma(2,[1:c-1])) ...
                                         - psi(sum(hp_posterior.gamma(:,[1:c-1]),1)), 2);
    end
  elseif isequal(opts.algorithm, 'non_dp')
    % E[log pi] ; pi is the weight of mixtures.
    E_log_p_of_z_given_other_z_c = psi(hp_posterior.tilde_alpha(c)) ...
        - psi(sum(hp_posterior.tilde_alpha));
  elseif isequal(opts.algorithm, 'csb') | isequal(opts.algorithm, 'cdp')
    E_log_p_of_z_given_other_z_c = E_log_p_of_z_given_other_z(:,c)';
  else
    error('unknown algorithm')
  end

  Precision = 0.5*hp_posterior.inv_B(:,:,c)*hp_posterior.eta(c);
  E_log_p_of_x = - 0.5*D*log(pi) - 0.5*detln(hp_posterior.B{c}) ...
      + 0.5*psi_sum(c) - 0.5*D/(hp_posterior.xi(c));
  if opts.use_kd_tree
    t2 = sum_x.*repmat(hp_posterior.m(:,c)',[D,1,N]);
    term_dependent_on_n = (t1 - t2 - permute(t2,[2,1,3]))./Na + ...
        repmat(hp_posterior.m(:,c)*hp_posterior.m(:,c)',[1,1,N]);
    E_log_p_of_x = - sum(sum(repmat(Precision,[1,1,N]).*term_dependent_on_n,2),1) + E_log_p_of_x;
  else
    d = data.given_data - repmat(hp_posterior.m(:,c),1,N);
    E_log_p_of_x = - sum(d.*(Precision*d),1) + E_log_p_of_x; % 1*N
  end
  log_lambda(:,c) = E_log_p_of_x + E_log_p_of_z_given_other_z_c;
end
if isequal(opts.algorithm, 'vdp')
  log_lambda(:,end) = log_lambda(:,end) - log(1- exp(psi(hp_prior.alpha) - psi(1+hp_prior.alpha)));
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [q_of_z, data, log_lambda] = mk_q_of_z(data, hp_posterior, hp_prior, opts, log_lambda);
% q_of_z: N*K
if nargin == 4
  log_lambda = mk_log_lambda(data, hp_posterior, hp_prior, opts);
end
q_of_z = exp(normalizeln(log_lambda, 1));
if opts.weight ~= 1
  q_of_z = q_of_z .* repmat(opts.weight, 1, size(q_of_z,2));
else
  q_of_z = q_of_z;
end

function M =normalizeln(M ,dimension);
M = lpt2lcpt(M, dimension);

function lcpt=lpt2lcpt(lpt,dimension);
% function lcpt=lpt2lcpt(lpt,dimension);
%
% make a log conditional probability table with log probability table
%
% lpt is a matrix.
% dimension must be either 1 or 2.
%
% lcpt = log( exp(lpt) / sum(exp(lpt)) )
%      = lpt - log(sum(exp(lpt)))
%

the_other_dimension=-(dimension-1.5)+1.5;
lpt=permute(lpt,[dimension,the_other_dimension]);
% now we can calculate as if dimension=1.

log_sum_exp_lpt = log_sum_exp(lpt,2); % Mx1
lcpt = lpt - repmat(log_sum_exp_lpt,1,size(lpt,2));

lcpt=permute(lcpt,[dimension,the_other_dimension]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function hp_prior = mk_hp_prior(data, opts)
if isfield(opts, 'xi0')
  hp_prior.xi0 = opts.xi0;
else
  hp_prior.xi0 = 0.01;
end
if isfield(opts, 'eta_p')
  eta_p = opts.eta_p;
else
  eta_p = 1;
end
if isfield(opts, 'alpha')
  hp_prior.alpha = opts.alpha;
else
  hp_prior.alpha = 1;
end
[D,N] = size(data.given_data);
if opts.use_kd_tree
  sum_xx = sum(reshape([data.kdtree(:).sum_xx],D,D,length(data.kdtree)),3);
  sum_x = sum([data.kdtree(:).sum_x],2);
  covariance = sum_xx/N - sum_x*sum_x'/(N*N);
else
  covariance = cov(data.given_data');
end
hp_prior.m0 = mean(data.given_data, 2);
if D > 16
  [dummy, max_eig] = power_method(covariance);
else
  max_eig = max(eig(covariance));
end
hp_prior.eta0 = eta_p * D + 1;
hp_prior.B0 = hp_prior.eta0 * max_eig * eye(D) * hp_prior.xi0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [vec,value]=power_method(A, start, precision)

switch nargin
 case 1
  start = ones(length(A),1);
  precision = 1e-10;
 case 2
  precision = 1e-10;
 case 3
 otherwise
  error 'invalid number of arguments'
end

%
% xp = A^p * x
% x(p+1) = lambda * xp   when p is big enough
% lambda = x(p+1)'*xp / xp'*xp
%
diff=precision+1;
x=start;
n=norm(x)+diff;
i = 0;
while diff> precision
  i = i + 1;
  y=A*x;
  n2 = norm(x);
  diff=abs(n2-n);
  n=n2;
  if n < 1.0e-200
    x = zeros(length(A), 1);
    break
  else
    x=y/n;
  end
  if i > 100
    break
  end
end
n = norm(x);
if n < 1.0e-200
  vec = zeros(length(A), 1);
else
  vec = x/n;
end
value=n;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function m=normalize(m,dim)
% return m normalized with 'dimension'
%
% e.g.
% m : i by j by k by ...
% m = sum(normalize(m,2), 2);
% m(i, :, k, ...) = ones(1, J, 1, ...)
%

dims = ones(1, ndims(m));
dims(dim) = size(m, dim);
m = m ./ repmat(sum(m, dim), dims);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Y] = detln( X )
% Y = logdet( X )
% return a log determinant of X
[d err] = chol(X);
if err
  error('error in Choleski disposition for detln');
end
Y = sum(log(diag(d))) *2;

% Local Variables: ***
% mode: matlab ***
% End: ***


classdef InertialSideSlipAngleConstraint < AbstractConstraint
    %InertialSideSlipAngleConstraint Summary of this class goes here
    %   Detailed explanation goes here

    properties
        normFact = 1;
        event LaunchVehicleEvent
        eventNode(1,1) ConstraintStateComparisonNodeEnum = ConstraintStateComparisonNodeEnum.FinalState;

        lb(1,1) double = 0;
        ub(1,1) double = 0;

        evalType(1,1) ConstraintEvalTypeEnum = ConstraintEvalTypeEnum.FixedBounds;
        stateCompType(1,1) ConstraintStateComparisonTypeEnum = ConstraintStateComparisonTypeEnum.Equals;
        stateCompEvent LaunchVehicleEvent
        stateCompNode(1,1) ConstraintStateComparisonNodeEnum = ConstraintStateComparisonNodeEnum.FinalState;
    end

    methods
        function obj = InertialSideSlipAngleConstraint(event, lb, ub)
            obj.event = event;
            obj.lb = lb;
            obj.ub = ub;

             obj.id = rand();
        end

        function [lb, ub] = getBounds(obj)
            lb = obj.lb;
            ub = obj.ub;
        end

        function [c, ceq, value, lwrBnd, uprBnd, type, eventNum, valueStateComp] = evalConstraint(obj, stateLog, celBodyData)
            type = obj.getConstraintType();

            switch obj.eventNode
                case ConstraintStateComparisonNodeEnum.FinalState
                    stateLogEntry = stateLog.getLastStateLogForEvent(obj.event);

                case ConstraintStateComparisonNodeEnum.InitialState
                    stateLogEntry = stateLog.getFirstStateLogForEvent(obj.event);

                otherwise
                    error('Unknown event node.');
            end

            ut = stateLogEntry.time;
            rVect = stateLogEntry.position;
            vVect = stateLogEntry.velocity;
            bodyInfo = stateLogEntry.centralBody;

            [~,~,inertAngOfSideslip] = stateLogEntry.attitude.getInertialAeroAngles(ut, rVect, vVect, bodyInfo);
            value = rad2deg(inertAngOfSideslip);

            if(obj.evalType == ConstraintEvalTypeEnum.StateComparison)
                switch obj.stateCompNode
                    case ConstraintStateComparisonNodeEnum.FinalState
                        stateLogEntryStateComp = stateLog.getLastStateLogForEvent(obj.stateCompEvent).deepCopy();

                    case ConstraintStateComparisonNodeEnum.InitialState
                        stateLogEntryStateComp = stateLog.getFirstStateLogForEvent(obj.stateCompEvent).deepCopy();

                    otherwise
                        error('Unknown event node.');
                end

                cartElem = stateLogEntryStateComp.getCartesianElementSetRepresentation();
                cartElem = cartElem.convertToFrame(stateLogEntry.centralBody.getBodyCenteredInertialFrame());
                stateLogEntryStateComp.setCartesianElementSet(cartElem);

                ut = stateLogEntryStateComp.time;
                rVect = stateLogEntryStateComp.position;
                vVect = stateLogEntryStateComp.velocity;
                bodyInfo = stateLogEntryStateComp.centralBody;

                [~,~,inertAngOfSideslip] = stateLogEntryStateComp.attitude.getInertialAeroAngles(ut, rVect, vVect, bodyInfo);
                valueStateComp = rad2deg(inertAngOfSideslip);
            else
                valueStateComp = NaN;
            end

            [c, ceq] = obj.computeCAndCeqValues(value, valueStateComp);

            lwrBnd = obj.lb;
            uprBnd = obj.ub;

            eventNum = obj.event.getEventNum();
        end

        function sF = getScaleFactor(obj)
            sF = obj.normFact;
        end

        function setScaleFactor(obj, sF)
            obj.normFact = sF;
        end

        function tf = usesStage(obj, stage)
            tf = false;
        end

        function tf = usesEngine(obj, engine)
            tf = false;
        end

        function tf = usesTank(obj, tank)
            tf = false;
        end

        function tf = usesEngineToTankConn(obj, engineToTank)
            tf = false;
        end

        function tf = usesEvent(obj, event)
            tf = obj.event == event;
            if(obj.evalType == ConstraintEvalTypeEnum.StateComparison)
                tf = tf || obj.stateCompEvent == event;
            end
        end

        function tf = usesStopwatch(obj, stopwatch)
            tf = false;
        end

        function tf = usesExtremum(obj, extremum)
            tf = false;
        end

        function tf = canUseSparseOutput(obj)
            tf = true;
        end

        function event = getConstraintEvent(obj)
            event = obj.event;
        end

        function type = getConstraintType(obj)
            type = 'Inertial Side Slip Angle';
        end

%         function name = getName(obj)
%             name = sprintf('%s - Event %i', obj.getConstraintType(), obj.event.getEventNum());
%         end

        function [unit, lbLim, ubLim, usesLbUb, usesCelBody, usesRefSc] = getConstraintStaticDetails(obj)
            unit = 'deg';
            lbLim = -360;
            ubLim = 360;
            usesLbUb = true;
            usesCelBody = false;
            usesRefSc = false;
        end

        function addConstraintTf = openEditConstraintUI(obj, lvdData)
%             addConstraintTf = lvd_EditGenericMAConstraintGUI(obj, lvdData);

            output = AppDesignerGUIOutput({false});
            lvd_EditGenericMAConstraintGUI_App(obj, lvdData, output);
            addConstraintTf = output.output{1};
        end
    end

    methods(Static)
        function constraint = getDefaultConstraint(~, ~)
            constraint = InertialSideSlipAngleConstraint(LaunchVehicleEvent.empty(1,0),0,0);
        end
    end
end


function [result] = smallMatrix(size)
    result =10^(-50)*ones(size, 'double', 'gpuArray');
end


function [eparams, errors] = msCalibrateEdgeClassifier(efeatures, adjlist, imsegs, eclassifier, labels, ncv)
% [eparams, errors] = calibrateEdgeClassifier(efeatures, adjlist, imsegs,
% eclassifier, ncv)

nimages = numel(imsegs);
for k = 1:ncv
    if ncv > 1
        testind = [(k-1)*nimages/ncv+1:k*nimages/ncv];
        trainind = setdiff([1:nimages], testind);
    else
        trainind = (1:nimages);
    end
    [edata{k}, elab{k}] = formatData(efeatures(trainind), adjlist(trainind), labels(trainind));
    econf{k} = test_boosted_dt_mc(eclassifier, edata{k});
%    econf{k} = 1 ./ (1+exp(-econf{k}));
end

for k = 1:ncv
    disp(['iter: ' num2str(k)])
    if ncv>1
        traink = setdiff([1:ncv], k);
    else
        traink = k;
    end
    eparams{k} = fminunc(@(x) objective(x, cat(1, econf{traink}), cat(1, elab{traink})), [-1 0], optimset('TolFun', 0.001));
end

for k = 1:ncv
    econf{k} = 1 ./ (1+exp(eparams{k}(1)*econf{k}+eparams{k}(2)));
end

elab = cat(1, elab{:});
econf = cat(1, econf{:});

eerror = mean((econf>0.5)~=elab);

econf2 = 1-abs(elab-econf);

ind1 = find(elab==0);
ind2 = find(elab==1);
px = [0.025:0.05:0.975];
f1 = ksdensity(econf(ind1), px, 'support', [0 1]);
f2 = ksdensity(econf(ind2), px, 'support', [0 1]);
fc = ksdensity(econf2, px, 'support', [0 1]);
%fc = fc;

errors.err = eerror;
errors.pneg = f1;
errors.ppos = f2;
errors.conf = fc;
errors.px = px;

medFS = 18;
bigFS = 20;

figure(1), hold on, plot(px, fc, 'y', 'LineWidth', 2);
%axis([0 1 0 1])
xlabel('Confidence in True Label', 'FontSize', medFS)
ylabel('Frequency', 'FontSize', medFS)
title('Same Label Confidence', 'FontSize', bigFS)
set(gca, 'FontSize', medFS)

figure(2), hold on, plot(px, f2 ./ (f1+f2), 'y', 'LineWidth', 2)
hold on, plot(px, px, '--k')
axis([0 1 0 1])
xlabel('Estimated Probability', 'FontSize', medFS)
ylabel('Empirical Probability', 'FontSize', medFS)
%title('Same Label Confidence', 'FontSize', bigFS)
set(gca, 'FontSize', medFS)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function err = objective(param, econf, elab)
econf = 1./(1+exp(param(1)*econf+param(2)));
px = [0.025:0.05:0.975];
f1 = ksdensity(econf(elab==0), px, 'support', [0 1])+1E-10;
f2 = ksdensity(econf(elab==1), px, 'support', [0 1])+1E-10;
f1 = f1 / sum(f1+f2);
f2 = f2 / sum(f1+f2);
err = sum((f1+f2).*(px - f2./(f1+f2)).^2);
disp(num2str([sum((f1+f2).*abs(px - f2./(f1+f2))) param]))

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

function [data, lab] = formatData(features, adjlist, labels)
% concatenate data and select ndata random datapoints

nimages = numel(features);

[tmp, nvars] = size(features{1});

% count edges
ne = 0;
for f = 1:nimages
    ne = ne + size(features{f}, 1);
end

data = zeros(ne, nvars);
lab = zeros(ne, 1);

% concatenate data
c = 0;
for f = 1:nimages
    cf = size(features{f}, 1);
    data(c+1:c+cf, :) = features{f};
    s1 = adjlist{f}(:, 1);
    s2 = adjlist{f}(:, 2);
    lab(c+1:c+cf) = (labels{f}(s1)==labels{f}(s2));
    c = c + cf;
end


function y = kron(X,Y)
%KRON (overloaded)

if isequal(X,1)
    y = Y;
    return;
end

if isequal(Y,1);
    y = X;
end

if (isa(X,'sdpvar') & isa(Y,'sdpvar'))
    y = [];
    s.type = '()';
    for i = 1:size(X,1)
        this_row = [];
        for j = 1:size(X,2);
            s.subs = {i,j};
            this_row = [this_row subsref(X,s)*Y];
        end
        y = [y;this_row];
    end
    return
end

if isa(X,'sdpvar')
    lmi_variables = getvariables(X);
    nv = length(lmi_variables);
    y  = X;
    sparse_Y = sparse(Y);
    % This one used also for checking size
    temp = kron(getbasematrix(X,0),sparse_Y);
    if size(Y,2)==1
        % Special case
        % [kron([N1(:) N2(:)],vec(M))]=[vec(kron(N1,M)) vec(kron(N2,M))]
        y.basis = kron(X.basis,sparse_Y);
    else
        y.basis=temp(:);
        X_base = X.basis;
        for i = 1:nv
           % temp = kron(getbasematrix(X,lmi_variables(i)),sparse_Y);
            temp = kron(reshape(X_base(:,i+1),X.dim),sparse_Y);
            y.basis(:,i+1) = temp(:);
        end;
    end
end

if isa(Y,'sdpvar')
    lmi_variables = getvariables(Y);
    nv = length(lmi_variables);
    y  = Y;
    sparse_X = sparse(X);
    % This one used also for checking size
    temp = kron(sparse_X,getbasematrix(Y,0));
    if size(X,1)==1
        % In this special case
        %[kron(vec(M),[N1(:) N2(:)])]==[vec(kron(M,N1)) vec(kron(M,N2))]
        y.basis = kron(sparse_X(:),Y.basis);
    else
        y.basis = temp(:);
        y.basis = spalloc(length(temp(:)),nv+1,0);
        y.basis(:,1) = temp(:);
        Y_base = Y.basis;
        for i = 1:nv
            % temp = kron(sparse_X,getbasematrix(Y,lmi_variables(i)));
            temp = kron(sparse_X,reshape(Y_base(:,i+1),Y.dim));
            y.basis(:,i+1) = temp(:);
        end
    end
end
y.dim(1) = size(temp,1);
y.dim(2) = size(temp,2);
y = clean(y);
% Reset info about conic terms
if isa(y,'sdpvar')
    y.conicinfo = [0 0];
end


function [g,geq,dg,dgeq,xevaled] = fmincon_con(x,model,xevaled)

global latest_xevaled
global latest_x_xevaled
% Early bail for linear problems
g = [];
geq = [];
dg = [];
dgeq = [];
if model.linearconstraints
    xevaled = [];
    return
end

if nargin<3
    if isequal(x,latest_x_xevaled)
        xevaled = latest_xevaled;
    else
        xevaled = zeros(1,length(model.c));
        xevaled(model.linearindicies) = x;
        xevaled = apply_recursive_evaluation(model,xevaled);
        latest_x_xevaled = x;
        latest_xevaled = xevaled;
    end
end

if model.nonlinearinequalities
    g = full(model.Anonlinineq*xevaled(:)-model.bnonlinineq);
end

if nnz(model.K.q) > 0
    top = 1;
    for i = 1:length(model.K.q)
        z = model.F_struc(top:top+model.K.q(i)-1,:)*[1;xevaled];
      %  g = [g;-(z(1)^2 - z(2:end)'*z(2:end))];
        g = [g;-(z(1) - sqrt(z(2:end)'*z(2:end)))];
        top = top + model.K.q(i);
    end
end

if model.nonlinearequalities
    geq = full(model.Anonlineq*xevaled(:)-model.bnonlineq);
end

dgAll_test = [];

if nargout == 2 || ~model.derivative_available
    return
elseif ~isempty(dgAll_test) & isempty(model.evalMap)
    dgAll = dgAll_test;
elseif isempty(model.evalMap) & (model.nonlinearinequalities==0) & (model.nonlinearequalities==0) & (model.nonlinearcones==0) & any(model.K.q)
    dg = computeConeDeriv(model,xevaled);
elseif isempty(model.evalMap) & (model.nonlinearinequalities | model.nonlinearequalities | model.nonlinearcones)
    n = length(model.c);
    linearindicies = model.linearindicies;
 %   xevaled = zeros(1,n);
 %   xevaled(linearindicies) = x;
    % FIXME: This should be vectorized

    news = model.fastdiff.news;
    allDerivemt = model.fastdiff.allDerivemt;
    c = model.fastdiff.c;

    if model.fastdiff.univariateDifferentiates
        zzz = c.*(x(model.fastdiff.univariateDiffMonom).^model.fastdiff.univariateDiffPower);
    else
        %  X = repmat(x(:)',length(c),1);
        O = ones(length(c),length(x));
        nz = find(allDerivemt);
        %  O(nz) = X(nz).^allDerivemt(nz);
        O(nz) = x(ceil(nz/length(c))).^allDerivemt(nz);
        zzz = c.*prod(O,2);
    end

    newdxx = model.fastdiff.newdxx;
    newdxx(model.fastdiff.linear_in_newdxx) = zzz;
    %newdxx = newdxx';

    if ~isempty(model.Anonlineq)
        dgeq = model.Anonlineq*newdxx;
    end
    if ~isempty(model.Anonlinineq)
        dg = model.Anonlinineq*newdxx;
    end

    if nnz(model.K.q)>0
        dg = [dg;computeConeDeriv(model,xevaled,newdxx);];
    end
else
    requested = model.fastdiff.requested;
    dx = apply_recursive_differentiation(model,xevaled,requested,model.Crecursivederivativeprecompute);
    conederiv = computeConeDeriv(model,xevaled,dx);
    if ~isempty(model.Anonlineq)
        dgeq = [model.Anonlineq*dx];
    end
    if ~isempty(model.Anonlinineq)
        dg = [model.Anonlinineq*dx];
    end
    if ~isempty(conederiv)
        dg = [dg;conederiv];
    end
end

if model.nonlinearequalities
    dgeq = dgeq';
end
if model.nonlinearinequalities | any(model.K.q)
    dg = dg';
end

function conederiv = computeConeDeriv(model,z,dzdx)
conederiv = [];
z = z(:);
if any(model.K.q)
    top = 1 + model.K.f + model.K.l;
    for i = 1:length(model.K.q)
        d = model.F_struc(top,1);
        c = model.F_struc(top,2:end)';
        b = model.F_struc(top+1:top+model.K.q(i)-1,1);
        A = model.F_struc(top+1:top+model.K.q(i)-1,2:end);

        if nargin == 2
            % No inner derivative
            A = A(:,model.linearindicies);
            c = c(model.linearindicies);
            % -c'*x - d + ||Ax+b||>=0
            e = A*z(model.linearindicies) + b;
            smoothed = sqrt(10^-10 + e'*e);
            conederiv = [conederiv;-c'+(A'*b + A'*A*z(model.linearindicies))'/smoothed];
           % conederiv = [conederiv;(2*A(:,model.linearindicies)'*(A(:,model.linearindicies)*z(model.linearindicies)+b)-2*c(model.linearindicies)*(c(model.linearindicies)'*z(model.linearindicies)+d))'];
        else

            % -c'*x - d + ||Ax+b||>=0
            e = A*z + b;
            smoothed = sqrt(10^-10 + e'*e);
            % conederiv = [conederiv;-c'+(dzdx'*A'*b + dzdx'*A'*A*z(model.linearindicies))'/smoothed];
            aux = z'*(A'*A-c*c')*dzdx+(b'*A-d*c')*dzdx;
            conederiv = [conederiv;(-dzdx'*c+aux'/smoothed)'];

            % inner derivative
            % aux = 2*z'*(A'*A-c*c')*dzdx+2*(b'*A-d*c')*dzdx;
            % conederiv = [conederiv;aux];

        end
        top = top + model.K.q(i);
    end
end


function x = p42_start ( n )

%*****************************************************************************80
%
%% P42_START returns a starting point for optimization for problem 42.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 March 2002
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables X.
%
%    Output, real X(N), a starting point for the optimization.
%
  x = [ 0.0, 1.0, 2.0 ]';

  return
end


function myrng()
% Author		: Tiep Vu (http://www.personal.psu.edu/thv102/)
% Time created	: Wed Jan 27 00:23:58 2016
% Last modified	: Wed Jan 27 00:23:59 2016
% Description	: make sure that random function does not return
%                 the same output

    c = clock; %get current time

    t = mod(floor(c(6))*13, 100); % c(6) is 'second'
    for i = 1: t
        randi(t);
    end
end


function rfParams = rmPlotGUI_getRFParams(model, modelName, viewType, coords, params)
% For the retinotopy model GUI: get PRF params for a single voxel.
%
%  rmParams = rmPlotGUI_getRFParams(model, modelName, viewType, coords, params)
%
% ras, 11/2008. Trying to make the large GUI code more modular and
% more manageable.
switch modelName,
    case {'2D pRF fit (x,y,sigma, positive only)',...
          '2D RF (x,y,sigma) fit (positive only)',...
          '1D pRF fit (x,sigma, positive only)'};
        rfParams = zeros(1,6);

        rfParams(1) = rmCoordsGet(viewType, model, 'x0', coords);
        rfParams(2) = rmCoordsGet(viewType, model, 'y0', coords);
        rfParams(3) = rmCoordsGet(viewType, model, 'sigmamajor',coords);
        rfParams(5) = rmCoordsGet(viewType, model, 'sigmaminor',coords);
        rfParams(6) = rmCoordsGet(viewType, model, 'sigmatheta',coords);

    case {'2D pRF fit (x,y,sigma_major,sigma_minor)' ...
            'oval 2D pRF fit (x,y,sigma_major,sigma_minor,theta)'};
        rfParams = zeros(1,4);

        rfParams(1) = rmCoordsGet(viewType, model, 'x0', coords);
        rfParams(2) = rmCoordsGet(viewType, model, 'y0', coords);
        rfParams(3) = rmCoordsGet(viewType, model, 'sigmamajor',coords);
        rfParams(5) = rmCoordsGet(viewType, model, 'sigmaminor',coords);
        rfParams(6) = rmCoordsGet(viewType, model, 'sigmatheta',coords);

    case 'unsigned 2D pRF fit (x,y,sigma)';
        rfParams = zeros(1,4);

        rfParams(1) = rmCoordsGet(viewType, model, 'x0', coords);
        rfParams(2) = rmCoordsGet(viewType, model, 'y0', coords);
        rfParams(3) = rmCoordsGet(viewType, model, 'sigmamajor',coords);

    case {'Double 2D pRF fit (x,y,sigma,sigma2, center=positive)',...
          'Difference 2D pRF fit (x,y,sigma,sigma2, center=positive)',...
          'Difference 1D pRF fit (x,sigma, sigma2, center=positive)'},
        rfParams = zeros(2,4);

        % get RF parameters
        rfParams(1,1) = rmCoordsGet(viewType, model, 'x0', coords);
        rfParams(1,2) = rmCoordsGet(viewType, model, 'y0', coords);
        rfParams(1,3) = rmCoordsGet(viewType, model, 'sigmamajor',coords);
        rfParams(2,1) = rmCoordsGet(viewType, model, 'x0', coords);
        rfParams(2,2) = rmCoordsGet(viewType, model, 'y0', coords);
        rfParams(2,3) = rmCoordsGet(viewType, model, 'sigma2major',coords);

    case {'Two independent 2D pRF fit (2*(x,y,sigma, positive only))',...
          'Mirrored 2D pRF fit (2*(x,y,sigma, positive only))',...
          'Shifted 2D pRF fit (2*(x,y,sigma, positive only))'},
        rfParams = zeros(2,3);

        % get RF parameters
        rfParams(1,1) = rmCoordsGet(viewType, model, 'x0', coords);
        rfParams(1,2) = rmCoordsGet(viewType, model, 'y0', coords);
        rfParams(1,3) = rmCoordsGet(viewType, model, 's',coords);
        rfParams(2,1) = rmCoordsGet(viewType, model, 'x02', coords);
        rfParams(2,2) = rmCoordsGet(viewType, model, 'y02', coords);
        rfParams(2,3) = rmCoordsGet(viewType, model, 's2',coords);

    case {'Sequential 2D pRF fit (2*(x,y,sigma, positive only))'},
        rfParams = zeros(2,3);

        % get RF parameters
        rfParams(1,1) = rmCoordsGet(viewType, model, 'x0', coords);
        rfParams(1,2) = rmCoordsGet(viewType, model, 'y0', coords);
        rfParams(1,3) = rmCoordsGet(viewType, model, 's',coords);
        rfParams(2,1) = rmCoordsGet(viewType, model, 'x02', coords);
        rfParams(2,2) = rmCoordsGet(viewType, model, 'y02', coords);
        rfParams(2,3) = rmCoordsGet(viewType, model, 's2',coords);

    case {'fitprf' 'css' '2D nonlinear pRF fit (x,y,sigma,exponent, positive only)' ...
            '2D nonlinear pRF fit with boxcar (x,y,sigma,exponent, positive only)'}
        rfParams = zeros(1,6);

        rfParams(1) =  rmCoordsGet(viewType, model, 'x0', coords);        % x coordinate (in deg)
        rfParams(2) = rmCoordsGet(viewType, model, 'y0', coords);         % y coordinate (in deg)
        rfParams(3) =  rmCoordsGet(viewType, model, 'sigmamajor',coords); % sigma (in deg)
        rfParams(6) =  rmCoordsGet(viewType, model, 'sigmatheta',coords); % sigma theta (0 unless we have anisotropic Gaussians)
        rfParams(7) =  rmCoordsGet(viewType, model, 'exponent'  ,coords); % pRF exponent
        rfParams(8) =  rmCoordsGet(viewType, model, 'bcomp1',    coords); % gain ?
        rfParams(5) =  rfParams(3) / sqrt(rfParams(7));                   % sigma adjusted by exponent (not for calculations - just for diplay)

    otherwise,
        error('Unknown modelName: %s',modelName{M.modelNum});
end;


function pop = InitializeFun(pop_size, chromosome_size)
pop = round(rand(pop_size, chromosome_size));
end


function [dfuncGrad] = gpsimMapFunctionalLikeGrad2(model, p)

% GPSIMMAPFUNCTIONALLIKEGRAD2 Compute the functional gradient for GPSIMMAP.
% FORMAT
% DESC computes the functional gradient of the log likelihood for
% use in the MAP approximation to the GPSIM posterior solution.
% ARG model : the model for which the gradient is to be computed.
% RETURN p: the gradient of the log likelihood with respect to the
% points of the function.
%
% SEEALSO : gpsimMapLikeGradientImplicit
%
% COPYRIGHT : Pei Gao, Magnus Rattray and Neil D. Lawrence, 2008

% SHEFFIELDML

dfuncGrad = zeros(1, model.numParams-model.kern.nParams);
gB = zeros(model.numGenes, 1);
gD = zeros(model.numGenes, 1);
gS = zeros(model.numGenes, 1);
ngParamk = model.ngParam/model.numGenes;

if isfield(model,'includeNoise') && model.includeNoise
  gn = zeros(model.numGenes, 1);
  noiseMat = ones(length(model.t),1)*model.noiseVar;
  yvar = model.yvar + noiseMat;
else
  yvar = model.yvar;
end

if isfield(model, 'includeRepression') && model.includeRepression
  galpha = zeros(model.numGenes, 1);
  nBaseParams = 4;
else
  nBaseParams = 3;
end

if model.ngParam > 0
  ggParam= zeros(model.numGenes, ngParamk);
end

numData = length(model.t);
for k = 1:model.numGenes
  if model.ngParam
    gInd = k;
  else
    gInd = 1;
  end
  for i=1:numData
    arg = model.t(i)-model.mapt(p);
    if arg >= 0
      ind = i + (k-1)*numData;
      beta_ik=1/yvar(ind);

      [dxdB dxdD dxdS dxdalpha dxdgParam] = gpsimXGradient(model, i, k);

      gB(k) = gB(k) - beta_ik*model.g_grad(p,gInd)*dxdB*exp(-model.D(k)*arg ...
                               +log(model.S(k)) +log(model.step));

      factor = model.ypred(model.times_index(i), k) - model.y(ind);
      gD(k) = gD(k) - beta_ik*model.g_grad(p,gInd)* (dxdD-arg*factor) ...
              *exp(-model.D(k)*arg+log(model.S(k)) +log(model.step));

      gS(k) = gS(k) - beta_ik*model.g_grad(p,gInd)* (dxdS*model.S(k) + ...
                       factor) *exp(-model.D(k)*arg+log(model.step));

      if isfield(model, 'includeRepression') && model.includeRepression
        galpha(k) = galpha(k) - beta_ik*model.g_grad(p,gInd)*dxdalpha ...
            *exp(-model.D(k)*arg+log(model.S(k)) +log(model.step));
      end

      if model.ngParam > 0
        ggParam(k,:) = ggParam(k,:) - beta_ik* (model.g_grad(p,gInd)* ...
            dxdgParam+factor*model.dggrad(p, gInd)) *exp(-model.D(k)* ...
                                arg+log(model.S(k))+log(model.step));
      end

      if isfield(model,'includeNoise') && model.includeNoise
        gn(k) = gn(k) + 2*sqrt(model.noiseVar(k))*beta_ik^2* ...
                model.g_grad(p,gInd)*model.S(k)*factor*exp(-model.D(k)*arg+log(model.step));
      end
    end
  end
end

geneParam = nBaseParams + ngParamk;
startPoint = 1;
endPoint = geneParam;

for k = 1:model.numGenes
  dfuncGrad(startPoint:(startPoint+2)) = [gB(k) gS(k) gD(k)];

  if isfield(model, 'includeRepression') && model.includeRepression
    dfuncGrad(startPoint+3) = galpha(k);
  end

  if model.ngParam > 0
    dfuncGrad((startPoint+nBaseParams):endPoint) = ggParam(k,:)';
  end
  startPoint = startPoint + geneParam;
  endPoint = endPoint + geneParam;
end

if isfield(model,'includeNoise') && model.includeNoise
  dfuncGrad(end-length(gn)+1:end) = gn';
end


% Visualize faces in a face recognition model.
% The model can be downloaded from
% https://github.com/AlfredXiangWu/face_verification_experiment .
% This model is trained with CASIA-webface, People_ID can be set from 1
% to 10575, to get the memorized face of the corresponding person.

% Based on paper:
% Feng Wang, Haijun Liu, Jian Cheng,
% Visualizing Deep Neural Network by Alternately Image Blurring and Deblurring
caffe.reset_all();
caffe.set_mode_gpu();
gpu_id = 0;  % we will use the first gpu in this demo
caffe.set_device(gpu_id);

% net_model = 'DeepFace_set003_inception.prototxt';% Please, remember to add force_backward:true to this file.
% net_weights = 'D:\project\lfw_face_verification_experiment\model\DeepFace_set003_net_iter.caffemodel';
net_model = 'LightenedCNN_B_deploy.prototxt';% Please, remember to add force_backward:true to this file.
net_weights = 'D:\project\lfw_face_verification_experiment\model\LightenedCNN_B.caffemodel';

mean_file = [];

train_net = caffe.Net(net_model,net_weights,'test');
if isempty(mean_file)
    mean_image = zeros(128,128);
else
    mean_image = caffe.read_mean(mean_file);
end;
for People_ID = 1:10575

% mean_image = mean_image(17:240,17:240,:);
% mean_image = mean_image + randn(size(mean_image));
input_data = zeros(size(mean_image,1), size(mean_image,2), 1, 1, 'single');

mean_file = 'webface_mean.proto';
mean_face = caffe.read_mean(mean_file);
mean_face = mean_face / 256;
input_data(:,:,1,1) = imresize(mean_face,[128,128]);

use_clip = false;
use_cv_norm = false;
use_weight_decay = false;
use_image_blur = false;
use_image_deblur = false;
use_gradient_blur = false;
use_dropout = false;
use_maxGray = false;

H = fspecial('gaussian',[7 7],1.2);
prob = train_net.forward({input_data});
% input_data = input_data - min(input_data(:));
% input_data = input_data / max(input_data(:));
[max_prob,max_idx] = sort(prob{1},'descend');
max_idx = People_ID;%max_idx(1);
this_prob = prob{1}(max_idx);
back_data = ones(size(prob{1}),'single') * -1;
back_data(max_idx) = 1;
back_cell = prob;
back_cell{1} = back_data;
blur_data = zeros(size(input_data));
base_lr = 0.01;
max_lr = 0.01;
lambda1 = 0.00001;
lambda2 = 0.1;
last_prob = -999;
momentum = 0.8;
momentum2 = 0.99;
lastgrad = zeros(size(mean_image));
lastgrad2 = zeros(size(mean_image));
mask = ones(size(mean_image,1), size(mean_image,2));
iter = 1;
dropout = 0.5;

while 1
    lr = base_lr;% * sqrt(this_prob / (1 - this_prob));
    res = train_net.backward(back_cell);

    bak_data = input_data;

    if use_gradient_blur
        res{1} = imfilter(res{1},H,'same');
    end;
    grad = res{1};

    if use_clip
        app_gradient = sum(abs(res{1} .* input_data(:,:,:,1)),3);
        app_gradient = app_gradient < mean(app_gradient(:)) * 0.5;
        clip_grad = reshape(res{1},[size(mean_image,1)*size(mean_image,2) 3]);
        clip_grad(app_gradient==1,:) = 0;
        clip_grad = reshape(clip_grad,size(input_data));
        res{1} = clip_grad;
    end;

    if use_cv_norm
        I = input_data(:,:,:,1);
%         Gx = sign(I(2:end-1,2:end-1,:) - I(1:end-2,2:end-1,:)) - sign(I(3:end,2:end-1,:) - I(2:end-1,2:end-1,:));
%         Gy = sign(I(2:end-1,2:end-1,:) - I(2:end-1,1:end-2,:)) - sign(I(2:end-1,3:end,:) - I(2:end-1,2:end-1,:));
        Gx = smoothL1(I(2:end-1,:,:) - I(1:end-2,:,:)) - smoothL1(I(3:end,:,:) - I(2:end-1,:,:));
        Gx = [smoothL1(I(1,:,:) - I(2,:,:)); Gx; smoothL1(I(end,:,:) - I(end-1,:,:))];
        Gy = smoothL1(I(:,2:end-1,:) - I(:,1:end-2,:)) - smoothL1(I(:,3:end,:) - I(:,2:end-1,:));
        Gy = [smoothL1(I(:,1,:) - I(:,2,:)) Gy smoothL1(I(:,end,:) - I(:,end-1,:))];
%         Gx = sign(I(2:end-1,:,:) - I(1:end-2,:,:)) - sign(I(3:end,:,:) - I(2:end-1,:,:));
%         Gx = [sign(I(1,:,:) - I(2,:,:)); Gx; sign(I(end,:,:) - I(end-1,:,:))];
%         Gy = sign(I(:,2:end-1,:) - I(:,1:end-2,:)) - sign(I(:,3:end,:) - I(:,2:end-1,:));
%         Gy = [sign(I(:,1,:) - I(:,2,:)) Gy sign(I(:,end,:) - I(:,end-1,:))];
            grad = grad - lambda2 * (Gx + Gy);
    end;

    lastgrad = (1 - momentum) * lr * res{1}   + momentum * lastgrad;%/ norm(res{1}(:))
    input_data(:,:,:,1) = input_data(:,:,:,1) + lastgrad;
%     lastgrad = (1 - momentum) * grad   + momentum * lastgrad;%/ norm(res(:))
%     lastgrad2 = (1 - momentum2) * grad.^2 + momentum2 * lastgrad2;%/ norm(res(:))
%     lg_correct = lastgrad ./ (1 - momentum^iter);
%     lg2_correct = lastgrad2 ./ (1 - momentum2^iter);
%     input_data(:,:,:,1) = input_data(:,:,:,1) + lr * lg_correct ./ (sqrt(lg2_correct) + 1e-8);

    if use_weight_decay
        input_data(:,:,:,1) = input_data(:,:,:,1) - lr * lambda1 * I;
    end;
    if use_maxGray
%         if max(input_data(:))>1
            input_data(:,:,:,1) = input_data(:,:,:,1) - min(input_data(:));
            input_data(:,:,:,1) = input_data(:,:,:,1) / max(input_data(:));
%         end;
    end;
%         input_data = (input_data -mean(input_data(:))) / std(input_data(:)) * 30;
%     end;

%     for_forward = reshape(input_data,[size(mean_image,1)*size(mean_image,2) 3]);
%     mask = rand(size(mean_image,1), size(mean_image,2)) < dropout;
%     for_forward(mask==1,:) = 0;
%     for_forward = reshape(for_forward,size(input_data));

    if mod(iter,10) ==0%&&iter<2000
        if mod(iter,20) ~= 0
            H = fspecial('gaussian',[5 5],rand()/2+0.5);
            if use_image_blur
                input_data(:,:,:,1) = imfilter(input_data(:,:,:,1),H,'same');
            end
        else
            if mod(iter,20) == 0
                if use_image_deblur
                    input_data(:,:,:,1) = deconvlucy(input_data(:,:,:,1), H);
                end;
            end;
        end;
    end;
    prob = train_net.forward({input_data});

    this_prob = prob{1}(max_idx);
    fprintf('id=%d,iter=%d,lr=%f,prob1=%f,last_prob=%f\n',People_ID,iter,lr,prob{1}(max_idx),last_prob);
    iter = iter + 1;

    if mod(iter,100)==0
        figure(2);
        % imshow(uint8(mean_image + input_data));
        output = input_data(:,:,:,1);
        output = output';
        imshow(output);
        I = output;
%         Gx = abs(I(2:end-1,2:end-1,:) - I(1:end-2,2:end-1,:)) + abs(I(3:end,2:end-1,:) - I(2:end-1,2:end-1,:));
%         Gy = abs(I(2:end-1,2:end-1,:) - I(2:end-1,1:end-2,:)) + abs(I(2:end-1,3:end,:) - I(2:end-1,2:end-1,:));
%         figure(3);hist(I(:),1000);
%         figure(4);hist(Gy(:),1000);
        if iter == 200
            break;
        end;
    end;
    if this_prob<last_prob
        base_lr = base_lr * 0.99;
%         input_data = bak_data;
    end;
    if this_prob>last_prob&&base_lr<max_lr%&& (this_prob-last_prob) / this_prob < 0.001
        base_lr = base_lr * 1.01;
%         input_data = bak_data;
    end;
%     if this_prob>last_prob
        last_prob = this_prob;
%     end;
    if lr<0.000001
        break;
    end;
end;
output = input_data(:,:,:,1);
output = output';
imwrite(output, ['gallery/' num2str(People_ID) '.png']);
end;


function Z = minus(A,B)
%minus           Overloaded

% Standard case A.cx + A.logdetP - (B.cx + B.logdetP)

if isa(A,'sdpvar') | isa(A,'double')
    if prod(size(A))>1
        error('Only scalar terms can be added to a logdet term');
    end
    Acx = A;
    Alog = {};
    Again = [];
else
    Z = A;
    Acx  = A.cx;
    Alog = A.P;
    Again = A.gain;
end

if isa(B,'sdpvar') | isa(B,'double')
    if prod(size(B))>1
        error('Only scalar terms can be added to a logdet term');
    end
    Bcx = B;
    Blog = {};
    Bgain = [];
else
    Z = B;
    Bcx  = B.cx;
    Blog = B.P;
    Bgain = B.gain;
end

if isempty(Acx)
    Acx = 0;
end
if isempty(Bcx)
    Bcx = 0;
end

if isequal(Acx-Bcx,0)
    Z.cx = [];
else
    Z.cx = Acx - Bcx;
end

Z.P = {Alog{:},Blog{:}};
Z.gain = [Again -Bgain];


function cout = medianfilt(cin, len)

%tstoolbox/@core/medianfilt
%   Syntax:
%     * medianfilt(cin,len)
%
%   Input Arguments:
%     * cin - core object
%
%   moving median filter
%
% Copyright 1997-2001 DPI Goettingen, License http://www.physik3.gwdg.de/tstool/gpl.txt

N = dlens(cin,1); 	% Laenge in der ersten Dimension (sollte auch die einzige sein (skalar))
ts = data(cin);		% Die Datenwerte des Input-Signals werden nach ts kopiert

md = zeros(N-len+1,1);

for i=1:N-len+1
	md(i) = median(ts(i:i+len-1));
end

cout = core(md);


function tapas_sem_trapp_int_check_input(u, theta, ptheta)
% Check whether the input is compatible with mpdcm. If not an error is
% returned.
%
% Input:
% u         -- Structure. Inputs to DCM in mpdcm format.
% theta     -- Structure. Model parameters in mpdcm format.
% ptheta    -- Structure. Priors of the model in mpdcm format.
%
% Output:
%

% aponteeduardo@gmail.com
%
% Author: Eduardo Aponte, TNU, UZH & ETHZ - 2015
% Copyright 2016 by Eduardo Aponte <aponteeduardo@gmail.com>
%

tapas_mpdcm_check_input_u(u);
tapas_mpdcm_check_input_ptheta(ptheta);

check_theta(theta)

su = size(u);
st = size(theta);

assert(su(1) == st(1), ...
    'mpdcm:fmri:int:input:dmatch', ...
    'Dimensions of u and theta doesn''t match')

su = size(u{1});
du = theta{1}.dim_u;
assert(su(1) == du, ...
    'mpdcm:fmri:int:input:theta:dim_u:match_u', ...
    'theta.dim_u doesn''t match u.');
end

function check_theta(theta)
%% Throws an error if something is wrong with theta

assert(iscell(theta), ...
    'mpdcm:fmri:int:input:theta:not_cell', ...
    'theta should be a cell array')
assert(ndims(theta) == 2, ...
    'mpdcm:fmri:int:input:theta:ndim', ...
    'theta should be two dimensional, number of dimensions is %d', ...
    ndims(theta))

ofD = 0;
nfD = 0;

for i = 1:numel(theta)

    assert(isstruct(theta{i}), ...
        'mpdcm:fmri:int:input:theta:cell:not_struct', ...
        'theta{%d} is not struct', i)

    assert(isscalar(theta{i}), ...
        'mpdcm:fmri:int:input:theta:cell:ndim', ...
        'theta{%d} should be 1 x 1', i)

    tapas_mpdcm_check_input_matrix(theta, [1, 1], 'dim_x', i);
    tapas_mpdcm_check_input_matrix(theta, [1, 1], 'dim_u', i);

    if i == 1
        dx = theta{i}.dim_x;
        du = theta{i}.dim_u;
    end

    assert(dx == theta{i}.dim_x, ...
        'mpdcm:fmri:int:input:theta:cell:dim_x:not_match', ...
        'theta{%d}.dim_x doesn''t match previous values', i);

    assert(du == theta{i}.dim_u, ...
        'mpdcm:fmri:int:input:theta:cell:dim_u:not_match', ...
        'theta{%d}.dim_u doesn''t match previous values', i);

    tapas_mpdcm_check_input_matrix(theta, [dx, dx], 'A', i);
    tapas_mpdcm_check_input_matrix(theta, [dx, dx], 'B', i);
    tapas_mpdcm_check_input_matrix(theta, [dx, du], 'C', i);

    tapas_mpdcm_check_input_matrix(theta, [dx, 1], 'x0', i);
    tapas_mpdcm_check_input_matrix(theta, [dx, 1], 'beta', i);
    tapas_mpdcm_check_input_matrix(theta, [dx, 1], 'theta', i);
    tapas_mpdcm_check_input_matrix(theta, [1, 1], 'tau', i);

end

end


function draw_images(data,wait,lim,dispsize)
% Draw data of image array to figure. Reduce size or scale image for resolutions above 800x600
%
% * Input parameters :
%    data [uint16(,,)]       image data array (height,width,count)
%    wait                    time to wait between images (optional) default: 0.01)
%    lim  [low up]           upper and lower limits to display (optional)
%                            default: [0 m], where m is maximal value calculated from first image
%    dispsize [width height] size of top-left corner to display (optional)
%                            default: full image scaled for bigger sizes
%
% * Output parameters :

 if(nargin<1)
  error('Wrong number of input arguments. Need data array');
 end

 if(nargin<2)
  wait=0.010;
 end

 if(nargin<3)
  ulim=max(max(data(10:end-10,10:end-10,1)));
  lim=[0 ulim];
 end

 disp(['waittime set to ',num2str(wait)]);
 disp(['limits set to [0 ',num2str(lim(2)),']']);

 [h,w,count]=size(data);

 if(nargin==4)
  if(dispsize(1)<w)
   w=dispsize(1);
  end
  if(dispsize(2)<h)
   h=dispsize(2);
  end
 end

 imah=draw_image(data(1:h,1:w,1),lim);
 for ima_nr=1:count
  set(imah,'CData',data(1:h,1:w,ima_nr),'CDataMapping','scaled');
  if(wait==0)
   disp('Press any key to proceed')
   pause
  else
   pause(wait);
  end
 end
 disp('Press "Enter" to close window and proceed');
 pause();
 close();
 pause(1);
 commandwindow;
end


% sim_spinecho_xN.m
% Robin Simpson and Jamie Near, 2014.
%
% USAGE:
% out = sim_spinecho(n,sw,Bfield,linewidth,sys,tau,Nechoes)
%
% DESCRIPTION:
% This function simulates a multi-echo spin-echo experiment with 'Nechoes' instantaneous RF pulses.
%
% INPUTS:
% n         = number of points in fid/spectrum
% sw        = desired spectral width in [Hz]
% Bfield    = main magnetic field strength in [T]
% linewidth = linewidth in [Hz]
% sys       = spin system definition structure
% tau       = echo time in [ms]
% Nechoes   = number of spin echoes (optional.  Default = 10);
%
% OUTPUTS:
% out       = simulated spectrum, in FID-A structure format, using multi-echo
%             spin-echo sequence.

function out = sim_spinecho_xN(n,sw,Bfield,linewidth,sys,tau,Nechoes)

if nargin<7
    Nechoes=10;
end

%Set water to centre
centreFreq=4.65;
for k=1:length(sys)
    sys(k).shifts=sys(k).shifts-centreFreq;
end

%Calculate Hamiltonian matrices and starting density matrix.
[H,d]=sim_Hamiltonian(sys,Bfield);

delay=tau/(2*Nechoes);

%BEGIN PULSE SEQUENCE************
d=sim_excite(d,H,'x');                            %EXCITE
for k=1:Nechoes
    d=sim_evolve(d,H,delay/1000);                        %Evolve by tau/2
    d=sim_rotate(d,H,180,'y');                       %180 degree refocusing pulse about y' axis.
    d=sim_evolve(d,H,delay/1000);                        %Evolve by tau/2
end
[out,dout]=sim_readout(d,H,n,sw,linewidth,90);  %Readout along y (90 degree phase);
%END PULSE SEQUENCE**************

%Correct the ppm scale:
out.ppm=out.ppm-(4.65-centreFreq);

%Fill in structure header fields:
out.seq=['spinecho_x' num2str(Nechoes)];
out.te=tau;
out.sim='ideal';

%Additional fields for compatibility with FID-A processing tools.
out.sz=size(out.specs);
out.date=date;
out.dims.t=1;
out.dims.coils=0;
out.dims.averages=0;
out.dims.subSpecs=0;
out.dims.extras=0;
out.averages=1;
out.rawAverages=1;
out.subspecs=1;
out.rawSubspecs=1;
out.flags.writtentostruct=1;
out.flags.gotparams=1;
out.flags.leftshifted=0;
out.flags.filtered=0;
out.flags.zeropadded=0;
out.flags.freqcorrected=0;
out.flags.phasecorrected=0;
out.flags.averaged=1;
out.flags.addedrcvrs=1;
out.flags.subtracted=1;
out.flags.writtentotext=0;
out.flags.downsampled=0;
out.flags.isFourSteps=0;


function [plots,rssi1,rssi2]=ad9361_LTE_2ch(ip,LTEmode,channel)

clc;

%% Pick up LTE parameters according to LTE Mode

if strcmp(LTEmode,'LTE1.4') == 1
    configuration = 'R.4';
    samplingrate = 1.92e6;
    bandwidth = 1.08e6;
    fir_data_file = 'LTE1p4_MHz.ftr';
elseif strcmp(LTEmode,'LTE3') == 1
    configuration = 'R.5';
    samplingrate = 3.84e6;
    bandwidth = 2.7e6;
    fir_data_file = 'LTE3_MHz.ftr';
elseif strcmp(LTEmode,'LTE5') == 1
    configuration = 'R.6';
    samplingrate = 7.68e6;
    bandwidth = 4.5e6;
    fir_data_file = 'LTE5_MHz.ftr';
elseif strcmp(LTEmode,'LTE10') == 1
    configuration = 'R.7';
    samplingrate = 15.36e6;
    bandwidth = 9e6;
    fir_data_file = 'LTE10_MHz.ftr';
else
    error('Please input LTE1.4, LTE3, LTE5 or LTE10.');
end

%% Generate LTE Signal using LTE System Toolbox

% Check for LST presence
if isempty(ver('lte'))
    error('ad9361_LTE:NoLST','Please install LTE System Toolbox to run this example.');
end

% Generate the LTE signal
txsim.RC = configuration; % Base RMC configuration
txsim.NCellID = 17;       % Cell identity
txsim.NFrame = 700;       % Initial frame number
txsim.TotFrames = 1;      % Number of frames to generate
txsim.RunTime = 20;       % Time period to loop waveform in seconds
txsim.DesiredCenterFrequency = 2.45e9; % Center frequency in Hz

% Generate RMC configuration and customize parameters
rmc = lteRMCDL(txsim.RC);
rmc.NCellID = txsim.NCellID;
rmc.NFrame = txsim.NFrame;
rmc.TotSubframes = txsim.TotFrames*10; % 10 subframes per frame
rmc.OCNG = 'On'; % Add noise to unallocated PDSCH resource elements

% Generate RMC waveform
trData = [1;0;0;1]; % Transport data
[eNodeBOutput,~,rmc] = lteRMCDLTool(rmc,trData);
txsim.SamplingRate = rmc.SamplingRate;

% Scale the signal for better power output and cast to int16. This is the
% native format for the SDR hardware. Since we are transmitting the same
% signal in a loop, we can do the cast once to save processing time.
powerScaleFactor = 0.7;
eNodeBOutput = eNodeBOutput.*(1/max(abs(eNodeBOutput))*powerScaleFactor);
eNodeBOutput = int16(eNodeBOutput*2^15);

%% Transmit and Receive using MATLAB libiio

% System Object Configuration
s = iio_sys_obj_matlab; % MATLAB libiio Constructor
s.ip_address = ip;
s.dev_name = 'ad9361';
s.in_ch_no = 4;
s.out_ch_no = 4;
s.in_ch_size = length(eNodeBOutput);
s.out_ch_size = length(eNodeBOutput)*2;

s = s.setupImpl();

% Configure the FIR filter on AD9361
s.writeFirData(fir_data_file);

input = cell(1, s.in_ch_no + length(s.iio_dev_cfg.cfg_ch));
output = cell(1, s.out_ch_no + length(s.iio_dev_cfg.mon_ch));

% Set the attributes of AD9361
input{s.getInChannel('RX_LO_FREQ')} = 2.45e9;
input{s.getInChannel('RX_SAMPLING_FREQ')} = samplingrate;
input{s.getInChannel('RX_RF_BANDWIDTH')} = bandwidth;
input{s.getInChannel('RX1_GAIN_MODE')} = 'slow_attack';
input{s.getInChannel('RX1_GAIN')} = 0;
input{s.getInChannel('RX2_GAIN_MODE')} = 'slow_attack';
input{s.getInChannel('RX2_GAIN')} = 0;
input{s.getInChannel('TX_LO_FREQ')} = 2.45e9;
input{s.getInChannel('TX_SAMPLING_FREQ')} = samplingrate;
input{s.getInChannel('TX_RF_BANDWIDTH')} = bandwidth;

% Keep transmiting and receiving the LTE signal
fprintf('Starting transmission at Fs = %g MHz\n',txsim.SamplingRate/1e6);
for i = 1:5
    fprintf('Transmitting Data Block %i ...\n',i);
    input{1} = real(eNodeBOutput);
    input{2} = imag(eNodeBOutput);
    input{3} = real(eNodeBOutput);
    input{4} = imag(eNodeBOutput);
    output = stepImpl(s, input);
end
fprintf('Transmission finished\n');

% Read the RSSI attributes of both channels
rssi1 = output{s.getOutChannel('RX1_RSSI')};
rssi2 = output{s.getOutChannel('RX2_RSSI')};

s.releaseImpl();

%% Post Processing of Caputured Data

if channel == 1
    I = output{1};
    Q = output{2};
elseif channel == 2
    I = output{3};
    Q = output{4};
else
    error('Please select a channel: 1 or 2.');
end
Rx = I+1i*Q;

% Call LTE Reciever Function
[plots]=LTEReceiver(Rx,samplingrate,configuration);


function sst = sort_sst(sst)
%SORT_SST: Sort SST (Event Start/Stop Times) based on start times
%
%USAGE: sst = sort_sst(sst)
%
%INPUTS: sst --> nx2 Start/Stop Time (datenum) array (unsorted)
%
%OUTPUTS: N --> nx2 Start/Stop Time (datenum) array (sorted)
%
% See also ADD_SST, CHK_T, COMPARE_SST, DELETE_SST, EXTRACT_SST, IS_SST,
%          ISTEQUAL, MERGE_SST, SEARCH_SST, SORT_SST, NAN2SST, SSD2SST,
%          SST2NAN, SST2SSD, SST2VAL, SST2WFA, WFA2SST
%
% Author: Dane Ketner, Alaska Volcano Observatory
% $Date$
% $Revision$

[S N] = sort(sst(:,1));
sst = [S; sst(N,2)];


function y = fx(x)
i = find((x > -pi) & (x <= 0));
y = 2*x;
y(i) = 0;
y(1) = y(end);


function obj = setHessianPattern(obj, hes_sp, sp_form)
    % This function configure the sparsity pattern of the Hessian
    % of the function
    %
    % The form of given sparsity pattern is determined by the input
    % argument 'sp_form'. It can be one of the followings:
    %  'MatrixForm': the sparsity pattern is given as a matrix
    %  (dense or sparse). In this case we extract the row and column
    %  indices of non-zero elements from this matrix
    %  'Indexform': the sparsity pattern is directly given as
    %  vectors row and column indices of non-zero entries
    %
    % @note In MATLAB's sparse matrix, the values of same set of
    % row and column indices are added together. For example,
    % sparse([1;1],[1;1],[1,2]) will results in a sparse matrix
    % with 1+2 = 3 at entry (1,1).
    %
    % Parameters:
    %  hes_sp: a matrix array consists of sparsity pattern of Hessian
    %  of each element of the function @type cell
    %  sp_form: the type of given sparsity pattern. @type char

    % default form
    if nargin < 3
        sp_form = 'MatrixForm';
    end

    dimDeps = sum([obj.DepVariables.Dimension]);

    if isempty(hes_sp)
        obj.Type = NlpFunction.LINEAR;
        obj.HessPattern.Rows = [];
        obj.HessPattern.Cols = [];
    else

        switch sp_form
            case 'MatrixForm'

                [m, n, n_mat] = size(hes_sp);

                assert(n_mat == obj.Dimension && m==dimDeps && n==dimDeps,...
                    'NlpFunction:wrongDimension',...
                    ['The size of the Hessian matrix is incorrect.\n',...
                    'It must be a %d x %d x %d matrix.\n'], ...
                    dimDeps, dimDeps, obj.Dimension);

                % check the dimension
                sp_rows = cell(n_mat,1);
                sp_cols = cell(n_mat,1);
                for i=1:n_mat
                    [sp_rows{i}, sp_cols{i}] = find(hes_sp(:,:,i));
                end
                obj.HessPattern.Rows = vertcat(sp_rows{:});
                obj.HessPattern.Cols = vertcat(sp_cols{:});

            case 'IndexForm'

                % check if the maximum index exceeds the size of the
                % function or dependent variabbles
                m = max(hes_sp(:,1)); % the maximum row index
                n = max(hes_sp(:,2)); % the maximum column index

                assert(m <= dimDeps && n <= dimDeps,...
                    'NlpFunction:wrongDimension',...
                    'The size of the Hessian matrix is incorrect.\n');

                obj.HessPattern.Rows = hes_sp(:,1);
                obj.HessPattern.Cols = hes_sp(:,2);

            otherwise
                error('Unspecified form of the sparsity pattern.\n');
        end
    end
    obj.nnzHess = length(obj.HessPattern.Rows);
end