function [ Aout ] = estimate_airlight( img, Amin, Amax, N, spacing, K, thres )
%Estimate airlight of an image, using a 3*2D Hough transform, where each
%point votes for a given location using a fixed set of angles.
%
%   This is an implementation of our paper:
%   Dana Berman, Tali Treibitz, Shai Avidan,
%   "Air-light Estimation using Haze-Lines", ICCP 2017
%
%   Input arguments:
%   img - input image (mandatory)
%   Amin (Amax) - minimal (maximal) value if the air-light. Optional
%               Can be used to reduce the search space and save time
%               Either a scalar (identical for all color channels) or a 3
%               value vector (different range for each color channel)
%   N - number of colors clusters to use (the image is converted to an
%       indexed image of at most N different cluster). Optional
%   spacing - air-light candidates' resolution, 1/M in the paper. Optional
%   K - angular resolution. Optional
%   thres - cone resolution, optional, default recommended

%% Verify input params, set defaults when necessary (same as published results)
if ~exist('thres','var') || isempty(thres), thres = 0.01 ; end;
if ~exist('spacing','var') || isempty(spacing), spacing = 0.02 ; end; %1/M in the paper
if ~exist('n_colors','var') || isempty(N), N = 1000 ; end; %number of colors clusters
if ~exist('K','var') || isempty(K), K = 40 ; end; %number of angles

% Define search range for the air-light. The search range is different for each
% color channel. These values were used in all of our experiments.
if ~exist('Amin','var') || isempty(Amin), Amin = [0,0.05,0.1]; end;
if ~exist('Amax','var') || isempty(Amax), Amax = 1; end;

% Air-light search range, accept a scalar if identical for all color channels
if isscalar(Amin), Amin = repmat(Amin,1,3); end
if isscalar(Amax), Amax = repmat(Amax,1,3); end

%% Convert input image to an indexed image
[img_ind, points] = rgb2ind(img, N);
[h,w,~] = size(img);
% Remove empty clusters
idx_in_use = unique(img_ind(:));
idx_to_remove = setdiff(0:(size(points,1)-1),idx_in_use);
points(idx_to_remove+1,:) = [];
img_ind_sequential = zeros(h,w);
for kk = 1:length(idx_in_use)
    img_ind_sequential(img_ind==idx_in_use(kk)) = kk;
end
% Now the min value of img_ind_sequential is 1 rather then 0, and the indices
% correspond to points

% Count the occurences if each index - this is the clusters' weight
[points_weight,~] = histcounts(img_ind_sequential(:),size(points,1));
points_weight = points_weight./(h*w);
if ~ismatrix(points), points = reshape(points,[],3); end % verify dim

%% Define arrays of candidate air-light values and angles
angle_list = reshape(linspace(0, pi, K),[],1);
% Use angle_list(1:end-1) since angle_list(end)==pi, which is the same line
% in 2D as since angle_list(1)==0
directions_all = [sin(angle_list(1:end-1)) , cos(angle_list(1:end-1)) ];

% Air-light candidates in each color channel
ArangeR = Amin(1):spacing:Amax(1);
ArangeG = Amin(2):spacing:Amax(2);
ArangeB = Amin(3):spacing:Amax(3);

%% Estimate air-light in each pair of color channels
% Estimate RG
Aall = generate_Avals(ArangeR, ArangeG);
[~, AvoteRG] = vote_2D(points(:,1:2), points_weight, directions_all, Aall, thres );
% Estimate GB
Aall = generate_Avals(ArangeG, ArangeB);
[~, AvoteGB] = vote_2D(points(:,2:3), points_weight, directions_all, Aall, thres );
% Estimate RB
Aall = generate_Avals(ArangeR, ArangeB);
[~, AvoteRB] = vote_2D(points(:,[1,3]), points_weight, directions_all, Aall, thres);

%% Find most probable airlight from marginal probabilities (2D arrays)
% Normalize (otherwise the numbers are quite large)
max_val = max( [max(AvoteRB(:)) , max(AvoteRG(:)) , max(AvoteGB(:)) ]);
AvoteRG2 = AvoteRG./max_val;
AvoteGB2 = AvoteGB./max_val;
AvoteRB2 = AvoteRB./max_val;
% Generate 3D volumes from 3 different 2D arrays
A11 = repmat( reshape(AvoteRG2, length(ArangeG),length(ArangeR))', 1,1,length(ArangeB));
tmp = reshape(AvoteRB2, length(ArangeB),length(ArangeR))';
A22 = repmat(reshape(tmp, length(ArangeR),1,length(ArangeB)) , 1,length(ArangeG),1);
tmp2 = reshape(AvoteGB2, length(ArangeB),length(ArangeG))';
A33 = repmat(reshape(tmp2, 1, length(ArangeG),length(ArangeB)) , length(ArangeR),1,1);
AvoteAll = A11.*A22.*A33;
[~, idx] = max(AvoteAll(:));
[idx_r,idx_g,idx_b] = ind2sub([length(ArangeR),length(ArangeG),length(ArangeB)],idx);
Aout = [ArangeR(idx_r), ArangeG(idx_g), ArangeB(idx_b)];

end % function estimate_airlight_2D

%% Sub functions

function Aall = generate_Avals(Avals1, Avals2)
%Generate a list of air-light candidates of 2-channels, using two lists of
%values in a single channel each
%Aall's length is length(Avals1)*length(Avals2)
Avals1 = reshape(Avals1,[],1);
Avals2 = reshape(Avals2,[],1);
A1 = kron(Avals1, ones(length(Avals2),1));
A2 = kron(ones(length(Avals1),1), Avals2);
Aall = [A1, A2];
end % function generate_Avals

function [Aout, Avote2] = vote_2D(points, points_weight, directions_all, Aall, thres)
n_directions = size(directions_all,1);
accumulator_votes_idx = false(size(Aall,1), size(points,1), n_directions);
for i_point = 1:size(points,1)
    for i_direction = 1:n_directions
		 % save time and ignore irelevant points from the get-go
        idx_to_use = find( (Aall(:, 1) > points(i_point, 1)) & (Aall(:, 2) > points(i_point, 2)));
        if isempty(idx_to_use), continue; end

        % calculate distance between all A options and the line defined by
        % i_point and i_direction. If the distance is smaller than a thres,
        % increase the cell in accumulator
        dist1 = sqrt(sum([Aall(idx_to_use, 1)-points(i_point, 1), Aall(idx_to_use, 2)-points(i_point, 2)].^2,2));
        %dist1 = dist1 - min(dist1);
        dist1 = dist1./sqrt(2) + 1;

        dist =  -points(i_point, 1)*directions_all(i_direction,2) + ...
            points(i_point, 2)*directions_all(i_direction,1) + ...
            Aall(idx_to_use, 1)*directions_all(i_direction,2) - ...
            Aall(idx_to_use, 2)*directions_all(i_direction,1);
        idx = abs(dist)<2*thres.*dist1;
        if ~any(idx), continue; end

        idx_full = idx_to_use(idx);
        accumulator_votes_idx(idx_full, i_point,i_direction) = true;
    end
end
% use only haze-lined that are supported by 2 points or more
accumulator_votes_idx2 = (sum(uint8(accumulator_votes_idx),2))>=2;
accumulator_votes_idx = bsxfun(@and, accumulator_votes_idx ,accumulator_votes_idx2);
accumulator_unique = zeros(size(Aall,1),1);
for iA = 1:size(Aall,1)
    idx_to_use = find(Aall(iA, 1) > points(:, 1) & (Aall(iA, 2) > points(:, 2)));
    points_dist = sqrt((Aall(iA,1) - points(idx_to_use,1)).^2+(Aall(iA,2) - points(idx_to_use,2)).^2);
    points_weight_dist = points_weight(idx_to_use).*(5.*exp(-reshape(points_dist,1,[]))+1);
    accumulator_unique(iA) = sum(points_weight_dist(any(accumulator_votes_idx(iA,idx_to_use,:),3)));
end
[~, Aestimate_idx] = max(accumulator_unique);
Aout = Aall(Aestimate_idx,:);
Avote2 = accumulator_unique;

end % function vote_2D


function [ftps2] = mmps22ftps2(mmps2)
% Convert acceleration from millimeters per square-second to feet per
% second squared
% Chad A. Greene 2012
ftps2 = mmps2*0.00328084;


% The null space of a 3DOF robot when considered redundant.
% if the task m=(vx, vy), then the robot is redundant in lambda=3
%
% Copyright (C) 2016, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function null_space_3dofplanar
% link lengths
robot = load_robot('example', '3dofplanar');
q = [pi/2 pi/2 pi/2]';

qs = [];
qds = [];
manips = [];
for i=1:300
    [qb, manip] = null_movement(robot, q);
    q = q + 0.1*qb;
    qs = [qs q];
    qds = [qds qb];
    manips = [manips manip];
end

animate(robot, qs(:,1:15:end))
figure, plot(manips), legend('manipulability')
figure, plot(qs'), legend('q_1', 'q_2', 'q_3')
figure, plot(qds'), legend('qd_1', 'qd_2', 'qd_3')
%q = q + 0.01*ns';

function [ns, manip] = null_movement(robot, q)
J = manipulator_jacobian(robot, q);
% consider only vx, vy
J = J(1:2, :);
iJm = moore_penrose(J);
P = (eye(3)-iJm*J)
% project to null space
ns = P*[1 0 0]';
manip = det(J*J')

function iJm = moore_penrose(J)
iJm = J'*inv(J*J');


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni

% relationship of linear convolution with circular convolution

% 4-point circular convolution
x1=[1,2,3,4];
x2=[3,2,5,1];
N=length(x1);
for m=0:N-1
p(m+1)=mod(-m,N);
x2s(1+m)=x2(1+p(m+1));
end
x2s
for n=0:N-1
    x2sn=circshift(x2s',n);
    y2(n+1)=x1*x2sn;
end
y2

% 7-point circular convolution
x11=[ x1 0 0 0];
x22=[x2 0 0 0];
N=length(x11);
for m=0:N-1
p(m+1)=mod(-m,N);
end
for m=0:N-1
x22s(1+m)=x22(1+p(m+1));
end
for n=0:N-1
    x22sn=circshift(x22s',n);
    y22(n+1)=x11*x22sn;
end
y22

%linear convolution
y1=conv(x1,x2)


function [diffx, diff_hor2]=horizondist(llhref, llhpos)
Ce2n0=llh2dcm_v000(llhref(1:2),[0;1]);
xyz_pos=ecef2geo_v000(llhpos,1);
xyz_ref=ecef2geo_v000(llhref,1);
diffx=Ce2n0*(xyz_pos-xyz_ref);
diff_hor2=sqrt(sum(diffx(1:2).^2));
end


% GP_PREDICT_PSEUDO - Computes the predictive distribution of a Gaussian
%                     process using pseudo inputs.
%
% Usage:
%
%   F_MEAN = GP_PREDICT_PSEUDO(Y, V, K_FP, K_PP, K_PH, K_H)
%   [F_MEAN, F_VAR] = GP_PREDICT_PSEUDO(...)
%
% The marginal of a conditional model
%
%   Y|F ~ N(F, V)
%
% is approximated by
%
%   Y ~ N(0, K_FP*INV(K_PP)*K_FP' + V)
%
% Y : Nx1 vector of observations
% V : NxN diagonal matrix of noise covariance, or a function V(X) which
%     solves V\X
% K_FP : NxM covariance matrix of function values at observation inputs
%        and at pseudo inputs
% K_PP : MxM covariance matrix of function values at pseudo inputs
% K_PH : MxK covariance matrix of function values at pseudo inputs and at
%        inputs to be predicted
% K_H : Kx1 vector of variances of function values at inputs to be
%       predicted
%
% See also GP_LEARN_PSEUDO, GP_PREDICT.

% Last modified 2011-01-27
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@tkk.fi)

function [f_mean, f_var] = gp_predict_pseudo(y, V, K_pf, K_pp, K_ph, k_h)

if isnumeric(V)
  % Get a solver for V\z
  inv_V = get_linsolve_cov(V);
else
  % A function handle which solves V\z
  inv_V = V;
end

% TODO:
%
% - Take K_pf instead of K_fp
%
% - Take V as a vector?

[L_p,p] = chol(K_pp, 'lower');
if p~=0
  figure
  imagesc(K_pp);
  error('Matrix not positive definite');
end

Z_f = linsolve_tril(L_p, K_pf);

Lambda = speye(size(K_pp)) + Z_f*inv_V(Z_f');
inv_Lambda = get_linsolve_cov(Lambda);

f_mean = K_ph' * linsolve_triu(L_p, inv_Lambda(Z_f*inv_V(y)), true);
if nargout >= 2
  Z_h = linsolve_tril(L_p, K_ph);
  f_var = k_h(:) - dot(Z_h, Z_h - inv_Lambda(Z_h),1)';
end

% $$$ Lambda = K_pp + K_fp' * inv_V(K_fp);
% $$$ inv_Lambda = get_linsolve_cov(Lambda);
% $$$ inv_Kpp = get_linsolve_cov(K_pp);
% $$$ f_mean = K_ph' * inv_Lambda(K_fp' * inv_V(y));
% $$$ f_var = k_h(:) - dot(K_ph, inv_Kpp(K_ph) - inv_Lambda(K_ph),1)';


function K = spikernel( k, d1, d2, ind1, ind2, kerparam)

% K = spikernel( k, d1, d2, ind1, ind2, kerparam)
% computes the spikernel between two sequences of spiking activity
%
% kerparam{1} = N; max subsequence lengths (the kernel compares subsequences of lengths 1 to N and returns a sum of kernels)
% kerparam{2} = lam; \lambda parameter from article
% kerparam{3} = mu; \mu parameter from article
% kerparam{4} = p; the q parameter in the article - the parameter that is used in the sum of kernels to weigh them differently
% kerparam{5} = bins; the number of bins used;          we need this to reshape the data correctly
% kerparam{6} = nneurons; the number of neurons;    we need this to reshape the data correctly
%
%   This code uses the spikernel function, defined in:
%   Spikernels: Embedding Spiking Neurons in Inner-Product Spaces
%   Lavi Shpigelman, Yoram Singer, Rony Paz and Eilon Vaadia
%   Advances in Neural Information Processing Systems (NIPS) 15
%   MIT Press, Cambridge, MA, 2003.
%
% for further details see Fspikernel.m

bins = kerparam{5};
nneurons = kerparam{6};

xx = get_x( d1, ind1);
yy = get_x( d2, ind2);
K = zeros( size( yy,1), size( xx,1));

for i = 1:size( xx, 1)
    for j = 1:size( yy,1)
        K( j, i) = Fspikernel( reshape( yy( j, :), bins, nneurons), reshape( xx( i,:), bins, nneurons), kerparam{1}, kerparam{2}, kerparam{3}, kerparam{4});
%        K( j, i) = K( i, j);
%       if bins >= 100 disp(sprintf('we''re at: (%d, %d)', i,j)); end   %<-- if it takes longer, print out, where we are
    end
end


function lik = lik_multinom(varargin)
%LIK_MULTINOM    Create a multinom likelihood structure
%
%  Description
%    LIK = LIK_MULTINOM creates multinom likelihood for multi-class
%    count data. The observed numbers in each class with C classes is given
%    as 1xC vector.
%
%    The likelihood is defined as follows:
%                              __ n                __ C
%      p(y|f^1, ..., f^C, z) = || i=1 [ gamma(N+1) || c=1 p_i^c^(y_i^c)/gamma(y_i^c+1)]
%
%    where p_i^c = exp(f_i^c)/ (sum_c=1^C exp(f_i^c)) is the succes
%    probability for class c, which is a function of the latent variable
%    f_i^c for the corresponding class and N=sum(y) is the number of trials.
%
%  See also
%    GP_SET, LIK_*

% Copyright (c) 2010 Jaakko Riihim�ki, Pasi Jyl�nki
% Copyright (c) 2010 Aki Vehtari
% Copyright (c) 2010 Jarno Vanhatalo

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

  ip=inputParser;
  ip.FunctionName = 'LIK_MULTINOM';
  ip.addOptional('lik', [], @isstruct);
  ip.parse(varargin{:});
  lik=ip.Results.lik;

  if isempty(lik)
    init=true;
    lik.type = 'Multinom';
    lik.nondiagW = true;
  else
    if ~isfield(lik,'type') || ~isequal(lik.type,'Multinom')
      error('First argument does not seem to be a valid likelihood function structure')
    end
    init=false;
  end

  if init
    % Set the function handles to the subfunctions
    lik.fh.pak = @lik_multinom_pak;
    lik.fh.unpak = @lik_multinom_unpak;
    lik.fh.ll = @lik_multinom_ll;
    lik.fh.llg = @lik_multinom_llg;
    lik.fh.llg2 = @lik_multinom_llg2;
    lik.fh.llg3 = @lik_multinom_llg3;
    lik.fh.predy = @lik_multinom_predy;
    lik.fh.invlink = @lik_multinom_invlink;
    lik.fh.recappend = @lik_multinom_recappend;
  end

end

function [w,s] = lik_multinom_pak(lik)
%LIK_MULTINOM_PAK  Combine likelihood parameters into one vector.
%
%  Description
%    W = LIK_MULTINOM_PAK(LIK) takes a likelihood structure LIK and
%    returns an empty verctor W. If Multinom likelihood had
%    parameters this would combine them into a single row vector
%    W (see e.g. lik_negbin). This is a mandatory subfunction used
%    for example in energy and gradient computations.
%
%
%  See also
%    LIK_MULTINOM_UNPAK, GP_PAK

  w = []; s = {};
end

function [lik, w] = lik_multinom_unpak(lik, w)
%LIK_MULTINOM_UNPAK  Extract likelihood parameters from the vector.
%
%  Description
%    W = LIK_MULTINOM_UNPAK(W, LIK) Doesn't do anything.
%
%    If Multinom likelihood had parameters this would extracts them
%    parameters from the vector W to the LIK structure. This is a
%    mandatory subfunction used for example in energy and gradient
%    computations.
%
%
%  See also
%    LIK_MULTINOM_PAK, GP_UNPAK

  lik=lik;
  w=w;
end

function ll = lik_multinom_ll(lik, y, f, z)
%LIK_MULTINOM_LL  Log likelihood
%
%  Description
%    LL = LIK_MULTINOM_LL(LIK, Y, F) takes a likelihood structure
%    LIK, class counts Y (NxC matrix), and latent values F (NxC
%    matrix). Returns the log likelihood, log p(y|f,z). This
%    subfunction is needed when using Laplace approximation or
%    MCMC for inference with non-Gaussian likelihoods. This
%    subfunction is also used in information criteria
%    (DIC, WAIC) computations.
%
%  See also
%    LIK_MULTINOM_LLG, LIK_MULTINOM_LLG3, LIK_MULTINOM_LLG2, GPLA_E

  f=reshape(f,size(y));
  expf = exp(f);
  p = expf ./ repmat(sum(expf,2),1,size(expf,2));
  N = sum(y,2);

  ll = sum(gammaln(N+1) - sum(gammaln(y+1),2) + sum(y.*log(p),2) );

end

function llg = lik_multinom_llg(lik, y, f, param, z)
%LIK_MULTINOM_LLG    Gradient of the log likelihood
%
%  Description
%    LLG = LIK_MULTINOM_LLG(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, class labels Y, and latent values F. Returns
%    the gradient of the log likelihood with respect to PARAM. At
%    the moment PARAM can be 'param' or 'latent'. This subfunction
%    is needed when using Laplace approximation or MCMC for inference
%    with non-Gaussian likelihoods.
%
%  See also
%    LIK_MULTINOM_LL, LIK_MULTINOM_LLG2, LIK_MULTINOM_LLG3, GPLA_E

  f=reshape(f,size(y));
  C = size(y,2);
  expf2 = exp(f);
  N=sum(y, 2);
  pi2 = (N*ones(1,C)).*expf2./(sum(expf2, 2)*ones(1,C));
  pi_vec=pi2(:);
  llg = y(:)-pi_vec;

end

function [pi_vec, pi_mat] = lik_multinom_llg2(lik, y, f, param, z)
%LIK_MULTINOM_LLG2  Second gradients of the log likelihood
%
%  Description
%    LLG2 = LIK_MULTINOM_LLG2(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, class labels Y, and latent values F. Returns
%    the Hessian of the log likelihood with respect to PARAM. At
%    the moment PARAM can be only 'latent'. LLG2 is a vector with
%    diagonal elements of the Hessian matrix (off diagonals are
%    zero). This subfunction is needed when using Laplace
%    approximation or EP for inference with non-Gaussian likelihoods.
%
%  See also
%    LIK_MULTINOM_LL, LIK_MULTINOM_LLG, LIK_MULTINOM_LLG3, GPLA_E

% multinom:
  [n,nout]=size(y);
  N = sum(y,2)*ones(1,nout);
  f=reshape(f,n,nout);

  expf2 = exp(f);
  pi2 = expf2./(sum(expf2, 2)*ones(1,nout));
  pi_vec=pi2(:).*N(:);

  pi_mat=zeros(nout*n, n);
  for i1=1:nout
    pi_mat((1+(i1-1)*n):(nout*n+1):end)=pi2(:,i1).*sqrt(N(:,i1));
  end
  %     D = diag(pi_vec);
  %     llg2 = -D + pi_mat*pi_mat';

end

function [dw_mat] = lik_multinom_llg3(lik, y, f, param, z)
%LIK_MULTINOM_LLG3  Third gradients of the log likelihood
%
%  Description
%    LLG3 = LIK_MULTINOM_LLG3(LIK, Y, F, PARAM) takes a likelihood
%    structure LIK, class labels Y, and latent values F and
%    returns the third gradients of the log likelihood with
%    respect to PARAM. At the moment PARAM can be only 'latent'.
%    LLG3 is a vector with third gradients. This subfunction is
%    needed when using Laplace approximation for inference with
%    non-Gaussian likelihoods.
%
%  See also
%    LIK_MULTINOM_LL, LIK_MULTINOM_LLG, LIK_MULTINOM_LLG2, GPLA_E, GPLA_G

  [n,nout] = size(y);
  f2 = reshape(f,n,nout);

  N=sum(y, 2);
  expf2 = exp(f2);
  pi2 = expf2./(sum(expf2, 2)*ones(1,nout));
  pi_vec=pi2(:);

  dw_mat=zeros(nout,nout,nout,n);

  for cc3=1:nout
    for ii1=1:n

      pic=pi_vec(ii1:n:(nout*n));
      for cc1=1:nout
        for cc2=1:nout

          % multinom third derivatives
          cc_sum_tmp=0;
          if cc1==cc2 && cc1==cc3 && cc2==cc3
            cc_sum_tmp=cc_sum_tmp+pic(cc1);
          end
          if cc1==cc2
            cc_sum_tmp=cc_sum_tmp-pic(cc1)*pic(cc3);
          end
          if cc2==cc3
            cc_sum_tmp=cc_sum_tmp-pic(cc1)*pic(cc2);
          end
          if cc1==cc3
            cc_sum_tmp=cc_sum_tmp-pic(cc1)*pic(cc2);
          end
          cc_sum_tmp=cc_sum_tmp+2*pic(cc1)*pic(cc2)*pic(cc3);

          dw_mat(cc1,cc2,cc3,ii1)=cc_sum_tmp.*N(ii1);
        end
      end
    end
  end

end

function [lpy, Ey, Vary] = lik_multinom_predy(lik, Ef, Varf, yt, zt)
%LIK_MULTINOM_PREDY  Returns the predictive mean, variance and density of y
%
%  Description
%    LPY = LIK_MULTINOM_PREDY(LIK, EF, VARF YT)
%    Returns logarithm of the predictive density PY of YT, that is
%        p(yt | y) = \int p(yt | f) p(f|y) df.
%    This requires also the incedence counts YT. This subfunction
%    is needed when computing posterior predictive distributions for
%    future observations.
%
%    [LPY, EY, VARY] = LIK_MULTINOM_PREDY(LIK, EF, VARF, YT) takes a
%    likelihood structure LIK, posterior mean EF and posterior
%    Variance VARF of the latent variable and returns the
%    posterior predictive mean EY and variance VARY of the
%    observations related to the latent variables. This subfunction
%    is needed when computing posterior predictive distributions for
%    future observations.
%

%
%  See also
%    GPLA_PRED, GPEP_PRED, GPMC_PRED

  N=sum(yt,2);
  S=10000;
  [ntest, nout]=size(yt);
  pi=zeros(ntest,nout);
  lpy=zeros(ntest,nout);
  Ey=zeros(ntest,nout);
  Vary=zeros(size(Varf));
  Ef=reshape(Ef(:),ntest,nout);
  [notused,notused,c] =size(Varf);
  if c>1
    mcmc=false;
  else
    mcmc=true;
    Varf=reshape(Varf(:), ntest, nout);
  end
  for i1=1:ntest
    if mcmc
      Sigm_tmp = (Varf(i1,:));
      f_star=bsxfun(@plus, Ef(i1,:), bsxfun(@times, sqrt(Sigm_tmp), ...
        randn(S,nout)));
    else
      Sigm_tmp=(Varf(:,:,i1)'+Varf(:,:,i1))./2;
      f_star=mvnrnd(Ef(i1,:), Sigm_tmp, S);
    end

    tmp = exp(f_star);
    tmp = tmp./(sum(tmp, 2)*ones(1,size(tmp,2)));

    if nargout > 1
        Ey(i1,:) = N(i1).*mean(tmp);
        for z1 = 1:nout;
          for z2 = 1:nout
            for z3=1:S
              Var_tmp(:,:,z3) = (diag(tmp(z3,:)) - tmp(z3,:)'*tmp(z3,:));
            end
            if mcmc
              Vary(i1+(0:nout-1)*ntest,:) = diag(N(i1).*mean(Var_tmp,3));
            else
              Vary(:,:,i1) = N(i1).*mean(Var_tmp,3);
            end
          end
        end
    end
    lpy=[];
    if ~isempty(yt)
      ytmp = repmat(yt(i1,:),S,1);
      lpy(i1,:) = log(mean( mnpdf(ytmp,tmp) ));
    end
  end
  lpy=lpy(:);
  Ey=Ey(:);
end

function p = lik_multinom_invlink(lik, f, z)
%LIK_MULTINOM_INVLINK Returns values of inverse link function
%
%  Description
%    P = LIK_MULTINOM_INVLINK(LIK, F) takes a likelihood structure LIK and
%    latent values F and returns the values of inverse link function P.
%    This subfunction is needed when using function gp_predprctmu.
%
%     See also
%     LIK_MULTINOM_LL, LIK_MULTINOM_PREDY
p = multinominv(f).*z;
end

function reclik = lik_multinom_recappend(reclik, ri, lik)
%RECAPPEND  Append the parameters to the record
%
%  Description
%    RECLIK = LIK_MULTINOM_RECAPPEND(RECLIK, RI, LIK) takes a
%    likelihood record structure RECLIK, record index RI and
%    likelihood structure LIK with the current MCMC samples of
%    the parameters. Returns RECLIK which contains all the old
%    samples and the current samples from LIK. This subfunction
%    is needed when using MCMC sampling (gp_mc).
%
%  See also
%    GP_MC

  if nargin == 2
    reclik.type = 'Multinom';
    reclik.nondiagW = true;

    % Set the function handles
    reclik.fh.pak = @lik_multinom_pak;
    reclik.fh.unpak = @lik_multinom_unpak;
    reclik.fh.ll = @lik_multinom_ll;
    reclik.fh.llg = @lik_multinom_llg;
    reclik.fh.llg2 = @lik_multinom_llg2;
    reclik.fh.llg3 = @lik_multinom_llg3;
    reclik.fh.predy = @lik_multinom_predy;
    reclik.fh.invlink = @lik_multinom_invlink;
    reclik.fh.recappend = @lik_multinom_recappend;
  end

end


function [out]=ImageRecover(y,D,CoefMatrix)

% method:  Sliding
% CoefMatrix  256*146405
% D 64*256

[N M]=size(y);
n=sqrt(size(D,1));
out=zeros(N,M);
weight=zeros(N,M);

i=1; j=1;
for k=1:(N-n+1)*(M-n+1)
    patch=reshape(D*CoefMatrix(:,k),[n,n]);
    out(i:i+n-1,j:j+n-1)=out(i:i+n-1,j:j+n-1)+patch;
    weight(i:i+n-1,j:j+n-1)=weight(i:i+n-1,j:j+n-1)+1;
    if i<N-n+1
        i=i+1;
    else
        i=1; j=j+1;
    end;
end;
out=out./weight;
return;


function fft_plot(samples, Fs)
    N = length(samples);
    df = Fs/N;
    f = -Fs/2:df:Fs/2-df;
    Y = fftshift(abs(fft(samples)));
    subplot(121);
    plot(f, Y);
    subplot(122);
    nfft = 128;
    spectrogram(samples, hanning(nfft), round(nfft*0.9), nfft, Fs);
end


function c_hist = my_color_hist_weight(img, weight)
% img in format (H,W,3)
% weight in format (H,W)

    num_bins = 10;
    c_hist = zeros(num_bins,num_bins,num_bins);
    img = single(img)/256;

    for row = 1:size(img,1)
        for col = 1:size(img,2)
            val = img(row,col,:);
            hist_bin = floor(val*num_bins)+1;
            c_hist(hist_bin(1),hist_bin(2),hist_bin(3)) = c_hist(hist_bin(1),hist_bin(2),hist_bin(3)) + weight(row,col);
        end
    end

%{
    weight = weight/max(weight(:));
    img(:,:,1) = floor(img(:,:,1) .* weight);
    img(:,:,2) = floor(img(:,:,2) .* weight);
    img(:,:,3) = floor(img(:,:,3) .* weight);
    r = histogram(img(:,:,1),100);
    g = histogram(img(:,:,2),100);
    b = histogram(img(:,:,3),100);
  %}
end


% [x, Covx, Covx_x, entropy] = rts_smoother_step(x, Covx, x_s, Covx_s, A, Q)

% Last modified 2011-10-19
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@aalto.fi)

function [x, Covx, Covx_x, entropy] = rts_smoother_step(x, Covx, x_s, Covx_s, A, Q)
% Perform the RTS smoothing step

x_p = A*x;
Covx_p = A*Covx*A' + Q;

S = (Covx*A') / Covx_p;
x = x + S*(x_s-x_p);
if nargout >= 2
  Covx = Covx + S*(Covx_s-Covx_p)*S';
end
if nargout >= 3
  Covx_x = S*Covx_s;
end
if nargout >= 4
  % Compute entropy term:
  %
  % INT[ p(x_n, x_(n+1) | Y) log p(x_n | Y, x_(n+1)) dx_n dx_(n+1) ]
  Cov_joint = [Covx    Covx_x
               Covx_x' Covx_s];
  entropy = gaussian_entropy(logdet_cov(Cov_joint), size(Cov_joint,1)) ...
            - gaussian_entropy(logdet_cov(Covx_s), size(Covx_s,1));

end


function output = relaxMtFitFuncLs(x, m, W_B, W_F, T2_B, R1, S0, t_m, t_s, t_r);
% function output = relaxMtFitFuncLs(x, m, W_B, W_F, T2_B, R1, S0, t_m, t_s, t_r);
% inputs:
%       x = [k, f] where k is the cross-relaxation rate constant defined
%           for the transition from free pool (F) to bound pool (B), f is the
%           fraction of bound spins expressed in terms of concentrations as f =
%           [B]/([B]+[F])
%       m: MT(:,ii) ??
%
%       W_B: Effective saturation rate of the bound pool
%            W_B(ii) = pi*(w1rms^2)*lorentzian (delta(ii), T2_B);% eqtn (2)
%       W_F: 1/R1_F .* Effective saturation rate of the free pool
%            W_F = (w1rms./(2*pi*delta)).^2/.055; --> eqtn(3)./R1_F
%            w1rms = 2400; % omega-1 RMS --> ???? where does this number
%            come from???
%       T2_B: T2 relaxation time of Bound pool, units= seconds
%           T2_B = 11e-6; "average-brain" [YY (2004), pg 411, column2, paragraph 1]
%       R1: observed relaxation rate (measured in the independent
%           experiment) [YY(2004), pg 411, column 1, parag 2]
%           nz = T1>0;
%           R1 = zeros(size(T1));
%           R1(nz) = 1./T1(nz);
%       S0: Synthetic reference image computed by equation (6) using PD(protein Density) and
%           R1 maps.
%       t_m: duration of an off-resonance RF pulse
%           t_m = 8e-3; %bese 8e-3
%       t_s: delay time BEFORE an exitation RF pulse
%           t_s = 5e-3; %bese 5e-3
%       t_r: delay time AFTER an exitation RF pulse
%           t_r = 19e-3; %bese 19e-3
%   Output:
%       output: ??? m/S0 - M_z(:,1)./m_norm(1);
%
%    Example:
%       output=relaxMtFitFunc(x, MT(:,ii), W_B, W_F, T2_B, R1(ii),S0(ii), t_m, t_s, t_r)
%

% C : diagonal matrix = diag(cos(alpha),1) corresponding to instant
% rotation of the magnetization Mz_F by an excitation pulse with a flip
% angle alpha
% cos(10*pi/180) = 0.984807753012
C = [ 0.984807753012 0;
            0        1 ];

k = x(1); % the k-parameter is passed through x
f = x(2); % the f-parameter is passed through x

if(f<=0.01 || f>=0.5 || k<=0.1 || k>=5)
   output = Inf;
   return;
end

%T2_B = 11e-6; %bese 11e-6

R1_B = 1; %bese 1
% Compute this term just one to save some cycles
kf = k*(1-f)/f;
R1_F = R1 - k*(R1_B - R1)/(R1_B - R1 + kf); % eqtn (4)
%W_F = R1_F*(w1rms./(2*pi*delta)).^2/.055; %bese .055
W_F = R1_F*W_F;

R = [ (-R1_F - k)     (kf)    ;
           (k)    (-R1_B - kf) ];

A = R1_F*R1_B + R1_F*kf + R1_B*k;
E_s = expm(R*t_s); % relaxation during delays before (t_s) an exitation RF pulse
E_r = expm(R*t_r); % relaxation during delays after (t_r) an exitation RF pulse

I = eye(2); %identity matrix

for ii = 1:length(W_B);
  W = [-W_F(ii), 0;
       0,   -W_B(ii)];
  E_m = expm((R + W)*t_m);% off-resonance saturation by an RF pulse w/ duration t_m
  D = A + (R1_F + k)*W_B(ii) + (R1_B + kf)*W_F(ii) + W_B(ii)*W_F(ii);
  M_eq = [1-f f]';
  M_ss = 1/D*[(1-f)*(A + R1_F*W_B(ii)) f*(A + R1_B*W_F(ii))]';
  term_1 = inv(I - E_s*E_m*E_r*C);
  %if (rcond(term_1)<1e-4), output = +Inf; return; end;
  term_2 = (E_s*E_m*(I-E_r) + I-E_s)*M_eq;
  term_3 = E_s*(I-E_m)*M_ss;
  M_z(ii,:) = term_1*(term_2 + term_3);
end

% Ova mozebi e nepotrebno, zasto m_norm treba da se zameni so S0
E_m = expm((R)*t_m);
D = A;
M_eq = [1-f f]';
M_ss = 1/D*[(1-f)*(A) f*(A)]';
term_1 = inv(I - E_s*E_m*E_r*C);
term_2 = (E_s*E_m*(I - E_r) + I-E_s)*M_eq;
term_3 = E_s*(I-E_m)*M_ss; %na pocetok treba E_s?
m_norm = term_1*(term_2 + term_3);
result = m/S0 - M_z(:,1)./m_norm(1);
output = sum(result.^2);
%output = result;

return;


function pass = test_coeffs_vals( pref )

% Grab some preferences
if ( nargin == 0 )
    pref = chebfunpref();
end
tol = 1e3*pref.techPrefs.chebfuneps;
j = 1;
%grab some coeffs
u = diskfun(@(x,y) exp(-cos(pi*(x+y))));
v = diskfun(@(x,y) x.*sin(x.*y));
f = [u;v];

% test coeffs2:
[x, y] = coeffs2(f);
pass(j) = norm(diskfun.coeffs2diskfun(x)-u, inf) < tol;
pass(j+1) = norm(diskfun.coeffs2diskfun(y)-v, inf) < tol;
j = j+2;

%parameters
[x, y] = coeffs2(f, 50, 60);
pass(j) = norm(x-coeffs2(u, 50, 60), inf) < tol;
pass(j+1) = norm(y-coeffs2(v, 50, 60), inf) < tol;
j = j+2;

% test coeffs2diskfunv:
f2 = diskfunv.coeffs2diskfunv(coeffs2(u), coeffs2(v));
pass(j) = norm(f - f2) < tol;
j = j+1;

%test coeffs2vals:
[x, y] = coeffs2(f2);
[u, v] = diskfunv.coeffs2vals(x,y);
pass(j) = norm(diskfun.coeffs2vals(x)-u, 'inf') < tol;
pass(j+1) = norm(diskfun.coeffs2vals(y)-v, 'inf')< tol;
j= j+2;

%testvals2coeffs:
[a,b] = diskfunv.vals2coeffs(u,v);
pass(j) = norm(x - a, 'inf');
pass(j+1) = norm(y - b, 'inf');

if (nargout > 0)
    pass = all(pass(:));
end
end


clear; close all
        % Impunerea primei solutii initiale
sol_init = bvpinit(linspace(0,4,5),[1 0]);
        % Gasirea primei solutii
sol = bvp4c(@fbound,@ffront,sol_init);
        % Evaluarea primei solutii
x = linspace(0,4);
y1 = bvpval(sol,x);
        % Impunerea solutiei initiale a doua
sol_init = bvpinit(linspace(0,4,5),[-1 0]);
        % Gasirea solutiei a doua
sol = bvp4c(@fbound,@ffront,sol_init);
        % Evaluarea solutiei a doua
y2 = bvpval(sol,x);
        % Reprezentarea grafica a celor doua solutii
plot(x,y1(1,:),x,y2(1,:),'Linewidth',1.5);
xlabel('x');
ylabel('y');
grid
legend('Prima solutie','A doua solutie')


clc
clear
close all

% This example shows how to interpolate the fMRI timecourse if the stimulus
% duration is not equal to the duration of the TR. Imagine that there are
% 100 TRs and each lasts for 1s, while your stimulus duration is only 0.5s
% The stimulus starts with the TR onset. To correctly code the stimulus
% onset in your design matrix you need 200 rows in your design matrix
% (but you only have 100 fMRI volumes). Interpolation of the timecourse is
% necessary to match the size of rows in your design matrix.
%%

% Simulated data properties.
TRs = 100;
TR = 1;
stimdur = 0.5;
TRs_after_resampling = TR/stimdur*TRs;

% Use an example hrf and create an example fMRI time series.
cond1 = zeros(TRs,1);
cond1(1:20:end) = 1;

hrf = getcanonicalhrf(0.5,1);
tcs = conv(cond1,hrf);
tcs = tcs(1:TRs);
figure(1);clf
plot(0:TR:TRs-TR,tcs,'-','LineWidth',2); hold on
ylabel('%BOLD')
xlabel('TRs')

% The following line resmaples the timecourse so that each timepoint will
% correspond to 0.5 s instead of 1 s. The tseriesinterp is not a function
% available in GLMsingle but you can download it form github
% https://github.com/cvnlab/knkutils/blob/master/timeseries/tseriesinterp.m

tcs_interp = tseriesinterp(tcs,1,0.5);

% plot the interpolated timeseries
plot(0:stimdur:TRs-stimdur,tcs_interp,'o','MarkerSize',3,'LineWidth',2)
legend box off
set(gca,'FontSize',15)

%%
whos tcs tcs_interp

% Notice that the lenght of the tcs_interp is double the length of tcs.

% With an interpolated timecourse now you can code your design matrix
% correctly. The design matrix is going to consist of 200 columns were 1 will
% specify the stimulus onset. Remember to specify the stimduration for
% GLMsingle as 0.5 s instead of 1 s.

%% show the dm as stem plot with correct length

dm = repelem(cond1,2);
find_rep = diff(dm);
dm(find_rep==-1) = 0;
dm(dm==0) = NaN;
stem(0:stimdur:TRs-stimdur,dm,'filled','LineWidth',2)
legend({'Original tcs';'Interpolated tcs';'Stimulus onset'},'Location','EastOutside')
set(gcf,'Position',[1000        1090        1234         247])


function imResult = blendMode_Phoenix(A, B, offsetW, offsetH)
%% Phoenix blending mode: This subtracts the lighter pixel from the darker
%   pixel, and adds 1, giving a bright result.
%
% Input:
%       A       -       Base Image
%       B       -       Top Image
%   offsetW     -   move picture B horizontally in respect to the top-left
%                   corner of picture A. Default value = 1.
%   offsetH     -   move picture B vertically in respect to the top-left
%                   corner of picture A. Default value = 1.
%
% Output:
%       imResult    -   Result of the blending, having the same size of the
%                       Base Image A.
%

%% Check Input
a = size(A);
b = size(B);
blendMode_checkInput(nargin, a, b, func2str(@blendMode_Phoenix));

if nargin < 3
    offsetW = 1;
    offsetH = 1;
end

if nargin < 4
    offsetH = 1;
end

%% Implementation
imResult = A;

if (((offsetW ~= 1) || (offsetH ~= 1)) || (sum(a == b) ~= length(a)))
    [A, B] = blendMode_ResizeImages(A, B, a, b, offsetW, offsetH);
end

C = min(A, B) - max(A, B) + 1;

if (((offsetW ~= 1) || (offsetH ~= 1)) || (sum(a == b) ~= length(a)))
    imResult = blendMode_CreateResult(imResult, C, offsetW, offsetH);
else
    imResult = C;
end

end


function [xPred, PPred, F, B] = ExtendedKalmanFilterX_PredictState(x,P,f,Q,u,b,Qu)
% EKALMANFILTERX_PREDICTSTATE Perform the discrete-time KF state prediction
% step, under the assumption of additive process noise.
%
% Parameters
% ----------
% x: column vector
%   The (xDim x 1) state estimate at the previous time-step.
% P: matrix
%   The (xDim x xDim) state covariance matrix at the previous
%   time-step.
% f: function handle
%   A (non-linear) state transition function.
% Q: matrix
%   The (xDim x xDim) process noise covariance matrix.
% u: column vector, optional
%   An optional (xDim x 1) control input.
%   If omitted, no control input is used.
% b: function handle, optional
%   A (non-linear) control gain function.
%   (Optional, Default = 1 if u provided, 0 otherwise)
% O: matrix, optional
%   An optional (xDim x xDim) control noise covariance
%   matrix. If omitted, Q is assumed to be 0.
%
% Returns
% -------
% xPred: column vector
%   The (xDim x 1) predicted state estimate.
% PPred: matrix
%   The (xDim x xDim) predicted state covariance matrix.
% F: matrix
%   The computed Jacobian transition matrix
% H: matrix
%   The computed (yDim x yDim) Jacobian measurement matrix
% B: matrix, optional
%   The computed Jacobian control gain matrix
%
% October 2017 Lyudmil Vladimirov, University of Liverpool.

    switch(nargin)
        case(4)
            u  = 0;
            b  = 0;
            Qu = 0;
        case(5)
            b  = 1;
            Qu = 0;
        case(6)
            Qu = 0;
    end

    % Prediction for state vector and covariance:
    [xPred,F] = ExtendedKalmanFilterX_computeJac(f,x);    %nonlinear update and linearization at current state
    PPred = F*P*F' + Q;                 %partial update

    % Compute Control Input (if applicable)
    [controlInputWithGain,B] = ExtendedKalmanFilterX_computeJac(b,u);

    % Add control input
    xPred = xPred + controlInputWithGain;
    PPred = PPred + B*Qu*B';
end


function y= DAB(T,P,MA,MB,sigmA,sigmB,epsA,epsB)
%DAB Calculates gas-phase diffusivity using Wilke-Lee equation, p. 17 Text.
%   DAB(T,P,MA,MB,sigmA,sigmB,epsA,epsB)
%   T = absolute temperature in K
%   P = pressure in bar
%   MA, MB = molecular weights
%   sigmA, sigmB, epsA, epsB = Lennard-Jones parameters
%   in Angstroms and Kelvin, respectively
%   DAB = diffusivity in square cm/sec.
sigmAB=(sigmA+sigmB)/2;
epsAB=sqrt(epsA*epsB);
x=T/epsAB;
a=1.06036;b=0.15610;c=0.19300;d=0.47635;
e1=1.03587;f=1.52996;g=1.76474;h=3.89411;
MAB=2*(1/MA+1/MB)^-1;
omega=a/x^b+c/exp(d*x)+e1/exp(f*x)+g/exp(h*x);
y=0.001*(3.03-0.98/sqrt(MAB))*T^1.5/(P*sigmAB^2*omega*sqrt(MAB));


function sigma = massey_berlekamp_M2(n,k,t,S,field)

%http://www.ee.ucla.edu/~matache/rsc/node8.html#SECTION00051000000000000000

%Step 2: Initialize variables
kk = 0;

for i = 1:n
    Kappa(1,i) = -Inf;
end
Kappa(1,1) = 0;

%Kappa

LAMBDA = 0;
Tau = [-inf 0];

done = 0;

%Step 3:
while (done ~= 1)
    %disp('K');

    kk = kk + 1;

    %disp('S(kk)');
    %S(kk)

    %disp('LAMBDA')
    %LAMBDA

    sum = -Inf;
    for i = 1:LAMBDA
        %Kappa(kk,i+1)
        %S(kk-i)
        sum = gfadd(sum,gfmul(Kappa(kk,i+1),S(kk-i),field),field);
    end

    %disp('Delta - sum')
    %sum

    delta(kk) = gfadd(S(kk),sum,field);

    %disp('delta');
    %delta

    %Step 4:
    if (delta(kk) == -Inf)
        for i = 1:n
            Kappa(kk+1,i) = Kappa(kk,i);
        end
    end

    if (delta(kk) ~= -Inf)

        for i = 1:n
            Kappa_i(i) = Kappa(kk-1+1,i);
        end

        Kappa_k = gfadd(Kappa_i,gfconv(delta(kk),Tau,field),field);

        while length(Kappa_k) < n
            Kappa_k = [Kappa_k -Inf];
        end

        for i = 1:length(Kappa_k)
            Kappa(kk+1,i) = Kappa_k(i);
        end

        %Step 7:
        if (2*LAMBDA < kk)
            LAMBDA = kk - LAMBDA;

            for i = 1:n
                Kappa_k(i) = Kappa(kk+1-1,i);
            end

            Tau = gfconv(Kappa_k,gfdiv(0,delta(kk),field),field);
        end
    end

    %Step 8:
    Tau = gfconv([-Inf 0],Tau,field);

    %step 9:
    if kk >= 2*t
        done = 1;
    end

    %Kappa
    %LAMBDA
    %Tau

end

for i = 1:n
    sigma(i) = Kappa(kk+1,i);
end


clear all,  close all

fs = 44.1e3;
t = 0:1/fs:1;
x1 = sin(2*pi*1000*t);
x2 = sin(2*pi*5000*t);
x = x1 + x2 + 0.5*randn(1, length(x1));
sound(x, fs)
spectrum(x, fs, 'plot');


function F=dwt_sr_fuse(I1,I2,zt,D,overlap,epsilon)
%    DWT-SR
%    Input:
%    I1 - input image A
%    I2 - input image B
%    zt - maximum decomposition level
%    D  - Dictionary for sparse representation
%    overlap - the overlapped pixels between two neighbor patches
%    epsilon - sparse reconstuction error
%    Output:
%    F  - fused image
%
%    The code is edited by Yu Liu, 01-09-2014.

%-------------------------------------------------------------------------%
%                                DWT
%-------------------------------------------------------------------------%
I1=double(I1);
I2=double(I2);
tempA=I1;
tempB=I2;

X=cell(zt,4);
Y=cell(zt,4);
Z=cell(zt,4);
for i=1:zt
    [X{i,1},X{i,2},X{i,3},X{i,4}]=dwt2(tempA,'db1','mode','per');
    tempA=X{i,1};
    [Y{i,1},Y{i,2},Y{i,3},Y{i,4}]=dwt2(tempB,'db1','mode','per');
    tempB=Y{i,1};
end

%-------------------------------------------------------------------------%
%                               low-pass fusion
%-------------------------------------------------------------------------%
Z{zt,1}=sparse_fusion(X{zt,1},Y{zt,1},D,overlap,epsilon);

%-------------------------------------------------------------------------%
%                               high-pass fusion
%-------------------------------------------------------------------------%
for i=zt:-1:1
    for j=2:4
        Z{i,j}=selc(X{i,j},Y{i,j},3);
    end
end

%-------------------------------------------------------------------------%
%                               IDWT
%-------------------------------------------------------------------------%
for i=zt:-1:1
    if i>1
        Z{i-1,1}=idwt2(Z{i,1},Z{i,2},Z{i,3},Z{i,4},'db1','mode','per');
    else
        F=idwt2(Z{i,1},Z{i,2},Z{i,3},Z{i,4},'db1','mode','per');
    end
end


function f14=f14(x)

Bound=[-65.536 65.536];

if nargin==0
    f14 = Bound;
else
aij=[   -32 -16 0 16 32 -32 -16 0 16 32 -32 -16 0 16 32 -32 -16 0 16 32 -32 -16 0 16 32;
    -32 -32 -32 -32 -32 -16 -16 -16 -16 -16 0 0 0 0 0 16 16 16 16 16 32 32 32 32 32];

x1=x(1,:)*ones(1,25);
x2=x(2,:)*ones(1,25);

j=cumsum(ones(1,25));

f14=1/(1/500+sum(1./(j+(x1-aij(1,:)).^6+(x2-aij(2,:)).^6)));
end


function [C, D] = WatsonSHCoeff(k)
% function [C, D] = WatsonSHCoeff(k)
% Computes the spherical harmonic (SH) coefficients of the Watson's
% distribution with the concentration parameter k (kappa) up to the 12th order
% and the derivatives if requested.
%
% Truncating at the 12th order gives good approximation for kappa up to 64.
%
% INPUTS:
%
% k should be an array of positive numbers, specifying a set of
% concentration parameters for the Watson's distribution.
%
% OUTPUTS:
%
% C will be a 2-D array and each row contains the SH coefficients of the
% orders 0, 2, 4, ..., to 2n for the parameter in the corresponding row in
% k.
%
% Note that the SH coefficients of the odd orders are always zero.
%
% D will be the 1st order derivative of C.
%
% author: Gary Hui Zhang (gary.zhang@ucl.ac.uk)
%

large = find(k>30);
exact = find(k>0.1);
approx = find(k<=0.1);
% Necessary to make matlab happy when k is a single value
exact = exact(:);
approx = approx(:);
large = large(:);

% The maximum order of SH coefficients (2n)
n = 6;

% Computing the SH coefficients
C = zeros(length(k),n+1);

% 0th order is a constant
C(:,1) = 2*sqrt(pi);

% Precompute the special function values
sk = sqrt(k(exact));
sk2 = sk.*k(exact);
sk3 = sk2.*k(exact);
sk4 = sk3.*k(exact);
sk5 = sk4.*k(exact);
sk6 = sk5.*k(exact);
sk7 = sk6.*k(exact);
k2 = k.^2;
k3 = k2.*k;
k4 = k3.*k;
k5 = k4.*k;
k6 = k5.*k;
k7 = k6.*k;

erfik = NODDI_erfi(sk);
ierfik = 1./erfik;
ek = exp(k(exact));
dawsonk = 0.5*sqrt(pi)*erfik./ek;

% for large enough kappa
C(exact,2) = 3*sk - (3 + 2*k(exact)).*dawsonk;
C(exact,2) = sqrt(5)*C(exact,2).*ek;
C(exact,2) = C(exact,2).*ierfik./k(exact);

C(exact,3) = (105 + 60*k(exact) + 12*k2(exact)).*dawsonk;
C(exact,3) = C(exact,3) -105*sk + 10*sk2;
C(exact,3) = .375*C(exact,3).*ek./k2(exact);
C(exact,3) = C(exact,3).*ierfik;

C(exact,4) = -3465 - 1890*k(exact) - 420*k2(exact) - 40*k3(exact);
C(exact,4) = C(exact,4).*dawsonk;
C(exact,4) = C(exact,4) + 3465*sk - 420*sk2 + 84*sk3;
C(exact,4) = C(exact,4)*sqrt(13*pi)/64./k3(exact);
C(exact,4) = C(exact,4)./dawsonk;

C(exact,5) = 675675 + 360360*k(exact) + 83160*k2(exact) + 10080*k3(exact) + 560*k4(exact);
C(exact,5) = C(exact,5).*dawsonk;
C(exact,5) = C(exact,5) - 675675*sk + 90090*sk2 - 23100*sk3 + 744*sk4;
C(exact,5) = sqrt(17)*C(exact,5).*ek;
C(exact,5) = C(exact,5)/512./k4(exact);
C(exact,5) = C(exact,5).*ierfik;

C(exact,6) = -43648605 - 22972950*k(exact) - 5405400*k2(exact) - 720720*k3(exact) - 55440*k4(exact) - 2016*k5(exact);
C(exact,6) = C(exact,6).*dawsonk;
C(exact,6) = C(exact,6) + 43648605*sk - 6126120*sk2 + 1729728*sk3 - 82368*sk4 + 5104*sk5;
C(exact,6) = sqrt(21*pi)*C(exact,6)/4096./k5(exact);
C(exact,6) = C(exact,6)./dawsonk;

C(exact,7) = 7027425405 + 3666482820*k(exact) + 872972100*k2(exact) + 122522400*k3(exact)  + 10810800*k4(exact) + 576576*k5(exact) + 14784*k6(exact);
C(exact,7) = C(exact,7).*dawsonk;
C(exact,7) = C(exact,7) - 7027425405*sk + 1018467450*sk2 - 302630328*sk3 + 17153136*sk4 - 1553552*sk5 + 25376*sk6;
C(exact,7) = 5*C(exact,7).*ek;
C(exact,7) = C(exact,7)/16384./k6(exact);
C(exact,7) = C(exact,7).*ierfik;

% for very large kappa
if size(large,1) > 0
  lnkd = log(k(large)) - log(30);
  lnkd2 = lnkd.*lnkd;
  lnkd3 = lnkd2.*lnkd;
  lnkd4 = lnkd3.*lnkd;
  lnkd5 = lnkd4.*lnkd;
  lnkd6 = lnkd5.*lnkd;
  C(large,2) = 7.52308 + 0.411538*lnkd - 0.214588*lnkd2 + 0.0784091*lnkd3 - 0.023981*lnkd4 + 0.00731537*lnkd5 - 0.0026467*lnkd6;
  C(large,3) = 8.93718 + 1.62147*lnkd - 0.733421*lnkd2 + 0.191568*lnkd3 - 0.0202906*lnkd4 - 0.00779095*lnkd5 + 0.00574847*lnkd6;
  C(large,4) = 8.87905 + 3.35689*lnkd - 1.15935*lnkd2 + 0.0673053*lnkd3 + 0.121857*lnkd4 - 0.066642*lnkd5 + 0.0180215*lnkd6;
  C(large,5) = 7.84352 + 5.03178*lnkd - 1.0193*lnkd2 - 0.426362*lnkd3 + 0.328816*lnkd4 - 0.0688176*lnkd5 - 0.0229398*lnkd6;
  C(large,6) = 6.30113 + 6.09914*lnkd - 0.16088*lnkd2 - 1.05578*lnkd3 + 0.338069*lnkd4 + 0.0937157*lnkd5 - 0.106935*lnkd6;
  C(large,7) = 4.65678 + 6.30069*lnkd + 1.13754*lnkd2 - 1.38393*lnkd3 - 0.0134758*lnkd4 + 0.331686*lnkd5 - 0.105954*lnkd6;
end

% for small kappa
C(approx,2) = 4/3*k(approx) + 8/63*k2(approx);
C(approx,2) = C(approx,2)*sqrt(pi/5);

C(approx,3) = 8/21*k2(approx) + 32/693*k3(approx);
C(approx,3) = C(approx,3)*(sqrt(pi)*0.2);

C(approx,4) = 16/693*k3(approx) + 32/10395*k4(approx);
C(approx,4) = C(approx,4)*sqrt(pi/13);

C(approx,5) = 32/19305*k4(approx);
C(approx,5) = C(approx,5)*sqrt(pi/17);

C(approx,6) = 64*sqrt(pi/21)*k5(approx)/692835;

C(approx,7) = 128*sqrt(pi)*k6(approx)/152108775;

if nargout == 1
	return;
end

% Computing the derivatives
dawsonk2 = dawsonk.^2;
idawsonk2 = 1./dawsonk2;

D = zeros(length(k),n+1);
D(:,1) = 0.0;

% exact
D(exact,2) = -k(exact) + (2*sk2 -sk).*dawsonk + 2*dawsonk2;
D(exact,2) = (.75*sqrt(5*pi))*D(exact,2)./k2(exact).*idawsonk2;

D(exact,3) = 21*k(exact) - 2*k2(exact);
D(exact,3) = D(exact,3) + (63*sk -44*sk2 + 4*sk3).*dawsonk;
D(exact,3) = D(exact,3) - (84 + 24*k(exact)).*dawsonk2;
D(exact,3) = D(exact,3)*(15*sqrt(pi)/32)./k3(exact).*idawsonk2;

D(exact,4) = -165*k(exact) + 20*k2(exact) - 4*k3(exact);
D(exact,4) = D(exact,4) + (-825*sk + 390*sk2 - 44*sk3 + 8*sk4).*dawsonk;
D(exact,4) = D(exact,4) + (990 + 360*k(exact) + 40*k2(exact)).*dawsonk2;
D(exact,4) = D(exact,4)*(21*sqrt(13*pi)/128)./k4(exact).*idawsonk2;

D(exact,5) = 225225*k(exact) - 30030*k2(exact) + 7700*k3(exact) - 248*k4(exact);
D(exact,5) = D(exact,5) + (1576575*sk - 600600*sk2 + 83160*sk3 - 15648*sk4 + 496*sk5).*dawsonk;
D(exact,5) = D(exact,5) - (1801800 + 720720*k(exact) + 110880*k2(exact) + 6720*k3(exact)).*dawsonk2;
D(exact,5) = D(exact,5)*(3*sqrt(17*pi)/2048)./k5(exact).*idawsonk2;

D(exact,6) = -3968055*k(exact) + 556920*k2(exact) - 157248*k3(exact) + 7488*k4(exact) - 464*k5(exact);
D(exact,6) = D(exact,6) + (-35712495*sk + 11834550*sk2 - 1900090*sk3 + 336960*sk4 - 15440*sk5 + 928*sk6).*dawsonk;
D(exact,6) = D(exact,6) + (39680550 + 16707600*k(exact) + 2948400*k2(exact) + 262080*k3(exact) + 10080*k4(exact)).*dawsonk2;
D(exact,6) = D(exact,6)*(11*sqrt(21*pi)/8192)./k6(exact).*idawsonk2;

D(exact,7) = 540571185*k(exact) - 78343650*k2(exact) + 23279256*k3(exact) - 1319472*k4(exact) + 119504*k5(exact) - 1952*k6(exact);
D(exact,7) = D(exact,7) + (5946283035*sk - 1786235220*sk2 + 319642092*sk3 - 53155872*sk4 + 2997456*sk5 - 240960*sk6 + 3904*sk7).*dawsonk;
D(exact,7) = D(exact,7) - (6486854220 + 2820371400*k(exact) + 537213600*k2(exact) + 56548800*k3(exact) + 3326400*k4(exact) + 88704*k5(exact)).*dawsonk2;
D(exact,7) = D(exact,7)*(65*sqrt(pi)/65536)./k7(exact).*idawsonk2;

% approximation
D(approx,2) = 4/3 + 16/63*k(approx) - 16/315*k2(approx) - 128/6237*k3(approx);
D(approx,2) = D(approx,2)*sqrt(pi/5);

D(approx,3) = 16/105*k(approx) + 32/1155*k2(approx) - 3712/675675*k3(approx) - 5888/2837835*k4(approx);
D(approx,3) = D(approx,3)*sqrt(pi);

D(approx,4) = 16/231*k2(approx) + 128/10395*k3(approx) - 256/106029*k4(approx);
D(approx,4) = D(approx,4)*sqrt(pi/13);

D(approx,5) = 128/19305*k3(approx) + 256/220077*k4(approx);
D(approx,5) = D(approx,5)*sqrt(pi/17);

D(approx,6) = 64/138567*k4(approx);
D(approx,6) = D(approx,6)*sqrt(pi/21);

D(approx,7) = 256/50702925*k5(approx);
D(approx,7) = D(approx,7)*sqrt(pi);


function d = empirical_dist(norm_type,x,y)

n = size(x,2);

% wasserstein integer conversion
Sc = 1e+6;

switch norm_type
    case {'wass1' 'wass2'}
        p = str2num(norm_type(5));
        C0 = distmat(x,y).^p;
        C = int32( Sc*C0 );
        [rho,varrho,u,v] = hungarianLSAP(C);
        d = sum( C0( (1:n) + (double(rho)'-1)*n ) ) / n;
        d = d^(1/p);
    case 'energy'
        k = @(a)-abs(a);
        mu = ones(n,1)/n;
        d = sqrt( rkhs_norm_invariant(k,x,mu,y,mu) );
    case 'gaussian'
        sigma = .3;
        k = @(a)exp( -(a).^2 / (2*sigma^2) );
        mu = ones(n,1)/n;
        d = sqrt( rkhs_norm_invariant(k,x,mu,y,mu) );
    otherwise
        error('Unknown');
end

end


function [res,resL2,qualMeasOut]=SART_TV(proj,geo,angles,niter,varargin)
% SART_TV solves Cone Beam CT image reconstruction using Oriented Subsets
%              Simultaneous Algebraic Reconstruction Technique algorithm
%
%   SART_TV(PROJ,GEO,ALPHA,NITER) solves the reconstruction problem
%   using the projection data PROJ taken over ALPHA angles, corresponding
%   to the geometry described in GEO, using NITER iterations.
%
%   SART_TV(PROJ,GEO,ALPHA,NITER,OPT,VAL,...) uses options and values for solving. The
%   possible options in OPT are:
%
%
%   'lambda':      Sets the value of the hyperparameter. Default is 1
%
%   'lambda_red':  Reduction of lambda. Every iteration
%                  lambda=lambdared*lambda. Default is 0.99
%
%   'Init':        Describes different initialization techniques.
%                  'none'     : Initializes the image to zeros (default)
%                  'FDK'      : Initializes image to FDK reconstruction
%                  'multigrid': Initializes image by solving the problem in
%                               small scale and increasing it when relative
%                               convergence is reached.
%                  'image'    : Initialization using a user specified
%                               image. Not recommended unless you really
%                               know what you are doing.
%   'InitImg'      an image for the 'image' initialization. Avoid.
%
%   'TViter'       number of iterations in the TV step. Default 50
%
%   'TVlambda'     hyperparameter in TV iteration. It gives the ratio of
%                  importance of the image vs the minimum total variation.
%                  default is 15. Lower means more TV denoising.
%
%   'Verbose'      1 or 0. Default is 1. Gives information about the
%                  progress of the algorithm.
%
%   'QualMeas'     Asks the algorithm for a set of quality measurement
%                  parameters. Input should contain a cell array of desired
%                  quality measurement names. Example: {'CC','RMSE','MSSIM'}
%                  These will be computed in each iteration.
% 'OrderStrategy'  Chooses the subset ordering strategy. Options are
%                  'ordered' : uses them in the input order, but divided
%                  'random'  : orders them randomly
%                  'angularDistance': chooses the next subset with the
%                                     biggest angular distance with the ones used.
% 'redundancy_weighting': true or false. Default is true. Applies data
%                         redundancy weighting to projections in the update step
%                         (relevant for offset detector geometry)
%  'groundTruth'  an image as grounf truth, to be used if quality measures
%                 are requested, to plot their change w.r.t. this known
%                 data.
%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
% This file is part of the TIGRE Toolbox
%
% Copyright (c) 2015, University of Bath and
%                     CERN-European Organization for Nuclear Research
%                     All rights reserved.
%
% License:            Open Source under BSD.
%                     See the full license at
%                     https://github.com/CERN/TIGRE/blob/master/LICENSE
%
% Contact:            tigre.toolbox@gmail.com
% Codes:              https://github.com/CERN/TIGRE/
% Coded by:           Ander Biguri
%--------------------------------------------------------------------------

%% Deal with input parameters
[lambda,res,lamdbared,verbose,QualMeasOpts,TViter,TVlambda,OrderStrategy,nonneg,gpuids,redundancy_weights,gt]=parse_inputs(proj,geo,angles,varargin);
measurequality=~isempty(QualMeasOpts) | ~any(isnan(gt(:)));
if ~any(isnan(gt(:)))
    QualMeasOpts{end+1}='error_norm';
    res_prev=gt;
    clear gt
end
if nargout<3 && measurequality
    warning("Image metrics requested but none catched as output. Call the algorithm with 3 outputs to store them")
    measurequality=false;
end
qualMeasOut=zeros(length(QualMeasOpts),niter);

resL2=zeros(1,niter);
if nargout>1
    computeL2=true;
else
    computeL2=false;
end

blocksize=1;
[alphablocks,orig_index]=order_subsets(angles,blocksize,OrderStrategy);

angles_reorder=cell2mat(alphablocks);
index_angles=cell2mat(orig_index);
% does detector rotation exist?
if ~isfield(geo,'rotDetector')
    geo.rotDetector=[0;0;0];
end
%% Create weighting matrices

% Projection weight, W
W=computeW(geo,angles,gpuids);

% Back-Projection weight, V
V=computeV(geo,angles,alphablocks,orig_index,'gpuids',gpuids);

if redundancy_weights
    % Data redundancy weighting, W_r implemented using Wang weighting
    % reference: https://iopscience.iop.org/article/10.1088/1361-6560/ac16bc

    num_frames = size(proj,3);
    W_r = redundancy_weighting(geo);
    W_r = repmat(W_r,[1,1,num_frames]);
    % disp('Size of redundancy weighting matrix');
    % disp(size(W_r));
    W = W.*W_r; % include redundancy weighting in W
end

%% Iterate
offOrigin=geo.offOrigin;
offDetector=geo.offDetector;
rotDetector=geo.rotDetector;
DSD=geo.DSD;
DSO=geo.DSO;
% TODO : Add options for Stopping criteria
for ii=1:niter
    if (ii==1 && verbose==1);tic;end
    % If quality is going to be measured, then we need to save previous image
    % THIS TAKES MEMORY!
    if measurequality && ~strcmp(QualMeasOpts,'error_norm')
        res_prev = res; % only store if necesary
    end

    for jj=1:size(angles,2)
        if size(offOrigin,2)==size(angles,2)
            geo.offOrigin=offOrigin(:,index_angles(:,jj));
        end
        if size(offDetector,2)==size(angles,2)
            geo.offDetector=offDetector(:,index_angles(:,jj));
        end
        if size(rotDetector,2)==size(angles,2)
            geo.rotDetector=rotDetector(:,index_angles(:,jj));
        end
        if size(DSD,2)==size(angles,2)
            geo.DSD=DSD(jj);
        end
        if size(DSO,2)==size(angles,2)
            geo.DSO=DSO(jj);
        end
        %         proj_err=proj(:,:,jj)-Ax(res,geo,angles(:,jj));     %                                 (b-Ax)
        %         weighted_err=W(:,:,jj).*proj_err;                   %                          W^-1 * (b-Ax)
        %         backprj=Atb(weighted_err,geo,angles(:,jj));         %                     At * W^-1 * (b-Ax)
        %         weigth_backprj=bsxfun(@times,1./V(:,:,jj),backprj); %                 V * At * W^-1 * (b-Ax)
        %         res=res+lambda*weigth_backprj;                      % x= x + lambda * V * At * W^-1 * (b-Ax)
        res=res+lambda* bsxfun(@times,1./V(:,:,jj),Atb(W(:,:,jj).*(proj(:,:,index_angles(:,jj))-Ax(res,geo,angles_reorder(:,jj),'gpuids',gpuids)),geo,angles_reorder(:,jj),'gpuids',gpuids));
        if nonneg
            res=max(res,0);
        end
    end

    % If quality is being measured
    if measurequality
        qualMeasOut(:,ii)=Measure_Quality(res,res_prev,QualMeasOpts);
    end

    lambda=lambda*lamdbared;
    % TV denoising
    res=im3DDenoise(res,'TV',TViter,TVlambda,'gpuids',gpuids);

    if computeL2
        geo.offOrigin=offOrigin;
        geo.offDetector=offDetector;
        geo.DSD=DSD;
        geo.rotDetector=rotDetector;
        resL2(ii)=im3Dnorm(proj(:,:,index_angles)-Ax(res,geo,angles,'gpuids',gpuids),'L2'); % Compute error norm2 of b-Ax
        % If the error is not minimized.
        if  ii~=1 && resL2(ii)>resL2(ii-1)
            if verbose
                disp(['Convergence criteria met, exiting on iteration number:', num2str(ii)]);
            end
            return
        end
    end

    if (ii==1 && verbose==1)
        expected_time=toc*niter;
        disp('SART_TV');
        disp(['Expected duration   :    ',secs2hms(expected_time)]);
        disp(['Expected finish time:    ',datestr(datetime('now')+seconds(expected_time))]);
        disp('');
    end
end

end

function initres=init_multigrid(proj,geo,alpha,TViter,TVlambda,gpuids)

finalsize=geo.nVoxel;
% start with 64
geo.nVoxel=[64;64;64];
geo.dVoxel=geo.sVoxel./geo.nVoxel;
if any(finalsize<geo.nVoxel)
    initres=zeros(finalsize');
    return;
end
niter=100;
initres=zeros(geo.nVoxel','single');
while ~isequal(geo.nVoxel,finalsize)

    % solve subsampled grid
    initres=SART_TV(proj,geo,alpha,niter,'Init','image','InitImg',initres,'Verbose',0,'TViter',TViter,'TVlambda',TVlambda,'gpuids',gpuids);

    % Get new dims.
    geo.nVoxel=geo.nVoxel*2;
    geo.nVoxel(geo.nVoxel>finalsize)=finalsize(geo.nVoxel>finalsize);
    geo.dVoxel=geo.sVoxel./geo.nVoxel;
    % Upsample!
    % (hopefully computer has enough memory............)
    [y, x, z]=ndgrid(linspace(1,size(initres,1),geo.nVoxel(1)),...
        linspace(1,size(initres,2),geo.nVoxel(2)),...
        linspace(1,size(initres,3),geo.nVoxel(3)));
    initres=interp3(initres,x,y,z);
    clear x y z
end
end

function [lambda,res,lamdbared,verbose,QualMeasOpts,TViter,TVlambda,OrderStrategy,nonneg,gpuids,redundancy_weights,gt]=parse_inputs(proj,geo,alpha,argin)
opts={'lambda','init','initimg','verbose','lambda_red','qualmeas','tviter','tvlambda','orderstrategy','nonneg','gpuids','redundancy_weighting','groundtruth'};
defaults=ones(length(opts),1);
% Check inputs
nVarargs = length(argin);
if mod(nVarargs,2)
    error('TIGRE:SART_TV:InvalidInput','Invalid number of inputs')
end
multigrid=false;
% check if option has been passed as input
for ii=1:2:nVarargs
    ind=find(ismember(opts,lower(argin{ii})));
    if ~isempty(ind)
        defaults(ind)=0;
    else
        error('TIGRE:SART_TV:InvalidInput',['Optional parameter "' argin{ii} '" does not exist' ]);
    end
end

for ii=1:length(opts)
    opt=opts{ii};
    default=defaults(ii);
    % if one option is not default, then extract value from input
    if default==0
        ind=double.empty(0,1);jj=1;
        while isempty(ind)
            ind=find(isequal(opt,lower(argin{jj})));
            jj=jj+1;
        end
        if isempty(ind)
            error('TIGRE:SART_TV:InvalidInput',['Optional parameter "' argin{jj} '" does not exist' ]);
        end
        val=argin{jj};
    end

    switch opt
        % % % % % % % Verbose
        case 'verbose'
            if default
                verbose=1;
            else
                verbose=val;
            end
            if ~is2014bOrNewer
                warning('TIGRE: Verbose mode not available for older versions than MATLAB R2014b');
                verbose=false;
            end
            % % % % % % % hyperparameter, LAMBDA
        case 'lambda'
            if default
                lambda=1;
            else
                if length(val)>1 || ~isnumeric(val)
                    error('TIGRE:SART_TV:InvalidInput','Invalid lambda')
                end
                lambda=val;
            end
        case 'lambda_red'
            if default
                lamdbared=0.99;
            else
                if length(val)>1 || ~isnumeric(val)
                    error('TIGRE:SART_TV:InvalidInput','Invalid lambda')
                end
                lamdbared=val;
            end
        case 'init'
            res=[];
            if default || strcmp(val,'none')
                res=zeros(geo.nVoxel','single');
                continue
            end
            if strcmp(val,'FDK')
                res=FDK(proj,geo,alpha);
                continue
            end
            if strcmp(val,'multigrid')
                multigrid=true;
                continue
            end
            if strcmp(val,'image')
                initwithimage=1;
                continue
            end
            if isempty(res)
                error('TIGRE:SART_TV:InvalidInput','Invalid Init option')
            end
            % % % % % % % ERROR
        case 'initimg'
            if default
                continue
            end
            if exist('initwithimage','var')
                if isequal(size(val),geo.nVoxel')
                    res=single(val);
                else
                    error('TIGRE:SART_TV:InvalidInput','Invalid image for initialization');
                end
            end
        case 'qualmeas'
            if default
                QualMeasOpts={};
            else
                if iscellstr(val)
                    QualMeasOpts=val;
                else
                    error('CBCT:SART_TV:InvalidInput','Invalid quality measurement parameters');
                end
            end
        case 'tviter'
            if default
                TViter=50;
            else
                TViter=val;
            end
        case 'tvlambda'
            if default
                TVlambda=50;
            else
                TVlambda=val;
            end
        case 'orderstrategy'
            if default
                OrderStrategy='random';
            else
                OrderStrategy=val;
            end

        case 'nonneg'
            if default
                nonneg=true;
            else
                nonneg=val;
            end
        case 'gpuids'
            if default
                gpuids = GpuIds();
            else
                gpuids = val;
            end
        case 'redundancy_weighting'
            if default
                redundancy_weights = true;
            else
                redundancy_weights = val;
            end
        case 'groundtruth'
            if default
                gt=nan;
            else
                gt=val;
            end
        otherwise
            error('TIGRE:SART_TV:InvalidInput',['Invalid input name:', num2str(opt),'\n No such option in SART()']);
    end
end
if multigrid
    res=init_multigrid(proj,geo,alpha,TViter,TVlambda,gpuids);
end

end


function Y_interp = interp_missing_data(Y)
% INTERP_MISSING_DATA - interpolate missing data using linear interpolation for each pixel
%
%  [Y_INTERP] = INTERP_MISSING_DATA(Y)
%
%  Given a matrix Y with possible NaN values, this function produces
%  a sparse matrix Y_INTERP that has the linearly interpolated values
%  of each pixel that exhibits a NaN value. The values are interpolated
%  over the last dimension of Y.
%
%  Example:
%     yy = [0 0 ; 0 0];
%     yy(:,:,2) = [0 NaN ; 0 2];
%     yy(:,:,3) = [0.1 0.2 ; 0 2.3];
%     Y_interp = interp_missing_data(yy);
%     % Y_interp is a sparse matrix :    (3,2)  0.1000
%
%  See also: INTERP1

sizY = size(Y);
dimY = length(sizY);
d = prod(sizY(1:dimY-1));
T = sizY(end);
mis_data = cell(d,1);

for i = 1:d
    [ii,jj,kk] = ind2sub(sizY(1:dimY-1),i);
    if dimY == 2
        ytemp = Y(i,:);
    elseif dimY == 3
        ytemp = squeeze(Y(ii,jj,:));
    elseif dimY == 4
        ytemp = squeeze(Y(ii,jj,kk,:));
    end
    f = isnan(ytemp(:));
    y_val = interp1(find(~f),ytemp(~f),find(f),'linear','extrap');
    mis_data{i} = [i*ones(length(y_val),1),find(f(:)),y_val(:)];
end

mis_data = cell2mat(mis_data);

Y_interp = sparse(mis_data(:,1),mis_data(:,2),mis_data(:,3),d,T);


%% Coherence-Enhancing Filtering
%
% Inspired by:
%
% * Joachim Weickert, "Coherence-Enhancing Shock Filters"
%   <http://www.mia.uni-saarland.de/Publications/weickert-dagm03.pdf>
%
% Sources:
%
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/python/coherence.py>
%

function varargout = coherence_demo_gui(im)
    % load color image
    if nargin < 1
        im = fullfile(mexopencv.root(), 'test', 'img001.jpg');
        img = cv.imread(im, 'Color',true);
    elseif ischar(im)
        img = cv.imread(im, 'Color',true);
    else
        img = im;
    end
    assert(size(img,3) == 3, 'RGB image expected');

    % create the UI
    h = buildGUI(img);
    if nargout > 0, varargout{1} = h; end
end

function img = coherence_filter(img, sigma, str_sigma, blend, niter)
    %COHERENCE_FILTER  Coherence-enhancing filter

    for i=1:niter
        gray = cv.cvtColor(img, 'RGB2GRAY');

        % dominant eigenvector
        eigen = cv.cornerEigenValsAndVecs(gray, 'BlockSize',str_sigma);
        x = eigen(:,:,3);  % x1 eigenvector of lambda_1
        y = eigen(:,:,4);  % y1 eigenvector of lambda_1

        % second order derivatives
        opts = {'KSize',sigma, 'DDepth','single'};
        gxx = cv.Sobel(gray, 'XOrder',2, 'YOrder',0, opts{:});
        gyy = cv.Sobel(gray, 'XOrder',0, 'YOrder',2, opts{:});
        gxy = cv.Sobel(gray, 'XOrder',1, 'YOrder',1, opts{:});
        gvv = x.*x.*gxx + 2*x.*y.*gxy + y.*y.*gyy;

        % dilation/erosion
        ero = cv.erode(img);
        dil = cv.dilate(img);
        if true
            img1 = cv.copyTo(dil, 'Dest',ero, 'Mask',gvv<0);
        else
            mask = repmat(gvv<0, [1 1 size(img,3)]);
            img1 = ero;
            img1(mask) = dil(mask);
        end

        % blend
        img = cv.addWeighted(img,1-blend, img1,blend, 0);
    end
end

function onChange(~,~,h)
    %ONCHANGE  Event handler for UI controls

    % retrieve current values from UI controls
    niter = round(get(h.slid(1), 'Value'));
    blend = get(h.slid(2), 'Value');
    sigma = round(get(h.slid(3), 'Value'));
    str_sigma = round(get(h.slid(4), 'Value'));
    set(h.txt(1), 'String',sprintf('Iterations: %d', niter));
    set(h.txt(2), 'String',sprintf('Blend Coeff: %.2f', blend));
    set(h.txt(3), 'String',sprintf('Integration Sigma: %d', sigma));
    set(h.txt(4), 'String',sprintf('Structure Sigma: %d', str_sigma));

    % apply coherence-enhancing filter
    out = coherence_filter(h.src, sigma*2+1, str_sigma*2+1, blend, niter);

    % show result
    set(h.img, 'CData',out);
    drawnow;
end

function h = buildGUI(img)
    %BUILDGUI  Creates the UI

    % parameters
    sz = size(img);
    sz(2) = max(sz(2), 300);  % minimum figure width

    % build the user interface (no resizing to keep it simple)
    h = struct();
    h.src = img;
    h.fig = figure('Name','Coherence', ...
        'NumberTitle','off', 'Menubar','none', 'Resize','off', ...
        'Position',[200 200 sz(2) sz(1)+105-1]);
    if ~mexopencv.isOctave()
        %HACK: not implemented in Octave
        movegui(h.fig, 'center');
    end
    h.ax = axes('Parent',h.fig, 'Units','pixels', 'Position',[1 105 sz(2) sz(1)]);
    if ~mexopencv.isOctave()
        h.img = imshow(img, 'Parent',h.ax);
    else
        %HACK: https://savannah.gnu.org/bugs/index.php?45473
        axes(h.ax);
        h.img = imshow(img);
    end
    h.txt(1) = uicontrol('Parent',h.fig, 'Style','text', ...
        'FontSize',10, 'HorizontalAlignment','left', ...
        'Position',[5 5 130 20], 'String','Iterations:');
    h.txt(2) = uicontrol('Parent',h.fig, 'Style','text', ...
        'FontSize',10, 'HorizontalAlignment','left', ...
        'Position',[5 30 130 20], 'String','Blend Coef:');
    h.txt(3) = uicontrol('Parent',h.fig, 'Style','text', ...
        'FontSize',10, 'HorizontalAlignment','left', ...
        'Position',[5 55 130 20], 'String','Integration Sigma:');
    h.txt(4) = uicontrol('Parent',h.fig, 'Style','text', ...
        'FontSize',10, 'HorizontalAlignment','left', ...
        'Position',[5 80 130 20], 'String','Structure Sigma:');
    h.slid(1) = uicontrol('Parent',h.fig, 'Style','slider', ...
        'Value',4, 'Min',0, 'Max',20, 'SliderStep',[1 4]./(20-0), ...
        'Position',[135 5 sz(2)-135-5 20]);
    h.slid(2) = uicontrol('Parent',h.fig, 'Style','slider', ...
        'Value',0.7, 'Min',0, 'Max',1, 'SliderStep',[0.01 0.1], ...
        'Position',[135 30 sz(2)-135-5 20]);
    h.slid(3) = uicontrol('Parent',h.fig, 'Style','slider', ...
        'Value',9, 'Min',0, 'Max',15, 'SliderStep',[1 3]./(15-0), ...
        'Position',[135 55 sz(2)-135-5 20]);
    h.slid(4) = uicontrol('Parent',h.fig, 'Style','slider', ...
        'Value',9, 'Min',0, 'Max',15, 'SliderStep',[1 3]./(15-0), ...
        'Position',[135 80 sz(2)-135-5 20]);

    % hook event handlers, and trigger default start
    set(h.slid, 'Callback',{@onChange,h}, ...
        'Interruptible','off', 'BusyAction','cancel');
    onChange([],[],h);
end


%
%  Another demo on the Jacobian for the Sawyer robot.
% JACOBIAN DEMO

% Copyright (C) 2017, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.odrg/licenses/>.
close all;

fprintf('\nThe demo shows how to compute the end effectors speed as a function of the joint speeds and viceversa')
%
%robot=load_robot('RETHINK','SAWYER');

%First compute the linear and angular speeds of the end effector given the
%joint speeds qd = [2 2 2 2 2 2 2] rad/s.
% Compute at joint position q = [0.1 0.1 0.1 0.1 0.1 0.1] rad
q = [-0.15 0.0 0.0 0 0.0 0.0 0.0]';
%qd_1 = [2 2 2 2 2 2 2]';

qd_1 = [0 1 0 0 0 0 0 ]';

drawrobot3d(robot, q)

% The result V is  [Vx Vy Vz Wx Wy Wz] [m/s m/s m/s rad/s rad/s rad/s]
% direct jacobian Here
V_1 = compute_end_velocity(robot, q, qd_1)

% Now, at the same position q, we would like to compute the join speeds qd
% that will bring the end effector to the speed V. The result should match
% the values in qd as defined earlier
qd_2 = compute_joint_velocity(robot, q, V_1)

V_2 = compute_end_velocity(robot, q, qd_2)

%cambia q de manera que qd_1 y qd_2 sean diferentes!

%SINGULARITIES
% The abb irb140 shows a singularity point at q = [0 0 0 0 0 0]. This means
% that the manipulator Jacobian J cannot be inverted (det(J)=0). If we
% repeat the former instructions for q = [0 0 0 0 0 0] we get a Warning
% indicating that J is badly conditioned and an unaccurate result.
% (Uncomment the following 4 lines to test this case)
%q = [0 0 0 0 0 0];
%qd = [1 1 1 1 1 1];
%V = compute_end_velocity(robot, q, qd)
%qd = compute_joint_velocity(robot, q, V)


function [dat, state] = ft_preproc_standardize(dat, begsample, endsample, state)

% FT_PREPROC_STANDARDIZE performs a z-transformation or standardization
% of the data. The standardized data will have a zero-mean and a unit
% standard deviation.
%
% Use as
%   [dat] = ft_preproc_standardize(dat, begsample, endsample)
% where
%   dat        data matrix (Nchans dat Ntime)
%   begsample  index of the begin sample for the mean and stdev estimate
%   endsample  index of the end sample for the mean and stdev estimate
%
% If no begin and end sample are specified, it will be estimated on the
% complete data.
%
% See also PREPROC

% Copyright (C) 2008, Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

if nargin<2 || isempty(begsample)
  begsample = 1;
end

if nargin<3 || isempty(endsample)
  endsample = size(dat,2);
end

if nargin<4
  state = [];
end

% preprocessing fails on channels that contain NaN
if any(isnan(dat(:)))
  ft_warning('FieldTrip:dataContainsNaN', 'data contains NaN values');
end

% get the data selection
y = dat(:,begsample:endsample);

% determine the size of the selected data: nChans dat nSamples
[m, n] = size(y);

% compute the sum and sum of squares
s  = sum(y,2);
ss = sum(y.^2,2);

% include the state information from the previous calls
if ~isempty(state)
  s  = s  + state.s;
  ss = ss + state.ss;
  n  = n  + state.n;
end

% compute the mean and standard deviation
my = s ./ n;
sy = sqrt((ss - (s.^2)./n) ./ (n-1));

% standardize the complete input data
dat = (dat - repmat(my, 1, size(dat, 2))) ./ repmat(sy, 1, size(dat, 2));

% remember the state
state.s  = s;  % sum
state.ss = ss; % sum of sqares
state.n  = n;  % number of samples


function test_mcmc_reflective()

N = 2;
mu = zeros(N,1);
Q = [1 1; -1 1]; %orth(randn(N));
D = (1-0.95)*eye(N);
D(1) = 1;
Cov = Q*D*Q';
%Cov = W'*W;
L = chol(Cov, 'lower');

get_logpdf = @(x) (@()gaussian_logpdf(x, mu, L));
get_dlogpdf = @(x) (@()gaussian_dlogpdf(x, mu, L));

x_init = 10*ones(N,1);
%x_init = zeros(N,1);

M = 1000;

% Test metropolis-hastings
y_mh = zeros(N,M);
if 1
  q = @(x) normrnd(x,0.3);
  logpdf_q = @(x,x0) normal_logpdf(x,x0,0.3);
  sampler = mcmc_init_metropolishastings(x_init, get_logpdf, q, logpdf_q);
  for m=1:M
    y_mh(:,m) = sampler();
  end
end

% Test hamiltonian
y_h = zeros(N,M);
if 1
  sampler = mcmc_init_hamiltonian(x_init, get_logpdf, get_dlogpdf, 0.3, 10);
  for m=1:M
    y_h(:,m) = sampler();
  end
end

% Test slice
y_s = zeros(N,M);
if 1
  sampler = mcmc_init_slicesampling(x_init, get_logpdf);
  for m=1:M
    y_s(:,m) = sampler();
  end
end

% Test inside reflective
y_r = zeros(N,M);
if 1
  sampler = mcmc_init_reflective(x_init, get_logpdf, get_dlogpdf, 0.01, ...
                                 300, 'type', 'inside');
  for m=1:M
    y_r(:,m) = sampler();
  end
  % TODO: Is this correct?
  y_r(:,isnan(y_r(1,:))) = [];
end

y = bsxfun(@plus, mu, L*randn(N,M));

figure(1)
clf();
plot(y(1,:), y(2,:), 'k.')
hold on
plot(y_mh(1,:), y_mh(2,:), 'r.')
plot(y_h(1,:), y_h(2,:), 'c.')
plot(y_s(1,:), y_s(2,:), 'b.')
plot(y_r(1,:), y_r(2,:), 'g.')

% Burn-in
M0 = 100;
y_mh = y_mh(:,M0:end);
y_h = y_h(:,M0:end);
y_s = y_s(:,M0:end);
y_r = y_r(:,M0:end);

M0 = 100;

T = Cov + mu*mu'
YY = y*y' / (size(y,2))
YY_mh = y_mh*y_mh' / (size(y_mh,2))
YY_h = y_h*y_h' / (size(y_h,2))
YY_s = y_s*y_s' / (size(y_s,2))
YY_r = y_r*y_r' / (size(y_r,2))

% Auto-correlation
figure(2)
clf();
lag = 100;
subplot(5,1,1)
plot(0:lag, acorr(y',lag))
subplot(5,1,2)
plot(0:lag, acorr(y_mh',lag))
subplot(5,1,3)
plot(0:lag, acorr(y_h',lag))
subplot(5,1,4)
plot(0:lag, acorr(y_s',lag))
subplot(5,1,5)
plot(0:lag, acorr(y_r',lag))


function [y, iter, lambda, status] = minimize_cubic_newton(H, g, sigma, options)
% Minimize a cubicly regularized quadratic via Newton root finding.
%
% [y, iter, lambda, status] = minimize_cubic_newton(H, g, sigma, options)
%
% Inputs: a symmetric matrix H of size n, a nonzero vector g of length n,
% a positive real sigma and an options structure. The code expects H to
% be tridiagonal, stored as a sparse matrix.
%
% The main output is a vector y of length n, which should minimize
%
%   f(y) = g'*y + (1/2)*y'*H*y + (1/3)*sigma*norm(y)^3.
%
% This is achieved by reducing the problem to a univariate root finding
% problem, where the unknown is a scalar lambda. This root is computed
% using a Newton method.
%
% Other outputs are iter (the number of Newton iterations completed),
% lambda (a real scalar, see below) and status. The latter is 0 if the
% target tolerance was reached, 1 if subsequent iterations induce no
% significant change, and -1 if the algorithm return because it reached
% the maximum number of iterations (see the options structure.)
% Non-negative status values are considered successes.
%
% The options structure must contain the following fields (between
% parentheses are some recommended values):
%   options.verbosity (3): to control how much information this function
%   prints to the command window. Anything below 6 silences the function.
%   options.maxiter_newton (100): maximum number of Newton iterations.
%   options.tol_newton (1e-16): tolerance on the root finding accuracy. See
%   in code for details.
%
% The code is based on Section 6 in
% Cartis, Gould and Toint, "Adaptive cubic regularisation methods for
% unconstrained optimization. Part I: motivation, convergence and numerical
% results", Mathematical Programming, 2011.
% https://link.springer.com/article/10.1007/s10107-009-0286-5
%
% Theorem 3.1 in the referenced paper states y is optimal if and only
% if it there exists a real lambda such that
%
% (H + lambda*I)y = -g,  lambda = sigma*||y||  and  H + lambda*I is psd,
%
% where psd means positive semidefinite. The other way around, if we
% find the corresponding scalar lambda, than we can recover y by
% solving a linear system (though this system might not have a unique
% solution in full generality.) Thus, the general strategy is to search
% for lambda rather than for y.
%
% See also: arc arc_lanczos

% This file is part of Manopt: www.manopt.org.
% Original authors: May 1, 2018,
%    Naman Agarwal, Brian Bullins, Nicolas Boumal and Coralia Cartis.
% Contributors:
% Change log:

    n = size(H, 1);

    % Pick an initial lambda that is cheap to compute and that surely makes
    % the shifted H positive definite.
    lambda = norm(H, 1) + 2;
    H_shifted = H + lambda*speye(n);

    % Compute the smallest eigenvalue of H, as we know the target lambda
    % must be at least as large as the negative of that, so that the
    % shifted H will be positive semidefinite.
    %
    % Since H ought to be sparse and tridiagonal, and since we only need
    % its smallest eigenvalue, this computation could be sped up
    % significantly. It does not appear to be a bottleneck, and eig is
    % simple and reliable, so we keep this for now.
    lambda_min = min(eig(H));
    left_barrier = max(0, -lambda_min);

    % Counter 'iter' holds the number of fully executed Newton iterations.
    iter = 0;
    while true

        if iter >= options.maxiter_newton
            % Iterations exceeded maximum number allowed.
            status = -1;
            return;
        end

        % If lambda has the correct value and the shifted H is positive
        % definite, then this y is a minimizer.
        y = -(H_shifted\g);
        ynorm = norm(y);

        % If the following quantity is zero, we have found a solution.
        phi = 1/ynorm - sigma/lambda;

        % Check if it is close enough to zero to stop.
        if abs(phi) <= options.tol_newton*ynorm
            status = 0;
            return;
        end
        psi = ynorm^2;

        % TODO: clarify this part of the code (see referenced paper).
        % The following is a Newton type of step on the equation
        % sigma/lambda = 1/sqrt(psi(lambda_prev)) ...
        %          - (lambda - lambda_prev)((psi'(lambda_prev))/2(psi)^1.5)
        delta_y = -(H_shifted\y);
        psi_prime = 2*(y'*delta_y);
        p0 = 2*sigma*(psi^(1.5));
        p1 = -2*psi - lambda*psi_prime;
        p2 = psi_prime;
        r = roots([p2 p1 p0]);
        del_lambda = max(r) - lambda;
        iter = iter + 1;

        % If the Newton step would bring us left of the left barrier, jump
        % instead to the midpoint between the left barrier and the current
        % lambda.
        if lambda + del_lambda <= left_barrier
            del_lambda = -.5*(lambda - left_barrier);
        end

        % If the step is so small that it numerically does not make a
        % difference when added to the current lambda, we stop.
        if abs(del_lambda) <= eps(lambda)
            status = 1;
            return;
        end

        % Update lambda
        H_shifted = H_shifted + del_lambda*speye(n);
        lambda = lambda + del_lambda;

        if options.verbosity >= 6
            fprintf(['lambda %.12e, ||y|| %.12e, lambda/sigma %.12e, ' ...
                     'phi %.12e\n\n'], lambda, ynorm, lambda / sigma, phi);
        end

    end

end


clc
clear
close all
addpath(genpath(pwd))

%  basic plotting
tmp_ = 5;
t1 = 0:pi/20:8*pi;
t2 = 8*pi:pi/20:16*pi;
y1_ = exp(-t1/tmp_ );
y2_ = exp(-t1/tmp_ ).*sin(tmp_ *t1);
t = [t1, t2];
y1 = [y1_, fliplr(y1_)];
y2 = [y2_, fliplr(y2_)];

figure;
plot(t, y2, 'Color', 'r', 'LineStyle', '-', 'LineWidth', 1.5)
hold on
plot(t, y1, 'Color', 'b', 'LineStyle', ':', 'LineWidth', 1.5)
plot(t, -y1, 'Color', 'b', 'LineStyle', ':','LineWidth', 1.5)
xlim([min(t), max(t)])

% add 2 zoomed zones
zp = BaseZoom();
zp.plot;
zp.plot;


function Sigma_Vector=alpha_generator(l,M,nk)

if l==0 || l>M-1 || nk>M
    display('wrong input parameter')
    Sigma_Vector=[];
    return
end

flag_one=0;

for k=1:nk-1

    temp(k)=exp(j*2*pi*k/nk);
    if temp(k)==exp(j*2*pi*l/M)
        flag_one=1;
    end

end

if flag_one==0
    Sigma_Vector=0;
else
    Sigma_Vector=1;
end


function varargout = linearCompressibility(C,varargin)
% computes the linear compressibility of an elasticity tensor
%
% Description
%
% $$\beta(x) = S_{ijkk} x_i x_j$$
%
% Input
%  C - elastic @stiffnessTensor
%  x - list of @vector3d
%
% Output
%  beta - linear compressibility in directions v
%

% compute linear compressibility from complience
[varargout{1:nargout}] = linearCompressibility(inv(C),varargin{:});


function [D, dD1, dD2] = dist_earth(coord1, coord2)
% [D, dD] = dist_earth(coord1, coord2)
% coord1 is 2 x N
% coord2 is 2 x M
% Returns N x M matrix D of mutual distances.

if nargin < 2 || isempty(coord2)
  coord2 = coord1;
end

R = 6371.01; % Spherical Earth radius approximation

n1 = cols(coord1);
n2 = cols(coord2);

D = zeros(n1, n2);

coord1 = pi/180 * coord1;
coord2 = pi/180 * coord2;
if nargout >= 2
  dD1 = zeros([n1, n2, 2]);
end
if nargout >= 3
  error('Hmm.. maybe you shouldn''t use third output? :)');
  dD2 = zeros([n1, n2, 2]);
end

for i=1:n1
  lat1 = coord1(2,i);
  lat2 = coord2(2,:);
  dlon = (coord2(1,:) - coord1(1,i));
  f = sin(lat2).*sin(lat1) + cos(lat2).*cos(lat1).*cos(dlon);
  f(f>=1-eps) = 1-eps; % correction because of numerical errors, f should be [-1,1]
  f(f<=eps-1) = eps-1; % correction because of numerical errors, f should be [-1,1]
  D(i,:) = R * acos(f);
  if ~isreal(D)
    coord1
    f
%    D
    error('Oohps! Complex distance..');
  end
  %dD_df(isinf(dD_df)) = 0;%-R ./ sqrt(1-f.^2);

  if nargout >= 2
    dD_df = -R ./ sqrt(1-f.^2);
    dD1(i,:,1) = pi/180 * dD_df .* cos(lat1) .* cos(lat2) .* sin(dlon);
    dD1(i,:,2) = pi/180 * dD_df .* (cos(lat1).*sin(lat2) - sin(lat1).* ...
                                    cos(lat2).*cos(dlon));
  end
% $$$   if any(isnan(dD1))
% $$$     dD1
% $$$     error('WTF?');
% $$$   end
% $$$   if nargout >= 3
% $$$     dD2(i,:,1) = -dD1(i,:,1);
% $$$     dD2(i,:,2) = dD_df .* (sin(lat1).*cos(lat2) - cos(lat1).*sin(lat2).*cos(dlon));
% $$$   end
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Spectral-spatial RF pulse design script in Matlab, based on
%"Spectral-spatial RF pulse design for through-plane phase precompensatory
%slice selection for T2*-weighted functional MRI", Chun-yu Yip et al,
%Magnetic Resonance in Medicine, 2009.
%
%Written by Chun-yu Yip, University of Michigan, Ann Arbor, 4/1/2009
%Current affiliation: Athinoula A. Martinos Center for Biomedical Imaging,
%Massachusetts General Hospital, Harvard Medical School, Charlestown, MA,
%USA.
%Email address: chunyuy@nmr.mgh.harvard.edu
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%Running this script requires that the image reconstruction toolbox be
%installed first: http://www.eecs.umich.edu/~fessler/irt/fessler.tgz. In
%your Matlab workspace, please "addpath" the folders in the Fessler
%toolbox. For example:
%
%addpath('fesslertoolboxlocation/utilities/');
%addpath('fesslertoolboxlocation/systems/');
%addpath('fesslertoolboxlocation/nufft/');
%addpath('fesslertoolboxlocation/wls/');
%addpath('fesslertoolboxlocation/mex/');
%addpath('fesslertoolboxlocation/mex/v7/');
%etc.
%
%You can add those lines in your startup.m file so that you do not have to
%add the paths manually everytime. You can type "path" at the matlab prompt
%to check that the paths are successfully added.
%
%
%To design pulse, you have to first load pulse design parameter values. To
%start, you can adopt those we used in our paper in the
%example_parameter_files folder. To keep track of parameters, we recommend
%that you create one folder for each pulse design occasion. For example,
%you can create a "siemens_parameter_files" folder and a
%"ge_parameter_files", or "parameter_files_1Jan2010".
%

[status,homepath] = system('pwd');
homepath = strtrim(homepath);
cd([homepath '/example_parameter_files/']);

kp = kparametersSPSP;           %k-space trajectory parameters
rfp = rfparametersSPSP;         %Pulse design parameters
iop = ioparametersSPSP;         %Input-output parameters

cd(homepath);
%
%kp, rfp, iop are matlab structures, whose fields can be accessed by, e.g.,
%"kp.pointtime". Please see parameter files for details of each field.

%Design z-gradient waveform, based on parameters in kp.
[kp,gz,kz,kf] = compute_gz_spsp(kp);

%Design complex-valued RF waveform iteratively using conjugate gradient
[b] = compute_rf_spsp_mgh(kp,rfp,gz,kz,kf);

%Write computed waveforms to files for simulation and/or scanner.
write2files_spsp(kp,rfp,iop,gz,b);

%Perform Bloch simulation in SPSP space
[mresult] = dosim7_spsp(kp,rfp,iop);


function [ber, rate] = OMP_method()

global Ns Nrf H Vn W_mopt Codebook_v Codebook_w;
n = 1;   %number of iterCodebook_vion
MSE =zeros(1,11);
W_D = W_mopt;  %initializCodebook_vion
W_RF = 1;
tw = trace(W_D'*(W_RF)'*W_RF*W_D);
[~,N_t] = size(H);
V_RF = exp( 1i*unifrnd(0,2*pi,N_t,Nrf) );
while(n<3 || (MSE(n-2)-MSE(n-1))>1e-4 &&n<=10)
    H_u = H'*W_RF*W_D;          %effective downlink channel
    Vn1 = tw*Vn;
    [V_RF,V_D] = MSEOMP (Nrf,H_u,Codebook_v,Vn1);
    tv = trace(V_RF*(V_D)*V_D'*V_RF');
    %%UEside
    H_d = H*V_RF*V_D;
    Vn2 = tv*Vn;
    [W_RF,W_D] = MSEOMP (Nrf,H_d,Codebook_w,Vn2); %the same formulCodebook_vion
    He = W_D'*W_RF'*H*V_RF*V_D;
    tw = trace(W_D'*(W_RF)'*W_RF*W_D);
    MSE(n) = trace(He * He'- He- He'+ eye(Ns)) + Vn2*tw;
    n = n + 1;
end
V_D = V_D/sqrt(tv);
%W_B = W_D*sqrt(tv);

V = V_RF * V_D;
W = W_RF * W_D;

ber = get_ber(V, W);
rate = get_rate(V, W);


function e = rmModelSearchFit_oneOvalGaussian(p,Y,Xv,Yv,stim,t)
% rmModelSearchFit_oneOvalGaussian - actual fit function of rmSearchFit
%
% error = rmModelSearchFit_oneOvalGaussian(p,Y,trends,Xgrid,YGrid,stimulusMatrix);
%
% Basic barebones fit of a single time-series. Error is returned in
% percentage: 100% is RSS of unfitted time-series. This way we can quantify
% the improvement of the fit independend of the variation in the raw
% time-series.
%
% 2006/06 SOD: wrote it.
% 2006/12 SOD: modifications for fmincon, this is litterally called >10000
% times so we cut every corner possible.

% make RF (taken from rfGaussian2d)
Xv = Xv - p(1);   % positive x0 moves center right
Yv = Yv - p(2);   % positive y0 moves center up

Xold = Xv;
Yold = Yv;
Xv = Xold .* cos(p(5)) - Yold .* sin(p(5));
Yv = Xold .* sin(p(5)) + Yold .* cos(p(5));

% make gaussian on current grid
RF = exp( -.5 * ((Yv ./ p(3)).^2 + (Xv ./ p(4)).^2));

% make prediction (taken from rfMakePrediction)
X = [stim*RF t];

% fit - inlining pinv
%b = pinv(X)*Y;
[U,S,V] = svd(X,0);
s = diag(S);
tol = numel(X) * eps(max(s));
r = sum(s > tol);
if (r == 0)
    pinvX = zeros(size(X'));
else
    s = diag(ones(r,1)./s(1:r));
    pinvX = V(:,1:r)*s*U(:,1:r)';
end
b = pinvX*Y;

% compute residual sum of squares (e)
% e = norm(Y - X*abs(b));
if b(1)>0,
    e = norm(Y - X*b);
else
    e = norm(Y).*(1+sum(abs(b(1))));
end
return;


function [nmps2] = mmps22nmps2(mmps2)
% Convert acceleration from millimeters per square-second to nanometers per
% second squared
% Chad A. Greene 2012
nmps2 = mmps2*1e+6;


% irreg_demo
%
% demo of an irregularly sampled problem
%
% Author Piet M.T. Broersen, September 2008
%
% The three statements :
%           ******************************************************
%           *  [asel bsel sellog]=ARMAsel_mis(ti,xi,ARmax,Tr,w); *
%           *  psdsel=arma2psd(asel,bsel);                       *
%           *  corsel=arma2cor(asel,bsel,50);                    *
%           ******************************************************
%  compute the selected estimated spectrum and autocorrelation function.
%  Sellog gives additional information and plots spectra of all estimated
%  AR models
%
%  ti : observation times [s]
%  xi : observation amplitudes
%  ARmax : higherst candidate AR order
%  Tr : equidistant resampling distance [s]
%  w ; slot width as fraction of Tr [s]
%
%  If the mean sampling frequency f0 Hz is high in comparison with the highest
%  frequency in the desired spectrum, it is advisable to use
%  nearest neighbor resampling and armasel.
%  See the example in simple_irreg_demo.
%  This gives very accurate spectra until f0/20 Hz and
%  about 50 % error due to resampling at f0/2pi.
%  The error becomes very large for higher frequencies and
%  armasel_irreg is advised for spectra higher than f0/2pi Hz.

clear all, close all, clc, echo off

load irreg_data
% row vectors of times ti and amplitudes xi of irregularly sampled observations

N=length(ti)

figure
plot(ti,xi,'p',ti,xi,':r')
title([int2str(N),' irregular benchmark data.'])
xlabel('\rightarrow time axis [s]')
axis tight

disp('Mean time between observations')
T0=(ti(N) - ti(1))/(N-1)

disp(' ')
disp('************************************************************************')
disp('Some warning messages from the OPTIM toolbox cannot be suppressed easily')
disp('It is not necessary to provide gradient information')
disp('Warnings generated by the MATLAB OPTIM routine fminunc can be ignored')
disp('************************************************************************')
disp(' ')
disp('Input values for ARMAsel_irreg')
ARmax=3
w=1/2            %*Tres   slot width, fraction of Tres
Tr=1/2000
disp(' ')
disp('[air bir sellogir] = ARMAsel_irreg(ti,xi,ARmax,Tr,w)')

[air bir sellogir] = ARMAsel_irreg(ti,xi,ARmax,Tr,w)

disp('************************************************************************')
disp(' ')
disp('New input for w')
w=1/8
disp(' ')
disp('[air2 bir2 sellogir2] = ARMAsel_irreg(ti,xi,ARmax,Tr,w)')
[air2 bir2 sellogir2] = ARMAsel_irreg(ti,xi,ARmax,Tr,w)

disp('************************************************************************')
disp(' ')
disp('New input for w and Tr')
w=1/2            %*Tres   slot width, fraction of Tres
Tr2=1/4000
disp(' ')
disp('[air3 bir3 sellogir3] = ARMAsel_irreg(ti,xi,ARmax,Tr,w)')
[air3 bir3 sellogir3] = ARMAsel_irreg(ti,xi,ARmax,Tr2,w)

[h_ir f_ir] = arma2psd(air,bir,1000,Tr);
h_ir2 = arma2psd(air2,bir2,1000,Tr);
[h_ir3 f_ir3] = arma2psd(sellogir3.AR_sel_corrected,1,1000,Tr2);

disp(' ')
disp('  rc AR_sel for w = 1/2 and for w = 1/8, Tr = 0.0005 s')
[dum rc_sel]=ar2arset(sellogir.ar);
[dum rc_sel2]=ar2arset(sellogir2.ar);
[dum rc_sel3]=ar2arset(sellogir3.AR_sel_corrected);
disp(rc_sel)
disp(rc_sel2)
disp('  rc AR_sel-corrected for w = 1/2, Tr = 0.00025 s')
disp(rc_sel3)

figure
loglog(f_ir,h_ir,f_ir,h_ir2,f_ir3,h_ir3)
legend('\it{Tr}\rm = 0.0005 s,  \itw\rm = 1/2 * \it{Tr}\rm s', ...
       '\it{Tr}\rm = 0.0005 s,  \itw\rm = 1/8 * \it{Tr}\rm s', ...
       '\it{Tr}\rm = 0.00025 s, \itw\rm = 1/2 * \it{Tr}\rm s',3)
title(['PSD of ',int2str(N),' irregular benchmark data.'])
xlabel('\rightarrow frequency [Hz]')
ylabel('\rightarrow Logarithm of power spectral density')
axis tight


% SENSE_recon_demo.m
% - demonstrates use of SENSE_recon.m, which implements the SENSE
% reconstruction
% - figure shows how Tikhonov regularization mitigates the increase in error
% as noise variance increases
% - applies complex AWGN to kspace
% - image must be even both directions
%
% 2012-06-08, Mai Le
% 2013-01-02 Mai Le, tweaks

% comparison = 1 will demonstrate effect of masks and regularization on MSE,
% but demo takes much longer. comparison = 0 runs masked SENSE w/o
% regularization
comparison = 0;

% number of coils
nc = 4;
% factor of undersampling
np = 2;
% dimension to undersample
for reduced_dim = [1 2]
    display(['SENSE reduced in dim ' num2str(reduced_dim)]);

    % set up "true" brain image
    %f.dir = [path_find_dir('mri') '/../data/mri/'];
    %f.xtrue = [f.dir 'brainweb_t1.jpg'];
    %mag_true = double(imread(f.xtrue))'; % true image magnitude
    mag_true = 25*ellipse_im(256);
    % requires image to be multiple of np in dimension of undersampling
    % and even for generating smap
    if (reduced_dim == 1)
        mag_true = mag_true(1:floor(size(mag_true,1)/(np*2))*(np*2),:);
    elseif (reduced_dim == 2)
        mag_true = mag_true(:,1:floor(size(mag_true,2)/(np*2))*(np*2));
    else
        fail 'bug'
    end

    dims = size(mag_true);
    % introduce planar phase
    [xx,yy] = ndgrid(-dims(1)/2:dims(1)/2-1,-dims(2)/2:dims(2)/2-1);
    ph_max = pi/5;
    ph = ph_max*xx/(dims(1)/2)+ph_max*yy/(dims(2)/2);
    image = mag_true.*exp(1i*ph);

    % generate sensitivity maps
    smap = mri_sensemap_sim('nx',dims(1),'ny',dims(2),'ncoil',nc,'rcoil',400);
    close;

    im_rep = repmat(image,[1 1 nc]);

    % apply sensitivity maps
    mapped_im = im_rep.*smap;

    % throw away k-space data
    im_fft = zeros(size(mapped_im));
    if reduced_dim == 1
        reduced_fft = zeros(dims(1)/np,dims(2),nc);
    elseif reduced_dim == 2
        reduced_fft = zeros(dims(1),dims(2)/np,nc);
    else
        fail 'bug'
    end
    for ii = 1:nc
        im_fft(:,:,ii) = fft2(mapped_im(:,:,ii));
        if reduced_dim == 1
            reduced_fft(:,:,ii) = im_fft(1:np:end,:,ii);
        elseif reduced_dim == 2
            reduced_fft(:,:,ii) = im_fft(:,1:np:end,ii);
        else
            fail 'bug'
        end
    end

    %% construct mask
    thresh = 0.005;
    thresh_mask = any(abs(mapped_im)>thresh,3);

    %% perform SENSE reconstruction for varying levels of noise
    f.sigmas = 0:50:550; % complex AWGN sdtd dev
    clear recon
    recon(1).im = zeros(dims(1),dims(2),length(f.sigmas));
    recon(1).err = zeros(length(f.sigmas),1);
    if comparison
        recon = repmat(recon,4,1);
    end
    nrms_fun = @(x,y) 100 * nrms(x(:), y(:));
    for ii = 1:length(f.sigmas)
        % introduce complex AWGN
        sigma = f.sigmas(ii);
        noisy_reduced_fft = reduced_fft + sigma*randn(size(reduced_fft)) + ...
            1i*sigma*randn(size(reduced_fft));

        % SENSE reconstruction
        recon(1).im(:,:,ii) = SENSE_recon(smap, noisy_reduced_fft, ...
            np, 'mask', thresh_mask, 'direction', reduced_dim);
        if comparison
            recon(2).im(:,:,ii) = SENSE_recon(smap, noisy_reduced_fft, ...
                np, 'direction', reduced_dim);
            recon(3).im(:,:,ii) = SENSE_recon(smap, noisy_reduced_fft, ...
                np, 'reg', 'Tikhonov', 'direction', reduced_dim);
            recon(4).im(:,:,ii) = SENSE_recon(smap, noisy_reduced_fft, ...
                np, 'mask', thresh_mask, 'reg', 'Tikhonov', 'direction', reduced_dim);
        end
        for num = 1:(1+3*comparison) % do all 4 if comparison
            recon(num).err(ii) = nrms_fun(image, recon(num).im(:,:,ii));
        end
    end
    %% plot error as a function of noise variance
    figure; plot(f.sigmas.^2, recon(1).err, 'bo');
    if (comparison)
        hold on; plot(f.sigmas.^2, recon(2).err,'ko');
        plot(f.sigmas.^2, recon(3).err, 'rx');
        plot(f.sigmas.^2, recon(4).err, 'gx');
        legend('no reg, mask', 'no reg, no mask', 'tikhonov, no mask', 'tikhonov, mask');
    end
    xlabel('variance of complex AWGN in kspace');
    ylabel('RMS error in reconstructed images');
    if (comparison)
        title('effect of Tikhonov regularization on noisy SENSE reconstruction')
    else
        title('RMSE as function of noise variance');
    end
    %% plot reconstructed image at sigma(ii)
    ii = 4; % change as desired
    clim = minmax(mag_true)';
    if (comparison)
        im(1, real(image));
        im(2, imag(image));
        titlef('imag of original image, noise variance: %d', f.sigmas(ii).^2);
        im(5, real(recon(1).im(:,:,ii)), clim);
        title('real of recon image, no reg');
        im(6, imag(recon(1).im(:,:,ii)), clim);
        title('imag of recon image, no reg');
        im(7, abs(real(image)-real(recon(1).im(:,:,ii)))+ ...
            abs(imag(image)-imag(recon(1).im(:,:,ii))), [-9 9]);
        title('abs(differences), no reg');
        im(9, real(recon(2).im(:,:,ii)), clim);
        title('real of recon image, Tikhonov');
        im(10, imag(recon(2).im(:,:,ii)), clim);
        title('imag of recon image, Tikhonov');
        im(11, abs(real(image)-real(recon(2).im(:,:,ii)))+ ...
            abs(imag(image)-imag(recon(2).im(:,:,ii))), [-9 9]);
        title('abs(differences), Tikhonov');
    else
        im(1, real(image), clim);
        title('real of original image');
        im(2, imag(image), clim);
        titlef('imag of original image, noise variance: %d', f.sigmas(ii).^2);
        im(5, real(recon(1).im(:,:,ii)), clim);
        title('real of recon image, masked');
        im(6, imag(recon(1).im(:,:,ii)), clim);
        title('imag of recon image, masked');
        im(7, abs(real(image)-real(recon(1).im(:,:,ii)))+ ...
            abs(imag(image)-imag(recon(1).im(:,:,ii))), [-9 9]);
        title('abs(differences), masked');
    end

prompt
end


% DEMUSPSVARGPLVM2 Demonstrate linear variational GPLVM (Bayesian PCA) on USPS data.

% VARGPLVM

% Fix seeds
randn('seed', 1e5);
rand('seed', 1e5);

dataSetName = 'usps';
experimentNo = 2;
printDiagram = 1;

% load data
[YTrain, lblsTrain, YTest, lblsTest] = lvmLoadData(dataSetName);

% Set up model
options = vargplvmOptions('dtcvar');
options.kern = {'linard2', 'white'};
options.numActive = 20;

options.optimiser = 'scg';
latentDim = 10;
d = size(YTrain, 2);

iters = 1000;
display = 1;

% create a separate vargplvm for each digit
for i=1:10
    %
    Y = YTrain(lblsTrain(:,i)==1,:);

    model = vargplvmCreate(latentDim, d, Y, options);
    %
    model = vargplvmParamInit(model, model.m, model.X);

    model = vargplvmOptimise(model, display, iters);

    varmodel{i} = model;
    %
end

iters = 100;
display = 0;

capName = dataSetName;
capName(1) = upper(capName(1));
modelType = model.type;
modelType(1) = upper(modelType(1));
save(['dem' capName modelType num2str(experimentNo) '.mat'], 'varmodel');

% measure performance on test data
indexPresent = 1:size(YTest,2);
TestError = 0;
for n=1:size(YTest,1)
    %
    % compute the approximate class conditional density for each digit
    for i=1:10
       prob(n,i) = vargplvmProbabilityCompute(varmodel{i}, YTest(n,:), indexPresent)
    end
    [maxP C] = max(prob(n,:));
    if lblsTest(n,C) == 0
        TestError = TestError + 1;
    end
    %
end

%
save(['dem' capName modelType num2str(experimentNo) '.mat'], 'varmodel', 'prob', 'TestError');