%% DEMO_stent_hexahedral_sweeping_02
% 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=3;

%% Contol parameters

controlParameterSet.stentRadius=3; %The outer radius of the stent
controlParameterSet.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
controlParameterSet.stentSectionHeight=0.1; %Height of the stent wire
controlParameterSet.stentSectionWidth=0.1; %Width of the stent wire
controlParameterSet.numStepsCircumference=(controlParameterSet.numPeriodsWave*numStepsPeriod)+1; %Number of sweeping steps across curve
controlParameterSet.overSampleFactorCurve=10; %Oversample factor curve
controlParameterSet.numSplitSteps_axial=1;
controlParameterSet.numSplitSteps_inward=1;
controlParameterSet.plotOn=0;
% controlParameterSet.waveAmplitude=0.9; %Amplitude of the sinusoidal modulation

sheetLayerThickness=0.025;
numStepsSheet=1;

%%

numSegments=8;
waveAmplitudes=0.6*ones(1,numSegments);
waveAmplitudes(2)=0.9;
offsetLevels=waveAmplitudes*2;

offSetTotal=0;

cFigure; hold on;
title('Stent hexahedral mesh','fontSize',fontSize);
colormap(gjet(4)); caxis([1 4]); icolorbar;
axisGeom;
camlight headlight;
drawnow;

E_stent_cell=cell(numSegments,1);
V_stent_cell=cell(numSegments,1);
E_sheet_cell=cell(numSegments,1);
V_sheet_cell=cell(numSegments,1);
for q=1:1:numSegments

    controlParameterSet.waveAmplitude=waveAmplitudes(q); %Amplitude of the sinusoidal modulation
    [E,V]=stentSegmentDesign(controlParameterSet);

    offSetTotal=offSetTotal+offsetLevels(q);
    V(:,3)=V(:,3)+offSetTotal;

    %%

    C=hexVol(E,V); %Get hexahedral element volumes

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

    [indBoundary]=tesBoundary(F,V);
    faceMarker=ones(size(E,1),1)*(1:6); %The 6 face colors for the hexahedral faces
    faceMarker=faceMarker(:); %Force as a column
    Fb=F(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

    %%
    gpatch(Fb,V,faceBoundaryMarker,'k',1);

    %%

    F_inner = Fb(faceBoundaryMarker==2,:);
    [edgesBoundaryInner]=patchBoundary(F_inner,V);

    edgesBottom=F_inner(:,[4 1]);
    edgesTop=F_inner(:,[2 3]);

    edgesBoundaryInnerTop=edgesBoundaryInner(all(ismember(edgesBoundaryInner,edgesTop),2),:);
    edgesBoundaryInnerBottom=edgesBoundaryInner(all(ismember(edgesBoundaryInner,edgesBottom),2),:);

    indCurveTop=edgeListToCurve(edgesBoundaryInnerTop);
    indCurveTop=flip(indCurveTop(1:end-1));
    indCurveBottom=edgeListToCurve(edgesBoundaryInnerBottom);
    indCurveBottom=indCurveBottom(1:end-1);

    plotV(V(indCurveTop(:),:),'r-','LineWidth',lineWidth);
    plotV(V(indCurveBottom(:),:),'b-','LineWidth',lineWidth);

    drawnow;

   %%
   if q==1
       [FQ,VQ]=patchCleanUnused(F_inner,V);
   else
       cPar.closeLoopOpt=1;
       cPar.patchType='quad';
       [Fq,Vq]=polyLoftLinear(V_curveTopPrevious,V(indCurveBottom(:),:),cPar);
%        gpatch(Fq,Vq,'rw','rw',1);
       [F_inner_clean,V_inner_clean]=patchCleanUnused(F_inner,V);
       [FQ,VQ]=joinElementSets({Fq,F_inner_clean},{Vq,V_inner_clean});
       [FQ,VQ]=mergeVertices(FQ,VQ);
%        gpatch(FQ,VQ,'rw','rw',1);
%        patchNormPlot(FQ,VQ);
   end

   [E_sheet,V_sheet,Fq1,Fq2]=quadThick(FQ,VQ,1,sheetLayerThickness,numStepsSheet);

   [F_sheet]=element2patch(E_sheet); %Create face data for plotting
   gpatch(F_sheet,V_sheet,'gw','gw',1);
   %        patchNormPlot(F_sheet,V_sheet);

   E_sheet_cell{q}=E_sheet;
   V_sheet_cell{q}=V_sheet;

   V_curveTopPrevious=V(indCurveTop(:),:);

   E_stent_cell{q}=E;
   V_stent_cell{q}=V;

end

%% Merge components

[E_stent,V_stent,C_stent]=joinElementSets(E_stent_cell,V_stent_cell);
[E_sheet,V_sheet,C_sheet]=joinElementSets(E_sheet_cell,V_sheet_cell);
[E,V,C]=joinElementSets({E_stent,E_sheet},{V_stent,V_sheet},{C_stent,C_sheet+max(C_stent)});
[E,V]=mergeVertices(E,V);

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

%%
cFigure; hold on;
title('Stent hexahedral mesh','fontSize',fontSize);
gpatch(F,V,CF,'none',1);
% patchNormPlot(F,V);
colormap gjet; icolorbar
axisGeom;
camlight headlight;
drawnow;

%% Export inp file
%
% elementStruct.E=E;
% elementStruct.E_ind=(1:size(E,1))';
% elementStruct.E_type='*ELEMENT, TYPE=C3D8, ELSET=PART-STENT';
% nodeStruct.N=V;
% nodeStruct.N_ind=(1:size(V,1))';
%
% pathName = fileparts(fileparts(mfilename('fullpath')));
% fileName=fullfile(pathName,'data','INP','stentMeshSheet.inp');
% export_INP(elementStruct,nodeStruct,fileName);

%% FUNCTIONS

function [E,V]=stentSegmentDesign(controlParameterSet)

%% parse input

stentRadius=controlParameterSet.stentRadius; %The outer radius of the stent
numPeriodsWave=controlParameterSet.numPeriodsWave; %The number of periods to use for a sinusoidal modulation
waveAmplitude=controlParameterSet.waveAmplitude; %Amplitude of the sinusoidal modulation
stentSectionHeight=controlParameterSet.stentSectionHeight; %Height of the stent wire
stentSectionWidth=controlParameterSet.stentSectionWidth; %Width of the stent wire
numStepsCircumference=controlParameterSet.numStepsCircumference; %Number of sweeping steps across curve
overSampleFactorCurve=controlParameterSet.overSampleFactorCurve; %Oversample factor curve
numSplitSteps_axial=controlParameterSet.numSplitSteps_axial;
numSplitSteps_inward=controlParameterSet.numSplitSteps_inward;
plotOn=controlParameterSet.plotOn;

%% plot settings
if plotOn==1
    fontSize=25;
    markerSize=10;
    lineWidth=1;
end

%% 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; ...
    ];

%%
% V=isualize stent section
if plotOn==1
    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;
end

%% 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
if plotOn==1
    cFigure; hold on;
    title('Stent guide curve','fontSize',fontSize);
    plotV(V_guide_curve,'k.-','lineWidth',lineWidth,'MarkerSize',markerSize);
    axisGeom;
    drawnow;
end

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

if plotOn==1
    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;
end

%% 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
[~,~,~,S]=sweepLoft(V_section,V_section,n3,n3,V_guide_curve,numStepsSweep,numTwist,0);

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

%% 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=numSplitSteps_axial;
[E,V]=subHex(E,V,nRefine,splitMethod);

splitMethod=4;
nRefine=numSplitSteps_inward;
[E,V]=subHex(E,V,nRefine,splitMethod);

%%
% Visualize hexahedral mesh

if plotOn==1

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

    cFigure; hold on;
    title('Stent hexahedral mesh','fontSize',fontSize);
    plotV(V_guide_curve,'k-','lineWidth',3);
    gpatch(F,V,'gw','k',1);
    patchNormPlot(F,V);
    axisGeom;
    camlight headlight;
    drawnow;
end

%%

end
%%
% _*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 ld = sn_ld(l, n_sensor, n_source, n_snapshot)
%SN_LD Sufficient statistic for source number detection in MDL/AIC.
%   This function is used internally.
%Syntax:
%   ld = SN_LD(l, n_sensor, n_source, n_snapshot);
%Inputs:
%   l - Eigenvalues of the covariance matrix in descending order.
%   n_sensor - Number of sensors used. Should match the length of l.
%   n_source - Number of sources.
%   n_snapshot - Number of snapshots.
%Outputs:
%   ld - Computed value.
%Reference:
%   H. L. Van Trees, Optimum array processing. New York: Wiley, 2002.

diff = n_sensor - n_source;
l = l(n_source+1:end);
ld = n_snapshot * diff * log(sum(l)/diff/(prod(l)^(1/diff)));

end


clear;
t0=0;
tf=10;
b0=[0.2 0.2];
[t,b]=ode45('dfun3',[t0, tf],b0);
plot(b(:,1),b(:,2));
hold on
b0=[0.1 0.1];
[t,b]=ode45('dfun3',[t0, tf],b0);
plot(b(:,1),b(:,2));
hold on
b0=[0.2 0.15];
[t,b]=ode45('dfun3',[t0, tf],b0);
plot(b(:,1),b(:,2));
hold on
xlabel('x1');
ylabel('x2');


function [Data_TV,avg_votenum] = Reversed_Sparse_Ball_faster(data, Sigma, Sigma_noise, method, index_mat_vote, S)

if strcmp( method, 'Std2' ) == 1 % Sparse Standard Ball Tensor Voting with my code

    disp('Std2');
    disp('faster version');
    D = size( data, 2 );
    N = size( data, 1 );
    votenum = 0;
    data_inv=(data(:, 1:D))';
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    F = S<sqrt(Sigma./3);
    Data_TV(:,1:D) = data(:,1:D);
    size(F)
    for i = 1 : size( data, 1 )
       Data_TV(i,D+1) = 1;
       Tensor = zeros(D,D); % 2nd order tensor - matrix
       temp_nei_i=index_mat_vote(:,i);
       length(temp_nei_i);
       l=length(temp_nei_i);
        for j = 1:l
        idxNeig = temp_nei_i(j);
       % idxNeig = (j);
        dis = S(j,i);
           % if (dis > 0) & (F(i,idxNeig))
         if (dis > 0) & (F(j,i))
                votenum = votenum + 1;
                dir = ( data(i,1:D) - data(idxNeig,1:D) ) ;
                Tensor = Tensor + exp( -dis^2/ Sigma ) * (eye(D) - dir'*dir);

            end
        end

       temp_dim=1;
       [U1,S1,V1] = rsvd(Tensor,temp_dim);
 %Extract first temp_dim eigenvlaues and k eigenvectors:

 %disp('fast SVD computing');

 if  isempty(S1)==1
     S1 = zeros(temp_dim,D);
     U1 = zeros(D,temp_dim);
     V1 = zeros(D,temp_dim);
 end
      Lam_k=S1(1:temp_dim,:);
      Vec_k=U1*V1';

% %         size(U1)
      % [L,J] = sort( diag(Lam) );
        [L,J] = sort( diag(Lam_k) );
        for k = 1 : temp_dim
        %Data_TV(i,D+1+k) =  Lam(J(D-k+1),J(D-k+1));% here the ranking is very important, a slight error before but revised now^_^
        Data_TV(i,D+1+k) =  Lam_k(J(temp_dim-k+1),J(temp_dim-k+1));
        end
        Data_TV(i, D+1+temp_dim+1 : (2*D)+1 )= 0;
        for k = 1 : temp_dim % instead of D
        %Data_TV( i, 2*D+1+(k-1)*D+1 : 2*D+1+k*D ) = Vec(:,J(D-k+1));
        Data_TV( i, 2*D+1+(k-1)*D+1 : 2*D+1+k*D ) = Vec_k(:,J(temp_dim-k+1));
        end
      %   error_vec(i)=error;
    end
    avg_votenum=votenum/N;
    disp('Average Vote is'); disp(votenum/N);

end


function [ x, w ] = rule06 ( n )

%*****************************************************************************80
%
%% RULE06 returns the rule of degree 6.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 July 2014
%
%  Author:
%
%    Original FORTRAN77 version by Hong Xiao, Zydrunas Gimbutas.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Hong Xiao, Zydrunas Gimbutas,
%    A numerical algorithm for the construction of efficient quadrature
%    rules in two and higher dimensions,
%    Computers and Mathematics with Applications,
%    Volume 59, 2010, pages 663-676.
%
%  Parameters:
%
%    Input, integer N, the number of nodes.
%
%    Output, real X(2,N), the coordinates of the nodes.
%
%    Output, real W(N), the weights.
%
  xs = [ ...
    0.4595981103653579E-16, ...
    0.9258200997725515E+00, ...
    0.6742045114073804E-16, ...
   -0.9258200997725515E+00, ...
   -0.3805544332083157E+00, ...
    0.3805544332083157E+00, ...
    0.3805544332083157E+00, ...
   -0.3805544332083157E+00, ...
   -0.8059797829185990E+00, ...
    0.8059797829185988E+00, ...
    0.8059797829185990E+00, ...
   -0.8059797829185988E+00 ];
  ys = [ ...
   -0.9258200997725515E+00, ...
   -0.1073032005210112E-16, ...
    0.9258200997725515E+00, ...
    0.1241105822293750E-15, ...
   -0.3805544332083157E+00, ...
   -0.3805544332083157E+00, ...
    0.3805544332083157E+00, ...
    0.3805544332083157E+00, ...
   -0.8059797829185988E+00, ...
   -0.8059797829185990E+00, ...
    0.8059797829185988E+00, ...
    0.8059797829185990E+00 ];
  ws = [ ...
    0.1711023816204485E+00, ...
    0.1711023816204485E+00, ...
    0.1711023816204485E+00, ...
    0.1711023816204485E+00, ...
    0.3681147816131979E+00, ...
    0.3681147816131979E+00, ...
    0.3681147816131979E+00, ...
    0.3681147816131979E+00, ...
    0.1678896179529011E+00, ...
    0.1678896179529011E+00, ...
    0.1678896179529011E+00, ...
    0.1678896179529011E+00 ];

  x(1,1:n) = xs(1:n);
  x(2,1:n) = ys(1:n);
  w(1:n) = ws(1:n);

  return
end


function [pyr,pind] = buildWUpyr(im, Nsc, daub_order);

% [PYR, INDICES] = buildWUpyr(IM, HEIGHT, DAUB_ORDER)
%
% Construct a separable undecimated orthonormal QMF/wavelet pyramid
% on matrix (or vector) IM.
%
% HEIGHT specifies the number of pyramid levels to build. Default
% is maxPyrHt(IM,FILT).  You can also specify 'auto' to use this value.
%
% DAUB_ORDER: specifies the order of the daubechies wavelet filter used
%
% PYR is a vector containing the N pyramid subbands, ordered from fine
% to coarse.  INDICES is an Nx2 matrix containing the sizes of
% each subband.

% JPM, Univ. de Granada, 03/2003, based on Rice Wavelet Toolbox
% function "mrdwt" and on Matlab Pyrtools from Eero Simoncelli.

if Nsc < 1,
    display('Error: Number of scales must be >=1.');
else,

Nor = 3; % fixed number of orientations;
h = daubcqf(daub_order);
[lpr,yh] = mrdwt(im, h, Nsc+1); % performs the decomposition

[Ny,Nx] = size(im);

% Reorganize the output, forcing the same format as with buildFullSFpyr2

pyr = [];
pind = zeros((Nsc+1)*Nor+2,2);    % Room for a "virtual" high pass residual, for compatibility
nband = 1;
for nsc = 1:Nsc+1,
    for nor = 1:Nor,
        nband = nband + 1;
        band = yh(:,(nband-2)*Nx+1:(nband-1)*Nx);
        sh = (daub_order/2 - 1)*2^nsc;  % approximate phase compensation
        if nor == 1,        % horizontal
            band = shift(band, [sh 2^(nsc-1)]);
        elseif nor == 2,    % vertical
            band = shift(band, [2^(nsc-1) sh]);
        else
            band = shift(band, [sh sh]);    % diagonal
        end
        if nsc>2,
            band = real(shrink(band,2^(nsc-2)));  % The low freq. bands are shrunk in the freq. domain
        end
        pyr = [pyr; vector(band)];
        pind(nband,:) = size(band);
    end
end

band = lpr;
band = shrink(band,2^Nsc);
pyr = [pyr; vector(band)];
pind(nband+1,:) = size(band);

end


% This is a 2D separable low pass filter for constructing Gaussian and
% Laplacian pyramids, built from a 1D 5-tap low pass filter.
%
% tom.mertens@gmail.com, August 2007
% sam.hasinoff@gmail.com, March 2011  [imfilter faster with 2D filter]
%

function f = pyramid_filter()
f = [.05, .25, .4, .25, .05];  % original [Burt and Adelson, 1983]
%f = [.0625, .25, .375, .25, .0625];  % binom-5
f = f'*f;
end


%CALIBRATIONMATRIXVALUES  Computes useful camera characteristics from the camera matrix
%
%     S = cv.calibrationMatrixValues(cameraMatrix, imageSize, apertureWidth, apertureHeight)
%
% ## Input
% * __cameraMatrix__ Input 3x3 camera matrix that can be estimated by
%   cv.calibrateCamera or cv.stereoCalibrate.
% * __imageSize__ Input image size `[w,h]` in pixels.
% * __apertureWidth__ Physical width in mm of the sensor.
% * __apertureHeight__ Physical height in mm of the sensor.
%
% ## Output
% * __S__ Struct with the following fields
%   * __fovx__ Output field of view in degrees along the horizontal sensor
%     axis.
%   * __fovy__ Output field of view in degrees along the vertical sensor axis.
%   * __focalLength__ Focal length of the lens in mm.
%   * __principalPoint__ Principal point `[cx,cy]` in mm.
%   * __aspectRatio__ Pixel aspect ratio `fy/fx`.
%
% The function computes various useful camera characteristics from the
% previously estimated camera matrix.
%
% ### Note
% Do keep in mind that the unity measure 'mm' stands for whatever unit of
% measure one chooses for the chessboard pitch (it can thus be any value).
%
% See also: cv.calibrateCamera, cv.stereoCalibrate
%


function [check,maxerr,yfit] = cvxfitfeas(U,y,err)

% Checks if a data set can be approximated with a convex function subject
% to a maximum residual constraint.
%
% Notation:
%
% U - matrix of input data points, with rows denoting the different points
% y - outputs corresponding to the inputs (row vector)
% err - maximum allowable residual
% check - returns 1 if it is possible to fit function, 0 otherwise
% maxerr - maximum residual of convex fit
% yfit - output values defining the fit

m = length(y);
n = length(U(1,:));

A1 = [-eye(m) zeros(m,m*n) -ones(m,1);
    eye(m) zeros(m,m*n) -ones(m,1)];
b1 = [-y'; y'];

A2 = zeros(m*(m-1),m+m*n+1);
lc = 1;
for i = 1:m
    for j = 1:m
        if i ~= j
            A2(lc,i) = 1;
            A2(lc,j) = -1;
            A2(lc,[1+m+(i-1)*n:m+i*n]) = U(j,:)-U(i,:);
            lc = lc + 1;
        end
    end
end

b2 = zeros(m*(m-1),1);

A = [A1;A2];
b = [b1;b2];

[xopt,maxerr,exit] = linprog([zeros(1,m+m*n) 1],A,b,[],[],[],[],[],optimset('disp','iter'));
if exit <= 0
    disp('Warning - Optimization did not terminate property.');
end
yfit = xopt(1:m);

if maxerr <= err
    check = 1;
else
    check = 0;
end

end


function n = norm(a)
n = sqrt(sum(a.^2));


% test for propagation and geodesic extraction on 2D planar shape

path(path, 'toolbox/');
path(path, 'data/');

name = 'chicken';
name = 'apple';
name = 'cavern';
name = 'camel';
name = 'giraffe';

rep = 'results/shape-geodesics/';
if not(exist(rep))
    mkdir(rep);
end

n = 128;
M = rescale( load_image(name,n), 0,1 );
M = perform_blurring(M,5);
M = double(M>0.5);

% make sure pixels on the boundary are black
if M(1)==1
    M = 1-M;
end

warning off;
imwrite(1-M, [rep name '-shape.png'], 'png');
warning off;

% compute geodesic distance
clf;
imagesc(M); axis image; axis off;
title('click on a point inside the shape');
[y,x] = ginput(1);
start_points = round([x y]');
W = ones(n);
L = zeros(n)-Inf; L(M==1) = +Inf;
options.constraint_map = L;
disp('Compute distance function');
[D,S,Q] = perform_fast_marching(W, start_points, options);

bound = compute_shape_boundary(M);
nbound = size(bound,1);
npaths = 30;
sel = round(linspace(1,nbound+1,npaths+1)); sel(end) = [];
end_points = bound(sel,:);

disp('Extract paths');
paths = {};
D1 = D; D1(M==0) = 1e9;
for i=1:npaths
    paths{i} = compute_geodesic(D1,end_points(i,:)');
%    paths{i} = compute_discrete_geodesic(D1,end_points(i,:)')';
end

ms = 30; lw = 3;
% display
A = convert_distance_color(D);
clf; hold on;
imageplot(A); axis image; axis off;
for i=1:npaths
    end_point = end_points(i,:);
    h = plot( paths{i}(2,:), paths{i}(1,:), 'k' );
    set(h, 'LineWidth', lw);
    h = plot(end_point(2),end_point(1), '.b');
    set(h, 'MarkerSize', ms);
end
h = plot(start_points(2),start_points(1), '.r');
set(h, 'MarkerSize', ms);
hold off;
colormap jet(256);
axis ij;
saveas(gcf, [rep name '-geodesics.png'], 'png');


function gray = grayFilter(img, varargin)
%GRAYFILTER Compute configuration map of a binary image
%
%   GRAY = grayFilter(IMG);
%   Returns a gray-scale image, with size dim(IMG)-1, containing values of
%   the 2x2 configuration of the original binary image.
%
%   Example:
%   img = [0 0 0 0 0;0 1 1 0 0;0 1 1 1 0;0 0 0 0 0];
%   grayFilter(img)
%   ans =
%      8    12     4     0
%     10    15    13     4
%      2     3     3     1
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 22/10/2004.
%

%   HISTORY
%   03/11/2004 : add 3x3 2D case.

% pre-processing
img = img~=0;
dim = size(img);
nd = length(dim);

% size of neighborhood to consider
nu=1;

% extract input parameters
if ~isempty(varargin)
    var = varargin{1};
    if length(var)==1
        nu = var;
    else
        coef = var;
        % find size of filter from length of coefficients.
        nu = power(log(length(coef))/log(2), 1/nd);
    end
end

if length(dim)==2
    % 2 dimensions
    NY = dim(1)-nu;
    NX = dim(2)-nu;

    if nu==1
        gray=1*img(1:NY,    1:NX) + ...
             2*img(1:NY,    2:NX+1) + ...
             4*img(2:NY+1,  1:NX) + ...
             8*img(2:NY+1,  2:NX+1) ;
    elseif nu==2
        gray =  1*img(1:NY,     1:NX) + ...
                2*img(1:NY,     2:NX+1) + ...
                4*img(1:NY,     3:NX+2) + ...
                8*img(2:NY+1,   1:NX) + ...
               16*img(2:NY+1,   2:NX+1) + ...
               32*img(2:NY+1,   3:NX+2) + ...
               64*img(3:NY+2,   1:NX) + ...
              128*img(3:NY+2,   2:NX+1) + ...
              256*img(3:NY+2,   3:NX+2) ;
    end

else
    % 3 dimensions
    NY = dim(1)-nu;
    NX = dim(2)-nu;
    NZ = dim(3)-nu;

    gray=   img(1:NY,    1:NX,   1:NZ) + ...
          2*img(1:NY,    2:NX+1, 1:NZ) + ...
          4*img(2:NY+1,  1:NX,   1:NZ) + ...
          8*img(2:NY+1,  2:NX+1, 1:NZ) + ...
         16*img(1:NY,    1:NX,   2:NZ+1) + ...
         32*img(1:NY,    2:NX+1, 2:NZ+1) + ...
         64*img(2:NY+1,  1:NX,   2:NZ+1) + ...
        128*img(2:NY+1,  2:NX+1, 2:NZ+1);
end


classdef IMOP6 < PROBLEM
% <multi> <real> <expensive/none>
% Benchmark MOP with irregular Pareto front
% a1 --- 0.05 --- Parameter a1
% a2 ---   10 --- Parameter a2
% K  ---    5 --- Parameter K

%------------------------------- Reference --------------------------------
% Y. Tian, R. Cheng, X. Zhang, M. Li, and Y. Jin, Diversity assessment of
% multi-objective evolutionary algorithms: Performance metric and benchmark
% problems, IEEE Computational Intelligence Magazine, 2019, 14(3): 61-74.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties(Access = private)
        a1 = 0.05;  % Parameter a1
        a2 = 10;    % Parameter a2
        K  = 5;     % Parameter K
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            [obj.a1,obj.a2,obj.K] = obj.ParameterSet(0.05,10,5);
            obj.M = 3;
            if isempty(obj.D); obj.D = 10; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            y1 = mean(PopDec(:,1:2:obj.K),2).^obj.a1;
            y2 = mean(PopDec(:,2:2:obj.K),2).^obj.a2;
            g  = sum((PopDec(:,obj.K+1:end)-0.5).^2,2);
            r  = max(0,min(sin(3*pi*y1).^2,sin(3*pi*y2).^2)-0.05);
            PopObj(:,1) = (1+g).*y1 + ceil(r);
            PopObj(:,2) = (1+g).*y2 + ceil(r);
            PopObj(:,3) = (0.5+g).*(2-y1-y2) + ceil(r);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            [x,y] = meshgrid(linspace(0,1,ceil(sqrt(N))));
            R = [x(:),y(:)];
            r = max(0,min(sin(3*pi*R(:,1)).^2,sin(3*pi*R(:,2)).^2)-0.05);
            R(:,3) = 1 - sum(R,2)/2;
            R = R + repmat(ceil(r),1,3);
            R = R(NDSort(R,1)==1,:);
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            [x,y] = meshgrid(linspace(0,1,50));
            z     = 1 - x/2 - y/2;
            R     = [x(:),y(:),z(:)];
            r     = max(0,min(sin(3*pi*R(:,1)).^2,sin(3*pi*R(:,2)).^2)-0.05);
            R     = R + repmat(ceil(r),1,3);
            fes   = NDSort(R,1) == 1;
            z(reshape(~fes,size(z))) = nan;
            R     = {x,y,z};
        end
    end
end


function chebcoeffs = chebvals2chebcoeffs(chebvals, kind)
%CHEBVALS2CHEBCOEFFS  Convert Chebyshev values to coefficients.
% 	CHEBCOEFFS = CHEBVALS2CHEBCOEFFS(CHEBVALS), converts the column vector
%   CHEBVALS of values on a second-kind Chebyshev grid (i.e, F(CHEBPTS(N)))
%   to a vector CHEBCOEFFS of the Chebyshev coefficients of the series
%       F(X) = C_CHEB(1)*T0(X) + ... + C_CHEB(N)*T{N-1}(X).
%
% 	CHEBVALS2CHEBCOEFFS(CHEBVALS, 1) is similar, but assumes the entries in
% 	CHEBVALS come from evaluating on a first-kind Chebyshev grid, i.e.,
%   F(CHEBPTS(N,1))).
%
% See also CHEBTECH2.VALS2COEFFS, CHEBPTS.

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

% Default to second-kind points
if ( nargin == 1 )
    kind = 2;
end

if ( kind == 1 )
    % This command is a wrapper for chebtech2/vals2coeffs.
    chebcoeffs = chebtech1.vals2coeffs(chebvals);
elseif ( kind == 2 )
    % This command is a wrapper for chebtech1/vals2coeffs.
    chebcoeffs = chebtech2.vals2coeffs(chebvals);
else
    error('CHEBFUN:chebvals2chebcoeffs:kind', ...
        'Invalid Chebyshev kind. Must be 1 or 2.')
end

end


% v_rotro2eu_tab: Calculate tables needed for v_rotro2eu
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 52 different rotation matrix patterns of -1,0,+1:                     %
%  1- 3: identity matrix rows in order: 123, 231, 312                   %
%  1- 3: negated identity matrix rows in order: 132, 213, 321           %
%  7-12: As 1-6 but with rows 2,3 negated                               %
% 13-18: As 1-6 but with rows 1,3 negated                               %
% 19-24: As 1-6 but with rows 1,2 negated                               %
% 25-33: +1 in position (i-24) and 0's in remainder of this row and col %
% 34-42: -1 in position (i-24) and 0's in remainder of this row and col %
% 43-51: 0 in position (i-42)                                           %
% 52: no special symmetry                                               %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
mes=[1:3 10:12 7:9 4:6]; % sign reversal look-up table
rtci=[2 3 5 6 8 9; 3 1 6 4 9 7; 1 2 4 5 7 8]';
rtsi=[3 2 6 5 9 8; 1 3 4 6 7 9; 2 1 5 4 8 7]';
rtr=[1 4 7 2 5 8 3 6 9]; % indices to transpose a vectorized 3x3 matrix
w6=ones(6,1); %
th6=3*w6;
x6=[2 1 2 1 2 1]'; % Index for sin components
scai=[0 0 0 1; 0 0 0 2; 0 0 0 3; 1 -1 0 1; 1 -1 0 2; 1 -1 0 3; 0 0 -1 1; 0 0 -1 2; 0 0 -1 3; -1 1 0 1; -1 1 0 2; -1 1 0 3]'; % [sin; -sin; cos; xyz] for fixed rotations
% create pattersn of non-zero entries
nzpatt=10*ones(3,3,52); % pattern of -1,0,+1
e3=eye(3);
nzpatt(:,:,1)=e3;
nzpatt(:,:,2)=e3([2 3 1],:);
nzpatt(:,:,3)=e3([3 1 2],:);
nzpatt(:,:,4)=-e3([1 3 2],:);
nzpatt(:,:,5)=-e3([2 1 3],:);
nzpatt(:,:,6)=-e3([3 2 1],:);
for j=1:3
    f3=-e3;
    f3(j,j)=1;
    for i=1:6
        nzpatt(:,:,i+6*j)=f3*nzpatt(:,:,i);
    end
end
for i=1:9
    ir=1+mod(i-1,3);
    ic=1+(i-ir)/3;
    nzpatt(:,ic,i+24)=0;
    nzpatt(ir,:,i+24)=0;
    nzpatt(ir,ic,i+24)=1;
        nzpatt(:,ic,i+33)=0;
    nzpatt(ir,:,i+33)=0;
    nzpatt(ir,ic,i+33)=-1;
    nzpatt(ir,ic,i+42)=0;
end
nzpattv=reshape(nzpatt,9,52); % vectorize the 3x3 matrices
nzpattc=reshape(sum(nzpatt~=0,1),3,52); % number of non-zero elements in each column
% now create transition map
trmap=zeros(52,12);  % result of applying transformation j to pattern i
zel=zeros(4,3,52); % elements to zero: [zero; non-zero; sine-sign; targ-sign],transformation,initial pattern
jm='xyz123456789'; % rotation patterns
for i=1:52
    for j=1:12
        rijv=reshape(v_rotqr2ro(v_roteu2qr(jm(j),pi/3))*nzpatt(:,:,i),9,1); % vectorized result of applying transformation
        rijv(abs(abs(abs(rijv)-0.5)-0.5)>1e-8)=10; % set entries to 10 unless close to -1,0,+1
        k=find(all(round(rijv)==nzpattv,1),1); % round to integers and find a match
        if isempty(k)
            error('cannot find match for (%d,%d)');
        else
            trmap(i,j)=k;
        end
        if j<=3
            icol=mod(find([nzpattc(:,i)==1 & nzpattc(:,k)==2;nzpattc(:,i)==2 & nzpattc(:,k)==3],1)-1,3)+1; % find the column to zero an element
            if ~isempty(icol)
                irow=(1:3)*(~nzpatt(:,icol,i) & nzpatt(:,icol,k)); % find zero that disappears
                jrow=6-j-irow; % find other row involved in rotation
                zel(:,j,i)=[3*icol-3+[irow; jrow]; mod(j-irow+1,3)-1; sign(nzpatt(jrow,icol,i))];
            end
        end
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% print zel
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fid=fopen('zel.txt','w');
fprintf(fid,'zel=reshape([');
for i=1:52
    if i>1
    fprintf(fid,';\n     ');
    end
    fprintf(fid,' %2d',zel(:,:,i));
end
fprintf(fid,']'',4,3,52);\n');
fclose(fid);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% print trmap
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fid=fopen('trmap.txt','w');
fprintf(fid,'trmap=[');
for i=1:52
    if i>1
    fprintf(fid,';\n     ');
    end
    fprintf(fid,' %2d',trmap(i,:));
end
fprintf(fid,'];\n');
fclose(fid);


% The OxyGEN proyect by Profoty.xyz
% Together against the Covid-19
% V4.0 rev.17/03/2020
%
% Website/blog: oxygen.protofy.xyz
% Contact: oxygen@protofy.xyz

%GEOMETRICAL PARAMETERS

clear;

%Length from the bearing to the hinge
l0 = 27.5;

%Length of the vertical wall that supports the hinge
h0 = 6.5;

%Lenght of the vertical support of the bearing
hb = 4.0;

%Bearing radius
br = 1.1;

%Array of different max and min positions of the bearing
h1a = [6.5, 7.1, 7.7, 8.3];
h2a = [20.5, 20.5, 20.5, 20.5];

%Number of cycles per cam turn (1 to 3).
nc = 1;

%Minimum radius of the camshaft
rmin = 5;

%BREATHING CYCLE PARAMETERS

%Duration of the inhale cycle / duration of the whole cycle
lambda1 = 0.500;
lambda2 = 0.060;

%Soft transition between inhale and exhale cycles
dpsi21 = 0.12;
dpsi12 = 0.15;

%Adjust parameters of the inhale curve
ga1 = pi;
gb1 = 1.2;
ff1 = 20;
sp1 = 1;

%Adjust parametes of the exhale curve
ga2 = 9;
gb2 = .5;
ff2 = 20;

%GENERATION OF THE CURVES AND THE CAMSHAFT

%Generation of the geometry

l = sqrt(l0^2+hb^2);

for i = 1:numel(h1a);

    ymin(i) = h1a(i);
    ymax(i) = h2a(i);

    alphamin(i) = acos((h0 - h1a(i)) / l);
    alphamax(i) = acos((h0 - h2a(i)) / l);

    xmin(i) = l*sin(alphamin(i));
    xmax(i) = l*sin(alphamax(i));

    d(i) = sqrt((xmin(i)-xmax(i))^2 + (ymin(i)-ymax(i))^2);

    alphatan(i) = (alphamin(i) + alphamax(i)) / 2;

    xtan(i) = l*sin(alphatan(i));
    ytan(i) = h0 - l*cos(alphatan(i));

    xsup(i) = xtan(i) + (d(i)/2)*cos(alphatan(i));
    xinf(i) = xtan(i) - (d(i)/2)*cos(alphatan(i));

    ysup(i) = ytan(i) + (d(i)/2)*sin(alphatan(i));
    yinf(i) = ytan(i) - (d(i)/2)*sin(alphatan(i));

    xcam(i) = xtan(i) + (d(i)/2 + rmin + br)*cos(alphatan(i));
    ycam(i) = ytan(i) + (d(i)/2 + rmin + br)*sin(alphatan(i));

end;

%Time increment
dt = 0.01;

%time coordinate during the whole cycle
theta = 0:dt:2*pi;

%Generation of the soft transition between inhale and exhale curves
psi1 = [];
psi2 = [];

for i = 1:numel(theta);

    if theta(i) < (lambda2-dpsi21/2)*2*pi;
        psi1(i) = 0;

    elseif theta(i) < (lambda2+dpsi21/2)*2*pi;
        psi1(i) = 0.5 + 0.5*cos((theta(i)-(lambda2+dpsi21/2)*2*pi)/(2*dpsi21));

    else theta(i) < (lambda1-dpsi12/2)*2*pi;
        psi1(i) = 1;

    end;

    psi2(i) = 1 - psi1(i);

end;

%Generation of the complete breathing cycle rho(theta)

rho = [];
rho1 = [];
rho2 = [];
rho2next = [];

rhomin = 1000;
rhomax = 0;

for i = 1:numel(theta);

    %Inhale curve
    rho1(i) = ff1*normpdf(sp1*theta(i), ga1, gb1);
    rho1next(i) = ff1*normpdf(sp1*theta(i)+sp1*2*pi, ga1, gb1);

    %Exhale curve
    rho2(i) = ff2*gampdf(theta(i), ga2, gb2);
    rho2next(i) = ff2*gampdf(theta(i)+2*pi, ga2, gb2);

    rho(i) = max(rho1(i), rho1next(i));

    %Capturing min and max in order to generate the normalized curve
    if rho(i) > rhomax
        rhomax = rho(i);
    end;

    if rho(i) < rhomin
        rhomin = rho(i);
    end;

end;

%Generation of a normalised curve and camshaft

rhonorm = [];
rhocam = [];

for i = 1:numel(theta);

    rhonorm(i) = (rho(i)-rhomin)/(rhomax-rhomin);

    for n = 1:numel(d)

        rhocam(n,i) = rmin + rhonorm(i)*d(n);

    end;

end;

%Generation of the first derivate of the camshaft geometry to analize and
%validate the design

drho = [];
drhonorm = [];
drhocam = [];

a = rmin;
b = 1/dt;

for i = 1 : numel(rho)-1;

    drho(i) = b*(rho(i+1)-rho(i));
    drhonorm(i) = b*(rhonorm(i+1)-rhonorm(i));
    drhocam(i) = b*(rhocam(i+1)-rhocam(i)) + a;

end;

drho(numel(rho)) = b*(rho(1)-rho(numel(rho)));
drhonorm(numel(rhonorm)) = b*(rhonorm(1)-rhonorm(numel(rhonorm)));
drhocam(numel(rhocam)) = b*(rhocam(1)-rhocam(numel(rhocam))) + a;

%X and Y coordinates during two cycles (for 2-cycle camshaft plot)

theta2 = [theta/2, (2*pi+theta)/2];

rhocam2 = [rhocam, rhocam];
drhocam2 = [drho, drho];

%X and Y coordinates during three cycles (for 3-cycle camshaft plot)

theta3 = [theta/3, (2*pi+theta)/3, (4*pi+theta)/3];

rhocam3 = [rhocam, rhocam, rhocam];
drhocam3 = [drho, drho, drho];

%GENERATION OF THE PLOTS

%Trying to print it out in real size (FAIL)
%set(gcf,'PaperUnits','centimeters');
%set(gcf,'PaperSize',[42 29.7]);

fpos = figure('Name', 'Dimensions', 'Units', 'centimeters', 'NumberTitle', 'off');
set(gcf,'PaperUnits','centimeters', 'PaperSize', [42/2 27.9/2]);
set(gcf, 'units', 'centimeters', 'position', [0, 0, 28, 20]);
fpos = gcf;
hold on

xlim([-5 35]);
ylim([0 30]);

for i = 1:numel(xmin)

    plot([0, xmin(i)], [h0, ymin(i)], '-x')
    plot([0, xmax(i)], [h0, ymax(i)], '-x')

    plot([0, xtan(i)], [h0, ytan(i)], '--xb')

    plot([xsup(i), xinf(i)], [ysup(i), yinf(i)], '--xr')
    scatter(xcam(i), ycam(i))

end;

hold off

%Plot of the breathing cycle interpolation by curves

fcycle = figure('Name', 'Breath Cycle curves', 'Units', 'centimeters', 'NumberTitle', 'off');

hold on
plot(theta, psi1, '-k')
plot(theta, psi2, '-k')
plot(theta, rho1, '-g')
plot(theta, rho1next, '-g')
plot(theta, rho2, '-g')
plot(theta, rho2next, '-g')
%plot(theta, drho, '-r')
plot(theta, rho, '-b')
hold off

set(gcf,'PaperUnits','centimeters', 'PaperSize', [42 27.9]);
set(gcf, 'units', 'centimeters', 'position', [0, 0, 27.9, 27.9]);
fcam3 = gcf;

%Plot of the normalized breathing cycle and first derivate

fnorm = figure('Name', 'Normalized Breath Cycle', 'Units', 'centimeters', 'NumberTitle', 'off');

hold on
%plot(theta, drhonorm, '-r')
plot(theta, rhonorm, '-b')
hold off

set(gcf,'PaperUnits','centimeters', 'PaperSize', [42/2 27.9/2]);
set(gcf, 'units', 'centimeters', 'position', [0, 0, 27.9, 27.9]);
fcam3 = gcf;

%Plot of a 1-cycle camshaft

for n = 1:numel(d)

    fcam1 = figure('Name', '1-Cycle Camshaft', 'Units', 'centimeters', 'NumberTitle', 'off');
    %hold on

    %polarplot(theta, drhocam, '-r')
    polar(theta, rhocam(n,:), '-b')

    %rlim([0 25]);
    hold off

    set(gcf,'PaperUnits','centimeters', 'PaperSize', [42 27.9]);
    set(gcf, 'units', 'centimeters', 'position', [0, 0, 27.9, 27.9]);
    fcam3 = gcf;

end

% %Plot of a 2-cycle camshaft
%
% fcam2 = figure('Name', '2-Cycle', 'Units', 'centimeters', 'NumberTitle', 'off');
%
% %hold on;
% %polar(theta, drhocam2, '-r')
% polar(theta2, rhocam2, '-b')
% %hold off;
%
% set(gcf,'PaperUnits','centimeters', 'PaperSize', [42 27.9]);
% set(gcf, 'units', 'centimeters', 'position', [0, 0, 27.9, 27.9]);
% fcam3 = gcf;
%
%
% %Plot of a 3-cycle camshaft
%
% fcam3 = figure('Name', '3-Cycle', 'Units', 'centimeters', 'NumberTitle', 'off');
%
% %hold on;
% %polar(theta,drhocam3)
% polar(theta3, rhocam3)
% %hold off;
%
% set(gcf,'PaperUnits','centimeters', 'PaperSize', [42 27.9]);
% set(gcf, 'units', 'centimeters', 'position', [0, 0, 27.9, 27.9]);
% fcam3 = gcf;

% The OxyGEN proyect by Profoty.xyz
% Together against the Covid-19
% V4.0 rev.17/03/2020
%
% Website/blog: oxygen.protofy.xyz
% Contact: oxygen@protofy.xyz


function [params, result] = ortho_default(params, W, w)
% Default DSS orthogonalization function
%   W = orthof(W)     for symmetric dss
%   w = orthof(W, w)  for deflation dss
%     W Matrix with projection vectors as rows. For deflation
%       algorithm only previously calculated projections are given.
%     w Currently iterated projection. Only for deflation algorithm.

% Copyright (C) 2004, 2005 DSS MATLAB package team (dss@cis.hut.fi).
% Distributed by Laboratory of Computer and Information Science,
% Helsinki University of Technology. http://www.cis.hut.fi/projects/dss/.
% $Id$

if nargin<2
    params.name = 'Default orthogonalization';
    params.description = 'Description of this function.';
    return;
end

if nargin>2
  % per component orthogonalization
  w = w - W' * W * w;
  result = w / norm(w);
else
  % symmetric orthogonalization
  W = real(inv(W * W')^(1/2))' * W;
  result = W;
end


function value = p05_exact ( dim_num )

%*****************************************************************************80
%
%% P05_EXACT returns the exact integral for problem 05.
%
%  Discussion:
%
%    The exact value is given only for DIM_NUM = 1, 2, 3, 4 or 5.
%    For other cases, the value R8_HUGE is returned instead.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 March 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Output, real VALUE, the exact value of the integral,
%    or R8_HUGE if the exact value is not known.
%
  if ( dim_num == 1 )

    value = log ( 3.0 );

  elseif ( dim_num == 2 )

    value = 5.0 * log ( 5.0 ) - 6.0 * log ( 3.0 );

  elseif ( dim_num == 3 )

    value = 0.5 * ( 49.0 * log ( 7.0 ) ...
      - 75.0 * log ( 5.0 ) + 27.0 * log ( 3.0 ) );

  elseif ( dim_num == 4 )

    value = 225.0 * log ( 3.0 ) + 125.0 * log ( 5.0 ) ...
      - 686.0 * log ( 7.0 ) / 3.0;

  elseif ( dim_num == 5 )

    value = ( ...
      - 65205.0 * log ( 3.0 ) ...
      - 6250.0 * log ( 5.0 ) ...
      + 24010.0 * log ( 7.0 ) ...
      + 14641.0 * log ( 11.0 ) ) / 24.0;

  else

    value = r8_huge ( );

  end

  return
end


function  [yz, dy] = pzextr1(iest, xest, yest)
% pzextr.m  uses extrapolation to evaluate nv functions at x=0 by fitting a
% polynomial to a sequence of estimates with progressively smaller values x= xest(1..np), and
% corresponding function vectors yest(1..nv,1..np). This call is number iest in the sequence
% of calls, see also bsstep1.m and pzextr.m
%
% output : the extrapolated function values, yz(1..nv,1..np) and their estimated error, dy(1..nv,1..np).

% D Vangheluwe 8 mrt 2005

global x_bulirsch_stoer  d_bulirsch_stoer

[nv np] = size(yest);

if np > 1
  x_bulirsch_stoer(iest,:) = xest;
% save current independent variable
  yz = yest;
  dy = yz;

%  x_bulirsch_stoer
%  d_bulirsch_stoer

  if iest == 1
% store first estimate in first column
     d_bulirsch_stoer(:,:,1) = yest;
  else
     c = yest;
     for k1 = 1:iest-1
        delta = 1 ./ (x_bulirsch_stoer(iest-k1,:) - xest);
        f1 = xest .* delta;
        f2 = x_bulirsch_stoer(iest-k1,:) .* delta;
        q = d_bulirsch_stoer(:,:,k1);
        d_bulirsch_stoer(:,:,k1) = dy;
        deltay = c - q;
        dy = repmat(f1, nv, 1) .* deltay;
        c = repmat(f2, nv, 1) .* deltay;
        yz = yz + dy;
     end
     d_bulirsch_stoer(:,:,iest) = dy;
  end

else  %np == 1

  x_bulirsch_stoer(iest) = xest';
% save current independent variable
  yz = yest;
  dy = yz;

%  x_bulirsch_stoer
%  d_bulirsch_stoer
  if iest == 1
% store first estimate in first column
     d_bulirsch_stoer(:,1) = yest;
  else
     c = yest;
     for k1 = 1:iest-1
        delta = 1 ./ (x_bulirsch_stoer(iest-k1) - xest);
        f1 = xest .* delta;
        f2 = x_bulirsch_stoer(iest-k1) .* delta;
        q = d_bulirsch_stoer(:,k1);
        d_bulirsch_stoer(:,k1) = dy;
        deltay = c - q;
        dy = f1 .* deltay;
        c = f2 .* deltay;
        yz = yz + dy;
     end
     d_bulirsch_stoer(:,iest) = dy;
  end

end  %np > 1


%Naive fanout
%define function from w to x
w=[];
x = tracer(w,'w -> x','dx -> dw','dw -> dx');

%define function from x to s
y = tracer(x,'x -> y','dy -> dx','dx -> dy');  % this does function composition
z = tracer(x,'x -> z','dz -> dx','dx -> dz');  % this does another composition
s = sum_of_functions(w,[1,1],y,z); % this does not do composition

fprintf('naive case: function value:\n');
[v,deriv] = s(1);

fprintf('naive case: gradient:\n');
[g,hess] = deriv(v);
fprintf('naive case: jacobian:\n');
hess(1);
fprintf('\n');

%Correct fanout
w=[];
x = tracer(w,'w -> x','dx -> dw','dw -> dx');

xx=[];
y = tracer(xx,'x -> y','dy -> dx','dx -> dy'); %no composition
z = tracer(xx,'x -> z','dz -> dx','dx -> dz'); %no composition
s = sum_of_functions(x,[1,1],y,z); %this does the composition once

fprintf('corrected: function value:\n');
[v,deriv] = s(1);

fprintf('corrected: gradient:\n');
[g,hess] = deriv(v);
fprintf('corrected: jacobian:\n');
hess(1);
fprintf('\n');


function tf = ispolycw_N(x, y)
%ISPOLYCW_N True if polygon vertices are in clockwise order
%
%   TF = ISPOLYCW(X, Y) returns true if the polygonal contour vertices
%   represented by X and Y are ordered in the clockwise direction.  X and Y
%   are numeric vectors with the same number of elements.
%
%   Alternatively, X and Y can contain multiple contours, either in
%   NaN-separated vector form or in cell array form.  In that case,
%   ISPOLYCW returns a logical array containing one true or false value
%   per contour.
%
%   ISPOLYCW always returns true for polygonal contours containing two or
%   fewer vertices.
%
%   Vertex ordering is not well defined for self-intersecting polygonal
%   contours.  For such contours, ISPOLYCW returns a result based on the
%   order or vertices immediately before and after the left-most of the
%   lowest vertices.  In other words, of the vertices with the lowest Y
%   value, find the vertex with the lowest X value.  For a few special
%   cases of self-intersecting contours, the vertex ordering cannot be
%   determined using only the left-most of the lowest vertices; for these
%   cases, ISPOLYCW uses a signed area test to determine the ordering.
%
%   Class Support
%   -------------
%   X and Y may be any numeric class.
%
%   Example
%   -------
%   Orientation of a square:
%
%       x = [0 1 1 0 0];
%       y = [0 0 1 1 0];
%       ispolycw(x, y)                     % Returns 0
%       ispolycw(fliplr(x), fliplr(y))     % Returns 1
%
%   See also POLY2CW, POLY2CCW, POLYBOOL.

% Copyright 2004-2009 The MathWorks, Inc.
% $Revision: 1.1.4.3 $  $Date: 2009/08/11 15:44:28 $

if isempty(x)
   tf = true;
   return;
end

if ~iscell(x)
   checkxy_N(x, y, mfilename, 'X', 'Y', 1, 2)
   is_row = (size(x,1) == 1);
   [x, y] = polysplit_N(x, y);
   if is_row
      x = x';
      y = y';
   end
end

tf = false(size(x));
for k = 1:numel(x)
   tf(k) = isContourClockwise(x{k}, y{k});
end

%----------------------------------------------------------------------
function tf = isContourClockwise(x, y)

if numel(x) <= 1
    tf = true;
    return;
end

is_closed = (x(1) == x(end)) && (y(1) == y(end));
if is_closed
    x(end) = [];
    y(end) = [];
end

[x, y] = removeDuplicates(x, y);
num_vertices = numel(x);
if num_vertices <= 2
    tf = true;
    return;
end

idx = findExtremeVertices(x, y);

if numel(idx) > 1
    % The same extreme vertex appears multiple, nonsuccessive times in
    % the vertex list.  Use signed area test.
    tf = signedArea(x, y) <= 0;
    return;
end

% Find the three vertices we are interested in: the left-most of the
% lowest vertices, as well as the ones immediately before and after it.
p = mod((idx - 1) + [-1, 0, 1], num_vertices) + 1;
xx = x(p);
yy = y(p);

if ~isfloat(xx)
    xx = double(xx);
end
if ~isfloat(yy)
    yy = double(yy);
end

ux = xx(2) - xx(1);
uy = yy(2) - yy(1);

vx = xx(3) - xx(2);
vy = yy(3) - yy(2);

a = ux*vy;
b = uy*vx;
if a == b
    % The left-most lowest vertex is the end-point of a kind of linear
    % "spur."  The contour doubles back on itself, such as in this case:
    % x = [0 1 1 0 0 -1 0];
    % y = [0 0 1 1 0 -1 0];
    % The left-most lowest vertex is (-1,-1), but we since this vertex
    % is the end-point of a spur, we can't tell the direction from it.
    % Use the signed polygon test.
    tf = signedArea(x, y) <= 0;
else
    tf = a < b;
end

%----------------------------------------------------------------------
function [xout, yout] = removeDuplicates(x, y)
num_vertices = numel(x);
k1 = [2:num_vertices 1];
k2 = 1:num_vertices;
dups = (x(k1) == x(k2)) & (y(k1) == y(k2));
xout = x;
yout = y;
xout(dups) = [];
yout(dups) = [];

%----------------------------------------------------------------------
function idx = findExtremeVertices(x, y)
% Return the indices of all the left-most lowest vertices in (x,y).

% Find the vertices with the minimum y.
idx = find(y == min(y));

x_subset = x(idx);
idx2 = (x_subset == min(x_subset));

idx = idx(idx2);

%----------------------------------------------------------------------
function a = signedArea(x, y)
% a = signedArea(x,y) returns twice the signed area of the polygonal
% contour represented by vectors x and y.  Assumes (x,y) is NOT closed.

% Reference:
% http://geometryalgorithms.com/Archive/algorithm_0101/algorithm_0101.htm

x = x - mean(x);
n = numel(x);
if n <= 2
    a = 0;
else
    i = [2:n 1];
    j = [3:n 1 2];
    k = (1:n);
    a = sum(x(i) .* (y(j) - y(k)));
end


function legendre_associated_normalized_values_test ( )

%*****************************************************************************80
%
%% LEGENDRE_ASSOCIATED_NORMALIZED_VALUES_TEST tests LEGENDRE_ASSOCIATED_NORMALIZED_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 September 2010
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LEGENDRE_ASSOCIATED_NORMALIZED_VALUES_TEST:\n' );
  fprintf ( 1, '  LEGENDRE_ASSOCIATED_NORMALIZED_VALUES stores values of\n' );
  fprintf ( 1, '  the normalized associated Legendre polynomials.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     N     M    X             P(N,M)(X)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, n, m, x, fx ] = legendre_associated_normalized_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %4d  %4d  %12f  %24.16f\n', n, m, x, fx );

  end

  return
end


function v=shear_bulk2poisson(mu,k)

v=((3.*k)-(2.*mu))./((6.*k)+(2.*mu));

end

%%
% _*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 [y,t,optw,W,C,confb95,yb] = sskernel(x,tin,W)
% [y,t,optw,W,C,confb95,yb] = sskernel(x,t,W)
%
% Function `sskernel' returns an optimized kernel density estimate
% using a Gauss kernel function.
%
% Examples:
% >> x = 0.5-0.5*log(rand(1,1e3)); t = linspace(0,3,1000);
% >> [y,t,optw] = sskernel(x,t);
% This example produces a vector of kernel density estimates, y, at points
% specified in a vector t, using an optimized bandwidth, optw (a standard
% deviation of a normal density function).
%
% >> sskernel(x);
% By calling the function without output arguments, the estimated density
% is displayed along with 95% bootstrap confidence intervals.
%
% Input arguments:
% x:    Sample data vector.
% tin (optinal):
%       Points at which estimation are computed. Please use fine resolution
%       to obtain a correct optimal bandwidth.
% W (optinal):
%       A vector of kernel bandwidths.
%       If W is provided, the optimal bandwidth is selected from the
%       elements of W.
%       * Do not search bandwidths smaller than a sampling resolution of data.
%       If W is not provided, the program searches the optimal bandwidth
%       using a golden section search method.
%
% Output arguments:
% y:    Estimated density
% t:    Points at which estimation was computed.
%       The same as tin if tin is provided.
%       (If the sampling resolution of tin is smaller than the sampling
%       resolution of the data, x, the estimation was done at smaller
%       number of points than t. The results, t and y, are obtained by
%       interpolating the low resolution sampling points.)
% optw: Optimal kernel bandwidth.
% W:    Kernel bandwidths examined.
% C:    Cost functions of W.
% conf95:
%       Bootstrap confidence intervals.
% yb:   Booststrap samples.
%
%
% Usage:
% >> [y,t,optw] = sskernel(x);
% When t is not given in the input arguments, i.e., the output argument t
% is generated automatically.
%
% >> W = linspace(0.01,1,20);
% >> [y,t,optw] = sskernel(x,t,W);
% The optimal bandwidth is selected from the elements of W.
%
% >> [y,t,optw] = sskernel(x,t,0.1);
% If the density estimate with a given bandwidth, simply put a scalar value
% as W. The computation is faster than the built-in function, ksdensity.
%
% >> [y,t,optw,confb95,yb] = sskernel(x);
% This additionally computes 95% bootstrap confidence intervals, confb95.
% The bootstrap samples are provided as yb.
%
%
% Optimization principle:
% The optimal bandwidth is obtained as a minimizer of the formula,
% sum_{i,j} \int k(x - x_i) k(x - x_j) dx  -  2 sum_{i~=j} k(x_i - x_j),
% where k(x) is the kernel function, according to
%
% Hideaki Shimazaki and Shigeru Shinomoto
% Kernel Bandwidth Optimization in Spike Rate Estimation
% Journal of Computational Neuroscience 2010
% http://dx.doi.org/10.1007/s10827-009-0180-4
%
% The above optimization is based on a principle of minimizing
% expected L2 loss function between the kernel estimate and an unknown
% underlying density function. An assumption is merely that samples
% are drawn from the density independently each other.
%
% For more information, please visit
% http://2000.jukuin.keio.ac.jp/shimazaki/res/kernel.html
%
% See also SSVKERNEL, SSHIST
%
% Bug fix
% 131004 fixed a problem for large values
%
% Hideaki Shimazaki
% http://2000.jukuin.keio.ac.jp/shimazaki

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Parameters Settings
x = reshape(x,1,numel(x));

if nargin == 1
    T = max(x) - min(x);
    [mbuf,nbuf,dt_samp] = find( sort(diff(sort(x))),1,'first');
    tin = linspace(min(x),max(x), min(ceil(T/dt_samp),1e3));
    t = tin;
    x_ab = x( logical((x >= min(tin)) .*(x <= max(tin))) ) ;
else
    T = max(tin) - min(tin);
    x_ab = x( logical((x >= min(tin)) .*(x <= max(tin))) ) ;
    [mbuf,nbuf,dt_samp] = find( sort(diff(sort(x_ab))),1,'first');

    if dt_samp > min(diff(tin))
        t = linspace(min(tin),max(tin), min(ceil(T/dt_samp),1e3));
    else
        t = tin;
    end
end

dt = min(diff(t));

% Create a finest histogram
y_hist = histc(x_ab,t-dt/2);
L = length(y_hist);
N = sum(y_hist);
y_hist = y_hist/N/dt;   %density

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compute a Cost Function

if nargin >= 3
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Global search
C = zeros(1,length(W));
C_min = Inf;

for k = 1: length(W)
	w = W(k);
    [C(k) yh] = CostFunction(y_hist,N,w,dt);

    if C(k) < C_min
        C_min = C(k);
        optw = w;
        y = yh;
    end
end

else
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Golden section search on a log-exp scale
% Initialize
Wmin = 2*dt; Wmax = 1*(max(x) - min(x));

tol = 10^-5;
phi = (sqrt(5) + 1)/2;        %golden ratio
%logexp = @(x) log(1+exp(x));
%ilogexp = @(x) log(exp(x)-1);

%a = Wmin; b = Wmax;
a = ilogexp(Wmin); b = ilogexp(Wmax);

c1 = (phi-1)*a + (2-phi)*b;
c2 = (2-phi)*a + (phi-1)*b;

f1 = CostFunction(y_hist,N,logexp(c1),dt);
f2 = CostFunction(y_hist,N,logexp(c2),dt);

k = 1;
while abs(b-a) > tol*(abs(c1)+abs(c2)) && k <= 20
	if (f1 < f2)
        b = c2;
        c2 = c1;

        c1 = (phi - 1)*a + (2 - phi)*b;

        f2 = f1;
        [f1 yh1] = CostFunction(y_hist,N,logexp(c1),dt);

        W(k) = logexp(c1);
        C(k) = f1;
        optw = logexp(c1);
        y = yh1./sum(yh1.*dt);  %make the final output a density
    else
        a = c1;
        c1 = c2;

        c2 = (2 - phi)*a + (phi - 1)*b;

        f1 = f2;
        [f2 yh2] = CostFunction(y_hist,N,logexp(c2),dt);

        W(k) = logexp(c2);
        C(k) = f2;
        optw = logexp(c2);
        y = yh2./sum(yh2.*dt);
    end

    k = k + 1;
end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Bootstrap Confidence Intervals
if nargout == 0 || nargout >= 6
    nbs = 1e3;        %number of bootstrap samples
    yb = zeros(nbs,length(tin));

    for i = 1: nbs,
        %y_histb = poissrnd(y_hist*dt*N)/dt/N;

        idx = ceil(rand(1,N)*N);
        xb = x_ab(idx);
        y_histb = histc(xb,t-dt/2)/dt/N;

        yb_buf = fftkernel(y_histb,optw/dt);
        yb_buf = yb_buf / sum(yb_buf*dt);

        yb(i,:) = interp1(t,yb_buf,tin);
    end

    ybsort = sort(yb);
    y95b = ybsort(floor(0.05*nbs),:);
    y95u = ybsort(floor(0.95*nbs),:);

    confb95 = [y95b; y95u];
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Return results
y = interp1(t,y,tin);
t = tin;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Display results
if nargout == 0
            hold on;

            line([t; t],[y95b; y95u]...
                    ,'Color',[7 7 7]/8,'LineWidth',1 );
            plot(t,y95b,'Color',[7 7 7]/9,'LineWidth',1);
            plot(t,y95u,'Color',[7 7 7]/9,'LineWidth',1);

            plot(t,y,'Color',[0.9 0.2 0.2],'LineWidth',2);

            grid on;
            ylabel('density');
            set(gca,'TickDir','out');
else
    if nargin >= 4
        if strcmp(option,'Visible')
            hold on;

            if nargout >= 6
                line([t; t],[y95b; y95u]...
                    ,'Color',[7 7 7]/8,'LineWidth',1 );
                plot(t,y95b,'Color',[7 7 7]/9,'LineWidth',1);
                plot(t,y95u,'Color',[7 7 7]/9,'LineWidth',1);
            end

            plot(t,y,'Color',[0.9 0.2 0.2],'LineWidth',1);

            grid on;
            ylabel('density');
            set(gca,'TickDir','out');

        end
    end
end

function [C yh] = CostFunction(y_hist,N,w,dt)
yh = fftkernel(y_hist,w/dt);  %density

%formula for density
C = sum(yh.^2)*dt - 2* sum(yh.*y_hist)*dt...
        + 2*1/sqrt(2*pi)/w/N;
C = C * N* N;

%formula for rate
%C = dt*sum( yh.^2 - 2*yh.*y_hist + 2/sqrt(2*pi)/w*y_hist );

function y = fftkernel(x,w)
% y = fftkernel(x,w)
%
% Function `fftkernel' applies the Gauss kernel smoother to
% an input signal using FFT algorithm.
%
% Input argument
% x:    Sample signal vector.
% w: 	Kernel bandwidth (the standard deviation) in unit of
%       the sampling resolution of x.
%
% Output argument
% y: 	Smoothed signal.
%
% MAY 5/23, 2012 Author Hideaki Shimazaki
% RIKEN Brain Science Insitute
% http://2000.jukuin.keio.ac.jp/shimazaki

L = length(x);
Lmax = max(1:L+3*w);
n = 2^(nextpow2(Lmax));

X = fft(x,n);

f = (0:n-1)/n;
f = [-f(1:n/2+1) f(n/2:-1:2)];

K = exp(-0.5*(w*2*pi*f).^2);

y = ifft(X.*K,n);

y = y(1:L);

function y = logexp(x)
if x<1e2
    y = log(1+exp(x));
else
    y = x;
end

function y = ilogexp(x)
%ilogexp = @(x) log(exp(x)-1);
if x<1e2
    y = log(exp(x)-1);
else
    y = x;
end


%--- help for trnd ---
%
% TRND   Random arrays from Student's t distribution.
%    R = TRND(V) returns an array of random numbers chosen from Student's t
%    distribution with V degrees of freedom.  The size of R is the size of
%    V.
%
%    R = TRND(V,M,N,...) or R = TRND(V,[M,N,...]) returns an M-by-N-by-...
%    array.
%
%    The t distribution with one degree of freedom is also known as the
%    Cauchy distribution.
%
%    See also TCDF, TINV, TPDF, TSTAT, NCTRND, RANDOM.
%
%    Reference page in Doc Center
%       doc trnd
%
%


function stroud_test01 ( )

%*****************************************************************************80
%
%% TEST01 tests BALL_F1_ND, BALL_F3_ND.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 April 2009
%
%  Author:
%
%    John Burkardt
%
  global FUNC_ND_INDEX;

  n_max = 3;
  num = function_nd_num ( );

  xc = [ 1.0, -1.0, 2.0 ];
  r = 2.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST01\n' );
  fprintf ( 1, '  For integrals in a ball in ND:\n' );
  fprintf ( 1, '  BALL_F1_ND approximates the integral;\n' );
  fprintf ( 1, '  BALL_F3_ND approximates the integral.\n' );
  fprintf ( 1, '\n' );

  for n = 2 : n_max

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Spatial dimension N = %d\n', n );
    fprintf ( 1, '  Ball center:\n' );
    for i = 1 : n
      fprintf ( 1, '%12f  ', xc(i) );
    end
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Ball radius = %f\n', r );
    fprintf ( 1, '  Ball volume = %f\n', ball_volume_nd ( n, r ) );
    fprintf ( 1, '\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, '    Rule:      F1          F3\n' );
    fprintf ( 1, '    F(X)\n' );
    fprintf ( 1, '\n' );

    for i = 1 : num

      FUNC_ND_INDEX = i;

      result1 = ball_f1_nd ( 'function_nd', n, xc, r );
      result2 = ball_f3_nd ( 'function_nd', n, xc, r );

      fname = function_nd_name ( i );

      fprintf ( 1, '  %7s  %12f  %12f\n', fname, result1, result2 );

    end

  end

  return
end


%% Mesh Generator for Tecplot
% Generating a 2D mesh for Tecplot for an axisymmetric diffuser.
% by Manuel Diaz, 2012.09.15.

clear all; close all;
%% Parameters
L0 = 0; L1 = 10 + L0; L2 = 6 + L1 + L0;
H0 = 0; H1 = 0.5;
theta = deg2rad(5); % theta = 5 degress converted to radians
H2 = H1 + L2*tan(theta);

%% Number of elements of edges (ne)
ne_x1 = 25; ne_x2 = 40; ne_y = 40;

%% End Points for the rectangular zones:
Z1=[ L0,H1 ; L1,H1 ; L1,H0 ; L0,H0 ]; % Zone1
Z2=[ L1,H1 ; L2,H2 ; L2,H0 ; L1,H0 ]; % Zone2

%% Zone 1:
% There are 4 edge lines, each having several edge points.
% We use the Function "Pinterpol" to create the edge points for zone 1.

E1 = Pinterpol(Z1(1,1),Z1(1,2),Z1(2,1),Z1(2,2),ne_x1);
E2 = Pinterpol(Z1(4,1),Z1(4,2),Z1(3,1),Z1(3,2),ne_x1);

% Now generating the mesh for Zone 1
M1 = zeros(ne_x1+1,ne_y+1,2);
for i = 1:(ne_x1+1);
    V = Pinterpol(E1(i,1),E1(i,2),E2(i,1),E2(i,2),ne_y);
    M1(i,1:(ne_y+1),1:2) = V; % mesh1
end

%% Zone 2:
% Similarly for zone 2
E1 = Pinterpol(Z2(1,1),Z2(1,2),Z2(2,1),Z2(2,2),ne_x2);
E2 = Pinterpol(Z2(4,1),Z2(4,2),Z2(3,1),Z2(3,2),ne_x2);

% generating the mesh for Zone 2
M2 = zeros(ne_x2+1,ne_y+1,2);
for i = 1:(ne_x2+1);
    V = Pinterpol(E1(i,1),E1(i,2),E2(i,1),E2(i,2),ne_y);
    M2(i,1:(ne_y+1),1:2) = V; % mesh2
end

%% To Tecplot
% Writing Output data for Tecplot format:

file = fopen('mesh1.tec','w');
% h1 gets the handel for the file "mesh1.tec".
% 'w' specifies that it will be written.
% similarly 'r' is for reading and 'a' for appending.

fprintf(file, 'TITLE = "Mesh from Matlab"\n');
fprintf(file, 'VARIABLES = "X" "Y"\n');
fprintf(file, 'ZONE T = "Inlet zone"\n');
fprintf(file, 'I = %d, J = %d, K = 1, F = POINT\n\n', ne_x1+1,ne_y+1);

for j = 1:(ne_y+1)
    for i = 1:(ne_x1+1)
        fprintf(file, '%f\t%f\n', M1(i,j,1),M1(i,j,2));
    end
end

fprintf(file, 'ZONE T="Diffuser zone"\n');
fprintf(file, 'I = %d, J=%d, k=1, F=POINT\n\n', ne_x2+1,ne_y+1);

for j = 1:(ne_y+1)
    for i = 1:(ne_x2+1)
        fprintf(file, '%f\t%f\n', M2(i,j,1),M2(i,j,2));
    end
end

fclose(file);


function [X,Y,Z]=cylinder2(x,y,z,r,N,o)
%
%   [X,Y,Z]=cylinder2(x,y,z,r,N,o)
%
%   Vectors x, y, z define the central line of cylindrical surface
%   and vector r defines the radius of cylindrical surface, and has
%   the same length as x, y, z.
%
%   N is the number of points around the circumference. The default value
%   is N = 100.
%
%   Matrix o defines ovality of the directrix (circle) of cylindrical
%   surface. Its elements take values between 0 and 1 and apply to the
%   primary and secondary axis of directrix. The defeault values are
%   o=ones(length(x),2).
%
%   Matrices X, Y, Z define the cylindrical surface, surf(X,Y,Z) displays
%   the cylindrical surface.
%
%   Example 1: Horn
%
%              f=linspace(0,4*pi,100);
%              x=cos(f); y=sin(f); z=f; r=-1/99*([1:100]-1)+1;
%              [X,Y,Z]=cylinder2(x,y,z,r);
%
%   Example 2: Pot
%
%              x=[0 0 1 1 .5 .5]; y=[0 0 3 3 1.5 1.5];
%              z=[0 0 2 2 1 1];   r=[0 1 1.5 1.2 1 0];
%              [X,Y,Z]=cylinder2(x,y,z,r,6); view([-150 25])
%
%   Example 3: Plumbing
%
%              x=[0 0 0 0 0 0  1 1 1 1 1 1]; y=[0 .2 .2 1 1 .5 .5 1 1 .2 .2 0];
%              z=[1 1 1 1 0 0 0 0 1 1 1 1]; r=[.2 .2 .1 .1 .1 .1 .1 .1 .1 .1 .2 .2];
%              o=[1 1 .5 .5 .5 1 1 .5 .5 .5 1 1; 1 1 1 1 1 .5 .5 1 1 1 1 1];
%              [X,Y,Z]=cylinder2(x,y,z,r,8,o);
%
%
%   Example 4: Triangle , closed central line
%
%              f=linspace(0,2*pi,4);
%              x=cos(f-pi/6); y=sin(f-pi/6); z=zeros(4,1); r=0.2*ones(4,1);
%              [X,Y,Z]=cylinder3(x,y,z,r); view([0 90])
%
%
%
%   Copyright (c) 2011, Version 2.2
%   Avni Pllana <avniu66@hotmail.com>

X=[]; Y=[]; Z=[];

if nargin<5
    N=100;
end

x=x(:); y=y(:); z=z(:); r=r(:);

nx=length(x);
ny=length(y);
nz=length(z);
nr=length(r);

an=[nx ny nz nr];
if ~ismember(diff(an),[0 0 0],'rows')
    disp(' ')
    disp('x, y, z, r must have the same length!')
else
    if nx<2
        disp(' ')
        disp('The length of x must be greater than 1!')
    else

      if nargin<6
        o=ones(nx,2);
      else
        [o1,o2]=size(o);
        if o2>o1
            o=o';
        end
      end

      C=[x y z];
      D=diff(C);

      for i=1:nx-1
         if norm(D(i,:))>0
            E(i,:)=D(i,:)/norm(D(i,:));
         else
            E(i,:)=[0 0 0];
         end

      end

      if norm(C(1,:)-C(nx,:))<1e-10 && norm(cross(E(1,:),E(nx-1,:)))>0
         Closed=1;
         nxx=nx+1;
         E=[E;E(1,:)];
      else
         Closed=0;
         nxx=nx;
      end

      f=linspace(0,2*pi,N+1);
      for j=1:N+1
          xcc(j)=cos(f(j));
          ycc(j)=sin(f(j));
          zcc(j)=0;
      end

      for i=1:nx

          if i==1

              if ~Closed

                  d=[];
                  k=i;

                  while k<nx-1
                      if norm(cross(E(k,:),E(k+1,:)))==0
                          k=k+1;
                          d=[d k];
                      else
                          break
                      end

                  end

                  if norm(E(1,:))==0;
                      vz=E(2,:);
                  else
                      vz=E(1,:);
                  end

                  if k==nx-1
                     if norm(cross([0 0 1],vz))~=0
                         va=cross([0 0 1],vz);
                     else
                         va=cross([1 1 1],vz);
                     end
                  else
                     va=cross(E(k+1,:),vz);
                  end

                  vy=va/norm(va);
                  vx=cross(vy,vz);
                  V=[vx;vy;vz];
                  Vp=V;
                  Vpp=V;
                  alf=0;

              else

                  d=[];
                  vb=(E(1,:)+E(nx-1,:))/2;
                  vz=vb/norm(vb);
                  va=cross(E(1,:),E(nx-1,:));
                  vy=va/norm(va);
                  vx=cross(vy,vz);
                  V=[vx;vy;vz];
                  Vpp=V;

                  alf=acos(E(1,:)*E(nx-1,:)');

                  if abs(alf>3*pi/4)
                     alf=0;
                  end

              end

          else

             if ismember(i,d)
                 V=Vp;
                 alf=0;
             else
                    d=[];
                    k=i;

                    while k<nx-1
                      if norm(cross(E(k,:),E(k+1,:)))==0
                          k=k+1;
                          d=[d k];
                      else
                          break
                      end

                    end

                    if ~isempty(d) || i==nx
                          if norm(E(i-1,:))==0;
                              vz=E(i-2,:);
                          else
                              if i==nx
                                vz=E(i-1,:);
                              else
                                vz=E(i,:);
                              end
                          end

                          va=Vpp(2,:);
                          vy=va/norm(va);
                          vx=cross(vy,vz);
                          V=[vx;vy;vz];

                          Vp=V;
                          alf=0;

                    end

                    if i<nxx

                          vb=(E(i,:)+E(i-1,:))/2;
                          vz=vb/norm(vb);
                          va=cross(E(i,:),E(i-1,:));
                          vy=va/norm(va);
                          vx=cross(vy,vz);
                          V=[vx;vy;vz];

                          %%%%%%%%%%%%%
                          ang=acos(vy*Vpp(2,:)');
                          Vang=cross(vy,Vpp(2,:));
                          if Vang*vz'>0
                              ang=-ang;
                          end

                          A=[cos(ang) sin(ang); -sin(ang) cos(ang)];
                          xy=A*[xcc;ycc];
                          xcc=xy(1,:);
                          ycc=xy(2,:);
                          %%%%%%%%%%%%
                          Vpp=V;

                          alf=acos(E(i,:)*E(i-1,:)');

                         if abs(alf>3*pi/4)
                            alf=0;
                         end
                     end
             end

          end

          xc=o(i,1)*r(i)/cos(alf/2)*xcc;
          yc=o(i,2)*r(i)*ycc;
          xyz=[xc;yc;zcc];

          M=pinv(V);

          xyz1=M*xyz+repmat([x(i);y(i);z(i)],1,N+1);

          X(:,i)=xyz1(1,:)';
          Y(:,i)=xyz1(2,:)';
          Z(:,i)=xyz1(3,:)';

      end

       figure
       surf(X,Y,Z)
       shading interp
       camlight
       grid on
       axis equal

    end
end


% Data file PILZ3
% Forced damped vibrations of a mechanical
% system with one degree of freedom, contained
% a reverse pendulum.
  s     = 1; % degree of freedom
  L     = '2.5*qt1^2 - 1/2*c*q1^2 - 3.9*cos(10*q1)'; % Lagrangian
  QN{1} = '-k*qt1 + 10*sin(p*t)'; % generalized non potential force
  Tend  = 20;    % upper bound of integration
  qj0   = 0.05;  % initial coordinate
  qtj0  = 0;     % initial velocity
  eps   = 1e-10; % desirable accuracy
  np    = 3;     % number of parameters
  P{1}  = 'c';   % spring stiffness
  P{2}  = 'k';   % coefficient of damping
  P{3}  = 'p';   % disturbance frequency


function [sol,times,ocp] = bouncingball

  before_contact = ocl.Stage([], @before_contact_vars, @before_contact_ode, 'N', 3, 'd', 2);
  after_contact = ocl.Stage(1, @after_contact_vars, @after_contact_ode, ...
                            @after_contact_cost, 'N', 5, 'd', 2);

  before_contact.setInitialStateBounds('s', 1);
  before_contact.setInitialStateBounds('v', 0);
  before_contact.setEndStateBounds('s', 0);

  after_contact.setEndStateBounds('s', 1);

  ocp = ocl.MultiStageProblem({before_contact, after_contact}, {@stage_transition});

  [sol,times] = ocp.solve(ocp.getInitialGuess());

  % stage 1
  figure;
  subplot(1,2,1)
  hold on; grid on;
  ocl.plot(times{1}.states, sol{1}.states.s)
  ocl.plot(times{1}.states, sol{1}.states.v)
  legend({'s','v'})
  xlabel('time [s]');
  ylim([-5 3])
  yticks(-5:3)
  title('stage 1')

  % stage 2
  subplot(1,2,2)
  hold on; grid on;
  ocl.plot(times{2}.states, sol{2}.states.s)
  ocl.plot(times{2}.states, sol{2}.states.v)
  ocl.stairs(times{2}.controls, sol{2}.controls.F)
  legend({'s','v','F'})
  xlabel('time [s]');
  ylim([-5 3])
  yticks(-5:3)
  title('stage 2')

end

function before_contact_vars(sh)
  sh.addState('s');
  sh.addState('v');
end

function before_contact_ode(sh,x,~,~,~)
  sh.setODE('s', x.v);
  sh.setODE('v', -10);
end

function after_contact_vars(sh)
  sh.addState('s');
  sh.addState('v');
  sh.addControl('F');
end

function after_contact_ode(sh,x,~,u,~)
  sh.setODE('s', x.v);
  sh.setODE('v', -10 + 10*u.F);
end

function after_contact_cost(ch,~,~,u,~)
  ch.add( u.F^2 );
end

function stage_transition(ch, x0, xF)
  % x0 current stage
  % xF previous stage
  ch.add(x0.s, '==', xF.s);
  ch.add(x0.v, '==', -xF.v/2);
end


function [outR, outID] = makeBayesWeightedCorr1(Pr, bID, varargin)
%function [outR, outID] = makeBayesWeightedCorr1(Pr, bID, preComputedQ (optional))

outID = unique(bID);
h = histc(bID, outID);
if isempty(varargin)
    Q = makeQForWeightedCorr(h, size(Pr, 2));
else
    Q = varargin{1};
end

outR = zeros(length(outID), 1);
for i = 1:length(outR)
    outR(i) = makeWeightedCorr1(Q{h(i)}, reshape(Pr(bID == outID(i), :), [], 1));
end

end

function out = makeWeightedCorr1(xy, w);
%function out = makeWeightedCorr1(xy, w);

mxy = sum(xy.*repmat(w, 1, 2)/sum(w));
covxy = sum(w.*(xy(:, 1) - mxy(1)).*(xy(:, 2) - mxy(2)))/sum(w);
covxx = sum(w.*(xy(:, 1) - mxy(1)).*(xy(:, 1) - mxy(1)))/sum(w);
covyy = sum(w.*(xy(:, 2) - mxy(2)).*(xy(:, 2) - mxy(2)))/sum(w);
out = covxy/(sqrt(covyy*covxx));
end


function [ xnew, ferr, q ] = roots_rc ( n, x, fx, q )

%*****************************************************************************80
%
%% ROOTS_RC solves a system of nonlinear equations using reverse communication.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 January 2013
%
%  Author:
%
%    Original FORTRAN77 version by Gaston Gonnet.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Gaston Gonnet,
%    On the Structure of Zero Finders,
%    BIT Numerical Mathematics,
%    Volume 17, Number 2, June 1977, pages 170-183.
%
%  Parameters:
%
%    Input, integer N, the number of equations.
%
%    Input, real X(N).  Before the first call, the user should
%    set X to an initial guess or estimate for the root.  Thereafter, the input
%    value of X should be the output value of XNEW from the previous call.
%
%    Input, real FX(N), the value of the function at XNEW.
%
%    Workspace, real ( kind = 8 ) Q(2*N+2,N+2).  Before the first call
%    for a given problem, the user must set Q(2*N+1,1) to 0.0.
%
%    Output, real ( kind = 8 ) XNEW(N), a new point at which a function
%    value is requested.
%
%    Output, real ( kind = 8 ) FERR, the function error, that is, the sum of
%    the absolute values of the most recently computed function vector.
%
%    Workspace, real ( kind = 8 ) Q(2*N+2,N+2).
%
  ferr = sum ( abs ( fx(1:n) ) );
%
%  Initialization if Q(2*N+1,1) = 0.0.
%
  if ( q(2*n+1,1) == 0.0 )

    for i = 1 : n
      for j = 1 : n + 1
        q(i,j) = 0.0;
        q(i+1,j) = 0.0;
      end
      q(i,i) = 100.0;
      q(i+n,i) = 1.0;
    end

    q(2*n+1,1:n) = 1.0E+30;
    q(2*n+2,1:n) = n;

    for i = 1 : n
      q(i+n,n+1) = x(i);
    end

    q(1:n,n+1) = fx(1:n);

    q(2*n+1,n+1) = ferr;
    q(2*n+2,n+1) = 0.0;
    damp = 0.99;

  else

    jsus = 1;
    for i = 2 : n + 1
      if ( 2 * n <= q(2*n+2,i) )
        q(2*n+1,i) = 1.0E+30;
      end
      if ( q(2*n+2,jsus) < floor ( ( n + 3 ) / 2 ) )
        jsus = i;
      end
      if ( ( n + 3 ) / 2 <= q(2*n+2,i) && q(2*n+1,jsus) < q(2*n+1,i) )
        jsus = i;
      end
    end

    for i = 1 : n
      q(i+n,jsus) = x(i);
      q(i,jsus) = fx(i);
    end

    q(2*n+1,jsus) = ferr;
    q(2*n+2,jsus) = 0;
    jsma = 1;
    damp = 0.0;

    for j = 1 : n + 1
      if ( 1.0E+30 / 10.0 < q(2*n+1,j) )
        damp = 0.99;
      end
      if ( q(2*n+1,j) < q(2*n+1,jsma) )
        jsma = j;
      end
    end

    if ( jsma ~= n + 1 )
      for i = 1 : 2 * n + 2
        t = q(i,jsma);
        q(i,jsma) = q(i,n+1);
        q(i,n+1) = t;
      end
    end

  end

  q(1:n,n+2) = q(1:n,n+1);
%
%  Call the linear equation solver, which should not destroy the matrix
%  in Q(1:N,1:N), and should overwrite the solution into Q(1:N,N+2).
%
  q(1:n,n+2) = q(1:n,1:n) \ q(1:n,n+2);

  sump = sum ( q(1:n,n+2) );

  if ( abs ( 1.0 - sump ) <= 1.0E-10 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'ROOT - Fatal error!\n' );
    fprintf ( 1, '  SUMP almost exactly 1.\n' );
    fprintf ( 1, '  SUMP = %g\n', sump );
    error ( 'ROOTS_RC - Fatal error!' );
  end

  for i = 1 : n
    xnew(i) = q(i+n,n+1);
    for j = 1 : n
      xnew(i) = xnew(i) - q(i+n,j) * q(j,n+2);
    end
%
%  If system not complete, damp the solution.
%
    xnew(i) = xnew(i) / ( 1.0 - sump ) * ( 1.0 - damp ) ...
      + q(i+n,n+1) * damp;

  end

  for j = 1 : n + 1
    q(2*n+2,j) = q(2*n+2,j) + 1.0;
  end

  return
end


function pass = test_univariate( pref )
% Rank-1 PDEs can be solved by univariate spectral methods.  Do the results
% match.
% Alex Townsend, March 2013.

if ( nargin < 1 )
    pref = chebfunpref();
end
tol = 1000*pref.cheb2Prefs.chebfun2eps;

% Simple example in y-variable.
N = chebop2(@(u) diff(u,2,1) + u); N.dbc = 1; N.ubc = 1; u = N \ 0;
L = chebop(@(u) diff(u,2) + u); L.lbc = 1; L.rbc = 1; v = L \ 0;

pass(1) = (length(u) == 1);
pass(2) = (norm(u(0,:) - v) < tol);

% Simple example in x-variable.
N = chebop2(@(u) diff(u,2,2) + u); N.lbc = 1; N.rbc = 1; u = N \ 0;
L = chebop(@(u) diff(u,2) + u); L.lbc = 1; L.rbc = 1; v = L \ 0;

pass(3) = (length(u) == 1);
pass(4) = (norm(u(:,0) - v.') < tol);

%
% % Simple example in x-variable.
% N = chebop2(@(u) diff(diff(u,1,1),1,2) + diff(u,1,1));
% N.lbc = 1; N.dbc = 1; u = N \ 0;
%
% L = chebop(@(u) diff(u,2) + u); L.lbc = 1; L.rbc = 1; v = L \ 0;
%
% pass(j) = (length(u) == 1);  j = j+1;
% pass(j) = (norm(u(:,0) - v) < tol); j = j+1;


function M = shapefitfactory(VJt)
% Linear manifold structure for optimization over the ShapeFit search space
%
% function M = shapefitfactory(VJt)
%
% Input: VJt is a matrix of size dxn, such that VJt * ones(n, 1) = 0.
%
% Returns M, a structure describing the Euclidean space of d-by-n matrices
% equipped with the standard Frobenius distance and associated trace inner
% product, as a manifold for Manopt. Matrices on M, denoted by T, have size
% dxn and obey T*ones(n, 1) = 0 (centered columns) and <VJt, T> = 1, where
% <A, B> = Trace(A' * B).
%
% See this paper: http://arxiv.org/abs/1506.01437
% ShapeFit: Exact location recovery from corrupted pairwise directions, 2015
% Paul Hand, Choongbum Lee, Vladislav Voroninski
%
% See also: shapefit_smoothed

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, June 18, 2015.
% Contributors:
% Change log:

    [d, n] = size(VJt);

    M.name = @() sprintf('ShapeFit space of size %d x %d', d, n);

    M.dim = @() d*n - d - 1;

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

    M.norm = @(x, d) norm(d, 'fro');

    M.dist = @(x, y) norm(x-y, 'fro');

    M.typicaldist = @() sqrt(d*n);

    M.proj = @(T, U) projection(U);
    VJt_normed = VJt / norm(VJt, 'fro');
    function PU = projection(U)
        % Center the columns
        PU = bsxfun(@minus, U, mean(U, 2));
        % Remove component along VJt
        % Note: these two actions can be executed separately, without
        % interference, owing to VJt having centered columns itself.
        PU = PU - (VJt_normed(:)'*U(:))*VJt_normed;
    end

    M.egrad2rgrad = M.proj;

    M.ehess2rhess = @(x, eg, eh, d) projection(eh);

    M.tangent = @(x, d) d;

    M.exp = @exp;
    function y = exp(x, d, t)
        if nargin == 3
            y = x + t*d;
        else
            y = x + d;
        end
    end

    M.retr = M.exp;

	M.log = @(x, y) y-x;

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

    M.randvec = @(x) randvec();
    function u = randvec()
        u = projection(randn(d, n));
        u = u / norm(u, 'fro');
    end

    % We exploit the fact that VJt_normed belongs to the manifold
    M.rand = @() VJt_normed + randn(1) * randvec();

    M.lincomb = @matrixlincomb;

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

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

    M.pairmean = @(x1, x2) .5*(x1+x2);

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

end


% ABHISHEK MEENA
% Department of Electrical Engineering
% IIT KANPUR
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clc
clear all

% Gaussian Kernel is taken

%%
% VARIABLE PARAMETERS CHANGE TO GET DIFFERENT RESULTS
hs = 8; %spatial bandwidth
hr= 7;   % range bandwidth
threshold_convergence_mean = 0.25;
bandwidth=[hs,hr];
%%
i=imread('42049.jpg');
[height,width,frame] = size(i);
x=zeros(5,height*width);
%% Going from RGB space to Luv using the function RGB2LUV
for j=1:height
    for l=1:width
        x(1,l+width*(j-1)) = j;
        x(2,l+width*(j-1)) = l;
        [x(3,l+width*(j-1)),x(4,l+width*(j-1)),x(5,l+width*(j-1))] = RGB2LUV(i(j,l,1),i(j,l,2),i(j,l,3));
    end
end
%%
%%
% finding the clusters  and plotting the clusters with their data points

% centres_clusters = centres of all clusters obtained

[centres_clusters,data2cluster,datapoints_cluster_no] = mean_shift_algorithm(x,bandwidth,threshold_convergence_mean);

no_clusters = length(datapoints_cluster_no);
figure(1);

hold on
color_vector = 'bgrcmykbgrcmykbgrcmykbgrcmykbgrcmykbgrmykbgrcmykbgrcmykbgrcmykbgrcmy';
for k = 1:no_clusters
    clusters_dataset_members = datapoints_cluster_no{k};
    cluster_centres = centres_clusters(:,k);
    plot(x(1,clusters_dataset_members),x(2,clusters_dataset_members),[color_vector(k) '.'])
    plot(cluster_centres(1),cluster_centres(2),'o','MarkerEdgeColor','k','MarkerFaceColor',color_vector(k), 'MarkerSize',13')
end
title(['data points with their cluster centres Gaussian Kernel (hs,hr)=',num2str(hs),',',num2str(hr)]);
hold off
%%
% creating a 3-D RGB matrix and then plotting the 3-D matrix to get the segmented image
[h2,w2] = size(centres_clusters);
zfilter=zeros(5,height*width);
for i12=1:w2
    mem=datapoints_cluster_no{i12,1};
    p1=size(mem);

    for s1=1:p1(1,2)
        zfilter(:,mem(s1))=centres_clusters(:,i12);
    end
end
zluv(:,:,1)=(reshape(zfilter(3,:),width,height))';
zluv(:,:,2)=(reshape(zfilter(4,:),width,height))';
zluv(:,:,3)=(reshape(zfilter(5,:),width,height))';
zrgb = colorspace('Luv->RGB',zluv);
zrgb1=round(zrgb*255);
figure(2)
imshow(zrgb);
title(['Segmented Image (hs,hr)=',num2str(hs),',',num2str(hr)]);
%%


%% Machine Learning Online Class - Exercise 3 | Part 1: One-vs-all

%  Instructions
%  ------------
%
%  This file contains code that helps you get started on the
%  linear exercise. You will need to complete the following functions
%  in this exericse:
%
%     lrCostFunction.m (logistic regression cost function)
%     oneVsAll.m
%     predictOneVsAll.m
%     predict.m
%
%  For this exercise, you will not need to change any code in this file,
%  or any other files other than those mentioned above.
%

%% Initialization
clear ; close all; clc

%% Setup the parameters you will use for this part of the exercise
input_layer_size  = 400;  % 20x20 Input Images of Digits
num_labels = 10;          % 10 labels, from 1 to 10
                          % (note that we have mapped "0" to label 10)

%% =========== Part 1: Loading and Visualizing Data =============
%  We start the exercise by first loading and visualizing the dataset.
%  You will be working with a dataset that contains handwritten digits.
%

% Load Training Data
fprintf('Loading and Visualizing Data ...\n')

load('ex3data1.mat'); % training data stored in arrays X, y
m = size(X, 1);

% Randomly select 100 data points to display
rand_indices = randperm(m);
sel = X(rand_indices(1:100), :);

displayData(sel);

fprintf('Program paused. Press enter to continue.\n');
pause;

%% ============ Part 2a: Vectorize Logistic Regression ============
%  In this part of the exercise, you will reuse your logistic regression
%  code from the last exercise. You task here is to make sure that your
%  regularized logistic regression implementation is vectorized. After
%  that, you will implement one-vs-all classification for the handwritten
%  digit dataset.
%

% Test case for lrCostFunction
fprintf('\nTesting lrCostFunction() with regularization');

theta_t = [-2; -1; 1; 2];
X_t = [ones(5,1) reshape(1:15,5,3)/10];
y_t = ([1;0;1;0;1] >= 0.5);
lambda_t = 3;
[J, grad] = lrCostFunction(theta_t, X_t, y_t, lambda_t);

fprintf('\nCost: %f\n', J);
fprintf('Expected cost: 2.534819\n');
fprintf('Gradients:\n');
fprintf(' %f \n', grad);
fprintf('Expected gradients:\n');
fprintf(' 0.146561\n -0.548558\n 0.724722\n 1.398003\n');

fprintf('Program paused. Press enter to continue.\n');
pause;
%% ============ Part 2b: One-vs-All Training ============
fprintf('\nTraining One-vs-All Logistic Regression...\n');

lambda = 0.1;
[all_theta] = oneVsAll(X, y, num_labels, lambda);

fprintf('Program paused. Press enter to continue.\n');
pause;

%% ================ Part 3: Predict for One-Vs-All ================

pred = predictOneVsAll(all_theta, X);

fprintf('\nTraining Set Accuracy: %f\n', mean(double(pred == y)) * 100);


function [ V ] = R2V( R )
%R2V converts 3x3 rotation matrix into a 1x3 angle-axis vector
%   Inputs -
%   R - a standard 3x3 transformation matrix
%
%   Outputs -
%   V - 1x3 vector of form [rx,ry,rz] where rx,ry,rz is an angle-axis
%   representation of the angle where the unit vector representing the axis
%   has been multipled by the angle of rotation about it

validateattributes(R, {'numeric'},{'size',[3,3]});

R = vrrotmat2vec(double(R));
V = R(1:3)*R(4);
V = V';
end


function [grad_scores] = normLayerBackprop(grad_alignWeights, alignWeights) %, maskedIds, params)
  %% from grad_alignWeights -> grad_scores
  % alignWeights a = softmax(scores)
  % Let's derive per indices grad align weight w.r.t scores
  %   der a_i / der s_j = der exp(s_i) / sum_k (exp(s_k)) / der s_j =
  %     (1/sum) * (der exp(s_i) / der s_j) - (exp(s_i)/sum^2)*exp(s_j) =
  %      a_i*I{i==j} - a_i*_a_j
  %
  % Now let's try to optimize the vector grad for a single example i:
  %   grad_score_i = (diag(a_i) - a_i*a_i')*grad_a_i
  %                = a_i.*grad_a_i - a_i*(a_i'*grad_a_i)
  %                = a_i.*grad_a_i - a_i*alpha_i
  % multiple examples: alpha = sum(a.*grad_a, 1) % 1*curBatchSize
  %     grad_scores = a.*grad - bsxfun(@times, a, alpha)
  % tmpResult = alignWeights.*grad_alignWeights; % numAttnPositions * curBatchSize

  tmpResult = alignWeights.*grad_alignWeights; % numAttnPositions * curBatchSize
  grad_scores = tmpResult - bsxfun(@times, alignWeights, sum(tmpResult, 1));

%   % assert
%   if params.assert
%     % compute grad_scores in a different way
%     grad_scores1 = zeroMatrix(size(grad_scores), params.isGPU, params.dataType);
%     for ii=1:params.curBatchSize
%       grad_scores1(:, ii) = (diag(alignWeights(:, ii))-alignWeights(:, ii)*alignWeights(:, ii)')*grad_alignWeights(:, ii);
%     end
%     assert(computeSum(grad_scores-grad_scores1, params.isGPU)<1e-5);
%
%     assert(computeSum(alignWeights(maskedIds), params.isGPU)==0);
%     assert(computeSum(grad_scores(maskedIds), params.isGPU)==0);
%     assert(computeSum(tmpResult(maskedIds), params.isGPU)==0);
%     tmpResult = bsxfun(@times, alignWeights, sum(tmpResult, 1));
%     assert(computeSum(tmpResult(maskedIds), params.isGPU)==0);
%   end
end


% SFR_Bif_Diag_Second_Method - Bifurcation diagram with history.
% Copyright Springer 2013 Stephen Lynch.
clear
format long;
halfN=19999;N=2*halfN+1;N1=1+halfN;
E(1)=0.4;B=0.15;Pmax=16;

% Ramp the power up
for n=1:halfN
    E(n+1)=sqrt(n*Pmax/N1)+B*E(n)*exp(1i*abs(E(n))^2);
    Esqr(n+1)=abs(E(n+1))^2;
end

% Ramp the power down
for n=N1:N
    E(n+1)=sqrt(2*Pmax-n*Pmax/N1)+B*E(n)*exp(1i*abs(E(n))^2);
    Esqr(n+1)=abs(E(n+1))^2;
end

for n=1:halfN
    Esqr1(n)=Esqr(N+1-n);
    ptsup(n)=n*Pmax/N1;
end

% Plot the bifurcation diagrams
fsize=15;
hold on
set(gca,'xtick',0:4:16,'FontSize',fsize)
set(gca,'ytick',0:5:25,'FontSize',fsize)
plot(ptsup(1:halfN),Esqr(1:halfN),'.','MarkerSize',1);
plot(ptsup(1:halfN),Esqr1(1:halfN),'.','MarkerSize',1);
xlabel('Input Power','FontSize',fsize);
ylabel('Output Power','FontSize',fsize);
hold off


function [data,p]=create_data(p)
if nargin==0
    p=struct();
end

gpr=load('gpr.dat');

p.zss = exp(mean(gpr(:,1)));
p.paiss = exp(mean(gpr(:,2)));
p.beta = (p.zss*p.paiss)/(exp(mean(gpr(:,3))));

start_date='1983Q1';
gt = gpr(:,1) - log(p.zss);
pit = gpr(:,2) - log(p.paiss);
rt = gpr(:,3) - log(p.zss) + log(p.beta) - log(p.paiss);

data=ts(start_date,[gt,pit,rt],{'GHAT','PAIHAT','RHAT'});
data=pages2struct(data);


function test_sampled_newton()

    clc;
    clear;
    close all;

    %% Set algorithms
    algorithms = {'L-BFGS-BKT','Newton-CHOLESKY','Newton-INEXACT','Subsamp-Newton-Uniform', 'Subsamp-Newton-RNS', 'Subsamp-Newton-LS'};

    % select problem
    problem_type = 'log_reg';
    %problem_type = 'lin_reg';

    lambda = 0.01;

    % prepare datasets
    if strcmp(problem_type, 'lin_reg')

        % generate synthtic data
        % sample data generating for training: y = w1*x1 + w2*x2 + ... * wd*1
        n = 10000;
        d = 10;
        std = 0.25;
        data = linear_regression_data_generator(n, d, std);

        x_train = data.x_train;
        y_train = data.y_train;
        x_test = data.x_test;
        y_test = data.y_test;

        % solution
        w_opt = pinv(x_train * x_train') * x_train * y_train';
        % for intersect
        d = d + 1;

        % define problem definitions
        problem = linear_regression(x_train, y_train, x_test, y_test, lambda);
        f_opt1 = problem.cost(w_opt);
        fprintf('f_opt: %.16e\n', f_opt1);

        w_opt = problem.calc_solution(100, 'lbfgs');
        f_opt2 = problem.cost(w_opt);
        fprintf('f_opt1: %.16e, f_opt2: %.16e, diff: %.16e\n', f_opt1, f_opt2, f_opt2 - f_opt1);

        if f_opt2 < f_opt1
            f_opt = f_opt2;
        else
            f_opt = f_opt1;
        end

    elseif strcmp(problem_type, 'log_reg')

        % read real-world dataset
        [data_y, data_X] = libsvmread('../data/libsvm/a9a');
        x_train = data_X';
        y_train = data_y';
        d = size(x_train,1);
        n = length(y_train);
        lambda = 0.5;

        if d == 0
            return;
        end

        % define problem definitions
        problem = logistic_regression(x_train, y_train, [], [], lambda);

        % calculate f_opt
        w_opt = problem.calc_solution(100, 'lbfgs');
        f_opt = problem.cost(w_opt);
        fprintf('f_opt: %.24e\n', f_opt);

    else
        return;
    end

    %% initialize
    w_init = zeros(d,1);
    batch_size = 1;
    w_list = cell(length(algorithms),1);
    info_list = cell(length(algorithms),1);

    %% 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_optgap = 1e-14;
        options.tol_gnorm = 1e-16;
        options.max_iter = 100;
        options.verbose = 1;
        options.f_opt = f_opt;
        options.store_w = false;
        options.permute_on = 1;
        options.f_opt = f_opt;
        options.batch_size = batch_size;

        switch algorithms{alg_idx}

            % Sub-sampled Newton methods
            case {'Subsamp-Newton-Uniform'}

                options.sub_mode = 'Uniform';
                options.subsamp_hess_size = 100*d;
                [w_list{alg_idx}, info_list{alg_idx}] = subsamp_newton(problem, options);

            case {'Subsamp-Newton-RNS'}

                options.sub_mode = 'RNS';
                options.subsamp_hess_size = 20*d;
                [w_list{alg_idx}, info_list{alg_idx}] = subsamp_newton(problem, options);

            case {'Subsamp-Newton-LS'}

                options.sub_mode = 'LS';
                options.subsamp_hess_size = 20*d;
                options.hess_update_freq = 10;
                [w_list{alg_idx}, info_list{alg_idx}] = subsamp_newton(problem, options);

            % Newton methodss
            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-BKT'}

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

            case {'Newton-CHOLESKY'}

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

            case {'Newton-INEXACT-BKT'}

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

            case {'Newton-INEXACT'}

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

            % BFGS variants
            case {'BFGS-BKT'}

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

            case {'L-BFGS-BKT'}

                options.step_alg = 'backtracking';
                options.mem_size = 50;
                [w_list{alg_idx}, info_list{alg_idx}] = lbfgs(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','optimality_gap', algorithms, w_list, info_list);
    display_graph('time','optimality_gap', algorithms, w_list, info_list);
    %display_graph('time','gnorm', algorithms, w_list, info_list);

end


y       = load('data/motorcycle.dat')';
time    = y(1, :);
delta   = y(2, [2:end 1]);
y       = y(3, :);

%% Spline smoothing model %%
spline              = ssm_spline(delta);
spline              = estimate(y, spline, [1 0.1]);
[alphahat V]        = statesmo(y, spline);
conf                = squeeze(1.96*realsqrt(V(1, 1, :)))';
[eps eta epsvar]    = disturbsmo(y, spline);

figure('Name', 'Motorcycle acceleration data analyzed by a cubic spline');
subplot(2, 1, 1), plot(time, alphahat(1, :), 'b'), hold all, plot(time, [alphahat(1, :)+conf; alphahat(1, :)-conf], 'b:'), scatter(time, y, 10, 'r', 's', 'filled'), hold off, title('Spline and 95% confidence intervals'), ylim([-140 80]), set(gca,'YGrid','on');
subplot(2, 1, 2), scatter(time, eps./realsqrt(epsvar), 10, 'r', 's', 'filled'), title('Standardized irregular'), set(gca,'YGrid','on');
if ispc, set(gcf, 'WindowStyle', 'docked'); end

%% Continuous local level model %%
abseps          = abs(eps);
contllm         = [ssm_gaussian ssmodel('continuous local level', 0, 1, 1, 1, ssmat(0, [], true, zeros(size(delta)), true), 'Qd', {@(X) exp(2*X)*delta}, {[]}, ssparam({'zeta var'}, '1/2 log'))];
[contllm logL]  = estimate(abseps, contllm, [1 0.1]);
alphahat        = statesmo(abseps, contllm);

figure('Name', 'Correction for heteroscedasticity');
subplot(3, 1, 1), plot(time, alphahat, 'b'), hold all, scatter(time, abseps, 10, 'r', 's', 'filled'), hold off, title('Absolute smoothed irregular and h^\ast_t'), ylim([0 87.5]);

%% Correction for heteroscedasticity %%
h2                  = (alphahat/alphahat(1)).^2;
splineh             = [ssmodel('Heteroscedastic noise', ssmat(0, [], true, zeros(size(h2)), true), zeros(1, 0), [], [], [], 'Hd', {@(X) exp(2*X)*h2}, {[]}, ssparam({'epsilon var'}, '1/2 log')) spline];
splineh             = estimate(y, splineh, [1 0.1]);
[alphahath Vh]      = statesmo(y, splineh);
confh               = squeeze(1.96*realsqrt(Vh(1, 1, :)))';
[epsh eta epsvarh]  = disturbsmo(y, splineh);

subplot(3, 1, 2), plot(time, alphahath(1, :), 'b'), hold all, plot(time, [alphahath(1, :)+confh; alphahath(1, :)-confh], 'b:'), scatter(time, y, 10, 'r', 's', 'filled'), hold off, title('Spline and 95% confidence intervals'), ylim([-140 80]), set(gca,'YGrid','on');
subplot(3, 1, 3), scatter(time, epsh./realsqrt(epsvarh), 10, 'r', 's', 'filled'), title('Standardized irregular'), set(gca,'YGrid','on');
if ispc, set(gcf, 'WindowStyle', 'docked'); end