function [T, M, S, DISTR, df] = dtiValTestStat(g1, g2, Y, mask)

% Computes voxel-wise statistics for two groups from a data array of
% diffusion tensors in dt6 format.
%
% The test is H0: both groups have the same eigenvalues, with possibly
% different unknown eigenvectors.
%
%   [T, M, S, DISTR, df] = dtiValTestStat(g1, g2, DT_ARRAY, [MASK])
%
% Input:
%   g1, g2      List of indices that correspond to each group out of 1:N
%                   E.g: g1 = 1:7, g2 = 8:14, N = 14
%   DT_ARRAY    Data array of size XxYxZx6xN (or nx6xN), where X, Y, Z are the volume
%                   dimensions and N is the number of subjects.
%                   (n is the number of voxels).
%   MASK        Optional XxYxZ binary array. Values of T, M and S are computed
%                   where mask = 1; in other voxels, T, M and S are set to 0.
%                   Default is entire volume.
%
% Output:
%   T           XxYxZx1 (or nx1) array of test statistics (0 where mask = 0)
%   M           XxYxZx6x2 (or nx6x2) array of mean tensors (0 where mask = 0)
%   S           XxYxZx1 (or nx1) array of variances (0 where mask = 0)
%   DISTR       'chi2' or 'f'
%   df          degrees of freedom of the appropriate distribution
%
% Utilities:    ndfun.m, dtiSplitTensor.m
%
% WARNING: If using Pentium 4, eliminate NaN's from array before running
% (processor bug).
%
% Copyright by Armin Schwartzman, 2005

% HISTORY:
%   2004.08.29 ASH (armins@stanford.edu) wrote it.
%

% Check inputs
if (ndims(Y)==2 | ndims(Y)==3),
    Ind = 1;    % Data in indexed nx6xN format
    Y = shiftdim(Y, -2);
else
    Ind = 0;    % Data in XxYxZx6xN format
end
if (ndims(Y)<4 | ndims(Y)>5),
    error('Wrong input format');
end
if (~exist('mask')),
    mask = ones([size(Y,1) size(Y,2) size(Y,3)]);
end

% Computations
N1 = length(g1);
N2 = length(g2);
N  = N1 + N2;

q = size(Y, 4);
p = max(roots([1/2 1/2 -q]));
Yavg1 = mean(Y(:,:,:,:,g1),5);
Yavg2 = mean(Y(:,:,:,:,g2),5);
Yavg = mean(Y(:,:,:,:,:),5);
M = cat(5, Yavg1, Yavg2);

[V1,L1] = dtiSplitTensor(Yavg1);
[V2,L2] = dtiSplitTensor(Yavg2);
[V,L] = dtiSplitTensor(Yavg);

% Total variance
d1 = Y(:,:,:,:,g1) - repmat(Yavg1,[1 1 1 1 N1]);
d2 = Y(:,:,:,:,g2) - repmat(Yavg2,[1 1 1 1 N2]);
S = sum(d1(:,:,:,1:p,:).^2, 4) + 2*sum(d1(:,:,:,p+1:q,:).^2, 4) + ...
    sum(d2(:,:,:,1:p,:).^2, 4) + 2*sum(d2(:,:,:,p+1:q,:).^2, 4);
S = sum(S, 5)/(q*(N-2));

% F version
DISTR = 'f';
df = [p, q*(N-2)];
T = N1*N2/N^2 * sum((L1 - L2).^2, 4);
T = df(2)/df(1) * T./(S*(N-2)/N) / q;

% Adjust output
if Ind,
    T = shiftdim(T, 2);
    M = shiftdim(M, 2);
    S = shiftdim(S, 2);
    L = shiftdim(L, 2);
end

return


function phiFaceAverage = tvdMean3D(phi, u, FL)
% This function gets the value of the field variable phi defined
% over the MeshStructure and calculates the upwind average on
% the cell faces, based on the direction of the velocity vector for a uniform mesh.
%
% SYNOPSIS:
%   phiFaceAverage = tvdMean3D(u, phi, FL)
%
% PARAMETERS:
%
%
% RETURNS:
%
%
% EXAMPLE:
%
% SEE ALSO:
%

% Written by Ali A. Eftekhari
% See the license file

% extract the velocity data
% note: size(ux) = [1:m+1, 1:n] and size(uy) = [1:m, 1:n+1]
ux = u.xvalue;
uy = u.yvalue;
uz = u.zvalue;

% check the size of the variable and the mesh dimension
Nx = u.domain.dims(1);
Ny = u.domain.dims(2);
Nz = u.domain.dims(3);
dx=repmat(0.5*(u.domain.cellsize.x(1:end-1)+u.domain.cellsize.x(2:end)), 1, Ny, Nz);
dy=repmat(0.5*(u.domain.cellsize.y(1:end-1)+u.domain.cellsize.y(2:end))', Nx, 1, Nz);
dz=zeros(1, 1, Nz+1);
dz(1,1,:)=0.5*(u.domain.cellsize.z(1:end-1)+u.domain.cellsize.z(2:end));
dz=repmat(dz, Nx, Ny, 1);

% define the tvd face vectors
phiX_p = zeros(Nx+1,Ny,Nz);
phiX_m = zeros(Nx+1,Ny,Nz);
phiY_p = zeros(Nx,Ny+1,Nz);
phiY_m = zeros(Nx,Ny+1,Nz);
phiZ_p = zeros(Nx,Ny,Nz+1);
phiZ_m = zeros(Nx,Ny,Nz+1);

% calculate the upstream to downstream gradient ratios for u>0 (+ ratio)
% x direction
dphiX_p = (phi.value(2:Nx+2, 2:Ny+1, 2:Nz+1)-phi.value(1:Nx+1, 2:Ny+1, 2:Nz+1))./dx;
rX_p = dphiX_p(1:end-1,:,:)./fsign(dphiX_p(2:end,:,:));
phiX_p(2:Nx+1,:,:) = phi.value(2:Nx+1, 2:Ny+1, 2:Nz+1)+0.5*FL(rX_p).* ...
    (phi.value(3:Nx+2,2:Ny+1,2:Nz+1)-phi.value(2:Nx+1,2:Ny+1,2:Nz+1));
phiX_p(1,:,:) = (phi.value(1,2:Ny+1,2:Nz+1)+phi.value(2,2:Ny+1,2:Nz+1))/2; % left boundary
% y direction
dphiY_p = (phi.value(2:Nx+1, 2:Ny+2, 2:Nz+1)-phi.value(2:Nx+1, 1:Ny+1, 2:Nz+1))./dy;
rY_p = dphiY_p(:,1:end-1,:)./fsign(dphiY_p(:,2:end,:));
phiY_p(:,2:Ny+1,:) = phi.value(2:Nx+1, 2:Ny+1, 2:Nz+1)+0.5*FL(rY_p).* ...
    (phi.value(2:Nx+1,3:Ny+2,2:Nz+1)-phi.value(2:Nx+1, 2:Ny+1,2:Nz+1));
phiY_p(:,1,:) = (phi.value(2:Nx+1,1,2:Nz+1)+phi.value(2:Nx+1,2,2:Nz+1))/2; % Bottom boundary
% z direction
dphiZ_p = (phi.value(2:Nx+1, 2:Ny+1, 2:Nz+2)-phi.value(2:Nx+1, 2:Ny+1, 1:Nz+1))./dz;
rZ_p = dphiZ_p(:,:,1:end-1)./fsign(dphiZ_p(:,:,2:end));
phiZ_p(:,:,2:Nz+1) = phi.value(2:Nx+1, 2:Ny+1, 2:Nz+1)+0.5*FL(rZ_p).* ...
    (phi.value(2:Nx+1,2:Ny+1,3:Nz+2)-phi.value(2:Nx+1,2:Ny+1,2:Nz+1));
phiZ_p(:,:,1) = (phi.value(2:Nx+1,2:Ny+1,1)+phi.value(2:Nx+1,2:Ny+1,2))/2; % Back boundary

% calculate the upstream to downstream gradient ratios for u<0 (- ratio)
% x direction
rX_m = dphiX_p(2:end,:,:)./fsign(dphiX_p(1:end-1,:,:));
phiX_m(1:Nx,:,:) = phi.value(2:Nx+1, 2:Ny+1, 2:Nz+1)+0.5*FL(rX_m).* ...
    (phi.value(1:Nx, 2:Ny+1, 2:Nz+1)-phi.value(2:Nx+1, 2:Ny+1, 2:Nz+1));
phiX_m(Nx+1,:,:) = (phi.value(end,2:Ny+1,2:Nz+1)+phi.value(end-1,2:Ny+1,2:Nz+1))/2; % right boundary
% y direction
rY_m = dphiY_p(:,2:end,:)./fsign(dphiY_p(:,1:end-1,:));
phiY_m(:,1:Ny,:) = phi.value(2:Nx+1,2:Ny+1,2:Nz+1)+0.5*FL(rY_m).* ...
    (phi.value(2:Nx+1,1:Ny,2:Nz+1)-phi.value(2:Nx+1,2:Ny+1,2:Nz+1));
phiY_m(:,Ny+1,:) = (phi.value(2:Nx+1, end,2:Nz+1)+phi.value(2:Nx+1, end-1,2:Nz+1))/2; % top boundary
% z direction
rZ_m = dphiZ_p(:,:,2:end)./fsign(dphiZ_p(:,:,1:end-1));
phiZ_m(:,:,1:Nz) = phi.value(2:Nx+1,2:Ny+1,2:Nz+1)+0.5*FL(rZ_m).* ...
    (phi.value(2:Nx+1,2:Ny+1,1:Nz)-phi.value(2:Nx+1,2:Ny+1,2:Nz+1));
phiZ_m(:,:,Nz+1) = (phi.value(2:Nx+1,2:Ny+1,end)+phi.value(2:Nx+1,2:Ny+1,end-1))/2; % front boundary

% calculate the average value
xvalue = (ux>0).*phiX_p+ ...
                        (ux<0).*phiX_m+ ...
                        0.5*(ux==0).*(phi.value(1:Nx+1,2:Ny+1,2:Nz+1)+phi.value(2:Nx+2,2:Ny+1,2:Nz+1));
yvalue = (uy>0).*phiY_p+ ...
                        (uy<0).*phiY_m+ ...
                        0.5*(uy==0).*(phi.value(2:Nx+1,1:Ny+1,2:Nz+1)+phi.value(2:Nx+1,2:Ny+2,2:Nz+1));
zvalue = (uz>0).*phiZ_p+ ...
                        (uz<0).*phiZ_m+ ...
                        0.5*(uz==0).*(phi.value(2:Nx+1,2:Ny+1,1:Nz+1)+phi.value(2:Nx+1,2:Ny+1,2:Nz+2));
phiFaceAverage=FaceVariable(phi.domain, xvalue, yvalue, zvalue);
end

function phi_out = fsign(phi_in)
% This function checks the value of phi_in and assigns an eps value to the
% elements that are less than or equal to zero, while keeping the signs of
% the nonzero elements
    phi_out = (abs(phi_in)>=eps).*phi_in+eps*(phi_in==0)+eps*(abs(phi_in)<eps).*sign(phi_in);
end


% Rotate position samples
%
% This function rotates provided position samples to specified angle.
%
%  USAGE
%   rpos = rotatePositions(pos, degAngle)
%   pos         Matrix with position samples.
%   degAngle    Rotation angle in degrees.
%   rpos        Rotated position samples. Same dimension as pos.
%
function rpos = rotatePositions(pos, degAngle)
    rpos(:, 2) = pos(:, 2)*cosd(degAngle) - pos(:, 3)*sind(degAngle);
    rpos(:, 3) = pos(:, 2)*sind(degAngle) + pos(:, 3)*cosd(degAngle);
    if size(pos, 2) > 3
        rpos(:, 4) = pos(:, 4)*cosd(degAngle) - pos(:, 5)*sind(degAngle);
        rpos(:, 5) = pos(:, 4)*sind(degAngle) + pos(:, 5)*cosd(degAngle);
    end
end


%MDL_FANUC10L  Create kinematic model of Fanuc AM120iB/10L robot
%
% MDL_FANUC10L is a script that creates the workspace variable R which
% describes the kinematic characteristics of a Fanuc AM120iB/10L robot
% using standard DH conventions.
%
% Also defines the workspace vector:
%   q0   mastering position.
%
% Notes::
% - SI units of metres are used.
%
% Author::
%  Wynand Swart,
%  Mega Robots CC, P/O Box 8412, Pretoria, 0001, South Africa
%  wynand.swart@gmail.com
%
% See also SerialLink, mdl_irb140, mdl_m16, mdl_motomanHP6, mdl_puma560.

% MODEL: Fanuc, AM120iB/10L, 6DOF, standard_DH

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

%Cell: 073-1555-430
%30 Sep 2007
%Fanuc AM120iB/10L robot

%            theta    d      a    alpha
clear L
L(1) = Link([0        0     0.15  -pi/2   0  ]);
L(2) = Link([0        0     0.77   pi     0  ]);
L(3) = Link([0        0     0.1   -pi/2   0  ]);
L(4) = Link([0       -0.96  0      pi/2   0  ]);
L(5) = Link([0        0     0     -pi/2   0  ]);
L(6) = Link([0       -0.1   0      0      0  ]);
%##########################################################
%Pose 0; At MASTERING position;
%##########################################################
q0 =[0   -pi/2   0   0   0   0];
R=SerialLink(L, 'name', 'Fanuc AM120iB/10L');
%##########################################################


function [ a, seed ] = r83s_random ( n, seed )

%*****************************************************************************80
%
%% R83S_RANDOM randomizes an R83S matrix.
%
%  Discussion:
%
%    The R83S storage format is used for a tridiagonal scalar matrix.
%    The vector A(3) contains the subdiagonal, diagonal, and superdiagonal
%    values that occur on every row.
%
%  Example:
%
%    Here is how an R83S matrix of order 5, stored as (A1,A2,A3), would
%    be interpreted:
%
%      A2  A3   0   0   0
%      A1  A2  A3   0   0
%       0  A1  A2  A3   0
%       0   0  A1  A2  A3
%       0   0   0  A1  A2
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 July 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the linear system.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real A(3), the R83 matrix.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  [ a(1:3), seed ] = r8vec_uniform_01 ( 3, seed );

  return
end


function [z,output] = minf_lbfgsdl(f,g,z0,options)
%MINF_LBFGSDL Minimize a function by L-BFGS with dogleg trust region.
%   [z,output] = minf_lbfgsdl(f,g,z0) starts at z0 and attempts to find a
%   local minimizer of the real-valued function f(z). The input variables z
%   may be a scalar, vector, matrix, tensor or even a (nested) cell array
%   of tensors and its contents may be real or complex.
%
%   If f(x) is a function of real variables x, the function g(x) should
%   compute the partial derivatives of f with respect to the real variables
%   x, i.e. g(xk) := df(xk)/dx. If f(z) is a function of complex variables
%   z, the function g(z) should compute two times the partial derivative
%   of f with respect to conj(z) (treating z as constant), i.e. g(zk) :=
%   2*df(zk)/d(conj(z)) = 2*conj(df(zk)/dz). If g is the empty matrix [],
%   the real gradient or scaled conjugate cogradient is approximated with
%   finite differences. The output of the function g(z) may have the same
%   structure as z (although this is not necessary). The structure output
%   returns additional information:
%
%      output.alpha      - The plane search step lengths in every
%                          iteration, if a plane search is selected.
%      output.delta      - The trust region radius at every step attempt.
%      output.fevals     - The total number of function calls.
%      output.fval       - The value of the objective function f in every
%                          iteration.
%      output.gevals     - The total number of gradient calls.
%      output.info       - The circumstances under which the procedure
%                          terminated:
%                             1: Objective function tolerance reached.
%                             2: Step size tolerance reached.
%                             3: Maximum number of iterations reached.
%      output.infops     - The circumstances under which the plane search
%                          terminated in every iteration.
%      output.iterations - The number of iterations.
%      output.relfval    - The difference in objective function value
%                          between every two successive iterates, relative
%                          to its initial value.
%      output.relstep    - The step size relative to the norm of the
%                          current iterate in every iteration.
%      output.rho        - The trustworthiness at every step attempt.
%
%   minf_lbfgsdl(f,g,z0,options) may be used to set the following options:
%
%      options.Delta =            - The initial trust region radius.
%      0.3*max(1,norm(z0))
%      options.Display = 10       - Displays the objective function value,
%                                   its difference with the previous
%                                   iterate relative to the first iterate
%                                   and the relative step size each
%                                   options.Display iterations. Set to 0 to
%                                   disable.
%      options.M =                - The number of updates to store.
%      min(30,length(z0))
%      options.MaxIter = 500      - The maximum number of iterations.
%      options.PlaneSearch        - The plane search used to minimize the
%      = false                      objective function in the plane spanned
%                                   by the steepest descent direction and
%                                   the Gauss-Newton step. Disables dogleg
%                                   trust region strategy. The method
%                                   should have the function signature
%                                   options.PlaneSearch(F,dF,z,p1,p2, ...
%                                   state,options.PlaneSearchOptions).
%      options.PlaneSearchOptions - The options structure passed to the
%                                   plane search search routine.
%      options.TolFun = 1e-6      - The tolerance for output.relfval.
%      options.TolX = 1e-8        - The tolerance for output.relstep.

%   Authors: Laurent Sorber (Laurent.Sorber@cs.kuleuven.be)
%            Marc Van Barel (Marc.VanBarel@cs.kuleuven.be)
%            Lieven De Lathauwer (Lieven.DeLathauwer@kuleuven-kulak.be)
%
%   References:
%   [1] L. Sorber, M. Van Barel, L. De Lathauwer, "Unconstrained
%       optimization of real functions in complex variables", SIAM J. Opt.,
%       Vol. 22, No. 3, 2012, pp. 879-898.

% Store the structure of the input space and evaluate the gradient.
dim = structure(z0);
fval = f(z0);
if ~isa(g,'function_handle') && isempty(g)
    grad = serialize(deriv(f,z0,fval));
else
    grad = serialize(g(z0));
end
z0 = serialize(z0);

% Check the options structure.
if nargin < 4, options = struct; end
if ~isfield(options,'Delta'), options.Delta = 0.3*max(1,norm(z0)); end
if ~isfield(options,'Display'), options.Display = 10; end
if ~isfield(options,'M'), options.M = min(30,length(z0)); end
if ~isfield(options,'MaxIter'), options.MaxIter = 500; end
if ~isfield(options,'PlaneSearch'), options.PlaneSearch = false; end
if ~isfield(options,'PlaneSearchOptions')
    options.PlaneSearchOptions = struct;
end
if ~isfield(options,'TolFun'), options.TolFun = 1e-6; end
if ~isfield(options,'TolX'), options.TolX = 1e-8; end

% Initialize the algorithm.
S = zeros(numel(z0),options.M);
Y = zeros(numel(z0),options.M);
a = zeros(1,options.M);
r = zeros(1,options.M);
m = 0;
midx = 0;

% L-BFGS with dogleg trust region.
output.alpha = [];
output.delta = options.Delta;
output.fevals = 1;
output.fval = fval;
output.gevals = 1;
output.info = false;
output.infops = [];
output.iterations = 0;
output.relfval = [];
output.relstep = [];
output.rho = [];
while ~output.info

    % Compute the quasi-Newton step pqn = -H*grad.
    pqn = -grad;
    for i = 1:m
        a(i) = r(midx(i))*real(S(:,midx(i))'*pqn);
        pqn = pqn-a(i)*Y(:,midx(i));
    end
    if m > 0
        y1 = Y(:,midx(1));
        y1y1 = y1'*y1;
        gamma = y1y1*r(midx(1));
        pqn = 1/gamma*pqn;
    end
    for i = m:-1:1
        b = r(midx(i))*real(Y(:,midx(i))'*pqn);
        pqn = pqn+(a(i)-b)*S(:,midx(i));
    end

    % Approximate the Cauchy point pcp = -alpha*grad, where alpha is equal
    % to arg min m(-alpha*grad) and m(p) is a second order model of f at z.
    gg = grad'*grad;
    if m == 0
        alpha = 1;
    else
        s1 = S(:,midx(1));
        gBg = gg-real(grad'*s1)^2/(s1'*s1)+real(grad'*y1)^2/y1y1;
        gBg = gamma*gBg;
        alpha = gg/gBg;
    end

    % Plane search in the plane spanned by {pqn,pcp}.
    if isa(options.PlaneSearch,'function_handle')
        if output.iterations == 0, pcp = zeros(size(grad));
        else pcp = -alpha*grad; end
        state = output; state.grad = grad;
        [alpha,outputps] = options.PlaneSearch( ...
            f,g,deserialize(z0,dim),deserialize(pqn,dim), ...
            deserialize(pcp,dim),state,options.PlaneSearchOptions);
        output.alpha(:,end+1) = alpha;
        if length(alpha) < 3, alpha(3) = 1; end
        p = alpha(1)*pqn+alpha(2)*pcp;
        z = deserialize(alpha(3)*(z0+p),dim);
        relstep = norm(p)/norm(z0); if isnan(relstep), relstep = 0; end
        if isfield(outputps,'fval'), fval = outputps.fval;
        else fval = f(z); end
        if isfield(outputps,'info')
            output.infops(end+1) = outputps.info;
        end
        rho = 1;
    else
        rho = -inf;
    end

    % Dogleg trust region.
    normpqn = norm(pqn);
    while rho <= 0

        % Compute the dogleg step p.
        delta = output.delta(end);
        if normpqn <= delta
            p = pqn;
            dfval = -0.5*real(grad'*pqn);
        elseif abs(alpha)*sqrt(gg) >= delta
            p = (-delta/sqrt(gg))*grad;
            dfval = delta*(sqrt(gg)-0.5*delta/alpha);
        else
            bma = pqn+alpha*grad; bmabma = bma'*bma;
            a = -alpha*grad; aa = alpha^2*gg;
            c = real(a'*bma);
            if c <= 0
                beta = (-c+sqrt(c^2+bmabma*(delta^2-aa)))/bmabma;
            else
                beta = (delta^2-aa)/(c+sqrt(c^2+bmabma*(delta^2-aa)));
            end
            p = a+beta*bma;
            dfval = 0.5*alpha*(1-beta)^2*gg- ...
                    0.5*beta *(2-beta)*real(grad'*pqn);
        end

        % Compute the trustworthiness rho.
        if dfval > 0
            z = deserialize(z0+p,dim);
            fval = f(z);
            rho = (output.fval(end)-fval)/dfval;
            if isnan(rho), rho = -inf; end
            output.rho(end+1) = rho;
            output.fevals = output.fevals+1;
        end

        % Update trust region radius delta.
        if rho > 0.5
            output.delta(end+1) = max(delta,2*norm(p));
        else
            sigma = (1-0.25)/(1+exp(-14*(rho-0.25)))+0.25;
            if normpqn < sigma*delta && rho < 0
                e = ceil(log2(normpqn/delta)/log2(sigma));
                output.delta(end+1) = sigma^e*delta;
            else
                output.delta(end+1) = sigma*delta;
            end
        end

        % Check for convergence.
        relstep = norm(p)/norm(z0); if isnan(relstep), relstep = 0; end
        if rho <= 0 && relstep <= options.TolX
            output.rho(end+1) = rho;
            fval = output.fval(end);
            z = deserialize(z0,dim);
            break;
        end

    end

    % Save current state.
    if rho > 0
        z0 = serialize(z);
        grad1 = grad;
    end

    % Evaluate the gradient and update step information.
    if rho > 0
        if isa(options.PlaneSearch,'function_handle') && ...
           output.iterations >= 1 && isfield(outputps,'grad')
            grad = outputps.grad;
        elseif ~isa(g,'function_handle') && isempty(g)
            grad = serialize(deriv(f,z,fval));
        else
            grad = serialize(g(z));
        end
        s = p;
        y = grad-grad1;
        sy = real(y'*s);
        if sy > 0
            m = min(m+1,options.M);
            midx = [midx(1)+1:-1:1,m:-1:midx(1)];
            S(:,midx(1)) = s;
            Y(:,midx(1)) = y;
            r(:,midx(1)) = 1/sy;
        end
    end

    % Update the output structure.
    output.fval(end+1) = fval;
    output.gevals = output.gevals+1;
    output.iterations = output.iterations+1;
    output.relfval(end+1) = ...
        abs(diff(output.fval(end:-1:end-1)))/abs(output.fval(1));
    output.relstep(end+1) = relstep;
    if output.relfval(end) <= options.TolFun, output.info = 1; end
    if output.relstep(end) <= options.TolX, output.info = 2; end
    if output.iterations >= options.MaxIter, output.info = 3; end

    % Display progress.
    if options.Display > 0 && (output.iterations == 1 || output.info || ...
       mod(output.iterations,options.Display) == 0)
        if output.iterations == 1
            bold = '%s';
            [~,~,~,~,v] = regexp(version('-release'),'([0-9]+)([ab])');
            if usejava('Desktop') && str2double(v{1}{1}) > 2011 || ...
               (str2double(v{1}{1}) == 2011 && strcmpi(v{1}{2},'b'))
                bold = '%s';
            end
        end
        if output.iterations == 1 || ...
           mod(output.iterations,15*options.Display) == 0
            fprintf('\n%7s%s','',sprintf(bold,'fval'));
            fprintf('%13s%s','',sprintf(bold,'relfval'));
            fprintf('%10s%s','',sprintf(bold,'relstep'));
            if isa(options.PlaneSearch,'function_handle')
                fprintf('%10s%s','',sprintf(bold,'alpha'));
            else
                fprintf('%10s%s','',sprintf(bold,'delta'));
                fprintf('%8s%s','',sprintf(bold,'rho'));
            end
            fprintf('\n%21s%9s = %4.e %6s = %4.e\n\n','=1/2*norm(F)^2', ...
                    'TolFun',options.TolFun,'TolX',options.TolX);
        end
        if output.iterations == 1
            fprintf('%4i: % 14.8e |\n',0,output.fval(1));
        end
        if isa(options.PlaneSearch,'function_handle')
            stralpha = [repmat('%10.4e ',1,size(output.alpha,1)) '\n'];
            fprintf(['%4i: % 14.8e | %14.8e | %14.8e | ' stralpha], ...
                    output.iterations,output.fval(end), ...
                    output.relfval(end),output.relstep(end), ...
                    abs(output.alpha(:,end)));
        else
            fprintf(['%4i: % 14.8e | %14.8e | %14.8e | ' ...
                     '%10.4e | %10.4e\n'],...
                    output.iterations,output.fval(end), ...
                    output.relfval(end),output.relstep(end), ...
                    output.delta(end),output.rho(end));
        end
    end

end

% Display termination message.
if options.Display > 0
    ahref = '\n%s\n\n';
    x = round(linspace(0,output.iterations,min(500,output.iterations)));
    if length(bold) > 2
        ahref = sprintf(['\n%%s\n\n'],mat2str(x'), ...
            mat2str([output.fval(x+1)' [nan output.relfval(x(2:end))]' ...
                    [nan output.relstep(x(2:end))]'],3));
    end
    switch output.info
        case 1, fprintf(ahref,'Objective function tolerance reached.');
        case 2, fprintf(ahref,'Step size tolerance reached.');
        case 3, fprintf(ahref,'Maximum number of iterations reached.');
    end
end

end

function [z,offset] = deserialize(z,dim,offset)
    if iscell(dim)
        v = z;
        z = cell(size(dim));
        if nargin < 3, offset = 0; end
        for i = 1:numel(z)
            if iscell(dim{i})
                [z{i},offset] = deserialize(v,dim{i},offset);
            else
                n = prod(dim{i}(:));
                z{i} = reshape(v(offset+(1:n)),dim{i});
                offset = offset+n;
            end
        end
    elseif ~isempty(dim)
        z = reshape(z,dim);
    end
end

function z = serialize(z)
    if iscell(z)
        for i = find(cellfun(@iscell,z(:).'))
            z{i} = serialize(z{i});
        end
        s = cellfun(@numel,z(:)); o = [0; cumsum(s)];
        c = z; z = zeros(o(end),1);
        for i = 1:length(s), z(o(i)+(1:s(i))) = c{i}(:); end
    else
        z = z(:);
    end
end

function dim = structure(z)
    if iscell(z)
        dim = cellfun(@size,z,'UniformOutput',false);
        for i = find(cellfun(@iscell,z(:).'))
            dim{i} = structure(z{i});
        end
    else
        dim = size(z);
        if numel(z) == dim(1), dim = []; end
    end
end


function Deff = electrolyteDiffusionCoefficients(ce,T,param,batterySection)
% electrolyteDiffusionCoefficients  Evaluates the diffusion coefficients for the electrolyte phase [m^2/s].
%
%   [Deff_p, Deff_s, Deff_n] = electrolyteDiffusionCoefficients(ce,T,param) evaluates
%   the diffusion coefficients for the anode, separator and cathode of the
%   battery.
%
%   Note that this is an interface for the main program. The authors
%   suggest to maintain the name of the function and its signature, while
%   modifying only the body of the script.
%
%   The diffusion coefficients can be evaluated in isothermal case
%   (param.TemperatureEnabled=0) or adiabatic case
%   (param.TemperatureEnabled=1 or 2).
%
%   You can modify the way that the diffusion coefficients are computed, as
%   function of electrolyte concentration and temperature. The main script
%   will pass also the param array.
%
%   The user can modify this script to meet particular requirements.

%   This file is part of the LIONSIMBA Toolbox
%
%	Official web-site: 	http://sisdin.unipv.it/labsisdin/lionsimba.php
% 	Official GitHUB: 	https://github.com/lionsimbatoolbox/LIONSIMBA
%
%   LIONSIMBA: A Matlab framework based on a finite volume model suitable for Li-ion battery design, simulation, and control
%   Copyright (C) 2016-2018 :Marcello Torchio, Lalo Magni, Davide Raimondo,
%                            University of Pavia, 27100, Pavia, Italy
%                            Bhushan Gopaluni, Univ. of British Columbia,
%                            Vancouver, BC V6T 1Z3, Canada
%                            Richard D. Braatz,
%                            Massachusetts Institute of Technology,
%                            Cambridge, Massachusetts 02142, USA
%
%   Main code contributors to LIONSIMBA 2.0:
%                           Ian Campbell, Krishnakumar Gopalakrishnan,
%                           Imperial college London, London, UK
%
%   LIONSIMBA is a free Matlab-based software distributed with an MIT
%   license.

if(param.TemperatureEnabled>=1)
    switch(batterySection)
        case 'p'
            Deff = param.eps_p^param.brugg_p*1e-4*10.^((-4.43-54./(T-229-5e-3*ce)-0.22e-3*ce));
        case 's'
            Deff = param.eps_s^param.brugg_s*1e-4*10.^((-4.43-54./(T-229-5e-3*ce)-0.22e-3*ce));
        case 'n'
            Deff = param.eps_n^param.brugg_n*1e-4*10.^((-4.43-54./(T-229-5e-3*ce)-0.22e-3*ce));
    end
else
    switch(batterySection)
        case 'p'
            Deff = repmat(param.Dp*param.eps_p^param.brugg_p,param.Np,1);
        case 's'
            Deff = repmat(param.Ds*param.eps_s^param.brugg_s,param.Ns,1);
        case 'n'
            Deff = repmat(param.Dn*param.eps_n^param.brugg_n,param.Nn,1);
    end
end

end


% GP_COV_KRON - Covariance function as a product of two covariance
%                  functions.
%
% Usage:
%
%   COVFUNC = GP_COV_PRODUCT(COVFUNC1, COVFUNC2, ...)
%
% The returned covariance function is called as
%
%   K = COVFUNC(THETA)
%   [K, DK] = COVFUNC(THETA)
%
% where THETA is a vector of parameters. The parameters of COVFUNC1 and
% COVFUNC2 are concatenated into a single parameter vector.
%
% If no arguments are given, dimensionality information is returned:
%
%   [N_THETA, N1, N2] = COVFUNC()
%
% N_THETA : Number of non-fixed parameters
% N1      : Number of rows in the covariance matrix
% N2      : Number of columns in the covariance matrix
%
% See also GP_COV, GP_COV_SUM.

% Last modified 2012-03-14
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@aalto.fi)

function func = gp_cov_kron(varargin)

if nargin < 2
  error('Must give at least two covariance functions')
end

func = @get_covariance;

covfuncs = varargin ;

% Number of covariance functions
n_funcs = nargin ;

% Number of parameters for each covariance function
n_theta = zeros(n_funcs,1) ;
% Dimensionalities of each covariance function
M = zeros(n_funcs,1) ;
N = zeros(n_funcs,1) ;
% Extract the values from the covariance functions
for i = 1:n_funcs
  [n_theta(i), M(i), N(i)] = varargin{i}();
end

% Indices of the hyperparameters for each covariance function
ind_theta = cell(n_funcs,1);
for i = 1:n_funcs
  ind_theta{i} = (1+sum(n_theta(1:(i-1)))):(sum(n_theta(1:i))) ;
end

%[n_theta2, M2, N2] = covfunc2();

%if M1 ~= M2 || N1 ~= N2
%  error('Can''t multiply covariance matrices with different dimensionalities');
%end

  function varargout = get_covariance(theta)

  if nargout == 0
    nout = 1;
  else
    nout = nargout;
  end

  varargout = cell(nout,1);

  out = cell(nout,1);
  %out2 = cell(nout,1);
  varargout = cell(nout,1);

  % Return only dimension information if requested
  if nargin == 0
    if nout >= 1
      varargout{1} = sum(n_theta); % number of parameters
      if nout >= 2
        varargout{2} = prod(M) ; % dimensionalities
        if nout >= 3
          varargout{3} = prod(N); % dimensionalities
        end
      end
    end
    return
  end

  if numel(theta) ~= sum(n_theta)
    error('Wrong number of parameters (%d), should be %d', numel(theta), ...
          sum(n_theta));
  end

  % Initialize covariance matrix and gradients
  varargout{1} = 1 ;
  if nout >= 2
    varargout{2} = cell(sum(n_theta),1) ;
    for i = 1:sum(n_theta)
      varargout{2}{i} = 1 ;
    end
  end

  % Compute the covariance matrix
  for i = 1:n_funcs
    [out{:}] = covfuncs{i}(theta(ind_theta{i}));
    varargout{1} = kron(varargout{1}, out{1});

    % Compute the derivative
    if nout >= 2 && ~isempty(ind_theta{i})
      % No derivative for these hyperparameters
      for n = 1:(ind_theta{i}(1)-1)
        varargout{2}{n} = kron(varargout{2}{n}, out{1}) ;
      end
      % Derivative for these hyperparameters
      for n = 1:n_theta(i)
        ind = ind_theta{i}(1) + n - 1 ;
        varargout{2}{ind} = kron(varargout{2}{ind}, out{2}{n}) ;
      end
      % No derivative for these hyperparameters
      for n = (ind_theta{i}(end)+1):sum(n_theta)
        varargout{2}{n} = kron(varargout{2}{n}, out{1}) ;
      end
    end
  end

  end

end

%function X = kronecker(varargin)

%X = varargin{1} ;
%for i = 2:nargin
%  X = kron(X, varargin{i}) ;
%end


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 [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


%        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 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


%==============================================================================
% 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(:));


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


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


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


function out = tn(FUN,x0,varargin)
%TN   Truncated Newton minimization.
%
%   OUT = TN(FUN,X0) minimizes FUN starting at the point X0 using a
%   Hessian-free truncated Newton method. The outer loop is Newton's
%   method, and the inner loop uses a conjugate gradient method to compute
%   an approximation to the Newton direction. Furthermore, the Hessian
%   vector product is approximated using forward finite differences using
%   HESSVEC_FD. FUN is a handle for a function that takes a single vector
%   input and returns two arguments --- the scalar function value and the
%   vector-valued gradient. See POBLANO_OUT for details of the output
%   parameters.
%
%   OUT = TN(FUN,X0,'param',value,...) specifies a
%   parameters and its value. See POBLANO_PARAMS for further details on
%   standard parameters. Additionally, TN requires
%
%   'CGSolver' - Matlab CG method to use {'symmlq'}
%     'symmlq' : symmlq (designed for symmetric indefinite systems)
%     'pcg' :    pcg (designed for symmetric positive definite systems)
%
%   'CGIters' - maximum number of conjugate gradient iterations allowed {5}
%
%   'CGTolType' - CG stopping tolerance type used ('quadratic')
%     'quadratic' :    || R || / || G || <  min(0.5,|| G ||)
%     'superlinear' :  || R || / || G || <  min(0.5,sqrt(|| G ||))
%     'fixed' :        || R || < CGTol
%   where R is the residual and G is the gradient of FUN at X.
%
%   'CGTol' - CG stopping tolerance when CGTolType is 'fixed' {1e-6}
%
%   'HessVecFDStep' - Hessian vector product finite difference step {1e-10}
%     0 - use the default step given in HESSVEC_FD
%     >0 - fixed value to use at the difference step
%
%   PARAMS = TN('defaults') returns a structure containing the
%   default parameters for the particular Poblano method.
%
%
%   Examples
%
%   Suppose the function and gradient of the objective function are
%   specified in an mfile named mysin.m:
%
%     function [f,g]=example1(x,a)
%     if nargin < 2, a = 1; end
%     f = sin(a*x);
%     g = a*cos(a*x);
%
%   We can call the optimization method (using its default
%   parameters) using the command:
%
%     out = tn(@(x) example1(x,3), pi/4);
%
%   To change a parameter, we can specify a param/value input pair
%   as follows:
%
%     out = tn(@(x) example1(x,3), pi/4, 'Display', 'final');
%
%   Alternatively, we can use a structure to define the parameters:
%
%     params.MaxIters = 2;
%     out = tn(@(x) example1(x,3), pi/4, params);
%
%   See also POBLANO_OUT, POBLANO_PARAMS, HESSVEC_FD, FUNCTION_HANDLE.
%
%MATLAB Poblano Toolbox.
%Copyright 2009-2012, Sandia Corporation.

%% Parse parameters

% Create parser
params = inputParser;

% Set Poblano parameters
params = poblano_params(params);

% Set parameters for this method
params.addParamValue('CGIters',5,@(x) x > 0);
params.addParamValue('CGTolType','quadratic',@(x) ismember(x,{'quadratic','superlinear','fixed'}));
params.addParamValue('CGTol',1e-6, @(x) x > 0);
params.addParamValue('HessVecFDStep',1e-10, @(x) x >= 0);
params.addParamValue('CGSolver','symmlq',@(x) ismember(x,{'symmlq','pcg'}));

% Parse input
params.parse(varargin{:});

%% Check input arguments
if (nargin == 1) && isequal(FUN,'defaults') && (nargout == 1)
    out = params.Results;
    return;
elseif (nargin < 2)
    error('Error: invalid input arguments');
end

%% Initialize
xk = x0;
[fk,gk] = feval(FUN,xk);
out = poblano_out(xk,fk,gk,1,params);
ak = 1.0;

cgIters = params.Results.CGIters;
tolType = params.Results.CGTolType;
cgTol = params.Results.CGTol;
hessvecFDstep = params.Results.HessVecFDStep;
solver = params.Results.CGSolver;

%% Main loop
while out.ExitFlag == -1

    % Compute step direction, pk
    ngk = norm(gk);
    switch tolType
        case 'quadratic'
            cgTol = min(1-eps,min(0.5,ngk)*ngk);       % quadratic convergence
        case 'superlinear'
            cgTol = min(1-eps,min(0.5,sqrt(ngk))*ngk); % superlinear convergence
        otherwise
    end

    % Setup Hessian vector product finite difference approximation
    if hessvecFDstep > 0
        hv = @(v) hessvec_fd(v,FUN,xk,gk,hessvecFDstep);
    else
        hv = @(v) hessvec_fd(v,FUN,xk,gk);
    end

    % Keep track of number of function calls are made using hessvec_fd
    global nfev_hessvec_fd;
    nfev_hessvec_fd = 0;

    % Compute step direction, pk
    [pk,cg_flag,cg_relres,ncgfev]= feval(solver, hv, -gk, cgTol, cgIters);

    % Compute step length
    [xk,fk,gk,ak,lsinfo,nfev] = poblano_linesearch(FUN,xk,fk,gk,ak,pk,params.Results);
    if (lsinfo ~= 1)
        if strcmp(params.Results.Display, 'iter')
            fprintf(1,[mfilename,': line search warning = %d\n'],lsinfo);
        end
        pk = -gk;
        [xk,fk,gk,ak,lsinfo,nfev] = poblano_linesearch(FUN,xk,fk,gk,ak,pk,params.Results);
    end

    % Update counts, check exit conditions, etc.
    out = poblano_out(xk,fk,gk,nfev+nfev_hessvec_fd,params,out);
end

clear nfev_hessvec_fd;


% mri_superres.m
% NOT DONE!
%
% Explore super-resolution in MRI with rotated propeller sequences
% Inspired by Benoit Desjardins' work.
% Copyright 2003-7-23, Jeff Fessler, The University of Michigan

%
% create k-space samples, in units of 1/mm
%
if ~isvar('kspaces'), printm 'setup kspace'
	fov = 256;	% [mm] typical brain FOV
	N0 = 32;	% nominal image size
	kmax = N0/2*(1/fov);	% for display axes

	k1 = ([-N0/2:N0/2-1]+0.0)/fov;
	[kk1, kk2] = ndgrid(k1, k1);
	kcart = [kk1(:), kk2(:)];

	Nprops = [1 5];	% # of propellers

	for il=1:length(Nprops)
		Nprop = Nprops(il);

		kspace = zeros(Nprop*N0^2, 2);
		for ip=0:Nprop-1
			ang = ip/Nprop * pi;
			rot = [cos(ang) sin(ang); -sin(ang) cos(ang)];
			kspace(ip*N0^2+[1:N0^2],:) = kcart * rot;
			if ip > 0, % avoid repeated DC sampling
				kspace((N0/2+1)^2-1,:) = [];
			end
		end

		kspaces{il} = kspace;
	end, clear ang rot ip il k1 kk1 kk2 kcart

	im plc 2 3
	im('subplot', 1)
	if im
		plot(kspace(:,1), kspace(:,2), '.')
		axis(1.1*[-1 1 -1 1]*kmax), axis square
		xlabel 'k_1 [mm^{-1}]', ylabel 'k_2 [mm^{-1}]'
		title(sprintf('%d k-space samples (%d)', size(kspace,1), Nprop))
	end

	clear kmax kspace
prompt
end

%
% true object
%
if 0 || ~isvar('xtrue'), printm 'setup object'

	% display images with many pixels...
	Ndisp = 256;
	x1d = [-Ndisp/2:Ndisp/2-1] / Ndisp * fov;
	[x1dd x2dd] = ndgrid(x1d, x1d);

	% parameter units all in [mm]
	obj = mri_objects('case1');
	xtrue = obj.image(x1dd, x2dd);

	clim = [0 2];
	im(2, x1d, x1d, xtrue, 'x true', clim), cbar

	clear x1dd x2dd xpar
prompt
end

%
% analytical k-space data
%
if 0 || ~isvar('yd'), printm 'data'

	% noiseless data
	rng(0)
	for ik=1:length(Nprops)
		kspace = kspaces{ik};
		ytrue{ik} = obj.kspace(kspace(:,1), kspace(:,2));
		yd{ik} = ytrue{ik} + 0 * randn(size(ytrue{ik}));
	end
	clear x1dd x2dd xpar kspace ytrue

	[xhatg, yd_g, xg, kg] = mri_grid_linear(kspaces{1}, yd{1}, N0, fov);
	im(3, kg{1}, kg{2}, abs(yd_g), '|y_d|'), cbar

	clear kg yd_g xhatg ik kspace
prompt
end

return

%
% iterative recon
%
if ~isvar('xpcg')

Nlist = [2.^[6:6]];
niter = 20;
beta = 2^-10 * size(kspace,1);
xpcg = {};

for in=1:length(Nlist)
	N = Nlist(in);

	% gridding estimate to initialize iterations
	[xhatg, yhatg, xg] = mri_grid_linear(kspace, yd, N, fov);
	tmp = sprintf('|x| gridding %d', N);
	im(5, xg{1}, xg{2}, abs(xhatg), tmp, clim), cbar

	%
	% create Gnufft class object
	%
	if 1 || ~isvar('G'), printm 'setup Gnufft object'
		omega = 2*pi*kspace*fov/N;
%		minmax(omega(:,1))
		G = Gnufft({omega, [N N], [6 6], 2*[N N], [N/2 N/2]});
	end

	%
	% reconstruct by PCG
	%
	if 1 || ~isvar('xpcg'), printm 'PCG with quadratic penalty'
		mask = true(N);
		R = Robject(mask, 'beta', beta);
		ytmp = yd(:) * (1/fov)^2 * N^2;	% scaling!
		xiter = qpwls_pcg(xhatg(:), G, 1, ytmp, 0, R.C, 1, niter);
		xpcg{in} = embed(xiter(:,end), mask);
		im(6, xg{1}, xg{2}, abs(xpcg{in}), '|x| pcg quad'), cbar
	end
end % fof
end % if

return

if 0
	im plc 2 3
	im(1, x1d, x1d, xtrue, clim, 'x true'), cbar
	xtick([-fov/2 0 fov/2-1])
	ytick([-fov/2 0 fov/2-1])
	for in=1:length(Nlist)
		x1g = [-N/2:N/2-1]'/N * fov;
		tmp = sprintf('N=%d', Nlist(in));
		im(in+1, x1g, x1g, abs(xpcg{in}), tmp, clim), cbar
		xtick([-fov/2 0 fov/2-1])
		ytick([-fov/2 0 fov/2-1])
	end
%	ir_savefig fig_mri_pixel_size_image
return
end

if 1
	clf
	h(1) = plot(x1d, xtrue(:,Ndisp/2+1), 'c-');
	hold on
	llist = {'r:', 'y--', 'g-.', 'r.', 'm-'};
	arg = {'true'};
	for in=1:length(Nlist)
		N = Nlist(in);
		x1g = [-N/2:N/2-1]'/N * fov;
		tmp = xpcg{in};
		h(in+1) = plot(x1g, abs(tmp(:,N/2+1)), llist{in});
		arg = {arg{:}, sprintf('N=%d', N)};
	end
	hold off
	axis([-fov/2 fov/2 0 2.1])
	xlabel 'horizontal position [mm]'
	ylabel '|f(x,0)|'
	set(0, 'DefaultTextFontSize', 12)
%	set(0, 'DefaultAxesFontSize', 14)
	legend(h, arg{:})
%	ir_savefig fig_mri_pixel_size_profile
end