function [vk, f] = ADMM3D_solver(psf,b,solverSettings)
% ADMM solver to compute 3D diffusercam images
% H: Impulse stack 3D array
% b: measurement image from camera
% solverSettings: user defined params. See DiffuserCam_settings.m for
% details

assert(size(psf,1) == size(b,1) || size(psf,2) == size(b,2),'image and impulse have different dimensions');

if ~isfield(solverSettings,'print_interval')
    solverSettings.print_interval = 1;
end

if ~isfield(solverSettings,'disp_figs')
    solverSettings.disp_figs = 1;
end

if ~isfield(solverSettings,'autotune')
    solverSettings.autotune = 1;
end

if ~isfield(solverSettings,'maxIter')
    solverSettings.maxIter = 200;
end

if ~isfield(solverSettings,'regularizer')
    solverSettings.regularizer = 'TV';
end

if ~isfield(solverSettings,'display_func')
    solverSettings.display_func = @(x)x;
end

if ~isfield(solverSettings,'cmap')
    solverSettings.cmap = 'gray';
end

if isfield(solverSettings,'save_vars')
    assert(iscell(solverSettings.save_vars),'solverSettings.save_vars must be a cell array of strings')
else
    solverSettings.save_vars = {'vk'};
end

if strcmpi(solverSettings.save_vars,'')
    solverSettings.save_vars = {'vk'};
end

mu1 = solverSettings.mu1;   %Set initial ADMM parameters
mu2 = solverSettings.mu2;
mu3 = solverSettings.mu3;

[Ny, Nx, Nz] = size(psf);   %Get problem size

% Setup convolutional forward op
p1 = floor(Ny/2);
p2 = floor(Nx/2);
%h(p1,p2,Nz/2) = 1;
pad2d = @(x)padarray(x,[p1,p2],'both');  %2D padding
crop2d = @(x)x(p1+1:end-p1,p2+1:end-p2,:); %2D cropping
crop3d = @(x)crop2d(x(:,:,1));   %3D cropping. This is D
vec = @(X)reshape(X,numel(X),1);
pad3d = @(x)padarray(pad2d(x),[0 0 Nz-1],'post');
psf = circshift(flip(psf,3),ceil(Nz/2)+1,3)/norm(psf(:));  %Shift impulse stack and normalize
Hs = fftn(ifftshift(pad2d(psf)));  %Compute 3D spectrum
Hs_conj = conj(Hs);
clear psf
Hfor = @(x)real((ifftn(Hs.*fftn((x)))));
Hadj = @(x)real((ifftn(Hs_conj.*fftn((x)))));
HtH = abs(Hs.*Hs_conj);

vk = 0*real(Hs);   %Initialize variables. vk is the primal (this is the image you want to find)
xi = vk;  % Dual associated with Mv = nu (boundary condition variables)
rho = vk;  % Dual associated with v = w   (nonnegativity)
Dtb = pad3d(b);

switch lower(solverSettings.regularizer)
    case('tv')
        PsiTPsi = generate_laplacian(vk);
        eta_1 = vk(1:end-1,:,:);  %Duals associatd with Psi v = u (TV sparsity)
        eta_2 = vk(:,1:end-1,:);   %zeros(2*Ny,2*Nx-1,Nz);
        eta_3 = vk(:,:,1:end-1);   %zeros(2*Ny,2*Nx,Nz-1);
        PsiT = @(P1,P2,P3)cat(1,P1(1,:,:),diff(P1,1,1),-P1(end,:,:)) + ...
            cat(2,P2(:,1,:),diff(P2,1,2),-P2(:,end,:)) + ...
            cat(3,P3(:,:,1),diff(P3,1,3),-P3(:,:,end));

        % Sparsifying map
        Psi = @(x)deal(-diff(x,1,1),-diff(x,1,2),-diff(x,1,3));
        [uk1, uk2, uk3] = Psi(vk);
        Lvk1 = uk1;
        Lvk2 = uk2;
        Lvk3 = uk3;
    case('tv_native')
        PsiTPsi = generate_laplacian(vk);
        PsiT = @(P1,P2,P3,P4)cat(1,P1(1,:,:),diff(P1,1,1),-P1(end,:,:)) + ...
            cat(2,P2(:,1,:),diff(P2,1,2),-P2(:,end,:)) + ...
            cat(3,P3(:,:,1),diff(P3,1,3),-P3(:,:,end)) + ...
            solverSettings.tau_n/solverSettings.tau*P4;

        %Sparsifying with gradient and l1
        Psi = @(x)deal(-diff(x,1,1),-diff(x,1,2),-diff(x,1,3),...
            solverSettings.tau_n/solverSettings.tau*x);
        [uk1, uk2, uk3, uk4] = Psi(vk);
        Lvk1 = uk1;
        Lvk2 = uk2;
        Lvk3 = uk3;
        Lvk4 = uk4;
        eta_1 = vk(1:end-1,:,:);  %Duals associatd with Psi v = u (TV sparsity)
        eta_2 = vk(:,1:end-1,:);   %zeros(2*Ny,2*Nx-1,Nz);
        eta_3 = vk(:,:,1:end-1);   %zeros(2*Ny,2*Nx,Nz-1);
        eta_4 = vk;
        PsiTPsi = PsiTPsi + solverSettings.tau_n^2/solverSettings.tau^2;
    case('native')

        PsiTPsi = 1;
        PsiT = @(x)x;
        Psi = @(x)x;   %Identity operator for native sparsity
        uk = vk;
        Lvk = uk;
        eta = uk;
end

v_mult = 1./(mu1*HtH + mu2*PsiTPsi + mu3);  %Denominator of v update (in 3D frequency space)

DtD = pad3d(ones(Ny, Nx, 'like', b)); % Initialize DtD with same datatype as input
nu_mult = 1./(DtD + mu1);   %denominator of nu update

n = 0;  %Initialize number of steps to 0

% Store solver parameters in structure, f
% Initialize residuals with NaNs
f.dual_resid_s = zeros(1,solverSettings.maxIter)./0;
f.primal_resid_s = zeros(1,solverSettings.maxIter)./0;
f.dual_resid_u = f.dual_resid_s;
f.primal_resid_u = f.dual_resid_u;
f.dual_resid_w = f.dual_resid_s;
f.primal_resid_w = f.dual_resid_s;
f.objective = f.primal_resid_u;
f.data_fidelity = f.primal_resid_u;
f.regularizer_penalty = f.primal_resid_u;
Hvkp = vk;
tic
while n<solverSettings.maxIter
    n = n+1;
    Hvk = Hvkp;
    nukp = nu_mult.*(mu1*(xi/mu1 + Hvk) + Dtb);
    wkp = max(rho/mu3 + vk,0);
    switch lower(solverSettings.regularizer)
        case('tv')
            [uk1, uk2, uk3] = DiffuserCam_soft_3d(Lvk1+eta_1/mu2, Lvk2+eta_2/mu2, Lvk3+eta_3/mu2,solverSettings.tau/mu2);
            vkp_numerator = mu3*(wkp-rho/mu3) + ...
                mu2*PsiT(uk1 - eta_1/mu2,uk2 - eta_2/mu2, uk3 - eta_3/mu2) + ...
                mu1*Hadj(nukp - xi/mu1);
        case('tv_native')
            [uk1, uk2, uk3, uk4] = DiffuserCam_soft_3d(Lvk1 + eta_1/mu2, Lvk2 + eta_2/mu2, ...
                Lvk3 + eta_3/mu2, solverSettings.tau/mu2, Lvk4 + eta_4/mu2);
            vkp_numerator = mu3*(wkp-rho/mu3) + ...
                mu2*PsiT(uk1 - eta_1/mu2,uk2 - eta_2/mu2, uk3 - eta_3/mu2, uk4 - eta_4/mu2) + ...
                mu1*Hadj(nukp - xi/mu1);
        case('native')
            uk = DiffuserCam_soft_3d([],[],[],solverSettings.tau_n/mu2,Lvk + eta/mu2);
            vkp_numerator = mu3*(wkp-rho/mu3) + mu2*PsiT(uk - eta/mu2) + mu1*Hadj(nukp - xi/mu1);
    end

    vkp = real(ifftn(v_mult .* fftn(vkp_numerator)));

    %Update dual and parameter for Hs=v constraint
    Hvkp = Hfor(vkp);
    r_sv = Hvkp-nukp;
    xi = xi + mu1*r_sv;
    f.dual_resid_s(n) = gather(mu1*norm(vec(Hvk - Hvkp)));
    f.primal_resid_s(n) = gather(norm(vec(r_sv)));
    [mu1, mu1_update] = ADMM3D_update_param(mu1,solverSettings.resid_tol,solverSettings.mu_inc,solverSettings.mu_dec,f.primal_resid_s(n),f.dual_resid_s(n));

    % Update dual and parameter for Ls=v
    f.data_fidelity(n) = gather(.5*norm(crop3d(Hvkp)-b,'fro')^2);
    switch lower(solverSettings.regularizer)
        case('tv')
            Lvk1_ = Lvk1;
            Lvk2_ = Lvk2;
            Lvk3_ = Lvk3;
            [Lvk1, Lvk2, Lvk3] = Psi(vkp);
            r_su_1 = Lvk1 - uk1;
            r_su_2 = Lvk2 - uk2;
            r_su_3 = Lvk3 - uk3;
            eta_1 = eta_1 + mu2*r_su_1;
            eta_2 = eta_2 + mu2*r_su_2;
            eta_3 = eta_3 + mu2*r_su_3;
            f.dual_resid_u(n) = gather(mu2*sqrt(norm(vec(Lvk1_ - Lvk1))^2 + norm(vec(Lvk2_ - Lvk2))^2 + norm(vec(Lvk3_ - Lvk3))^2));
            f.primal_resid_u(n) = gather(sqrt(norm(vec(r_su_1))^2 + norm(vec(r_su_2))^2 + norm(vec(r_su_3))^2));
            f.regularizer_penalty(n) = gather(solverSettings.tau*(sum(vec(abs(Lvk1))) + sum(vec(abs(Lvk2))) + sum(vec(abs(Lvk3)))));

        case('tv_native')
            Lvk1_ = Lvk1;
            Lvk2_ = Lvk2;
            Lvk3_ = Lvk3;
            Lvk4_ = Lvk4;
            [Lvk1, Lvk2, Lvk3, Lvk4] = Psi(vkp);
            r_su_1 = Lvk1 - uk1;
            r_su_2 = Lvk2 - uk2;
            r_su_3 = Lvk3 - uk3;
            r_su_4 = Lvk4 - uk4;
            eta_1 = eta_1 + mu2*r_su_1;
            eta_2 = eta_2 + mu2*r_su_2;
            eta_3 = eta_3 + mu2*r_su_3;
            eta_4 = eta_4 + mu2*r_su_4;
            f.dual_resid_u(n) = gather(mu2*sqrt(norm(vec(Lvk1_ - Lvk1))^2 + norm(vec(Lvk2_ - Lvk2))^2 + ...
                norm(vec(Lvk3_ - Lvk3))^2 + norm(vec(Lvk4_ - Lvk4))^2));
            f.primal_resid_u(n) = gather(sqrt(norm(vec(r_su_1))^2 + norm(vec(r_su_2))^2 + ...
                norm(vec(r_su_3))^2 + norm(vec(r_su_4))^2));
           f.regularizer_penalty(n) = gather(solverSettings.tau*(sum(vec(abs(Lvk1))) +...
               sum(vec(abs(Lvk2))) + sum(vec(abs(Lvk3)))) + ...
               solverSettings.tau_n*sum(vec(abs(Lvk4))));
        case('native')
            Lvk_ = Lvk;
            Lvk = Psi(vkp);
            r_su = Lvk - uk;
            eta = eta + mu2*r_su;
            f.dual_resid_u(n) = gather(mu2*norm(vec(Lvk_ - Lvk)));
            f.primal_resid_u(n) = gather(norm(vec(r_su)));
            f.regularizer_penalty(n) = gather(solverSettings.tau_n*(sum(vec(abs(Lvk)))));
    end
    f.objective(n) = f.data_fidelity(n) + f.regularizer_penalty(n);

    [mu2, mu2_update] = ADMM3D_update_param(mu2,solverSettings.resid_tol,...
        solverSettings.mu_inc,solverSettings.mu_dec,...
        f.primal_resid_u(n),f.dual_resid_u(n));

    % Update nonnegativity dual and parameter (s=w)
    r_sw = vkp-wkp;
    rho = rho + mu3*r_sw;
    f.dual_resid_w(n) = gather(mu3*norm(vec(vk - vkp)));
    f.primal_resid_w(n) = gather(norm(vec(r_sw)));
    [mu3, mu3_update] = ADMM3D_update_param(mu3,solverSettings.resid_tol,solverSettings.mu_inc,solverSettings.mu_dec,f.primal_resid_w(n),f.dual_resid_w(n));

    %Update filters
    if mu1_update || mu2_update || mu3_update
        %fprintf('Mu updates: %i \t %i \t %i\n',mu1_update, mu2_update, mu3_update);
        mu_update = 1;
    else
        mu_update = 0;
    end
    if mu_update
        v_mult = 1./(mu1*HtH + mu2*PsiTPsi + mu3);  %This is the frequency space division fo S update
        nu_mult = 1./(DtD + mu1);
    end

    vk = vkp;

    if mod(n,solverSettings.save_every) == 0
        fprintf('saving state %i...\n',n)
        out_file = save_state(solverSettings,n);
        save(out_file,solverSettings.save_vars{:});
        fprintf('done saving\n')
    end

    if mod(n,solverSettings.print_interval) == 0
        t_iter = toc/solverSettings.print_interval;
         fprintf('iter: %i \t t: %.2g \t cost: %.2g \t data_fidelity: %.2g \t norm: %.2g \t Primal v: %.2g \t Dual v: %.2g \t Primal u: %.2g \t Dual u: %.2g \t Primal w: %.2g \t Dual w: %.2g \t mu1: %.2g \t mu2: %.2g \t mu3: %.2g \n',...
            n,t_iter,f.objective(n),f.data_fidelity(n),f.regularizer_penalty(n),f.primal_resid_s(n), f.dual_resid_s(n),f.primal_resid_u(n), f.dual_resid_u(n),f.primal_resid_w(n), f.dual_resid_w(n),mu1,mu2,mu3)
            %disp([n,f.objective(n),f.data_fidelity(n),f.regularizer_penalty(n),f.primal_resid_s(n), f.dual_resid_s(n),f.primal_resid_u(n), f.dual_resid_u(n),f.primal_resid_w(n), f.dual_resid_w(n),mu1,mu2,mu3])
        tic;
    end
    if mod(n,solverSettings.disp_figs) == 0
        draw_figures(vk,solverSettings)
    end
end
end

% Private function to display figures
function draw_figures(xk, solverSettings)
set(0,'CurrentFigure',solverSettings.fighandle)
if numel(size(xk))==2
    imagesc(solverSettings.display_func(xk))
    axis image
    colorbar
    colormap(solverSettings.color_map);

elseif numel(size(xk))==3
    xk = solverSettings.disp_crop(xk);
    subplot(1,3,1)

    im1 = squeeze(sum(xk,3));
    imagesc(solverSettings.display_func(im1));
    hold on
    axis image
    colormap (solverSettings.cmap)
    %colorbar
    caxis([0 prctile(im1(:),solverSettings.disp_percentile)])
    set(gca,'fontSize',6)
    axis off
    title('XY')
    hold off

    subplot(1,3,2)
    im2 = squeeze(max(xk,[],1));
    imagesc(im2);
    hold on
    %axis image
    colormap (solverSettings.cmap)
    %colorbar
    set(gca,'fontSize',8)
    caxis([0 prctile(im2(:),solverSettings.disp_percentile)])
    title('XZ')
    axis off
    hold off

    subplot(1,3,3)
    im3 = squeeze(max(xk,[],2));
    imagesc(solverSettings.disp_func(im3));
    hold on
    %axis image
    colormap (solverSettings.cmap)
    title('YZ')
    colorbar
    set(gca,'fontSize',8)
    caxis([0 prctile(im3(:),solverSettings.disp_percentile)]);
    axis off
    hold off

end

drawnow
end

function PsiTPsi = generate_laplacian(lapl)  %Takes in an array and makes laplacian on same grid (fft shifted)
    %lapl = zeros(2*Ny,2*Nx,Nz);    %Compute laplacian in closed form. This is the kernal to compute Psi'Psi
    lapl(1) = 6;
    lapl(1,2,1) = -1;
    lapl(2,1,1) = -1;
    lapl(1,1,2) = -1;
    lapl(1,end,1) = -1;
    lapl(end,1,1) = -1;
    lapl(1,1,end) = -1;
    PsiTPsi = abs(fftn(lapl));   %Compute power spectrum of laplacian
end

function [mu_out, mu_update] = ADMM3D_update_param(mu,resid_tol,mu_inc,mu_dec,r,s)
    if r > resid_tol*s
        mu_out = mu*mu_inc;
        mu_update = 1;
    elseif r*resid_tol < s
        mu_out = mu/mu_dec;
        mu_update = -1;
    else
        mu_out = mu;
        mu_update = 0;
    end
end


function [theta,HH,C,P,hrf,fit,e,param] = Anneal_Logit(theta0,t,tc,Run)
%
% [theta,HH,C,P] = Anneal_Logit(theta0,t,tc,Run)
%
% Estimate inverse logit (IL) HRF model using Simulated Annealing
% Creates fitted curve - 3 logistic functions to be summed together - from parameter estimates
%
% INPUT: theta0, t, tc, Run
% Run = stick function
% tc = time course
% t = vector of time points
% theta0 = initial value for the parameter vector
%
% By Martin Lindquist and Tor Wager
% Edited 12/12/06
%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Initial values

iter = 15000;                               % Number of iterations
theta = theta0;                             % Set initial value for the parameter vector
h0 = cost(theta0,t,tc,Run);                 % Calculate cost of initial estimate
LB = [0.05, 1, 0, 0.05, 5, 0, 10];      % Lower bounds for parameters
UB = [10, 15, 5, 10, 15, 5, 30];           % Upper bounds for parameters

%
% These values may need tweaking depending on the individual situation.
%

r1= 0.001;                                 % A parameters
r1b= 0.001;                                % A parameters
r2 = 0.05;                                 % T parameters
r3 = 0.001;                                % delta parameters

t1 = [1 4];
t1b = [6];
t2 = [2 5 7];
t3 = [3];

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

u = zeros(1,7);

HH = zeros(1+iter,7);               % Keep track of theta_i
HH(1,:) = theta0;
P = zeros(1+iter,1);
C = zeros(1+iter,1);                % Keep track of the cost function
C(1) = h0;

cnt = 0;
for i=1:iter,

    T = 100/log(1+i);    %Temperature function (may require tweaking)
    th = zeros(1,7);
    ind = 0;

    % Choose a new candidate solution theta_{i+1}, based on a random perturbation of the current solution of theta_{i}.
    % Check new parameters are within accepted bounds
    while ( (sum((LB-th)>0) + sum((th-UB)>0)) > 0),

        % Perturb solution

        u(t1) = normrnd(0,r1,1,2);
        u(t1b) = normrnd(0,r1b,1,1);
        u(t2) = normrnd(0,r2,1,3);
        u(t3) = normrnd(0,r3,1,1);

        % Update solution
        th = theta + u;
        ind = ind + 1;

        if(ind > 500),
            warning('stuck!');
            return;
        end;
    end;

    h = cost(th,t,tc,Run);
    C(i+1) = h;
    delta = h - h0;

    % Determine whether to update the parameter vector.
    if (unifrnd(0,1) < min(exp(-delta/T),1)),
        theta = th;
        h0=h;
        cnt = cnt+1;
    end;

    HH(i+1,:) = theta;
    P(i+1) = min(exp(-delta/T),1);

end;

%cnt/iter

[a,b] = min(C);
theta = HH(b,:);
%h

% Additional outputs
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Get HRF for final model
if nargout > 4
    hrf = Get_Logit(theta(1:7),t);                   % Calculate HRF estimate (fit, given theta)
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Convolve HRF and stick function
if nargout > 5
    len = length(Run);
    fit = conv(Run, hrf);
    fit = fit(1:len);
    e = tc - fit;
end

if nargout > 7
    [param] = get_parameters_logit(hrf,t,theta);
end

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

return


function [affineMat,pixDim,planC] = getScanAffineMat(planC,scanNum)

if ischar(planC)
    planC = loadPlanC(planC);
end

indexS = planC{end};

if ~exist('scanNum','var') || isempty(scanNum)
    scanNum = 1;
end

iHat = [1; 0; 0; 0];
jHat = [0; 1; 0; 0];
kHat = [0; 0; 1; 0];

N = numel(planC{indexS.scan}(scanNum).scanInfo);

try
    iop = planC{indexS.scan}(scanNum).scanInfo(1).imageOrientationPatient;
    if isempty(iop)
        disp('defaulting to HFS orientation');
        iop = [1 0 0 0 1 0]';
    end
    ipp = (planC{indexS.scan}(scanNum).scanInfo(end).imagePositionPatient - planC{indexS.scan}(scanNum).scanInfo(1).imagePositionPatient)/(N-1);
    if isempty(ipp)
            ipp = [0 0 -planC{indexS.scan}(scanNum).scanInfo(1).sliceThickness*10]';
    end
catch err
    disp(err);
    disp('defaulting to HFS orientation');
    iop = [1 0 0 0 1 0]';
    ipp = [0 0 -planC{indexS.scan}(scanNum).scanInfo(1).sliceThickness*10]';
end
iop = iop(:);
ipp = ipp(:);

pixsp = 10*[planC{indexS.scan}(scanNum).scanInfo(1).grid1Units planC{indexS.scan}(scanNum).scanInfo(1).grid2Units];
sliceThickness = planC{indexS.scan}(scanNum).scanInfo(1).sliceThickness * 10;
% voxel_size = [pixsp sliceThickness];
planeMat = [pixsp(2)*iop(4:end) pixsp(1)*iop(1:3)]; %.*[-1 -1;-1 -1; 1 1];

%planeMat = [pixsp(2)*iop(1:3) pixsp(1)*iop(4:end)].*[-1 -1;-1 -1; 1 1];
% [~,orientationStr,~] = returnViewerAxisLabels(planC,scanNum);
% if strcmpi('FFP',orientationStr) || strcmpi('FFS',orientationStr)
%     originLPS = planC{indexS.scan}(scanNum).scanInfo(1).imagePositionPatient;
% else
    originLPS = planC{indexS.scan}(scanNum).scanInfo(end).imagePositionPatient;
    if isempty(originLPS)
        originLPS = [0; 0; 0];
    end
% end
originLPS = originLPS(:);
rawAffineMat = [planeMat ipp originLPS; 0 0 0 1];

rawPixDim = [pixsp(2) pixsp(1) sliceThickness];
[~,xCol] = max(abs(rawAffineMat * iHat)); %[1; 0; 0; 0]))
[~,yCol] = max(abs(rawAffineMat * jHat)); %[0; 1; 0; 0]))
[~,zCol] = max(abs(rawAffineMat * kHat)); %[0; 0; 1; 0]))

pixDim = [rawPixDim(xCol) rawPixDim(yCol) rawPixDim(zCol)];

% LIA -> RAS
affIdent = eye(4);
affIdent(xCol,xCol) = -1;
affIdent(yCol,yCol) = -1;
% ##affIdent

affineMat = rawAffineMat * affIdent;

zCorrect = [9 10 3 7];
affineMat(zCorrect) = - affineMat(zCorrect);


function [y, Sigma_y, beta] = ml_gmr(Priors, Mu, Sigma, x, in, out)
%
% This function performs Gaussian Mixture Regression (GMR), using the
% parameters of a Gaussian Mixture Model (GMM). Given partial input data,
% the algorithm computes the expected distribution for the resulting
% dimensions. By providing temporal values as inputs, it thus outputs a
% smooth generalized version of the data encoded in GMM, and associated
% constraints expressed by covariance matrices.
%
% Inputs -----------------------------------------------------------------
%   o Priors:  1 x K array representing the prior probabilities of the K GMM
%              components.
%   o Mu:      D x K array representing the centers of the K GMM components.
%   o Sigma:   D x D x K array representing the covariance matrices of the
%              K GMM components.
%   o x:       P x N array representing N datapoints of P dimensions.
%   o in:      1 x P array representing the dimensions to consider as
%              inputs.
%   o out:     1 x Q array representing the dimensions to consider as
%              outputs (D=P+Q).
% Outputs ----------------------------------------------------------------
%   o y:       Q x N array representing the retrieved N datapoints of
%              Q dimensions, i.e. expected means.
%   o Sigma_y: Q x Q x N array representing the N expected covariance
%              matrices retrieved.
%
% Copyright (c) 2006 Sylvain Calinon, LASA Lab, EPFL, CH-1015 Lausanne,
%               Switzerland, http://lasa.epfl.ch
%
% The program is free for non-commercial academic use.
% Please contact the authors if you are interested in using the
% software for commercial purposes. The software must not be modified or
% distributed without prior permission of the authors.
% Please acknowledge the authors in any academic publications that have
% made use of this code or part of it. Please use this BibTex reference:
%
% @article{Calinon06SMC,
%   title="On Learning, Representing and Generalizing a Task in a Humanoid
%     Robot",
%   author="S. Calinon and F. Guenter and A. Billard",
%   journal="IEEE Transactions on Systems, Man and Cybernetics, Part B.
%     Special issue on robot learning by observation, demonstration and
%     imitation",
%   year="2006",
%   volume="36",
%   number="5"
% }

nbData = size(x,2);
nbVar = size(Mu,1);
nbStates = size(Sigma,3);

%% Fast matrix computation (see the commented code for a version involving
%% one-by-one computation, which is easier to understand).
%%
%% Compute the influence of each GMM component, given input x
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:nbStates
  Pxi(:,i) = Priors(i).*ml_gaussPDF(x, Mu(in,i), Sigma(in,in,i));
end
beta = Pxi./repmat(sum(Pxi,2)+realmin,1,nbStates);

% ind = find(sum(beta,2) == 0);
% if ~isempty(ind)
%     for i=1:nbStates
%         tmp = x(:,ind)' - repmat(Mu(in,i)',length(ind),1);
%         score(i,:) = Priors(i)*(sum((tmp/Sigma(in,in,i)).*tmp, 2));
%     end
%     [i i]=min(score);
%     beta=beta';
%     beta(nbStates.*(ind-1)+i')=1;
%     beta=beta';
% end
%% Compute expected means y, given input x
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for j=1:nbStates
  y_tmp(:,:,j) = repmat(Mu(out,j),1,nbData) + Sigma(out,in,j)/(Sigma(in,in,j)) * (x-repmat(Mu(in,j),1,nbData));
end
beta_tmp = reshape(beta,[1 size(beta)]);
y_tmp2 = repmat(beta_tmp,[length(out) 1 1]) .* y_tmp;
y = sum(y_tmp2,3);
%% Compute expected covariance matrices Sigma_y, given input x
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% This seems to be the computation of the marginal covariance matrix SigmaY
% if nargout > 1
%     for j=1:nbStates
%         Sigma_y_tmp(:,:,1,j) = Sigma(out,out,j) - (Sigma(out,in,j)/(Sigma(in,in,j))*Sigma(in,out,j));
%     end
%     beta_tmp = reshape(beta,[1 1 size(beta)]);
%     Sigma_y_tmp2 = repmat(beta_tmp.*beta_tmp, [length(out) length(out) 1 1]) .* repmat(Sigma_y_tmp,[1 1 nbData 1]);
%     Sigma_y = sum(Sigma_y_tmp2,4);
% end

% %%%%%% Compute expected output distribution, given input x^i %%%%%%
if nargout > 1
    M = nbData; K = nbStates;
    Sigma_y = zeros(length(out), length(out), M);
    for i=1:M
        %%%% Eq.11: Compute expected covariance matrices, given input x^i %%%%
        for k=1:K
            % Full conditional variance equations (Nadia's way)
            mu_k_tmp     = Mu(out,k) + Sigma(out,in,k)*inv(Sigma(in,in,k)) * (x(:,i)-Mu(in,k));
            Sigmak_y_tmp = Sigma(out,out,k) - (Sigma(out,in,k)*inv(Sigma(in,in,k))*Sigma(in,out,k));
            Sigma_y(:,:,i) = Sigma_y(:,:,i) + beta(i,k) .* (mu_k_tmp'*mu_k_tmp + Sigmak_y_tmp);
        end

        % Full conditional variance equations (Nadia's way)
        Sigma_y(:,:,i) = Sigma_y(:,:,i) - (y(:,i)'*y(:,i));
    end
end

% %% Slow one-by-one computation (better suited to understand the algorithm)
% %%
% %% Compute the influence of each GMM component, given input x
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% for i=1:nbStates
%   Pxi(:,i) = gaussPDF(x, Mu(in,i), Sigma(in,in,i));
% end
% beta = (Pxi./repmat(sum(Pxi,2)+realmin,1,nbStates))';
% %% Compute expected output distribution, given input x
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% y = zeros(length(out), nbData);
% Sigma_y = zeros(length(out), length(out), nbData);
% for i=1:nbData
%   % Compute expected means y, given input x
%   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   for j=1:nbStates
%     yj_tmp = Mu(out,j) + Sigma(out,in,j)*inv(Sigma(in,in,j)) * (x(:,i)-Mu(in,j));
%     y(:,i) = y(:,i) + beta(j,i).*yj_tmp;
%   end
%   % Compute expected covariance matrices Sigma_y, given input x
%   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   for j=1:nbStates
%     Sigmaj_y_tmp = Sigma(out,out,j) - (Sigma(out,in,j)*inv(Sigma(in,in,j))*Sigma(in,out,j));
%     Sigma_y(:,:,i) = Sigma_y(:,:,i) + beta(j,i)^2.* Sigmaj_y_tmp;
%   end
% end


function [F,sE,sC] = spm_log_evidence(varargin)
% Return the log-evidence of a reduced model (under Laplace approximation)
% FORMAT [F,sE,sC] = spm_log_evidence(qE,qC,pE,pC,rE,rC)
% FORMAT [F,sE,sC] = spm_log_evidence(qE,qC,pE,pC,priorfun,varargin)
% FORMAT [F,sE,sC] = spm_log_evidence(qE,qC,pE,pC)
%
% qE,qC    - posterior expectation and covariance of full model
% pE,pC    - prior expectation and covariance of full model
% rE,rC    - prior expectation and covariance of reduced model
% or
% priorfun - inline function that returns prior moments
%            {rE rC} = priorfun(varargin{:})
%
% or (if omitted) rE = 0 and rC = 0;
%
% F        - reduced log-evidence: ln p(y|reduced model) - ln p(y|full model)
% [sE,sC]  - posterior expectation and covariance of reduced model
%
%--------------------------------------------------------------------------
% This routine assumes the reduced model is nested within a full model and
% that the posteriors (and priors) are Gaussian. Nested here means that the
% prior precision of the reduced model, minus the prior precision of the
% full model is positive definite. We additionally assume that the prior
% means are unchanged. The two input argument formats are for use with
% spm_argmax.
%
% See also: spm_log_evidence_reduce
%__________________________________________________________________________
% Copyright (C) 2005 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_log_evidence.m 6856 2016-08-10 17:55:05Z karl $

% Compute reduced log-evidence
%==========================================================================

% check to see if priors are specified by a function
%--------------------------------------------------------------------------
qE = varargin{1};
qC = varargin{2};
pE = varargin{3};
pC = varargin{4};

try
    priors = varargin{5}(varargin{6:end});
    rE     = priors{1};
    rC     = priors{2};
catch
    try
        rE = varargin{5};
        rC = varargin{6};
    catch
        n  = size(qC,1);
        rE = sparse(n,1);
        rC = sparse(n,n);
    end
end

% check to see if prior oovaiances are structures
%--------------------------------------------------------------------------
if isstruct(pC) || iscell(pC), pC = diag(spm_vec(pC)); end
if isstruct(qC) || iscell(qC), qC = diag(spm_vec(qC)); end
if isstruct(rC) || iscell(rC), rC = diag(spm_vec(rC)); end

% reduced subspace
%--------------------------------------------------------------------------
qE  = spm_vec(qE);
pE  = spm_vec(pE);
rE  = spm_vec(rE);

if nargout < 2
    dE  = pE - rE;
    dC  = pC - rC;
    k   = find(dE | any(dC,2));
    if ~isempty(k)
        qE  = qE(k);
        pE  = pE(k);
        rE  = rE(k);
        qC  = qC(k,k);
        pC  = pC(k,k);
        rC  = rC(k,k);
    end
end

% fix tolerance for matrix inversions
%--------------------------------------------------------------------------
TOL   = exp(-16);

% remove fixed parameters under full model
%--------------------------------------------------------------------------
i     = find(diag(pC) > TOL);

% preliminaries
%--------------------------------------------------------------------------
qP    = spm_inv(qC(i,i),TOL);
pP    = spm_inv(pC(i,i),TOL);
rP    = spm_inv(rC(i,i),TOL);
sP    = qP + rP - pP;
sC    = spm_inv(sP,TOL);
pC    = spm_inv(pP,TOL);
sE    = qP*qE(i) + rP*rE(i) - pP*pE(i);

% log-evidence
%--------------------------------------------------------------------------
F     = spm_logdet(rP*qP*sC*pC) ...
      - (qE(i)'*qP*qE(i) + rE(i)'*rP*rE(i) - pE(i)'*pP*pE(i) - sE'*sC*sE);
F     = F/2;

% restore full conditional density
%--------------------------------------------------------------------------
if nargout > 1
    rE(i)   = sC*sE;
    rC(i,i) = sC;
    sE      = spm_unvec(rE,varargin{1});
    sC      = rC;
end


%        NOTE        NOTE              NOTE

% NOTE: THIS CODE IS the work of 'Pascal Getreuer ' which converts all the
% possible spaces to various different spaces
%Source : Mathworks

function varargout = colorspace(Conversion,varargin)
%%% Input parsing %%%
if nargin < 2, error('Not enough input arguments.'); end
[SrcSpace,DestSpace] = parse(Conversion);

if nargin == 2
   Image = varargin{1};
elseif nargin >= 3
   Image = cat(3,varargin{:});
else
   error('Invalid number of input arguments.');
end

FlipDims = (size(Image,3) == 1);

if FlipDims, Image = permute(Image,[1,3,2]); end
if ~isa(Image,'double'), Image = double(Image)/255; end
if size(Image,3) ~= 3, error('Invalid input size.'); end

SrcT = gettransform(SrcSpace);
DestT = gettransform(DestSpace);

if ~ischar(SrcT) && ~ischar(DestT)
   % Both source and destination transforms are affine, so they
   % can be composed into one affine operation
   T = [DestT(:,1:3)*SrcT(:,1:3),DestT(:,1:3)*SrcT(:,4)+DestT(:,4)];
   Temp = zeros(size(Image));
   Temp(:,:,1) = T(1)*Image(:,:,1) + T(4)*Image(:,:,2) + T(7)*Image(:,:,3) + T(10);
   Temp(:,:,2) = T(2)*Image(:,:,1) + T(5)*Image(:,:,2) + T(8)*Image(:,:,3) + T(11);
   Temp(:,:,3) = T(3)*Image(:,:,1) + T(6)*Image(:,:,2) + T(9)*Image(:,:,3) + T(12);
   Image = Temp;
elseif ~ischar(DestT)
   Image = rgb(Image,SrcSpace);
   Temp = zeros(size(Image));
   Temp(:,:,1) = DestT(1)*Image(:,:,1) + DestT(4)*Image(:,:,2) + DestT(7)*Image(:,:,3) + DestT(10);
   Temp(:,:,2) = DestT(2)*Image(:,:,1) + DestT(5)*Image(:,:,2) + DestT(8)*Image(:,:,3) + DestT(11);
   Temp(:,:,3) = DestT(3)*Image(:,:,1) + DestT(6)*Image(:,:,2) + DestT(9)*Image(:,:,3) + DestT(12);
   Image = Temp;
else
   Image = feval(DestT,Image,SrcSpace);
end

%%% Output format %%%
if nargout > 1
   varargout = {Image(:,:,1),Image(:,:,2),Image(:,:,3)};
else
   if FlipDims, Image = permute(Image,[1,3,2]); end
   varargout = {Image};
end

return;

function [SrcSpace,DestSpace] = parse(Str)
% Parse conversion argument

if ischar(Str)
   Str = lower(strrep(strrep(Str,'-',''),'=',''));
   k = find(Str == '>');

   if length(k) == 1         % Interpret the form 'src->dest'
      SrcSpace = Str(1:k-1);
      DestSpace = Str(k+1:end);
   else
      k = find(Str == '<');

      if length(k) == 1      % Interpret the form 'dest<-src'
         DestSpace = Str(1:k-1);
         SrcSpace = Str(k+1:end);
      else
         error(['Invalid conversion, ''',Str,'''.']);
      end
   end

   SrcSpace = alias(SrcSpace);
   DestSpace = alias(DestSpace);
else
   SrcSpace = 1;             % No source pre-transform
   DestSpace = Conversion;
   if any(size(Conversion) ~= 3), error('Transformation matrix must be 3x3.'); end
end
return;

function Space = alias(Space)
Space = strrep(strrep(Space,'cie',''),' ','');

if isempty(Space)
   Space = 'rgb';
end

switch Space
case {'ycbcr','ycc'}
   Space = 'ycbcr';
case {'hsv','hsb'}
   Space = 'hsv';
case {'hsl','hsi','hls'}
   Space = 'hsl';
case {'rgb','yuv','yiq','ydbdr','ycbcr','jpegycbcr','xyz','lab','luv','lch'}
   return;
end
return;

function T = gettransform(Space)
% Get a colorspace transform: either a matrix describing an affine transform,
% or a string referring to a conversion subroutine
switch Space
case 'ypbpr'
   T = [0.299,0.587,0.114,0;-0.1687367,-0.331264,0.5,0;0.5,-0.418688,-0.081312,0];
case 'yuv'
   % sRGB to NTSC/PAL YUV
   % Wikipedia: http://en.wikipedia.org/wiki/YUV
   T = [0.299,0.587,0.114,0;-0.147,-0.289,0.436,0;0.615,-0.515,-0.100,0];
case 'ydbdr'
   % sRGB to SECAM YDbDr
   % Wikipedia: http://en.wikipedia.org/wiki/YDbDr
   T = [0.299,0.587,0.114,0;-0.450,-0.883,1.333,0;-1.333,1.116,0.217,0];
case 'yiq'
   % sRGB in [0,1] to NTSC YIQ in [0,1];[-0.595716,0.595716];[-0.522591,0.522591];
   % Wikipedia: http://en.wikipedia.org/wiki/YIQ
   T = [0.299,0.587,0.114,0;0.595716,-0.274453,-0.321263,0;0.211456,-0.522591,0.311135,0];
case 'ycbcr'
   % sRGB (range [0,1]) to ITU-R BRT.601 (CCIR 601) Y'CbCr
   % Wikipedia: http://en.wikipedia.org/wiki/YCbCr
   % Poynton, Equation 3, scaling of R'G'B to Y'PbPr conversion
   T = [65.481,128.553,24.966,16;-37.797,-74.203,112.0,128;112.0,-93.786,-18.214,128];
case 'jpegycbcr'
   % Wikipedia: http://en.wikipedia.org/wiki/YCbCr
   T = [0.299,0.587,0.114,0;-0.168736,-0.331264,0.5,0.5;0.5,-0.418688,-0.081312,0.5]*255;
case {'rgb','xyz','hsv','hsl','lab','luv','lch','cat02lms'}
   T = Space;
otherwise
   error(['Unknown color space, ''',Space,'''.']);
end
return;

function Image = rgb(Image,SrcSpace)
% Convert to sRGB from 'SrcSpace'
switch SrcSpace
case 'rgb'
   return;
case 'hsv'
   % Convert HSV to sRGB
   Image = huetorgb((1 - Image(:,:,2)).*Image(:,:,3),Image(:,:,3),Image(:,:,1));
case 'hsl'
   % Convert HSL to sRGB
   L = Image(:,:,3);
   Delta = Image(:,:,2).*min(L,1-L);
   Image = huetorgb(L-Delta,L+Delta,Image(:,:,1));
case {'xyz','lab','luv','lch','cat02lms'}
   % Convert to CIE XYZ
   Image = xyz(Image,SrcSpace);
   % Convert XYZ to RGB
   T = [3.2406, -1.5372, -0.4986; -0.9689, 1.8758, 0.0415; 0.0557, -0.2040, 1.057];
   R = T(1)*Image(:,:,1) + T(4)*Image(:,:,2) + T(7)*Image(:,:,3);  % R
   G = T(2)*Image(:,:,1) + T(5)*Image(:,:,2) + T(8)*Image(:,:,3);  % G
   B = T(3)*Image(:,:,1) + T(6)*Image(:,:,2) + T(9)*Image(:,:,3);  % B
   % Desaturate and rescale to constrain resulting RGB values to [0,1]
   AddWhite = -min(min(min(R,G),B),0);
   R = R + AddWhite;
   G = G + AddWhite;
   B = B + AddWhite;
   % Apply gamma correction to convert linear RGB to sRGB
   Image(:,:,1) = gammacorrection(R);  % R'
   Image(:,:,2) = gammacorrection(G);  % G'
   Image(:,:,3) = gammacorrection(B);  % B'
otherwise  % Conversion is through an affine transform
   T = gettransform(SrcSpace);
   temp = inv(T(:,1:3));
   T = [temp,-temp*T(:,4)];
   R = T(1)*Image(:,:,1) + T(4)*Image(:,:,2) + T(7)*Image(:,:,3) + T(10);
   G = T(2)*Image(:,:,1) + T(5)*Image(:,:,2) + T(8)*Image(:,:,3) + T(11);
   B = T(3)*Image(:,:,1) + T(6)*Image(:,:,2) + T(9)*Image(:,:,3) + T(12);
   Image(:,:,1) = R;
   Image(:,:,2) = G;
   Image(:,:,3) = B;
end

% Clip to [0,1]
Image = min(max(Image,0),1);
return;

function Image = xyz(Image,SrcSpace)
% Convert to CIE XYZ from 'SrcSpace'
WhitePoint = [0.950456,1,1.088754];

switch SrcSpace
case 'xyz'
   return;
case 'luv'
   % Convert CIE L*uv to XYZ
   WhitePointU = (4*WhitePoint(1))./(WhitePoint(1) + 15*WhitePoint(2) + 3*WhitePoint(3));
   WhitePointV = (9*WhitePoint(2))./(WhitePoint(1) + 15*WhitePoint(2) + 3*WhitePoint(3));
   L = Image(:,:,1);
   Y = (L + 16)/116;
   Y = invf(Y)*WhitePoint(2);
   U = Image(:,:,2)./(13*L + 1e-6*(L==0)) + WhitePointU;
   V = Image(:,:,3)./(13*L + 1e-6*(L==0)) + WhitePointV;
   Image(:,:,1) = -(9*Y.*U)./((U-4).*V - U.*V);                  % X
   Image(:,:,2) = Y;                                             % Y
   Image(:,:,3) = (9*Y - (15*V.*Y) - (V.*Image(:,:,1)))./(3*V);  % Z
case {'lab','lch'}
   Image = lab(Image,SrcSpace);
   % Convert CIE L*ab to XYZ
   fY = (Image(:,:,1) + 16)/116;
   fX = fY + Image(:,:,2)/500;
   fZ = fY - Image(:,:,3)/200;
   Image(:,:,1) = WhitePoint(1)*invf(fX);  % X
   Image(:,:,2) = WhitePoint(2)*invf(fY);  % Y
   Image(:,:,3) = WhitePoint(3)*invf(fZ);  % Z
case 'cat02lms'
    % Convert CAT02 LMS to XYZ
   T = inv([0.7328, 0.4296, -0.1624;-0.7036, 1.6975, 0.0061; 0.0030, 0.0136, 0.9834]);
   L = Image(:,:,1);
   M = Image(:,:,2);
   S = Image(:,:,3);
   Image(:,:,1) = T(1)*L + T(4)*M + T(7)*S;  % X
   Image(:,:,2) = T(2)*L + T(5)*M + T(8)*S;  % Y
   Image(:,:,3) = T(3)*L + T(6)*M + T(9)*S;  % Z
otherwise   % Convert from some gamma-corrected space
   % Convert to sRGB
   Image = rgb(Image,SrcSpace);
   % Undo gamma correction
   R = invgammacorrection(Image(:,:,1));
   G = invgammacorrection(Image(:,:,2));
   B = invgammacorrection(Image(:,:,3));
   % Convert RGB to XYZ
   T = inv([3.2406, -1.5372, -0.4986; -0.9689, 1.8758, 0.0415; 0.0557, -0.2040, 1.057]);
   Image(:,:,1) = T(1)*R + T(4)*G + T(7)*B;  % X
   Image(:,:,2) = T(2)*R + T(5)*G + T(8)*B;  % Y
   Image(:,:,3) = T(3)*R + T(6)*G + T(9)*B;  % Z
end
return;

function Image = hsv(Image,SrcSpace)
% Convert to HSV
Image = rgb(Image,SrcSpace);
V = max(Image,[],3);
S = (V - min(Image,[],3))./(V + (V == 0));
Image(:,:,1) = rgbtohue(Image);
Image(:,:,2) = S;
Image(:,:,3) = V;
return;

function Image = hsl(Image,SrcSpace)
% Convert to HSL
switch SrcSpace
case 'hsv'
   % Convert HSV to HSL
   MaxVal = Image(:,:,3);
   MinVal = (1 - Image(:,:,2)).*MaxVal;
   L = 0.5*(MaxVal + MinVal);
   temp = min(L,1-L);
   Image(:,:,2) = 0.5*(MaxVal - MinVal)./(temp + (temp == 0));
   Image(:,:,3) = L;
otherwise
   Image = rgb(Image,SrcSpace);  % Convert to sRGB
   % Convert sRGB to HSL
   MinVal = min(Image,[],3);
   MaxVal = max(Image,[],3);
   L = 0.5*(MaxVal + MinVal);
   temp = min(L,1-L);
   S = 0.5*(MaxVal - MinVal)./(temp + (temp == 0));
   Image(:,:,1) = rgbtohue(Image);
   Image(:,:,2) = S;
   Image(:,:,3) = L;
end
return;

function Image = lab(Image,SrcSpace)
% Convert to CIE L*a*b* (CIELAB)
WhitePoint = [0.950456,1,1.088754];

switch SrcSpace
case 'lab'
   return;
case 'lch'
   % Convert CIE L*CH to CIE L*ab
   C = Image(:,:,2);
   Image(:,:,2) = cos(Image(:,:,3)*pi/180).*C;  % a*
   Image(:,:,3) = sin(Image(:,:,3)*pi/180).*C;  % b*
otherwise
   Image = xyz(Image,SrcSpace);  % Convert to XYZ
   % Convert XYZ to CIE L*a*b*
   X = Image(:,:,1)/WhitePoint(1);
   Y = Image(:,:,2)/WhitePoint(2);
   Z = Image(:,:,3)/WhitePoint(3);
   fX = f(X);
   fY = f(Y);
   fZ = f(Z);
   Image(:,:,1) = 116*fY - 16;    % L*
   Image(:,:,2) = 500*(fX - fY);  % a*
   Image(:,:,3) = 200*(fY - fZ);  % b*
end
return;

function Image = luv(Image,SrcSpace)
% Convert to CIE L*u*v* (CIELUV)
WhitePoint = [0.950456,1,1.088754];
WhitePointU = (4*WhitePoint(1))./(WhitePoint(1) + 15*WhitePoint(2) + 3*WhitePoint(3));
WhitePointV = (9*WhitePoint(2))./(WhitePoint(1) + 15*WhitePoint(2) + 3*WhitePoint(3));

Image = xyz(Image,SrcSpace); % Convert to XYZ
Denom = Image(:,:,1) + 15*Image(:,:,2) + 3*Image(:,:,3);
U = (4*Image(:,:,1))./(Denom + (Denom == 0));
V = (9*Image(:,:,2))./(Denom + (Denom == 0));
Y = Image(:,:,2)/WhitePoint(2);
L = 116*f(Y) - 16;
Image(:,:,1) = L;                        % L*
Image(:,:,2) = 13*L.*(U - WhitePointU);  % u*
Image(:,:,3) = 13*L.*(V - WhitePointV);  % v*
return;

function Image = lch(Image,SrcSpace)
% Convert to CIE L*ch
Image = lab(Image,SrcSpace);  % Convert to CIE L*ab
H = atan2(Image(:,:,3),Image(:,:,2));
H = H*180/pi + 360*(H < 0);
Image(:,:,2) = sqrt(Image(:,:,2).^2 + Image(:,:,3).^2);  % C
Image(:,:,3) = H;                                        % H
return;

function Image = cat02lms(Image,SrcSpace)
% Convert to CAT02 LMS
Image = xyz(Image,SrcSpace);
T = [0.7328, 0.4296, -0.1624;-0.7036, 1.6975, 0.0061; 0.0030, 0.0136, 0.9834];
X = Image(:,:,1);
Y = Image(:,:,2);
Z = Image(:,:,3);
Image(:,:,1) = T(1)*X + T(4)*Y + T(7)*Z;  % L
Image(:,:,2) = T(2)*X + T(5)*Y + T(8)*Z;  % M
Image(:,:,3) = T(3)*X + T(6)*Y + T(9)*Z;  % S
return;

function Image = huetorgb(m0,m2,H)
% Convert HSV or HSL hue to RGB
N = size(H);
H = min(max(H(:),0),360)/60;
m0 = m0(:);
m2 = m2(:);
F = H - round(H/2)*2;
M = [m0, m0 + (m2-m0).*abs(F), m2];
Num = length(m0);
j = [2 1 0;1 2 0;0 2 1;0 1 2;1 0 2;2 0 1;2 1 0]*Num;
k = floor(H) + 1;
Image = reshape([M(j(k,1)+(1:Num).'),M(j(k,2)+(1:Num).'),M(j(k,3)+(1:Num).')],[N,3]);
return;

function H = rgbtohue(Image)
% Convert RGB to HSV or HSL hue
[M,i] = sort(Image,3);
i = i(:,:,3);
Delta = M(:,:,3) - M(:,:,1);
Delta = Delta + (Delta == 0);
R = Image(:,:,1);
G = Image(:,:,2);
B = Image(:,:,3);
H = zeros(size(R));
k = (i == 1);
H(k) = (G(k) - B(k))./Delta(k);
k = (i == 2);
H(k) = 2 + (B(k) - R(k))./Delta(k);
k = (i == 3);
H(k) = 4 + (R(k) - G(k))./Delta(k);
H = 60*H + 360*(H < 0);
H(Delta == 0) = nan;
return;

function Rp = gammacorrection(R)
Rp = zeros(size(R));
i = (R <= 0.0031306684425005883);
Rp(i) = 12.92*R(i);
Rp(~i) = real(1.055*R(~i).^0.416666666666666667 - 0.055);
return;

function R = invgammacorrection(Rp)
R = zeros(size(Rp));
i = (Rp <= 0.0404482362771076);
R(i) = Rp(i)/12.92;
R(~i) = real(((Rp(~i) + 0.055)/1.055).^2.4);
return;

function fY = f(Y)
fY = real(Y.^(1/3));
i = (Y < 0.008856);
fY(i) = Y(i)*(841/108) + (4/29);
return;

function Y = invf(fY)
Y = fY.^3;
i = (Y < 0.008856);
Y(i) = (fY(i) - 4/29)*(108/841);
return;


function [fp, fm] = partition(f)
% PARTITION     Partition a spherefun into its even/periodic
%   odd/anti-periodic parts.
%
%   [fp, fm] = partition(f) partitions f into two spherefuns fp & fm with
%   the following properties:
%
%   fp has a CDR decomposition such that C is even and R is pi periodic
%   fm has a CDR decomposition such that C is odd and R is pi anti-periodic
%
% See also COMBINE

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

if ( ~isa(f, 'spherefun') )
    error('SPHEREFUN:partition:unknown', ['Undefined function '...
        'partition'' for input argument of type %s.'], class(f));
end

if ( isempty(f) )
    fp = spherefun();
    fm = spherefun();
    return
end

% Do the even-pi-periodic case first
id = f.idxPlus;
if ( isempty(id) )
    fp = spherefun();
else
    fp = f;
    fp.cols = fp.cols(:, id);
    fp.rows = fp.rows(:, id);
    fp.pivotValues = fp.pivotValues(id);
    fp.pivotLocations = fp.pivotLocations(id, :);
    fp.idxPlus = 1:length(id);
    fp.idxMinus = [];
end

% Now do the odd case
id = f.idxMinus;
if ( isempty(id) )
    fm = spherefun();
else
    fm = f;
    fm.cols = fm.cols(:, id);
    fm.rows = fm.rows(:, id);
    fm.pivotValues = fm.pivotValues(id);
    fm.pivotLocations = fm.pivotLocations(id, :);
    fm.idxMinus = 1:length(id);
    fm.idxPlus = [];
    fm.nonZeroPoles = 0;
end

end


%
% Just like the "sort" function,
% except that ties are broken randomly
% rather than preserving the original order
%
% Output
%   B: sorted vector v
%   IX: B = v(IX)
function [B,IX] = randsort(v,direction)

    assert(isvector(v));
    assert(strcmp(direction,'ascend') || strcmp(direction,'descend'));

    n = numel(v);
    perm = randperm(n);
    v = v(perm);
    [B,IX2] = sort(v(:),1,direction);

    IX = vec(1:n);
    IX = IX(perm);
    IX = IX(IX2);
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% initlist.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function [x0,f0,l,L,istar,ncall] = initlist(fcn,data,u,v)
% generates an initialization list with the aid of line searches
%
% Input:
% fcn = 'fun' 	name of function fun(data,x), x an n-vector
% data		data vector
% [u,v]       	box in which the optimization is carried out (u, v
%             	n-vectors)
% Output:
% x0		array with n rows and at least 3 columns; the ith row
%		contains the initialization list values for the ith
%		coordinate
% f0		corresponding function values
% l		x0(i,l(i)) is the ith coordinate of the initial point
% L
% istar		x0(i,istar(i)) is the ith coordinate of the final best
%		point
% ncall		number of function calls used in the program
%
% Uses the following functions/m-files:
% gls.m and its subprograms
%

function [x0,f0,l,L,istar,ncall] = initlist(fcn,data,u,v)
ncall = 0;
nloc = 5;
small = 0.1;
smaxls = 25;
n = length(u);
x = min(max(u,0),v);	% absolutely smallest point
f = feval(fcn,data,x);
ncall = ncall + 1;
for i = 1:n
  alist = 0;
  flist = f;
  p = zeros(n,1);
  p(i) = 1;
  [alist,flist,nfls] = gls(fcn,data,u,v,x,p,alist,flist,nloc,small,smaxls);
  ncall = ncall + nfls;
  [alist1,flist1] = lspost(alist,flist);
  if isempty(find(alist1==0))
    alist1 = [alist1 0];
    flist1 = [flist1 f];
  end
  if length(alist1) < 3
    if isempty(find(alist1==alist(length(alist))))
      alist1 = [alist1 alist(length(alist))];
      flist1 = [flist1 flist(length(alist))];
    end
    if length(alist1) < 3
      if isempty(find(alist1==alist(1)))
        alist1 = [alist1 alist(1)];
        flist1 = [flist1 flist(length(alist))];
      end
      if length(alist1) < 3
        k = round((1+length(alist))/2);
        alist1 = [alist1 alist(k)];
        flist1 = [flist1 flist(k)];
      end
    end
  end
  [alist,ind] = sort(alist1);
  flist = flist1(ind);
  l(i) = find(alist == 0);
  [f1,istar(i)] = min(flist);
  L(i) = length(alist);
  x0(i,1:L(i)) = alist + x(i);
  f0(1:L(i),i) = flist';
  x(i) = x0(i,istar(i));
  f = feval(fcn,data,x);
end


function [varargout] = nanmin(varargin)
%NANMIN Minimum value, ignoring NaNs.
%   M = NANMIN(A) returns the minimum of A with NaNs treated as missing.
%   For vectors, M is the smallest non-NaN element in A.  For matrices, M
%   is a row vector containing the minimum non-NaN element from each
%   column.  For N-D arrays, NANMIN operates along the first non-singleton
%   dimension.
%
%   [M,NDX] = NANMIN(A) returns the indices of the minimum values in A.  If
%   the values along the first non-singleton dimension contain more than
%   one minimal element, the index of the first one is returned.
%
%   M = NANMIN(A,B) returns an array the same size as A and B with the
%   smallest elements taken from A or B.  Either one can be a scalar.
%
%   [M,NDX] = NANMIN(A,[],DIM) operates along the dimension DIM.
%
%   See also MIN, NANMAX, NANMEAN, NANMEDIAN, NANVAR, NANSTD.

%   Copyright 1993-2004 The MathWorks, Inc.
%   $Revision: 1.1.8.1 $  $Date: 2010/03/16 00:15:52 $

% Call [m,ndx] = min(a,b) with as many inputs and outputs as needed
[varargout{1:nargout}] = min(varargin{:});


function ell_demo ( )

%*****************************************************************************80
%
%% ELL_DEMO demonstrates MESH2D on the L-shaped region.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 April 2014
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'ELL_DEMO:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Demonstrate MESH2D on the L-shaped region.\n' );

  clf

  warning off
%
%  #1) Simple input, 6 vertices.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  EXAMPLE 1:\n' );
  fprintf ( 1, '  Minimal Input\n' );
  fprintf ( 1, '  Use 6 vertices on the boundary.\n' );

  v = [ 0.0, 0.0; 2.0, 0.0; 2.0, 1.0; 1.0, 1.0; 1.0, 2.0; 0.0, 2.0 ];

  [ p, t ] = mesh2d ( v );

  [ nv, ~ ] = size ( v );
  [ np, ~ ] = size ( p );
  [ nt, ~ ] = size ( t );
  fprintf ( 1, '  %d boundary vertices input, %d nodes and %d triangles created\n', nv, np, nt );
  pause
%
%  #2) = Example #1, with some small segments.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  EXAMPLE 2:\n' );
  fprintf ( 1, '  Set a few small boundary segments.\n' );
  fprintf ( 1, '  Use 8 vertices on the boundary.\n' );

  v = [ 0.0, 0.0; 2.0, 0.0; 2.0, 0.25; 2.0, 0.5; 2.0, 1.0; 1.0, 1.0; 1.0, 2.0; 0.0, 2.0 ];

  [ p, t ] = mesh2d ( v );

  [ nv, ~ ] = size ( v );
  [ np, ~ ] = size ( p );
  [ nt, ~ ] = size ( t );
  fprintf ( 1, ...
    '  %d boundary vertices input, %d nodes and %d triangles created\n', ...
    nv, np, nt );
  pause
%
%  #3) = Example #1, but now I specify a maximum element size of 0.2.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  EXAMPLE 3:\n' );
  fprintf ( 1, '  Set maximum element size HDATA.HMAX = 0.1\n' );

  v = [ 0.0, 0.0; 2.0, 0.0; 2.0, 1.0; 1.0, 1.0; 1.0, 2.0; 0.0, 2.0 ];

  hdata = [];
  hdata.hmax = 0.1;

  [ p, t ] = mesh2d ( v, [], hdata );

  [ nv, ~ ] = size ( v );
  [ np, ~ ] = size ( p );
  [ nt, ~ ] = size ( t );
  fprintf ( 1, '  %d boundary vertices input, %d nodes and %d triangles created\n', nv, np, nt );
  pause
%
%  #4) = Example #1, but now specify a density function.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  EXAMPLE 4:\n' );
  fprintf ( 1, '  Specify small elements near reentrant corner using a size function.\n' );

  v = [ 0.0, 0.0; 2.0, 0.0; 2.0, 1.0; 1.0, 1.0; 1.0, 2.0; 0.0, 2.0 ];

  hdata = [];
  hdata.fun = @hfun1;

  [ p, t ] = mesh2d ( v, [], hdata );

  [ nv, ~ ] = size ( v );
  [ np, ~ ] = size ( p );
  [ nt, ~ ] = size ( t );
  fprintf ( 1, '  %d boundary vertices input, %d nodes and %d triangles created\n', nv, np, nt );
  pause
%
%  #5) Same as #1, but refine.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  EXAMPLE 5:\n' );
  fprintf ( 1, '  Repeat example #1, then call refine ( ).\n' );

  v = [ 0.0, 0.0; 2.0, 0.0; 2.0, 1.0; 1.0, 1.0; 1.0, 2.0; 0.0, 2.0 ];

  [ p, t ] = mesh2d ( v );

  [ nv, ~ ] = size ( v );
  [ np, ~ ] = size ( p );
  [ nt, ~ ] = size ( t );
  fprintf ( 1, '  %d boundary vertices input, %d nodes and %d triangles created\n', nv, np, nt );
  pause

  [ p, t ] = refine ( p, t );
%
%  Since REFINE doesn't redraw the mesh, I had to guess how to do it here.
%
   clf
   hold on
   plot ( v(:,1 ), v(:,2), 'r.', 'MarkerSize', 32 );
   axis equal
   pause
   plot(p(:,1),p(:,2),'b.', 'Markersize', 16 )
   plot ( v(:,1), v(:,2), 'r.', 'MarkerSize', 32 );
   pause ( )
   patch('faces',t(:,:),'vertices',p,'facecolor','w','edgecolor','b');
%  patch('faces',edge,'vertices',v,'facecolor','none','edgecolor','k')
   plot ( p(:,1), p(:,2), 'b.', 'Markersize', 16 )
   plot ( v(:,1), v(:,2), 'r.', 'MarkerSize', 32 );
   axis equal off;
   hold off

   print ( '-dpng', 'ell_mesh5.png' );

  [ nv, ~ ] = size ( v );
  [ np, ~ ] = size ( p );
  [ nt, ~ ] = size ( t );
  fprintf ( 1, '  %d boundary vertices input, %d nodes and %d triangles created\n', nv, np, nt );

  pause
%
%  #6) Same as #2, but smooth.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  EXAMPLE 6:\n' );
  fprintf ( 1, '  Repeat example #2, then call smoothmesh ( ).\n' );

  v = [ 0.0, 0.0; 2.0, 0.0; 2.0, 0.25; 2.0, 0.5; 2.0, 1.0; 1.0, 1.0; 1.0, 2.0; 0.0, 2.0 ];

  [ p, t ] = mesh2d ( v );

  [ nv, ~ ] = size ( v );
  [ np, ~ ] = size ( p );
  [ nt, ~ ] = size ( t );
  fprintf ( 1, '  %d boundary vertices input, %d nodes and %d triangles created\n', nv, np, nt );
  pause

  [ p, t ] = smoothmesh ( p, t );

  [ nv, ~ ] = size ( v );
  [ np, ~ ] = size ( p );
  [ nt, ~ ] = size ( t );
  fprintf ( 1, '  %d boundary vertices input, %d nodes and %d triangles created\n', nv, np, nt );

  redisplay ( v, p, t );

  pause
%
%  Close the figure.
%
  close ( gcf )
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'ELL_DEMO:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  return
end
function h = hfun1 ( x, y )

%*****************************************************************************80
%
%% HFUN1 is a size-function for the L-shaped region.
%
%  Discussion:
%
%    The smallest size is at (1.0,1.0), and sizes increase as their distance
%    from that point increases.
%
  h = 0.01 + 0.1 * sqrt ( ( x - 1.0 ).^2  + ( y - 1.0 ).^2 );

  return
end


clear all;
%% Look at particular section
% Pick the gait example to observe
% load('TetrapodExample');
load('TripodExample');

% Plot the velocity of the leg tips
figure('pos',[153, 427, 560, 420]); hold on; axis tight; set(gcf,'color','w'); fontsize(16)
imagesc(example_vel);
xlabel('Time (seconds)')
xticklabels(xticks/Fs);
ylabel('Leg Tip')
yticks([1:6])
yticklabels({'RF','RM','RH','LF','LM','LH'})
ax1 = gca;
caxis([-10 10]);

% Plot the rasters
figure('pos',[850, 634, 848, 334]); hold on; axis tight; set(gcf,'color','w'); fontsize(16)
imagesc(example_stance); colormap('gray');
xlabel('Time (seconds)')
xticklabels(xticks/Fs);
ylabel('Leg Tip')
yticks([1:6])
yticklabels({'LF','LM','LH','RF','RM','RH'})
ax2 = gca;

% plot the forward velocity of the fly
figure('pos',[850, 359, 854, 186]); hold on; axis tight; set(gcf,'color','w'); fontsize(16)
plot(example_fv.*Fs)
xlabel('Time (seconds)')
xticklabels(xticks/Fs);
ylabel('Forward Velocity (mm/s)')
ax3 = gca;
ylim([0 40]);

% plot the raster of tripod, tetrapod, or non-canonical
figure('pos',[849, 114, 931, 150]); hold on; axis tight; set(gcf,'color','w'); fontsize(16)
example_gait = sum(example_stance,1);
example_gait(~(example_gait == 3 | example_gait == 4)) = 5;
imagesc(example_gait);colormap('jet');h = colorbar;
xlabel('Time (seconds)')
xticklabels(xticks/Fs);
yticks([])
ylabel(h,'Number of legs in stance')
ax4 = gca;
linkaxes([ax1,ax2,ax3,ax4],'x')

%% Plot the emission probabilities for each hidden states
load('GaitVectors3.mat');
emissions = hmm.ESTEMIT;
temp = emissions(2,:);
emissions(2,:) = emissions(3,:);
emissions(3,:) = temp;
figure; hold on;
imagesc(emissions);
for i = 1:size(emissions,1)
    for j = 1:size(emissions,2)
        caption = sprintf('%.2f',emissions(i,j));
        text(j,i,caption,'Fontsize',10,'FontWeight','bold','HorizontalAlignment','center','Color',[0 0 0]);
    end
end
axis ij;
axis tight
xlabel('Number of Legs in Stance');
yticks([1 2 3]);
yticklabels({'Tripod','Tetrapod','Non-canonical'})
xticklabels({'0','1','2','3','4','5','6'})
fontsize(16)

figure; hold on;
plot(emissions','LineWidth',3);
xlabel('Number of Legs in Stance');
xticklabels({'0','1','2','3','4','5','6'})
legend({'Tripod','Tetrapod','Non-canonical'})
ylabel('Emission Probability')

%% Plot the distribution of speeds
load('Gait_Speed_Distributions');
figure; hold on;
plot(edges1(1:end-1),N1,'LineWidth',3)
plot(edges2(1:end-1),N2,'LineWidth',3)
plot(edges3(1:end-1),N3,'LineWidth',3)
xlabel('Forward Velocity (ms)')
ylabel('Count')
xlim(speed_lim)
legend({'Tripod','Tetrapod','Non-canonical'})
fontsize(16)

%% Plot the velocity Distributions
load('Cluster_Velocity_Distributions');
figure; hold on;
cmap = spring(num_states);
for i = 1:num_states
    plot(edges{i}(1:end-1),N{i},'Color',cmap(num_states + 1 -i,:),'LineWidth',3);
end
grid on;
xlabel('Forward Velocity')
ylabel('Probability')
axis tight;
ylim([0 .2])

%% Plotting the mean and std with bounded lines
load('Swing_Velocity_Over_Time');
cmap = parula(numel(speed_levels));
p_lines = cell1(numel(speed_levels)-1);
figure('pos',[568, 186, 1036, 798]); figclosekey; set(gcf,'color','w'); hold on;
for i = 1:size(leg_vel_at_speed,1)
    yci = zeros(2,size(leg_vel_at_speed,2));
    yci(1,:) = leg_vel_std_at_speed(i,:);
    yci(2,:) = leg_vel_std_at_speed(i,:);
    [p_lines{i},~] = boundedline(win*10,leg_vel_at_speed(i,:),yci','alpha','cmap',cmap(i,:));
    p_lines{i}.LineWidth = 3;
end

% Legend
leg = cell([1 numel(speed_levels)-1]);
for i = 1:numel(speed_levels)-1
    leg{i} = sprintf('%d - %d mm/s',speed_levels(i),speed_levels(i+1));
end
l = legend([p_lines{:}],leg);
l.Position = [0.7503 0.6488 0.1573 0.3239];
fontsize(16)
xlabel('Time from swing onset (ms)')
ylabel('Swing velocity (mm/s)')
% export_fig('figs/Swing_Velocity_vs_Time_Confidences.png','-r300')

%% Plot the Swing_and_Stance_versus_Velocity
load('Swing_and_Stance_versus_Velocity')
figure; figclosekey, hold on;

yci = zeros(2,numel(stance_dur_std));
yci(1,:) = stance_dur_std;
yci(2,:) = stance_dur_std;
[bl1,~] = boundedline(stance_edges(2:end),stance_dur_mu',yci','alpha');

yci = zeros(2,numel(swing_dur_std));
yci(1,:) = swing_dur_std;
yci(2,:) = swing_dur_std;
[bl2,~] = boundedline(swing_edges(2:end),swing_dur_mu',yci','alpha','r');

yticklabels(round(yticks*10))
ylabel('Durations (ms)');
xlabel('Average Body Speed (mm/s)');
axis tight
legend([bl1,bl2],{'Stance','Swing'})
fontsize(16);


%%=====================================================================
%% $RCSfile: tps_test_script.m,v $
%% $Author: bjian $
%% $Date: 2008/11/24 08:59:03 $
%% $Revision: 1.1 $
%%=====================================================================

% preparation: set ctrl pts
ctrl_pts = rand(25,2);
[PP,kernel] = tps_set_ctrl_pts(ctrl_pts);
lambda = 0;

m = 10;
n = 10;
err = ones(m,n);
for i=1:m
    % step1: set landmarks;
    landmarks = rand(100,2);
    [U,Pm,Q1,Q2,R] = tps_set_landmarks(landmarks,ctrl_pts);

    for j=1:n
        % step2: set parameters;
        param0 = rand(25,2);
        [target, bending] = tps_warp(PP,kernel,U,Pm,param0);
        param1 = tps_compute_param(PP,kernel,U,Pm,Q1,Q2,R,lambda,target);
        err(i,j) = norm(param1-param0);
    end
end
max(err(:))


%==============================================================================
% This code is part of the Matlab-based toolbox
% LagLDDDM - A Lagrangian Gauss--Newton--Krylov Solver for Mass- and
%                        Intensity-Preserving Diffeomorphic Image Registration
%
% For details and license info see
% - https://github.com/C4IR/FAIR.m/tree/master/add-ons/LagLDDMM
%
% function [T,dT] = getPICMatrixAnalyticIntegral(omega,m,hp,xp,eps,varargin)
%
% Building push-forward matrix for a Particle-In-Cell method using anlytic
% integration. See also description in Sec. 3 of the paper:
%
%  ##2
%
%
% Input:
%  omega    - representation of computational domain
%  mc       - discretization size of sampling grid of data
%  mf       - discretization size of high-resolution image
%  hp       - cell size on particle grid used for computing particle's mass
%             which is rho_i \approx prod(hp)* \rho(x_i)
%  xp       - particle positions, size(xp) = [dim*np,1];
%  eps      - width of particles
%  rho      - particles mass, size(rho)=[np,1]
%  varargin - optional additional input
%
% Output:
%  T        - push-forward matrix, i.e. rho(xp) = C*rho
%  dT       - derivative of (C(xp)*rho) with respect to xp.
%
% =========================================================================
function [T,dT] = getPICMatrixAnalyticIntegral(omega,mc,mf,xp,varargin)

if nargin==0, help(mfilename); runMinimalExample; return; end

hp = (omega(2:2:end)-omega(1:2:end))./mf;   % cell-size in particle mesh
epsP = hp;                                  % width of particles
doDerivative = (nargout==2);
for k=1:2:length(varargin) % overwrites defaults
    eval([varargin{k},'=varargin{',int2str(k+1),'};']);
end;

dT = [];
dim = numel(omega)/2;
xp  = reshape(xp,[],dim);
np  = size(xp,1);         % number of particles
n   = prod(mc);            % number of voxels in sampling grid

h   = (omega(2:2:end)-omega(1:2:end))./mc;
pwidth = (ceil(epsP./h));    % upper bound for support of basis functions

% map particle positions to the domain [0,m(1)] x [0,m(dim)]
for i=1:dim
    xp(:,i) = (xp(:,i)-omega(2*i-1))/h(i);
end
% omega = zeros(1,2*dim);omega(2:2:end) = mc;

% get cell index of particles center of mass
P = ceil(xp);
w = xp-(P-1);

switch dim
    case 1
        B = reshape(int1D(w,pwidth,epsP,h,hp),[],1);
        J = repmat(  (1:np)',[2*pwidth+1,1]);
        I = reshape(bsxfun(@plus,P(:,1),-pwidth:pwidth),[],1);

        valid = (1<=I) & (I<=mc(1));
        I = I(valid); J = J(valid); B = B(valid);

        T = sparse(I,J,B,n,np);
        if doDerivative
            bx = reshape(diff1D(w,pwidth,epsP,h,hp),[],1);
            ids = (1:numel(bx)); ids = ids(valid);
            dT = @(rho) sparse(I,J,rho(J).*bx(ids),n,np)/h;
        end

    case 2
        s2i  = @(i) i(:,1)+ mc(1)   *(i(:,2)-1);   % sub2ind for cell-centered grid

        B1 = int1D(w(:,1),pwidth(1),epsP(1),h(1),hp(1));
        B2 = int1D(w(:,2),pwidth(2),epsP(2),h(2),hp(2));
        if doDerivative
            b1 = diff1D(w(:,1),pwidth(1),epsP(1),h(1),hp(1));
            b2 = diff1D(w(:,2),pwidth(2),epsP(2),h(2),hp(2));
        end

        nVoxel = prod([size(B1,2),size(B2,2)]);
        J = repmat((1:np)',[nVoxel 1]);
        I = zeros([np*nVoxel,2]); B = zeros(np*nVoxel,1); pp = 1;
        bx = B; by = B;
        for px = -pwidth(1):pwidth(1)
            for py = -pwidth(2):pwidth(2)
                idx = (pp-1)*np+(1:np); pp = pp+1;
                I(idx,:) = [P(:,1)+px, P(:,2)+py];

                B(idx) =  B1(:,px+pwidth(1)+1).*B2(:,py+pwidth(2)+1);

                if doDerivative
                    bx(idx) = B2(:,py+pwidth(2)+1).*b1(:,px+pwidth(1)+1);
                    by(idx) = B1(:,px+pwidth(1)+1).*b2(:,py+pwidth(2)+1);
               end
            end
        end
        valid = (1<=I(:,1)) & (I(:,1)<=mc(1)) & (1<=I(:,2)) & (I(:,2)<=mc(2));
        I = s2i(I(valid,:));
        J = J(valid);
        B = B(valid);
        T = sparse(I,J,B,n,np);

        if doDerivative
            ids = (1:numel(bx))'; ids = ids(valid);
            dT = @(rho) [ sparse(I,J,rho(J).*bx(ids)/h(1),n,np), sparse(I,J,rho(J).*by(ids)/h(2),n,np)];
        end
    case 3
        % sub2ind for cell-centered grid
        s2i  = @(i) i(:,1)+ mc(1)   *(i(:,2)-1)+(mc(1)  )*(mc(2)  )*(i(:,3)-1);

        B1 = int1D(w(:,1),pwidth(1),epsP(1),h(1),hp(1));
        B2 = int1D(w(:,2),pwidth(2),epsP(2),h(2),hp(2));
        B3 = int1D(w(:,3),pwidth(3),epsP(3),h(3),hp(3));

        nVoxel = prod([size(B1,2),size(B2,2),size(B3,2)]);
        I = zeros([np*nVoxel,3]); J = repmat( (1:np)',[nVoxel,1]); B = zeros(np*nVoxel,1);
        pp = 1;
        if doDerivative
            b1 = diff1D(w(:,1),pwidth(1),epsP(1),h(1),hp(1));
            b2 = diff1D(w(:,2),pwidth(2),epsP(2),h(2),hp(2));
            b3 = diff1D(w(:,3),pwidth(3),epsP(3),h(3),hp(3));

            bx = B; by = B; bz = B;
        end
        for pz = -pwidth(3):pwidth(3)
            i3 = P(:,3)+pz;
            B3t = B3(:,pz+pwidth(3)+1);
            for py = -pwidth(2):pwidth(2)
                i2 = P(:,2)+py;
                B2t = B2(:,py+pwidth(2)+1);
                B23 = B3t.*B2t;
                for px = -pwidth(1):pwidth(1)
                    idx = (pp-1)*np+(1:np); pp = pp+1;
                    I(idx,1) = P(:,1)+px;
                    I(idx,2)=i2;
                    I(idx,3)=i3;

                    % remove cells that lie out of the domain
                    B1t = B1(:,px+pwidth(1)+1);

                    B(idx)   =  B1t.*B23;

                    if doDerivative
                       % compute derivatives of weights
                        bx(idx)     = B23.*b1(:,px+pwidth(1)+1);
                        by(idx)     = B1t.*B3t.*b2(:,py+pwidth(2)+1);
                        bz(idx)     = B1t.*B2t.*b3(:,pz+pwidth(3)+1);
                    end
                end
            end
        end
        valid =    (1<=I(:,1)) & (I(:,1)<=mc(1)) ...
            & (1<=I(:,2)) & (I(:,2)<=mc(2)) ...
            & (1<=I(:,3)) & (I(:,3)<=mc(3));

        I = s2i(I(valid,:));
        J = J(valid);
        B = B(valid);
        T = sparse(I,J,B,n,np);

        if doDerivative
            ids = (1:numel(bx))'; ids = ids(valid);
            D1 = @(rho) sparse(I,J,rho(J).*bx(ids)/h(1),n,np);
            D2 = @(rho) sparse(I,J,rho(J).*by(ids)/h(2),n,np);
            D3 = @(rho) sparse(I,J,rho(J).*bz(ids)/h(3),n,np);
            dT = @(rho) [D1(rho), D2(rho), D3(rho)];
        end

    otherwise
        error('dimension must be 1,2,3')
end

function Bij = int1D(w,pwidth,eps,h,hp)
Bij = zeros(numel(w),2*pwidth+1);
Bleft = B(-pwidth-w,eps,h);
for p = -pwidth:pwidth
    Bright = B(1+p-w,eps,h);
    Bij(:,p+pwidth+1)  = hp*(Bright - Bleft);
    Bleft = Bright;
end
function Bij = diff1D(w,pwidth,eps,h,hp)
Bij = zeros(numel(w),2*pwidth+1);
Bleft = b(-pwidth-w,eps,h);
for p = -pwidth:pwidth
    Bright = b(1+p-w,eps,h);
    Bij(:,p+pwidth+1)  = hp*(Bright - Bleft);
    Bleft = Bright;
end
Bij = - Bij;

function bij = b(x,eps,h)
bij = zeros(numel(x),1);

ind1 = (-eps/h<=x)&(x<=0);
ind2 = (0<x)&(x<=eps/h);

bij(ind1)  = 1 + h*x(ind1)./eps;
bij(ind2)  = 1 - h*x(ind2)./eps;
bij = bij /eps;

function Bij = B(x,eps,h)
Bij = zeros(numel(x),1);

ind1 = (-eps/h<=x)&(x<=0);
ind2 = (0<x)&(x<=eps/h);
ind3 = (eps/h<x);

Bij(ind1) = x(ind1) + 1./(2*eps/h).*x(ind1).^2+eps/(h*2);
Bij(ind2) = x(ind2) - 1./(2*eps/h).*x(ind2).^2+eps/(h*2);
Bij(ind3) = eps/h;
Bij = Bij/eps;

function [T,dT] = derivativeTestFctn(omega,m,hc,xp,rho)
[T,dT] = feval(mfilename,omega,m,hc,xp);
T = T*rho;
dT = dT(rho);

function runMinimalExample

%  ========== 1 D ==================
omega = [0 1]; mc = 64; mf = 64;
hc     = (omega(2:2:end)-omega(1:2:end))./mc;
hf     = (omega(2:2:end)-omega(1:2:end))./mf;
rho   = ones(mc,1); rho([1:8,end-8:end]) = 0;

xp = getCellCenteredGrid(omega,mc);
xp = xp +.02;

% transport rho
C = feval(mfilename,omega,mf,mc,xp);
rhonew = C*rho;

% visualize result
figure(1);clf;
subplot(1,2,1)
plot(getCellCenteredGrid(omega,mc),rho);
title(sprintf('rho, mass:%e',prod(hc)*sum(rho)));

subplot(1,2,2)
plot(getCellCenteredGrid(omega,mf),rhonew)
title(sprintf('rhonew, mass:%e',prod(hf)*sum(rhonew)));
% check derivative
fctn = @(xp) derivativeTestFctn(omega,mf,mc,xp,rho);
checkDerivative(fctn,xp(:));
%  ========== 2 D ==================
omega = [-2 2 -2 2]; mc = [32 32]; mf = mc*2;
hc     = (omega(2:2:end)-omega(1:2:end))./mc;
hf     = (omega(2:2:end)-omega(1:2:end))./mf;
f      = @(x) 0 + (sqrt(x(:,1).^2 + .4*x(:,2).^2)<.7);
rho    = f(reshape(getCellCenteredGrid(omega,mc),[],2));

% choose parameters
xp = getCellCenteredGrid(omega,mc);
xp = xp+.1*hc(1) ;

% transport rho
C = feval(mfilename,omega,mf,mc,xp);
rhonew = C*rho;

% visualize result
figure(2); clf;
subplot(1,2,1);
viewImage2Dsc(rho,omega,mc);
title(sprintf('rho, mass:%e',prod(hc)*sum(rho)));

subplot(1,2,2);
viewImage2Dsc(rhonew,omega,mf);
title(sprintf('rho, mass:%e',prod(hf)*sum(rhonew)));

% check derivative
fctn = @(xp) derivativeTestFctn(omega,mf,mc,xp,rho);
checkDerivative(fctn,xp(:));

%  ========== 3 D ==================
omega = [-2 2 -2 2 -3 3]; mc = [16 32 24]; mf = mc;
hc     = (omega(2:2:end)-omega(1:2:end))./mc;
hf     = (omega(2:2:end)-omega(1:2:end))./mf;
f      = @(x) 0 + (sqrt(x(:,1).^2 + .3*x(:,2).^2 + .2*x(:,3).^2)<.7);
rho    = f(reshape(getCellCenteredGrid(omega,mc),[],3));

% choose parameters
xp = getCellCenteredGrid(omega,mc);
xp = xp+0.1*hc(1) ;

% transport rho
C = feval(mfilename,omega,mf,mc,xp);
rhonew = C*rho;

% visualize result
figure(2); clf;
subplot(1,2,1);
imgmontage(rho,omega,mc);
title(sprintf('rho, mass:%e',prod(hc)*sum(rho)));

subplot(1,2,2);
imgmontage(rhonew,omega,mf);
title(sprintf('rho, mass:%e',prod(hf)*sum(rhonew)));

% check derivative
fctn = @(xp) derivativeTestFctn(omega,mf,mc,xp,rho);
checkDerivative(fctn,xp(:));


%PREX_PARZEN Parzen based denisities and classifiers
%
% PRTools example to show the differences between various ways to use the
% PARZEN procedures for estimating densities and classifiers.

help prex_parzen

delfigs
figure
echo on

  delfigs
  a = gendath;   % two normally distributed classes, different covariances
  w = a*parzenc; % Parzen classifier, single smoothing parameter optimizing
                 % the classification error
  figure(1); scatterd(a); % show scatterplot
  plotm(w);  plotc(w);    % show densities and classifier
  title('Densities and classifier by PARZENC')
  w = a*parzendc;% Parzen classifier, smoothing parameter per class
                 % optimizing class densities
  figure(2); scatterd(a); % show scatterplot
  plotm(w);  plotc(w);    % show densities and classifier
  title('Densities and classifier by PARZENDC')
  w = a*parzenm; % Parzen density, smoothing parameter per class
                 % optimizing class densities, combined to single density
  figure(3); scatterd(a); % show scatterplot
  plotm(w);  plotc(w);    % show density
  title('Density by parzenm on labeled data')
  w = +a*parzenm; % Parzen density, classes combined, so just a single
                  % smoothing parameter optimizing overall density
  figure(4); scatterd(+a);% show scatterplot
  plotm(w);               % show density
  title('Density by parzenm on unlabeled data')

echo off
showfigs


function [BIscores]=mvg_superpixelBBintegralScore(superpixels, windows, config)

%% Default settings
if nargin<3
    config.probDist='Gaussian'; % 'Dist' 'Gaussian' 'DistGaussian'
    config.GaussSigma=3;
    config.exponent=2;
    config.dilate=3;
    config.verbose=4;
end

%% Find bounding boxes and make boundary mask image
lbs=unique(superpixels);
numSpix=length(lbs);
spixBox=zeros(numSpix,4);
for i=1:numSpix
    [rw,cl]=find(superpixels==lbs(i));
    spixBox(i,:)=[min(cl),min(rw),max(cl),max(rw)];
end

%% Make boundary mask
boundaryMask=zeros(size(superpixels));
for i=1:numSpix
    boundaryMask(spixBox(i,2):spixBox(i,4),spixBox(i,1))=1;
    boundaryMask(spixBox(i,2):spixBox(i,4),spixBox(i,3))=1;
    boundaryMask(spixBox(i,2),spixBox(i,1):spixBox(i,3))=1;
    boundaryMask(spixBox(i,4),spixBox(i,1):spixBox(i,3))=1;
end

%% Dilate
if config.dilate>0
    structEl=strel('disk',config.dilate);
    boundaryMask=imdilate(boundaryMask,structEl);
end

%% Make distance transform based probability
switch config.probDist
    case 'Dist'
        probFun=1-bwdist(boundaryMask);
        probFun=(probFun-min(probFun(:)))/max((probFun(:)-min(probFun(:))));
        probFun=probFun.^config.exponent;
    case 'Gaussian'
        GaussianFun=makeGaussian_(config.GaussSigma*[1 1]);
        probFun=conv2(double(boundaryMask),GaussianFun,'same');
    case 'DistGaussian'
        probFun=1-bwdist(boundaryMask);
        probFun=(probFun-min(probFun(:)))/max((probFun(:)-min(probFun(:))));
        probFun=probFun.^config.exponent;
        GaussianFun=makeGaussian(config.GaussSigma*[1 1]);
        probFun=conv2(double(probFun),GaussianFun,'same');
    otherwise
        error('Unknown method');
end

%% Integrate over window borders to get BI score
BIscores=integrateOverWindowBorder_(probFun,windows);

%%%%%%%%%%%%%%%%%%%%%%%%
% Additional functions %
%%%%%%%%%%%%%%%%%%%%%%%%

%%% Integrate over the windows border %%%
function [windowIntegral,winMeasure]=integrateOverWindowBorder_(probFun,windows)

%% Initialize
if size(windows,2)>4
    windows=windows(:,1:4);
end
numWindow=size(windows,1);
intLineImg=zeros(size(probFun,1),size(probFun,2),2);
windowIntegral=zeros(numWindow,1);

%% Form line integral image
intLineImg(:,:,1)=cumsum(probFun,1); % Row sums
intLineImg(:,:,2)=cumsum(probFun,2); % Column sums
intLineImg=[zeros(size(intLineImg,1),1,2),intLineImg]; % Add zero column
intLineImg=[zeros(1,size(intLineImg,2),2);intLineImg]; % Add zero row

%% If given windows are in normalized coordinates, denormalize them
if max(windows(:))<1.000001
    imgCol=size(probFun,2);
    imgRow=size(probFun,1);
    windows(:,[1,3])=windows(:,[1,3])*(imgCol-1)+1;
    windows(:,[2,4])=windows(:,[2,4])*(imgRow-1)+1;
end

%% Round windows to integer coordinates
windows=max(round(windows(:,1:4)),1);

%% Loop over windows and compute bounding box scores
for i=1:numWindow
    % Get critical points
    Xmin=windows(i,1);
    Ymin=windows(i,2);
    Xmax=windows(i,3);
    Ymax=windows(i,4);

    % Compute sums (start each one pixel further to avoid taking corners twice)
    topSum=intLineImg(Ymin+1,Xmax+1,2)-intLineImg(Ymin+1,Xmin+1,2);
    bottomSum=intLineImg(Ymax+1,Xmax+1,2)-intLineImg(Ymax+1,Xmin+1,2);
    leftSum=intLineImg(Ymax+1,Xmin+1,1)-intLineImg(Ymin+1,Xmin+1,1);
    rightSum=intLineImg(Ymax+1,Xmax+1,1)-intLineImg(Ymin+1,Xmax+1,1);
    %% For reference the full sums, which count corner points twice
    %topSum=intLineImg(Ymin+1,Xmax+1,2)-intLineImg(Ymin+1,Xmin,2);
    %bottomSum=intLineImg(Ymax+1,Xmax+1,2)-intLineImg(Ymax+1,Xmin,2);
    %leftSum=intLineImg(Ymax+1,Xmin+1,1)-intLineImg(Ymin,Xmin+1,1);
    %rightSum=intLineImg(Ymax+1,Xmax+1,1)-intLineImg(Ymin,Xmax+1,1);

    % Assing sum over bounding box to window score
    windowIntegral(i)=topSum+bottomSum+leftSum+rightSum;

end

%% If two output arguments are required, return also box size and edge length
if nargin>1
    boxWidth=windows(:,3)-windows(:,1)+1;
    boxHeight=windows(:,4)-windows(:,2)+1;

    winMeasure.Perimeter=2*boxWidth+2*boxHeight-4; % Need to subtract extra corners (that's why -4)
    winMeasure.Area=boxWidth.*boxHeight;
end

%%% Make Gaussian funtion %%%
function [GaussianFun]=makeGaussian_(GaussianSigma,WindowRadius)

%% Return trivial case
if min(GaussianSigma)<eps
    GaussianFun=1;
    return;
end

%% Default settings
if nargin<2
    %WindowRadius=max(round(2*GaussianSigma),1);
    WindowRadius=round(2*GaussianSigma);
end

%% Initialize
dim=length(GaussianSigma); % How many dimensions in output Gaussian (3D is max and ordering is row, column, and third dimension).
GaussianSigma=2*GaussianSigma.^2; %Turn sigma from standard deviation to variance (2* is to include 1/(2*sigma^2) already here).
% If only one value for size is given and more dimensions are required, use same size for all dimensions.
if length(WindowRadius)==1 && dim>1
    WindowRadius=WindowRadius(1)*ones(1,dim);
end

%% Generate Gaussian function to the required dimensions
if dim==1
    d1=-WindowRadius(1):WindowRadius(1); % spatial coordinates
    GaussianFun=exp(-((d1.^2)/GaussianSigma(1))); % Gaussian values

elseif dim==2
    [d2,d1]=meshgrid(-WindowRadius(2):WindowRadius(2),-WindowRadius(1):WindowRadius(1)); % spatial coordinates
    GaussianFun=exp(-((d1.^2)/GaussianSigma(1)+(d2.^2)/GaussianSigma(2))); % Gaussian values

elseif dim==3
    [d2,d1,d3]=meshgrid(-WindowRadius(2):WindowRadius(2),-WindowRadius(1):WindowRadius(1),-WindowRadius(3):WindowRadius(3)); % spatial coordinates
    GaussianFun=exp(-((d1.^2)/GaussianSigma(1)+(d2.^2)/GaussianSigma(2)+(d3.^2)/GaussianSigma(3))); % Gaussian values

else
    error('Not implemented');
end

%% Normalize data to have sum equal to fun.
GaussianFun=GaussianFun/sum(GaussianFun(:));


%PLOTO Plot objects as 1-D functions of the feature number
%
%   [HH HO HC] = PLOTO(A,N)
%
% INPUT
%   A   Dataset
%   N   Integer
%
% OUTPUT
%   HH  Lines handles
%   HO  Object identifier handles
%   HC  Class number handles
%
% DESCRIPTION
% Produces 1-D function plots for all the objects in dataset A. The plots
% are organised as subplots, N on a row. Default is the squareroot of the
% number of objects. Object identifiers and class numbers are written in
% the correspopnding plots.
%
% See also DATASETS

% Copyright: R.P.W. Duin, r.p.w.duin@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

function [h_out1,h_out2,h_out3] = ploto(a,p)

	if nargin < 2, p = []; end
	[m,k,c] = getsize(a);
	nlab = getnlab(a);

	% Define the color for each of the classes:
	if c == 2
		map = [0 0 1; 1 0 0];
	else
		map = hsv(c);
	end

	% Make subplots for each object, so a grid of p x q subplots is
	% defined
	h = [];
	if ~isempty(p)
		q = ceil(m/p);
	elseif m > 3
		p = ceil(sqrt(m)); q = ceil(m/p);
	else
		p = m; q = 1;
	end
	% Get the object labels
	labs = getlabels(a);
	ymin = min(a.data(:));
	ymax = max(a.data(:));
	V = [1 k ymin ymax];
	% Make the plot for each of the objects:
	h = [];
	ho = [];
	hc = [];
	s = sprintf('Plot %i objects: ',m);
	prwaitbar(m,s);
	for j = 1:m
		if isdatafile(a) | 1
			prwaitbar(m,j,[s int2str(j)]);
			b = +prdataset(a(j,:));
			ymin = min(b);
			ymax = max(b);
			k = length(b);
			V = [1 k ymin ymax];
		else
			b = +a(j,:);
		end
		% Create the subplots with the correct sizes:
		subplot(q,p,j)
		hh = plot(b);
		set(gca,'xtick',[]);
		set(gca,'ytick',[]);
		axis(gca,V);
		ho = [ho text(2,ymax-0.15*(ymax-ymin),getident(a(j,:),'string'))];
		hc = [hc text(3*k/4,ymax-0.15*(ymax-ymin),num2str(nlab(j)))];
		h = [h hh];
		hold on
	end
	prwaitbar(0);

	% The last details to take care of:
	if nargout > 0
		h_out1 = h;
		h_out2 = ho;
		h_out3 = hc;
	end

	return


function [depth_map_in_meters, is_disparity_invalid] =...
    depth_in_meters_cityscapes_with_invalid_parts(input_disparity,...
    camera_parameters_file)
%DEPTH_IN_METERS_CITYSCAPES_WITH_INVALID_PARTS  Compute depth map in meters from
%provided Cityscapes disparity map and relevant camera parameters.
%
%   INPUTS:
%
%   -|input_disparity|: matrix of uint16 format with same resolution as
%   Cityscapes RGB images.
%
%   -|camera_parameters_file|: full path to JSON file where camera parameters
%   are stored.
%
%   OUTPUTS:
%
%   -|depth_map_in_meters|: matrix of double format containing depth in meters.
%   It contains invalid measurements that are known to be such and are set to
%   infinity by convention, and may also contain more erroneous values that are
%   not known to be such and can assume any positive value.
%
%   -|is_disparity_invalid|: boolean matrix, where true indicates that the
%   corresponding pixel in |depth_map_in_meters| contains a wrong infinity
%   value.

% Identify known wrong values for disparity, based on specifications provided in
% the Cityscapes README.
is_disparity_invalid = input_disparity == 0;

% Compute the disparity in pixels, based on specifications provided in the
% Cityscapes README.
disparity_in_pixels = disparity_in_pixels_cityscapes(input_disparity);
is_disparity_zero = disparity_in_pixels == 0;

% Retrieve baseline and focal length in x-axis, which are both required in order
% to get the absolute scale of the depth map.
[B, f_x] = camera_parameters_cityscapes(camera_parameters_file);

% Compute the depth as inversely proportional to disparity. Wherever the
% disparity is zero, the depth is equal to infinity. Convention: any known
% invalid disparity is assigned infinite depth.
depth_map_in_meters = zeros(size(disparity_in_pixels));
depth_map_in_meters(~is_disparity_invalid & ~is_disparity_zero) =...
    B * f_x ./ disparity_in_pixels(~is_disparity_invalid & ~is_disparity_zero);
depth_map_in_meters(is_disparity_zero | is_disparity_invalid) = Inf;

end


function r8mat_transpose_print_test ( )

%*****************************************************************************80
%
%% R8MAT_TRANSPOSE_PRINT_TEST tests R8MAT_TRANSPOSE_PRINT;
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2009
%
%  Author:
%
%    John Burkardt
%
  m = 7;
  n = 12;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8MAT_TRANSPOSE_PRINT_TEST\n' );
  fprintf ( 1, '  R8MAT_TRANSPOSE_PRINT prints a R8MAT,\n' );
  fprintf ( 1, '  transposed.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix row order M =    %d\n', m );
  fprintf ( 1, '  Matrix column order N = %d\n', n );
%
%  Set the matrix.
%
  for i = 1 : m
    for j = 1 : n
      a(i,j) = i * 100 + j;
    end
  end

  r8mat_transpose_print ( m, n, a, '  The transposed matrix A:' );

  return
end


function sF = dthetadtheta(sF)
% second derivative in direction theta

s = size(sF);
sF = reshape(sF, []);

sF.fhat(1) = 0; % exclude some special cases
fhat_theta = zeros((sF.bandwidth+2)^2, length(sF));
for m = 0:sF.bandwidth+1
  if 0 <= m-1
    fhat_theta(m^2+1:(m+1)^2, :) = (m-1)*sqrt((m^2-(-m:m)'.^2)/((2*m-1)*(2*m+1))).*[zeros(1, length(sF)); sF.fhat((m-1)^2+1:m^2, :); zeros(1, length(sF))];
  end
  if m+1 <= sF.bandwidth
    fhat_theta(m^2+1:(m+1)^2, :) = fhat_theta(m^2+1:(m+1)^2, :)-(m+2)*sqrt(((m+1)^2-(-m:m)'.^2)/((2*m+1)*(2*m+3))).*sF.fhat((m+1)^2+2:(m+2)^2-1, :);
  end
end

sF_theta = S2FunHarmonic(fhat_theta);

fhat_theta_theta = zeros((sF_theta.bandwidth+2)^2, length(sF));
for m = 0:sF_theta.bandwidth+1
  if 0 <= m-1
    fhat_theta_theta(m^2+1:(m+1)^2, :) = (m-1)*sqrt((m^2-(-m:m)'.^2)/((2*m-1)*(2*m+1))).*[zeros(1, length(sF)); sF_theta.fhat((m-1)^2+1:m^2, :); zeros(1, length(sF))];
  end
  if m+1 <= sF_theta.bandwidth
    fhat_theta_theta(m^2+1:(m+1)^2, :) = fhat_theta_theta(m^2+1:(m+1)^2, :)-(m+2)*sqrt(((m+1)^2-(-m:m)'.^2)/((2*m+1)*(2*m+3))).*sF_theta.fhat((m+1)^2+2:(m+2)^2-1, :);
  end
end

sF_theta_theta = S2FunHarmonic(fhat_theta_theta);

f = @(v) (sF_theta_theta.eval(v)-cos(v.theta).*sF_theta.eval(v))./max(sin(v.theta).^2, 0.1);
sF = S2FunHarmonic.quadrature(f, 'bandwidth', sF.bandwidth);
sF = reshape(sF, s);

end


subject_path = [dataset_path, '/subject_%d'];
intrinsics_path_format = [subject_path, '/face/intrinsic.txt'];
extrinsics_path_format = [subject_path, '/face/extrinsic.txt'];
img_dir_format = [subject_path, '/face/%08d/image_cropped'];
recon_dir_format = [subject_path, '/face/%08d/reconstruction'];

num_cam = 107;
cameras_to_plane_offset = 0.85; % determine where is considered as ground plane
img_size = [200, 250]; % size of cropped image, (w, h)

load('./model/model.mat'); % load following variables:
% meanface - 84 x 3
% meanface_inner - 50 x 3
% Basel_vertex_indices - 1 x 3448
% shape_basis_84 - 252 x 63
% expression_basis_84 - 252 x 6,
% shape_basis_3DMM - 10344 x 63
% expression_basis_3DMM - 10344 x 6
% meanface_3Dmm - 10344 X 1
% tri_list - 6736 x 3

inner_kps_indices = 1:50;
left_contour_kps_indices = [67 65 64 62 61 59 56 53];
right_contour_kps_indices = [70 73 76 77 79 81 82 83];

% get 66 landmarks of meanface (50 inner kps + 16 contour kps)
meanface_contour_left_8 = meanface(left_contour_kps_indices, :); % 8 x 3
meanface_contour_right_8 = meanface(right_contour_kps_indices, :); % 8 x 3
meanface_66 = [meanface_inner; meanface_contour_left_8; meanface_contour_right_8]; % 66 x 3
meanface_v = reshape(meanface_66', [], 1);  % 198 x 1 (x1,y1,z1,....)

% get shape and expression basis for 66 landmarks
valid_landmark_indices = [ inner_kps_indices, left_contour_kps_indices, ...
    right_contour_kps_indices]; % 1 x 66 indices of 84 landmarks to be used for pca fitting
valid_landmark_indices_ = reshape([3 * valid_landmark_indices - 2; ...
    3 * valid_landmark_indices - 1; 3 * valid_landmark_indices], [], 1); % 198 x 1
shape_basis_66 = shape_basis_84(valid_landmark_indices_, :); % 198 x 63
expression_basis_66 = expression_basis_84(valid_landmark_indices_, :); % 198 x 6

% get used shape and expression basis
num_used_shapePC = 10;
num_used_expressionPC = 6;
shape_basis_66 = shape_basis_66(:, 1:num_used_shapePC);
expression_basis_66 = expression_basis_66(:, 1:num_used_expressionPC);
shape_basis_3DMM = shape_basis_3DMM(:, 1:num_used_shapePC);
expression_basis_3DMM = expression_basis_3DMM(:, 1:num_used_expressionPC);

% get mesh index triples for constructing triangles
tri_list = tri_list + 1; % change indices to start from 1 instead of 0


function [ a, kpvt, info ] = dsifa ( a, lda, n )

%*****************************************************************************80
%
%% DSIFA factors a real symmetric matrix.
%
%  Discussion:
%
%    To solve A*X = B, follow DSIFA by DSISL.
%
%    To compute inverse(A)*C, follow DSIFA by DSISL.
%
%    To compute determinant(A), follow DSIFA by DSIDI.
%
%    To compute inertia(A), follow DSIFA by DSIDI.
%
%    To compute inverse(A), follow DSIFA by DSIDI.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 June 2005
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real A(LDA,N), the symmetric matrix to be factored.  Only
%    the diagonal and upper triangle are used.
%
%    Input, integer LDA, the leading dimension of the array A.
%
%    Input, integer N, the order of the matrix.
%
%    Output, real A(LDA,N), a block diagonal matrix and the multipliers which
%    were used to obtain it.  The factorization can be written A = U*D*U'
%    where U is a product of permutation and unit upper triangular
%    matrices, U' is the transpose of U, and D is block diagonal
%    with 1 by 1 and 2 by 2 blocks.
%
%    Output, integer KPVT(N), the pivot indices.
%
%    Output, integer INFO, error flag.
%    0, normal value.
%    K, if the K-th pivot block is singular.  This is not an error
%    condition for this subroutine, but it does indicate that DSISL
%    or DSIDI may divide by zero if called.
%

%
%  ALPHA is used in choosing pivot block size.
%
  alpha = ( 1.0 + sqrt ( 17.0 ) ) / 8.0;

  info = 0;
%
%  Main loop on K, which goes from N to 1.
%
  k = n;

  while ( 0 < k )

    if ( k == 1 )
      kpvt(1) = 1;
      if ( a(1,1) == 0.0 )
        info = 1;
      end
      return
    end
%
%  This section of code determines the kind of
%  elimination to be performed.  When it is completed,
%  KSTEP will be set to the size of the pivot block, and
%  SWAP will be set to .true. if an interchange is required.
%
    km1 = k - 1;
    absakk = abs ( a(k,k) );
%
%  Determine the largest off-diagonal element in column K.
%
    imax = idamax ( k-1, a(1:k-1,k), 1 );
    colmax = abs ( a(imax,k) );

    if ( alpha * colmax <= absakk )

      kstep = 1;
      swap = 0;
%
%  Determine the largest off-diagonal element in row IMAX.
%
    else

      rowmax = 0.0;
      imaxp1 = imax + 1;
      for j = imaxp1 : k
        rowmax = max ( rowmax, abs ( a(imax,j) ) );
      end

      if ( imax ~= 1 )
        jmax = idamax ( imax-1, a(1:imax-1,imax), 1 );
        rowmax = max ( rowmax, abs ( a(jmax,imax) ) );
      end

      if ( alpha * rowmax <= abs ( a(imax,imax) ) )
        kstep = 1;
        swap = 1;
      elseif ( alpha * colmax * ( colmax / rowmax ) <= absakk )
        kstep = 1;
        swap = 0;
      else
        kstep = 2;
        swap = ( imax ~= k-1 )
      end

    end
%
%  Column K is zero.
%  Set INFO and iterate the loop.
%
    if ( max ( absakk, colmax ) == 0.0 )

      kpvt(k) = k;
      info = k;
%
%  1 x 1 pivot block.
%
%  Perform an interchange.
%
    elseif ( kstep ~= 2 )

      if ( swap )

        [ a(1:imax,imax), a(1:imax,k) ] = ...
          dswap ( imax, a(1:imax,imax), 1, a(1:imax,k), 1 );

        for jj = imax : k
          j = k + imax - jj;
          t = a(j,k);
          a(j,k) = a(imax,j);
          a(imax,j) = t;
        end

      end
%
%  Perform the elimination.
%
      for jj = 1 : k-1
        j = k - jj;
        mulk = -a(j,k) / a(k,k);
        t = mulk;
        a(1:j,j) = daxpy ( j, t, a(1:j,k), 1, a(1:j,j), 1 );
        a(j,k) = mulk;
      end
%
%  Set the pivot array.
%
      if ( swap )
        kpvt(k) = imax;
      else
        kpvt(k) = k;
      end
%
%  2 x 2 pivot block.
%
%  Perform an interchange.
%
    else

      if ( swap )

        [ a(1:imax,imax), a(1:imax,k-1) ] = ...
          dswap ( imax, a(1:imax,imax), 1, a(1:imax,k-1), 1 );

        for jj = imax : k-1
          j = k-1 + imax - jj;
          t = a(j,k-1);
          a(j,k-1) = a(imax,j);
          a(imax,j) = t;
        end

        t = a(k-1,k);
        a(k-1,k) = a(imax,k);
        a(imax,k) = t;

      end
%
%  Perform the elimination.
%
      if ( k-2 ~= 0 )

        ak = a(k,k) / a(k-1,k);
        akm1 = a(k-1,k-1) / a(k-1,k);
        denom = 1.0 - ak * akm1;

        for jj = 1 : k-2

          j = k - 1 - jj;
          bk = a(j,k) / a(k-1,k);
          bkm1 = a(j,k-1) / a(k-1,k);
          mulk = ( akm1 * bk - bkm1 ) / denom;
          mulkm1 = ( ak * bkm1 - bk ) / denom;
          t = mulk;
          a(1:j,j) = daxpy ( j, t, a(1:j,k), 1, a(1:j,j), 1 );
          t = mulkm1;
          a(1:j,j) = daxpy ( j, t, a(1:j,k-1), 1, a(1:j,j), 1 );
          a(j,k) = mulk;
          a(j,k-1) = mulkm1;

        end

      end
%
%  Set the pivot array.
%
      if ( swap )
        kpvt(k) = -imax;
      else
        kpvt(k) = 1 - k;
      end

      kpvt(k-1) = kpvt(k);

    end

    k = k - kstep;

  end

  return
end


function [params, transform] = sqexpKernExtractParam(kern)

% SQEXPKERNEXTRACTPARAM Extract parameters from the SQEXP kernel structure.
% FORMAT
% DESC Extract parameters from the pre-built compound squared
% exponential kernel structure into a vector of parameters for
% optimisation.
% ARG kern : the kernel structure containing the parameters to be
% extracted.
% RETURN param : vector of parameters extracted from the kernel. If
% the field 'transforms' is not empty in the kernel matrix, the
% parameters will be transformed before optimisation (for example
% positive only parameters could be logged before being returned).
%
% DESC Extract parameters and parameter names from the pre-built
% compound squared exponential kernel structure.
% ARG kern : the kernel structure containing the parameters to be
% extracted.
% RETURN param : vector of parameters extracted from the kernel. If
% the field 'transforms' is not empty in the kernel matrix, the
% parameters will be transformed before optimisation (for example
% positive only parameters could be logged before being returned).
% RETURN names : celly array of strings containing parameter names.
%
% SEEALSO sqexpKernParamInit, sqexpKernExpandParam, kernExtractParam, scg, conjgrad
%
% COPYRIGHT : Neil D. Lawrence, 2004
%
% KERN

params = [kern.inverseWidth kern.rbfVariance kern.biasVariance ...
          kern.whiteVariance];
if nargout > 1
  names{1} = 'sqexp inverse width';
  names{2} = 'sqexp rbf variance';
  names{3} = 'sqexp bias variance';
  names{4} = 'sqexp white variance';
end


target_vy = [-0.8, -0.6, -0.4, -0.2, 0, 0.2, 0.4, 0.6, 0.8];
% target_vy = [0];
target_vx = [-0.4, -0.3, -0.2, -0.1, 0, 0.1, 0.2, 0.3, 0.4];
% target_vx = [0];
subfolder_name = 'library5';

N_mid = 11;%floor(nlp.Phase(1).NumNode/2)+1;
N_vx = length(target_vx);
N_vy = length(target_vy);
% gait = param.gait;
t_all = cell(N_vx, N_vy);
q_all = cell(N_vx, N_vy);
dq_all = cell(N_vx, N_vy);
px_all = cell(N_vx, N_vy);
py_all = cell(N_vx, N_vy);
vx_all = cell(N_vx, N_vy);
vy_all = cell(N_vx, N_vy);
for i = 1:N_vx
    vx = target_vx(i);
    for j = 1:N_vy
        vy = target_vy(j);
        data_name = fullfile('local', subfolder_name, sprintf('gait_X%0.1f_Y%.1f.mat', vx, vy));
        param = load(data_name);

        t_all{i,j} = [param.gait(3).tspan];%, param.gait(3).tspan];
        q_all{i,j} = [param.gait(3).states.x];%, param. gait(3).states.x];
        dq_all{i,j} = [param.gait(3).states.dx];%, param. gait(3).states.dx];
        px_all{i,j} = [param.gait(3).states.x(1,:)];%ones(size(t_all{i,j}))*vx;
        py_all{i,j} = [param.gait(3).states.x(2,:)];%ones(size(t_all{i,j}))*vy;
        vx_all{i,j} = [param.gait(3).states.dx(1,:)];%ones(size(t_all{i,j}))*vx;
        vy_all{i,j} = [param.gait(3).states.dx(2,:)];%ones(size(t_all{i,j}))*vy;

    end

end

%%
idx = [3:6 7:9, 12:14];
joint_names = {    'BasePosX'
    'BasePosY'
    'BasePosZ'
    'BaseRotX'
    'BaseRotY'
    'BaseRotZ'
    'qHRight'
    'qARight'
    'qBRight'
    'fourBarARight'
    'fourBarBRight'
    'qHLeft'
    'qALeft'
    'qBLeft'
    'fourBarALeft'
    'fourBarBLeft'};
for k=1:length(idx)
    f = figure(k+100); clf;
    f.Name = joint_names{idx(k)};
    set(f, 'WindowStyle', 'docked');

    ax = axes(f); %#ok<LAXES>
    hold(ax);

    for i = 1:N_vx
        for j = 1:N_vy
            t = t_all{i,j};
            q = q_all{i,j};
            dq = dq_all{i,j};
            py = py_all{i,j};
            px = px_all{i,j};
            vy = vy_all{i,j};
            vx = vx_all{i,j};
            scatter3(ax,t, vx,dq(idx(k),:));
%             scatter3(ax,t,vx,q(idx(k),:));

        end

    end

end
%%
idx = [7:9, 12:14];
for k=1:length(idx)
    f = figure(k+200); clf;
    f.Name = joint_names{idx(k)};
    set(f, 'WindowStyle', 'docked');

    ax = axes(f); %#ok<LAXES>
    hold(ax);

    for i = 1:N_vx
        for j = 1:N_vy
            t = t_all{i,j};
            q = q_all{i,j};
            dq = dq_all{i,j};
            py = py_all{i,j};
            px = px_all{i,j};
            vy = vy_all{i,j};
            vx = vx_all{i,j};
            plot3(ax, vx(N_mid), vy(N_mid), q(idx(k),N_mid),'*','MarkerSize',4);
%             scatter3(ax,t,vx,q(idx(k),:));
            [S, V] = meshgrid(-0.5:0.01:0.5, -1:0.02:1);
            L = zeros(size(S));
            for ii = 1:size(S, 1)
                for jj = 1:size(S, 2)
                    L(ii, jj) = P(k,:)*[1; S(ii, jj); V(ii, jj)];
                    %L(i, j) = l(n);
                end
            end
            surface(ax, S, V, L);
        end

    end

end

%%
idx = [7:9, 12:14];
X = zeros(2, N_vx*N_vy);
Y = zeros(length(idx), N_vx*N_vy);
dY = zeros(length(idx), N_vx*N_vy);
ii = 1;
for i = 1:N_vx
    for j = 1:N_vy
        t = t_all{i,j};
        q = q_all{i,j};
        dq = dq_all{i,j};

        %         X(:,ii) = [q(1:2,N_mid);dq(1:2,N_mid)];
        X(:,ii) = dq(1:2,N_mid);
        Y(:,ii) = q(idx,N_mid);
        dY(:,ii) = dq(idx,N_mid);
        ii = ii+1;
    end
end
% x = [ones(1,45); X];
x = [ones(1,81); X];
P = Y/x;
dP = dY/x;

%%
y1 = dY(4,:);
x1 = X(1,:);
x2 = X(2,:);
%%
0.0008997

0.06608

%% Save Training Data
save('Ayonga3DinsertionFunction', 'P','dP')

%%
% P = zeros(length(idx), 3);
% dP = zeros(length(idx), 3);
%
% for i=1:length(idx)
%
%     P(:,i) = Y(i,:)/X;
%     dP(:,i) = dY(i,:)/X;
% end

%%
f = figure(2);clf;
f.Name = 'Sideway Velocity Feature';
set(f, 'WindowStyle', 'docked');
ax = axes(f);
hold(ax);
for i = 1:N_vx
    t = t_all{i,5};
    dq = dq_all{i,5};
    plot(ax, t, dq(1,:));
    plot(ax, t(N_mid), dq(1,N_mid),'*','MarkerSize',4);
end
grid on
%%
f = figure(3);clf;
f.Name = 'Forward Velocity Feature';
set(f, 'WindowStyle', 'docked');
ax = axes(f);
hold(ax);
for j = 1:N_vy
    t = t_all{3,j};
    dq = dq_all{3,j};
    plot(ax, t, dq(2,:));
    plot(ax, t(N_mid), dq(2,N_mid),'*','MarkerSize',4);
end
grid on
%%
f = figure(4);clf;
f.Name = 'Sideway Position Feature';
set(f, 'WindowStyle', 'docked');
ax = axes(f);
hold(ax);
for i = 1:N_vx
    t = t_all{i,5};
    q = q_all{i,5};
    plot(ax, t, q(1,:));
    plot(ax, t(N_mid), q(1,N_mid),'*','MarkerSize',4);
end
grid on
%%
f = figure(5);clf;
f.Name = 'Forward Position Feature';
set(f, 'WindowStyle', 'docked');
ax = axes(f);
hold(ax);
for j = 1:N_vy
    t = t_all{3,j};
    q = q_all{3,j};
    plot(ax, t, q(2,:));
    plot(ax, t(N_mid), q(2,N_mid),'*','MarkerSize',4);
end
grid on

%%
f = figure(6);clf;
f.Name = 'Sideway Position Feature';
set(f, 'WindowStyle', 'docked');
ax = axes(f);
hold(ax);
for i = 1:N_vx
    q = q_all{i,5};
    dq = dq_all{i,5};
    plot(ax, q(1,:), dq(1,:));
    plot(ax, q(1,N_mid), dq(1,N_mid),'*','MarkerSize',4);
end
grid on
%%
f = figure(7);clf;
f.Name = 'Forward Position Feature';
set(f, 'WindowStyle', 'docked');
ax = axes(f);
hold(ax);
for j = 1:N_vy
    q = q_all{3,j};
    dq = dq_all{3,j};
    plot(ax, q(2,:), dq(2,:));
    plot(ax, q(2,N_mid), dq(2,N_mid),'*','MarkerSize',4);
end
grid on


classdef MUSIC < handle
    % Implements MUSIC-MMV algorithm for sparse approximation

    properties
        % Indicates if log messages should be printed at each iteration
        Verbose = false
    end

    properties(SetAccess=private)
        % The dictionary
        Dict
        % Ambient signal dimensions
        N
        % Number of atoms in dictionary
        D
        % Sparsity level of representations (may be negative)
        K
        % Result of a solver
        result
    end

    methods
        function self  = MUSIC(Dict, K)
            % We assume that all the columns in dictionary are normalized.
            if isa(Dict, 'spx.dict.Operator')
                self.Dict = Dict;
            elseif ismatrix(Dict)
                self.Dict = spx.dict.MatrixOperator(Dict);
            else
                error('Unsupported operator.');
            end
            [self.N, self.D] = size(Dict);
            if nargin < 2
                error('Sparsity level must be specified.');
            end
            self.K = K;
        end

        function result  = solve(self, Y)
            % Initialization
            % Solves approximation problem using OMP
            d = self.D;
            n = self.N;
            % The number of signals being approximated.
            s = size(Y, 2);
            dict = self.Dict;
            % Active indices
            omega = [];
            % Estimate
            Z = zeros(d, s);
            % Orthonormalize Y
            U = orth(Y); % Y is n x s. U is n x r.
            % Verify its rank
            k = self.K;
            r = rank(U);
            if r < k
                error('MUSIC principle cannot be directly applied.');
            end
            % Prepare the projection operator
            P = eye(n) - U*U'; % P is n x n
            % Compute the projection of each atom on
            % orthogonal complement of R(U)
            % compute the norms of projections
            projection_norms = zeros(1, d);
            for i=1:d
                projection_norms(i) = norm(P * (dict.column(i)) );
            end
            % sort the norms
            [sorted_norms, indices] = sort(projection_norms);
            % disp(sorted_norms);
            % Pick first K indices as the solution support.
            omega = indices(1:self.K);
            % Solve least squares problem
            subdict = columns(dict, omega);
            tmp = linsolve(subdict, Y);
            % Updated solution
            Z(omega, :) = tmp;
            % Let us update the residual.
            R = Y - dict.apply(Z);
            resNorm = norm(R, 'fro');
            % Solution vector
            result.Z = Z;
            % Residual obtained
            result.R = R;
            % Solution support
            result.support = omega;
            % residual Frobenius norm
            result.residual_frobenius_norm = resNorm;
            self.result = result;
        end
    end
end