function C = fdct_wrapping(x, is_real, finest, nbscales, nbangles_coarse)

% fdct_wrapping.m - Fast Discrete Curvelet Transform via wedge wrapping - Version 1.0
%
% Inputs
%   x           M-by-N matrix
%
% Optional Inputs
%   is_real     Type of the transform
%                   0: complex-valued curvelets
%                   1: real-valued curvelets
%               [default set to 0]
%   finest      Chooses one of two possibilities for the coefficients at the
%               finest level:
%                   1: curvelets
%                   2: wavelets
%               [default set to 2]
%   nbscales    number of scales including the coarsest wavelet level
%               [default set to ceil(log2(min(M,N)) - 3)]
%   nbangles_coarse
%               number of angles at the 2nd coarsest level, minimum 8,
%               must be a multiple of 4. [default set to 16]
%
% Outputs
%   C           Cell array of curvelet coefficients.
%               C{j}{l}(k1,k2) is the coefficient at
%                   - scale j: integer, from finest to coarsest scale,
%                   - angle l: integer, starts at the top-left corner and
%                   increases clockwise,
%                   - position k1,k2: both integers, size varies with j
%                   and l.
%               If is_real is 1, there are two types of curvelets,
%               'cosine' and 'sine'. For a given scale j, the 'cosine'
%               coefficients are stored in the first two quadrants (low
%               values of l), the 'sine' coefficients in the last two
%               quadrants (high values of l).
%
% See also ifdct_wrapping.m, fdct_wrapping_param.m
%
% By Laurent Demanet, 2004

X = fftshift(fft2(ifftshift(x)))/sqrt(prod(size(x)));
[N1,N2] = size(X);
if nargin < 2, is_real = 0; end;
if nargin < 3, finest = 2; end;
if nargin < 4, nbscales = ceil(log2(min(N1,N2)) - 3); end;
if nargin < 5, nbangles_coarse = 16; end;

% Initialization: data structure
nbangles = [1, nbangles_coarse .* 2.^(ceil((nbscales-(nbscales:-1:2))/2))];
if finest == 2, nbangles(nbscales) = 1; end;
C = cell(1,nbscales);
for j = 1:nbscales
    C{j} = cell(1,nbangles(j));
end;

% Loop: pyramidal scale decomposition
M1 = N1/3;
M2 = N2/3;
if finest == 1,

    % Initialization: smooth periodic extension of high frequencies
    bigN1 = 2*floor(2*M1)+1;
    bigN2 = 2*floor(2*M2)+1;
    equiv_index_1 = 1+mod(floor(N1/2)-floor(2*M1)+(1:bigN1)-1,N1);
    equiv_index_2 = 1+mod(floor(N2/2)-floor(2*M2)+(1:bigN2)-1,N2);
    X = X(equiv_index_1,equiv_index_2);
        % Invariant: equiv_index_1(floor(2*M1)+1) == (N1 + 2 - mod(N1,2))/2
        % is the center in frequency. Same for M2, N2.
    window_length_1 = floor(2*M1) - floor(M1) - 1 - (mod(N1,3)==0);
    window_length_2 = floor(2*M2) - floor(M2) - 1 - (mod(N2,3)==0);
        % Invariant: floor(M1) + floor(2*M1) == N1 - (mod(M1,3)~=0)
        % Same for M2, N2.
    coord_1 = 0:(1/window_length_1):1;
    coord_2 = 0:(1/window_length_2):1;
    [wl_1,wr_1] = fdct_wrapping_window(coord_1);
    [wl_2,wr_2] = fdct_wrapping_window(coord_2);
    lowpass_1 = [wl_1, ones(1,2*floor(M1)+1), wr_1];
    if mod(N1,3)==0, lowpass_1 = [0, lowpass_1, 0]; end;
    lowpass_2 = [wl_2, ones(1,2*floor(M2)+1), wr_2];
    if mod(N2,3)==0, lowpass_2 = [0, lowpass_2, 0]; end;
    lowpass = lowpass_1'*lowpass_2;
    Xlow = X .* lowpass;

    scales = nbscales:-1:2;

else

    M1 = M1/2;
    M2 = M2/2;
    window_length_1 = floor(2*M1) - floor(M1) - 1;
    window_length_2 = floor(2*M2) - floor(M2) - 1;
    coord_1 = 0:(1/window_length_1):1;
    coord_2 = 0:(1/window_length_2):1;
    [wl_1,wr_1] = fdct_wrapping_window(coord_1);
    [wl_2,wr_2] = fdct_wrapping_window(coord_2);
    lowpass_1 = [wl_1, ones(1,2*floor(M1)+1), wr_1];
    lowpass_2 = [wl_2, ones(1,2*floor(M2)+1), wr_2];
    lowpass = lowpass_1'*lowpass_2;
    hipass = sqrt(1 - lowpass.^2);
    Xlow_index_1 = ((-floor(2*M1)):floor(2*M1)) + ceil((N1+1)/2);
    Xlow_index_2 = ((-floor(2*M2)):floor(2*M2)) + ceil((N2+1)/2);
    Xlow = X(Xlow_index_1, Xlow_index_2) .* lowpass;
    Xhi = X;
    Xhi(Xlow_index_1, Xlow_index_2) = Xhi(Xlow_index_1, Xlow_index_2) .* hipass;
    C{nbscales}{1} = fftshift(ifft2(ifftshift(Xhi)))*sqrt(prod(size(Xhi)));
    if is_real, C{nbscales}{1} = real(C{nbscales}{1}); end;

    scales = (nbscales-1):-1:2;

end;
for j = scales,

    M1 = M1/2;
    M2 = M2/2;
    window_length_1 = floor(2*M1) - floor(M1) - 1;
    window_length_2 = floor(2*M2) - floor(M2) - 1;
    coord_1 = 0:(1/window_length_1):1;
    coord_2 = 0:(1/window_length_2):1;
    [wl_1,wr_1] = fdct_wrapping_window(coord_1);
    [wl_2,wr_2] = fdct_wrapping_window(coord_2);
    lowpass_1 = [wl_1, ones(1,2*floor(M1)+1), wr_1];
    lowpass_2 = [wl_2, ones(1,2*floor(M2)+1), wr_2];
    lowpass = lowpass_1'*lowpass_2;
    hipass = sqrt(1 - lowpass.^2);
    Xhi = Xlow;                 % size is 2*floor(4*M1)+1 - by - 2*floor(4*M2)+1
    Xlow_index_1 = ((-floor(2*M1)):floor(2*M1)) + floor(4*M1) + 1;
    Xlow_index_2 = ((-floor(2*M2)):floor(2*M2)) + floor(4*M2) + 1;
    Xlow = Xlow(Xlow_index_1, Xlow_index_2);
    Xhi(Xlow_index_1, Xlow_index_2) = Xlow .* hipass;
    Xlow = Xlow .* lowpass;     % size is 2*floor(2*M1)+1 - by - 2*floor(2*M2)+1

    % Loop: angular decomposition
    l = 0;
    nbquadrants = 2 + 2*(~is_real);
    nbangles_perquad = nbangles(j)/4;
    for quadrant = 1:nbquadrants
        M_horiz = M2 * (mod(quadrant,2)==1) + M1 * (mod(quadrant,2)==0);
        M_vert = M1 * (mod(quadrant,2)==1) + M2 * (mod(quadrant,2)==0);
        if mod(nbangles_perquad,2),
            wedge_ticks_left = round((0:(1/(2*nbangles_perquad)):.5)*2*floor(4*M_horiz) + 1);
            wedge_ticks_right = 2*floor(4*M_horiz) + 2 - wedge_ticks_left;
            wedge_ticks = [wedge_ticks_left, wedge_ticks_right(end:-1:1)];
        else
            wedge_ticks_left = round((0:(1/(2*nbangles_perquad)):.5)*2*floor(4*M_horiz) + 1);
            wedge_ticks_right = 2*floor(4*M_horiz) + 2 - wedge_ticks_left;
            wedge_ticks = [wedge_ticks_left, wedge_ticks_right((end-1):-1:1)];
        end;
        wedge_endpoints = wedge_ticks(2:2:(end-1));         % integers
        wedge_midpoints = (wedge_endpoints(1:(end-1)) + wedge_endpoints(2:end))/2;
                % integers or half-integers

        % Left corner wedge
        l = l+1;
        first_wedge_endpoint_vert = round(2*floor(4*M_vert)/(2*nbangles_perquad) + 1);
        length_corner_wedge = floor(4*M_vert) - floor(M_vert) + ceil(first_wedge_endpoint_vert/4);
        Y_corner = 1:length_corner_wedge;
        [XX,YY] = meshgrid(1:(2*floor(4*M_horiz)+1),Y_corner);
        width_wedge = wedge_endpoints(2) + wedge_endpoints(1) - 1;
        slope_wedge = (floor(4*M_horiz) + 1 - wedge_endpoints(1))/floor(4*M_vert);
        left_line = round(2 - wedge_endpoints(1) + slope_wedge*(Y_corner - 1));
                                                            % integers
        [wrapped_data, wrapped_XX, wrapped_YY] = deal(zeros(length_corner_wedge,width_wedge));
        first_row = floor(4*M_vert)+2-ceil((length_corner_wedge+1)/2)+...
            mod(length_corner_wedge+1,2)*(quadrant-2 == mod(quadrant-2,2));
        first_col = floor(4*M_horiz)+2-ceil((width_wedge+1)/2)+...
            mod(width_wedge+1,2)*(quadrant-3 == mod(quadrant-3,2));
                % Coordinates of the top-left corner of the wedge wrapped
                % around the origin. Some subtleties when the wedge is
                % even-sized because of the forthcoming 90 degrees rotation
        for row = Y_corner
            cols = left_line(row) + mod((0:(width_wedge-1))-(left_line(row)-first_col),width_wedge);
            admissible_cols = round(1/2*(cols+1+abs(cols-1)));
            new_row = 1 + mod(row - first_row, length_corner_wedge);
            wrapped_data(new_row,:) = Xhi(row,admissible_cols) .* (cols > 0);
            wrapped_XX(new_row,:) = XX(row,admissible_cols);
            wrapped_YY(new_row,:) = YY(row,admissible_cols);
        end;
        slope_wedge_right = (floor(4*M_horiz)+1 - wedge_midpoints(1))/floor(4*M_vert);
        mid_line_right = wedge_midpoints(1) + slope_wedge_right*(wrapped_YY - 1);
                % not integers in general
        coord_right = 1/2 + floor(4*M_vert)/(wedge_endpoints(2) - wedge_endpoints(1)) * ...
            (wrapped_XX - mid_line_right)./(floor(4*M_vert)+1 - wrapped_YY);
        C2 = 1/(1/(2*(floor(4*M_horiz))/(wedge_endpoints(1) - 1) - 1) + 1/(2*(floor(4*M_vert))/(first_wedge_endpoint_vert - 1) - 1));
        C1 = C2 / (2*(floor(4*M_vert))/(first_wedge_endpoint_vert - 1) - 1);
        wrapped_XX((wrapped_XX - 1)/floor(4*M_horiz) + (wrapped_YY-1)/floor(4*M_vert) == 2) = ...
            wrapped_XX((wrapped_XX - 1)/floor(4*M_horiz) + (wrapped_YY-1)/floor(4*M_vert) == 2) + 1;
        coord_corner = C1 + C2 * ((wrapped_XX - 1)/(floor(4*M_horiz)) - (wrapped_YY - 1)/(floor(4*M_vert))) ./ ...
            (2-((wrapped_XX - 1)/(floor(4*M_horiz)) + (wrapped_YY - 1)/(floor(4*M_vert))));
        wl_left = fdct_wrapping_window(coord_corner);
        [wl_right,wr_right] = fdct_wrapping_window(coord_right);
        wrapped_data = wrapped_data .* (wl_left .* wr_right);

        switch is_real
            case 0
                wrapped_data = rot90(wrapped_data,-(quadrant-1));
                C{j}{l} = fftshift(ifft2(ifftshift(wrapped_data)))*sqrt(prod(size(wrapped_data)));
            case 1
                wrapped_data = rot90(wrapped_data,-(quadrant-1));
                x = fftshift(ifft2(ifftshift(wrapped_data)))*sqrt(prod(size(wrapped_data)));
                C{j}{l} = sqrt(2)*real(x);
                C{j}{l+nbangles(j)/2} = sqrt(2)*imag(x);
        end;

        % Regular wedges
        length_wedge = floor(4*M_vert) - floor(M_vert);
        Y = 1:length_wedge;
        first_row = floor(4*M_vert)+2-ceil((length_wedge+1)/2)+...
            mod(length_wedge+1,2)*(quadrant-2 == mod(quadrant-2,2));
        for subl = 2:(nbangles_perquad-1);
            l = l+1;
            width_wedge = wedge_endpoints(subl+1) - wedge_endpoints(subl-1) + 1;
            slope_wedge = ((floor(4*M_horiz)+1) - wedge_endpoints(subl))/floor(4*M_vert);
            left_line = round(wedge_endpoints(subl-1) + slope_wedge*(Y - 1));
            [wrapped_data, wrapped_XX, wrapped_YY] = deal(zeros(length_wedge,width_wedge));
            first_col = floor(4*M_horiz)+2-ceil((width_wedge+1)/2)+...
                mod(width_wedge+1,2)*(quadrant-3 == mod(quadrant-3,2));
            for row = Y
                cols = left_line(row) + mod((0:(width_wedge-1))-(left_line(row)-first_col),width_wedge);
                new_row = 1 + mod(row - first_row, length_wedge);
                wrapped_data(new_row,:) = Xhi(row,cols);
                wrapped_XX(new_row,:) = XX(row,cols);
                wrapped_YY(new_row,:) = YY(row,cols);
            end;
            slope_wedge_left = ((floor(4*M_horiz)+1) - wedge_midpoints(subl-1))/floor(4*M_vert);
            mid_line_left = wedge_midpoints(subl-1) + slope_wedge_left*(wrapped_YY - 1);
            coord_left = 1/2 + floor(4*M_vert)/(wedge_endpoints(subl) - wedge_endpoints(subl-1)) * ...
                (wrapped_XX - mid_line_left)./(floor(4*M_vert)+1 - wrapped_YY);
            slope_wedge_right = ((floor(4*M_horiz)+1) - wedge_midpoints(subl))/floor(4*M_vert);
            mid_line_right = wedge_midpoints(subl) + slope_wedge_right*(wrapped_YY - 1);
            coord_right = 1/2 + floor(4*M_vert)/(wedge_endpoints(subl+1) - wedge_endpoints(subl)) * ...
                (wrapped_XX - mid_line_right)./(floor(4*M_vert)+1 - wrapped_YY);
            wl_left = fdct_wrapping_window(coord_left);
            [wl_right,wr_right] = fdct_wrapping_window(coord_right);
            wrapped_data = wrapped_data .* (wl_left .* wr_right);
            switch is_real
                case 0
                    wrapped_data = rot90(wrapped_data,-(quadrant-1));
                    C{j}{l} = fftshift(ifft2(ifftshift(wrapped_data)))*sqrt(prod(size(wrapped_data)));
                case 1
                    wrapped_data = rot90(wrapped_data,-(quadrant-1));
                    x = fftshift(ifft2(ifftshift(wrapped_data)))*sqrt(prod(size(wrapped_data)));
                    C{j}{l} = sqrt(2)*real(x);
                    C{j}{l+nbangles(j)/2} = sqrt(2)*imag(x);
            end;
        end;

        % Right corner wedge
        l = l+1;
        width_wedge = 4*floor(4*M_horiz) + 3 - wedge_endpoints(end) - wedge_endpoints(end-1);
        slope_wedge = ((floor(4*M_horiz)+1) - wedge_endpoints(end))/floor(4*M_vert);
        left_line = round(wedge_endpoints(end-1) + slope_wedge*(Y_corner - 1));
        [wrapped_data, wrapped_XX, wrapped_YY] = deal(zeros(length_corner_wedge,width_wedge));
        first_row = floor(4*M_vert)+2-ceil((length_corner_wedge+1)/2)+...
            mod(length_corner_wedge+1,2)*(quadrant-2 == mod(quadrant-2,2));
        first_col = floor(4*M_horiz)+2-ceil((width_wedge+1)/2)+...
            mod(width_wedge+1,2)*(quadrant-3 == mod(quadrant-3,2));
        for row = Y_corner
            cols = left_line(row) + mod((0:(width_wedge-1))-(left_line(row)-first_col),width_wedge);
            admissible_cols = round(1/2*(cols+2*floor(4*M_horiz)+1-abs(cols-(2*floor(4*M_horiz)+1))));
            new_row = 1 + mod(row - first_row, length_corner_wedge);
            wrapped_data(new_row,:) = Xhi(row,admissible_cols) .* (cols <= (2*floor(4*M_horiz)+1));
            wrapped_XX(new_row,:) = XX(row,admissible_cols);
            wrapped_YY(new_row,:) = YY(row,admissible_cols);
        end;
        slope_wedge_left = ((floor(4*M_horiz)+1) - wedge_midpoints(end))/floor(4*M_vert);
        mid_line_left = wedge_midpoints(end) + slope_wedge_left*(wrapped_YY - 1);
        coord_left = 1/2 + floor(4*M_vert)/(wedge_endpoints(end) - wedge_endpoints(end-1)) * ...
            (wrapped_XX - mid_line_left)./(floor(4*M_vert) + 1 - wrapped_YY);
        C2 = -1/(2*(floor(4*M_horiz))/(wedge_endpoints(end) - 1) - 1 + 1/(2*(floor(4*M_vert))/(first_wedge_endpoint_vert - 1) - 1));
        C1 = -C2 * (2*(floor(4*M_horiz))/(wedge_endpoints(end) - 1) - 1);
        wrapped_XX((wrapped_XX - 1)/floor(4*M_horiz) == (wrapped_YY - 1)/floor(4*M_vert)) = ...
            wrapped_XX((wrapped_XX - 1)/floor(4*M_horiz) == (wrapped_YY - 1)/floor(4*M_vert)) - 1;
        coord_corner = C1 + C2 * (2-((wrapped_XX - 1)/(floor(4*M_horiz)) + (wrapped_YY - 1)/(floor(4*M_vert)))) ./ ...
            ((wrapped_XX - 1)/(floor(4*M_horiz)) - (wrapped_YY - 1)/(floor(4*M_vert)));
        wl_left = fdct_wrapping_window(coord_left);
        [wl_right,wr_right] = fdct_wrapping_window(coord_corner);

        wrapped_data = wrapped_data .* (wl_left .* wr_right);
        switch is_real
            case 0
                wrapped_data = rot90(wrapped_data,-(quadrant-1));
                C{j}{l} = fftshift(ifft2(ifftshift(wrapped_data)))*sqrt(prod(size(wrapped_data)));
            case 1
                wrapped_data = rot90(wrapped_data,-(quadrant-1));
                x = fftshift(ifft2(ifftshift(wrapped_data)))*sqrt(prod(size(wrapped_data)));
                C{j}{l} = sqrt(2)*real(x);
                C{j}{l+nbangles(j)/2} = sqrt(2)*imag(x);
        end;

        if quadrant < nbquadrants, Xhi = rot90(Xhi); end;
    end;
end;

% Coarsest wavelet level
C{1}{1} = fftshift(ifft2(ifftshift(Xlow)))*sqrt(prod(size(Xlow)));
if is_real == 1,
    C{1}{1} = real(C{1}{1});
end;


function [S,m,psi] = ComputeStokesQuad(HH,HV,VH,VV,SmoothSize)
%COMPUTESTOKESQUAD: Alpha/Entropy approximation for quad pol data (used
%Stokes for dual so we're keeping the name for consistency)

%Pauli basis
P_11 = fastrunmean(abs(HH+VV).*abs(HH+VV),[SmoothSize SmoothSize],'mean');
P_22 = fastrunmean(abs(HH-VV).*abs(HH-VV),[SmoothSize SmoothSize],'mean');
P_33 = fastrunmean(abs(HV-VH).*abs(HV-VH),[SmoothSize SmoothSize],'mean');

%span is the trace of the Pauli basis
S = P_11 + P_22 + P_33;

N_11 = P_11./S;
N_22 = P_22./S;
N_33 = P_33./S;

Frob = sqrt(N_11.*N_11 + N_22.*N_22 + N_33.*N_33);
m = 1.5*(1-Frob.*Frob);
psi = acosd(sqrt(N_11));
%psi = (pi/2)*(1-N_11);

end


function K=compute_permutation_constraint4(V)

% Copyright (C) <2007>  <Francesc Moreno-Noguer, Vincent Lepetit, Pascal Fua>
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the version 3 of the GNU General Public License
% as published by the Free Software Foundation.
%
% 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/>.
%
% Francesc Moreno-Noguer, CVLab-EPFL, September 2007.
% fmorenoguer@gmail.com, http://cvlab.epfl.ch/~fmoreno/

%[B11,B12,...,B33]=lambda1*v1+lambda2*v2+lambda3*v3

N=size(V,2); %dimension of the kernel
n=4; %dimension of Bij
idx=[1 2 3 4; 2 5 6 7; 3 6 8 9; 4 7 9 10];

%1.-Generation of the first set of equations Bii.Bjj=Bij.Bii  (n(n-1)/2 eqs).
nrowsK=n*(n-1)/2+n*(n-1)*n/2;
ncolsK=N*(N+1)/2;
K=zeros(nrowsK,ncolsK);

t=1;
for i=1:n
    for j=i+1:n
        offset=1;
        for a=1:N
            for b=a:N
                if a==b
                    K(t,offset)=V(idx(i,i),a)*V(idx(j,j),a)-V(idx(i,j),a)*V(idx(i,j),a);
                else
                    K(t,offset)=V(idx(i,i),a)*V(idx(j,j),b)-V(idx(i,j),a)*V(idx(i,j),b)+...
                                V(idx(i,i),b)*V(idx(j,j),a)-V(idx(i,j),b)*V(idx(i,j),a);
                end
                offset=offset+1;
            end

        end
        t=t+1;
        %fprintf('t:%d\t offset:%d\n',t,offset);
    end
end

for k=1:n
    for j=k:n
        for i=1:n
            if (i~=j & i~=k)
                offset=1;
                for a=1:N
                    for b=a:N
                        if a==b
                            K(t,offset)=V(idx(i,j),a)*V(idx(i,k),a)-V(idx(i,i),a)*V(idx(j,k),a);
                        else
                            K(t,offset)=V(idx(i,j),a)*V(idx(i,k),b)-V(idx(i,i),a)*V(idx(j,k),b)+...
                                        V(idx(i,j),b)*V(idx(i,k),a)-V(idx(i,i),b)*V(idx(j,k),a);
                        end
                        offset=offset+1;
                    end

                end
                t=t+1;
                %fprintf('t:%d\t offset:%d\n',t,offset);
            end
        end
    end
end


function [logp, yhat, res] = tapas_condhalluc_obs(r, infStates, ptrans)
% Calculates the log-probability of response y=1 under the unit-square sigmoid model
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2015-2016 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Transform beta to its native space
be = exp(ptrans(1));

% Initialize returned log-probabilities as NaNs so that NaN is
% returned for all irregualar trials
n = size(infStates,1);
logp = NaN(n,1);
yhat = NaN(n,1);
res  = NaN(n,1);

% Check input format
if size(r.u,2) ~= 2
    error('tapas:hgf:CondHalluc:InputsIncompatible', 'Inputs incompatible with condhalluc_obs observation model. See tapas_condhalluc_obs_config.m.')
end

% Get true-positive rate corresponding to stimuli
tp = r.u(:,2);

% Weed irregular trials out
mu1hat = infStates(:,1,1);
mu1hat(r.irr) = [];
y = r.y(:,1);
y(r.irr) = [];
tp(r.irr) = [];

% Calculate belief x using Bayes' theorem
x = tp.*mu1hat./(tp.*mu1hat + (1-mu1hat).^2);

% Belief is mu1hat in trials where there is no tone
x(find(tp==0)) = mu1hat(find(tp==0));

% Calculate log-probabilities for non-irregular trials
reg = ~ismember(1:n,r.irr);
logp(reg) = -log(1+exp(-be.*(2.*x-1).*(2.*y-1)));
yhat(reg) = x;
res(reg) = (y-x)./sqrt(x.*(1-x));

return;


function e = mgVcyclefracLap1d(A,r,pde,square,h,s,option)

global mA

%% Setting
if ~isfield('option','smootherType')
    smootherType = option.smootherType;
else
    smootherType = 3;
end
if ~isfield('option','smootherstep')
    smoothingstep = option.smoothingstep;
else
    smoothingstep = 3;
end
if ~exist('smootherType','var'), smootherType = 3; end
% mapping parameter
if s == 0.5
    gamma = 1;
else
    gamma = 3/(2*s)+0.1;
end

%% line smoothing
N = length(r);
rold = r;
x0 = square(1); x1 = square(2);
y0 = square(3); y1 = square(4);
nx = (x1-x0)/h+1;  % number of grid points in x-direction
ny = (y1-y0)/h+1;  % number of grid points in x-direction
% 1D vector to 2D matrix
r2d = reshape(r,ny,nx);
e = zeros(N,1);
e2d = reshape(e,ny,nx);
% index for Red-Black smoothing
nodeidx = reshape(1:N,ny,nx);
redidx = 2:2:nx-1;
blackidx = 3:2:nx-2;
rlinear = nodeidx(:,redidx);
blinear = nodeidx(:,blackidx);
for it = 1:smoothingstep
    switch smootherType
        case 0
            e = e + tril(A)\r;
            r = rold - A*e;
            e2d = reshape(e,ny,nx);
        case 1
            for k = 2:nx-1 % only update interiori lines
                % form residual
                idx = (k-1)*ny+1:k*ny;  % the end nodes is included?
                de = A(idx,idx)\r2d(:,k);
                e2d(:,k) = e2d(:,k) + de;
                r2d(:,k) = 0;  % exact solve
                if k<nx-1   % update right column
                    r2d(:,k+1) = r2d(:,k+1) - A(idx+ny,idx)*de;
                end
                if k>2    % update left column
                % for left-to-right ordering, the left r(idx-ny) is zero
                    r2d(:,k-1) = - A(idx-ny,idx)*de;
                end
            end
        case 2 % weighted Jacobi
            for k = 2:nx-1
                % form residual
                idx = (k-1)*ny+1:k*ny;
                de = A(idx,idx)\r(idx);
                e(idx) = e(idx) + 0.25*de;
            end
            r = rold - A*e;
        case 3 % red-black block Gauss-Seidel
            idx = ny+1:2*ny;
            % 1: red lines
            de = A(idx,idx)\r2d(:,redidx);
            e2d(:,redidx) = e2d(:,redidx) + de;
            % update residual
            r2d(:,redidx) = 0;
            r2d(:,blackidx) = r2d(:,blackidx)-reshape(A(blinear,rlinear)*de(:),ny,length(blackidx));
            % previous r2d(:,blackidx) = 0;
            % 2: black lines
            de = A(idx,idx)\r2d(:,blackidx);
            e2d(:,blackidx) = e2d(:,blackidx) + de;
            % update residual
            r2d(:,blackidx) = 0;
            r2d(:,redidx) = -reshape(A(rlinear,blinear)*de(:),ny,length(redidx));
            % previous r2d(:,redidx) = 0;
    end
end

%% Transfer operator
% prolongation and restriction in x-direction
Ix = prolongation1d(log2(nx-1)-1);
Rx = Ix';
% prolongation and restriction in y-direction
nxc = (nx - 1)/2 + 1;
nyc = (ny - 1)/2 + 1;
% geometric quantity
My = ny - 1;
Tyf = ((0:My)'/My).^gamma*(y1-y0) + y0;
hf = diff(Tyf);
Myc = nyc - 1;
Tyc = ((0:Myc)'/Myc).^gamma*(y1-y0) + y0;
hc = diff(Tyc);
jc = 2:nyc-1;  % interiori points in the coarse grid
j = 2*jc-1;  % index of coarse points in the fine grids
alpha = zeros(1,nyc);
beta = zeros(1,nyc);
alpha(jc) = hf(j)./hc(jc);
beta(jc) = hf(j-1)./hc(jc-1);
alpha(1) = 1-beta(2);
jc = 2:(nyc-1);
jf = 2*jc-1;  % index of coarse points in the fine grids
% due to the Neumann boundary condition, 1 is included
ii = [1 jf 2 jf+1 jf-1];
jj = [1 jc 1 jc jc];
ss = [ones(1,nyc-1) alpha(1:nyc-1) beta(2:nyc-1)];
Iy = sparse(ii,jj,ss,ny,nyc);
Ry = Iy';

%% Restriction
% 1D vector to 2D matrix
% r2d = reshape(r,ny,nx);
rc2d = Ry*r2d*Ix;
rc = rc2d(:);

%% Coarse grid correction
% option.solver = 'none';
% [u,eqn] = fracLap1d(square,2*h,pde,option);
level = -log2(h);
Ac = mA{level-1};
fixedNode = [1:nyc (2:nxc-1)*nyc (nxc-1)*nyc+(1:nyc)];
isBdNode = false(length(rc),1);
isBdNode(fixedNode) = true;
freeNode = find(~isBdNode);
if level <= 3
    ec = zeros(length(rc),1);
    ec(freeNode) = Ac(freeNode,freeNode)\rc(freeNode);
else
    ec = mgVcyclefracLap1d(Ac,rc,pde,square,2*h,s,option);
end

%% Prolongation
% 1D vector to 2D matrix
ec2d = reshape(ec,nyc,nxc);
e2d = e2d + Iy*ec2d*Rx;
e = e2d(:);
% update residual when e updated. remember we are solving Ae = rold.
r = rold - A*(e2d(:));
r2d = reshape(r,ny,nx);

%% Post-smoothing
for it = 1:smoothingstep
    switch smootherType
        case 0 % pointwise G-S smoothing
            e = e + triu(A)\r;
            r = rold - A*e;
            e2d = reshape(e,ny,nx);
        case 1
            for k = nx-1:-1:2
                % form residual
                idx = (k-1)*ny+1:k*ny;
                de = A(idx,idx)\r2d(:,k);
                e2d(:,k) = e2d(:,k) + de;
                r2d(:,k) = 0;
                if k < nx-1   % update right column
                % for right-to-left ordering, the right r(idx+ny) is zero
                    r2d(:,k+1) = - A(idx+ny,idx)*de;
                end
                if k>2    % update previous column
                    r2d(:,k-1) = r2d(:,k-1) - A(idx-ny,idx)*de;
                end
            end
        case 2 % weighted Jacobi
            for k = 1:nx
                % form residual
                idx = (k-1)*ny+1:k*ny;
                de = A(idx,idx)\r(idx);
                e(idx) = e(idx) + 0.25*de;
            end
            r = rold - A*e;
        case 3 % red-black block Gauss-Seidel
            idx = ny+1:2*ny;
            % 1: red lines
            de = A(idx,idx)\r2d(:,redidx);
            e2d(:,redidx) = e2d(:,redidx) + de;
            % update residual
            r2d(:,redidx) = 0;
            r2d(:,blackidx) = r2d(:,blackidx)-reshape(A(blinear,rlinear)*de(:),ny,length(blackidx));
            % previous r2d(:,blackidx) = 0;
            % 2: black lines
            de = A(idx,idx)\r2d(:,blackidx);
            e2d(:,blackidx) = e2d(:,blackidx) + de;
            % update residual
            r2d(:,blackidx) = 0;
            r2d(:,redidx) = -reshape(A(rlinear,blinear)*de(:),ny,length(redidx));
            % previous r2d(:,redidx) = 0;
    end
end
e = e2d(:);


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function y = sabrtmax(asabr,b,r,nsabr,f,fbar,eps)
% calclates the maximum time for the sabr approximation formula
% to be valid (see Risk paper by Doust)

    a = asabr / eps;
    n = nsabr / eps;

    zF = f^(1-b)/asabr/(1-b);
    zFbar = fbar.^(1-b)/asabr/(1-b);

    fav = sqrt(f*fbar);
    gamma1 = b./fav;
    gamma2 = b*(b-1)./fav.^2;

    kH = 0.125 * (2*gamma2-gamma1.^2)*a^2.*fav.^(2*b) ...
        + 0.75 * r*n*a*gamma1.*fav.^b ...
        + 0.125*(2-3*r^2)*n^2;

    z = zF - zFbar;
    integral = (f^(1-b)-fbar.^(1-b)) / (1-b);

    xz = 1/nsabr *log((sqrt(1-2*nsabr*r*z+nsabr^2*z.^2)-r+nsabr*z)/(1-r));
    if f==fbar
       y = 12 ./(eps^2*8 * kH);
    else
       y = 12 ./(eps^2*(8 * kH + a^2*(log(f./fbar)./integral .* (z./xz)).^2));
    end
end


function se3mat = VecTose3(V)
% *** CHAPTER 3: RIGID-BODY MOTIONS ***
% Takes a 6-vector (representing a spatial velocity).
% Returns the corresponding 4x4 se(3) matrix.
% Example Input:
%
% clear; clc;
% V = [1; 2; 3; 4; 5; 6];
% se3mat = VecTose3(V)
%
% Output:
% se3mat =
%     0    -3     2     4
%     3     0    -1     5
%    -2     1     0     6
%     0     0     0     0

se3mat = [VecToso3(V(1: 3)), V(4: 6); 0, 0, 0, 0];
end


function [gnodes, gedges] = relativeNeighborhoodGraph(points)
%RELATIVENEIGHBORHOODGRAPH Relative Neighborhood Graph of a set of points
%
%   [NODES, EDGES] = relativeNeighborhoodGraph(POINTS)
%   EDGES = relativeNeighborhoodGraph(POINTS)
%
%   The Relative Neighborhood Graph (RNG) is a subgraph of the Delaunay
%   Triangulation computed from the same set of points. The Gabriel graph
%   and the euclidean minimal spanning tree (EMST) are subgraphs of the
%   RNG.
%
%   Example
%     nodes = rand(100, 2) * 100;
%     edges = relativeNeighborhoodGraph(nodes);
%     figure; drawGraph(nodes, edges);
%
%   See also
%     gabrielGraph, euclideanMST
%

% ------
% Author: David Legland
% e-mail: david.legland@nantes.inra.fr
% Created: 2016-03-02,    using Matlab 8.6.0.267246 (R2015b)
% Copyright 2016 INRA - Cepia Software Platform.

% first compute Delaunay triangulation to reduce further computations
DT = delaunayTriangulation(points);
E = edges(DT);

% compute edge lengths
nEdges = size(E, 1);
edgeLengths = zeros(nEdges, 1);
for i = 1:nEdges
    edgeLengths(i) = distancePoints(points(E(i,1),:), points(E(i,2),:));
end

% identify indices of faces attached to each vertex
vertexFaces = vertexAttachments(DT);

% iterate over edges to check if the should be kept
keepEdge = true(nEdges, 1);
for iEdge = 1:nEdges
    iVertex1 = E(iEdge, 1);
    iVertex2 = E(iEdge, 2);
    vertex1 = points(iVertex1, :);
    vertex2 = points(iVertex2, :);

    % compute indices of faces containing one of the two vertices
    inds = [vertexFaces{iVertex1} vertexFaces{iVertex2}];
    localFaces = DT.ConnectivityList(inds, :);

    % compute indices of vertices is the first neighborhood of the edge
    inds = unique(localFaces);
    inds(ismember(inds, [iVertex1 iVertex2])) = [];

    % compute max of distances to both original vertices
    dists1 = distancePoints(vertex1, points(inds, :));
    dists2 = distancePoints(vertex2, points(inds, :));
    distsMax = max(dists1, dists2);

    % keep edge if all points are outside the "lunule" defined by the edge
    if edgeLengths(iEdge) > min(distsMax)
        keepEdge(iEdge) = false;
    end
end

% filter edges
gedges = E(keepEdge, :);

% format output
gnodes = points;
if nargin == 1
    gnodes = gedges;
end


% Calculate arena coverage (the amount of space an animal has covered during experiment)
%
% Calculates arena coverage.
%
%  USAGE
%   coverage = analyses.arenaCoverage(pos, binWidth, shape, dimensions)
%   pos         Position samples, matrix of size at least Nx2. Format is either [t x] or [t x y].
%   binWidth    Width of horizontal and vertical bins in cm. If only one value is provided, then
%               the same bin width is used in both directions.
%   shape       Arena shape, integer. One of the values of bntConstants.ArenaShape.
%   dimensions  Vector of arena dimensions, the actual number of elements depends on arena shape.
%
%   coverage    Arena coverage, float in range [0..100].
%
function coverage =  arenaCoverage(pos, binWidth, shape, dimensions)
    inp = inputParser;

    checkPosDimensions = @(x) size(x, 2) >= 2;
    checkBinWidth = @(x) helpers.isdvector(x, '>=0') && length(x) <= 2;
    checkShape = @(x) ismember(x, helpers.ArenaShape.allShapes());
    checkDims = @(x) helpers.isdvector(x, '>=0') && (x(1) > 0);

    addRequired(inp, 'pos', checkPosDimensions);
    addRequired(inp, 'binWidth', checkBinWidth);
    addRequired(inp, 'shape', checkShape);
    addRequired(inp, 'dimensions', checkDims);

    parse(inp, pos, binWidth, shape, dimensions);

    t = pos(:, 1);
    x = pos(:, 2);
    if size(pos, 2) > 2
        y = pos(:, 3);
    else
        y = [];
    end

    binWidthX = binWidth(1);
    if length(binWidth) == 1
        binWidthY = binWidthX;
    else
        binWidthY = binWidth(2);
    end

    % filter out points that lie outside arena. This is important for circles
    % because they do not fully cover all the bins.
    if shape == bntConstants.ArenaShape.Circle
        radius = dimensions(1)/2;
        ind = sqrt(x.^2 + y.^2) > radius;
        x(ind) = nan;
        y(ind) = nan;
    end

    limitsX = [-dimensions(1)/2 dimensions(1)/2];
    [xBinned, nBinsX, edgesX] = helpers.bin(x, limitsX, binWidthX);
    nBins = nBinsX;

    if ~isempty(y)
        if length(dimensions) == 1
            if shape == bntConstants.ArenaShape.Track
                yLength = 1;
            else
                yLength = dimensions(1);
            end
        else
            if shape == bntConstants.ArenaShape.Track && dimensions(2) == 0
                yLength = 1;
            else
                yLength = dimensions(2);
            end
        end

        limitsY = [-yLength/2 yLength/2];
        [yBinned, nBinsY, edgesY] = helpers.bin(y, limitsY, binWidthY);
        nBins = [nBinsX nBinsY];
    end

    dt = diff(t);
    dt(end+1) = dt(end);

    if isempty(y)
        occupancy = general.accumulate(xBinned, dt, nBinsX)';
    else
        occupancy = general.accumulate([xBinned yBinned], dt, nBins)';
    end

    switch shape
        case bntConstants.ArenaShape.Circle
            radius = dimensions(1)/2;
            radiusBin = ceil(radius / binWidthX) + 1;

            halfSize = ceil(size(occupancy)/2);
            [rr, cc] = meshgrid(1:nBinsX, 1:nBinsY);

            distMap = sqrt((cc - halfSize(2)).^2 + (rr - halfSize(1)).^2); % each element is the distance
                                                % from the middle of the map to current point
            outerCircle = distMap >= radiusBin;
            distMap(outerCircle) = nan;

            numBins = sum(sum(isfinite(distMap)));
            coverage = (length(find(occupancy > 0)) / numBins) * 100;
        otherwise
            coverage = length(find(occupancy > 0)) / prod(nBins) * 100;
    end
end


classdef matRad_EUD < DoseObjectives.matRad_DoseObjective
% matRad_EUD Implements a penalized equivalent uniform dose objective
%   See matRad_DoseObjective for interface description
%
% References
%   -
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Copyright 2020 the matRad development team.
%
% This file is part of the matRad project. It is subject to the license
% terms in the LICENSE file found in the top-level directory of this
% distribution and at https://github.com/e0404/matRad/LICENSES.txt. No part
% of the matRad project, including this file, may be copied, modified,
% propagated, or distributed except according to the terms contained in the
% LICENSE file.
%
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    properties (Constant)
        name = 'EUD';
        parameterNames = {'EUD^{ref}', 'k'};
        parameterTypes = {'dose','numeric'};
    end

    properties
        parameters = {0, 3.5};
        penalty = 1;
    end

    methods
        function obj = matRad_EUD(penalty,eudRef, eudExponent)
            %If we have a struct in first argument
            if nargin == 1 && isstruct(penalty)
                inputStruct = penalty;
                initFromStruct = true;
            else
                initFromStruct = false;
                inputStruct = [];
            end

            %Call Superclass Constructor (for struct initialization)
            obj@DoseObjectives.matRad_DoseObjective(inputStruct);

            %now handle initialization from other parameters
            if ~initFromStruct
                if nargin >= 3 && isscalar(eudExponent)
                    obj.parameters{2} = eudExponent;
                end

                if nargin >= 2 && isscalar(eudRef)
                    obj.parameters{1} = eudRef;
                end

                if nargin >= 1 && isscalar(penalty)
                    obj.penalty = penalty;
                end
            end
        end

        %% Calculates the Objective Function value
        function fDose = computeDoseObjectiveFunction(obj,dose)
            % get exponent for EUD
            k = obj.parameters{2};

            % calculate power sum
            powersum = sum(dose.^k);

            %Calculate objective

            %This check is not needed since dose is always positive
            %if powersum > 0
            fDose = obj.penalty * (nthroot(powersum/numel(dose),k) - obj.parameters{1})^2;
            %end
        end

        %% Calculates the Objective Function gradient
        function fDoseGrad  = computeDoseObjectiveGradient(obj,dose)
            % get exponent for EUD
            k = obj.parameters{2};

            %numerical stability
            dose(dose == 0) = 0.001;

            % calculate power sum
            powersum = sum(dose.^k);

            %This check is not needed since dose is always positive
            %if powersum > 0

            %derivatives = nthroot(1/numel(dose),k) * powersum^((1-k)/k) * (dose.^(k-1));
            fDoseGrad = 2 * obj.penalty * nthroot(1/numel(dose),k) * powersum^((1-k)/k) * (dose.^(k-1)) .* (nthroot(powersum/numel(dose),k) - obj.parameters{1});
            %end
            if any(~isfinite(fDoseGrad)) % check for inf and nan for numerical stability
                error(['EUD computation failed. Reduce exponent to resolve numerical problems.']);
            end
        end
    end

end


function x = vecpostproc(x, a)
% VECPOSTPROC is post-processing of a D-dimensional vector.
%
%   V = vecpostproc(V) outputs L2 normalized vector:
%     V = V ./ L2NORM(V);
%
%   V = vecpostproc(V, A) outputs L2 and power-law normalized vector:
%     V = SIGN(X) .* ABS(X) .^ A;
%     V = V ./ L2NORM(V);
%
% Authors: F. Radenovic, G. Tolias, O. Chum. 2017.

    if ~exist('a'), a = 1; end
    x = replacenan (l2_normalize (powerlaw (x, a)));

function x = l2_normalize(x)
    l = sqrt(sum(x.^2));
    x = bsxfun(@rdivide,x,l);
    x = replacenan(x);

function x = powerlaw (x, a)
	if a == 1, return; end
	x = sign (x) .* abs(x)  .^ a;

function y = replacenan (x, v)
	if ~exist ('v')
	  v = 0;
	end
	y = x;
	y(isnan(x)) = v;


%{
This file shows an example of using the full power of TFOCS. If you
are not familiar with basic usage of TFOCS, please see other demos
first.

We make an example that uses:
    -several variables (2), and the variables are matrices, not just vectors
    -several constraints (4)
    -affine operators (linear plus offset), and two of the operators
        are matrix --> matrix operators
    -debug mode

For small complicated examples like this, TFOCS is not necessarily
faster than software like CVX, since it takes more iterations than an
interior point method and there is some overhead in the TFOCS software
since the software is meant for flexibility rather than absolute speed.

However, if you take any complicated problem and scale the size by a factor
of 100, then CVX won't be able to handle it at all. TFOCS will do
just fine (and it might even be less than 100x as slow, since now the overhead
is not significant).

The problem we will solve:

min_{X1, X2} smooth1(X1)+smooth2(X2) +  sum_{i=1}^4  g_i( A1_i*X1 + A2_i*X2 + B_i )

meaning that we have 2 variables (X1 and X2), both of which are matrices,
and each variable has its own smooth function.
For non-smooth and/or constraint terms (indexed by "i"), we have 4 functions
( i = 1,2,3,4) g_i, and each has it's own affine operator in X1 and X2.
So each of the 4 affine operators has three parts: the portion linear in X1,
the portion linear in X2, and the constant offset "B_i".

To be explicit, the smooth functions are linear and quadratic, resp.:

smooth1(X1) = dot( s1, X1 ) + 3.4                  ("s1" is a matrix the same size as X1)
smooth2(X2) = dot( X2, X2 ) + dot( s2, X2 ) + 4.5  ("s2" is a matrix the same size as X2)

and the non-smooth/constraint terms are:

g_1(z)  = indicator set of the positive orthant of R^10
g_2(z)  = ||z||_2 (usual Euclidean norm) in R^15
g_3(z)  = ||z||_1 (l1 norm) in R^{10 x 20 }. This views a 10 x 20 matrix as a 200 x 1 vector.
g_4(z)  = indicator set of positive orthant of R^{20 x 22 }, i.e. each element must be >= 0 coordinate-wise

The affine operators are picked arbitrary, but some of them (#3 and #4) have a range that
is a set of matrices, rather than a set of vectors, in order to make
this more interesting.

%}

fileName = fullfile('reference_solutions','complicatedProblem1');
randn('state',29324);
rand('state',9332);
% rng(3481); % this only works on new versions of Matlab

% -- Variables --
% Two sets of variables, X1 (a matrix of size n1 x n2 ) and X2 (matrix of N1 x N2 )
n1 = 10; n2 = 20;
N1 = 12; N2 = 18;
X1 = zeros( n1, n2 );
X2 = zeros( N1, N2 );

% -- the smooth terms --
%   Note: the inner product used is the matrix inner product
%       that induces the Frobenius norm.
s1      = randn(n1,n2);
smooth1 = smooth_linear( s1, 3.4 );
s2      = randn(N1,N2);
smooth2 = smooth_quad( 1, s2, 4.5 );
% and the same thing, but in a format that CVX likes:
smoothF = @(X1,X2) vec(X1)'*vec(s1) + 3.4 + vec(X2)'*vec(s2) + 4.5 + ...
    sum_square(vec(X2))/2;

% -- some proximal terms --

nProx   = 4;
prox1   = proj_Rplus;
prox2   = proj_l2;          % primal is norm( , 2)
prox3   = proj_linf(1);     % this can take matrix varaibles...
prox4   = proj_Rplus;       % this can take matrix variables...
% Sizes of proxes (i.e. sizes of dual variables, i.e. size of range of linear terms)
d1      = [ 10, 1  ];
d2      = [ 15, 1  ];
d3      = [ n1, n2 ];
d4      = [ 20, 22 ];

% -- and linear terms --

% for prox1:
const1  = randn(d1);
temp1   = randn( prod(d1), n1*n2);
A1_X1   = linop_compose( linop_matrix(temp1), linop_vec([n1,n2]) );
temp2   = randn( prod(d1), N1*N2);
A1_X2   = linop_compose( linop_matrix(temp2), linop_vec([N1,N2]) );

A1      = @(X1,X2) temp1*vec(X1) + temp2*vec(X2) + const1;

% for prox2: (matrix variable)
const2  = randn(d2);
temp1   = randn( prod(d2), n1*n2);
A2_X1   = linop_compose( linop_matrix(temp1), linop_vec([n1,n2]) );
temp2   = randn( prod(d2), N1*N2);
A2_X2   = linop_compose( linop_matrix(temp2), linop_vec([N1,N2]) );

A2      = @(X1,X2) temp1*vec(X1) + temp2*vec(X2) + const2;

% for prox3:
const3  = 0;
A3_X1   = 63.4;     % this represents abstract scaling, i.e. any size input
A3_X2   = 0;        % this reprsents the zero linear operator

A3      = @(X1,X2) A3_X1*X1;

% for prox4: (matrix variable)
const4  = randn(d4);
temp1   = randn( prod(d4), n1*n2);
A4_X1   = linop_compose( linop_matrix(temp1), linop_vec([n1,n2]) );
rs      = linop_adjoint( linop_vec(d4) );
A4_X1   = linop_compose( rs, A4_X1 );
mat1    = @(x) reshape( x, d4(1), d4(2) );

temp2   = randn( prod(d4), N1*N2);
A4_X2   = linop_compose( linop_matrix(temp2), linop_vec([N1,N2]) );
rs      = linop_adjoint( linop_vec(d4) );
A4_X2   = linop_compose( rs, A4_X2 );

A4      = @(X1,X2) mat1( temp1*vec(X1) + temp2*vec(X2) ) + const4;

% -- set the smoothing parameter --
mu = 1;

if exist([fileName,'.mat'],'file')
    load(fileName); % contains X_CVX
    fprintf('Loaded problem from %s\n', fileName );
else
    % Get reference solution in CVX
    % First, get a solution to the smoothed version:
    cvx_begin
        variables X1(n1,n2) X2(N1,N2)
        % it's important that we use norm(vec(...),1) and not just norm(...,1),
        %   otherwise CVX interprets this with the wrong implicit inner product
        minimize(     smoothF( X1, X2 ) + ...
            norm( A2(X1,X2), 2 ) + norm( vec(A3(X1,X2)) , 1 ) + ...
            mu*( sum_square(vec(X1)) + sum_square(vec(X2)) )/2       )
        subject to
            A1(X1,X2) >= 0  % constraint is dual of prox1
            A4(X1,X2) >= 0  % constraint is dual of prox2
    cvx_end
    X_CVX_smoothed{1} = X1;
    X_CVX_smoothed{2} = X2;

    % Second, get a solution to the unsmoothed version
    cvx_begin
        variables X1(n1,n2) X2(N1,N2)
        minimize(     smoothF( X1, X2 ) + ...
            norm( A2(X1,X2), 2 ) + norm( vec(A3(X1,X2)) , 1 ) )
        subject to
            A1(X1,X2) >= 0
            A4(X1,X2) >= 0
    cvx_end
    X_CVX{1} = X1;
    X_CVX{2} = X2;

    save(fileName,'X_CVX', 'X_CVX_smoothed');
    fprintf('Saved data to file %s\n', fileName);
end

% Verify constraint are satisfied
fprintf('Is A1(x1,x2) >= 0? The min element is %g\n', min(    A1(X_CVX{1},X_CVX{2} )) )
fprintf('Is A2(x1,x2) >= 0? The min element is %g\n', min(min(A4(X_CVX{1},X_CVX{2}))) )
%% before running TFOCS, scale the problem perhaps?
% This is one way to see how big the norms are:
% nrm11    = linop_test( A1_X1 ); % 15.7
% nrm12    = linop_test( A1_X2 ); % 16.6
%
% nrm21    = linop_test( A2_X1 ); % 17.8
% nrm22    = linop_test( A2_X2 ); % 18.6
%
% nrm31    = linop_test( A3_X1 ); % 63.4
% nrm32    = linop_test( A3_X2 ); % 0
%
% nrm41    = linop_test( A4_X1 ); % 35.6
% nrm42    = linop_test( A4_X2 ); % 34.6

%% now, run TFOCS. First, try it without continuation

x0 = [];
z0 = [];
opts    = struct('continuation',false,'maxits',1500,'debug',true); % using 'debug' mode to print out useful information
opts.printEvery     = 50;

% Pick a scaling factor "s" (optional: set to "1" to have no effect)
s = .5; % helps a little bit
% (note that if we scale prox3 by "s", then we multiply the corresponding
%  affine part by "s", rather than divide them by "s", since prox3
%  is really for the dual and not the primal).

mu  = 1;
opts.errFcn{1} = @(f,d,p) norm( p{1}-X_CVX_smoothed{1}); % compare to smoothed reference solution
opts.errFcn{2} = @(f,d,p) norm( p{2}-X_CVX_smoothed{2});

[xAll,outParam,optsOut] = tfocs_SCD( {smooth1,smooth2}, ...
    { A1_X1, A1_X2, const1; A2_X1, A2_X2, const2; ...
    A3_X1*s, A3_X2*s, const3*s; A4_X1, A4_X2, const4 }, ...
    {prox1,prox2,prox_scale(prox3,s),prox4},...
    mu, x0, z0, opts );

mnConstraint1    = min(min( A1(xAll{1},xAll{2} ) ) );
mnConstraint2    = min(min( A4(xAll{1},xAll{2} ) ) );
fprintf('First constraint violated by:   %g\n', mnConstraint1);
fprintf('Second constraint violated by:  %g\n', mnConstraint2 );

% Check that we are within acceptable limits
er = sqrt(norm( xAll{1} - X_CVX_smoothed{1} )^2 + norm( xAll{2} - X_CVX_smoothed{2} )^2 )/...
    sqrt( norm(X_CVX_smoothed{1})^2 + norm(X_CVX_smoothed{2})^2 ); % should be about .006

if er > 0.04 || mnConstraint1 < -.01 || mnConstraint2 < -.01
    error('Failed the test');
else
    disp('This test successfully passed');
end

%% run TFOCS with continuation
x0 = [];
z0 = [];
% type "tfocs" at the command to see possible options
opts    = struct('continuation',true,'maxits',1500);
opts.printEvery     = 50;
opts.tol            = 1e-4;
opts.stopCrit       = 4;
opts.printStopCrit  = true;

% type "continuation" at the command to see possible options
contOpts    = struct( 'maxIts', 8 , 'muDecrement', 0.8 );
% ask for increased accuracy on the final solve
contOpts.finalTol = 1e-5;

s = .5; % helps a little bit
% (note that if we scale prox3 by "s", then we multiply the corresponding
%  affine part by "s", rather than divide them by "s", since prox3
%  is really for the dual and not the primal).

mu  = 10;
opts.errFcn{1} = @(f,d,p) norm( p{1}-X_CVX{1}); % compare to unsmoothed reference solution
opts.errFcn{2} = @(f,d,p) norm( p{2}-X_CVX{2});

[xAll,outParam,optsOut] = tfocs_SCD( {smooth1,smooth2}, ...
    { A1_X1, A1_X2, const1; A2_X1, A2_X2, const2; ...
    A3_X1*s, A3_X2*s, const3*s; A4_X1, A4_X2, const4 }, ...
    {prox1,prox2,prox_scale(prox3,s),prox4},...
    mu, x0, z0, opts, contOpts );

mnConstraint1    = min(min( A1(xAll{1},xAll{2} ) ) );
mnConstraint2    = min(min( A4(xAll{1},xAll{2} ) ) );
fprintf('First constraint violated by:   %g\n', mnConstraint1);
fprintf('Second constraint violated by:  %g\n', mnConstraint2 );

% Check that we are within acceptable limits
er = sqrt(norm( xAll{1} - X_CVX{1} )^2 + norm( xAll{2} - X_CVX{2} )^2 )/...
    sqrt( norm(X_CVX{1})^2 + norm(X_CVX{2})^2 ); % should be about .006

if er > 0.4 || mnConstraint1 < -.01 || mnConstraint2 < -.01
    error('Failed the test');
else
    disp('This test successfully passed');
end

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


% demo_geodet     December 16, 2012

% demonstrates the geocentric-to-geodetic functions

% Orbital Mechanics with Matlab

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

clear all;

global req flat

% conversion factor - degrees-to-radians

rtd = 180.0d0 / pi;

% Earth equatorial radius (kilometers)

req = 6378.1363;

% Earth flattening factor (non-dimensional)

flat = 1.0 / 298.257;

% Earth polar axis (kilometers)

rpolar = req * (1.0d0 - flat);

% eci position vector (kilometers)

reci(1) = -.586479273288D+04;
reci(2) = -.178173078828D+04;
reci(3) = -.215629990858D+04;

rmag = norm(reci);

dec = asin(reci(3) / rmag);

[alt1, lat1] = geodet1 (rmag, dec);

clc; home;

fprintf('geocentric declination    %14.8f  degrees \n\n', rtd * dec);

fprintf('geocentric radius         %14.8f  kilometers \n\n', rmag);

fprintf('\ngeodet1 function\n');
fprintf('================\n\n');

fprintf('geodetic latitude         %14.8f  degrees \n\n', rtd * lat1);

fprintf('geodetic altitude         %14.8f  kilometers \n\n', alt1);

[lat2, alt2] = geodet2(dec, rmag);

fprintf('\ngeodet2 function\n');
fprintf('================\n\n');

fprintf('geodetic latitude         %14.8f  degrees \n\n', rtd * lat2);

fprintf('geodetic altitude         %14.8f  kilometers \n\n', alt2);

[alt5, lat5] = geodet5 (req, rpolar, reci);

fprintf('\ngeodet5 function\n');
fprintf('================\n\n');

fprintf('geodetic latitude         %14.8f  degrees \n\n', rtd * lat5);

fprintf('geodetic altitude         %14.8f  kilometers \n\n', alt5);

[alt6, lat6] = geodet6 (req, rpolar, reci);

fprintf('\ngeodet6 function\n');
fprintf('================\n\n');

fprintf('geodetic latitude         %14.8f  degrees \n\n', rtd * lat6);

fprintf('geodetic altitude         %14.8f  kilometers \n\n', alt6);


function y = softmax(x)
    M = bsxfun(@minus, x, max(x, [], 1));
    numerator = exp(M);
    denominator = sum(numerator) + 0.00001;
    y = bsxfun(@rdivide, numerator, denominator) + 0.00001;
end


function x=v_pcma2lin(p,m,s)
%V_PCMU2LIN Convert A-law PCM to linear X=(P,M,S)
%	lin = v_pcma2lin(pcma,m,s) where pcma contains a vector or matrix
%	of A-law values in the range 0 to 255.
%	No checking is performed to see that numbers are in this range.
%
%	Input values are exclusive ored with m (default=85)
%
%	Output values are divided by the scale factor s:
%
%		   s		Output Range
%
%		   1		+-4032	(integer values)
%		2017.396342	+-1.998616 (default)
%		4032		+-1
%		4096		+-0.984375 (+-1 nominal full scale)
%
%	The default value of s is 2017.396342 which equals
%	sqrt((1120^2 + 2624^2)/2). This factor follows ITU standard G.711 and
%	the sine wave with PCM-A values [225 244 244 225 97 116 116 97]
%	has a mean square value of unity corresponding to 0 dBm0.

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: v_pcma2lin.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   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 2 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 can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin<3
  t=4.95688418E-4;
  if nargin<2 m=85; end
else
  t=1/s;
end

if m q=bitxor(p,m); else q=p; end;
k=rem(q,16);
g=floor(q/128);
e=(q-k-128*g)/16;
f=(abs(e-1)-e+1)/2;
x=(2*g-1).*(pow2(k+16.5,e)+f.*(k-15.5))*t;


function D = DLSI_updateD(D, E, F, A, lambda, opts)
% function D = DLSI_updateD(D, E, F, A, lambda, opts)
% problem: `D = argmin_D -2trace(ED') + trace(FD'*D) + lambda *||A*D||F^2,`
% subject to: `||d_i||_2^2 <= 1`
% where F is a positive semidefinite matrix
% ========= aproach: ADMM ==============================
% rewrite: `[D, Z] = argmin -2trace(ED') + trace(FD'*D) + lambda ||A*Z||_F^2,`
%     subject to `D = Z; ||d_i||_2^2 <= 1`
% aproach 1: ADMM.
% 1. D = -2trace(ED') + trace(FD'*D) + rho/2 ||D - Z + U||_F^2,
%     s.t. ||d_i||_2^2 <= 1
% 2. Z = argmin lambda*||A*Z|| + rho/2||D - Z + U||_F^2
% 3. U = U + D - Z
% solve 1: D = argmin -2trace(ED') + trace(FD'*D) + rho/2 ||D - W||_F^2
%                       with W = Z - U;
%            = argmin -2trace((E - rho/2*W)*D') +
%               trace((F + rho/2 * eye())*D'D)
% solve 2: derivetaive: 0 = 2A'AZ + rho (Z - V) with V = D + U
% `Z = B*rhoV` with `B = (2*lambda*A'*A + rho I)^{-1}`
% `U = U + D - Z`
% -----------------------------------------------
% Author: Tiep Vu, thv102@psu.edu, 5/11/2016
%         (http://www.personal.psu.edu/thv102/)
% -----------------------------------------------

    if nargin == 0
        clc;
        d = 300;
        N = 10;
        k = 5;
        k2 = 495;
        load('tmp.mat');
        lambda = 0.01;

        opts.show = 0;
        opts.max_iter = 300;
        opts.verbose = 1;

    end
    if nargin == 6
        opts.lambda = lambda;
    elseif nargin == 5
        opts = lambda;
    end
    %%
    function cost = calcost(D)
        cost =  -2*trace(E*D') + trace(F*D'*D) + lambda*normF2(A*D);
    end
    %%
    iter = 0;
    rho = 1.0;
    Z_old = D;
    U = zeros(size(D));
    I_k = eye(size(D,2));
    % B = inv(2*lambda*A'*A + rho*I_k2); However, this might be very expensive if size(A, 2) is big, which is common
    % Instead, we can use the Sherman–Morrison formula at
    % https://en.wikipedia.org/wiki/Sherman%E2%80%93Morrison_formula#Generalization_(Woodbury_Matrix_Identity)
    X = 2*lambda/rho*A';
    Y = A;
    B1 = X*inv(eye(size(Y, 1)) + Y*X);
    tol = 1e-5;
    optsD.max_iter = 100;
    optsD.tol = 1e-8;
    while iter < opts.max_iter
        iter = iter + 1;
        %% ========= update D ==============================
        W = Z_old - U;
        E2 = E + rho/2 * W;
        F2 = F + rho/2*I_k;
        D = ODL_updateD(D, E2, F2, optsD);
        %% ========= update Z ==============================
        V = D + U;
        % Z_new = rho*B*V; slow
        Z_new = rho*(V - B1*(Y*V)); % fast
        e1 = normF2(D - Z_new);
        e2 = rho*normF2(Z_new - Z_old);
        if (e1 < optsD.tol && e2 < optsD.tol)
            break;
        end
        if opts.verbose
            cost = calcost(D);
            fprintf('iter = %3d | costD = %5.4f | normF2(D - Z) = %5.4f | rho(Z_new - Z_old) = %5.4f\n', iter, cost, e1, e2);
        end
        %% ========= update U ==============================
        U = U + D - Z_new;
        Z_old = Z_new;
    end
%     disp(t1)
%     disp(t2)
    if nargin == 0
        D = [];
    end
end


function [err,time,solver,eqn] = femPoisson3(mesh,pde,option,varargin)
%% FEMPOISSON3 solve Poisson equation by various finite element methods
%
%   FEMPOISSON3 computes approximations to the Poisson equation on a
%   sequence of meshes obtained by uniform refinement of a input mesh.
%
% See also Poisson, crack, Lshape
%
% Copyright (C)  Long Chen. See COPYRIGHT.txt for details.

%% Check input arguments
if isfield(mesh,'node') && isfield(mesh,'elem')
    node = mesh.node;
    elem = double(mesh.elem);
else
    [node,elem] = cubemesh([0,1,0,1,0,1],0.25); % default mesh is a cube
end
if isfield(mesh,'bdFlag')
    bdFlag = mesh.bdFlag;
else
    bdFlag = setboundary3(node,elem,'Dirichlet');
end
if ~exist('option','var'), option = []; end
if ~exist('pde','var')
    pde = sincosdata3;                          % default data
end

%% Parameters
option = femoption(option);
maxIt = option.maxIt;
maxN = option.maxN;
L0 = option.L0;
elemType = option.elemType;
refType = option.refType;

%% Generate an initial mesh
for k = 1:L0
    if strcmp(refType,'red')
        [node,elem,bdFlag] = uniformrefine3(node,elem,bdFlag);
    elseif strcmp(refType,'bisect')
        [node,elem,bdFlag] = uniformbisect3(node,elem,bdFlag);
    end
end

%% Initialize err
errL2 = zeros(maxIt,1);   errH1 = zeros(maxIt,1);
erruIuh = zeros(maxIt,1); errMax = zeros(maxIt,1);
errTime = zeros(maxIt,1); solverTime = zeros(maxIt,1);
assembleTime = zeros(maxIt,1); meshTime = zeros(maxIt,1);
itStep = zeros(maxIt,1);  stopErr = zeros(maxIt,1); flag = zeros(maxIt,1);
N = zeros(maxIt,1); h = zeros(maxIt,1);

%% Finite Element Method
for k = 1:maxIt
%     bdFlag = sparse(double(bdFlag));
    % solve the equation
    switch elemType
        case 'P1'     % piecewise linear function P1 element
            [soln,eqn,info] = Poisson3(node,elem,bdFlag,pde,option);
        case 'CR'     % piecewise linear function CR element
            [soln,eqn,info] = Poisson3CR(node,elem,bdFlag,pde,option);
        case 'P2'     % piecewise quadratic function
            [soln,eqn,info] = Poisson3P2(node,elem,bdFlag,pde,option);
        case 'WG'     % weak Galerkin element
            [soln,eqn,info] = Poisson3WG(node,elem,bdFlag,pde,option);
    end
    uh = soln.u;
    % compute error
    t = cputime;
    if isfield(pde,'Du')
        if isfield(soln,'Du') && ~isempty(soln.Du) % Du is in the output
            errH1(k) = getH1error3(node,elem,pde.Du,soln.Du);
        else
            errH1(k) = getH1error3(node,elem,pde.Du,uh);
        end
    end
    if isfield(pde,'exactu')
        errL2(k) = getL2error3(node,elem,pde.exactu,uh);
        % interpolation
        switch elemType
            case 'P1'
                uI = Lagrangeinterpolate(pde.exactu,node,elem);
            case 'CR'
                uI = Lagrangeinterpolate(pde.exactu,node,elem,'CR',eqn.face);
            case 'P2'
                uI = Lagrangeinterpolate(pde.exactu,node,elem,'P2',eqn.edge);
            case 'WG'
                uI = Lagrangeinterpolate(pde.exactu,node,elem,'WG',eqn.face);
        end
        erruIuh(k) = sqrt((uh-uI)'*eqn.A*(uh-uI));
        errMax(k) = max(abs(uh-uI));
    end
    errTime(k) = cputime - t;
    % record time
    solverTime(k) = info.solverTime;
    assembleTime(k) = info.assembleTime;
    if option.printlevel>1
        fprintf('Time to compute the error %4.2g s \n H1 err %4.2g    L2err %4.2g \n',...
                 errTime(k), errH1(k), errL2(k));
    end
    % record solver information
    itStep(k) = info.itStep;
    stopErr(k) = info.stopErr;
    flag(k) = info.flag;
    % plot
    N(k) = length(soln.u);
    h(k) = 1./(size(node,1)^(1/3)-1);
    if  strcmp(elemType,'WG') % modify size for WG
        if ~isfield(option,'reducesystem') || (option.reducesystem == 1)
            N(k) = N(k) - size(elem,1); % reduced system
        end
    end
    if option.plotflag && N(k) < 2e4 % show mesh and solution for small size
        switch elemType
        case 'P1'     % piecewise linear function P1 element
            figure(1);  showresult3(node,elem,uh);
        case 'CR'     % piecewise linear function CR element
            continue;
        case 'P2'     % piecewise quadratic function
            figure(1);  showresult3(node,elem,uh(1:size(node,1)));
        case 'WG'     % weak Galerkin element
            continue;
        end
    end
    if N(k) > maxN
        break;
    end
    % refine mesh
    t = cputime;
    if strcmp(refType,'red')
        [node,elem,bdFlag] = uniformrefine3(node,elem,bdFlag);
    elseif strcmp(refType,'bisect')
        [node,elem,bdFlag] = uniformbisect3(node,elem,bdFlag);
    end
    meshTime(k) = cputime - t;
end

%% Plot convergence rates
if option.rateflag
    figure;
    set(gcf,'Units','normal');
    set(gcf,'Position',[0.25,0.25,0.55,0.4]);
    subplot(1,2,1)
    showrateh2(h(1:k),errH1(1:k),1,'-*','||Du-Du_h||',...
               h(1:k),errL2(1:k),1,'k-+','||u-u_h||');
    title(['Convergence Rate of 3D ', elemType, '-element']);
    subplot(1,2,2)
    showrateh2(h(1:k),erruIuh(1:k),1,'m-+','||Du_I-Du_h||',...
               h(1:k),errMax(1:k),1,'r-*','||u_I-u_h||_{\infty}');
    title(['Convergence Rate of 3D ', elemType, '-element']);
end

%% Output
err = struct('h',h(1:k),'N',N,'H1',errH1(1:k),'L2',errL2(1:k),...
             'uIuhH1',erruIuh(1:k),'uIuhMax',errMax(1:k));
time = struct('N',N,'err',errTime(1:k),'solver',solverTime(1:k), ...
              'assemble',assembleTime(1:k),'mesh',meshTime(1:k));
solver = struct('N',N(1:k),'itStep',itStep(1:k),'time',solverTime(1:k),...
                'stopErr',stopErr(1:k),'flag',flag(1:k));

%% Display error and time
if option.dispflag
    disp('Table: Error')
    colname = {'#Dof','h','||u-u_h||','||Du-Du_h||','||DuI-Du_h||','||uI-u_h||_{max}'};
    disptable(colname,err.N,[],err.h,'%0.3e',err.L2,'%0.5e',err.H1,'%0.5e',...
              err.uIuhH1,'%0.5e',err.uIuhMax,'%0.5e');

    disp('Table: CPU time')
    colname = {'#Dof','Assemble','Solve','Error','Mesh'};
    disptable(colname,time.N,[],time.assemble,'%0.2e',time.solver,'%0.2e',...
                      time.err,'%0.2e',time.mesh,'%0.2e');
end


function visible_probability = hidden_state_to_visible_probabilities(rbm_w, hidden_state)
% <rbm_w> is a matrix of size <number of hidden units> by <number of visible units>
% <hidden_state> is a binary matrix of size <number of hidden units> by <number of configurations that we're handling in parallel>.
% The returned value is a matrix of size <number of visible units> by <number of configurations that we're handling in parallel>.
% This takes in the (binary) states of the hidden units, and returns the activation probabilities of the visible units, conditional on those states.
    m_ = rbm_w' * hidden_state;
    visible_probability = 1 ./ (1 + e .^ (-m_));
end


% Created for the blood glucose step response
% by John D. Hedengren
% 02 Sept 2003

global u A

% Steady State Initial Conditions for the States
% Basal values of glucose and insulin conc.
G_ss = 4.5; % mmol/L
X_ss = 15; % mU/L
I_ss = 15; % mU/L

x_ss = [G_ss;I_ss;X_ss];

% Steady State Initial Condition for the Control
u_ss = 16.667; % mU/min

% Steady State for the Disturbance
d_ss = 0; % mmol/L-min

% Final Time (min)
tf = 400;

[t,x] = ode15s('blood_glucose',[0 tf],x_ss);

% Separate out the state values
G = x(:,1);
X = x(:,2);
I = x(:,3);

% Plot the results
figure(1);
plot(t,G);
legend('Glucose');
xlabel('Time (min)');
ylabel('mmol/L');

figure(2);
plot(t,I,t,X);
legend('Plasma Insulin','Remote Compartment Insulin');
xlabel('Time (min)');
ylabel('mU/L');


function [ npart, a, rank ] = partn_successor ( n, nmax, npart, a, rank )

%*****************************************************************************80
%
%% PARTN_SUCCESSOR computes partitions whose largest part is NMAX.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 January 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Donald Kreher, Douglas Simpson,
%    Combinatorial Algorithms,
%    CRC Press, 1998,
%    ISBN: 0-8493-3988-X,
%    LC: QA164.K73.
%
%  Parameters:
%
%    Input, integer N, the integer to be partitioned.
%    N must be positive.
%
%    Input, integer NMAX, the maximum size of any part of the
%    partition.  1 <= NMAX <= N.
%
%    Input/output, integer NPART, the number of parts of the
%    partition.  1 <= NPART <= N.
%
%    Input/output, integer A(N), contains the partition.
%    A(1) through A(NPART) contain the nonzero integers which
%    sum to N.
%
%    Input/output, integer RANK, the rank.
%    If RANK = -1 on input, then the routine understands that this is
%    the first call, and that the user wishes the routine to supply
%    the first element in the ordering, which has RANK = 0.
%    In general, the input value of RANK is increased by 1 for output,
%    unless the very last element of the ordering was input, in which
%    case the output value of RANK is 0.
%

%
%  Return the first element.
%
  if ( rank == -1 )
    a(1) = nmax;
    npart = n + 1 - nmax;
    a(2:npart) = 1;
    rank = 0;
    return
  end
%
%  Check.
%
  ierror = partn_sf_check ( n, nmax, npart, a );

  if ( ierror ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'PARTN_SUCCESSOR - Fatal error!\n' );
    fprintf ( 1, '  The input array is illegal.\n' );
    fprintf ( 1, '  IERROR = %d\n', ierror );
    error ( 'PARTN_SUCCESSOR - Fatal error!' );
  end
%
%  If there are at least two parts, and the next to last is not NMAX,
%  then rob the last part and pay the next to the last part.
%  Then, if the next to last part is too big, swap it leftwards.
%
  if ( 1 < npart )

    if ( a(npart-1) < nmax )

      a(npart) = a(npart) - 1;
      a(npart-1) = a(npart-1) + 1;
      index = npart - 1;

      while ( 1 )

        if ( index <= 1 )
          break
        end

        if ( a(index) <= a(index-1) )
          break
        end

        temp       = a(index-1);
        a(index-1) = a(index);
        a(index)   = temp;

        index = index - 1;

      end
%
%  Sum the tail.
%
      temp = sum ( a(index+1:npart) );
%
%  Spread the sum as 1's.
%
      npart = index + temp;
      a(index+1:npart) = 1;
      rank = rank + 1;
      return

    end
%
%  Otherwise, we've reached the last item.
%  Return the first one.
%
  else

    npart = n + 1 - nmax;
    a(1) = nmax;
    a(2:npart) = 1;
    rank = 0;
    return

  end

  return
end


function s = sharpness(im, metric)
%SHARPNESS   Calculate image sharpness metric.
%
% DESCRIPTION:
%       sharpness returns a scalar metric related the sharpness of the 2D
%       or 3D image matrix defined by im. By default, the metric is based
%       on the Brenner gradient which returns the sum of the centered
%       finite-difference at each matrix element in each Cartesian
%       direction. Metrics calculated using the Sobel operator or the
%       normalised variance can also be returned by setting the input
%       paramater metric.
%
%       For further details, see B. E. Treeby, T. K. Varslot, E. Z. Zhang,
%       J. G. Laufer, and P. C. Beard, "Automatic sound speed selection in
%       photoacoustic image reconstruction using an autofocus approach," J.
%       Biomed. Opt., vol. 16, no. 9, p. 090501, 2011.
%
% USAGE:
%       s = sharpness(im)
%       s = sharpness(im, metric)
%
% INPUTS:
%       im          - 2D or 3D image data to evaluate
%
% OPTIONAL INPUTS:
%       metric      - sharpness metric. Supported values are:
%                       'Brenner'           (default)
%                       'Tenenbaum'
%                       'NormVariance'
%
% OUTPUTS:
%       s           - computed sharpness metric
%
% ABOUT:
%       author      - Bradley Treeby
%       date        - 15th January 2012
%       last update - 15th January 2012
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox

% This file is part of k-Wave. k-Wave 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.
%
% k-Wave 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 Lesser General Public License
% along with k-Wave. If not, see <http://www.gnu.org/licenses/>.

% check for metric input
if nargin == 1
   metric = 'Brenner';
end

switch metric

    case 'Brenner'

        % compute sharpness metric based on the Brenner gradient
        switch numDim(im)
            case 2

                % compute metric
                bren_x = (im(1:end-2, :) - im(3:end, :)).^2;
                bren_y = (im(:, 1:end-2) - im(:, 3:end)).^2;
                s = sum(bren_x(:)) + sum(bren_y(:));

            case 3

                % compute metric
                bren_x = (im(1:end-2, :, :) - im(3:end, :, :)).^2;
                bren_y = (im(:, 1:end-2, :) - im(:, 3:end, :)).^2;
                bren_z = (im(:, :, 1:end-2) - im(:, :, 3:end)).^2;
                s = sum(bren_x(:)) + sum(bren_y(:)) + sum(bren_z(:));

        end

    case 'Tenenbaum'

        % compute sharpness metric based on the Tenenbaum gradient
        switch numDim(im)
            case 2

                % define the 2D sobel gradient operator
                sobel = [-1 0 1; -2 0 2; -1 0 1];

                % compute metric
                s = conv2(sobel, im).^2 + conv2(sobel.', im).^2;
                s = sum(s(:));

            case 3

                % define the 3D sobel gradient operator
                sobel3D(:, :, 1) = [1 2 1; 2 4 2; 1 2 1];
                sobel3D(:, :, 2) = zeros(3);
                sobel3D(:, :, 3) = -sobel3D(:, :, 1);

                % compute metric
                s = convn(im, sobel3D).^2 + convn(im, permute(sobel3D, [3 1 2])).^2 +  convn(im, permute(sobel3D, [2 3 1])).^2;
                s = sum(s(:));

        end

    case 'NormVariance'

        % compute sharpness metric based on the normalised variance
        mu = mean(im(:));
        s = sum((im(:) - mu).^2)./mu;

end


%% This file will generate the guess of the iSINDYc left hand side.
% Last Updated: 2019/05/08
% Coded By: K
%
% Note that this file is modified. When the iter = 6, we will add dx6*x6^4
% to the left hand side guess.
function [P_Data,P_sym]=GuessLib(X,dX,iter,u,Highest_Poly_Order,Highest_Trig_Order,Highest_U_Order)
%% First get the size of the X matrix, determin the data length and the number of variables we have.
[Data_Length,Variable_Number]=size(X);
[~,Variable_Number_dX]=size(dX);
[~,Variable_Number_u]=size(u);
% Also create the symbolic variable
Symbol=sym('z',[Variable_Number,1]);
Symbol_dX=sym('dz',[Variable_Number,1]);
Symbol_u=sym('u',[Variable_Number_u,1]);

%% Now according the Highest Polynomial Order entered, we will calculate the data matrix.
Data=[];
Index=1;

%% First calculate the polynomial term

%Order Zero:
Data(:,Index)=ones(Data_Length,1);
Sym_Struct{1,Index}=sym(1);

%Order One:
if Highest_Poly_Order>=1
    for i=1:Variable_Number
        Index=Index+1;
        Data(:,Index)=X(:,i);
        Sym_Struct{1,Index}=Symbol(i,1);
    end
end

%Order Two:
if Highest_Poly_Order>=2
    for i=1:Variable_Number
        for j=i:Variable_Number
            Index=Index+1;
            Data(:,Index)=X(:,i).*X(:,j);
            Sym_Struct{1,Index}=Symbol(i,1)*Symbol(j,1);
        end
    end
end

%Order Three:
if Highest_Poly_Order>=3
    for i=1:Variable_Number
        for j=1:Variable_Number
            for k=1:Variable_Number
                Index=Index+1;
                Data(:,Index)=X(:,i).*X(:,j).*X(:,k);
                Sym_Struct{1,Index}=Symbol(i,1)*Symbol(j,1)*Symbol(k,1);
            end
        end
    end
end

%Order Four:
if Highest_Poly_Order>=4
    for i=1:Variable_Number
        for j=1:Variable_Number
            for k=1:Variable_Number
                for p=1:Variable_Number
                    Index=Index+1;
                    Data(:,Index)=X(:,i).*X(:,j).*X(:,k).*X(:,p);
                    Sym_Struct{1,Index}=Symbol(i,1)*Symbol(j,1)*Symbol(k,1)*Symbol(p,1);
                end
            end
        end
    end
end

%% Then add the Trigonometric Function in the output data:

%Order One:
if Highest_Trig_Order>=1
    for i=1:Variable_Number
        Index=Index+1;
        Data(:,Index)=sin(X(:,i));
        Sym_Struct{1,Index}=sin(Symbol(i,1));
    end
    for i=1:Variable_Number
        Index=Index+1;
        Data(:,Index)=cos(X(:,i));
        Sym_Struct{1,Index}=cos(Symbol(i,1));
    end
end

%Order Two:
if Highest_Trig_Order>=2
    %
    for i=1:Variable_Number
        for j=i+1:Variable_Number
            Index=Index+1;
            Data(:,Index)=sin(X(:,i)-X(:,j)).^2;
            Sym_Struct{1,Index}=sin(Symbol(i,1)-Symbol(j,1))^2;
        end
    end
    for i=1:Variable_Number
        for j=i+1:Variable_Number
            Index=Index+1;
            Data(:,Index)=cos(X(:,i)-X(:,j)).^2;
            Sym_Struct{1,Index}=cos(Symbol(i,1)-Symbol(j,1))^2;
        end
    end
end

%Order Three:
if Highest_Trig_Order>=3
    %
    for i=1:Variable_Number
        for j=1:Variable_Number
            Index=Index+1;
            Data(:,Index)=sin(X(:,i)+X(:,j));
            Sym_Struct{1,Index}=sin(Symbol(i,1)+Symbol(j,1));
        end
    end
    for i=1:Variable_Number
        for j=1:Variable_Number
            Index=Index+1;
            Data(:,Index)=cos(X(:,i)+X(:,j));
            Sym_Struct{1,Index}=cos(Symbol(i,1)+Symbol(j,1));
        end
    end
    %
    for i=1:Variable_Number
        for j=i+1:Variable_Number
            Index=Index+1;
            Data(:,Index)=sin(X(:,i)-X(:,j));
            Sym_Struct{1,Index}=sin(Symbol(i,1)-Symbol(j,1));
        end
    end
    for i=1:Variable_Number
        for j=i+1:Variable_Number
            Index=Index+1;
            Data(:,Index)=cos(X(:,i)-X(:,j));
            Sym_Struct{1,Index}=cos(Symbol(i,1)-Symbol(j,1));
        end
    end
end

%Order Four:
if Highest_Trig_Order>=4
    for i=1:Variable_Number
        for j=i+1:Variable_Number
            Index=Index+1;
            Data(:,Index)=sin(X(:,i)-X(:,j)).*X(:,i).^2;
            Sym_Struct{1,Index}=sin(Symbol(i,1)-Symbol(j,1))*Symbol(i,1)^2;
        end
    end
    %
    for i=1:Variable_Number
        for j=i+1:Variable_Number
            Index=Index+1;
            Data(:,Index)=cos(X(:,i)-X(:,j)).*X(:,i).^2;
            Sym_Struct{1,Index}=cos(Symbol(i,1)-Symbol(j,1))*Symbol(i,1)^2;
        end
    end
    %
    for i=1:Variable_Number
        for j=1:Variable_Number
            Index=Index+1;
            Data(:,Index)=sin(X(:,i)-2*X(:,j));
            Sym_Struct{1,Index}=sin(Symbol(i,1)-2*Symbol(j,1));
        end
    end
    %
    for i=1:Variable_Number
        for j=1:Variable_Number
            Index=Index+1;
            Data(:,Index)=cos(X(:,i)-2*X(:,j));
            Sym_Struct{1,Index}=cos(Symbol(i,1)-2*Symbol(j,1));
        end
    end
    %
    for i=1:Variable_Number
        for j=i:Variable_Number
            Index=Index+1;
            Data(:,Index)=sin(2*X(:,i)-2*X(:,j)).*X(:,i).^2;
            Sym_Struct{1,Index}=sin(2*Symbol(i,1)-2*Symbol(j,1))*Symbol(i,1)^2;
        end
    end
    %
    for i=1:Variable_Number
        for j=i:Variable_Number
            Index=Index+1;
            Data(:,Index)=cos(2*X(:,i)-2*X(:,j)).*X(:,i).^2;
            Sym_Struct{1,Index}=cos(2*Symbol(i,1)-2*Symbol(j,1))*Symbol(i,1)^2;
        end
    end
end

% Add the guess x6^4 manually for the selected states.
if iter==1 || iter==2 || iter==6
    Index=Index+1;
    Data(:,Index)=X(:,6).^4;
    Sym_Struct{1,Index}=Symbol(6,1)^4;
end

pin=Index;
j=0;
%% Frome here, we add the right hand side to our data

%If the Highest_U_Order is zero, we assume that our system does not have
%control input, vice versa.
if Highest_U_Order==0
    for k=1:pin
        j=j+1;
        P_Data(:,j)=dX(:,1).*Data(:,k);
        P_sym{1,j}=Symbol_dX(iter,1)*(Sym_Struct{1,k});
    end
else
    for i=1:Variable_Number_u
        for k=1:pin
            j=j+1;
            P_Data(:,j)=u(:,1).*Data(:,k);
            P_sym{1,j}=Symbol_u(1,1)*(Sym_Struct{1,k});
        end
    end
end


function [textures] = getGLSZMtextures_STS(GLSZM)
% -------------------------------------------------------------------------
% function [textures] = getGLSZMtextures_STS(GLSZM)
% -------------------------------------------------------------------------
% DESCRIPTION:
% This function computes texture features from an input Gray-Level Size
% Zone Matrix (GLSZM). (STS study)
% -------------------------------------------------------------------------
% REFERENCES:
% [1] Galloway, M. M. (1975). Texture analysis using gray level run lengths.
%     Computer Graphics and Image Processing, 4(2), 172–179.
% [2] Chu, A., Sehgal, C. M., & Greenleaf, J. F. (1990). Use of gray value
%     distribution of run lengths for texture analysis. Pattern Recognition
%     Letters, 11(6), 415-419.
% [3] Dasarathy, B. V., & Holder, E. B. (1991). Image characterizations
%     based on joint gray level-run length distributions. Pattern
%     Recognition Letters, 12(8), 497-502.
% [4] Thibault, G., Fertil, B., Navarro, C., Pereira, S., Cau, P., Levy,
%     N., Mari, J.-L. (2009). Texture Indexes and Gray Level Size Zone
%     Matrix. Application to Cell Nuclei Classification. In Pattern
%     Recognition and Information Processing (PRIP) (pp. 140–145).
% -------------------------------------------------------------------------
% INPUTS:
% - GLSZM: Gray-Level Size Zone Matrix.
%
% ** 'GLSZM' should be the output from 'getGLSZM.m' **
% -------------------------------------------------------------------------
% OUTPUTS:
% - textures: Struture specifying the values of different GLSZM texture
%             features as defined below.
% -------------------------------------------------------------------------
% AUTHOR(S): Martin Vallieres <mart.vallieres@gmail.com>
% -------------------------------------------------------------------------
% HISTORY:
% - Creation: January 2013
% - Revision: May 2015
% -------------------------------------------------------------------------
% STATEMENT:
% This file is part of <https://github.com/mvallieres/radiomics/>,
% a package providing MATLAB programming tools for radiomics analysis.
% --> Copyright (C) 2015  Martin Vallieres
%
%    This package 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 package 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 package.  If not, see <http://www.gnu.org/licenses/>.
% -------------------------------------------------------------------------

% USEFUL MATRICES, VECTORS AND QUANTITIES
sz = size(GLSZM); % Size of GLSZM
nRuns = sum(GLSZM(:));
cVect = 1:sz(2); rVect = 1:sz(1);% Row and column vectors
[cMat,rMat] = meshgrid(cVect,rVect); % Column and row indicators for each entry of the GLSZM
pg = sum(GLSZM,2)'; % Gray-Level Run-Number Vector
pr = sum(GLSZM); % Run-Length Run-Number Vector

% COMPUTATION OF TEXTURE FEATURES
% 1. Small Zone Emphasis (SZE), Ref.[1,4]
textures.SZE = (pr*(cVect.^(-2))')/nRuns;

% 2. Large Zone Emphasis (LZE), Ref.[1,4]
textures.LZE = (pr*(cVect.^2)')/nRuns;

% 3. Gray-Level Nonuniformity (GLN), adapted from Ref.[1,4]
textures.GLN = sum(pg.^2)/nRuns;

% 4. Zone-Size Nonuniformity (ZSN), adapted from Ref.[1,4]
textures.ZSN = sum(pr.^2)/nRuns;

% 5. Zone Percentage (ZP), adapted from Ref.[1,4]
textures.ZP = nRuns/(pr*cVect');

% 6. Low Gray-Level Zone Emphasis (LGZE), Ref.[2,4]
textures.LGZE = (pg*(rVect.^(-2))')/nRuns;

% 7. High Gray-Level Zone Emphasis (HGZE), Ref.[2,4]
textures.HGZE = (pg*(rVect.^2)')/nRuns;

% 8. Small Zone Low Gray-Level Emphasis (SZLGE), Ref.[3,4]
textures.SZLGE = sum(sum(GLSZM.*(rMat.^(-2)).*(cMat.^(-2))))/nRuns;

% 9. Small Zone High Gray-Level Emphasis (SZHGE), Ref.[3,4]
textures.SZHGE = sum(sum(GLSZM.*(rMat.^2).*(cMat.^(-2))))/nRuns;

% 10. Large Zone Low Gray-Level Emphasis (LZLGE), Ref.[3,4]
textures.LZLGE = sum(sum(GLSZM.*(rMat.^(-2)).*(cMat.^2)))/nRuns;

% 11. Large Zone High Gray-Level Emphasis (LZHGE), Ref.[3,4]
textures.LZHGE = sum(sum(GLSZM.*(rMat.^2).*(cMat.^2)))/nRuns;

% New features according to Ref.[4]
GLSZM = GLSZM./nRuns; % In the future, this operation will be applied at the beginning of the function
pg=sum(GLSZM,2)'; pr=sum(GLSZM);
ug = (pg*rVect')/(sz(1)*sz(2));
ur = (pr*cVect')/(sz(1)*sz(2));

% 12. Gray-Level Variance (GLV), adapted from Ref.[4]
GLV = 0;
for g = 1:sz(1)
    for r = 1:sz(2)
        GLV = GLV + (GLSZM(g,r)*g-ug)^2;
    end
end
textures.GLV = GLV/(sz(1)*sz(2));

% 13. Zone-Size Variance (ZSV), adapted from Ref.[4]
ZSV = 0;
for g = 1:sz(1)
    for r = 1:sz(2)
        ZSV = ZSV + (GLSZM(g,r)*r-ur)^2;
    end
end
textures.ZSV = ZSV/(sz(1)*sz(2));

end


function B = bernoulli(N)
%BERNOULLI   Bernoulli polynomials as chebfuns.
%   B = BERNOULLI(N) returns a quasimatrix of the first N+1 Bernoulli
%   polynomials on [0,1].
%
%   Example (Bernolli numbers):
%      B = cheb.bernoulli(4);
%      format rat, B(0,:)

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

x = chebfun('x', [0,1]);

% Initalize the constant 1 as the first Bernoulli polynomial.
B = 0*x + 1;
% Compute the requested polynomials.
for j = 1:N
    B(:,j+1) = j*cumsum(B(:,j));
    B(:,j+1) = B(:,j+1) - sum(B(:,j+1));
end

end


function M = stiefelfactory(n, p, k)
% Returns a manifold structure to optimize over orthonormal matrices.
%
% function M = stiefelfactory(n, p)
% function M = stiefelfactory(n, p, k)
%
% The Stiefel manifold is the set of orthonormal nxp matrices. If k
% is larger than 1, this is the Cartesian product of the Stiefel manifold
% taken k times. The metric is such that the manifold is a Riemannian
% submanifold of R^nxp equipped with the usual trace inner product, that
% is, it is the usual metric.
%
% Points are represented as matrices X of size n x p x k (or n x p if k=1,
% which is the default) such that each n x p matrix is orthonormal,
% i.e., X'*X = eye(p) if k = 1, or X(:, :, i)' * X(:, :, i) = eye(p) for
% i = 1 : k if k > 1. Tangent vectors are represented as matrices the same
% size as points.
%
% By default, k = 1.
%
% See also: grassmannfactory rotationsfactory

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Dec. 30, 2012.
% Contributors:
% Change log:
%  July  5, 2013 (NB) : Added ehess2rhess.
%  Jan. 27, 2014 (BM) : Bug in ehess2rhess corrected.
%  June 24, 2014 (NB) : Added true exponential map and changed the randvec
%                       function so that it now returns a globally
%                       normalized vector, not a vector where each
%                       component is normalized (this only matters if k>1).

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

    if k == 1
        M.name = @() sprintf('Stiefel manifold St(%d, %d)', n, p);
    elseif k > 1
        M.name = @() sprintf('Product Stiefel manifold St(%d, %d)^%d', n, p, k);
    else
        error('k must be an integer no less than 1.');
    end

    M.dim = @() k*(n*p - .5*p*(p+1));

    M.inner = @(x, d1, d2) d1(:).'*d2(:);

    M.norm = @(x, d) norm(d(:));

    M.dist = @(x, y) error('stiefel.dist not implemented yet.');

    M.typicaldist = @() sqrt(p*k);

    M.proj = @projection;
    function Up = projection(X, U)

        XtU = multiprod(multitransp(X), U);
        symXtU = multisym(XtU);
        Up = U - multiprod(X, symXtU);

% The code above is equivalent to, but much faster than, the code below.
%
%     Up = zeros(size(U));
%     function A = sym(A), A = .5*(A+A'); end
%     for i = 1 : k
%         Xi = X(:, :, i);
%         Ui = U(:, :, i);
%         Up(:, :, i) = Ui - Xi*sym(Xi'*Ui);
%     end

    end

    M.tangent = M.proj;

    % For Riemannian submanifolds, converting a Euclidean gradient into a
    % Riemannian gradient amounts to an orthogonal projection.
	M.egrad2rgrad = M.proj;

    M.ehess2rhess = @ehess2rhess;
    function rhess = ehess2rhess(X, egrad, ehess, H)
        XtG = multiprod(multitransp(X), egrad);
        symXtG = multisym(XtG);
        HsymXtG = multiprod(H, symXtG);
        rhess = projection(X, ehess - HsymXtG);
    end

    M.retr = @retraction;
    function Y = retraction(X, U, t)
        if nargin < 3
            t = 1.0;
        end
        Y = X + t*U;
        for i = 1 : k
            [Q, R] = qr(Y(:, :, i), 0);
            % The instruction with R assures we are not flipping signs
            % of some columns, which should never happen in modern Matlab
            % versions but may be an issue with older versions.
            Y(:, :, i) = Q * diag(sign(sign(diag(R))+.5));
        end
    end

    M.exp = @exponential;
    function Y = exponential(X, U, t)
        if nargin == 2
            t = 1;
        end
        tU = t*U;
        Y = zeros(size(X));
        for i = 1 : k
            % From a formula by Ross Lippert, Example 5.4.2 in AMS08.
            Xi = X(:, :, i);
            Ui = tU(:, :, i);
            Y(:, :, i) = [Xi Ui] * ...
                         expm([Xi'*Ui , -Ui'*Ui ; eye(p) , Xi'*Ui]) * ...
                         [ expm(-Xi'*Ui) ; zeros(p) ];
        end

    end

    M.hash = @(X) ['z' hashmd5(X(:))];

    M.rand = @random;
    function X = random()
        X = zeros(n, p, k);
        for i = 1 : k
            [Q, unused] = qr(randn(n, p), 0); %#ok<NASGU>
            X(:, :, i) = Q;
        end
    end

    M.randvec = @randomvec;
    function U = randomvec(X)
        U = projection(X, randn(n, p, k));
        U = U / norm(U(:));
    end

    M.lincomb = @lincomb;

    M.zerovec = @(x) zeros(n, p, k);

    M.transp = @(x1, x2, d) projection(x2, d);

    M.vec = @(x, u_mat) u_mat(:);
    M.mat = @(x, u_vec) reshape(u_vec, [n, p, k]);
    M.vecmatareisometries = @() true;

end

% Linear combination of tangent vectors
function d = lincomb(x, a1, d1, a2, d2) %#ok<INUSL>

    if nargin == 3
        d = a1*d1;
    elseif nargin == 5
        d = a1*d1 + a2*d2;
    else
        error('Bad use of stiefel.lincomb.');
    end

end


function [ n_data, mu, sigma, x, fx ] = cauchy_cdf_values ( n_data )

%*****************************************************************************80
%
%% CAUCHY_CDF_VALUES returns some values of the Cauchy CDF.
%
%  Discussion:
%
%    In Mathematica, the function can be evaluated by:
%
%      Needs["Statistics`ContinuousDistributions`"]
%      dist = CauchyDistribution [ mu, sigma ]
%      CDF [ dist, x ]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real MU, the mean of the distribution.
%
%    Output, real SIGMA, the variance of the distribution.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 12;

  fx_vec = [ ...
     0.5000000000000000E+00, ...
     0.8524163823495667E+00, ...
     0.9220208696226307E+00, ...
     0.9474315432887466E+00, ...
     0.6475836176504333E+00, ...
     0.6024163823495667E+00, ...
     0.5779791303773693E+00, ...
     0.5628329581890012E+00, ...
     0.6475836176504333E+00, ...
     0.5000000000000000E+00, ...
     0.3524163823495667E+00, ...
     0.2500000000000000E+00 ];

  mu_vec = [ ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.4000000000000000E+01, ...
     0.5000000000000000E+01 ];

  sigma_vec = [ ...
     0.5000000000000000E+00, ...
     0.5000000000000000E+00, ...
     0.5000000000000000E+00, ...
     0.5000000000000000E+00, ...
     0.2000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.4000000000000000E+01, ...
     0.5000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01 ];

  x_vec = [ ...
     0.1000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.4000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.3000000000000000E+01, ...
     0.3000000000000000E+01 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    mu = 0.0;
    sigma = 0.0;
    x = 0.0;
    fx = 0.0;
  else
    mu = mu_vec(n_data);
    sigma = sigma_vec(n_data);
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function g_hofstadter_test ( )

%*****************************************************************************80
%
%% G_HOFSTADTER_TEST tests G_HOFSTADTER.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 October 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'G_HOFSTADTER_TEST\n' );
  fprintf ( 1, '  G_HOFSTADTER evaluates Hofstadter''s recursive\n' );
  fprintf ( 1, '  G function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     N   G(N)' );
  fprintf ( 1, '\n' );

  for i = 0 : 30
    g = g_hofstadter ( i );
    fprintf ( 1, '  %4d  %4d\n', i, g );
  end

  return
end


function [q,Qv] = v2q(v)

% V2Q Rotaiton vector to quaternion conversion.
%   [Q,Qv] = V2Q(V) returns the quaternion Q correscponding to the rotation
%   encoded in rotation vector V, and the associated Jacobian Qv = dQ/dV.

if nargout == 1

    [a,u] = v2au(v);
    q = au2q(a,u);

else
    a = sqrt(dot(v,v));

    if isnumeric(a) && a < 1e-6

        % Use small signal approximation:
        q = [1-norm(v)^2/8
            v(:)/2];

        Qv = [-1/4*v(:)'
            0.5*eye(3)];

    else

        [a,u,Av,Uv] = v2au(v);
        [q,Qa,Qu] = au2q(a,u);
        Qv = Qa*Av + Qu*Uv;

    end
end

return

%%
syms r s t real
v = [r;s;t];
[q,Qv] = v2q(v)

simplify(Qv - jacobian(q,v))

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function [ a, l, r ] = i4vec_part_quick_a ( n, a )

%*****************************************************************************80
%
%% I4VEC_PART_QUICK_A reorders an I4VEC as part of a quick sort.
%
%  Discussion:
%
%    The routine reorders the entries of A.  Using A(1) as a key,
%    all entries of A that are less than or equal to the key will
%    precede the key which precedes all entries that are greater than the key.
%
%  Example:
%
%    Input:
%
%      N = 8
%
%      A = ( 6, 7, 3, 1, 6, 8, 2, 9 )
%
%    Output:
%
%      L = 3, R = 6
%
%      A = ( 3, 1, 2, 6, 6, 8, 9, 7 )
%            -------        -------
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 February 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries of A.
%
%    Input, integer A(N), the array to be checked.
%
%    Output, integer A(N), has been reordered as described above.
%
%    Output, integer L, R, the indices of A that define the three segments.
%    Let KEY = the input value of A(1).  Then
%    I <= L                 A(I) < KEY;
%         L < I < R         A(I) = KEY;
%                 R <= I    KEY < A(I).
%
  if ( n < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4VEC_PART_QUICK_A - Fatal error!\n' );
    fprintf ( 1, '  N < 1.\n' );
    error ( 'I4VEC_PART_QUICK_A - Fatal error!' );
  elseif ( n == 1 )
    l = 0;
    r = 2;
    return;
  end

  key = a(1);
  m = 1;
%
%  The elements of unknown size have indices between L+1 and R-1.
%
  l = 1;
  r = n + 1;

  for i = 2 : n

    if ( key < a(l+1) )
      r = r - 1;
      [ a(r), a(l+1) ] = i4_swap ( a(r), a(l+1) );
    elseif ( a(l+1) == key )
      m = m + 1;
      [ a(m), a(l+1) ] = i4_swap ( a(m), a(l+1) );
      l = l + 1;
    elseif ( a(l+1) < key )
      l = l + 1;
    end

  end
%
%  Now shift small elements to the left, and KEY elements to center.
%
  for i = 1 : l - m
    a(i) = a(i+m);
  end
%
%  Out of bounds here, occasionally.
%
  l = l - m;

  a(l+1:l+m) = key;

  return
end


function value = r4_cos ( x )

%*****************************************************************************80
%
%% R4_COS evaluates the cosine of an R4 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the cosine of X.
%

  persistent ntsn
  persistent pi2rec
  persistent pihi
  persistent pilo
  persistent pirec
  persistent sincs
  persistent xmax
  persistent xsml
  persistent xwarn
%
%  pihi + pilo = pi.  pihi is exactly representable on all machines
%  with at least 8 bits of precision.  whether it is exactly
%  represented depends on the compiler.  this routine is more
%  accurate if it is exactly represented.
%
  pi2 = 1.57079632679489661923;
  pi2rec = 0.636619772367581343;
  pihi = 3.140625;
  pilo = 9.6765358979323846E-04;
  pirec = 0.31830988618379067;

  if ( isempty ( ntsn ) )

    sincs = [ ...
      -0.374991154955873175840, ...
      -0.181603155237250201864, ...
      +0.005804709274598633559, ...
      -0.000086954311779340757, ...
      +0.000000754370148088851, ...
      -0.000000004267129665056, ...
      +0.000000000016980422945, ...
      -0.000000000000050120579, ...
      +0.000000000000000114101, ...
      -0.000000000000000000206 ]';

    ntsn = r4_inits ( sincs, 10, 0.1 * r4_mach ( 3 ) );
    xsml = sqrt ( 2.0 * r4_mach ( 3 ) );
    xmax = 1.0 / r4_mach ( 4 );
    xwarn = sqrt ( xmax );

  end

  absx = abs ( x );
  y = absx + pi2;

  if ( xmax < y )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_COS - Warning!\n' );
    fprintf ( 1, '  No precision because |X| is big.\n' );
    value = 0.0;
    return
  end

  if ( xwarn < y )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_COS - Warning!\n' );
    v '  Answer < half precision because |X| is big.\n' );
  end

  value = 1.0;

  if ( absx < xsml )
    return
  end

  xn = r4_aint ( y * pirec + 0.5 );
  n2 = r4_aint ( mod ( xn, 2.0 ) + 0.5 );
  xn = xn - 0.5;
  f = ( absx - xn * pihi ) - xn * pilo;

  xn = 2.0 * ( f * pi2rec ) * ( f * pi2rec ) - 1.0;
  value = f + f * r4_csevl ( xn, sincs, ntsn );

  if ( n2 ~= 0 )
    value = - value;
  end

  if ( value < - 1.0 )
    value = - 1.0;
  elseif ( 1.0 < value )
    value = + 1.0;
  end

  return
end


function jed = cws_to_jed_gps ( c, w, s )

%*****************************************************************************80
%
%% CWS_TO_JED_GPS converts a GPS CWS date to a JED.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 June 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer C, integer W, real S,
%    the GPS cycle/week/second date.
%
%    Output, real JED, the corresponding Julian Ephemeris Date.
%
  jed_epoch = epoch_to_jed_gps ( );

  d = ( 7 * ( 1024 * c + w ) ) + s / ( 24.0 * 60.0 * 60.0 );

  jed = jed_epoch + d;

  return
end