%IKINE_SYM  Symbolic inverse kinematics
%
% Q = R.IKINE_SYM(K, OPTIONS) is a cell array (Cx1) of inverse kinematic
% solutions of the SerialLink object ROBOT.  The cells of Q represent the
% different possible configurations.  Each cell of Q is a vector (Nx1), and
% element J is the symbolic expressions for the J'th joint angle.  The
% solution is in terms of the desired end-point pose of the robot which is
% represented by the symbolic matrix (3x4) with elements
%      nx ox ax tx
%      ny oy ay ty
%      nz oz az tz
% where the first three columns specify orientation and the last column
% specifies translation.
%
% K <= N can have only specific values:
%  - 2 solve for translation tx and ty
%  - 3 solve for translation tx, ty and tz
%  - 6 solve for translation and orientation
%
% Options::
%
% 'file',F    Write the solution to an m-file named F
%
% Example::
%
%         mdl_planar2
%         sol = p2.ikine_sym(2);
%         length(sol)
%         ans =
%               2       % there are 2 solutions
%         s1 = sol{1}  % is one solution
%         q1 = s1(1);      % the expression for q1
%         q2 = s1(2);      % the expression for q2
%
% Notes::
% - Requires the Symbolic Toolbox for MATLAB.
% - This code is experimental and has a lot of diagnostic prints.
% - Based on the classical approach using Pieper's method.

% Copyright (C) 1993-2015, 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/>.
%
% http://www.petercorke.com

function out = ikine_sym(srobot, N, varargin)

    %
    % Given a robot model the following steps are performed:
    % 1. Convert model to symbolic form
    % 2. Find relevant trig equations and solve them for joint angles
    % 3. Write an M-file to implement the solution
    %      xikine(T)
    %      xikine(T, S) where S is a 3 vector with elements 1 or 2 to select
    %       the first or second solution for the corresponding joint.
    %
    % TODO:
    %  - handle the wrist joints, only first 3 joints so far
    %  - handle base and tool transforms

    opt.file = [];
    opt = tb_optparse(opt, varargin);

    % make a symbolic representation of the passed robot
    srobot = sym(srobot);
    q = srobot.gencoords();

    % test N DOF has an allowable value
    switch N
        case 2
        case 3
        case 6
        otherwise
            error('RTB:ikine_sym:badarg', 'Can only solve for 2,3,6 DOF');
    end

    % define symbolic elements of the homogeneous transform
    syms nx ox ax tx
    syms ny oy ay ty
    syms nz oz az tz
    syms d3

    % inits
    Q = {};
    trigsubOld = [];
    trigsubNew = [];

    % loop over each joint variable
    for j=1:N
        fprintf('----- solving for joint %d\n', j);

        % create some equations to sift through
        [left,right] = pieper(srobot, j, 'left');

        % decide which equations to look at
        if j <= 3
            % for first three joints only focus on translational part
            left = left(1:3, 4); left = left(:);
            right = right(1:3, 4); right = right(:);
        else
            % for last three joints only focus on rotational part
            left = left(1:3, 1:3); left = left(:);
            right = right(1:3, 1:3); right = right(:);
        end

        % substitute sin/cos for preceding joint as S/C, essentially removes
        % the joint variables from the equations and treats them as constants.
        if ~isempty(trigsubOld)
            left = subs(left, trigsubOld, trigsubNew);
            right = subs(right, trigsubOld, trigsubNew);
        end

        % then simplify the LHS
        %   do it after the substitution to prevent sum of angle terms being introduced
        left = simplify(left);

        % search for a solveable equation:
        %    function of current joint variable on the LHS
        %    constant element on the RHS
        k = NaN;
        for i=1:length(left)
            if hasonly(left(i), j) && isconstant(right(i))
                k = i;
                break;
            end
        end

        eq = [];

        if ~isnan(k)
            % create the equation to solve: LHS-RHS == 0
            eq = left(k) - right(k);
        else
            % ok, we weren't lucky, try another strategy

            % find all equations:
            %    function of current joint variable on the LHS

            k = [];
            for i=1:length(left)
                % has qj on the left and constant on the right
                if hasonly(left(i), j)
                    k = [k i];
                end
            end

            % hopefully we found two of them
            if length(k) < 2
                continue;
            end

            % we did, lets see if the sum square RHS is constant
            rhs = simplify(right(k(1))^2 + right(k(2))^2); % was simple
            if isconstant( rhs )
                % it is, let's sum and square the LHS
                fprintf('lets square and add %d %d\n', k);

                eq = simplify( expand( left(k(1))^2 + left(k(2))^2 ) ) - rhs; % was simple
            end
        end

        % expand the list of joint variable subsitutions
        fprintf('subs sin/cos q%d for S/C\n', j);
        trigsubOld = [trigsubOld mvar('sin(q%d)', j) mvar('cos(q%d)', j)];
        trigsubNew = [trigsubNew mvar('S%d', j) mvar('C%d', j)];

        if isempty(eq)
            fprintf('cant solve this equation');
            k
            left(k)==right(k)
            error('cant solve');
        end
        % now solve the equation
        if srobot.links(j).isrevolute()
            % for revolute joint it will be a trig equation, do we know how to solve it?
            Q{j} = solve_joint(eq, j );
            if isempty(Q)
                warning('cant solve this kind of equation');
            end
        else
            fprintf('prismatic case\n')
            q = sym( sprintf('q%d', j) );
            Q{j} = solve( eq == 0, q);
        end
    end

    % final simplification
    %  get rid of C^2+S^2 and C^4, S^4 terms
    fprintf('**final simplification pass\n')

    % create a list of simplifications
    %  substitute S^2 = 1-C^2, S^4=(1-C^2)^2
    tsubOld = [];
    tsubNew = [];
    for j=1:N
        tsubOld = [tsubOld mvar('S%d', j)^2 mvar('S%d', j)^4];
        tsubNew = [tsubNew 1-mvar('C%d', j)^2 (1-mvar('C%d', j)^2)^2];
    end

    for j=1:N
        for k=1:5
            % seem to need to iterate this, not quite sure why
            Q{j} = simplify( expand( subs(Q{j}, tsubOld, tsubNew) ) );
        end
    end

    % Q is a cell array of equations for joint variables
    if nargout > 0
        out = Q;
    end

    if ~isempty(opt.file)
        fprintf('**generate MATLAB code\n')
        gencode(Q);
    end
end

%PIEPER Return a set of equations using Pieper's method
%
% [L,R] = pieper(robot, n, which)
%
% If robot has link matrix A1 A2 A3 A4 then returns 12 equations from equating the coefficients of
%
%  A1' T = A2 A3 A4     n=1, which='left'
%  A2' A1' T = A3 A4    n=2, which='left'
%  A3' A2' A1' T = A4   n=3, which='left'
%
%  T A4' = A1 A2 A3     n=1, which='right'
%  T A4' A3' = A1 A2    n=2, which='right'
%  T A4' A3' A2' = A1   n=3, which='right'
%
% Judicious choice of the equations can lead to joint solutions

function [L,R] = pieper(robot, n, which)

    if nargin < 3
        which = 'left';
    end

    syms nx ox ax tx real
    syms ny oy ay ty real
    syms nz oz az tz real

    T = [nx ox ax tx
        ny oy ay ty
        nx oz az tz
        0  0  0  1 ];

    T = inv(robot.base) * T * inv(robot.tool);

    q = robot.gencoords();

    % Create the symbolic A matrices
    for j=1:robot.n
        A{j} = robot.links(j).A(q(j));
    end

    switch which
        case 'left'
            left = T;
            for j=1:n
                left = inv(A{j}) * left ;
            end

            right = eye(4,4);
            for j=n+1:robot.n
                right = right * A{j};
            end

        case 'right'
            left = T;
            for j=1:n
                left = left * inv(A{robot.n-j+1});
            end

            right = eye(4,4);
            for j=1:(robot.n-n)
                right = right * A{j};
            end
    end

    %     left = simple(left);
    %     right = simple(right);

    if nargout == 0
        left == right
    elseif nargout == 1
        L = left;
    elseif nargout == 2
        L = left;
        R = right;
    end
end

%SOLVE_JOINT Solve a trigonometric equation
%
% S = SOLVE_JOINT(EQ, J) solves the equation EQ=0 for the joint variable qJ.
% The result is a cell array of solutions.
%
% The equations must be of the form:
%  A cos(qJ) + B sin(qJ) = 0
%  A cos(qJ) + B sin(qJ) = C
%
% where A, B, C are arbitrarily complex expressions.  qJ can be the only
% joint variable in the expression.

function s = solve_joint(eq, j)

    sinj = mvar('sin(q%d)', j);
    cosj = mvar('cos(q%d)', j);

    A = getcoef(eq, cosj);
    B = getcoef(eq, sinj);

    if isempty(A) || isempty(B)
        warning('don''t know how to solve this kind of equation');
    end

    C = -simplify(eq - A*cosj - B*sinj);  % was simple

    if C == 0
        % A cos(q) + B sin(q) = 0
        s(2) = atan2(A, -B);
        s(1) = atan2(-A, B);
    else
        % A cos(q) + B sin(q) = C
        r = sqrt(A^2 + B^2 - C^2);
        phi = atan2(A, B);

        s(2) = atan2(C, r) - phi;
        s(1) = atan2(C, -r) - phi;
    end
    if nargout == 0
        try
            eval(s)
        catch
            s
        end
    end
end

%MVAR Create a symbolic variable
%
% V = MVAR(FMT, ARGS) is a symbolic variable created using SPRINTF
%
% eg. mvar('q%d', j)
%
% The symbolic is explicitly declared to be real.

function v = mvar(fmt, varargin)

    if isempty(strfind(fmt, '('))
        % not a function
        v = sym( sprintf(fmt, varargin{:}), 'real' );
    else
        v = sym( sprintf(fmt, varargin{:}) );

    end
end

%HASONLY Determine if an expression contains only certain joint variables
%
% S = HASONLY(E L) is true if the joint variables (q1, q2 etc.) in the expression E
% are listed in the vector L.
%
% Eg. hasonly('sin(q1)*cos(q2)*cos(q4)', [1 2 3]) -> true
% Eg. hasonly('sin(q1)*cos(q2)*cos(q4)', [1]) -> false

function s = hasonly(eq, j)

    q = findq(eq);
    if isempty(q)
        s = false;
    else
        s = all(ismember(j, findq(eq)));
    end
end

%ISCONSTANT Determine if an expression is free of joint variables
%
% S = ISCONSTANT(E) is true if the expression E contains no joint variables such
% q1, q2 etc.

function s = isconstant(eq)
    s = isempty(findq(eq));
end

%FINDQ Find the joint variables in expression
%
% Q = FINDQ(E) returns a list of integers indicating the joint variables found
% in the expression E.  For instance an instance of 'q1' would cause a 1 to be
% returned and so on.
%
% Eg. findq('sin(q1)*cos(q2)+S3') -> [1 2]

function q = findq(s)

    q = [];

    for var=symvar(s)
        if isempty(var)
            break
        end
        varname = char(var);
        if varname(1) == 'q'
            q = [q str2num(varname(2:end))];
        end
    end
end

function coef = getcoef(eq, trig)
    z = children( collect(eq, trig) );
    z = children( z(1) );
    coef = z(1);
end

% Output a joint expression to a file

function s = gencode(Q, filename)

    function s = G(s, fmt, varargin)
        s = strvcat(s, sprintf(fmt, varargin{:}));
    end

    s = 'function q = xikine(T, sol)';
    s = G(s, '  if nargin < 2; sol = ones(1, %d); end', length(Q));
    s = G(s, '  px = T(1,4); py = T(2,4); pz = T(3,4);');

    for j=1:3
        Qj = Q{j};   % cast it to subclass
        if length(Qj) == 1
            s = G(s, '  q(%d) = %s', j, matgen2(Qj));
        elseif length(Qj) == 2
            s = G(s, '  if sol(%d) == 1', j);
            s = G(s, '    q(%d) = %s', j, matgen2(Qj(1)));
            s = G(s, '  else');
            s = G(s, '    q(%d) = %s', j, matgen2(Qj(2)));
            s = G(s, '  end');

        end

        s = G(s, '  S%d = sin(q(%d));', j, j);
        s = G(s, '  C%d = cos(q(%d));', j, j);
        s = G(s, ' ');

    end
    s = G(s, 'end');

    fp = fopen(filename, 'w');
    for i=1:numrows(s)
        fprintf(fp, '%s\n', deblank(s(i,:)));
    end
    fclose(fp);

end

% Generate MATLAB code from an expression
%
% Requires a bit of a hack, a subclass of sym (sym2) to do this

function s = matgen2(e)

    s = matgen(sym2(e));

    k = strfind(s, '=');
    s = deblank( s(k+2:end) );
end


function [PSNR, yRGB_est] = CBM3D(yRGB, zRGB, sigma, profile, print_to_screen, colorspace)
%
%  CBM3D is algorithm for attenuation of additive white Gaussian noise from
%  color RGB images. This algorithm reproduces the results from the article:
%
%  [1] K. Dabov, A. Foi, V. Katkovnik, and K. Egiazarian, "Color image
%   denoising via sparse 3D collaborative filtering with grouping constraint in
%   luminance-chrominance space," submitted to IEEE Int. Conf. Image Process.,
%   January 2007, in review, preprint at http://www.cs.tut.fi/~foi/GCF-BM3D.
%
%  FUNCTION INTERFACE:
%
%  [PSNR, yRGB_est] = CBM3D(yRGB, zRGB, sigma, profile, print_to_screen, colorspace)
%
%  ! The function can work without any of the input arguments,
%   in which case, the internal default ones are used !
%
%  BASIC USAGE EXAMPLES:
%
%     Case 1) Using the default parameters (i.e., image name, sigma, etc.)
%
%      [PSNR, yRGB_est] = CBM3D;
%
%     Case 2) Using an external noisy image:
%
%      % Read an RGB image and scale its intensities in range [0,1]
%      yRGB = im2double(imread('image_House256rgb.png'));
%      % Generate the same seed used in the experimental results of [1]
%      randn('seed', 0);
%      % Standard deviation of the noise --- corresponding to intensity
%      %  range [0,255], despite that the input was scaled in [0,1]
%      sigma = 25;
%      % Add the AWGN with zero mean and standard deviation 'sigma'
%      zRGB = yRGB + (sigma/255)*randn(size(yRGB));
%      % Denoise 'zRGB'. The denoised image is 'yRGB_est', and 'NA = 1'
%      %  because the true image was not provided
%      [NA, yRGB_est] = CBM3D(1, zRGB, sigma);
%      % Compute the putput PSNR
%      PSNR = 10*log10(1/mean((yRGB(:)-yRGB_est(:)).^2))
%      % show the noisy image 'zRGB' and the denoised 'yRGB_est'
%      figure; imshow(min(max(zRGB,0),1));
%      figure; imshow(min(max(yRGB_est,0),1));
%
%     Case 3) If the original image yRGB is provided as the first input
%      argument, then some additional information is printed (PSNRs,
%      figures, etc.). That is, "[NA, yRGB_est] = BM3D(1, zRGB, sigma);" in the
%      above code should be replaced with:
%
%      [PSNR, yRGB_est] = CBM3D(yRGB, zRGB, sigma);
%
%
%  INPUT ARGUMENTS (OPTIONAL):
%     1) yRGB (M x N x 3): Noise-free RGB image (needed for computing PSNR),
%                           replace with the scalar 1 if not available.
%     2) zRGB (M x N x 3): Noisy RGBimage (intensities in range [0,1] or [0,255])
%     3) sigma (double)  : Std. dev. of the noise (corresponding to intensities
%                            in range [0,255] even if the range of zRGB is [0,1])
%     4) profile (char)  : 'np' --> Normal Profile
%                          'lc' --> Fast Profile
%     5) print_to_screen : 0 --> do not print output information (and do
%                                not plot figures)
%                          1 --> print information and plot figures
%     6) colorspace (char): 'opp'   --> use opponent colorspace
%                          'yCbCr' --> use yCbCr colorspace
%
%  OUTPUTS:
%     1) PSNR (double)          : Output PSNR (dB), only if the original
%                                 image is available, otherwise PSNR = 0
%     2) yRGB_est (M x N x 3): Final RGB estimate (in the range [0,1])
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copyright (c) 2007-2011 Tampere University of Technology.
% All rights reserved.
% This work should only be used for nonprofit purposes.
%
% AUTHORS:
%     Kostadin Dabov, email: dabov _at_ cs.tut.fi
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% In case, there is no input image (zRGB or yRGB), then use the filename
%%%%  below to read an original image (might contain path also). Later,
%%%%  artificial AWGN noise is added and this noisy image is processed
%%%%  by the CBM3D.
%%%%
image_name = [
%    'kodim12.png'
   'image_Lena512rgb.png'
%     'image_House256rgb.png'
%    'image_Peppers512rgb.png'
%    'image_Baboon512rgb.png'
%    'image_F16_512rgb.png'
   ];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%  Quality/complexity trade-off
%%%%
%%%%  'np' --> Normal Profile (balanced quality)
%%%%  'lc' --> Low Complexity Profile (fast, lower quality)
%%%%
%%%%  'high' --> High Profile (high quality, not documented in [1])
%%%%
%%%%  'vn' --> This profile is automatically enabled for high noise
%%%%           when sigma > 40
%%%%
%%%%  'vn_old' --> This is the old 'vn' profile that was used in [1].
%%%%           It gives inferior results than 'vn' in most cases.
%%%%
if (exist('profile') ~= 1)
    profile         = 'np'; %% default profile
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%  Specify the std. dev. of the corrupting noise
%%%%
if (exist('sigma') ~= 1),
   sigma                = 50; %% default standard deviation of the AWGN
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%  Colorspace in which we perform denoising. BM is applied to the first
%%%%  component and the matching information is re-used for the other two.
%%%%
if (exist('colorspace') ~= 1),
    colorspace              = 'opp'; %%% (valid colorspaces are: 'yCbCr' and 'opp')
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% Following are the parameters for the Normal Profile.
%%%%

%%%% Select transforms ('dct', 'dst', 'hadamard', or anything that is listed by 'help wfilters'):
transform_2D_HT_name     = 'bior1.5'; %% transform used for the HT filt. of size N1 x N1
transform_2D_Wiener_name = 'dct';     %% transform used for the Wiener filt. of size N1_wiener x N1_wiener
transform_3rd_dim_name   = 'haar';    %% transform used in the 3-rd dim, the same for HT and Wiener filt.

%%%% Hard-thresholding (HT) parameters:
N1                  = 8;   %% N1 x N1 is the block size used for the hard-thresholding (HT) filtering
Nstep               = 3;   %% sliding step to process every next reference block
N2                  = 16;  %% maximum number of similar blocks (maximum size of the 3rd dimension of a 3D array)
Ns                  = 39;  %% length of the side of the search neighborhood for full-search block-matching (BM), must be odd
tau_match           = 3000;%% threshold for the block-distance (d-distance)
lambda_thr2D        = 0;   %% threshold parameter for the coarse initial denoising used in the d-distance measure
lambda_thr3D        = 2.7; %% threshold parameter for the hard-thresholding in 3D transform domain
beta                = 2.0; %% parameter of the 2D Kaiser window used in the reconstruction

%%%% Wiener filtering parameters:
N1_wiener           = 8;
Nstep_wiener        = 3;
N2_wiener           = 32;
Ns_wiener           = 39;
tau_match_wiener    = 400;
beta_wiener         = 2.0;

%%%% Block-matching parameters:
stepFS              = 1;  %% step that forces to switch to full-search BM, "1" implies always full-search
smallLN             = 'not used in np'; %% if stepFS > 1, then this specifies the size of the small local search neighb.
stepFSW             = 1;
smallLNW            = 'not used in np';
thrToIncStep        = 8;  %% used in the HT filtering to increase the sliding step in uniform regions

if strcmp(profile, 'lc') == 1,

    Nstep               = 6;
    Ns                  = 25;
    Nstep_wiener        = 5;
    N2_wiener           = 16;
    Ns_wiener           = 25;

    thrToIncStep        = 3;
    smallLN             = 3;
    stepFS              = 6*Nstep;
    smallLNW            = 2;
    stepFSW             = 5*Nstep_wiener;

end

% Profile 'vn' was proposed in
%  Y. Hou, C. Zhao, D. Yang, and Y. Cheng, 'Comment on "Image Denoising by Sparse 3D Transform-Domain
%  Collaborative Filtering"', accepted for publication, IEEE Trans. on Image Processing, July, 2010.
% as a better alternative to that initially proposed in [1] (which is currently in profile 'vn_old')
if (strcmp(profile, 'vn') == 1) | (sigma > 40),

    N2                  = 32;
    Nstep               = 4;

    N1_wiener           = 11;
    Nstep_wiener        = 6;

    lambda_thr3D        = 2.8;
    thrToIncStep        = 3;
    tau_match_wiener    = 3500;
    tau_match           = 25000;

    Ns_wiener           = 39;

end

% The 'vn_old' profile corresponds to the original parameters for strong noise proposed in [1].
if (strcmp(profile, 'vn_old') == 1) & (sigma > 40),

    transform_2D_HT_name = 'dct';

    N1                  = 12;
    Nstep               = 4;

    N1_wiener           = 11;
    Nstep_wiener        = 6;

    lambda_thr3D        = 2.8;
    lambda_thr2D        = 2.0;
    thrToIncStep        = 3;
    tau_match_wiener    = 3500;
    tau_match           = 5000;

    Ns_wiener           = 39;

end

decLevel = 0;        %% dec. levels of the dyadic wavelet 2D transform for blocks (0 means full decomposition, higher values decrease the dec. number)
thr_mask = ones(N1); %% N1xN1 mask of threshold scaling coeff. --- by default there is no scaling, however the use of different thresholds for different wavelet decompoistion subbands can be done with this matrix

if strcmp(profile, 'high') == 1,

    decLevel     = 1;
    Nstep        = 2;
    Nstep_wiener = 2;
    lambda_thr3D = 2.5;
    vMask = ones(N1,1); vMask((end/4+1):end/2)= 1.01; vMask((end/2+1):end) = 1.07; %% this allows to have different threhsolds for the finest and next-to-the-finest subbands
    thr_mask = vMask * vMask';
    beta         = 2.5;
    beta_wiener  = 1.5;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% Note: touch below this point only if you know what you are doing!
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%% Check whether to dump information to the screen or reamin silent
dump_output_information = 1;
if (exist('print_to_screen') == 1) & (print_to_screen == 0),
    dump_output_information = 0;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% Create transform matrices, etc.
%%%%
[Tfor, Tinv]   = getTransfMatrix(N1, transform_2D_HT_name, decLevel);  %% get (normalized) forward and inverse transform matrices
[TforW, TinvW] = getTransfMatrix(N1_wiener, transform_2D_Wiener_name); %% get (normalized) forward and inverse transform matrices

if (strcmp(transform_3rd_dim_name, 'haar') == 1) | (strcmp(transform_3rd_dim_name(end-2:end), '1.1') == 1),
    %%% If Haar is used in the 3-rd dimension, then a fast internal transform is used, thus no need to generate transform
    %%% matrices.
    hadper_trans_single_den         = {};
    inverse_hadper_trans_single_den = {};
else
    %%% Create transform matrices. The transforms are later applied by
    %%% matrix-vector multiplication for the 1D case.
    for hpow = 0:ceil(log2(max(N2,N2_wiener))),
        h = 2^hpow;
        [Tfor3rd, Tinv3rd]   = getTransfMatrix(h, transform_3rd_dim_name, 0);
        hadper_trans_single_den{h}         = single(Tfor3rd);
        inverse_hadper_trans_single_den{h} = single(Tinv3rd');
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% 2D Kaiser windows used in the aggregation of block-wise estimates
%%%%
if beta_wiener==2 & beta==2 & N1_wiener==8 & N1==8 % hardcode the window function so that the signal processing toolbox is not needed by default
    Wwin2D = [ 0.1924    0.2989    0.3846    0.4325    0.4325    0.3846    0.2989    0.1924;
        0.2989    0.4642    0.5974    0.6717    0.6717    0.5974    0.4642    0.2989;
        0.3846    0.5974    0.7688    0.8644    0.8644    0.7688    0.5974    0.3846;
        0.4325    0.6717    0.8644    0.9718    0.9718    0.8644    0.6717    0.4325;
        0.4325    0.6717    0.8644    0.9718    0.9718    0.8644    0.6717    0.4325;
        0.3846    0.5974    0.7688    0.8644    0.8644    0.7688    0.5974    0.3846;
        0.2989    0.4642    0.5974    0.6717    0.6717    0.5974    0.4642    0.2989;
        0.1924    0.2989    0.3846    0.4325    0.4325    0.3846    0.2989    0.1924];
    Wwin2D_wiener = Wwin2D;
else
    Wwin2D           = kaiser(N1, beta) * kaiser(N1, beta)'; % Kaiser window used in the aggregation of the HT part
    Wwin2D_wiener    = kaiser(N1_wiener, beta_wiener) * kaiser(N1_wiener, beta_wiener)'; % Kaiser window used in the aggregation of the Wiener filt. part
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% If needed, read images, generate noise, or scale the images to the
%%%% [0,1] interval
%%%%
if (exist('yRGB') ~= 1) | (exist('zRGB') ~= 1)
    yRGB        = im2double(imread(image_name));  %% read a noise-free image
    randn('seed', 0);                          %% generate seed
    zRGB        = yRGB + (sigma/255)*randn(size(yRGB)); %% create a noisy image
else % external images
    image_name = 'External image';

    % convert zRGB to double precision
    zRGB = double(zRGB);

    % convert yRGB to double precision
    yRGB = double(yRGB);

    % if zRGB's range is [0, 255], then convert to [0, 1]
    if (max(zRGB(:)) > 10), % a naive check for intensity range
        zRGB = zRGB / 255;
    end

    % if yRGB's range is [0, 255], then convert to [0, 1]
    if (max(yRGB(:)) > 10), % a naive check for intensity range
        yRGB = yRGB / 255;
    end
end

if (size(zRGB,3) ~= 3) | (size(zRGB,4) ~= 1),
    error('CBM3D accepts only input RGB images (i.e. matrices of size M x N x 3).');
end

% Check if the true image yRGB is a valid one; if not, then we cannot compute PSNR, etc.
yRGB_is_invalid_image = (length(size(zRGB)) ~= length(size(yRGB))) | (size(zRGB,1) ~= size(yRGB,1)) | (size(zRGB,2) ~= size(yRGB,2)) | (size(zRGB,3) ~= size(yRGB,3));
if (yRGB_is_invalid_image),
    dump_output_information = 0;
end

[Xv, Xh, numSlices] = size(zRGB);              %%% obtain image sizes

if numSlices ~= 3
    fprintf('Error, an RGB color image is required!\n');
    return;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% Change colorspace, compute the l2-norms of the new color channels
%%%%
[zColSpace l2normLumChrom] = function_rgb2LumChrom(zRGB, colorspace);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% Print image information to the screen
%%%%
if dump_output_information == 1,
    fprintf(sprintf('Image: %s (%dx%dx%d), sigma: %.1f\n', image_name, Xv, Xh, numSlices, sigma));
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% Step 1. Basic estimate by collaborative hard-thresholding and using
%%%% the grouping constraint on the chrominances.
%%%%
tic;
y_hat = bm3d_thr_color(zColSpace, hadper_trans_single_den, Nstep, N1, N2, lambda_thr2D,...
    lambda_thr3D, tau_match*N1*N1/(255*255), (Ns-1)/2, sigma/255, thrToIncStep, single(Tfor), single(Tinv)', inverse_hadper_trans_single_den, single(thr_mask), 'unused arg', 'unused arg', l2normLumChrom, Wwin2D, smallLN, stepFS );
estimate_elapsed_time = toc;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% Step 2. Final estimate by collaborative Wiener filtering and using
%%%% the grouping constraint on the chrominances.
%%%%
tic;
yRGB_est = bm3d_wiener_color(zColSpace, y_hat, hadper_trans_single_den, Nstep_wiener, N1_wiener, N2_wiener, ...
    'unused_arg', tau_match_wiener*N1_wiener*N1_wiener/(255*255), (Ns_wiener-1)/2, sigma/255, 'unused arg', single(TforW), single(TinvW)', inverse_hadper_trans_single_den, 'unused arg', 'unused arg', l2normLumChrom, Wwin2D_wiener, smallLNW, stepFSW );
wiener_elapsed_time = toc;

yRGB_est = double(yRGB_est);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% Convert back to RGB colorspace
%%%%
yRGB_est = function_LumChrom2rgb(yRGB_est, colorspace);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% Calculate final estimate's PSNR and ISNR, print them, and show the
%%%% denoised image
%%%%
PSNR = 0; %% Remains 0 if the true image yRGB is not available
if (~yRGB_is_invalid_image), % then we assume yRGB is a valid image
    PSNR = 10*log10(1/mean((yRGB(:)-yRGB_est(:)).^2));
end

if dump_output_information == 1,
    fprintf(sprintf('FINAL ESTIMATE (total time: %.1f sec), PSNR: %.2f dB\n', ...
        wiener_elapsed_time + estimate_elapsed_time, PSNR));

    figure, imshow(min(max(zRGB,0),1)); title(sprintf('Noisy %s, PSNR: %.3f dB (sigma: %d)', ...
        image_name(1:end-4), 10*log10(1/mean((yRGB(:)-zRGB(:)).^2)), sigma));

    figure, imshow(min(max(yRGB_est,0),1)); title(sprintf('Denoised %s, PSNR: %.3f dB', ...
        image_name(1:end-4), PSNR));
end

return;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Some auxiliary functions
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [Tforward, Tinverse] = getTransfMatrix (N, transform_type, dec_levels)
%
% Create forward and inverse transform matrices, which allow for perfect
% reconstruction. The forward transform matrix is normalized so that the
% l2-norm of each basis element is 1.
%
% [Tforward, Tinverse] = getTransfMatrix (N, transform_type, dec_levels)
%
%  INPUTS:
%
%   N               --> Size of the transform (for wavelets, must be 2^K)
%
%   transform_type  --> 'dct', 'dst', 'hadamard', or anything that is
%                       listed by 'help wfilters' (bi-orthogonal wavelets)
%                       'DCrand' -- an orthonormal transform with a DC and all
%                       the other basis elements of random nature
%
%   dec_levels      --> If a wavelet transform is generated, this is the
%                       desired decomposition level. Must be in the
%                       range [0, log2(N)-1], where "0" implies
%                       full decomposition.
%
%  OUTPUTS:
%
%   Tforward        --> (N x N) Forward transform matrix
%
%   Tinverse        --> (N x N) Inverse transform matrix
%

if exist('dec_levels') ~= 1,
    dec_levels = 0;
end

if N == 1,
    Tforward = 1;
elseif strcmp(transform_type, 'hadamard') == 1,
    Tforward    = hadamard(N);
elseif (N == 8) & strcmp(transform_type, 'bior1.5')==1 % hardcoded transform so that the wavelet toolbox is not needed to generate it
    Tforward =  [ 0.353553390593274   0.353553390593274   0.353553390593274   0.353553390593274   0.353553390593274   0.353553390593274   0.353553390593274   0.353553390593274;
       0.219417649252501   0.449283757993216   0.449283757993216   0.219417649252501  -0.219417649252501  -0.449283757993216  -0.449283757993216  -0.219417649252501;
       0.569359398342846   0.402347308162278  -0.402347308162278  -0.569359398342846  -0.083506045090284   0.083506045090284  -0.083506045090284   0.083506045090284;
      -0.083506045090284   0.083506045090284  -0.083506045090284   0.083506045090284   0.569359398342846   0.402347308162278  -0.402347308162278  -0.569359398342846;
       0.707106781186547  -0.707106781186547                   0                   0                   0                   0                   0                   0;
                       0                   0   0.707106781186547  -0.707106781186547                   0                   0                   0                   0;
                       0                   0                   0                   0   0.707106781186547  -0.707106781186547                   0                   0;
                       0                   0                   0                   0                   0                   0   0.707106781186547  -0.707106781186547];
elseif (N == 8) & strcmp(transform_type, 'dct')==1 % hardcoded transform so that the signal processing toolbox is not needed to generate it
    Tforward = [ 0.353553390593274   0.353553390593274   0.353553390593274   0.353553390593274   0.353553390593274   0.353553390593274   0.353553390593274   0.353553390593274;
       0.490392640201615   0.415734806151273   0.277785116509801   0.097545161008064  -0.097545161008064  -0.277785116509801  -0.415734806151273  -0.490392640201615;
       0.461939766255643   0.191341716182545  -0.191341716182545  -0.461939766255643  -0.461939766255643  -0.191341716182545   0.191341716182545   0.461939766255643;
       0.415734806151273  -0.097545161008064  -0.490392640201615  -0.277785116509801   0.277785116509801   0.490392640201615   0.097545161008064  -0.415734806151273;
       0.353553390593274  -0.353553390593274  -0.353553390593274   0.353553390593274   0.353553390593274  -0.353553390593274  -0.353553390593274   0.353553390593274;
       0.277785116509801  -0.490392640201615   0.097545161008064   0.415734806151273  -0.415734806151273  -0.097545161008064   0.490392640201615  -0.277785116509801;
       0.191341716182545  -0.461939766255643   0.461939766255643  -0.191341716182545  -0.191341716182545   0.461939766255643  -0.461939766255643   0.191341716182545;
       0.097545161008064  -0.277785116509801   0.415734806151273  -0.490392640201615   0.490392640201615  -0.415734806151273   0.277785116509801  -0.097545161008064];
elseif (N == 8) & strcmp(transform_type, 'dst')==1 % hardcoded transform so that the PDE toolbox is not needed to generate it
    Tforward = [ 0.161229841765317   0.303012985114696   0.408248290463863   0.464242826880013   0.464242826880013   0.408248290463863   0.303012985114696   0.161229841765317;
       0.303012985114696   0.464242826880013   0.408248290463863   0.161229841765317  -0.161229841765317  -0.408248290463863  -0.464242826880013  -0.303012985114696;
       0.408248290463863   0.408248290463863                   0  -0.408248290463863  -0.408248290463863                   0   0.408248290463863   0.408248290463863;
       0.464242826880013   0.161229841765317  -0.408248290463863  -0.303012985114696   0.303012985114696   0.408248290463863  -0.161229841765317  -0.464242826880013;
       0.464242826880013  -0.161229841765317  -0.408248290463863   0.303012985114696   0.303012985114696  -0.408248290463863  -0.161229841765317   0.464242826880013;
       0.408248290463863  -0.408248290463863                   0   0.408248290463863  -0.408248290463863                   0   0.408248290463863  -0.408248290463863;
       0.303012985114696  -0.464242826880013   0.408248290463863  -0.161229841765317  -0.161229841765317   0.408248290463863  -0.464242826880013   0.303012985114696;
       0.161229841765317  -0.303012985114696   0.408248290463863  -0.464242826880013   0.464242826880013  -0.408248290463863   0.303012985114696  -0.161229841765317];
elseif strcmp(transform_type, 'dct') == 1,
    Tforward    = dct(eye(N));
elseif strcmp(transform_type, 'dst') == 1,
    Tforward    = dst(eye(N));
elseif strcmp(transform_type, 'DCrand') == 1,
    x = randn(N); x(1:end,1) = 1; [Q,R] = qr(x);
    if (Q(1) < 0),
        Q = -Q;
    end;
    Tforward = Q';
else %% a wavelet decomposition supported by 'wavedec'
    %%% Set periodic boundary conditions, to preserve bi-orthogonality
    dwtmode('per','nodisp');

    Tforward = zeros(N,N);
    for i = 1:N
        Tforward(:,i)=wavedec(circshift([1 zeros(1,N-1)],[dec_levels i-1]), log2(N), transform_type);  %% construct transform matrix
    end
end

%%% Normalize the basis elements
Tforward = (Tforward' * diag(sqrt(1./sum(Tforward.^2,2))))';

%%% Compute the inverse transform matrix
Tinverse = inv(Tforward);

return;

function [y, A, l2normLumChrom]=function_rgb2LumChrom(xRGB, colormode)
% Forward color-space transformation   ( inverse transformation is function_LumChrom2rgb.m )
%
% Alessandro Foi - Tampere University of Technology - 2005 - 2006   Public release v1.03 (March 2006)
% -----------------------------------------------------------------------------------------------------------------------------------------------
%
% SYNTAX:
%
%   [y A l2normLumChrom] = function_rgb2LumChrom(xRGB, colormode);
%
% INPUTS:
%   xRGB  is RGB image with range [0 1]^3
%
%   colormode = 'opp', 'yCbCr', 'pca', or a custom 3x3 matrix
%
%       'opp'     Opponent color space ('opp' is equirange version)
%       'yCbCr'   The standard yCbCr (e.g. for JPEG images)
%       'pca'     Principal components   (note that this transformation is renormalized to be equirange)
%
% OUTPUTS:
%   y  is color-transformed image (with range typically included in or equal to [0 1]^3, depending on the transformation matrix)
%
%   l2normLumChrom (optional) l2-norm of the transformation (useful for noise std calculation)
%   A  transformation matrix  (used necessarily if colormode='pca')
%
%   NOTES:  -  If only two outputs are used, then the second output is l2normLumChrom, unless colormode='pca';
%           -  'opp' is used by default if no colormode is specified.
%
%
% USAGE EXAMPLE FOR PCA TRANSFORMATION:
%  %%%%  -- forward color transformation --
%    if colormode=='pca'
%       [zLumChrom colormode] = function_rgb2LumChrom(zRGB,colormode); % 'colormode' is assigned a 3x3 transform matrix
%    else
%       zLumChrom = function_rgb2LumChrom(zRGB,colormode);
%    end
%
%  %%%% [ ... ]  Some processing  [ ... ]
%
%  %%%%  -- inverse color transformation --
%    zRGB=function_LumChrom2rgb(zLumChrom,colormode);
%

if nargin==1
    colormode='opp';
end
change_output=0;
if size(colormode)==[3 3]
    A=colormode;
    l2normLumChrom=sqrt(sum(A.^2,2));
else
    if strcmp(colormode,'opp')
        A=[1/3 1/3 1/3; 0.5  0  -0.5; 0.25  -0.5  0.25];
    end
    if strcmp(colormode,'yCbCr')
        A=[0.299   0.587   0.114;   -0.16873660714285  -0.33126339285715   0.5;   0.5  -0.4186875  -0.0813125];
    end
    if strcmp(colormode,'pca')
        A=princomp(reshape(xRGB,[size(xRGB,1)*size(xRGB,2) 3]))';
        A=A./repmat(sum(A.*(A>0),2)-sum(A.*(A<0),2),[1 3]);  %% ranges are normalized to unitary length;
    else
        if nargout==2
            change_output=1;
        end
    end
end

%%%% Make sure that each channel's intensity range is [0,1]
maxV = sum(A.*(A>0),2);
minV = sum(A.*(A<0),2);
yNormal = (reshape(xRGB,[size(xRGB,1)*size(xRGB,2) 3]) * A' - repmat(minV, [1 size(xRGB,1)*size(xRGB,2)])') * diag(1./(maxV-minV)); % put in range [0,1]
y = reshape(yNormal, [size(xRGB,1) size(xRGB,2) 3]);

%%%% The l2-norm of each of the 3 transform basis elements
l2normLumChrom = diag(1./(maxV-minV))*sqrt(sum(A.^2,2));

if change_output
    A=l2normLumChrom;
end

return;

function yRGB=function_LumChrom2rgb(x,colormode)
% Inverse color-space transformation   ( forward transformation is function_rgb2LumChrom.m )
%
% Alessandro Foi - Tampere University of Technology - 2005 - 2006   Public release v1.03 (March 2006)
% -----------------------------------------------------------------------------------------------------------------------------------------------
%
% SYNTAX:
%
%   yRGB = function_LumChrom2rgb(x,colormode);
%
% INPUTS:
%  x  is color-transformed image (with range typically included in or equal to [0 1]^3, depending on the transformation matrix)
%
%  colormode = 'opp', 'yCbCr', or a custom 3x3 matrix (e.g. provided by the forward transform when 'pca' is selected)
%
%       'opp'      opponent color space ('opp' is equirange version)
%       'yCbCr'    standard yCbCr (e.g. for JPEG images)
%
% OUTPUTS:
%   x  is RGB image (with range [0 1]^3)
%
%
% NOTE:    'opp' is used by default if no colormode is specified
%

if nargin==1
    colormode='opp';
end
if size(colormode)==[3 3]
    A=colormode;
    B=inv(A);
else
    if strcmp(colormode,'opp')
        A =[1/3 1/3 1/3; 0.5  0  -0.5; 0.25  -0.5  0.25];
        B =[1 1 2/3;1 0 -4/3;1 -1 2/3];
    end
    if strcmp(colormode,'yCbCr')
        A=[0.299   0.587   0.114;   -0.16873660714285  -0.33126339285715   0.5;   0.5  -0.4186875  -0.0813125];
        B=inv(A);
    end
end

%%%% Make sure that each channel's intensity range is [0,1]
maxV = sum(A.*(A>0),2);
minV = sum(A.*(A<0),2);
xNormal = reshape(x,[size(x,1)*size(x,2) 3]) * diag(maxV-minV) +  repmat(minV, [1 size(x,1)*size(x,2)])'; % put in range [0,1]
yRGB = reshape(xNormal * B', [ size(x,1) size(x,2) 3]);

return;


function [dStates, contactForces] = dynamics_singleStanceOne(States, Actuators, Parameters)
% function [dStates, Actuators] = dynamics_singleStanceOne(States, Actuators, Parameters)
%
% Computer Generated File -- DO NOT EDIT
%
% This function was created by the function Write_ContinuousDynamics()
% 10-Dec-2013 19:40:47
%
% Dymanics Model: retractable double pendulum biped
% Motion Phase: Single Stance One
%
% Matthew Kelly
% Cornell University
%

x = States(1,:); % (m) Foot One horizontal position
y = States(2,:); % (m) Foot One vertical position
th1 = States(3,:); % (rad) Leg One absolute angle
th2 = States(4,:); % (rad) Leg Two absolute angle
L1 = States(5,:); % (m) Leg One length
L2 = States(6,:); % (m) Leg Two length
dx = States(7,:); % (m/s) Foot One horizontal velocity
dy = States(8,:); % (m/s) Foot One vertical velocity
dth1 = States(9,:); % (rad/s) Leg One absolute angular rate
dth2 = States(10,:); % (rad/s) Leg Two absolute angular rate
dL1 = States(11,:); % (m/s) Leg One extension rate
dL2 = States(12,:); % (m/s) Leg Two extensioin rate

F1 = Actuators(1,:); % (N) Compresive axial force in Leg One
F2 = Actuators(2,:); % (N) Compresive axial force in Leg Two
T1 = Actuators(3,:); % (Nm) External torque applied to Leg One
T2 = 0; %Foot Two not in contact with ground!
Thip = Actuators(5,:); % (Nm) Torque acting on Leg Two from Leg One

m1 = Parameters.m1; % (kg) Foot One mass
m2 = Parameters.m2; % (kg) Foot Two mass
M = Parameters.M; % (kg) Hip mass
g = Parameters.g; % (m/s^2) Gravity

% Constraints for this phase:
ddx = 0; %(m) Foot One horizontal position
ddy = 0; %(m) Foot One vertical position
H2 = 0; %(N) Foot Two, horizontal contact force
V2 = 0; %(N) Foot Two, vertical contact force

dStates = zeros(size(States));
dStates(1:6,:) = States((1+6):(6+6),:);
dStates(7,:) = zeros(1,size(States,2));
dStates(8,:) = zeros(1,size(States,2));
dStates(9,:) = -(L2.*Thip - L2.*T1 + L1.*T2.*cos(th1 - th2) + L1.*Thip.*cos(th1 - th2) - F2.*L1.*L2.*sin(th1 - th2) + 2.*L1.*L2.*M.*dL1.*dth1 + L1.*L2.*M.*ddy.*cos(th1) + L1.*L2.*M.*g.*cos(th1) - L1.*L2.*M.*ddx.*sin(th1))./(L1.^2.*L2.*M);
dStates(10,:) = (L1.*M.*T2 + L1.*M.*Thip - L2.*T1.*m2.*cos(th1 - th2) + L2.*Thip.*m2.*cos(th1 - th2) + L1.*T2.*m2.*cos(th1 - th2).^2 + L1.*Thip.*m2.*cos(th1 - th2).^2 + L1.*T2.*m2.*sin(th1 - th2).^2 + L1.*Thip.*m2.*sin(th1 - th2).^2 + L1.*L2.*M.*V2.*cos(th2) - H2.*L1.*L2.*M.*sin(th2) - F1.*L1.*L2.*m2.*sin(th1 - th2) - 2.*L1.*L2.*M.*dL2.*dth2.*m2 - L1.*L2.*M.*ddy.*m2.*cos(th2) - L1.*L2.*M.*g.*m2.*cos(th2) + L1.*L2.*M.*ddx.*m2.*sin(th2) - L1.*L2.*M.*ddx.*m2.*cos(th1 - th2).*sin(th1) + L1.*L2.*M.*ddx.*m2.*sin(th1 - th2).*cos(th1) + L1.*L2.*M.*ddy.*m2.*sin(th1 - th2).*sin(th1) + L1.*L2.*M.*g.*m2.*sin(th1 - th2).*sin(th1) + L1.*L2.*M.*ddy.*m2.*cos(th1 - th2).*cos(th1) + L1.*L2.*M.*g.*m2.*cos(th1 - th2).*cos(th1))./(L1.*L2.^2.*M.*m2);
dStates(11,:) = -(T2.*sin(th1 - th2) + Thip.*sin(th1 - th2) - F1.*L2 + F2.*L2.*cos(th1 - th2) - L1.*L2.*M.*dth1.^2 + L2.*M.*ddx.*cos(th1) + L2.*M.*ddy.*sin(th1) + L2.*M.*g.*sin(th1))./(L2.*M);
dStates(12,:) = (T1.*m2.*sin(th1 - th2) - Thip.*m2.*sin(th1 - th2) + F2.*L1.*M + H2.*L1.*M.*cos(th2) + L1.*M.*V2.*sin(th2) - F1.*L1.*m2.*cos(th1 - th2) + F2.*L1.*m2.*cos(th1 - th2).^2 + F2.*L1.*m2.*sin(th1 - th2).^2 + L1.*L2.*M.*dth2.^2.*m2 - L1.*M.*ddx.*m2.*cos(th2) - L1.*M.*ddy.*m2.*sin(th2) - L1.*M.*g.*m2.*sin(th2) + L1.*M.*ddx.*m2.*cos(th1 - th2).*cos(th1) + L1.*M.*ddy.*m2.*cos(th1 - th2).*sin(th1) - L1.*M.*ddy.*m2.*sin(th1 - th2).*cos(th1) + L1.*M.*g.*m2.*cos(th1 - th2).*sin(th1) - L1.*M.*g.*m2.*sin(th1 - th2).*cos(th1) + L1.*M.*ddx.*m2.*sin(th1 - th2).*sin(th1))./(L1.*M.*m2);

% contactForces(1,:) == H1 == (N) Foot One, horizontal contact force
% contactForces(2,:) == V1 == (N) Foot One, vertical contact force
% contactForces(3,:) == H2 == (N) Foot Two, horizontal contact force
% contactForces(4,:) == V2 == (N) Foot Two, vertical contact force
contactForces = zeros(4,size(States,2));
contactForces(1,:) = (Thip.*sin(th1) - T1.*sin(th1) + L1.*ddx.*m1 + F1.*L1.*cos(th1))./L1;
contactForces(2,:) = (T1.*cos(th1) - Thip.*cos(th1) + L1.*ddy.*m1 + L1.*g.*m1 + F1.*L1.*sin(th1))./L1;
contactForces(3,:) = zeros(1,size(States,2));
contactForces(4,:) = zeros(1,size(States,2));

end


function [fig_name_tca]=ps_mean_v(ifg_list,n_boot,subtract_switches,use_small_baselines,aps_flag)
%PS_MEAN_V Calculate mean velocities and their standard deviations
%   PS_MEAN_V(IFG_LIST,N_BOOT,SUBTRACT_SWITCHES) where IFG_LIST gives indices
%   of interferograms to be included in the calculation,N_BOOT specifies number
%   of bootstrap iterations and RAMP_FLAG is 1 to subtract orbital ramps
%
%   Andy Hooper, Jan 2008
%
%   ======================================================================
%   06/2009 AH: Subtract orbital ramp if present
%   06/2009 AH: Process smaller chunks to reduce memory needs
%   02/2010 AH: Revert to v3.1 version
%   02/2010 AH: Replace unwrap_ifg_index with drop_ifg_index
%   03/2010 AH: 3rd input changed to 'subtract_switches'
%   03/2010 AH: Use small baselines option added
%   03/2010 AH: var/cov added to inversion
%   05/2010 AH: Include master if there are not ifgs before and after
%   06/2010 AH: small change to ps_mean_v.m
%   03/2014 AH: -a etc added
%   04/2014 DB: fix fig_name_tca output in case no aps correction
%   05/2014 DB: Fix v-doa to v-dao
%   05/2014 DB: For APS related corrections include deramping on the fly
%   05/2014 DB: big fix in case of a nan
%   07/2014 EH: Fix for nanmean in case of single element compared to vector
%   03/2015 DB: Do all deramping on the fly, make consistent for TRAIN release
%   05/2017 DB: use stamps save to save larger variables than 2GB.
%   06/2017 DB: Include the option to choose between chol decompostion for
%               bootstrapping (not to invert covariance often), or conventional
%               bootstrapping requiring to invert covariance matrix for each iteration.
%   ======================================================================

fprintf('Calculating standard deviation of mean velocity...\n')

chol_flag = 'n';

if nargin<1
    ifg_list=[];
end
if nargin<2
    n_boot=100;
end

if nargin<3 | isempty(subtract_switches)
   subtract_switches='';
end

if nargin<4
    use_small_baselines=0;
end

if nargin<5
    aps_flag=1;
end

load psver
psname=['./ps',num2str(psver)];
phuwname=['./phuw',num2str(psver)];
phuwsbresname=['./phuw_sb_res',num2str(psver)];
ifgstdname=['./ifgstd',num2str(psver)];
sclaname=['./scla',num2str(psver)];
apsname=['./tca',num2str(psver)];
tidename=['./tide',num2str(psver)];

mvname=['mv',num2str(psver)];

ps=load(psname);

drop_ifg_index=getparm('drop_ifg_index');
fig_name_tca = '';
if strcmpi(getparm('small_baseline_flag'),'y')
    if use_small_baselines==0
        phuw=load(phuwname,'unwrap_ifg_index_sm');
        if isfield(phuw,'unwrap_ifg_index_sm');
            unwrap_ifg_index=phuw.unwrap_ifg_index_sm;
        else
            unwrap_ifg_index=[1:ps.n_image];
        end
        phuwres=load(phuwsbresname,'sm_cov');
        if isfield(phuwres,'sm_cov');
            ifg_cov=phuwres.sm_cov;
        else
            ifg_cov=eye(ps.n_image);
        end
    else
        unwrap_ifg_index=setdiff([1:ps.n_ifg],drop_ifg_index);
        phuwname=['./phuw_sb',num2str(psver)];
        sclaname=['./scla_sb',num2str(psver)];
        apsname=['./tca_sb',num2str(psver)];
        tidename=['./tide_sb',num2str(psver)];

        phuwres=load(phuwsbresname,'sb_cov');
        if isfield(phuwres,'sb_cov');
            ifg_cov=phuwres.sb_cov;
        else
            ifg_cov=eye(ps.n_ifg);
        end
    end
else
    use_small_baselines=0;
    unwrap_ifg_index=setdiff([1:ps.n_ifg],drop_ifg_index);
    if ~exist([ifgstdname,'.mat'],'file')
        ifg_cov=eye(ps.n_ifg);
    else ifgstd=load(ifgstdname);
      if isfield(ifgstd,'ifg_std');
        ifgvar=(ifgstd.ifg_std*pi/181).^2;
        ifg_cov=diag(ifgvar);
      else
        ifg_cov=eye(ps.n_ifg);
      end
    end
end

uw=load(phuwname);
ph_uw=uw.ph_uw;
clear uw

switch(subtract_switches)
case('')
case('d')
    scla=load(sclaname);
    ph_uw=ph_uw - scla.ph_scla;
    clear scla
case('o')
    [ph_uw] = ps_deramp(ps,ph_uw);
case('a')
    aps=load(apsname);
    [aps_corr,fig_name_tca] = ps_plot_tca(aps,aps_flag);
    ph_uw=ph_uw - aps_corr;
    clear aps aps_corr
case('ao')
    aps=load(apsname);
    [aps_corr,fig_name_tca] = ps_plot_tca(aps,aps_flag);
    ph_uw=ph_uw - aps_corr;
    [ph_uw] = ps_deramp(ps,ph_uw);
    clear aps aps_corr
case('do')
    scla=load(sclaname);
    ph_uw=ph_uw - scla.ph_scla;
    [ph_uw] = ps_deramp(ps,ph_uw);
case('da')
    scla=load(sclaname);
    ph_uw=ph_uw - scla.ph_scla;
    clear scla
    aps=load(apsname);
    [aps_corr,fig_name_tca] = ps_plot_tca(aps,aps_flag);
    ph_uw=ph_uw - aps_corr;
    clear aps aps_corr
case('dat')
    tide=load(tidename);
    scla=load(sclaname);
    ph_uw=ph_uw - scla.ph_scla;
    clear scla
    aps=load(apsname);
    [aps_corr,fig_name_tca] = ps_plot_tca(aps,aps_flag);
    ph_uw=ph_uw - aps_corr-tide.ph_tide;
    clear aps aps_corr
case('dao')
    scla=load(sclaname);
    ph_uw=ph_uw - scla.ph_scla ;
    [ph_uw] = ps_deramp(ps,ph_uw);
    aps=load(apsname);
    [aps_corr,fig_name_tca] = ps_plot_tca(aps,aps_flag);
    ph_uw=ph_uw - aps_corr;
    clear aps aps_corr
otherwise
        error('unknown subtract flags')
end

if use_small_baselines==0
    %AH2CHECK %%%%%%% & unwrap_ifg_index(1)~=ps.master_ix & unwrap_ifg_index(end)~=ps.master_ix
    unwrap_ifg_index=setdiff(unwrap_ifg_index,ps.master_ix);
end

ph_all=zeros(ps.n_ps,1);
ref_ps=ps_setref;
if ~isempty(ifg_list)
    unwrap_ifg_index=intersect(unwrap_ifg_index,ifg_list);
    ifg_list=[];
end
ph_uw=ph_uw(:,unwrap_ifg_index);
ifg_cov=ifg_cov(unwrap_ifg_index,unwrap_ifg_index);
if rank(ifg_cov)<size(ifg_cov,1)
    error('Interferograms included for which there is no unwrapped solution')
end

if use_small_baselines==0
    day=ps.day(unwrap_ifg_index)-ps.master_day;
else
    day=ps.day(ps.ifgday_ix(unwrap_ifg_index,2))-ps.day(ps.ifgday_ix(unwrap_ifg_index,1));
end
N=length(day);
ph_uw=ph_uw-repmat(nanmean(ph_uw(ref_ps,:),1),ps.n_ps,1);
lambda=getparm('lambda');
ph_uw=double(ph_uw/4/pi*lambda*1000)';
G=[ones(N,1),double(day)/365.25] ;

% computing mean velocity
mean_v=lscov(G,ph_uw,ifg_cov)';
mean_v=mean_v(:,2);

% perform the bootstrapping
n=10000; % process n PS at a time
i=1;
mean_v_std=NaN(ps.n_ps,1,'single');
rand_init=sum(100*clock);
n_ifg = size(ph_uw,1);

if strcmpi(chol_flag,'y')
    % cholesky decomposition of the set of linear equations including
    % covariance. This allows to use the decomposition values in a uniform
    % LSQ inversion. i.e. the covariance matrix does not need to be
    % inverted each time.
    ph_uw=chol(inv(ifg_cov))*ph_uw;
    G=chol(inv(ifg_cov))*G;

    % looping over the PS segments
    while i<ps.n_ps
        if i+n< ps.n_ps
           i_end=i+n-1;
        else
           i_end=ps.n_ps;
        end
        ph_bit=ph_uw(:,i:i_end);

        % in-case of nans and only a subset being processed
        ix_non_nan = sum(isnan(ph_bit),1)==0;

        % fix the random generator, such its the same for each segment
        rand('twister',rand_init);

        % skip the segment in case of nan's
        if sum(ix_non_nan)>0
            if sum(ix_non_nan)== length(ix_non_nan)
                [mean_v_dist,boot_ix] = bootstrp(n_boot, @(x) single(lscov(G(x,:),ph_bit(x,:))), [1:N]);
                 temp = std(mean_v_dist(:,2:2:end))';
            else
                [mean_v_dist_temp,boot_ix] = bootstrp(n_boot, @(x) single(lscov(G(x,:),ph_bit(x,ix_non_nan))), [1:N]);
                temp = NaN([size(ph_bit,2) 1]);
                temp(ix_non_nan) = std(mean_v_dist_temp(:,2:2:end))';
                clear mean_v_dist_temp
            end
            mean_v_std(i:i_end) =temp;
        end
        i=i+n;
        fprintf('%d PS processed\n',i-1)
    end

else
    % conventional bootstrapping where the coveriance matrix is inverted on
    % each set PS segments.
    while i<ps.n_ps
        if i+n< ps.n_ps
           i_end=i+n-1;
        else
           i_end=ps.n_ps;
        end
        ph_bit=ph_uw(:,i:i_end);

        % in-case of nans and only a subset being processed
        ix_non_nan = sum(isnan(ph_bit),1)==0;

        % fix the random generator, such its the same for each segment
        rand('twister',rand_init)

        % skip the segment in case of nan's
        if sum(ix_non_nan)>0
            if sum(ix_non_nan)== length(ix_non_nan)
                [mean_v_dist,boot_ix] = bootstrp(n_boot, @(x) single(lscov(G(x,:),ph_bit(x,:),ifg_cov)), [1:N]);
                temp = std(mean_v_dist(:,2:2:end))';
            else
               fprintf('test\n')
               [mean_v_dist_temp,boot_ix] = bootstrp(n_boot, @(x) single(lscov(G(x,:),ph_bit(x,ix_non_nan),ifg_cov(ix_non_nan,ix_non_nan))), [1:N]);
               temp = NaN([size(ph_bit,2) 1]);
               temp(ix_non_nan) = std(mean_v_dist_temp(:,2:2:end))';
               clear mean_v_dist_temp
            end
            mean_v_std(i:i_end) =temp;
        end
        i=i+n;
        fprintf('%d PS processed\n',i-1)
    end
end

stamps_save(mvname,n_boot,subtract_switches,mean_v,mean_v_std)


function Y = TT_weingarten(V, Z, ind)
% Weingarten map computation for the fixed TT-rank manifold.
%
% NOTE: this manifold requires the use of a modified version of TTeMPS_1.1,
% which is packaced with Manopt and can be found in
% manopt/manopt/manifolds/ttfixedrank/TTeMPS_1.1
%
% Y = TT_weingarten(V, Z, ind)
%
% This function implements the efficient way of computing the Weingarten
% map for the Riemannian submanifold of fixed TT-rank tensors embedded in
% its natural embedding Euclidean space, as described in the paper:
%
%   Psenka and Boumal,
%   Second-order optimization for tensors with fixed tensor-train rank,
%   Optimization workshop at NeurIPS 2020.
%
% Y is the output tangent vector, V is a tangent vector (which contains
% needed information of base point X), Z is a tensor (embedding space).
%
% TT_weingarten supports the following formats for Z:
% 1) Full
% 2) TT-format (any rank)
% 3) sparse, with non-zero indices indexed by ind (see fixedTTrankfactory).
%
% See also: fixedTTrankfactory

% This file is part of Manopt: www.manopt.org.
% Original author: Michael Psenka, Nov. 24, 2020.
% Contributors: Nicolas Boumal
% Change log:

    % Preliminary tangent vector set-up for Y
    d = V.order;
    r = V.rank;
    n = V.size;

    Y = V; % all properties except for dU cores inherited from V
    normV = norm(V);
    V = (1 / normV) * V;
    Y.dU = cell(1, d);

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Begin calculating variational components
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    % Pre-calculate all R-components as needed for tangent space conversion
    R = cell(1, d-1);
    % Intermediary used to calculate R-terms for re-orthogonalization
    Xr = V.U;

    for k = d:-1:2

        sz = size(Xr{k});

        if length(sz) == 2
            sz = [sz, 1]; %#ok<AGROW>
        end

        [Q, R{k - 1}] = qr_unique(unfold(Xr{k}, 'right')');
        Xr{k} = reshape(Q', [size(Q, 2), sz(2), sz(3)]);

        Xr{k - 1} = tensorprod_ttemps(Xr{k - 1}, R{k - 1}, 3);
    end

    % Calculate V.dU under the original parametrization of tangent space

    dUR = cell(1, d);

    for k = 1:(d - 1)
        dUR{k} = unfold(V.dU{k}, 'left') / R{k}';
        dUR{k} = reshape(dUR{k}, [r(k), n(k), r(k + 1)]);
    end

    dUR{d} = V.dU{d};

    % Calculate ~X^tV_{\ge k} efficiently
    XtVg = cell(1, d);

    XtVg{d} = conj(unfold(V.V{d}, 'right')) * unfold(V.dU{d}, 'right').';
    XtVgTmp = conj(unfold(V.V{d}, 'right')) * unfold(V.U{d}, 'right').';

    for k = (d - 1):-1:2
        tmp = tensorprod_ttemps(V.V{k}, XtVg{k + 1}', 3);
        XtVg{k} = conj(unfold(tmp, 'right')) * unfold(V.U{k}, 'right').';

        tmp2 = tensorprod_ttemps(V.V{k}, XtVgTmp', 3);
        XtVg{k} = XtVg{k} + conj(unfold(tmp2, 'right')) * unfold(dUR{k}, 'right').';
        XtVgTmp = conj(unfold(tmp2, 'right')) * unfold(V.U{k}, 'right').';
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%% CASES FOR TYPE OF EUCLIDEAN GRADIENT Z %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    if isa(Z, 'TTeMPS') || isa(Z, 'TTeMPS_tangent_orth')% Z either TT-tensor or tangent vec

        if isa(Z, 'TTeMPS_tangent_orth')
            Z = tangent_to_TTeMPS(Z); % efficient to treat both as TTeMPS case
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%%%%%%%%%%%%%               DIAGONAL TERMS                 %%%%%%%%%%%%%%%%%%
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        ZVr = cell(1, d); % stores values of V_<^T(Z<k>)
        Zr = cell(1, d);  % stores values of X_<^T(Z<k>)
        ZVl = cell(1, d); % stores values of (Z<k>)^T(V_>)
        Zl = cell(1, d);  % stores values of (Z<k>)^T(X_>)

        ZVr{d}  = conj(unfold(Z.U{d}, 'right')) * unfold(V.dU{d}, 'right').';
        XtVgTmp = conj(unfold(Z.U{d}, 'right')) * unfold(V.U{d},  'right').';

        for k = (d - 1):-1:2
            tmp = tensorprod_ttemps(Z.U{k}, ZVr{k + 1}', 3);
            ZVr{k} = conj(unfold(tmp, 'right')) * unfold(V.U{k}, 'right').';

            tmp2 = tensorprod_ttemps(Z.U{k}, XtVgTmp', 3);
            ZVr{k} = ZVr{k} + conj(unfold(tmp2, 'right')) * unfold(dUR{k}, 'right').';
            XtVgTmp = conj(unfold(tmp2, 'right')) * unfold(V.U{k}, 'right').';
        end

        for k=1:d
            ZVr{k} = ZVr{k}';
        end

        Zr{d} = conj(unfold(V.V{d}, 'right')) * unfold(Z.U{d}, 'right').';

        for k = (d - 1):-1:2
            tmp = tensorprod_ttemps(V.V{k}, Zr{k + 1}', 3);
            Zr{k} = conj(unfold(tmp, 'right')) * unfold(Z.U{k}, 'right').';
        end

        % Computation of ZVl and Zl

        ZVl{1} = unfold(dUR{1}, 'left')' * unfold(Z.U{1}, 'left');
        Zl{1}  = unfold(V.U{1}, 'left')' * unfold(Z.U{1}, 'left');

        for k = 2:(d - 1)
            % (V_k-1)(U_k)
            tmp = tensorprod_ttemps(V.U{k}, ZVl{k - 1}', 1);
            ZVl{k} = unfold(tmp, 'left')' * unfold(Z.U{k}, 'left');
            % + (X_k-1)(dU_k)
            tmp = tensorprod_ttemps(dUR{k}, Zl{k - 1}', 1);
            ZVl{k} = ZVl{k} + unfold(tmp, 'left')' * unfold(Z.U{k}, 'left');
            % update Zr to keep up w/ recursive definition
            tmp = tensorprod_ttemps(V.U{k}, Zl{k - 1}', 1);
            Zl{k} = unfold(tmp, 'left')' * unfold(Z.U{k}, 'left');
        end

        ZZ = cell(1, d); % final cores for normal (I \otimes X)Z(X^T)
        Zv = cell(1, d); % final cores for (I \otimes X)Z(V^T)
        vZ = cell(1, d); % final cores for (I \otimes V)Z(X^T)

        % contract to first core
        ZZ{1} = tensorprod_ttemps(Z.U{1}, Zr{2}, 3);
        % contract to inner cores
        for k = 2:(d - 1)
            res = tensorprod_ttemps(Z.U{k}, Zl{k - 1}, 1);
            ZZ{k} = tensorprod_ttemps(res, Zr{k + 1}, 3);
        end

        % contract to last core
        ZZ{d} = tensorprod_ttemps(Z.U{d}, Zl{d - 1}, 1);

        Zv{1} = tensorprod_ttemps(Z.U{1}, ZVr{2}, 3);

        for k = 2:(d - 1)
            res = tensorprod_ttemps(Z.U{k}, Zl{k - 1}, 1);
            Zv{k} = tensorprod_ttemps(res, ZVr{k + 1}, 3);
        end

        Zv{d} = tensorprod_ttemps(Z.U{d}, Zl{d - 1}, 1);

        vZ{1} = tensorprod_ttemps(Z.U{1}, Zr{2}, 3);

        for k = 2:(d - 1)
            res = tensorprod_ttemps(Z.U{k}, ZVl{k - 1}, 1);
            vZ{k} = tensorprod_ttemps(res, Zr{k + 1}, 3);
        end

        vZ{d} = tensorprod_ttemps(Z.U{d}, ZVl{d - 1}, 1);

        % Left unfold everything to apply additional operations
        for k = 1:d
            ZZ{k} = unfold(ZZ{k}, 'left');
            Zv{k} = unfold(Zv{k}, 'left');
            vZ{k} = unfold(vZ{k}, 'left');
        end
        % %%%%%%%%%%%%%%%%%%%%%%%%% TESTING %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        % 1st and last cases special, so they're computed outside the loop
        UL  = unfold(V.U{1}, 'left');
        dUL = unfold(dUR{1}, 'left');

        % right variational term without (I - UU^T) projection
        rightV = (Zv{1} - ZZ{1} * XtVg{2}) / R{1};

        Y.dU{1} = -dUL * UL' * ZZ{1} + rightV - UL * (UL' * rightV);

        for k = 2:(d - 1)
            UL  = unfold(V.U{k}, 'left');
            dUL = unfold(dUR{k}, 'left');

            % right variational term without (I - UU^T) projection
            rightV = (Zv{k} - ZZ{k} * XtVg{k + 1}) / R{k};

            Y.dU{k} = vZ{k} - UL * (UL' * vZ{k}) - dUL * UL' * ZZ{k} ...
                       + rightV - UL * (UL' * rightV);
        end

        Y.dU{d} = vZ{d};

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%%%%%%%%%%%%%                  CROSS TERMS                 %%%%%%%%%%%%%%%%%%
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        for k = 1:(d - 1)
            U = unfold(V.U{k}, 'left');
            ZZ{k} = ZZ{k} - U * (U' * ZZ{k});
            ZZ{k} = reshape(ZZ{k}, [r(k), n(k), r(k + 1)]);
        end

        ZZ{d} = reshape(ZZ{d}, [r(d), n(d), 1]);

        % Double loop to represent P_k DP_m, 1 <= k < d
        for k = 1:(d - 1)

            UL = unfold(V.U{k}, 'left');
            dUL = unfold(dUR{k}, 'left');

            for m = 1:(k - 1)
                VtZ = crossTermVariational(k, m, ZZ, dUR{m}, V);
                projUVtZ = VtZ - UL * (UL' * VtZ);

                Y.dU{k} = Y.dU{k} - projUVtZ;
            end

            for m = (k + 1):d
                ZtX = crossTermMatrixRight(k + 1, m, ZZ, V);
                Y.dU{k} = Y.dU{k} + dUL * ZtX;
            end

        end

        % Final terms for k = d
        for m = 1:(d - 1)
            VtZ = crossTermVariational(d, m, ZZ, dUR{m}, V);
            Y.dU{d} = Y.dU{d} - VtZ;
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%FINAL RESHAPE%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        for k = 1:d
            Y.dU{k} = reshape(Y.dU{k}, [r(k), n(k), r(k + 1)]);
        end

    elseif ~exist('ind', 'var')% Z is a full array

        %{
        Note that you can split up the full expression for the Weingarten map in
        the following way:

        1) Diagonal terms(P_i DP_i terms, or non - cross terms)
        a) This can be split into(left variational) + (right variational),
        which correspond to two terms in product rule expansion
        2) Cross terms(P_i DP_j, we show mathematically equivalent expression
        in companion paper.)
        a) This likewise can be split into(i < j) + (i > j)
        %}

        % Cell that represents the LEFT VARIATIONAL SIDE
        Zv = cell(1, d);
        % Cell that represents the RIGHT VARIATIONAL SIDE
        vZ = cell(1, d);

        % Initialization step
        Zv{d} = Z(:);
        vZ{d} = Z(:);

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%%%%%%%%%%%%%               DIAGONAL TERMS                 %%%%%%%%%%%%%%%%%%
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        %%%%%%%%%%%%%%%               LEFT VARIATIONAL               %%%%%%%%%%%%%%%%%%

        % First, calculate where the right side is the variational matrix
        % Note that the calculation here is split between VR^-1 and XX^tVR^-1
        % in (I - XX^t)VR^{-1} through xx and xx2 terms respectively

        % (d-1) term done separately to initialize the temp variable handoff
        zz = reshape(Z, [prod(n(1:(d - 1))), n(d)]);
        xx = transpose(unfold(V.dU{d}, 'right'));
        xxTmp = transpose(unfold(V.U{d}, 'right')); % temp var to help calc xx terms
        xx2 = transpose(unfold(V.V{d}, 'right'));

        % Z-matrix-k-variational, represents the xx calculation for Zv{k}
        Zmkv = zz * xx;
        vZ{d - 1} = zz * xx2;
        Zv{d - 1} = (Zmkv - vZ{d - 1} * XtVg{d}) / R{d - 1};

        % auxillery term for computing anything with variational matrix
        ZZtmp = zz * xxTmp;

        % Main calcualtion for right size of Zv
        for k = (d - 2):-1:1
            zz = reshape(Zmkv, [prod(n(1:k)), n(k + 1) * r(k + 2)]);
            xx = transpose(unfold(V.U{k + 1}, 'right'));
            Zmkv = zz * xx;

            zzTmp = reshape(ZZtmp, [prod(n(1:k)), n(k + 1) * r(k + 2)]);
            xxTmp = transpose(unfold(dUR{k + 1}, 'right'));
            Zmkv = Zmkv + zzTmp * xxTmp;
            ZZtmp = zzTmp * xx;

            % NOTE: vZ is only used here to store these values for later use
            % It stores the standard right side calc for the orthogonal projector
            zz2 = reshape(vZ{k + 1}, [prod(n(1:k)), n(k + 1) * r(k + 2)]);
            xx2 = transpose(unfold(V.V{k + 1}, 'right'));
            vZ{k} = zz2 * xx2;

            Zv{k} = (Zmkv - vZ{k} * XtVg{k + 1}) / R{k};
        end

        % Standard left side calculations for Zv
        for k = 2:d

            for i = 1:(k - 1)
                Z_i = reshape(Zv{k}, [r(i) * n(i), prod(n(i + 1:k)) * r(k + 1)]);
                X_i = unfold(V.U{i}, 'left');
                Zm = X_i' * Z_i;
                Zv{k} = Zm;
            end

            Zv{k} = reshape(Zv{k}, [r(k) * n(k), r(k + 1)]);
        end

        % Final U core projector mult for Zv
        for k = 1:(d - 1)
            U = unfold(V.U{k}, 'left');
            Zv{k} = Zv{k} - U * (U' * Zv{k});
        end

        %%%%%%%%%%%%%%%               RIGHT VARIATIONAL               %%%%%%%%%%%%%%%%%%

        % represents standard projection, also needed as intermediary for this section
        ZZ = vZ;
        % Start left side calculation for vZ
        for k = 2:d

            % Do first in loop separately to initialize intermediary variable
            Z_i = reshape(vZ{k}, [n(1), prod(n(2:k)) * r(k + 1)]);
            dU_i = unfold(dUR{1}, 'left');
            Zm = dU_i' * Z_i;
            vZ{k} = Zm;

            X_i = unfold(V.U{1}, 'left');
            ZZ{k} = X_i' * Z_i;

            for m = 2:(k - 1)
                Z_i = reshape(vZ{k}, [r(m) * n(m), prod(n(m + 1:k)) * r(k + 1)]);
                X_i = unfold(V.U{m}, 'left');
                Zm = X_i' * Z_i;

                Z_tmp = reshape(ZZ{k}, [r(m) * prod(n(m)), prod(n(m + 1:k)) * r(k + 1)]);
                dU_i = unfold(dUR{m}, 'left');

                vZ{k} = Zm + dU_i' * Z_tmp;

                ZZ{k} = X_i' * Z_tmp;
            end

            vZ{k} = reshape(vZ{k}, [r(k) * n(k), r(k + 1)]);
            ZZ{k} = reshape(ZZ{k}, [r(k) * n(k), r(k + 1)]);
        end

        % Final U core projector mult for vZ

        % 1st core separate since vZ{1} = 0 up until now. Purely for efficiency
        U  = unfold(V.U{1},  'left');
        dU = unfold(V.dU{1}, 'left');
        vZ{1} = -dU * U' * ZZ{1};

        for k = 2:(d - 1)
            U  = unfold(V.U{k},  'left');
            dU = unfold(V.dU{k}, 'left');
            vZ{k} = vZ{k} - U * (U' * vZ{k});

            vZ{k} = vZ{k} - dU * U' * ZZ{k};

        end

        for k = 1:d - 1
            Y.dU{k} = Zv{k} + vZ{k};
        end

        Y.dU{d} = vZ{d};

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%%%%%%%%%%%%%                  CROSS TERMS                 %%%%%%%%%%%%%%%%%%
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        for k = 1:(d - 1)
            % now only need ZZ as dU cores of normal projection of V
            U = unfold(V.U{k}, 'left');
            ZZ{k} = ZZ{k} - U * (U' * ZZ{k});
            ZZ{k} = reshape(ZZ{k}, [r(k), n(k), r(k + 1)]);
        end

        ZZ{d} = reshape(ZZ{d}, [r(d), n(d), 1]);

        % Double loop to represent P_k DP_m, 1 <= k < d
        for k = 1:(d - 1)

            UL  = unfold(V.U{k},  'left');
            dUL = unfold(V.dU{k}, 'left');

            for m = 1:(k - 1)
                VtZ = crossTermVariational(k, m, ZZ, dUR{m}, V);

                projUVtZ = VtZ - UL * (UL' * VtZ);

                Y.dU{k} = Y.dU{k} - projUVtZ;
            end

            for m = (k + 1):d
                ZtX = crossTermMatrixRight(k + 1, m, ZZ, V);
                Y.dU{k} = Y.dU{k} + dUL * ZtX;
            end

        end

        % Final terms for k = d
        for m = 1:(d - 1)
            VtZ = crossTermVariational(d, m, ZZ, dUR{m}, V);

            Y.dU{d} = Y.dU{d} - VtZ;
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FINAL RESHAPE %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        for k = 1:d
            Y.dU{k} = reshape(Y.dU{k}, [r(k), n(k), r(k + 1)]);
        end

    else
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% SPARSE CASE %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        % permuted U, V, and dU cells for the efficient computation in C
        CU = cell(1, d);
        CV = cell(1, d);
        CdUR = cell(1, d);

        for k = 1:d
            CU{k} = permute(V.U{k}, [1 3 2]);
            CV{k} = permute(V.V{k}, [1 3 2]);
            CdUR{k} = permute(dUR{k}, [1 3 2]);
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%%%%%%%%%%%%%               DIAGONAL TERMS                 %%%%%%%%%%%%%%%%%%
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        [ZZ, vZ, Zv] = weingarten_omega(n, r, CU, CV, CdUR, ind.', Z);

        for k = 1:d

            ZZ{k} = reshape(ZZ{k}, [r(k), r(k + 1), n(k)]);
            ZZ{k} = unfold(permute(ZZ{k}, [1 3 2]), 'left');

            vZ{k} = reshape(vZ{k}, [r(k), r(k + 1), n(k)]);
            vZ{k} = unfold(permute(vZ{k}, [1 3 2]), 'left');

            Zv{k} = reshape(Zv{k}, [r(k), r(k + 1), n(k)]);
            Zv{k} = unfold(permute(Zv{k}, [1 3 2]), 'left');
        end

        % 1st and last cases special, so they're computed outside the loop
        UL  = unfold(V.U{1}, 'left');
        dUL = unfold(dUR{1}, 'left');

        % right variational term without (I - UU^T) projection
        rightV = (Zv{1} - ZZ{1} * XtVg{2}) / R{1};
        Y.dU{1} = -dUL * UL' * ZZ{1} + rightV - UL * (UL' * rightV);

        for k = 2:(d - 1)
            UL  = unfold(V.U{k}, 'left');
            dUL = unfold(dUR{k}, 'left');

            % right variational term without (I - UU^T) projection
            rightV = (Zv{k} - ZZ{k} * XtVg{k + 1}) / R{k};
            Y.dU{k} = vZ{k} - UL * (UL' * vZ{k}) - dUL * UL' * ZZ{k} + rightV - UL * (UL' * rightV);
            % norm(vZ{k} - UL * (UL' * vZ{k}) + (Zv{k} - UL * (UL' * Zv{k})) / R{k}) / norm(V)
            % norm(V)
        end

        Y.dU{d} = vZ{d};

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%%%%%%%%%%%%%                  CROSS TERMS                 %%%%%%%%%%%%%%%%%%
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        for k = 1:(d - 1)
            U = unfold(V.U{k}, 'left');
            ZZ{k} = ZZ{k} - U * (U' * ZZ{k});
            ZZ{k} = reshape(ZZ{k}, [r(k), n(k), r(k + 1)]);
        end

        ZZ{d} = reshape(ZZ{d}, [r(d), n(d), 1]);

        % Double loop to represent P_k DP_m, 1 <= k < d
        for k = 1:(d - 1)

            UL  = unfold(V.U{k}, 'left');
            dUL = unfold(dUR{k}, 'left');

            for m = 1:(k - 1)
                % XtZ = crossTermMatrixLeft(k, m, ZZ, V);
                VtZ = crossTermVariational(k, m, ZZ, dUR{m}, V);
                projUVtZ = VtZ - UL * (UL' * VtZ);

                Y.dU{k} = Y.dU{k} - projUVtZ; %+ dUL * XtZ;

                % Y.dU{k} = Y.dU{k} - (projUk * kron(eye(n(k)), Vllk') - dUkL * Xlek') * Zkm;
            end

            for m = (k + 1):d
                ZtX = crossTermMatrixRight(k + 1, m, ZZ, V);
                Y.dU{k} = Y.dU{k} + dUL * ZtX;

                %         Y.dU{k} = Y.dU{k} + dUkL * Zkm' * Xg{k+1};
            end

        end

        % Final terms for k = d
        for m = 1:(d - 1)
            VtZ = crossTermVariational(d, m, ZZ, dUR{m}, V);

            Y.dU{d} = Y.dU{d} - VtZ;

            %     Y.dU{d} = Y.dU{d} - kron(eye(n(d)), Vlld') * Zkm;
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%FINAL RESHAPE%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        for k = 1:d
            Y.dU{k} = reshape(Y.dU{k}, [r(k), n(k), r(k + 1)]);
        end

        Y = (normV) * Y;
    end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Helper methods
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Efficient ZtX term for P_k DP_m, m > k
% Zp here is the projection of Z (just dU cores)
% V is only passed for access to U and V cores
function ZtX = crossTermMatrixRight(k, m, Zp, V)
    ZtX = conj(unfold(Zp{m}, 'right')) * unfold(V.V{m}, 'right').';

    for p = (m - 1):-1:k
        tmp = tensorprod_ttemps(V.U{p}, (ZtX)', 3);
        ZtX = conj(unfold(tmp, 'right')) * unfold(V.V{p}, 'right').';
    end

end

% Efficient XtZ term for P_k DP_m, m < k
% Zp here is the projection of Z
% V is only passed for access to U and V cores
function XtZ = crossTermMatrixLeft(k, m, Zp, V) %#ok<DEFNU>

    XtZ = unfold(V.U{m}, 'left')' * unfold(Zp{m}, 'left');

    for p = (m + 1):k
        XtZ = tensorprod_ttemps(V.U{p}, (XtZ)', 1);
        XtZ = unfold(XtZ, 'left')' * unfold(V.V{p}, 'left');
    end

end

% Efficient VtZ (more accurately kron(eye(n(k)), V') * Z) term for P_k DP_m, m < k
% Zp here is the projection of Z, dURm is normally parametrized dU{m}
% V is only passed for access to U and V cores
function VtZ = crossTermVariational(k, m, Zp, dURm, V)

    VtZ = unfold(dURm, 'left')' * unfold(Zp{m}, 'left');

    for p = (m + 1):(k - 1)
        VtZ = tensorprod_ttemps(V.U{p}, (VtZ)', 1);
        VtZ = unfold(VtZ, 'left')' * unfold(V.V{p}, 'left');
    end

    VtZ = tensorprod_ttemps(V.V{k}, VtZ, 1);
    VtZ = unfold(VtZ, 'left');

end


function [err_N, sG] = ptv_local_nuclear_metric(volfix, voldef, pix_resolution, mask, singular_coefs, P_SZ, norm_type)
%% todo:
% * volfix
% * zero mean, choose std???
% * making zero mean changes gradient!

%     norm_type = 3;
    abs_type = 0;
    deps = 1e-3;

    volsz = [size(voldef, 1), size(voldef, 2), size(voldef, 3)];
    Nd = 3;
    if volsz(3) == 1
        Nd = 2;
    end

    P_SZ = round(P_SZ);
    Nimgs = size(voldef, 5);
    if Nd == 2
        P_SZ = [P_SZ, 1];
    end

    err_N = 0;
    sG = zeros([volsz, Nimgs], 'like', voldef);
    if Nd == 2
        for i1 = 1 : P_SZ(1) : volsz(1)
        for i2 = 1 : P_SZ(2) : volsz(2)
            i11 = min(i1 + P_SZ(1) - 1, volsz(1));
            i22 = min(i2 + P_SZ(2) - 1, volsz(2));
            if i11-i1 < 2 || i22 - i2 < 2
                continue;
            end
            patch = voldef(i1:i11, i2:i22, 1, :, :);
            if norm_type == 1
                patch = patch - sum(sum(sum(patch,1),2),3) / (size(patch,1)*size(patch,2)*size(patch,3));
            elseif norm_type == 2
                patch = patch - mean(patch(:));
            elseif norm_type == 3
                patch = patch - sum(sum( patch, 4), 5) / (size(patch,4)*size(patch,5));
            elseif norm_type == 4
                patch = patch - sum(sum(sum(patch,1),2),3) / (size(patch,1)*size(patch,2)*size(patch,3));
                patch = patch - sum(sum( patch, 4), 5) / (size(patch,4)*size(patch,5));
            end

            patch0 = reshape(patch, [size(patch, 1), size(patch, 2), size(patch, 3), Nimgs]);
            if abs_type == 1
                patch = abs(patch);
            elseif abs_type == 2
                patch = sqrt(patch.^2 + deps);
            end

            if ~isempty(mask)
                mask_cur = logical(mask(i1:i11, i2:i22));
            else
                mask_cur = [];
            end
            if isempty(mask_cur) || any(mask_cur(:))
                [terr, tgrad] = ptv_nuclear_metric([], patch, pix_resolution, mask_cur, singular_coefs);
                err_N = err_N + terr;

                if abs_type == 1
                    tgrad = tgrad.*sign(patch0);
                elseif abs_type == 2
                    tgrad = tgrad .* patch0 ./ sqrt(patch0.^2 + deps);
                end

                if norm_type == 1
                    tgrad = tgrad - sum(sum(sum(tgrad, 1), 2), 3) / (size(patch,1)*size(patch,2)*size(patch,3));
                elseif norm_type == 2
                    tgrad = tgrad - mean(tgrad(:));
                elseif norm_type == 3
                    tgrad = tgrad - sum(sum( tgrad, 4), 5) / (size(patch,4)*size(patch,5));
                elseif norm_type == 4
                    tgrad = tgrad - sum(sum( tgrad, 4), 5) / (size(patch,4)*size(patch,5));
                    tgrad = tgrad - sum(sum(sum(tgrad, 1), 2), 3) / (size(patch,1)*size(patch,2)*size(patch,3));
                end

                sG(i1:i11, i2:i22, 1, :) = tgrad;
            end
        end
        end
    else %3d
        for i1 = 1 : P_SZ(1) : volsz(1)
        for i2 = 1 : P_SZ(2) : volsz(2)
        for i3 = 1 : P_SZ(3) : volsz(3)
            i11 = min(i1 + P_SZ(1) - 1, volsz(1));
            i22 = min(i2 + P_SZ(2) - 1, volsz(2));
            i33 = min(i3 + P_SZ(3) - 1, volsz(3));
            if i11-i1 < 2 || i22 - i2 < 2 || i33 - i3 < 2
                continue;
            end
            patch = voldef(i1:i11, i2:i22, i3:i33, :, :);
            if norm_type == 1
                patch = patch - sum(sum(sum(patch,1),2),3) / (size(patch,1)*size(patch,2)*size(patch,3));
            elseif norm_type == 2
                patch = patch - mean(patch(:));
            elseif norm_type == 3
                patch = patch - sum(sum( patch, 4), 5) / (size(patch,4)*size(patch,5));
            elseif norm_type == 4
                patch = patch - sum(sum(sum(patch,1),2),3) / (size(patch,1)*size(patch,2)*size(patch,3));
                patch = patch - sum(sum( patch, 4), 5) / (size(patch,4)*size(patch,5));
            end

            patch0 = reshape(patch, [size(patch, 1), size(patch, 2), size(patch, 3), Nimgs]);
            if abs_type == 1
                patch = abs(patch);
            elseif abs_type == 2
                patch = sqrt(patch.^2 + deps);
            end

            if ~isempty(mask)
                mask_cur = logical(mask(i1:i11, i2:i22, i3:i33));
            else
                mask_cur = [];
            end
            if isempty(mask_cur) || any(mask_cur(:))
                [terr, tgrad] = ptv_nuclear_metric([], patch, pix_resolution, mask_cur, singular_coefs);
                err_N = err_N + terr;
                if abs_type == 1
                    tgrad = tgrad.*sign(patch0);
                elseif abs_type == 2
                    tgrad = tgrad .* patch0 ./ sqrt(patch0.^2 + deps);
                end
                if norm_type == 1
                    tgrad = tgrad - sum(sum(sum(tgrad, 1), 2), 3) / (size(patch,1)*size(patch,2)*size(patch,3));
                elseif norm_type == 2
                    tgrad = tgrad - mean(tgrad(:));
                elseif norm_type == 3
                    tgrad = tgrad - sum(sum( tgrad, 4), 5) / (size(patch,4)*size(patch,5));
                elseif norm_type == 4
                    tgrad = tgrad - sum(sum( tgrad, 4), 5) / (size(patch,4)*size(patch,5));
                    tgrad = tgrad - sum(sum(sum(tgrad, 1), 2), 3) / (size(patch,1)*size(patch,2)*size(patch,3));
                end
                sG(i1:i11, i2:i22, i3:i33, :) = tgrad;
            end
        end
        end
        end
    end
    err_N = err_N * prod(pix_resolution);
    sG = sG(1:volsz(1), 1:volsz(2), 1:volsz(3), :, :) * prod(pix_resolution);
end

% function [err_N, sG] = ptv_local_nuclear_metric(volfix, voldef, pix_resolution, mask, singular_coefs, P_SZ)
% %% todo:
% % * volfix
% % * zero mean, choose std???
% % * making zero mean changes gradient!
%
% %     P_SZ = [15,15,15];
%     volsz = [size(voldef, 1), size(voldef, 2), size(voldef, 3)];
%     Nd = 3;
%     if volsz(3) == 1
%         Nd = 2;
%     end
%
%     P_SZ = round(P_SZ);
%     Nimgs = size(voldef, 5);
%     if Nd == 2
%         P_SZ = [P_SZ, 1];
%     end
%
%     volsz_p = ceil(volsz./P_SZ) .* P_SZ;
%
%     if Nd == 2
%         volsz_p(3) = 1;
%     end
%
%     voldef_p = zeros([volsz_p, 1, Nimgs]);
%     voldef_p(1:volsz(1), 1:volsz(2), 1:volsz(3), :, :) = voldef;
%
%     if ~isempty(mask)
%         if Nd == 3
%             mask_p = zeros([volsz_p]);
%             mask_p(1:volsz(1), 1:volsz(2), 1:volsz(3)) = mask;
%         else
%             mask_p = zeros([volsz_p]);
%             mask_p(1:volsz(1), 1:volsz(2), 1) = mask;
%         end
%     end
%
%     err_N = 0;
%     sG = zeros([volsz, Nimgs], 'like', voldef);
%     if Nd == 2
%         for i1 = 1 : P_SZ(1) : volsz(1)
%         for i2 = 1 : P_SZ(2) : volsz(2)
%             patch = voldef_p(i1:i1+P_SZ(1)-1, i2:i2+P_SZ(2)-1, 1, :, :);
%             patch = patch - sum(sum(sum(patch,1),2),3) / numel(patch);
% %             patch = patch ./ std(std(std(patch,[],1),[],2),[],3);
%             if ~isempty(mask)
%                 mask_cur = logical(mask_p(i1:i1+P_SZ(1)-1, i2:i2+P_SZ(2)-1));
%             else
%                 mask_cur = [];
%             end
%             if isempty(mask_cur) || any(mask_cur(:))
%                 [terr, tgrad] = ptv_nuclear_metric([], patch, pix_resolution, mask_cur, singular_coefs);
%                 err_N = err_N + terr;
%                 tgrad = tgrad - sum(sum(sum(tgrad, 1), 2), 3) / numel(patch);
%                 sG(i1:i1+P_SZ(1)-1, i2:i2+P_SZ(2)-1, 1, :) = tgrad;
%             end
%         end
%         end
%     else %3d
%         for i1 = 1 : P_SZ(1) : volsz(1)
%         for i2 = 1 : P_SZ(2) : volsz(2)
%         for i3 = 1 : P_SZ(3) : volsz(3)
%             patch = voldef_p(i1:i1+P_SZ(1)-1, i2:i2+P_SZ(2)-1, i3:i3+P_SZ(3)-1, :, :);
%             patch = patch - sum(sum(sum(patch,1),2),3) / numel(patch);
%             if ~isempty(mask)
%                 mask_cur = logical(mask_p(i1:i1+P_SZ(1)-1, i2:i2+P_SZ(2)-1, i3:i3+P_SZ(3)-1));
%             else
%                 mask_cur = [];
%             end
%             if isempty(mask_cur) || any(mask_cur(:))
%                 [terr, tgrad] = ptv_nuclear_metric([], patch, pix_resolution, mask_cur, singular_coefs);
%                 err_N = err_N + terr;
%                 sG(i1:i1+P_SZ(1)-1, i2:i2+P_SZ(2)-1, i3:i3+P_SZ(3)-1, :) = tgrad;
%             end
%         end
%         end
%         end
%     end
%     err_N = err_N * prod(pix_resolution);
%     sG = sG(1:volsz(1), 1:volsz(2), 1:volsz(3), :, :) * prod(pix_resolution);
% end


function significant = spectsignificance(tests,alpha)
%
% From a estiamtion of p-values through the specttest function,
% It returns 0 or 1 for each power of coherence value, if the p-value is
% below the level of significance 'alpha'.
%
% INPUTS:
%
% tests                 The output of specttest, i.e. a cell
%                       where each element corresponds to one subject
% alpha                 Significance level
%
% OUTPUT:
%
% significant           Struct with 4 fields: lower, higher, lower_corr, higher_corr.
%                       ('_corr' reflects to multiple comparisons correction)
%                       Each field has a field 'states', just like each
%                       element of tests, with fields state(k).psd and
%                       state(k).coh containing 1 if the value is
%                       significant and 0 otherwise
%
% Author: Diego Vidaurre, OHBA, University of Oxford (2017)

if nargin < 2, alpha = 0.01; end

significant = struct();
significant.higher = struct();
significant.higher.state = struct();
significant.higher_corr = struct();
significant.higher_corr.state = struct();
significant.lower = struct();
significant.lower.state = struct();
significant.lower_corr = struct();
significant.lower_corr.state = struct();

K = length(tests.higher.state);
for k = 1:K
    significant.higher.state(k).psd = tests.higher.state(k).psd < alpha;
    significant.higher_corr.state(k).psd = tests.higher_corr.state(k).psd < alpha;
    significant.lower.state(k).psd = tests.lower.state(k).psd < alpha;
    significant.lower_corr.state(k).psd = tests.lower_corr.state(k).psd < alpha;
    significant.higher.state(k).coh = tests.higher.state(k).coh < alpha;
    significant.higher_corr.state(k).coh = tests.higher_corr.state(k).coh < alpha;
    significant.lower.state(k).coh = tests.lower.state(k).coh < alpha;
    significant.lower_corr.state(k).coh = tests.lower_corr.state(k).coh < alpha;
end

end


function [a_vec, a_vec_flt] = KiserTMOv(hdrv, filenameOutput, tmo_alpha, tmo_dn_clamping, tmo_white, tmo_gamma, tmo_quality, tmo_video_profile)
%
%
%      KiserTMOv(hdrv, filenameOutput, tmo_alpha, tmo_dn_clamping, tmo_white, tmo_gamma, tmo_quality, tmo_video_profile)
%
%
%       Input:
%           -hdrv: a HDR video structure; use hdrvread to create a hdrv
%           structure
%           -filenameOutput: output filename (if it has an image extension,
%           single files will be generated)
%           -tmo_alpha: \alpha_A, \alpha_B, \alpha_C coefficients
%           costants in the paper (Equation 3a, 3b, and 3c)
%           -tmo_dn_clamping: a boolean value (0 or 1) for setting black
%           and white levels clamping
%           -tmo_white: white point;
%           -tmo_gamma: gamma for encoding the frame
%           -tmo_quality: the output quality in [1,100]. 100 is the best quality
%           1 is the lowest quality.%
%           -tmo_video_profile: the compression profile (encoder) for compressing the stream.
%           Please have a look to the profile of VideoWriter from the MATLAB
%           help. Depending on the version of MATLAB some profiles may be not
%           be present.
%
%     Copyright (C) 2013-17 Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%     The paper describing this technique is:
%     "Real-time Automated Tone Mapping System for HDR Video"
% 	  by Chris Kiser, Erik Reinhard, Mike Tocci and Nora Tocci
%     in IEEE International Conference on Image Processing, 2012
%
%

if(~exist('tmo_alpha', 'var'))
    tmo_alpha = 0.98;
end

if(~exist('tmo_dn_clamping', 'var'))
    tmo_dn_clamping = 0;
end

if(~exist('tmo_white', 'var'))
    tmo_white = 1e6;
end

if(tmo_white <= 0.0)
    tmo_white = 1e6;
end

if(~exist('tmo_gamma', 'var'))
    tmo_gamma = -2.2;
end

if(tmo_gamma < 0)
    bsRGB = 1;
else
    bsRGB = 0;
end

if(~exist('tmo_quality', 'var'))
    tmo_quality = 95;
end

if(~exist('tmo_video_profile', 'var'))
    tmo_video_profile = 'MPEG-4';
end

name = RemoveExt(filenameOutput);
ext  = fileExtension(filenameOutput);

bVideo = 0;
writerObj = 0;

if(strcmp(ext, 'avi') == 1 | strcmp(ext, 'mp4') == 1)
    bVideo = 1;
    writerObj = VideoWriter(filenameOutput, tmo_video_profile);
    writerObj.FrameRate = hdrv.FrameRate;
    writerObj.Quality = tmo_quality;
    open(writerObj);
end

hdrv = hdrvopen(hdrv);

disp('Tone Mapping...');

beta_clamping   = 0.999;
beta_clamping_c = (1.0 - beta_clamping);

a_vec = [];
a_vec_flt = [];

maxLprev = 0;

mkdir(name);

for i=1:hdrv.totalFrames
    disp(['Processing frame ', num2str(i)]);
    [frame, hdrv] = hdrvGetFrame(hdrv, i);

    %only physical values
    frame = RemoveSpecials(frame);
    frame(frame < 0) = 0;

    if(tmo_dn_clamping)
        %clamp black and white levels
        L = RemoveSpecials(lum(frame));
        %compute CDF's histogram
        [histo, bound, ~] = HistogramHDR(L, 256, 'log10', [], 1);
        histo_cdf = cumsum(histo);
        histo_cdf = histo_cdf/max(histo_cdf(:));
        [~, ind] = min(abs(histo_cdf - beta_clamping));
        maxL = 10^(ind * (bound(2) - bound(1)) / 256 + bound(1));

        [~, ind] = min(abs(histo_cdf-beta_clamping_c));
        minL = 10^(ind * (bound(2) - bound(1)) / 256 + bound(1));

        frame(frame > maxL) = maxL;
        frame(frame < minL) = minL;
        frame = frame - minL;
    end

    %compute statistics for the current frame
    L = lum(frame);
    Lav = logMean(L);
    maxL = max(L(:));
    A = maxL - Lav;
    B = Lav - min(L(:));

    if(i == 1)
        maxLprev = maxL;
        Aprev = A;
        Bprev = B;
        aprev = 0.18 * 2^(2 * (B - A) / (A + B));
    end

    %leaky integration
    tmo_alpha_c = 1.0 - tmo_alpha;
    An = tmo_alpha_c * Aprev + tmo_alpha * A;
    Aprev = An;

    Bn = tmo_alpha_c * Bprev + tmo_alpha * B;
    Bprev = Bn;

    a = SceneKey(An, Bn);
    an = tmo_alpha_c * aprev + tmo_alpha * a;
    aprev = an;

    %tone mapping
    [frameOut, ~, ~] = ReinhardTMO(frame, an, tmo_white, 'global');

    %example
    a_vec = [a_vec, maxL];
    maxLn =  0.5 * maxLprev + 0.5 *maxL;
    maxLprev = maxLn;
    a_vec_flt = [a_vec_flt, maxLn ];

    %gamma/sRGB encoding
    if(bsRGB)
        frameOut = ClampImg(ConvertRGBtosRGB(frameOut, 0), 0, 1);
    else
        frameOut = ClampImg(GammaTMO(frameOut, tmo_gamma, 0, 0), 0, 1);
    end

    if(bVideo)
        writeVideo(writerObj, frameOut);
    else
        imwrite(frameOut, [name, '/frame_', sprintf('%.10d',i), '.', ext]);
    end

    %update statistics for the next frame
    Aprev = A;
    Bprev = B;
    aprev = a;
end
disp('OK');

if(bVideo)
    close(writerObj);
end

hdrvclose(hdrv);

end

function a = SceneKey(An, Bn)
    a = 0.18 * 2^(2 * (Bn - An) / (An + Bn));
end


function value = r8_mop ( i )

%*****************************************************************************80
%
%% R8_MOP returns the I-th power of -1 as an R8 value.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 November 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer I, the power of -1.
%
%    Output, real R8_MOP, the I-th power of -1.
%
  if ( mod ( i, 2 ) == 0 )
    value = + 1.0;
  else
    value = - 1.0;
  end

  return
end


function M = callback_blurring( M, dir, options )

% callback_blurring - callback for the deconvolution with wavelets
%
%   M = callback_blurring( M, dir, options );
%
%   Copyright (c) 2007 Gabriel Peyre

if isfield(options, 'eta')
    eta = options.eta;
else
    eta = 5;
end
if isfield(options, 'eta')
    Jmin = options.Jmin;
else
    Jmin = 4;
end

n = sqrt(size(M,1));
M = reshape(M,n,n);

if dir==1
    % wavelet synthesis and then blurring
    M = perform_wavelet_transform(M, Jmin, -1, options );
    M = perform_blurring( M, eta );
else
    % transpose
    M = perform_blurring( M, eta );
    M = perform_wavelet_transform(M, Jmin, 1, options );
end

M = M(:);