%% DEMO_stent_hexahedral_sweeping
% Below is a demonstration for:
%
% * Creating a hexahedral mesh for a vascular stent by sweeping allong a
% curve and copying over the segments.

%% Keywords
% * Sweeping, sweepLoft
% * Hexahedral mesh
% * stent, vascular
% * Exporting Abaqus, .inp

%%
clear; close all; clc;

%%
% plot settings

fontSize=25;
markerSize=10;
lineWidth=1;

%% Contol parameters

stentRadius=3; %The outer radius of the stent
numPeriodsWave=10; %The number of periods to use for a sinusoidal modulation
numStepsPeriod=100; %Number of sweeping steps allong a single period segment for sweeping
waveAmplitude=0.9; %Amplitude of the sinusoidal modulation
stentSectionHeight=0.1; %Height of the stent wire
stentSectionWidth=0.1; %Width of the stent wire
numStepsCircumference=(numPeriodsWave*numStepsPeriod)+1; %Number of sweeping steps across curve
overSampleFactorCurve=10; %Oversample factor curve
numSegments=8; %Number of stent segments to stack
segmentAxialOffset=waveAmplitude*2; %Axial offset for stacking stents

%% Build stent section
% The rectangular stent wire section is created here.

V_section=[-stentSectionWidth/2  stentSectionHeight/2 0; ...
            stentSectionWidth/2  stentSectionHeight/2 0; ...
            stentSectionWidth/2 -stentSectionHeight/2 0; ...
           -stentSectionWidth/2 -stentSectionHeight/2 0; ...
            ];

%%
% Visualize stent section
cFigure; hold on;
title('Stent section','fontSize',fontSize);
plotV(V_section,'b.-','lineWidth',lineWidth,'MarkerSize',markerSize);
view(2); axis tight; axis equal; grid on; box on;
set(gca,'fontSize',fontSize);
drawnow;

%% Create guide curve
% The sweepLoft (see |HELP_sweepLoft|) is created here. First and angle
% based parameterization is created. Next this curve is evenly sample
% across the curve length (see |HELP_evenlySampleCurve|).
t=linspace(0,2*pi,numStepsCircumference*overSampleFactorCurve); %Angles
t=t(1:end-1); %Remove last point so it is not closed for resampling
x=stentRadius.*sin(t); %x-coordinates
y=stentRadius.*cos(t); %y-coordinates
z=waveAmplitude.*sin(numPeriodsWave*t); %z-coordinates
V_guide_curve=[x(:) y(:) z(:)]; %Collected curve nodes
[V_guide_curve] = evenlySampleCurve(V_guide_curve,numStepsCircumference-1,'pchip',1); %Resample curve evenly
V_guide_curve(end+1,:)=V_guide_curve(1,:); %Append start to end so it is a closed loop

%%
% Visualize guide curve
cFigure; hold on;
title('Stent guide curve','fontSize',fontSize);
plotV(V_guide_curve,'k.-','lineWidth',lineWidth,'MarkerSize',markerSize);
axisGeom;
drawnow;

%% Position stent section at the start and end of the guide curve
% Next the section is translated and rotated so it is placed at the start
% of the guide curve such that the curve normal points allong the curve.

% Create rotation matrix
n3=vecnormalize(V_guide_curve(2,:)-V_guide_curve(1,:)); %Out of section normal direction z ish direction
[~,indMin]=min(dot(n3(ones(1,2),:),[1 0 0; 0 1 0],2)); %Get index most appropriate initial other axis
switch indMin
    case 1
        n1=[1 0 0]; %Initialized x direction
        n2=vecnormalize(cross(n3,n1)); %y ish direction
        n1=vecnormalize(cross(n2,n3)); %Proper x ish direction
        R=[n1; n2; n3]; %Rotation matrix
    case 2
        n2=[0 1 0]; %Initialized y direction
        n1=vecnormalize(cross(n2,n3)); %x ish direction
        n2=vecnormalize(cross(n3,n1)); %Proper y ish direction
        R=[n1; n2; n3]; %Rotation matrix
end

p1=V_guide_curve(1,:); %The start node
V_section=V_section*R; %Rotate the section
V_section=V_section+p1(ones(size(V_section,1),1),:); % Translate coordinate to start

%%
% Visualize guide curve
cFigure; hold on;
title('Stent section positioned on guide curve','fontSize',fontSize);
plotV(V_guide_curve,'k-','lineWidth',1);
plotV(V_section,'k.-','lineWidth',lineWidth,'MarkerSize',markerSize);
quiverVec(p1,n1,1,'r');
quiverVec(p1,n2,1,'g');
quiverVec(p1,n3,1,'b');
axisGeom;
drawnow;

%% Sweeping section allong curve
% Normally |sweepLoft| produces patch data as an output (e.g. faces and
% vertices). However these outputs are supressed here and the coordinate
% mesh output is instead used to create a hexahedral mesh. See also |HELP_sweepLoft|

numTwist=0; %Number of additional twists of loft feature around guide curve
numStepsSweep=numStepsCircumference; %Number of steps for loft feature from sketch 1 to sketch 2
plotOn=0; %Turn on plotting to view lofting behaviour
[~,~,~,S]=sweepLoft(V_section,V_section,n3,n3,V_guide_curve,numStepsSweep,numTwist,plotOn);

%% Construct hexahedral mesh

X=S.X'; Y=S.Y'; Z=S.Z'; %Coordinate matrices
V=[X(:) Y(:) Z(:)]; %Create node list

F=reshape((1:1:size(V,1)),4,size(V,1)/4)'; %All top and bottom faces
E=[F(2:end,:) F(1:end-1,:)]; %The hexahedral elements
[E,V]=mergeVertices(E,V); %Merge nodes (start and end are not shared yet)
C=hexVol(E,V); %Get hexahedral element volumes

[F,CF]=element2patch(E,C); %Create face data for plotting

%%
% Visualize hexahedral mesh

cFigure; hold on;
title('Stent hexahedral mesh','fontSize',fontSize);
plotV(V_guide_curve,'k-','lineWidth',3);
gpatch(F,V,CF,'k',1);
% patchNormPlot(F,V); %Check normal directions
colormap gjet; colorbar;
axisGeom;
camlight headlight;
drawnow;

%% Refine mesh
% The swept mesh can be refined through slitting. The splitting can be
% homogeneous or only in a particular direction (see HELP_subHex|)
% Split method explanation:
% 1: Overall splitting in all directions
% 2: Split allong curve direction
% 3: Split axially
% 4: Splint inward

splitMethod=3;
nRefine=1;
[E,V,Cs]=subHex(E,V,nRefine,splitMethod);

splitMethod=4;
nRefine=1;
[E,V,Css]=subHex(E,V,nRefine,splitMethod);
C=hexVol(E,V); %Get hexahedral element volumes

Cs=Cs(Css); %Colors for original element indices (and sweeping steps)

[F,CF]=element2patch(E,C); %Create face data

%%
% Visualize hexahedral mesh
cFigure; hold on;
title('Stent hexahedral mesh','fontSize',fontSize);
plotV(V_guide_curve,'k-','lineWidth',3);
gpatch(F,V,CF,'k',1);
% patchNormPlot(F,V);
colormap gjet; colorbar;
axisGeom;
camlight headlight;
drawnow;

%% Create additional segments
E_cell=repmat({E},1,numSegments);
V_cell=repmat({V},1,numSegments);
for q=1:1:numSegments
    V_cell{q}(:,3)=V_cell{q}(:,3)+((q-1)*segmentAxialOffset);
end
[ET,VT,CT]=joinElementSets(E_cell,V_cell);

[FT,CTF]=element2patch(ET,CT); %Create face data for plotting

%% Create face labelling to construct node sets

[indBoundary]=tesBoundary(FT,VT);
faceMarker=ones(size(ET,1),1)*(1:6); %The 6 face colors for the hexahedral faces
faceMarker=faceMarker(:); %Force as a column
FTb=FT(indBoundary,:); %Select the boundary faces (which will exclude tops (1) and bottoms (2))
faceBoundaryMarker=faceMarker(indBoundary,:)-2; %Get boundary colors and subtract 2 so they are 1-4

%%
% Visualize face labels

cFigure; hold on;
title('Stent hexahedral mesh','fontSize',fontSize);
gpatch(FTb,VT,faceBoundaryMarker,'none',1);
% patchNormPlot(F,V); %Check normal directions
colormap gjet; colorbar;
axisGeom;
camlight headlight;
drawnow;

%%
% Visualize composed segments and animate sweep

hf=cFigure;
gtitle('Stent hexahedral mesh',fontSize);
subplot(1,2,1); hold on;
plotV(V_guide_curve,'k-','lineWidth',3);
hp=gpatch(F,V,'gw','k',1);
plotV(V_guide_curve,'k-','lineWidth',3);
axisGeom;
camlight headlight;
axis off; axis manual;

subplot(1,2,2); hold on;
gpatch(FT,VT,'gw','none',1);
axisGeom;
camlight headlight;
lighting gouraud
axis off;
drawnow;

%%

[F,CFs]=element2patch(E,Cs); %Create face data for plotting

nSteps=40; %Number of animation steps
animStruct.Time=round(linspace(1,max(CFs(:)),nSteps));%Create the time vector

for q=1:1:nSteps
    F_now=F(ismember(CFs,1:animStruct.Time(q)),:);
    %Set entries in animation structure
    animStruct.Handles{q}=hp; %Handles of objects to animate
    animStruct.Props{q}={'Faces'}; %Properties of objects to animate
    animStruct.Set{q}={F_now}; %Property values for to set in order to animate
end

anim8(hf,animStruct);

%% Create node set for inner surface nodes

FTb_inner=FTb(faceBoundaryMarker==2,:); %Inner faces
indicesNodesInner=unique(FTb_inner(:)); %Inner nodes
indicesElementsInner=find(sum(ismember(ET,indicesNodesInner),2)==4); %Inner elements

%%
% Visualize face labels

[F_inner_elements]=element2patch(ET(indicesElementsInner,:)); %Create face data for plotting
C_side=ones(numel(indicesElementsInner),1)*(1:1:6);
C_side=C_side(:);

cFigure; hold on;
title('Stent hexahedral mesh','fontSize',fontSize);
gpatch(FTb,VT,'kw','none',0.2);
% gpatch(FTb_inner,VT,'gw','k',1);
gpatch(F_inner_elements,VT,C_side,'none',1);
% plotV(VT(indicesNodesInner,:),'g.','MarkerSize',25);
colormap(gjet(6)); icolorbar;
axisGeom;
camlight headlight;
drawnow;

%% Setup structure to define an Abaqus inp file

%%--> Heading
abaqus_spec.Heading.COMMENT{1}='Job name: ABAQUS inp file creation demo';
abaqus_spec.Heading.COMMENT{2}='Generated by: GIBBON';

%%--> Preprint
abaqus_spec.Preprint.ATTR.echo='NO';
abaqus_spec.Preprint.ATTR.model='NO';
abaqus_spec.Preprint.ATTR.history='NO';
abaqus_spec.Preprint.ATTR.contact='NO';

%--> Part

% Node
nodeIds=(1:1:size(VT,1))';
abaqus_spec.Part.COMMENT='This section defines the part geometry in terms of nodes and elements';
abaqus_spec.Part.ATTR.name='Stent';
abaqus_spec.Part.Node={nodeIds,VT};

% Element
elementIds=(1:1:size(ET,1))';
abaqus_spec.Part.Element{1}.ATTR.type='C3D8';%'C3D8R';
abaqus_spec.Part.Element{1}.VAL={elementIds,ET};

% Element sets
abaqus_spec.Part.Elset{1}.ATTR.elset='Set-1';
abaqus_spec.Part.Elset{1}.VAL=elementIds;

surfaceElementSetName='elementSetSideSurface';
abaqus_spec.Part.Elset{2}.ATTR.elset=surfaceElementSetName;
abaqus_spec.Part.Elset{2}.ATTR.internal=''; %Remains hidden uppon import
abaqus_spec.Part.Elset{2}.VAL=indicesElementsInner(:);

% Surfaces
sidePick=5;
abaqus_spec.Part.Surface{1}.ATTR.type='ELEMENT';
abaqus_spec.Part.Surface{1}.ATTR.name=[surfaceElementSetName,'_side',num2str(sidePick)];
abaqus_spec.Part.Surface{1}.VAL={surfaceElementSetName,['S',num2str(sidePick)]};

% Sections
abaqus_spec.Part.Solid_section.ATTR.elset='Set-1';
abaqus_spec.Part.Solid_section.ATTR.material='Elastic';

%--> Assembly
abaqus_spec.Assembly.ATTR.name='Assembly-1';
abaqus_spec.Assembly.Instance.ATTR.name='Stent-assembly';
abaqus_spec.Assembly.Instance.ATTR.part='Stent';

abaqus_spec.Assembly.Nset{1}.ATTR.nset='Set-1';
abaqus_spec.Assembly.Nset{1}.ATTR.instance=abaqus_spec.Assembly.Instance.ATTR.name;
abaqus_spec.Assembly.Nset{1}.VAL=indicesNodesInner(:);

%%

%%--> Material
abaqus_spec.Material.ATTR.name='Elastic';
abaqus_spec.Material.Elastic=[0.5 0.4];

%%--> Step
abaqus_spec.Step.ATTR.name='Step-1';
abaqus_spec.Step.ATTR.nlgeom='YES';
abaqus_spec.Step.Static=[0.1 1 1e-5 0.1];

% % Boundary
% abaqus_spec.Step.Boundary{1}.VAL={'Set-1',[1,1]};
% abaqus_spec.Step.Boundary{2}.VAL={'Set-2',[2,2]};
% abaqus_spec.Step.Boundary{3}.VAL={'Set-3',[3,3]};
% abaqus_spec.Step.Boundary{4}.VAL={'Set-4',[3,3],-0.1};

%Output
abaqus_spec.Step.Restart.ATTR.write='';
abaqus_spec.Step.Restart.ATTR.frequency=0;

abaqus_spec.Step.Output{1}.ATTR.field='';
abaqus_spec.Step.Output{1}.ATTR.variable='PRESELECT';
abaqus_spec.Step.Output{2}.ATTR.history='';
abaqus_spec.Step.Output{2}.ATTR.variable='PRESELECT';
% abaqus_spec.Step.Node_print.ATTR.nset='all';
% abaqus_spec.Step.Node_print.ATTR.frequency = 1;
% abaqus_spec.Step.Node_print.VAL='COORD';
% abaqus_spec.Step.El_print.VAL='S';

%% Creating the INP file
% You can use |abaqusStruct2inp| to write the structure data to a file.

%Create file name for INP file
defaultFolder = fileparts(fileparts(mfilename('fullpath')));
savePath=fullfile(defaultFolder,'data','temp');
fileName=fullfile(savePath,'tempModel.inp');
[~,fileNamePart,~]=fileparts(fileName);

% Export INP file
abaqusStruct2inp(abaqus_spec,fileName);

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function Test_symYY()
% function test98()
% Test for symfixedrankYYquotientfactory geometry (low-rank PSD matrix completion)
%
% Paper link: http://www.di.ens.fr/~fbach/journee2010_sdp.pdf
%

% This file is part of Manopt: www.manopt.org.
% Original author: Bamdev Mishra, July 11, 2013.
% Contributors:
% Change log:

% We know about this warning, so it is safe to turn it off for now.
warning('off', 'manopt:symfixedrankYYfactory:exp');

clc; close all;

% Problem data
n = 1000;
r = 5;
Y_org = randn(n, r);
A = Y_org*Y_org';

% Create the problem structure
% quotient YY geometry
problem.M = symfixedrankYYfactory(n, r);

df = problem.M.dim();
p = 3.5*df/(n*n);
mask = spones(sprandsym(n, p));
prop_known = sum(sum(mask == 1))/(n*n);
fprintf('Fraction of entries given: %f \n', full(prop_known));

problem.cost = @(Y) norm(mask.*(Y*Y' - A),'fro')^2;

egrad = @(Y) 4*((mask.^2).*(Y*Y' - A))*Y;
ehess = @(Y, U) 4*((mask.^2) .* (Y*Y' - A))*U + 4*((mask.^2) .* (Y*U' + U*Y'))*Y;

problem.grad = @(Y) problem.M.egrad2rgrad(Y, egrad(Y));
problem.hess = @(Y, U) problem.M.ehess2rhess(Y, egrad(Y), ehess(Y, U), U);

% % Check numerically whether gradient and Hessian are correct
%     checkgradient(problem);
%     drawnow;
%     pause;
%     checkhessian(problem);
%     drawnow;
%     pause;

% Initialization
[U0, S0, ~ ] = svds(mask.*A, r);
Y0 = U0*(S0.^0.5);

% Options (not mandatory)
options.maxiter = inf;
options.maxinner = 30;
options.maxtime = 120;
options.tolgradnorm = 1e-5;
options.Delta_bar = n * r;
options.Delta0 = options.Delta_bar / 8;

% Pick an algorithm to solve the problem
[Yopt costopt info] = trustregions(problem, Y0, options);
% [Yopt costopt info] = steepestdescent(problem, Y0, options);
% [Yopt costopt info] = conjugategradient(problem, Y0, options);

end


function [S, V, D, Sigma2] = MySVD(A)
[m, n] = size(A);
if 2*m < n
    AAT = A*A';
    [S, Sigma2, D] = svd(AAT);
    Sigma2 = diag(Sigma2);
    V = sqrt(Sigma2);
    tol = max(size(A)) * eps(max(V));
    R = sum(V > tol);
    V = V(1:R);
    S = S(:,1:R);
    D = A'*S*diag(1./V);
    V = diag(V);
    return;
end
if m > 2*n
    [S, V, D, Sigma2] = MySVD(A');
    mid = D;
    D = S;
    S = mid;
    return;
end
[S,V,D] = svd(A);
Sigma2 = diag(V).^2;


function y = sturm(X,BC,F,G,R)
% STURM  Solve the Sturm-Liouville equation:
%        d( F*dY/dX )/dX - G*Y = R using linear finite elements.
%
%        Y = STURM(X,BC,'F','G','R')
%
%        INPUT:
%        X  - a one-dimensional grid-point array of length N.
%        BC - is a 2 by 3 matrix [A1, B1, C1 ; An, Bn, Cn]
%             specifying the boundary conditions, which are of
%             the form: A1*Y'(1) + B1*Y(1) = C1 and
%             An*Y'(n) + Bn*Y(n) = Cn. Dirichlet boundary
%             conditions can be applied by setting A1 and/or An
%             to zero.
%        F(X), G(X) and R(X) are user-supplied functions. These
%        must be provided as separate M-files (F.M, G.M and R.M),
%        and must return an array of same shape and length as X.
%
%        The result can be displayed by: plot(X,Y).
%
% Alex Pletzer: pletzer@pppl.gov (Aug. 97/July 99).
%

[n1,n2] = size(X);
x = X(:);
n=length(x);    % # of f e
n1 = n-1;
x = sort(x);
xmin = x(1); xmax = x(n);
dx = diff(x); x = x(1:n1);

% boundary conditions

[nbc1,nbc2] = size(BC);
if ([nbc1 nbc2] ~= [2 3]),
        disp('Error calling STURM: Wrong format for BC specification')
        disp('the second argument must be a 2 times 3 matrix')
        disp('Homgeneous Dirichlet boundary conditions assumed')
        BC = [0 1 0;0 1 0]
end

if (BC(1,:) == [0 0 0]) | (BC(2,:) == [0 0 0]),
        disp('Error calling STURM: Null BC specification')
        BC
        disp('Homgeneous Dirichlet boundary conditions assumed')
        BC = [0 1 0;0 1 0]
end

a1 = BC(1,1); b1 = BC(1,2); c1 = BC(1,3);
an = BC(2,1); bn = BC(2,2); cn = BC(2,3);

% 3pt gauss' grid

abcis = [-0.77459666924148,0.0,+0.77459666924148];
abcis = (abcis+1)/2;
weigh = [0.5555555555556,0.88888888888889,0.55555555555556];
weigh = weigh/2;

x1 = x + dx*abcis(1);
x2 = x + dx*abcis(2);
x3 = x + dx*abcis(3);

xg = reshape( [conj(x1');conj(x2');conj(x3')],1,3*n1);

% f, g  and s functions

fg = feval(F,xg);
gg = feval(G,xg);
sg = feval(R,xg);

% construct matrices

fg = conj(reshape(fg,3,n1)');
gg = conj(reshape(gg,3,n1)');
sg = conj(reshape(sg,3,n1)');
g1 = gg(:,1); g2 = gg(:,2); g3 = gg(:,3);

% "kinetic"

k1 = (fg*weigh')./dx;
k1 = [0;k1]; k2 = k1(2:n);
k2 = [k2;0];
kd = k2 + k1;   % diagonal
ko = - k1(2:n); % diagonal-1
k = sparse( diag(kd) + diag(ko,-1) + diag(ko,+1) );

% "potential"

v1 = ( gg*conj(weigh.*abcis.^2)'         ).*dx; % / * /

v2 = ( gg*conj(weigh.*abcis.*(1-abcis))' ).*dx; % / * \

v3 = ( gg*conj(weigh.*(1-abcis).^2)'     ).*dx; % \ * \

v1 = [0;v1]; v3 = [v3;0];

vd = v1 + v3;
vo = v2;
v = sparse( diag(vd) + diag(vo,-1) + diag(vo,+1) );

% total

w = k + v;

% source

s1 = - ( sg*conj(weigh.*abcis)'         ).*dx; % /
s2 = - ( sg*conj(weigh.*(1-abcis))'     ).*dx; % \
s1 = [0;s1]; s2 = [s2;0];
s = s2 + s1;

% boundary conditions

        % left endpoint
        if a1 == 0,
                disp('Dirichlet BC at left endpoint:')
                disp(['y(1) = ',num2str(c1/b1)])
                w(1,:) = zeros(1,n); w(1,1)=1; s(1)=c1/b1;
        else
                disp('General BC at left endpoint:')
                disp(['y_x(1) + ',num2str(b1/a1),' y(1) = ',num2str(c1/a1)])
                w(1,1) = w(1,1) - b1/a1; s(1) = s(1) - c1/a1;
        end

        % right endpoint
        if an == 0,
                disp('Dirichlet BC at right endpoint:')
                disp(['y(n) = ',num2str(cn/bn)])
                w(n,:) = zeros(1,n); w(n,n)=1; s(n)=cn/bn;
        else
                disp('General BC at right endpoint:')
                disp(['y_x(n) + ',num2str(bn/an),' y(n) = ',num2str(cn/an)])
                w(n,n) = w(n,n) + bn/an; s(n) = s(n) + cn/an;
        end

% solve linear system

y=w\s;

[m1,m2] = size(y);
if (m1 == n2) & (m2 == n1), y = conj(y'); end


function edge = clipLine(line, box, varargin)
%CLIPLINE Clip a line with a box.
%
%   EDGE = clipLine(LINE, BOX);
%   LINE is a straight line given as a 4 element row vector: [x0 y0 dx dy],
%   with (x0 y0) being a point of the line and (dx dy) a direction vector,
%   BOX is the clipping box, given by its extreme coordinates:
%   [xmin xmax ymin ymax].
%   The result is given as an edge, defined by the coordinates of its 2
%   extreme points: [x1 y1 x2 y2].
%   If line does not intersect the box, [NaN NaN NaN NaN] is returned.
%
%   Function works also if LINE is a N-by-4 array, if BOX is a Nx4 array,
%   or if both LINE and BOX are N-by-4 arrays. In these cases, EDGE is a
%   N-by-4 array.
%
%
%   Example
%   line = [30 40 10 0];
%   box = [0 100 0 100];
%   res = clipLine(line, box)
%   res =
%       0 40 100 40
%
%   See also
%   lines2d, boxes2d, edges2d
%   clipEdge, clipRay, clipLine3d
%

% ------
% Author: David Legland
% E-mail: david.legland@inra.fr
% Created: 2007-08-27, using Matlab 7.4.0.287 (R2007a)
% Copyright 2007-2022 INRA - Cepia Software Platform

% adjust size of two input arguments
nLines = size(line, 1);
nBoxes = size(box, 1);
if nLines == 1 && nBoxes > 1
    line = repmat(line, nBoxes, 1);
elseif nBoxes == 1 && nLines > 1
    box = repmat(box, nLines, 1);
elseif nLines ~= nBoxes
    error('bad sizes for input');
end

% allocate memory
nLines = size(line, 1);
edge   = zeros(nLines, 4);

% main loop on lines
for i = 1:nLines
    % extract limits of the box
    xmin = box(i, 1);
    xmax = box(i, 2);
    ymin = box(i, 3);
    ymax = box(i, 4);

    % use direction vector for box edges similar to direction vector of the
    % line in order to reduce computation errors
    delta = hypot(line(i,3), line(i,4));

	% compute intersection with each edge of the box
    px1 = intersectLines(line(i,:), [xmin ymin delta 0]);   % lower edge
    px2 = intersectLines(line(i,:), [xmax ymin 0 delta]);   % right edge
    py1 = intersectLines(line(i,:), [xmax ymax -delta 0]);  % upper edge
    py2 = intersectLines(line(i,:), [xmin ymax 0 -delta]);  % left edge

    % remove undefined intersections (case of lines parallel to box edges)
    points = [px1 ; px2 ; py1 ; py2];
    points = points(isfinite(points(:,1)), :);

    % sort points according to their position on the line
    pos = linePosition(points, line(i,:));
    [pos, inds] = sort(pos); %#ok<ASGLU>
    points = points(inds, :);

    % create clipped edge by using the two points in the middle
    ind = size(points, 1)/2;
    inter1 = points(ind,:);
    inter2 = points(ind+1,:);
    edge(i, 1:4) = [inter1 inter2];

    % check that middle point of the edge is contained in the box
    midX = mean(edge(i, [1 3]));
    xOk = xmin <= midX && midX <= xmax;
    midY = mean(edge(i, [2 4]));
    yOk = ymin <= midY && midY <= ymax;

    % if one of the bounding condition is not met, set edge to NaN
    if ~(xOk && yOk)
        edge (i,:) = NaN;
    end
end


function linplus_test14 ( )

%*****************************************************************************80
%
%% TEST14 tests R8BB_FA, R8BB_SL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2009
%
%  Author:
%
%    John Burkardt
%
  n1 = 0;
  n2 = 10;
  n = n1 + n2;
  ml = 0;
  mu = 0;
  na = ( 2 * ml + mu + 1 ) * n1 + 2 * n1 * n2 + n2 * n2;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST14\n' );
  fprintf ( 1, '  For a border banded matrix:\n' );
  fprintf ( 1, '  R8BB_FA factors;\n' );
  fprintf ( 1, '  R8BB_SL solves.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix order N     = %d\n', n );
  fprintf ( 1, '  Matrix suborder N1 = %d\n', n1 );
  fprintf ( 1, '  Matrix suborder N2 = %d\n', n2 );
  fprintf ( 1, '  Lower bandwidth ML = %d\n', ml );
  fprintf ( 1, '  Upper bandwidth MU = %d\n', mu );
%
%  Set the matrix.
%
  [ a, seed ] = r8bb_random ( n1, n2, ml, mu, seed );
%
%  Set the desired solution.
%
  x = r8vec_indicator ( n );
%
%  Compute the corresponding right hand side.
%
  b = r8bb_mxv ( n1, n2, ml, mu, a, x );
%
%  Factor the matrix.
%
  [ a_lu, pivot, info ] = r8bb_fa ( n1, n2, ml, mu, a );

  if ( info ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEST14 - Fatal error!\n' );
    fprintf ( 1, '  R8BB_FA claims the matrix is singular.\n' );
    fprintf ( 1, '  The value of INFO is %d\n', info );
    return
  end
%
%  Solve the system.
%
  x = r8bb_sl ( n1, n2, ml, mu, a_lu, pivot, b );

  r8vec_print ( n, x, '  Solution:' );

  return
end


clear all

Nsamples = 41500;
EulerSaved = zeros(Nsamples, 3);

dt = 0.01;

for k=1:Nsamples
  [p q r] = GetGyro();
  A = eye(4) + dt*1/2*[ 0  -p  -q  -r;
                        p   0   r  -q;
                        q  -r   0   p;
                        r   q  -p   0
                      ];

  [ax ay] = GetAccel();
  [phi theta] = EulerAccel(ax, ay);
  z = EulerToQuaternion(phi, theta, 0);

  [phi theta psi] = EulerKalman(A, z);

  EulerSaved(k, :) = [ phi theta psi ];
end

PhiSaved   = EulerSaved(:, 1) * 180/pi;
ThetaSaved = EulerSaved(:, 2) * 180/pi;
PsiSaved   = EulerSaved(:, 3) * 180/pi;

t = 0:dt:Nsamples*dt-dt;

figure
plot(t, PhiSaved)

figure
plot(t, ThetaSaved)

figure
plot(t, PsiSaved)


function [X,Y,Z]=oblate0(eta,theta,psi)
% [X,Y,Z]=oblate0(eta,theta,psi) defines
% oblate spheroidal coordinates
X=cosh(eta).*sin(theta).*cos(psi);
Y=cosh(eta).*sin(theta).*sin(psi);
Z=sinh(eta).*cos(theta);


%==============================================================================
% This code is part of the Finite Element Method app for the Matlab-based toolbox
%  FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR/FAIRFEM
%==============================================================================
%
% function [Sc,dS,d2S] = elasticFEM(uc,xc,tri,varargin)
%
% linear elastic regularizer for tetrahedral finite element discretization
% with linear basis functions
%
% S(u) = \int || B * u  ||^2 dx
%
% where B is the navier lame operator
%
% Input:
%   uc    - coefficients of current displacement
%   yRef  - coefficients of reference transformation, yc = uc + yRef
%   Mesh  - representation of mesh
%
% Output:
%    Sc    - hyperelastic energy
%    dS    - first derivative
%    d2S   - Hessian
%
% see also elastic.m (staggered grid version)
%==============================================================================

function [Sc,dS,d2S] = elasticFEM(uc,~,Mesh,varargin)

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

persistent A alphaOld
if ~exist('A','var'),        A        = []; end;
if ~exist('alphaOld','var'), alphaOld = []; end;

% default parameter
matrixFree  = 0;
alpha       = 1;
mu          = 1;
lambda      = 0;

for k=1:2:length(varargin), % overwrites default parameter
    eval([varargin{k},'=varargin{',int2str(k+1),'};']);
end;

if not(matrixFree), % matrix-based
    build = isempty(alphaOld) || any(alphaOld~=[alpha mu lambda]) ||...
        isempty(A) || size(A,2)~=numel(uc);
    if build,
        alphaOld = [alpha mu lambda];
        A = getElasticMatrixFEM(Mesh,mu,lambda);
        A = alpha * (A'* A);
    end;
    dS  = uc'*A;
    Sc  = 0.5*dS*uc;
    d2S = A;
else % matrix-free
    d2S.regularizer = regularizer;
    d2S.alpha  = alpha;
    d2S.By     = @(u,Mesh) elasticOperator(u,Mesh,mu,lambda,'By');
    d2S.BTy    = @(u,Mesh) elasticOperator(u,Mesh,mu,lambda,'BTy');
    d2S.B      = @(Mesh) getElasticMatrixFEM(Mesh,mu,lambda);
    d2S.diag   = @(Mesh) getDiag(Mesh,mu,lambda,alpha);
    d2S.solver = @FEMMultiGridsolveGN;
    d2S.d2S  = @(uc,Mesh) ...
        alpha * ...
        elasticOperator(elasticOperator(uc,Mesh,mu,lambda,'By'),Mesh,mu,lambda,'BTy');

    dS   = d2S.d2S(uc,Mesh)';
    Sc   = .5*dS*uc;
end

function By = elasticOperator(u,Mesh,mu,lambda,flag)
vol  = sqrt(Mesh.vol);
a    = sdiag(sqrt(mu)*vol);
b    = sdiag(sqrt(mu+lambda)*vol);
flag = [num2str(Mesh.dim) 'D-' flag];
switch flag
    case '2D-By'
        u  = reshape(u,[],2);
        dx1 = Mesh.mfdx1.D ; dx2 = Mesh.mfdx2.D;

        By = [...
            a*dx1(u(:,1));...
            a*dx2(u(:,1));...
            a*dx1(u(:,2));...
            a*dx2(u(:,2));...
            b*dx1(u(:,1))+b*dx2(u(:,2))...
            ];
    case '2D-BTy'
        u = reshape(u,[],5);
        dx1 = Mesh.mfdx1.Dadj ; dx2 = Mesh.mfdx2.Dadj;

        By = [dx1(a*u(:,1))+dx2(a*u(:,2))+dx1(b*u(:,5));...
            dx1(a*u(:,3))+dx2(a*u(:,4))+dx2(b*u(:,5))];
    case '3D-By'
        u  = reshape(u,[],3);
        dx1 = Mesh.mfdx1.D ; dx2 = Mesh.mfdx2.D; dx3 = Mesh.mfdx3.D;

        By = [...
                a*dx1(u(:,1));...
                a*dx2(u(:,1));...
                a*dx3(u(:,1));...
                a*dx1(u(:,2));...
                a*dx2(u(:,2));...
                a*dx3(u(:,2));...
                a*dx1(u(:,3));...
                a*dx2(u(:,3));...
                a*dx3(u(:,3));...
                b*dx1(u(:,1))+b*dx2(u(:,2))+b*dx3(u(:,3))...
            ];
    case '3D-BTy'
        u = reshape(u,[],10);
        dx1 = Mesh.mfdx1.Dadj ; dx2 = Mesh.mfdx2.Dadj; dx3 = Mesh.mfdx3.Dadj;

        By = [dx1(a*u(:,1))+dx2(a*u(:,2))+dx3(a*u(:,3))+dx1(b*u(:,10));...
            dx1(a*u(:,4))+dx2(a*u(:,5))+dx3(a*u(:,6))+dx2(b*u(:,10));...
            dx1(a*u(:,7))+dx2(a*u(:,8))+dx3(a*u(:,9))+dx3(b*u(:,10))...
            ];

    otherwise
        error('unknown flag: %s',flag);
end

% get diagonal of d2S (interesting in matrix free mode)
function D = getDiag(Mesh,mu,lambda,alpha)
dim = Mesh.dim;
vol = Mesh.vol;
if dim==2
    xn = Mesh.xn;
    x1 =  Mesh.mfPi(xn,1);
    x2 =  Mesh.mfPi(xn,2);
    x3 =  Mesh.mfPi(xn,3);
    e1 =  x1 - x3;
    e2 =  x2 - x3;

    % compute gradients of basis functions
    dphi1 =   [ e2(:,2) -e2(:,1)]./[2*vol,2*vol];
    dphi2 =   [-e1(:,2)  e1(:,1)]./[2*vol,2*vol];
    dphi3 = -dphi1 - dphi2;

    % get boundaries
    Dxi = @(i)   Mesh.mfPi(vol.*dphi1(:,i).^2,1) ...
        + Mesh.mfPi(vol.*dphi2(:,i).^2,2) ...
        + Mesh.mfPi(vol.*dphi3(:,i).^2,3); % diagonal of Dx1'*Dx1

    D1 = (2*mu+lambda)*Dxi(1)+            mu*Dxi(2);
    D2 = mu*Dxi(1)           + (2*mu+lambda)*Dxi(2);
    D = [D1;D2];
else
    xn = Mesh.xn;
    % compute edges
    v1   = Mesh.mfPi(xn,1);
    v2   = Mesh.mfPi(xn,2);
    v3   = Mesh.mfPi(xn,3);
    v4   = Mesh.mfPi(xn,4);
    e1   =  v1 - v4;
    e2   =  v2 - v4;
    e3   =  v3 - v4;
    % compute inverse transformation to reference element
    Ainv =  [
        e2(:,2).*e3(:,3)-e2(:,3).*e3(:,2), ...
        -(e1(:,2).*e3(:,3)-e1(:,3).*e3(:,2)), ...
        e1(:,2).*e2(:,3)-e1(:,3).*e2(:,2), ...
        -(e2(:,1).*e3(:,3)-e2(:,3).*e3(:,1)), ...
        e1(:,1).*e3(:,3)-e1(:,3).*e3(:,1), ...
        -(e1(:,1).*e2(:,3)-e1(:,3).*e2(:,1)), ...
        e2(:,1).*e3(:,2)-e2(:,2).*e3(:,1), ...
        -(e1(:,1).*e3(:,2)-e1(:,2).*e3(:,1)), ...
        e1(:,1).*e2(:,2)-e1(:,2).*e2(:,1), ...
        ];
    detA = e1(:,1).*Ainv(:,1) + e2(:,1).*Ainv(:,2)+ e3(:,1).*Ainv(:,3);

    % compute gradients of basis functions
    dphi1 =   Ainv(:,1:3)./repmat(detA,[1 3]);
    dphi2 =   Ainv(:,4:6)./repmat(detA,[1 3]);
    dphi3 =   Ainv(:,7:9)./repmat(detA,[1 3]);
    dphi4 =   1 - dphi1 - dphi2 - dphi3;

    Dxi = @(i) Mesh.mfPi(vol.*dphi1(:,i).^2,1) +  Mesh.mfPi(vol.*dphi2(:,i).^2,2)  ...
        + Mesh.mfPi(vol.*dphi3(:,i).^2,3) +  Mesh.mfPi(vol.*dphi4(:,i).^2,4);

    D1 = (2*mu+lambda)*Dxi(1)+            mu*Dxi(2) + mu *Dxi(3);
    D2 = (2*mu+lambda)*Dxi(2)+            mu*Dxi(1) + mu *Dxi(3);
    D3 = (2*mu+lambda)*Dxi(3)+            mu*Dxi(1) + mu *Dxi(2);
    D = [D1;D2;D3];
end
D = alpha*reshape(D,[],1);

function B = getElasticMatrixFEM(Mesh,mu,lambda)
vol = sqrt(Mesh.vol);
a   = sdiag(sqrt(mu)*vol);
b   = sdiag(sqrt(mu+lambda)*vol);
switch Mesh.dim
    case 2
        dx1 = Mesh.dx1; dx2 = Mesh.dx2;
        B = [a*dx1;a*dx2];
        B = [ blkdiag(B,B); b*dx1, b*dx2];
    case 3
        dx1 = Mesh.dx1; dx2 = Mesh.dx2; dx3 = Mesh.dx3;
        B = [a*dx1;a*dx2;a*dx3];
        B = [ blkdiag(B,B,B); b*dx1, b*dx2, b*dx3];
end

% shortcut for sparse diagonal matrix
function A = sdiag(v)
A = spdiags(v(:),0,numel(v),numel(v));

function runMinimalExample
% ====== 2D
omega = [0,10,0,8]; m = [17,16]; p = [5,6];
w = zeros([p,2]);  w(3,3,1) = 0.06; w(3,4,2) = -0.05;
xn = reshape(getNodalGrid(omega,m),[],2);
yn = splineTransformation2D(w(:),xn(:),'omega',omega,'m',m+1,'p',p,'Q',[]);

Mesh = TriMesh1(omega,m);

regOptn = {'alpha',1,'mu',1,'lambda',0};

fctn = @(y) feval(mfilename,y-xn(:),xn(:),Mesh,regOptn{:},'matrixFree',0);
[Sc, dS, d2S] = fctn(yn(:));
checkDerivative(fctn,yn(:));

[Scmf, dSmf, d2Smf] = feval(mfilename,yn(:)-xn(:),xn(:),Mesh,regOptn{:},'matrixFree',1);

err1 = abs(Sc-Scmf)/abs(Sc);
err2 = norm(dS-dSmf)/norm(dS);

y = randn(5*size(Mesh.tri,1),1); x = rand(numel(xn),1);
err3 = norm(d2S*x-d2Smf.d2S(x,Mesh))/norm(d2S*x);
err4 = abs(y'*d2Smf.By(x,Mesh)-x'*d2Smf.BTy(y,Mesh));
fprintf('%s, MFerror(S,dS,d2S): [%1.2e,%1.2e,%1.2e] OK? %d\n\t\tAdjointError: %1.2e, OK? %d\n',...
    mfilename,err1,err2,err3,max([err1;err2;err3])<1e-14,err4,err4<1e-13);
err5 = norm(full(diag(d2S))-d2Smf.diag(Mesh))/norm(full(diag(d2S)));
fprintf('\t\tDiagErrorMF: %1.2e OK? %d\n',err5,err5<1e-13);

% ====== 3D
omega = [0,10,0,8,0 3]; m = [3,5,6];
Mesh = TetraMesh1(omega,m);

xn = Mesh.xn;
yn = xn + randn(size(xn));

regOptn = {'alpha',1,'mu',1,'lambda',0};

fctn = @(y) feval(mfilename,y-xn(:),xn(:),Mesh,regOptn{:},'matrixFree',0);
[Sc, dS, d2S] = fctn(yn(:));
checkDerivative(fctn,yn(:));

[Scmf, dSmf, d2Smf] = feval(mfilename,yn(:)-xn(:),xn(:),Mesh,regOptn{:},'matrixFree',1);

err1 = abs(Sc-Scmf)/abs(Sc);
err2 = norm(dS-dSmf)/norm(dS);

y = randn(10*size(Mesh.tri,1),1); x = rand(numel(xn),1);
err3 = norm(d2S*x-d2Smf.d2S(x,Mesh))/norm(d2S*x);
err4 = abs(y'*d2Smf.By(x,Mesh)-x'*d2Smf.BTy(y,Mesh));
fprintf('%s, MFerror(S,dS,d2S): [%1.2e,%1.2e,%1.2e] OK? %d\n\t\tAdjointError: %1.2e, OK? %d\n',...
    mfilename,err1,err2,err3,max([err1;err2;err3])<1e-14,err4,err4<1e-13);
err5 = norm(full(diag(d2S))-d2Smf.diag(Mesh))/norm(full(diag(d2S)));
fprintf('\t\tDiagErrorMF: %1.2e OK? %d\n',err5,err5<1e-13);


% Control of a water tank temperature
% Case of Proportional-Integral Control
% Author's Data: Housam BINOUS
% Department of Chemical Engineering
% National Institute of Applied Sciences and Technology
% Tunis, TUNISIA
% Email: binoushousam@yahoo.com

% function PI_control

function xdot=PI_control(t,x)

V = 100;
F = 10;
rho = 1;
cp = 4.19;
Q0 = 2500;
TRset = 80;
T0 = 20;
TauSens = 2;
Kp = 300;
TauQ = 1;
TauI=9;

QC= Q0 + Kp*(TRset - x(3))+Kp/TauI*x(4);

xdot(1) = 1/(V*rho*cp)*(F*rho*cp*(T0-x(1))+x(2));
xdot(2) = (QC-x(2))/TauQ;
xdot(3) = 1/TauSens*(x(1)-x(3));
xdot(4) = TRset - x(3);

xdot=xdot';


function nd = lagrange_interp_nd_size ( m, n_1d )

%*****************************************************************************80
%
%% LAGRANGE_INTERP_ND_SIZE sizes an M-dimensional Lagrange interpolant.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 September 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N_1D(M), the order of the 1D rule to be used
%    in each dimension.
%
%    Output, integer ND, the number of points in the product grid.
%
  nd = prod ( n_1d(1:m) );

  return
end


%% rhombicDodecahedronMesh
% Below is a demonstration of the features of the |rhombicDodecahedronMesh| function

%%
clear; close all; clc;

%% Syntax
% |[Fc_Q,Fc_T,Ft_Q,Ft_T,Ct_Q,Ct_T,Vt]=rhombicDodecahedronMesh(r,nCopies)|

%% Description
% Creates a rhombic dodecahedron mesh where r sets the radias and nCopies
% (a 1x3 vector) sets the number of copies in the x, y, and z direction.
% The output consists of:
%
% Fc_Q, Fc_T: the quadrilateral and triangular face cell arrays (1 cell
% entry per element).
%
% Ft_Q, Ft,T: the quadrilateral and triangular face arrays
%
% Ct_Q, Ct,T: color/label data for the face arrays
%
% Vt: the vertex array

%%
% Plot settings
fontSize=15;
faceAlpha1=0.8;

%% Creating a mesh of rhombic dodecahedra

r=0.5; %Radii, results in a width of 1
nCopies=[3 3 3]; %Number of offset copies

[E,V,C,F,CF]=rhombicDodecahedronMesh(r,nCopies);

%%
% Plotting results
cFigure; hold on;
title('A mesh of rhombicDodecahedra','FontSize',fontSize);
gpatch(F,V,CF,'k',faceAlpha1);
colormap(gjet);
axisGeom(gca,fontSize);
camlight('headlight');
drawnow;

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function M = convectionUpwindTermCylindrical1D(u)
% This function uses the upwind scheme to discretize a 1D
% convection term in the form \grad (u \phi) where u is a face vactor
% It also returns the x and y parts of the matrix of coefficient.
%
% SYNOPSIS:
%   M = convectionUpwindTermCylindrical1D(u)
%
% PARAMETERS:
%
%
% RETURNS:
%
%
% EXAMPLE:
%
% SEE ALSO:
%

% extract data from the mesh structure
Nr = u.domain.dims(1);
G = 1:Nr+2;
DXp = u.domain.cellsize.x(2:end-1);
rp = u.domain.cellcenters.x;
rf = u.domain.facecenters.x;

% define the vectors to store the sparse matrix data
iix = zeros(3*(Nr+2),1);
jjx = zeros(3*(Nr+2),1);
sx = zeros(3*(Nr+2),1);

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

% reassign the east, west, north, and south velocity vectors for the
% code readability
ue = ux(2:Nr+1);		uw = ux(1:Nr);
re = rf(2:Nr+1);     rw = rf(1:Nr);

% find the velocity direction for the upwind scheme
ue_min = min(ue,0);	ue_max = max(ue,0);
uw_min = min(uw,0);	uw_max = max(uw,0);

% calculate the coefficients for the internal cells
AE = reshape(re.*ue_min./(DXp.*rp),Nr,1);
AW = reshape(-rw.*uw_max./(DXp.*rp),Nr,1);
APx = reshape((re.*ue_max-rw.*uw_min)./(DXp.*rp),Nr,1);

% correct for the cells next to the boundary
% Left boundary:
APx(1) = APx(1)-rw(1)*uw_max(1)/(2*rp(1)*DXp(1));   AW(1) = AW(1)/2;
% Right boundary:
AE(end) = AE(end)/2;    APx(end) = APx(end)+re(end)*ue_min(end)/(2*rp(end)*DXp(end));

% build the sparse matrix based on the numbering system
rowx_index = reshape(G(2:Nr+1),Nr,1); % main diagonal x
iix(1:3*Nr) = repmat(rowx_index,3,1);
jjx(1:3*Nr) = [reshape(G(1:Nr),Nr,1); reshape(G(2:Nr+1),Nr,1); reshape(G(3:Nr+2),Nr,1)];
sx(1:3*Nr) = [AW; APx; AE];

% build the sparse matrix
kx = 3*Nr;
M = sparse(iix(1:kx), jjx(1:kx), sx(1:kx), Nr+2, Nr+2);


close all;
clear all;
clc;
rng('default');

png_export = true;
pdf_export = false;

load ('bin/figure_1_spherical_dict_model_1bp_success_with_k.mat');

mf = spx.graphics.Figures();

mf.new_figure('Recovery probability with K for BP-MMV');
hold all;
legends = cell(1, num_ss);
for ns=1:num_ss
    S = Ss(ns);
    plot(Ks, bp_success_with_k(ns, :));
    legends{ns} = sprintf('S=%d', S);
end
grid on;
xlabel('Sparsity Level');
ylabel('Empirical Recovery Rate');
legend(legends);


function [err_p,elerr_p] = cdpost_bc(viscosity,aez,fez,elerror,xy,ev,ebound)
%cdpost_bc postprocesses local Poisson error estimator
%   [err_p,elerr_p] = cdpost_bc(viscosity,aez,fez,elerror,xy,ev,ebound);
%   input
%          viscosity   viscosity parameter
%          aez         elementwise Poisson problem matrices
%          fez         elementwise rhs vectors
%          elerror     elementwise error estimate (without BC imposition)
%          xy          vertex coordinate vector
%          ev          element mapping matrix
%          ebound      element edge boundary matrix
%   output
%          err_p       global error estimate
%          elerr_p     elementwise error estimate
%   IFISS function: DJS; 5 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
      x=xy(:,1); y=xy(:,2);
      nel=length(ev(:,1));
      lev=[ev,ev(:,1)]; elerr_p=elerror;
%
% recompute contributions from elements with Dirichlet boundaries
      nbde=length(ebound(:,1));
      ebdy = zeros(nel,1);
      edge = zeros(nel,1);
% isolate boundary elements
      for el = 1:nbde
      ee = ebound(el,1);
      ebdy(ee) = ebdy(ee)+1; edge(ee)=ebound(el,2);
      end
%
% two edge elements
      k2=find(ebdy==2);
      nel2b=length(k2);
% loop over two edge elements
      for el = 1:nel2b
      el2e=k2(el);
      kk=find(ebound(:,1) == el2e);
      edges=ebound(kk,2);
% set up original matrix and RHS vector
	  ae=squeeze(aez(el2e,1:5,1:5));
      fe=fez(el2e,:)';
% set up local coordinates and impose interpolated error as Dirichlet bc
      xl=x(lev(el2e,:)); yl=y(lev(el2e,:));
      [bae,fe] = localbc_pcd(ae,fe,edges,xl,yl);
% solve local problem
        err=(viscosity*bae)\fe;
        elerr_p(el2e,1) = err'*fe/viscosity;
      end
% end of element loop
%
% one edge elements
      k1=find(ebdy==1);
      nel1b=length(k1);
% loop over one edge elements
      for el = 1:nel1b
      el1e=k1(el);
      kk=find(ebound(:,1) == el1e);
      edges=ebound(kk,2);
% set up original matrix and RHS vector
      fe=fez(el1e,:)';
	  ae=squeeze(aez(el1e,1:5,1:5));
% set up local coordinates and impose interpolated error as Dirichlet bc
      xl=x(lev(el1e,:)); yl=y(lev(el1e,:));
      [bae,fe] = localbc_pcd(ae,fe,edges,xl,yl);
% solve for local estimate
        err=(viscosity*bae)\fe;
        elerr_p(el1e,1) = err'*fe/viscosity;
      end
% end of element loop
%
      err_p = sqrt(sum(elerr_p));
      elerr_p = sqrt(elerr_p);
      fprintf('done\n')
      fprintf('estimated global error (in energy):  %10.6e\n',err_p)
 return


%%******************************************************************
%% geometric_mean: an example with rotated cone variables
%%
%%  max {prod(d+B*x) : d+B*x > 0, x <= 10}
%%
%%  where B = 4xn matrix,
%%        d = 4x1 vector
%%
%% [blk,At,C,b,xx] = geometric_mean(B,d,solve);
%%
%% E.g. p = 6; m = 4; B = [rand(2,p); -rand(2,p)]; d = rand(m,1);
%%
%% SDPT3: version 3.0
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last modified: 2 Feb 01
%%******************************************************************

   function [blk,At,C,b,xx] = geometric_mean(B,d,solve);

   if (nargin == 2); solve = 0; end

   n = size(B,2);
   zz = zeros(1,n);
   r2 = sqrt(2);
   blk{1,1} = 'r'; blk{1,2} = [3,3,3];
   At{1,1} = -[B(1,:),0,0,0; B(2,:),0,0,0; zz,r2,0,0; ...
               B(3,:),0,0,0; B(4,:),0,0,0; zz,0,r2,0; ...
	       zz, 1,0,0; zz, 0,1,0; zz, 0,0,r2];
   C{1,1} = [d(1:2); 0; d(3:4); 0; 0;0;0];
   b = [zeros(n,1); 0;0;1];
   blk{2,1} = 'l'; blk{2,2} = n;
   At{2,1} = [eye(n), zeros(n,3)]; C{2,1} = 10*ones(n,1);
   if (solve)
      [bblk,AAt,CC,bb,T] = convertRcone(blk,At,C,b);
      [obj,X,y,Z] = sqlp(bblk,AAt,CC,bb);
      xx = y(1:n);
   end
%%******************************************************************


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

%*****************************************************************************80
%
%% R8MAT_ORTH_UNIFORM returns a random orthogonal matrix.
%
%  Properties:
%
%    The inverse of A is equal to A'.
%
%    A * A'  = A' * A = I.
%
%    Columns and rows of A have unit Euclidean norm.
%
%    Distinct pairs of columns of A are orthogonal.
%
%    Distinct pairs of rows of A are orthogonal.
%
%    The L2 vector norm of A*x = the L2 vector norm of x for any vector x.
%
%    The L2 matrix norm of A*B = the L2 matrix norm of B for any matrix B.
%
%    The determinant of A is +1 or -1.
%
%    All the eigenvalues of A have modulus 1.
%
%    All singular values of A are 1.
%
%    All entries of A are between -1 and 1.
%
%  Discussion:
%
%    Thanks to Eugene Petrov, B I Stepanov Institute of Physics,
%    National Academy of Sciences of Belarus, for convincingly
%    pointing out the severe deficiencies of an earlier version of
%    this routine.
%
%    Essentially, the computation involves saving the Q factor of the
%    QR factorization of a matrix whose entries are normally distributed.
%    However, it is only necessary to generate this matrix a column at
%    a time, since it can be shown that when it comes time to annihilate
%    the subdiagonal elements of column K, these (transformed) elements of
%    column K are still normally distributed random values.  Hence, there
%    is no need to generate them at the beginning of the process and
%    transform them K-1 times.
%
%    For computational efficiency, the individual Householder transformations
%    could be saved, as recommended in the reference, instead of being
%    accumulated into an explicit matrix format.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Pete Stewart,
%    Efficient Generation of Random Orthogonal Matrices With an Application
%    to Condition Estimators,
%    SIAM Journal on Numerical Analysis,
%    Volume 17, Number 3, June 1980, pages 403-409.
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real A(N,N), the orthogonal matrix.
%
%    Output, integer SEED, a seed for the random number generator.
%
  a = zeros ( n, n );
%
%  Start with A = the identity matrix.
%
  for i = 1 : n
    for j = 1 : n
      if ( i == j )
        a(i,j) = 1.0;
      else
        a(i,j) = 0.0;
      end
    end
  end
%
%  Now behave as though we were computing the QR factorization of
%  some other random matrix.  Generate the N elements of the first column,
%  compute the Householder matrix H1 that annihilates the subdiagonal elements,
%  and set A := A * H1' = A * H.
%
%  On the second step, generate the lower N-1 elements of the second column,
%  compute the Householder matrix H2 that annihilates them,
%  and set A := A * H2' = A * H2 = H1 * H2.
%
%  On the N-1 step, generate the lower 2 elements of column N-1,
%  compute the Householder matrix HN-1 that annihilates them, and
%  and set A := A * H(N-1)' = A * H(N-1) = H1 * H2 * ... * H(N-1).
%  This is our random orthogonal matrix.
%
  for j = 1 : n-1
%
%  Set the vector that represents the J-th column to be annihilated.
%
    x(1:j-1) = 0.0;

    for i = j : n
      [ x(i), seed ] = r8_normal_01 ( seed );
    end
%
%  Compute the vector V that defines a Householder transformation matrix
%  H(V) that annihilates the subdiagonal elements of X.
%
    v = r8vec_house_column ( n, x, j );
%
%  Postmultiply the matrix A by H'(V) = H(V).
%
    a = r8mat_house_axh ( n, a, v );

  end

  return
end


function i4_factorial2_values_test ( )

%*****************************************************************************80
%
%% I4_FACTORIAL2_VALUES_TEST demonstrates the use of I4_FACTORIAL2_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 February 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'I4_FACTORIAL2_VALUES_TEST:\n' );
  fprintf ( 1, '  I4_FACTORIAL2_VALUES returns values of\n' );
  fprintf ( 1, '  the double factorial function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     N         N!!\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, n, fn ] = i4_factorial2_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %4d  %10d\n',  n, fn );

  end

  return
end


function [b,a]=v_potsband(fs)
%V_POTSBAND Design filter for 300-3400 telephone bandwidth [B,A]=(FS)
%
%Input: FS=sample frequency in Hz
%
%Output: B/A is a discrete time bandpass filter with a passband gain of 1
%
%The filter meets the specifications of G.151 for any sample frequency
%and has a gain of -3dB at the passband edges.

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: v_potsband.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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

szp=[0.19892796195357i; -0.48623571568937+0.86535995266875i];
szp=[[0; -0.97247143137874] szp conj(szp)];
% s-plane zeros and poles of high pass 3'rd order chebychev2 filter with -3dB at w=1
zl=2./(1-szp*tan(300*pi/fs))-1;
al=real(poly(zl(2,:)));
bl=real(poly(zl(1,:)));
sw=[1;-1;1;-1];
bl=bl*(al*sw)/(bl*sw);
zh=2./(szp/tan(3400*pi/fs)-1)+1;
ah=real(poly(zh(2,:)));
bh=real(poly(zh(1,:)));
bh=bh*sum(ah)/sum(bh);
b=conv(bh,bl);
a=conv(ah,al);


function [psnr, ssim] = compute_difference(im, imgt, SRF)
% COMPUTE_DIFFERENCE: compute image quality
%
% Input:
%     - im:   super-resolved image
%     - imgt: groundtruth high-resolution image
%     - SRF:  super-resolution factor
% Output:
%     - psnr: Peak signal-to-noise ratio
%     - ssim: Structural similarity index
%     - ifc:  Information fidelity criterion

% =========================================================================
% Retrieve only luminance channel
% =========================================================================
if size(im,3)>1
    im = rgb2ycbcr(im);
    im = im(:,:,1);
end

if size(imgt,3)>1
    imgt = rgb2ycbcr(imgt);
    imgt = imgt(:,:,1);
end

% =========================================================================
% Remove border pixels as some methods (e.g., A+) do not predict border pixels
% =========================================================================
cropPix     = SRF;
im          = shave(im, [cropPix, cropPix]);
imgt        = shave(imgt, [cropPix, cropPix]);

% Convert to double (with dynamic range 255)
im          = double(im);
imgt        = double(imgt);

% =========================================================================
% Compute Peak signal-to-noise ratio (PSNR)
% =========================================================================
mse = mean(mean((im - imgt).^2, 1), 2);
psnr = 10*log10(255*255/mse);

% =========================================================================
% Compute Structural similarity index (SSIM index)
% =========================================================================
[ssim, ~] = ssim_index(imgt, im);

% =========================================================================
% Compute information fidelity criterion (IFC)
% =========================================================================
% ifc = ifcvec(imgt, im);

end

function I = shave(I, border)
I = I(1+border(1):end-border(1), ...
      1+border(2):end-border(2), :, :);
end


function set_tick_timestamps(axes, use_milliseconds)

%% Find nice round numbers to use
number_of_ticks = 6;
x_lim = xlim(axes);
xrange = x_lim(2) - x_lim(1);
xrange = xrange / number_of_ticks;
nice = floor(log10(xrange));
frac = 1;
if nice > 1
    frac = 4;
end
nice = round(xrange * frac / 10^nice) / frac * 10^nice;

x_tick = floor(x_lim(1)):nice:ceil(x_lim(2));

x_tick_minutes = floor(x_tick / 60);
x_tick_seconds = mod(x_tick,60);

if use_milliseconds
        labels = compose('%.0f:%05.2f', x_tick_minutes', x_tick_seconds');
else
        labels = compose('%.0f:%04.1f', x_tick_minutes', x_tick_seconds');
end

x_tick=x_tick(1,2:end-1);
labels=labels(2:end-1,1);

xticks(axes, x_tick)
xticklabels(axes,labels);
end


%#eml
function R = radix4FFT3_FixPtEML(s)
    % This is a radix-4 FFT, using decimation in frequency
    % The input signal can be floating point or fixed point
    % Works with real or complex input
    % Modified for Embedded MATLAB support

    % Initialize variables and signals
    % NOTE: The length of the input signal should be a power of 4: 4, 16, 64, 256, etc.
    N = length(s);
    M = log2(N)/2;

    % Initialize variables for floating or fixed point sim
    if isfi(s)
        NT = numerictype(s);
        FM = fimath(s);
        wl = NT.WordLength;
        W=fi(exp(-1*2j*pi*(0:N-1)/N), 1, wl, wl - 2, FM);
        S = fi(complex(zeros(size(s))), NT, FM);
        R = fi(complex(zeros(size(s))), 1, wl, wl  - 1 - 2*M, FM);
        sTemp = fi(complex(zeros(size(s))), NT, FM);
        x = fi(complex(zeros(1,4)), NT, FM);
    else
        W=exp(-1*2j*pi*(0:N-1)/N);
        S = complex(zeros(size(s)));
        R = complex(zeros(size(s)));
        sTemp = complex(zeros(size(s)));
        x = complex(zeros(1,4));
    end

    % FFT algorithm
    % Calculate butterflies for first M-1 stages
    sTemp = s;
    for stage = 0:M-2
        for n=1:N/4
            for m=1:4
                x(m) = sTemp(n + (m-1)*N/4);
            end
            S((1:4)+(n-1)*4) = radix4bfly(x, floor((n-1)/(4^stage)) *(4^stage), 1, W);
        end
        sTemp = S;
    end

    % Calculate butterflies for last stage
    for n=1:N/4
        for m=1:4
            x(m) = sTemp(n + (m-1)*N/4);
        end
        S((1:4)+(n-1)*4) = radix4bfly(x, floor((n-1)/(4^stage)) * (4^stage), 0, W);
    end

    % Rescale the final output
    R(:) = S*N;

end

function Z = radix4bfly(x,segment,stageFlag,W)
    % For the last stage of a radix-4 FFT all the ABCD multiplers are 1.
    % Use the stageFlag variable to indicate the last stage
    % stageFlag = 0 indicates last FFT stage, set to 1 otherwise

    % Initialize variables and scale by 1/4
    if isfi(x)
        a=bitshift(x(1),-2);b=bitshift(x(2),-2);c=bitshift(x(3),-2);d=bitshift(x(4),-2);
    else
        a=x(1)*.25;b=x(2)*.25;c=x(3)*.25;d=x(4)*.25;
    end

    % Radix-4 Algorithm
    A=a+b+c+d;
    B=(a-b+c-d)*W(2*segment*stageFlag + 1);
    C=(a-complex(-imag(b),real(b))-c+complex(-imag(d),real(d)))*W(segment*stageFlag + 1);
    D=(a+complex(-imag(b),real(b))-c-complex(-imag(d),real(d)))*W(3*segment*stageFlag + 1);

    Z = [A B C D];

end


function [xUpdate,PUpdate,innov,Pzz,W]=KalmanUpdate(xPred,PPred,z,R,H,condIssue)
%KALMANUPDATE Perform the measurement update step in the standard linear
%             Kalman filter. This assumes that the predicted target state
%             and the measurement are both multivariate Gaussian
%             distributed.
%
%INPUTS: xPred The xDimX1 predicted target state.
%        PPred The xDimXxDim predicted state covariance matrix.
%            z The zDimX1 measurement vector.
%            R The zDimXzDim measurement covariance matrix.
%            H The optional zDimXxDim measurement matrix. The measurement
%              is modeled as z=H*x+noise. If this parameter is omitted or
%              an empty matrix is passed, then H will be taken as a
%              zDimXzDim identity matrix followed by columns of zeros
%              (Assuming that zDim<=xDim. Otherwise, H must be provided).
%    condIssue An optional parameter indicating whether a conditioning
%              issue with the measurement covariance matrix. Singularity
%              can occur if two elements are perfectly correlated. This
%              replaces the matrix inverse with a pseudoinverse.
%
%OUTPUTS: xUpdate The xDim X 1 updated target state vector.
%         PUpdate The updated xDim X xDim state covariance matrix.
%      innov, Pzz The zDimX1 innovation and the zDimXzDim innovation
%                 covariance matrix are returned in case one wishes to
%                 analyze the consistency of the estimator or use those
%                 values in gating or likelihood evaluation.
%               W The xDimXzDim gain used in the update. This can be
%                 useful when gating and using the function
%                 calcMissedGateCov.
%
%Given a prediction of the target state xPred with an associated covariance
%matrix PPred, and assuming that they are the mean and covariance matrix of
%a Gaussian distribution, under the measurement model
%z=H*x+w
%where z is the measurement, x is the true target state, H is a zDimXxDim
%matrix and w is additive Gaussian noise, this function updates the state
%estimate and its associated covariance estimate.
%
%The Joseph-form covariance update is used for improved numerical
%stability. The algorithm is derived in Chapter 5 of [1].
%
%EXAMPLE:
%This example simulates a linear dynamic process that extracts Cartesian
%measurements. it then looks at the measurement error reduction factor
%(MERF), and the normalized estimation error squared (NEES). The MERF
%should be less than 1 or else there is no point in using the filter-
%connecting the dots works better. The NEES should be close to 1 to
%indicate covariance consistency.
% numMonteCarlo=500;
% numSteps=50;
% %Power spectral density of the process noise.
% q0=processNoiseSuggest('PolyKal-ROT',2*9.8,1);
% xDim=4;%2D position and velocity.
% T=1;%prediction interval.
% F=FPolyKal(T,xDim,1);
% Q=QPolyKal(T,xDim,1,q0);
% SQ=chol(Q,'lower');
% R=[50^2,-1000;
%    -1000,50^2];
% SR=chol(R,'lower');
% H=[eye(2,2),zeros(2,2)];
% zDim=size(H,1);
%
% z=zeros(zDim,numSteps,numMonteCarlo);
% xTrue=zeros(xDim,numSteps,numMonteCarlo);
% %The initial state.
% xTrue(:,1,:)=repmat([1e3;0;75;-50],[1,1,numMonteCarlo]);
% maxVel=400;%Used for single-point initiation.
% higherDerivStdDev=maxVel/sqrt(2);%used for single-point initiation.
%
% xEst=zeros(xDim,numSteps,numMonteCarlo);
% PEst=zeros(xDim,xDim,numSteps,numMonteCarlo);
% for curRun=1:numMonteCarlo
%     z(:,1,curRun)=H*xTrue(:,1,curRun)+SR*randn(zDim,1);
%     for curStep=2:numSteps
%         xTrue(:,curStep,curRun)=F*xTrue(:,curStep-1,curRun)+SQ*randn(xDim,1);
%         z(:,curStep,curRun)=H*xTrue(:,curStep,curRun)+SR*randn(zDim,1);
%     end
%
%     %One-point initialization. This works in this instance, because H
%     %extracts the Cartesian components.
%     [xInit,PInit]=onePointCartInit(z(:,1,curRun),R,higherDerivStdDev,1);
%     xEst(:,1,curRun)=xInit;
%     PEst(:,:,1,curRun)=PInit;
%     for curStep=2:numSteps
%         [xPred,PPred]=discKalPred(xEst(:,curStep-1,curRun,1),PEst(:,:,curStep-1,curRun,1),F,Q);
%         [xUpdate,PUpdate]=KalmanUpdate(xPred,PPred,z(:,curStep,curRun),R,H);
%         xEst(:,curStep,curRun)=xUpdate;
%         PEst(:,:,curStep,curRun)=PUpdate;
%     end
% end
%
% posTrue=xTrue(1:2,:,:);
% %The MERF requires the measurement to be in the same coordinates are the
% %corresponding element sof the state.
% MERF=calcMERF(posTrue,z,xEst(1:2,:,:),0,true);
% NEES=calcNEES(xTrue,xEst,PEst);
% figure(1)
% clf
% hold on
% plot(MERF,'-k','linewidth',2)
% h1=xlabel('step');
% h2=ylabel('Measurement Error Reduction Factor');
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
%
% figure(2)
% clf
% hold on
% plot(NEES,'-r','linewidth',2)
% h1=xlabel('step');
% h2=ylabel('NEES');
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
%
%REFERENCES:
%[1] Y. Bar-Shalom, X. R. Li, and T. Kirubarajan, Estimation with
%    Applications to Tracking and Navigation. New York: John Wiley and
%    Sons, Inc, 2001.
%
%July 2012 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

xDim=size(xPred,1);

if(nargin<6||isempty(condIssue))
    condIssue=false;
end

if(nargin<5||isempty(H))
	zDim=size(z,1);
    H=[eye(zDim,zDim),zeros(zDim,xDim-zDim)];
end

zPred=H*xPred;
innov=z-zPred;

Pzz=R+H*PPred*H';
%Ensure symmetry
Pzz=(Pzz+Pzz')/2;

if(condIssue==false)
    opts.SYM=true;
    opts.RECT=false;
    opts.TRANSA=false;
    W=linsolve(Pzz,H*PPred,opts)';%W=PPred*H'/Pzz;
else
    %If there is possibly poor conditioning.
    W=PPred*H'*pinv(Pzz);
end

xUpdate=xPred+W*innov;

temp=W*H;
temp=eye(xDim,xDim)-temp;
PUpdate=temp*PPred*temp'+W*R*W';

%Ensure symmetry
PUpdate=(PUpdate+PUpdate')/2;
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function idxSelection = uniformSampling(X, voxelSize)

% Preparations -----------------------------------------------------------------

% No. of points
noPoi = size(X,1);

% Logical indices for selection of points (true = selected)
idxSelection = false(noPoi,1);

% Find voxel centers -----------------------------------------------------------

% Point with smallest coordinates
minPoi = min(X, [], 1);

% Rounded local origin for voxel structure (voxels of different pcs have coincident voxel centers if mod(100, voxelSize) == 0)
localOrigin = (floor(minPoi/100))*100;

% Find 3-dimensional indices of voxels in which points are lying
idxVoxel = [floor( (X(:,1)-localOrigin(1)) / voxelSize ) ...
            floor( (X(:,2)-localOrigin(2)) / voxelSize ) ...
            floor( (X(:,3)-localOrigin(3)) / voxelSize )];

% Remove multiple voxels
[idxVoxelUnique, ~, ic] = unique(idxVoxel, 'rows'); % ic contains "voxel index" for each point

% Coordinates of voxel centers
XVoxelCenter = [localOrigin(1) + voxelSize/2 + idxVoxelUnique(:,1)*voxelSize ...
                localOrigin(2) + voxelSize/2 + idxVoxelUnique(:,2)*voxelSize ...
                localOrigin(3) + voxelSize/2 + idxVoxelUnique(:,3)*voxelSize];

% No. of voxel (equal to no. of selected points)
noVoxel = size(XVoxelCenter,1);

% Select points nearest to voxel centers ---------------------------------------

% Sort indices and points (in order to find points inside of voxels very fast in the next loop)
[ic, idxSort] = sort(ic);
X = X(idxSort,:);

idxJump = find(diff(ic));

% Example (3 voxel)
% ic         = [1 1 1 2 2 2 3]';
% diff(ic)   = [ 0 0 1 0 0 1 ]';
% idxJump    = [     3     6 ]';
%
% idxInVoxel = [1 2 3]; for voxel 1
% idxInVoxel = [4 5 6]; for voxel 2
% idxInVoxel = [7    ]; for voxel 3

for i = 1:noVoxel

    % Find indices of points inside of voxel (very, very fast this way)
    if i == 1
        idxInVoxel = [1:idxJump(i)];
    elseif i == noVoxel
        idxInVoxel = [idxJump(i-1)+1:noPoi];
    else
        idxInVoxel = [idxJump(i-1)+1:idxJump(i)];
    end

    % Distance of points to voxel center
    dist2voxelCenter = sqrt((X(idxInVoxel,1)-XVoxelCenter(i,1)).^2 + (X(idxInVoxel,2)-XVoxelCenter(i,2)).^2 + (X(idxInVoxel,3)-XVoxelCenter(i,3)).^2);

    % Find index of point with smallest distance to voxel center
    [~, idxSelectedPoi] = min(dist2voxelCenter);

    % Select point
    idxSelection(idxSort(idxInVoxel(idxSelectedPoi))) = true;

    % 4Debug
    % idxInVoxel
    % idxSelectedPoi
    % XVoxelCenter(i,:)
    % X(idxInVoxel,:)
    % dist2voxelCenter
    % maxDist = sqrt(3*(voxelSize/2)^2);
    % correct(i) = all(dist2voxelCenter <= maxDist);

end

end


function [ vX, mX ] = SolveLsL2Prox( mA, vB, paramLambda, numIterations )
% ----------------------------------------------------------------------------------------------- %
%[ vX, mX ] = SolveLsL1Prox( mA, vB, lambdaFctr, numIterations )
% Solve L2 Regularized Least Squares Using Proximal Gradient (PGM) Method.
% Input:
%   - mA                -   Input Matirx.
%                           The model matrix.
%                           Structure: Matrix (m X n).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - vB                -   input Vector.
%                           The model known data.
%                           Structure: Vector (m X 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - paramLambda       -   Parameter Lambda.
%                           The L1 Regularization parameter.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: (0, inf).
%   - numIterations     -   Number of Iterations.
%                           Number of iterations of the algorithm.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range {1, 2, ...}.
% Output:
%   - vX                -   Output Vector.
%                           Structure: Vector (n X 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
% References
%   1.  Wikipedia PGM - https://en.wikipedia.org/wiki/Proximal_gradient_method.
% Remarks:
%   1.  Using vanilla PGM.
% Known Issues:
%   1.  A
% TODO:
%   1.  B
% Release Notes:
%   -   1.0.000     23/08/2017
%       *   First realease version.
% ----------------------------------------------------------------------------------------------- %

mAA = mA.' * mA;
vAb = mA.' * vB;
vX  = pinv(mA) * vB; %<! Dealing with "Fat Matrix"

stepSize = 1 / (2 * (norm(mA, 2) ^ 2));
% stepSize = 1 / sum(mA(:) .^ 2); %<! Faster to calculate, conservative (Hence slower)

mX = zeros([size(vX, 1), numIterations]);
mX(:, 1) = vX;

for ii = 2:numIterations

    vG = (mAA * vX) - vAb;
    vX = ProxL2(vX - (stepSize * vG), stepSize * paramLambda);

    mX(:, ii) = vX;

end

end

function [ vX ] = ProxL2( vX, lambdaFactor )

% Soft Thresholding L2
% See https://math.stackexchange.com/a/2094088/33
vX = (1 - (lambdaFactor / max(norm(vX, 2), lambdaFactor))) * vX;

end


function [ result, node_num ] = sphere01_triangle_quad_icos2v ( a_xyz, ...
  b_xyz, c_xyz, factor, fun )

%*****************************************************************************80
%
%% SPHERE01_TRIANGLE_QUAD_ICOS2V: vertex rule, subdivide then project.
%
%  Discussion:
%
%    This function estimates an integral over a spherical triangle on the
%    unit sphere.
%
%    This function sets up an icosahedral grid, and subdivides each
%    edge of the icosahedron into FACTOR subedges.  These edges define a grid
%    within each triangular icosahedral face.   All of these calculations are
%    done, essentially, on the FLAT faces of the icosahedron.  Only then are
%    the triangle vertices projected to the sphere.
%
%    This function uses a more sophisticated projection scheme than
%    SPHERE01_TRIANGLE_QUAD_ICOS1V, but this does not seem to improve
%    the results significantly.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 April 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A_XYZ(3), B_XYZ(3), C_XYZ(3), the vertices
%    of the spherical triangle.
%
%    Input, integer FACTOR, the subdivision factor, which must
%    be at least 1.
%
%    Input, function v = FUN ( x ), evaluates the integrand at the point X.
%
%    Output, real RESULT, the estimated integral.
%
%    Output, integer NODE_NUM, the number of evaluation points.
%

%
%  Initialize the integral data.
%
  result = 0.0;
  area_total = 0.0;
  node_num = 0;
%
%  Deal with subtriangles that have same orientation as face.
%
  for f1 = 0 : factor - 1
    for f2 = 0 : factor - f1 - 1
      f3 = factor - f1 - f2;

      a2_xyz = sphere01_triangle_project2 ( a_xyz, b_xyz, c_xyz, f1 + 1, f2,     f3 - 1 );
      b2_xyz = sphere01_triangle_project2 ( a_xyz, b_xyz, c_xyz, f1,     f2 + 1, f3 - 1 );
      c2_xyz = sphere01_triangle_project2 ( a_xyz, b_xyz, c_xyz, f1,     f2,     f3 );

      area = sphere01_triangle_vertices_to_area ( a2_xyz, b2_xyz, c2_xyz );

      node_num = node_num + 3;
      va = fun ( a2_xyz );
      vb = fun ( b2_xyz );
      vc = fun ( c2_xyz );
      result = result + area * ( va + vb + vc ) / 3.0;
      area_total = area_total + area;

      end
    end
%
%  Deal with subtriangles that have opposite orientation as face.
%
  for f3 = 0 : factor - 2
    for f2 = 1 : factor - f3 - 1
      f1 = factor - f2 - f3;

      a2_xyz = sphere01_triangle_project2 ( a_xyz, b_xyz, c_xyz, f1 - 1, f2,     f3 + 1 );
      b2_xyz = sphere01_triangle_project2 ( a_xyz, b_xyz, c_xyz, f1,     f2 - 1, f3 + 1 );
      c2_xyz = sphere01_triangle_project2 ( a_xyz, b_xyz, c_xyz, f1,     f2,     f3 );

      area = sphere01_triangle_vertices_to_area ( a2_xyz, b2_xyz, c2_xyz );

      node_num = node_num + 3;
      va = fun ( a2_xyz );
      vb = fun ( b2_xyz );
      vc = fun ( c2_xyz );
      result = result + area * ( va + vb + vc ) / 3.0;
      area_total = area_total + area;

    end
  end

  return
end


%mg_diff   GMG preconditioner for diffusion problem
%IFISS scriptfile: AR, HCE, DJS; 15 April 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage

nc=log2(length(y)-1);
%
% compute new MG data or reload existing data?
compute_mg = default('compute / load MG data? 1/2 (default 1)',1);
if compute_mg==2
   load mgdata_diff
else
   h=2^(1-nc);
   fprintf('Setting up MG data ...')
   % top level
   mgdata(nc).matrix=Agal;
   if domain==1 % square
      mgdata(nc).prolong=mg_prolong(2^nc,2^nc,x,y);
   elseif domain==2 % L-shaped
      x=x';y=y';
      mgdata(nc).prolong=mg_prolong_ell(nc,x,y);
   elseif domain==3 % step
      x=x';y=y';
      mgdata(nc).prolong=mg_prolong_step(nc,x,y);
   else
      error('Multigrid solver not implemented for this domain')
   end
   xn=x;yn=y;
   % loop over remaining levels
   for level = nc-1:-1:2;
      xn=xn(1:2:end);yn=yn(1:2:end);
      if domain==1 % square
         mgdata(level).matrix=mg_diff_setup(xn,yn);
         mgdata(level).prolong=mg_prolong(2^level,2^level,xn,yn);
      elseif domain==2
         mgdata(level).matrix=mg_diff_setup_ell(xn,yn);
         mgdata(level).prolong=mg_prolong_ell(level,xn,yn);
      else
         mgdata(level).matrix=mg_diff_setup_step(xn,yn);
         mgdata(level).prolong=mg_prolong_step(level,xn,yn);
      end
   end
   fprintf('done\n')
   gohome, cd datafiles, save mgdata_diff mgdata
   if domain~=1, x=x';y=y';, end
end
%
% MG parameters
smooth = default('Jacobi / ILU smoother? 1/3 (default is ILU)',3);
if smooth==3
   % point ILU
   sweeps=1;stype=1;
elseif smooth==2
   % Gauss-Seidel
   stype = default('point / line Gauss-Seidel? 1/2 (default is line)',2);
   if stype==2
      sweeps = default('number of Gauss-Seidel sweeps? 1/2/3/4 (default is 2)',2);
   else
      sweeps=1;
   end
else
   % point Jacobi
   sweeps=1;stype=1;
end
npre = default('number of pre-smoothing steps? (default is 1)',1);
npost = default('number of post-smoothing steps? (default is 1)',1);
%
% construct smoother
if domain==1
   smooth_data = mg_smooth(mgdata,nc,sweeps,smooth,stype);
elseif domain==2
   smooth_data = mg_smooth_ell(mgdata,nc,sweeps,smooth,stype);
else
   smooth_data = mg_smooth_step(mgdata,nc,sweeps,smooth,stype);
end


function [zs] = besselJRobinzeros(c,k,N,freqCenter)

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

xmin = max(freqCenter - pi*N,0);
xmax = freqCenter + 2*pi*N; % Using zn ~ pi*n
if c == Inf
    % Returns the N first zeros of the function
    % Jk(x)
    zs = AllZeros(@(x)(besselj(k,x)),xmin,xmax,5*(N+freqCenter));

else
    % Returns the N first zeros of the function
    % c Jk(x) + xJk'(x)

    switch N
        case 0
            derBess = @(x)(-besselj(1,x));
        otherwise
            derBess = @(x)(0.5*(besselj(k-1,x)-besselj(k+1,x)));
    end

    zs = AllZeros(@(x)(c*besselj(k,x)+x*derBess(x)),xmin,xmax,5*(N+freqCenter));

end

[~,I] = sort(abs(zs-freqCenter));
zs = zs(I);

if zs(1)==0
    zs = zs(2:(N+1));
else
    zs = zs(1:N);
end

zs = zs(:);

end

function z=AllZeros(f,xmin,xmax,N)
% Inputs :
% f : function of one variable
% [xmin - xmax] : range where f is continuous containing zeros
% N : control of the minimum distance (xmax-xmin)/N between two zeros
if (nargin<4)
    N=100;
end
options=optimset('Display','off');
dx=(xmax-xmin)/N;
x2=xmin;
y2=f(x2);
z=[];
for i=1:N
    x1=x2;
    y1=y2;
    x2=xmin+i*dx;
    y2=f(x2);
    if (y1*y2<=0)                              % Rolle's theorem : one zeros (or more) present
        z=[z,fsolve(f,(x2*y1-x1*y2)/(y1-y2),options)]; % Linear approximation to guess the initial value in the [x1,x2] range.
    end
end

end


function [slope, intercept, averslope, averIC] = fdsurfft(im)
% FDSURFFT Compute fractal dimension (slope) of surface image im and draw rose plots
%          of slope and intercept
%     im: input array of surface image (grayvalue or range image)
%     slope: an array of size 24 which stores the average slopes in 24
%            directions
%     intercept: an array of size 24 which stores the average intercepts in 24
%                directions
%     averslope: average slope for all directions
%     averIC: average intercept for all directions

% This is a matlab version of John C. Russ 's program

% Written by Mr. Jianbo Zhang, J.Zhang@ewi.utwente.nl
% 2 Nov, 2004

tic
NUM_DIR = 24; % number of directions that the frequency space is uniformally divided
NUM_RAD = 30; % number of points that the radius is uniformally divided
if nargin < 1,
    error('Missed input argument which must be an array!')
end

[M N] = size(im);
xctr = 1 + bitshift(N, -1); % x coordinate of center point
yctr = 1 + bitshift(M, -1); % y coordinate of center point
imMean = mean(im(:));
fim = fftshift(fft2(double(im) - imMean));

% power spectrum
mag = log(fim .* conj(fim)+ 10 ^ (-6));
sumBrite = zeros(NUM_DIR, NUM_RAD); %accumulation magnitude for each direction and radius
nCount = zeros(NUM_DIR, NUM_RAD); %number of magnitude
radius = zeros(2 * NUM_RAD,1);%accumulation magnitude for all directions
radCount = zeros(2 * NUM_RAD,1);% number of magintude for all directions

%Compute phase image and phase histogram
phaseim = zeros(M,N);
phase = zeros(180);
for j = 1:M
    for i = 1:N
        realv = real(fim(j,i));
        imagv = imag(fim(j,i));
        if realv == 0
            value = pi/2;
        else
            value = atan((imagv / realv));
            phaseim(j, i) = value;
            ang = floor(180 * (pi / 2 + value) / pi);
        end
        if ang < 0
            ang = 0;
        end
        if ang > 179
            ang = 179;
        end
        phase(ang + 1) = phase(ang + 1) + 1;
    end
end
maxphase = max(phase);
figure; imshow(phaseim, []);
title('Phase image');
figure; plot(phase / maxphase);
title('Phase histogram (0...2 \pi)');%
axis off

%accumulation of magnitude for each dirction and radius
rmax = log(min(M,N)/2);% maximum radius
for j = 1:M
    if j ~= yctr
        yval = yctr - j;
        y2 = yval * yval;
        for i = 1:N
            if i ~= xctr
                xval = i - xctr;
                rho = log(sqrt(y2 + xval * xval));
                if rho > 0 & rho <= rmax
                    mval = mag(j,i);
                    temp = yval /xval;
                    theta = atan(temp);
                    if xval < 0
                        theta = theta + pi;
                    end
                    if theta < 0
                        theta = theta + 2 * pi;
                    end

                    ang = floor(NUM_DIR * theta /(2* pi));
                    if ang > NUM_DIR - 1 | ang < 0
                        ang = NUM_DIR - 1;
                    end
                    k = floor(2*NUM_RAD * rho / rmax);
                    h = floor(k/2);
                    if k > 2 * NUM_RAD - 1
                        h = NUM_RAD - 1;
                        k = 2 * NUM_RAD - 1;
                    end

                    if h >= 5
                        sumBrite(ang + 1, h + 1) = sumBrite(ang + 1, h + 1) + mval;
                        nCount(ang + 1, h + 1) = nCount(ang + 1, h + 1) + 1;
                    end
                    if k >= 5
                        radius(k + 1) = radius(k + 1) + mval;
                        radCount(k + 1) = radCount(k + 1) + 1;
                    end
                end
            end
        end
    end
end

%linear regression
for ang = 1:NUM_DIR
    sumx = 0;
    sumy = 0;
    sumx2 = 0;
    sumxy = 0;
    sumn = 0;
    for range = 6:NUM_RAD
        if nCount(ang, range) > 0
            yval = sumBrite(ang, range)/nCount(ang, range);
            xval = (range -1) * rmax / NUM_RAD;
            sumx = sumx + xval;
            sumy = sumy + yval;
            sumx2 = sumx2 + xval * xval;
            sumxy = sumxy + xval * yval;
            sumn = sumn + 1;
        end
    end
    slope(ang) = (sumn * sumxy - sumx * sumy) / (sumn * sumx2 - sumx * sumx);
    intercept(ang) = (sumy - slope(ang) * sumx) / sumn;
end

%compute average slope over all directions and scales
sumn = 0;
for k = 6:(2 * NUM_RAD)
    if radCount(k) > 0
        sumn = sumn + 1;
        yval(sumn) = radius(k) / radCount(k);
        tempr(sumn) = (k -1) * rmax / (2 * NUM_RAD);
    end
end
p = polyfit(tempr,yval,1);
averslope = p(1);
averIC = p(2);

fitln = polyval(p,tempr);
figure; plot(tempr,yval,tempr,fitln,'r-');
title('Log Log plot of Magn. vs Freq.');
ylabel('Log Magnitude');
xlabel('Log Frequency');

slope(NUM_DIR + 1) = slope(1);
intercept(NUM_DIR + 1) = intercept(1);

%draw rose plot of slope and intercept
ang = 1: (NUM_DIR + 1);
figure;
polar(pi/NUM_DIR + (ang -1) * 2* pi / NUM_DIR, intercept(ang), 'r-');
title('Rose plot of intercept');
figure;
polar(pi/NUM_DIR + (ang - 1)* 2 * pi / NUM_DIR, abs(slope(ang)), '-');
title('Rose plot of slope');
disp(['Elapsed time: ' num2str(toc)]);


function [flag, u, v, t] = rayTriangleIntersection (o, d, p0, p1, p2)
% Ray/triangle intersection using the algorithm proposed by Möller and Trumbore (1997).
%
% Input:
%    o : origin.
%    d : direction.
%    p0, p1, p2: vertices of the triangle.
% Output:
%    flag: (0) Reject, (1) Intersect.
%    u,v: barycentric coordinates.
%    t: distance from the ray origin.
% Author:
%    Jesus Mena

    epsilon = 0.00001;

    e1 = p1-p0;
    e2 = p2-p0;
    q  = cross(d,e2);
    a  = dot(e1,q); % determinant of the matrix M

    if (a>-epsilon && a<epsilon)
        % the vector is parallel to the plane (the intersection is at infinity)
        [flag, u, v, t] = deal(0,0,0,0);
        return;
    end;

    f = 1/a;
    s = o-p0;
    u = f*dot(s,q);

    if (u<0.0)
        % the intersection is outside of the triangle
        [flag, u, v, t] = deal(0,0,0,0);
        return;
    end;

    r = cross(s,e1);
    v = f*dot(d,r);

    if (v<0.0 || u+v>1.0)
        % the intersection is outside of the triangle
        [flag, u, v, t] = deal(0,0,0,0);
        return;
    end;

    t = f*dot(e2,r); % verified!
    flag = 1;
    return;
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Author: Eugenio Alcala Baselga
% Date: 02/06/2018
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

classdef automatic_dynamic_control
    %...
    properties (Constant)
        V_vec       = [2 18];
        Steer_vec   = [deg2rad(-25) deg2rad(25)];
        Alpha_vec   = [-0.1 0.1];

        sigma_vec   = [deg2rad(-25)+deg2rad(30) deg2rad(25)+deg2rad(30)];

        steer_min   = -deg2rad(25);
        steer_max   = deg2rad(25);
        sigma_min   = -deg2rad(25) + deg2rad(30);
        sigma_max   = deg2rad(25) + deg2rad(30);

%         Force_max   = 3500;
%         Force_min   = 0;

        Ts          = 0.001;
        gamma       = 0.001;

        Q       = [ 0.05 0 0 0 0 0 0;         %v
                    0 0.01 0 0 0 0 0;         %alpha
                    0 0 0.01 0 0 0 0;         %w
                    0 0 0 0.01 0 0 0;         %Fxr
                    0 0 0 0 10000 0 0;       %delta
                    0 0 0 0 0 90000 0;        %w integral
                    0 0 0 0 0    0 0];       %v integral
        R       = [0.01  0;
                   0    10];

    end

    methods
    end

end


function w = dwt(x, J, af)

% Discrete 1-D Wavelet Transform
%
% USAGE:
%    w = dwt(x, J, af)
% INPUT:
%    x - N-point vector, where
%            1) N is divisible by 2^J
%            2) N >= 2^(J-1)*length(af)
%    J - number of stages
%    af - analysis filters
%    af(:, 1) - lowpass filter (even length)
%    af(:, 2) - highpass filter (evenlength)
% OUTPUT:
%    w{j}, j = 1..J+1 - DWT coefficients
% EXAMPLE:
%    [af, sf] = farras;
%    x = rand(1,64);
%    w = dwt(x,3,af);
%    y = idwt(w,3,sf);
%    err = x - y;
%    max(abs(err))
%
% WAVELET SOFTWARE AT POLYTECHNIC UNIVERSITY, BROOKLYN, NY
% http://taco.poly.edu/WaveletSoftware/

for k = 1:J
    [x w{k}] = afb(x, af);
end
w{J+1} = x;


function x = dipole_cdf_inv ( cdf, a, b )

%*****************************************************************************80
%
%% DIPOLE_CDF_INV inverts the Dipole CDF.
%
%  Discussion:
%
%    A simple bisection method is used.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real CDF, the value of the CDF.
%
%    Input, real A, B, the parameters of the PDF.
%    -1.0 <= B <= 1.0.
%
%    Output, real X, the corresponding argument of the CDF.
%
  it_max = 100;
  tol = 0.0001;
%
%  Take care of horrible input.
%
  if ( cdf <= 0.0 )
    x = -r8_huge ( )
    return
  elseif ( 1.0 <= cdf )
    x = r8_huge ( )
    return
  end
%
%  Seek X1 < X < X2.
%
  x1 = -1.0;

  while ( 1 )

    cdf1 = dipole_cdf ( x1, a, b );

    if ( cdf1 <= cdf )
      break
    end

    x1 = 2.0 * x1;

  end

  x2 = 1.0;

  while ( 1 )

    cdf2 = dipole_cdf ( x2, a, b );

    if ( cdf <= cdf2 )
      break
    end

    x2 = 2.0 * x2;

  end
%
%  Now use bisection.
%
  it = 0;

  while ( 1 )

    it = it + 1;

    x3 = 0.5 * ( x1 + x2 );
    cdf3 = dipole_cdf ( x3, a, b );

    if ( abs ( cdf3 - cdf ) < tol )
      x = x3;
      break
    end

    if ( it_max < it )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'DIPOLE_CDF_INV - Fatal error!\n' );
      fprintf ( 1, '  Iteration limit exceeded.\n' );
      error ( 'DIPOLE_CDF_INV - Fatal error!' );
    end

    if ( ( cdf3 <= cdf & cdf1 <= cdf ) | ( cdf <= cdf3 & cdf <= cdf1 ) )
      x1 = x3;
      cdf1 = cdf3;
    else
      x2 = x3;
      cdf2 = cdf3;
    end

  end

  return
end


%
% References:
%
% C. Lu. A Library of ADMM for Sparse and Low-rank Optimization. National University of Singapore, June 2016.
% https://github.com/canyilu/LibADMM.
% C. Lu, J. Feng, S. Yan, Z. Lin. A Unified Alternating Direction Method of Multipliers by Majorization
% Minimization. IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 40, pp. 527-541, 2018
%

addpath(genpath(cd))
clear

%% Examples for testing the low-rank tensor models
% For detailed description of the sparse models, please refer to the Manual.

opts.mu = 1e-6;
opts.rho = 1.1;
opts.max_iter = 500;
opts.DEBUG = 1;

%% Tensor RRPCA based on sum of nuclear norm minimization (rpca_snn)
n1 = 50;
n2 = n1;
n3 = n1;
r = 5
L = rand(r,r,r);
U1 = rand(n1,r);
U2 = rand(n2,r);
U3 = rand(n3,r);
L = nmodeproduct(L,U1,1);
L = nmodeproduct(L,U2,2);
L = nmodeproduct(L,U3,3); % low rank part

p = 0.05;
m = p*n1*n2*n3;
temp = rand(n1*n2*n3,1);
[~,I] = sort(temp);
I = I(1:m);
Omega = zeros(n1,n2,n3);
Omega(I) = 1;
E = sign(rand(n1,n2,n3)-0.5);
S = Omega.*E; % sparse part, S = P_Omega(E)

Xn = L+S;

lambda = sqrt([max(n1,n2*n3), max(n2,n1*n3), max(n3,n1*n2)]);
lambda = [1 1 1]
[Lhat,Shat,err,iter] = trpca_snn(Xn,lambda,opts);

err
iter

%% low-rank tensor completion based on sum of nuclear norm minimization (lrtc_snn)
n1 = 50;
n2 = n1;
n3 = n1;
r = 5;
X = rand(r,r,r);
U1 = rand(n1,r);
U2 = rand(n2,r);
U3 = rand(n3,r);
X = nmodeproduct(X,U1,1);
X = nmodeproduct(X,U2,2);
X = nmodeproduct(X,U3,3);
p = 0.5;
omega = find(rand(n1*n2*n3,1)<p);
M = zeros(n1,n2,n3);
M(omega) = X(omega);

lambda = [1 1 1];
[Xhat,err,iter] = lrtc_snn(M,omega,lambda,opts);
err
iter
RSE = norm(X(:)-Xhat(:))/norm(X(:))

%% regularized low-rank tensor completion based on sum of nuclear norm minimization (lrtcR_snn)
n1 = 50;
n2 = n1;
n3 = n1;
r = 5;
X = rand(r,r,r);
U1 = rand(n1,r);
U2 = rand(n2,r);
U3 = rand(n3,r);
X = nmodeproduct(X,U1,1);
X = nmodeproduct(X,U2,2);
X = nmodeproduct(X,U3,3);
p = 0.5;
omega = find(rand(n1*n2*n3,1)<p);
M = zeros(n1,n2,n3);
M(omega) = X(omega);
lambda = [1 1 1];
[Xhat,err,iter] = lrtcR_snn(M,omega,lambda,opts);
err
iter

%% Tensor RRPCA based on tensor nuclear norm minimization (rpca_tnn)
n1 = 50;
n2 = n1;
n3 = n1;
r = 0.1*n1 % tubal rank
L1 = randn(n1,r,n3)/n1;
L2 = randn(r,n2,n3)/n2;
L = tprod(L1,L2); % low rank part

p = 0.1;
m = p*n1*n2*n3;
temp = rand(n1*n2*n3,1);
[~,I] = sort(temp);
I = I(1:m);
Omega = zeros(n1,n2,n3);
Omega(I) = 1;
E = sign(rand(n1,n2,n3)-0.5);
S = Omega.*E; % sparse part, S = P_Omega(E)

Xn = L+S;
lambda = 1/sqrt(n3*max(n1,n2));

tic
[Lhat,Shat] = trpca_tnn(Xn,lambda,opts);

RES_L = norm(L(:)-Lhat(:))/norm(L(:))
RES_S = norm(S(:)-Shat(:))/norm(S(:))
trank = tubalrank(Lhat)

%% low-rank tensor completion based on tensor nuclear norm minimization (lrtc_tnn)
n1 = 50;
n2 = n1;
n3 = n1;
r = 0.1*n1 % tubal rank
L1 = randn(n1,r,n3)/n1;
L2 = randn(r,n2,n3)/n2;
X = tprod(L1,L2); % low rank part
p = 0.5;
omega = find(rand(n1*n2*n3,1)<p);
M = zeros(n1,n2,n3);
M(omega) = X(omega);

[Xhat,obj,err,iter] = lrtc_tnn(M,omega,opts);

err
iter
RSE = norm(X(:)-Xhat(:))/norm(X(:))
trank = tubalrank(Xhat)

%% regularized low-rank tensor completion based on tensor nuclear norm minimization (lrtcR_tnn)
n1 = 50;
n2 = n1;
n3 = n1;
r = 0.1*n1 % tubal rank
L1 = randn(n1,r,n3)/n1;
L2 = randn(r,n2,n3)/n2;
X = tprod(L1,L2); % low rank part
p = 0.5;
omega = find(rand(n1*n2*n3,1)<p);
M = zeros(n1,n2,n3);
M(omega) = X(omega);

lambda = 0.5;
[Xhat,Ehat,obj,err,iter] = lrtcR_tnn(M,omega,lambda,opts);
err
iter

%% low-rank tensor recovery from Gaussian measurements based on tensor nuclear norm minimization (lrtr_Gaussian_tnn)
n1 = 30;
n2 = n1;
n3 = 5;
r = 0.2*n1; % tubal rank
X = tprod(randn(n1,r,n3),randn(r,n2,n3)); % size: n1*n2*n3

m = 3*r*(n1+n2-r)*n3+1; % number of measurements
n = n1*n2*n3;
A = randn(m,n)/sqrt(m);

b = A*X(:);
Xsize.n1 = n1;
Xsize.n2 = n2;
Xsize.n3 = n3;

opts.DEBUG = 1;
[Xhat,obj,err,iter]  = lrtr_Gaussian_tnn(A,b,Xsize,opts);

RSE = norm(Xhat(:)-X(:))/norm(X(:))
trank = tubalrank(Xhat)


function  test_linear_svm()

    clc;
    clear;
    close all;

    %% Set algorithms
    if 0
        algorithms = gd_solver_list('ALL');
    else
        %algorithms = gd_solver_list('LS');
        %algorithms = gd_solver_list('NCG');
        %algorithms = gd_solver_list('BFGS');
        algorithms = {'SD-STD', 'SD-BKT'};
    end

    % # of classes (must not change)
    l = 2;

    %% prepare dataset
    if 1     % generate synthetic data
        n = 100;    % # of samples per class
        d = 3;      % # of dimensions
        std = 0.15; % standard deviation

        data = multiclass_data_generator(n, d, l, std);
        d = d + 1; % adding '1' row for intersect

        % train data
        x_train = [data.x_train; ones(1,l*n)];
        % assign y (label) {1,-1}
        y_train(data.y_train<=1.5) = -1;
        y_train(data.y_train>1.5) = 1;

        % test data
        x_test = [data.x_test; ones(1,l*n)];
        % assign y (label) {1,-1}
        y_test(data.y_test<=1.5) = -1;
        y_test(data.y_test>1.5) = 1;

    else    % load real-world data
        data = importdata('../data/mushroom/mushroom.mat');
        n = size(data.X,1);
        d = size(data.X,2) + 1;
        x_in = [data.X ones(n,1)]';
        y_in = data.y';

        perm_idx = randperm(n);
        x = x_in(:,perm_idx);
        y = y_in(perm_idx);

        % split data into train and test data
        % train data
        n_train = floor(n/8);
        x_train = x(:,1:n_train);
        y_train = y(1:n_train);
        x_train_class1 = x_train(:,y_train>0);
        x_train_class2 = x_train(:,y_train<0);
        n_class1 = size(x_train_class1,2);
        n_class2 = size(x_train_class2,2);

        % test data
        x_test = x(:,n_train+1:end);
        y_test = y(n_train+1:end);
        x_test_class1 = x_test(:,y_test>0);
        x_test_class2 = x_test(:,y_test<0);
        n_test_class1 = size(x_test_class1,2);
        n_test_class2 = size(x_test_class2,2);
        n_test = n_test_class1 + n_test_class2;

    end
    lambda = 0.1;
    w_opt = zeros(d,1);

    % set plot_flag
    if d > 4
        plot_flag = false;  % too high dimension
    else
        plot_flag = true;
    end

    %% define problem definitions
    problem = linear_svm(x_train, y_train, x_test, y_test, lambda);

    %% initialize
    w_init = randn(d,1);

    w_list = cell(length(algorithms),1);
    info_list = cell(length(algorithms),1);

    %% calculate solution
    if norm(w_opt)
    else
        % calculate solution
        w_opt = problem.calc_solution(1000);
    end
    f_opt = problem.cost(w_opt);
    fprintf('f_opt: %.24e\n', f_opt);

    %% perform algorithms
    for alg_idx=1:length(algorithms)
        fprintf('\n\n### [%02d] %s ###\n\n', alg_idx, algorithms{alg_idx});

        clear options;
        % general options for optimization algorithms
        options.w_init = w_init;
        options.tol_gnorm = 1e-10;
        options.max_iter = 100;
        options.verbose = true;
        options.f_opt = f_opt;
        options.store_w = true;

        switch algorithms{alg_idx}
            case {'SD-STD'}

                options.step_alg = 'fix';
                options.step_init = 1;
                [w_list{alg_idx}, info_list{alg_idx}] = sd(problem, options);

            case {'SD-BKT'}

                options.step_alg = 'backtracking';
                [w_list{alg_idx}, info_list{alg_idx}] = sd(problem, options);

            case {'SD-EXACT'}

                options.step_alg = 'exact';
                [w_list{alg_idx}, info_list{alg_idx}] = sd(problem, options);

            case {'SD-WOLFE'}

                options.step_alg = 'strong_wolfe';
                [w_list{alg_idx}, info_list{alg_idx}] = sd(problem, options);

            case {'SD-SCALE-EXACT'}

                options.sub_mode = 'SCALING';
                options.step_alg = 'exact';
                [w_list{alg_idx}, info_list{alg_idx}] = sd(problem, options);

            case {'Newton-STD'}

                [w_list{alg_idx}, info_list{alg_idx}] = newton(problem, options);

            case {'Newton-DAMP'}

                options.sub_mode = 'DAMPED';
                options.step_alg = 'backtracking';
                [w_list{alg_idx}, info_list{alg_idx}] = newton(problem, options);

            case {'Newton-CHOLESKY'}

                options.sub_mode = 'CHOLESKY';
                options.step_alg = 'backtracking';
                [w_list{alg_idx}, info_list{alg_idx}] = newton(problem, options);

            case {'CG-PRELIM'}

                options.sub_mode = 'PRELIM';
                options.step_alg = 'exact';
                %options.beta_alg = 'PR';
                [w_list{alg_idx}, info_list{alg_idx}] = cg(problem, options);

            case {'CG-BKT'}

                options.sub_mode = 'STANDARD';
                options.step_alg = 'backtracking';
                %options.beta_alg = 'PR';
                [w_list{alg_idx}, info_list{alg_idx}] = cg(problem, options);

            case {'CG-EXACT'}

                options.sub_mode = 'STANDARD';
                options.step_alg = 'exact';
                %options.beta_alg = 'PR';
                [w_list{alg_idx}, info_list{alg_idx}] = cg(problem, options);

            case {'CG-PRECON-EXACT'}

                options.sub_mode = 'PRECON';
                % diagonal scaling
                options.M = diag(diag(A));
                options.step_alg = 'exact';
                options.beta_alg = 'PR';

                [w_list{alg_idx}, info_list{alg_idx}] = cg(problem, options);

            case {'NCG-BTK'}

                options.sub_mode = 'STANDARD';
                options.step_alg = 'backtracking';
                options.beta_alg = 'PR';
                [w_list{alg_idx}, info_list{alg_idx}] = ncg(problem, options);

            case {'NCG-WOLFE'}

                options.sub_mode = 'STANDARD';
                options.step_alg = 'strong_wolfe';
                options.beta_alg = 'PR';
                [w_list{alg_idx}, info_list{alg_idx}] = ncg(problem, options);

            case {'BFGS-H-BKT'}

                options.step_alg = 'backtracking';
                [w_list{alg_idx}, info_list{alg_idx}] = bfgs(problem, options);

            case {'BFGS-H-EXACT'}

                options.step_alg = 'exact';
                [w_list{alg_idx}, info_list{alg_idx}] = bfgs(problem, options);

            case {'BFGS-B-BKT'}

                options.step_alg = 'backtracking';
                options.update_mode = 'B';
                [w_list{alg_idx}, info_list{alg_idx}] = bfgs(problem, options);

            case {'BFGS-B-EXACT'}

                options.step_alg = 'exact';
                options.update_mode = 'B';
                [w_list{alg_idx}, info_list{alg_idx}] = bfgs(problem, options);

            case {'DAMPED-BFGS-BKT'}

                options.step_alg = 'backtracking';
                options.update_mode = 'Damping';
                [w_list{alg_idx}, info_list{alg_idx}] = bfgs(problem, options);

            case {'DAMPED-BFGS-EXACT'}

                options.step_alg = 'exact';
                options.update_mode = 'Damping';
                [w_list{alg_idx}, info_list{alg_idx}] = bfgs(problem, options);

            case {'L-BFGS-BKT'}

                options.step_alg = 'backtracking';
                [w_list{alg_idx}, info_list{alg_idx}] = lbfgs(problem, options);

            case {'L-BFGS-EXACT'}

                options.step_alg = 'exact';
                [w_list{alg_idx}, info_list{alg_idx}] = lbfgs(problem, options);

            case {'L-BFGS-WOLFE'}

                options.step_alg = 'strong_wolfe';
                [w_list{alg_idx}, info_list{alg_idx}] = lbfgs(problem, options);

            case {'BB'}

                options.step_alg = 'exact';
                [w_list{alg_idx}, info_list{alg_idx}] = bb(problem, options);

            case {'SGD'}

                options.batch_size = 1;
                options.step = 0.1 * options.batch_size;
                %options.step_alg = 'decay';
                options.step_alg = 'fix';

                [w_list{alg_idx}, info_list{alg_idx}] = sgd(problem, options);

            otherwise
                warn_str = [algorithms{alg_idx}, ' is not supported.'];
                warning(warn_str);
                w_list{alg_idx} = '';
                info_list{alg_idx} = '';
        end

    end

    %% plot all
    close all;

    % display iter vs cost/gnorm
    display_graph('iter','cost', algorithms, w_list, info_list);
    display_graph('iter','gnorm', algorithms, w_list, info_list);

    % draw convergence sequence
    w_history = cell(1);
    cost_history = cell(1);
    for alg_idx=1:length(algorithms)
        w_history{alg_idx} = info_list{alg_idx}.w;
        cost_history{alg_idx} = info_list{alg_idx}.cost;
    end
    draw_convergence_sequence(problem, w_opt, algorithms, w_history, cost_history);

    % display classification results
    y_pred_list = cell(length(algorithms),1);
    accuracy_list = cell(length(algorithms),1);
    for alg_idx=1:length(algorithms)
        p = problem.prediction(w_list{alg_idx});
        % calculate accuracy
        accuracy_list{alg_idx} = problem.accuracy(p);

        fprintf('Classificaiton accuracy: %s: %.4f\n', algorithms{alg_idx}, problem.accuracy(p));

        % convert from {1,-1} to {1,2}
        p(p==-1) = 2;
        p(p==1) = 1;
        % predict class
        y_pred_list{alg_idx} = p;
    end

    % convert from {1,-1} to {1,2}
    y_train(y_train==-1) = 2;
    y_train(y_train==1) = 1;
    y_test(y_test==-1) = 2;
    y_test(y_test==1) = 1;
    if plot_flag
        display_classification_result(problem, algorithms, w_list, y_pred_list, accuracy_list, x_train, y_train, x_test, y_test);
    end

end


function figure_num = circle_segment_test06 ( figure_num )

%*****************************************************************************80
%
%% CIRCLE_SEGMENT_TEST06 samples using CIRCLE_SEGMENT_SAMPLE_FROM_HEIGHT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 May 2013
%
%  Author:
%
%    John Burkardt
%
  if ( nargin < 1 )
    figure_num = 0;
  end

  n = 100;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CIRCLE_SEGMENT_TEST06\n' );
  fprintf ( 1, '  CIRCLE_SEGMENT_SAMPLE_FROM_HEIGHT samples a circle segment.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Plot %d points from several segments.\n', n );
  fprintf ( 1, '\n' );

  r = 1.0;
  theta = pi;

  for i = 0 : 3

    h = circle_segment_height_from_angle ( r, theta );

    thetah = theta / 2.0;

    an = 0.5 * pi + linspace ( -thetah, +thetah, 51 );
    x = r * cos ( an );
    y = r * sin ( an );
    x = [ x, x(1) ];
    y = [ y, y(1) ];

    [ xs, ys, seed ] = circle_segment_sample_from_height ( r, h, n, seed );

    figure_num = figure_num + 1;
    figure ( figure_num );
    clf
    hold on
    plot ( x, y, 'k-' );
    plot ( xs, ys, 'b.', 'Markersize', 20 );
    grid on
    axis equal
    xlabel ( '<--- X --->' );
    ylabel ( '<--- Y --->' );
    title ( sprintf ( 'Segment with THETA = %g, R = %g H = %g\n', theta, r, h ) );
    hold off

    filename = sprintf ( 'sample_t%g.png', theta );
    print ( '-dpng', filename );
    fprintf ( 1, '  Created graphics file "%s".\n', filename );

    theta = theta / 2.0;

  end

  return
end


function geometry_test044 ( )

%*****************************************************************************80
%
%% TEST044 tests SEGMENTS_INT_1D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 April 2009
%
%  Author:
%
%    John Burkardt
%
  test_num = 7;

  q1_test = [ -1.0, 3.0, 1.0,  0.5, 0.25, 0.5, 2.0 ];
  q2_test = [  1.0, 2.0, 2.0, -3.0, 0.50, 0.5, 2.0 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST044\n' );
  fprintf ( 1, '  SEGMENTS_INT_1D determines the intersection [R1,R2]\n' );
  fprintf ( 1, '  of line segments [P1,P2] and [Q1,Q2] in 1D.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  DIST is negative for overlap,\n' );
  fprintf ( 1, '  0 for point intersection,\n' );
  fprintf ( 1, '  positive if there is no overlap.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, ...
    '  Test      P1        P2        Q1        Q2        R1        R2        DIST\n' );
  fprintf ( 1, '\n' );

  p1 = -1.0;
  p2 = 1.0;

  for test = 1 : test_num

    q1 = q1_test(test);
    q2 = q2_test(test);

    [ dist, r1, r2 ] = segments_int_1d ( p1, p2, q1, q2 );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  %4d  %8f  %8f  %8f  %8f  %8f  %8f  %8f\n', ...
      test, p1, p2, q1, q2, r1, r2, dist );

  end

  return
end