function [binId, cumArea] = hist(grains,varargin)
% plot a grain size histogram
%
% Syntax
%   hist(grains)
%   hist(grains,n) % specify the number of bins
%
%   % use an arbitrary property for the histogramm
%   hist(grains,grains.equivalentRadius)
%
%   binId = hist(grains) % returns the bin for each grain
%
% Input
%   grains - @grain2d
%   n      - number of bins, default ist 15
%

[mtexFig,isNew] = newMtexFigure(varargin{:});
mtexFig.keepAspectRatio = false;

% exract area and maybe an aditional property
area = grains.area;
if nargin>1 && isnumeric(varargin{1}) && length(varargin{1}) == length(grains)
  prop = varargin{1};
  varargin(1) = [];
else
  prop = area;
end

if ~isempty(varargin) && isnumeric(varargin{1})
  nbins = varargin{1};
else
  nbins = 15;
end

idList = grains.indexedPhasesId;

bins = linspace(0,max(prop)+eps,nbins);

cumArea = zeros(numel(bins)-1,numel(idList));
binId = zeros(length(grains),1);

% loop through all phases
for id = 1:numel(idList)

  phaseId = grains.phaseId==idList(id);
  phaseArea = area(phaseId);
  phaseProp = prop(phaseId);

  % find for each area the binId
  [~,~,binIdLocal] = histcounts(phaseProp,bins);
  binId(phaseId) = binIdLocal;

  % compute the sum of areas belonging to the same bin
  for i = 1:numel(bins)-1
    cumArea(i,id) = sum(phaseArea(binIdLocal == i)) ./ sum(area);
  end

end

% plot the result as a bar plot
binCenter = 0.5*(bins(1:end-1)+bins(2:end));
binWidth = 1 + 0.5*size(cumArea,2)>1;
bar(binCenter,100*cumArea,'BarWidth',binWidth,'parent',mtexFig.gca)
xlim(mtexFig.gca,[bins(1),bins(end)])
if all(prop == area)
  title(mtexFig.gca,'grain size distribution')
  xlabel(mtexFig.gca,'grain area');
end
ylabel(mtexFig.gca,'relative area (%)')

min = grains.mineralList(idList);
legend(min{:})

if isNew, mtexFig.drawNow(varargin{:});end

if nargout == 0, clear binId; end


function [X, fX, i] = kafbox_minimize(X, f, length, varargin)

% Minimize a differentiable multivariate function.
%
% Usage: [X, fX, i] = minimize(X, f, length, P1, P2, P3, ... )
%
% where the starting point is given by "X" (D by 1), and the function named in
% the string "f", must return a function value and a vector of partial
% derivatives of f wrt X, the "length" gives the length of the run: if it is
% positive, it gives the maximum number of line searches, if negative its
% absolute gives the maximum allowed number of function evaluations. You can
% (optionally) give "length" a second component, which will indicate the
% reduction in function value to be expected in the first line-search (defaults
% to 1.0). The parameters P1, P2, P3, ... are passed on to the function f.
%
% The function returns when either its length is up, or if no further progress
% can be made (ie, we are at a (local) minimum, or so close that due to
% numerical problems, we cannot get any closer). NOTE: If the function
% terminates within a few iterations, it could be an indication that the
% function values and derivatives are not consistent (ie, there may be a bug in
% the implementation of your "f" function). The function returns the found
% solution "X", a vector of function values "fX" indicating the progress made
% and "i" the number of iterations (line searches or function evaluations,
% depending on the sign of "length") used.
%
% The Polack-Ribiere flavour of conjugate gradients is used to compute search
% directions, and a line search using quadratic and cubic polynomial
% approximations and the Wolfe-Powell stopping criteria is used together with
% the slope ratio method for guessing initial step sizes. Additionally a bunch
% of checks are made to make sure that exploration is taking place and that
% extrapolation will not be unboundedly large.
%
% See also: checkgrad
%
% Copyright (C) 2001 - 2006 by Carl Edward Rasmussen (2006-09-08).

INT = 0.1;    % don't reevaluate within 0.1 of the limit of the current bracket
EXT = 3.0;                  % extrapolate maximum 3 times the current step-size
MAX = 20;                         % max 20 function evaluations per line search
RATIO = 10;                                       % maximum allowed slope ratio
SIG = 0.1; RHO = SIG/2; % SIG and RHO are the constants controlling the Wolfe-
% Powell conditions. SIG is the maximum allowed absolute ratio between
% previous and new slopes (derivatives in the search direction), thus setting
% SIG to low (positive) values forces higher precision in the line-searches.
% RHO is the minimum allowed fraction of the expected (from the slope at the
% initial point in the linesearch). Constants must satisfy 0 < RHO < SIG < 1.
% Tuning of SIG (depending on the nature of the function to be optimized) may
% speed up the minimization; it is probably not worth playing much with RHO.

% The code falls naturally into 3 parts, after the initial line search is
% started in the direction of steepest descent. 1) we first enter a while loop
% which uses point 1 (p1) and (p2) to compute an extrapolation (p3), until we
% have extrapolated far enough (Wolfe-Powell conditions). 2) if necessary, we
% enter the second loop which takes p2, p3 and p4 chooses the subinterval
% containing a (local) minimum, and interpolates it, unil an acceptable point
% is found (Wolfe-Powell conditions). Note, that points are always maintained
% in order p0 <= p1 <= p2 < p3 < p4. 3) compute a new search direction using
% conjugate gradients (Polack-Ribiere flavour), or revert to steepest if there
% was a problem in the previous line-search. Return the best value so far, if
% two consecutive line-searches fail, or whenever we run out of function
% evaluations or line-searches. During extrapolation, the "f" function may fail
% either with an error or returning Nan or Inf, and minimize should handle this
% gracefully.

if max(size(length)) == 2, red=length(2); length=length(1); else red=1; end
if length>0, S='Linesearch'; else S='Function evaluation'; end

i = 0;                                            % zero the run length counter
ls_failed = 0;                             % no previous line search has failed
[f0 df0] = feval(f, X, varargin{:});          % get function value and gradient
fX = f0;
i = i + (length<0);                                            % count epochs?!
s = -df0; d0 = -s'*s;           % initial search direction (steepest) and slope
x3 = red/(1-d0);                                  % initial step is red/(|s|+1)

while i < abs(length)                                      % while not finished
  i = i + (length>0);                                      % count iterations?!

  X0 = X; F0 = f0; dF0 = df0;                   % make a copy of current values
  if length>0, M = MAX; else M = min(MAX, -length-i); end

  while 1                             % keep extrapolating as long as necessary
    x2 = 0; f2 = f0; d2 = d0; f3 = f0; df3 = df0;
    success = 0;
    while ~success && M > 0
      try
        M = M - 1; i = i + (length<0);                         % count epochs?!
        [f3 df3] = feval(f, X+x3*s, varargin{:});
        if isnan(f3) || isinf(f3) || any(isnan(df3)+isinf(df3)), error(''), end
        success = 1;
      catch                                % catch any error which occured in f
        x3 = (x2+x3)/2;                                  % bisect and try again
      end
    end
    if f3 < F0, X0 = X+x3*s; F0 = f3; dF0 = df3; end         % keep best values
    d3 = df3'*s;                                                    % new slope
    if d3 > SIG*d0 || f3 > f0+x3*RHO*d0 || M == 0  % are we done extrapolating?
      break
    end
    x1 = x2; f1 = f2; d1 = d2;                        % move point 2 to point 1
    x2 = x3; f2 = f3; d2 = d3;                        % move point 3 to point 2
    A = 6*(f1-f2)+3*(d2+d1)*(x2-x1);                 % make cubic extrapolation
    B = 3*(f2-f1)-(2*d1+d2)*(x2-x1);
    x3 = x1-d1*(x2-x1)^2/(B+sqrt(B*B-A*d1*(x2-x1))); % num. error possible, ok!
    if ~isreal(x3) || isnan(x3) || isinf(x3) || x3 < 0 % num prob | wrong sign?
      x3 = x2*EXT;                                 % extrapolate maximum amount
    elseif x3 > x2*EXT                  % new point beyond extrapolation limit?
      x3 = x2*EXT;                                 % extrapolate maximum amount
    elseif x3 < x2+INT*(x2-x1)         % new point too close to previous point?
      x3 = x2+INT*(x2-x1);
    end
  end                                                       % end extrapolation

  while (abs(d3) > -SIG*d0 || f3 > f0+x3*RHO*d0) && M > 0  % keep interpolating
    if d3 > 0 || f3 > f0+x3*RHO*d0                         % choose subinterval
      x4 = x3; f4 = f3; d4 = d3;                      % move point 3 to point 4
    else
      x2 = x3; f2 = f3; d2 = d3;                      % move point 3 to point 2
    end
    if f4 > f0
      x3 = x2-(0.5*d2*(x4-x2)^2)/(f4-f2-d2*(x4-x2));  % quadratic interpolation
    else
      A = 6*(f2-f4)/(x4-x2)+3*(d4+d2);                    % cubic interpolation
      B = 3*(f4-f2)-(2*d2+d4)*(x4-x2);
      x3 = x2+(sqrt(B*B-A*d2*(x4-x2)^2)-B)/A;        % num. error possible, ok!
    end
    if isnan(x3) || isinf(x3)
      x3 = (x2+x4)/2;               % if we had a numerical problem then bisect
    end
    x3 = max(min(x3, x4-INT*(x4-x2)),x2+INT*(x4-x2));  % don't accept too close
    [f3 df3] = feval(f, X+x3*s, varargin{:});
    if f3 < F0, X0 = X+x3*s; F0 = f3; dF0 = df3; end         % keep best values
    M = M - 1; i = i + (length<0);                             % count epochs?!
    d3 = df3'*s;                                                    % new slope
  end                                                       % end interpolation

  if abs(d3) < -SIG*d0 && f3 < f0+x3*RHO*d0          % if line search succeeded
    X = X+x3*s; f0 = f3; fX = [fX' f0]';                     % update variables
    fprintf('%s %6i;  Value %4.6e\r', S, i, f0);
    s = (df3'*df3-df0'*df3)/(df0'*df0)*s - df3;   % Polack-Ribiere CG direction
    df0 = df3;                                               % swap derivatives
    d3 = d0; d0 = df0'*s;
    if d0 > 0                                      % new slope must be negative
      s = -df0; d0 = -s'*s;                  % otherwise use steepest direction
    end
    x3 = x3 * min(RATIO, d3/(d0-realmin));          % slope ratio but max RATIO
    ls_failed = 0;                              % this line search did not fail
  else
    X = X0; f0 = F0; df0 = dF0;                     % restore best point so far
    if ls_failed || i > abs(length)         % line search failed twice in a row
      break;                             % or we ran out of time, so we give up
    end
    s = -df0; d0 = -s'*s;                                        % try steepest
    x3 = 1/(1-d0);
    ls_failed = 1;                                    % this line search failed
  end
end
fprintf('\n');


%% STUDY_02_wrist_scaffold_single_fiber_iFEA_viscoelastic
% Below is a demonstration for:
%
% * Inverse FEA based material parameter optimisation
% * febio_spec version 3.0
% * febio, FEBio
% * hexahedral elements, hex8
% * static, solid
% * hyperelastic, Ogden
% * displacement logfile
% * stress logfile

clear; close all; clc;

%%
% Plot settings
fontSize=40;
markerSize=25;
lineWidth=3;

%% Control parameters
% Path names
defaultFolder = fileparts(fileparts(mfilename('fullpath')));
savePath=fullfile(defaultFolder,'2021_Nataliya_Perevoshchikova_wrist_project','data','temp');
loadPathMatProp=fullfile(defaultFolder,'2021_Nataliya_Perevoshchikova_wrist_project','data','mat_prop','PCL');

%%
% Building a quadrilateral circular mesh
pointSpacingHeight=0.1;
d=0.35;
h=10;
xSpacing=0.5;
ySpacing=0.4;
nCopies_x=8;
nCopies_y=8;

r=d/2;
ne=2; %Elements in radius
f=0.6; %Fraction (with respect to outer radius) where central square appears
NumStrands=4*7;
%Create the mesh
[Fq,Vq]=discQuadMesh(ne,r,f);

%%
% Lofting to a fiber
Vq(:,3)=0;
Vq1=Vq;
Vq1(:,3)=Vq1(:,3)-h/2;
Vq2=Vq;
Vq2(:,3)=Vq2(:,3)+h/2;

numStepsSweep=ceil(h/pointSpacingHeight);
xc=zeros(numStepsSweep,1);
yc=zeros(numStepsSweep,1);
zc=linspace(-h/2,h/2,numStepsSweep)';
Vc=[xc yc zc];

[~,~,~,S]=sweepLoft(Vq1,Vq2,[0 0 1],[0 0 1],Vc,numStepsSweep,0,0);

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

I=size(Vq,1)*((1:1:numStepsSweep)-1);
I=I(ones(size(Fq,1),1),:);
I=I(:);

FQ=repmat(Fq,numStepsSweep,1)+I(:,ones(size(Fq,2),1));
E=[FQ(1:end-size(Fq,1),:) FQ(size(Fq,1)+1:end,:)]; %The hexahedral elements
F_start=FQ(1:size(Fq,1),:);
F_end=FQ(end-size(Fq,1)+1:end,:);

[E,V,ind1,ind2]=mergeVertices(E,V); %Merge nodes (start and end are not shared yet)

[F]=element2patch(E); %Element faces
F_start=ind2(F_start); %Start faces for BC's
F_end=ind2(F_end); %End faces for BC's
ind=tesBoundary(F,V); %Indices of boundary faces
Fb=F(ind,:); %Boundary faces (for plotting)

%%
% Visualizing mesh

cFigure; hold on;
title('Hexahedral mesh one fiber','fontSize',fontSize);
gpatch(Fb,V,'gw','k',0.5);
gpatch(F_start,V,'rw','k',1);
gpatch(F_end,V,'bw','k',1);
axisGeom;
camlight headlight;
set(gca,'FontSize',fontSize);
drawnow;

%%
% Defining file names
febioFebFileNamePart='tempModel';
febioFebFileName=fullfile(savePath,[febioFebFileNamePart,'.feb']); %FEB file name
febioLogFileName=[febioFebFileNamePart,'.txt']; %FEBio log file name
febioLogFileName_disp=[febioFebFileNamePart,'_disp_out.txt']; %Log file name for exporting displacement
febioLogFileName_force=[febioFebFileNamePart,'_force_out.txt']; %Log file name for exporting force
febioLogFileName_stress=[febioFebFileNamePart,'_stress_out.txt']; %Log file name for exporting stress

C1=57.21894783089972;
C2=57.23071414361289;
K=11444.966197451262;
M1=2.00012552045284;
M2=-1.9997730268725347;
%Initial Viscoelastic parameters
g1_ini=1.25;%Viscoelastic QLV proportional coefficient
t1_ini=25;%Viscoelastic QLV time coefficient
P=[t1_ini g1_ini C1 M1 C2 M2];
displacementMagnitude=0.6030;

% Experimental viscoelastic properties
matname = fullfile(loadPathMatProp,'Viscoelastic_properties.mat');
load(matname);

cFigure;
title('Viscoelastic region','fontSize',fontSize);
hold on;
xlabel('Time, s','FontSize',fontSize);
ylabel('Load rate','FontSize',fontSize);
plot(V_time,lc,'k.-','LineWidth',3);
set(gca,'FontSize',fontSize);
camlight headlight;

[M,I]=max(lc);
time_load=V_time(1:I);
time_unload=V_time(I+1:end);
lc_load=lc(1:I);
lc_unload=lc(I+1:end);

% FEA control settings
max_refs=50; %Max reforms
max_ups=0; %Set to zero to use full-Newton iterations
opt_iter=6; %Optimum number of iterations
max_retries=10; %Maximum number of retires
k_factor=100;
NumStrands=4*7;

t_load=max(time_load); %Time from start to max load
t_step_ini1=t_load/50; %Initial desired step size
numTimeSteps1=round(t_load/t_step_ini1); %Number of time steps desired
t_step1=t_load/numTimeSteps1; %Step size
dtmin1=t_step1/100; %Smallest allowed step size
dtmax1=t_step1; %Largest allowed step size

t_unload=V_time(end)-max(time_load);
t_step_ini2=t_step_ini1; %Initial desired step size
numTimeSteps2=round(t_unload/t_step_ini2); %Number of time steps desired
t_step2=t_unload/numTimeSteps2; %Step size
dtmin2=t_step2/100; %Smallest allowed step size
dtmax2=t_step2; %Largest allowed step size

cFigure;
title('Viscoelastic region','fontSize',fontSize);
hold on;
xlabel('Time, s','FontSize',fontSize);
ylabel('Load rate','FontSize',fontSize);
plot(time_load,lc_load,'r.-','LineWidth',3);
plot(time_unload,lc_unload,'k.-','LineWidth',3);
set(gca,'FontSize',fontSize);
camlight headlight;

%% DEFINE BC's
%Supported nodes
bcRigidList=unique(F_start(:));

%Prescribed force nodes
bcPrescribeList=unique(F_end(:));
bcPrescribeMagnitudes=displacementMagnitude(ones(1,numel(bcPrescribeList)),:);

%Visualize BC's
cFigure; hold on;
title('Boundary conditions','FontSize',fontSize);
gpatch(F,V,'y','k',1);

hl(1)=plotV(V(bcRigidList,:),'k.','MarkerSize',markerSize);
hl(2)=plotV(V(bcPrescribeList,:),'r.','MarkerSize',markerSize);

legend(hl,{'BC full support','BC prescribed Z displacement'})
axisGeom;
camlight headlight;
set(gca,'FontSize',fontSize);
drawnow;

%% Defining the FEBio input structure
% See also |febioStructTemplate| and |febioStruct2xml| and the FEBio user
% manual.

%Get a template with default settings
[febio_spec]=febioStructTemplate;

%febio_spec version
febio_spec.ATTR.version='3.0';

%Module section
febio_spec.Module.ATTR.type='solid';

analysisType='STATIC';
%Control sections for each step
febio_spec.Step.step{1}.Control=febio_spec.Control; %Copy from template
febio_spec.Step.step{1}.ATTR.id=1;
febio_spec.Step.step{1}.Control.analysis=analysisType;
febio_spec.Step.step{1}.Control.time_steps=numTimeSteps1;
febio_spec.Step.step{1}.Control.step_size=t_step1;
febio_spec.Step.step{1}.Control.solver.max_refs=max_refs;
febio_spec.Step.step{1}.Control.solver.max_ups=max_ups;
febio_spec.Step.step{1}.Control.time_stepper.dtmin=dtmin1;
febio_spec.Step.step{1}.Control.time_stepper.dtmax=dtmax1;
febio_spec.Step.step{1}.Control.time_stepper.max_retries=max_retries;
febio_spec.Step.step{1}.Control.time_stepper.opt_iter=opt_iter;

febio_spec.Step.step{2}.Control=febio_spec.Control; %Copy from template
febio_spec.Step.step{2}.ATTR.id=2;
febio_spec.Step.step{2}.Control.analysis=analysisType;
febio_spec.Step.step{2}.Control.time_steps=numTimeSteps2;
febio_spec.Step.step{2}.Control.step_size=t_step2;
febio_spec.Step.step{2}.Control.solver.max_refs=max_refs;
febio_spec.Step.step{2}.Control.solver.max_ups=max_ups;
febio_spec.Step.step{2}.Control.time_stepper.dtmin=dtmin2;
febio_spec.Step.step{2}.Control.time_stepper.dtmax=dtmax2;
febio_spec.Step.step{2}.Control.time_stepper.max_retries=max_retries;
febio_spec.Step.step{2}.Control.time_stepper.opt_iter=opt_iter;

%Remove control field (part of template) since step specific control sections are used
febio_spec=rmfield(febio_spec,'Control');

materialName1='Material1';
febio_spec.Material.material{1}.ATTR.name=materialName1;

%Viscoelastic part
febio_spec.Material.material{1}.ATTR.type='uncoupled viscoelastic';
febio_spec.Material.material{1}.ATTR.id=1;
febio_spec.Material.material{1}.g1=g1_ini;
febio_spec.Material.material{1}.t1=t1_ini;

%Elastic part
febio_spec.Material.material{1}.elastic{1}.ATTR.type='Ogden';
febio_spec.Material.material{1}.elastic{1}.c1=C1;
febio_spec.Material.material{1}.elastic{1}.m1=M1;
febio_spec.Material.material{1}.elastic{1}.c2=C2;
febio_spec.Material.material{1}.elastic{1}.m2=M2;
febio_spec.Material.material{1}.elastic{1}.k=K;

%Geometry section
% -> Nodes
febio_spec.Mesh.Nodes{1}.ATTR.name='Object1'; %The node set name
febio_spec.Mesh.Nodes{1}.node.ATTR.id=(1:size(V,1))'; %The node id's
febio_spec.Mesh.Nodes{1}.node.VAL=V; %The nodel coordinates

% % -> Elements
partName1='Part1';
febio_spec.Mesh.Elements{1}.ATTR.name=partName1; %Name of the element set
febio_spec.Mesh.Elements{1}.ATTR.mat=1; %material index for this set
febio_spec.Mesh.Elements{1}.ATTR.type='hex8'; %Element type of this set
febio_spec.Mesh.Elements{1}.elem.ATTR.id=(1:1:size(E,1))'; %Element id's
febio_spec.Mesh.Elements{1}.elem.VAL=E;
%

%MeshDomains section
febio_spec.MeshDomains.SolidDomain.ATTR.name=partName1;
febio_spec.MeshDomains.SolidDomain.ATTR.mat=materialName1;

% % -> NodeSets
% -> NodeSets
nodeSetName1='bcSupportList';
nodeSetName2='bcPrescribeList';

febio_spec.Mesh.NodeSet{1}.ATTR.name=nodeSetName1;
febio_spec.Mesh.NodeSet{1}.node.ATTR.id=bcRigidList(:);
%
febio_spec.Mesh.NodeSet{2}.ATTR.name=nodeSetName2;
febio_spec.Mesh.NodeSet{2}.node.ATTR.id=bcPrescribeList(:);
%
% %Boundary condition section
% % -> Fix boundary conditions
febio_spec.Boundary.bc{1}.ATTR.type='fix';
febio_spec.Boundary.bc{1}.ATTR.node_set=nodeSetName1;
febio_spec.Boundary.bc{1}.dofs='x';

febio_spec.Boundary.bc{2}.ATTR.type='fix';
febio_spec.Boundary.bc{2}.ATTR.node_set=nodeSetName1;
febio_spec.Boundary.bc{2}.dofs='y';

febio_spec.Boundary.bc{3}.ATTR.type='fix';
febio_spec.Boundary.bc{3}.ATTR.node_set=nodeSetName1;
febio_spec.Boundary.bc{3}.dofs='z';

febio_spec.Boundary.bc{4}.ATTR.type='fix';
febio_spec.Boundary.bc{4}.ATTR.node_set=nodeSetName2;
febio_spec.Boundary.bc{4}.dofs='x';

febio_spec.Boundary.bc{5}.ATTR.type='fix';
febio_spec.Boundary.bc{5}.ATTR.node_set=nodeSetName2;
febio_spec.Boundary.bc{5}.dofs='y';

febio_spec.Boundary.bc{6}.ATTR.type='prescribe';
febio_spec.Boundary.bc{6}.ATTR.node_set=nodeSetName2;
febio_spec.Boundary.bc{6}.dof='z';
febio_spec.Boundary.bc{6}.scale.ATTR.lc=1;
febio_spec.Boundary.bc{6}.scale.VAL=displacementMagnitude;
febio_spec.Boundary.bc{6}.relative=0;

%LoadData section
% -> load_controller
febio_spec.LoadData.load_controller{1}.ATTR.id=1;
febio_spec.LoadData.load_controller{1}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{1}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{1}.points.point.VAL=[0 0;time_load lc_load;time_unload lc_unload];

%Output section
% -> log file
febio_spec.Output.logfile.ATTR.file=febioLogFileName;
febio_spec.Output.logfile.node_data{1}.ATTR.file=febioLogFileName_disp;
febio_spec.Output.logfile.node_data{1}.ATTR.data='ux;uy;uz';
febio_spec.Output.logfile.node_data{1}.ATTR.delim=',';
febio_spec.Output.logfile.node_data{1}.VAL=1:size(V,1);

febio_spec.Output.logfile.node_data{2}.ATTR.file=febioLogFileName_force;
febio_spec.Output.logfile.node_data{2}.ATTR.data='Rx;Ry;Rz';
febio_spec.Output.logfile.node_data{2}.ATTR.delim=',';
febio_spec.Output.logfile.node_data{2}.VAL=1:size(V,1);

febio_spec.Output.logfile.element_data{1}.ATTR.file=febioLogFileName_stress;
febio_spec.Output.logfile.element_data{1}.ATTR.data='sx';
febio_spec.Output.logfile.element_data{1}.ATTR.delim=',';
febio_spec.Output.logfile.element_data{1}.VAL=1:size(E,1);

%% Quick viewing of the FEBio input file structure
% The |febView| function can be used to view the xml structure in a MATLAB
% figure window.

%%
% febView(febio_spec); %Viewing the febio file|

%% Exporting the FEBio input file
% Exporting the febio_spec structure to an FEBio input file is done using
% the |febioStruct2xml| function.

febioStruct2xml(febio_spec,febioFebFileName); %Exporting to file and domNode

%% Running the FEBio analysis
% To run the analysis defined by the created FEBio input file the
% |runMonitorFEBio| function is used. The input for this function is a
% structure defining job settings e.g. the FEBio input file name. The
% optional output runFlag informs the user if the analysis was run
% succesfully.

febioAnalysis.run_filename=febioFebFileName; %The input file name
febioAnalysis.run_logname=febioLogFileName; %The name for the log file
febioAnalysis.disp_on=1; %Display information on the command window
febioAnalysis.disp_log_on=1; %Display convergence information in the command window
febioAnalysis.runMode='internal';%'external';

[runFlag]=runMonitorFEBio(febioAnalysis);%START FEBio NOW!!!!!!!!

%% Import FEBio results

if runFlag==1 %i.e. a succesful run

    % Importing nodal displacements from a log file
    [~, N_disp_mat,~]=importFEBio_logfile(fullfile(savePath,febioLogFileName_disp));

    N_disp_mat=N_disp_mat(:,2:end,:);
    sizImport=size(N_disp_mat);
    sizImport(3)=sizImport(3)+1;
    N_disp_mat_n=zeros(sizImport);
    N_disp_mat_n(:,:,2:end)=N_disp_mat;
    N_disp_mat=N_disp_mat_n;
    DN=N_disp_mat(:,:,end);
    DN_magnitude=sqrt(sum(DN(:,1).^2,2)); %X
    V_def=V+DN;
    [CF]=vertexToFaceMeasure(F,DN_magnitude);

    %Importaing force from log file
    [time_mat, Force_mat,~]=importFEBio_logfile(fullfile(savePath,febioLogFileName_force));
    time_mat=[0; time_mat(:)]; %Time
    Force_mat=Force_mat(:,2:end,:);
    sizImport=size(Force_mat);
    sizImport(3)=sizImport(3)+1;
    Force_mat_n=zeros(sizImport);
    Force_mat_n(:,:,2:end)=Force_mat;
    Force_mat=Force_mat_n;
    time_sim=time_mat;
    %%
    % Plotting the simulated results using |anim8| to visualize and animate
    % deformations

    % Create basic view and store graphics handle to initiate animation
    hf=cFigure; %Open figure
    gtitle([febioFebFileNamePart,': Press play to animate']);
    hp=gpatch(F,V_def,CF,'k',1); %Add graphics object to animate
    gpatch(F,V,0.5*ones(1,3),'none',0.25); %A static graphics object

    axisGeom(gca,fontSize);
    colormap(gjet(250)); colorbar;
    caxis([0 max(DN_magnitude)]);
    axis([min(V_def(:,1)) max(V_def(:,1)) min(V_def(:,2)) max(V_def(:,2)) min(V_def(:,3)) max(V_def(:,3))]); %Set axis limits statically
    view(130,25); %Set view direction
    camlight headlight;

    % Set up animation features
    animStruct.Time=time_mat; %The time vector
    for qt=1:1:size(N_disp_mat,3) %Loop over time increments
        DN=N_disp_mat(:,:,qt); %Current displacement
        DN_magnitude=sqrt(sum(DN.^2,2)); %Current displacement magnitude
        V_def=V+DN; %Current nodal coordinates
        [CF]=vertexToFaceMeasure(F,DN_magnitude); %Current color data to use

        %Set entries in animation structure
        animStruct.Handles{qt}=[hp hp]; %Handles of objects to animate
        animStruct.Props{qt}={'Vertices','CData'}; %Properties of objects to animate
        animStruct.Set{qt}={V_def,CF}; %Property values for to set in order to animate
    end
    anim8(hf,animStruct); %Initiate animation feature
    drawnow;

    %%
    % Calculate the simulated applied uniaxial stretch
    DZ_set=N_disp_mat(bcPrescribeList,3,:); %Z displacements of the prescribed set
    DZ_set=mean(DZ_set,1); %Calculate mean Z displacements across nodes
    DZ_set=squeeze(DZ_set);

    %Calculated the simulated force
    FZ_set=Force_mat(bcPrescribeList,3,:);
    FZ_set=sum(FZ_set,1); %Calculate mean X force across nodes
    FZ_set=squeeze(FZ_set);

    disp_sim=DZ_set;
    load_sim=FZ_set*NumStrands;

        %%
    % Visualize curves

    hf=cFigure;
    hold on;
    title('Cyclic load-displacement curve','FontSize',fontSize);
    xlabel('Time','FontSize',fontSize); ylabel('Load','FontSize',fontSize);

    hl(1)=plot(time_mat,load_sim,'r.-','lineWidth',lineWidth*1.1,'markerSize',markerSize*1.3);
    hl(2)=plot(V_time,V_load,'k-','lineWidth',lineWidth);

    legend(hl,{'Simulation','Experiment'},'Location','northwest');

    view(2); axis tight;  grid on; axis square; box on;
    set(gca,'FontSize',fontSize);
    drawnow;

end

%% Create structures for optimization
% Material structure
mat_struct.id=1; %Material id

mat_struct.par_names={'t1','g1','c1','m1','c2','m2','k'}; %Parameter names
mat_struct.par_values={t1_ini g1_ini C1 M1 C2 M2 K}; %Parameter values

objectiveStruct.parNormFactors=abs(P); %This will normalize the parameters to ones(size(P))
objectiveStruct.Pb_struct.xx_c=P; %Parameter constraining centre
objectiveStruct.Pb_struct.xxlim=[[P(1)/100 0.01 P(3)/100 -30 P(5)/100 -30]' [P(1)*1e3 50 P(3)*100 30 P(5)*100 30]']; %Parameter bounds

febioAnalysis.time_on=0;
febioAnalysis.time_log_on=0;

%What should be known to the objective function:
objectiveStruct.h=hl(1);
objectiveStruct.bcPrescribeList=bcPrescribeList;
objectiveStruct.time_exp=V_time;
objectiveStruct.load_exp=V_load;
objectiveStruct.febioAnalysis=febioAnalysis;
objectiveStruct.febio_spec=febio_spec;
objectiveStruct.febioFebFileName=febioFebFileName;
objectiveStruct.mat_struct=mat_struct;
objectiveStruct.k_factor=k_factor;
objectiveStruct.NumStrands=NumStrands;

%Optimisation settings
maxNumberIterations=100; %Maximum number of optimization iterations
maxNumberFunctionEvaluations=maxNumberIterations*10; %Maximum number of function evaluations, N.B. multiple evaluations are used per iteration
functionTolerance=1e-25; %Tolerance on objective function value
parameterTolerance=0.001; %Tolerance on parameter variation
displayTypeIterations='iter';

objectiveStruct.method=2;

%File names of output files
output_names.load=fullfile(savePath,febioLogFileName_force);

objectiveStruct.run_output_names=output_names;

%% start optimization

Pn=P./objectiveStruct.parNormFactors;

switch objectiveStruct.method
    case 1 %fminsearch and Nelder-Mead
        OPT_options=optimset('fminsearch'); % 'Nelder-Mead simplex direct search'
        OPT_options = optimset(OPT_options,'MaxFunEvals',maxNumberFunctionEvaluations,...
            'MaxIter',maxNumberIterations,...
            'TolFun',functionTolerance,...
            'TolX',parameterTolerance,...
            'Display',displayTypeIterations,...
            'FinDiffRelStep',1e-2,...
            'DiffMaxChange',0.5);
        [Pn_opt,OPT_out.fval,OPT_out.exitflag,OPT_out.output]= fminsearch(@(Pn) objectiveFunctionIFEA(Pn,objectiveStruct,savePath),Pn,OPT_options);
    case 2 %lsqnonlin and Levenberg-Marquardt
        OPT_options = optimoptions(@lsqnonlin,'Algorithm','levenberg-marquardt');
        OPT_options = optimoptions(OPT_options,'MaxFunEvals',maxNumberFunctionEvaluations,...
            'MaxIter',maxNumberIterations,...
            'TolFun',functionTolerance,...
            'TolX',parameterTolerance,...
            'Display',displayTypeIterations,...
            'FinDiffRelStep',1e-2,...
            'DiffMaxChange',0.5);
        [Pn_opt,OPT_out.resnorm,OPT_out.residual]= lsqnonlin(@(Pn) objectiveFunctionIFEA(Pn,objectiveStruct,savePath),Pn,[],[],OPT_options);
end

%%
[Fopt,OPT_stats_out]=objectiveFunctionIFEA(Pn_opt,objectiveStruct,savePath);

%% Unnormalize and constrain parameters

P_opt=Pn_opt.*objectiveStruct.parNormFactors; %Scale back, undo normalization

%Constraining parameters
for q=1:1:numel(P)
    [P(q)]=boxconstrain(P(q),objectiveStruct.Pb_struct.xxlim(q,1),objectiveStruct.Pb_struct.xxlim(q,2),objectiveStruct.Pb_struct.xx_c(q));
end

disp_text=sprintf('%6.16e,',P_opt); disp_text=disp_text(1:end-1);
disp(['P_opt=',disp_text]);

%%
hf1=cFigure;
%title('displacement load curves optimised','FontSize',fontSize);
xlabel('Time (s)','FontSize',fontSize); ylabel('Force (N)','FontSize',fontSize); zlabel('Z','FontSize',fontSize); hold on;

Hn(1)=plot(OPT_stats_out.time_sim,OPT_stats_out.load_sim,'r.-','lineWidth',lineWidth*1.1,'markerSize',markerSize*1.3);
%Hn(1)=plot(OPT_stats_out.time_sim,OPT_stats_out.load_sim,'r-','lineWidth',lineWidth,'markerSize',markerSize);
%Hn(2)=plot(V_time,V_load,'g-','lineWidth',lineWidth);
Hn(2)=plot(V_time,V_load,'k-','lineWidth',lineWidth);

legend(Hn,{'Simulation','Experiment'},'Location','northwest');
view(2); axis tight;  grid on;
set(gca,'FontSize',fontSize);
drawnow;

function [Fopt,OPT_stats_out]=objectiveFunctionIFEA(Pn,objectiveStruct,savePath)

%%

febioFebFileName=objectiveStruct.febioFebFileName;
febio_spec=objectiveStruct.febio_spec;

%% Unnormalize and constrain parameters

P=Pn.*objectiveStruct.parNormFactors; %Scale back, undo normalization
P_in=P; %Proposed P

%Constraining parameters
for q=1:1:numel(P)
    [P(q)]=boxconstrain(P(q),objectiveStruct.Pb_struct.xxlim(q,1),objectiveStruct.Pb_struct.xxlim(q,2),objectiveStruct.Pb_struct.xx_c(q));
end

%% Setting material parameters

%Acces material parameters

mat_struct=objectiveStruct.mat_struct;
mat_struct.par_values={P(1) P(2) P(3) P(4) P(5) P(6) (P(3)+P(5))*objectiveStruct.k_factor};

disp('SETTING MATERIAL PARAMETERS...');
disp(['Proposed (norm.): ',sprintf(repmat('%6.6e ',[1,numel(Pn)]),Pn)]);
disp(['Proposed        : ',sprintf(repmat('%6.6e ',[1,numel(P_in)]),P_in)]);
disp(['Set (constr.)   : ',sprintf(repmat('%6.6e ',[1,numel(P)]),P)]);

%Assign material parameters
matId=mat_struct.id;
for q=1:1:2
    parNameNow=mat_struct.par_names{q};
    parValuesNow=mat_struct.par_values{q};
    febio_spec.Material.material{matId}.(parNameNow)=mat2strIntDouble(parValuesNow);
end

for q=3:1:numel(mat_struct.par_names)
    parNameNow=mat_struct.par_names{q};
    parValuesNow=mat_struct.par_values{q};
    febio_spec.Material.material{matId}.elastic{1}.(parNameNow)=mat2strIntDouble(parValuesNow);
end

%febView(febio_spec); %Viewing the febio file|

febioStruct2xml(febio_spec,febioFebFileName); %Exporting to file and domNode
disp('Done')

%% START FEBio

[runFlag]=runMonitorFEBio(objectiveStruct.febioAnalysis);

pause(0.1);

bcPrescribeList=objectiveStruct.bcPrescribeList;
time_exp=objectiveStruct.time_exp;
load_exp=objectiveStruct.load_exp;

if runFlag==1

    %Importing force from log file
    [time_mat, Force_mat,~]=importFEBio_logfile(objectiveStruct.run_output_names.load);
    time_mat=[0; time_mat(:)]; %Time
    Force_mat=Force_mat(:,2:end,:); %Remove index
    sizImport=size(Force_mat);
    sizImport(3)=sizImport(3)+1;
    Force_mat_n=zeros(sizImport);
    Force_mat_n(:,:,2:end)=Force_mat;
    Force_mat=Force_mat_n; %With added zeros for t=0

    %Calculated the simulated force
    FZ_set=Force_mat(bcPrescribeList,3,:);
    FZ_set=sum(FZ_set,1); %Calculate mean X force across nodes
    FZ_set=squeeze(FZ_set);

    time_sim=time_mat;
    load_sim=FZ_set*objectiveStruct.NumStrands; %Multiply force by number of strands

    if ~isempty(objectiveStruct.h)
        objectiveStruct.h.XData=time_sim;
        objectiveStruct.h.YData=load_sim;
        drawnow;
    end

    load_sim_exp = interp1(time_sim,load_sim,time_exp,'pchip');

    %Derive Fopt
    loadDev=load_exp-load_sim_exp;

    switch objectiveStruct.method
        case 1
            Fopt=sum((loadDev).^2)/length(loadDev); %Sum of squared differences
        case 2
            Fopt=sum((loadDev).^2)/length(loadDev);%loadDev(:); %Squared differences
    end

    OPT_stats_out.load_sim=load_sim;
    OPT_stats_out.time_sim=time_sim;
    OPT_stats_out.loadDev=loadDev;
    OPT_stats_out.Fopt=Fopt;

else %Output NaN
    switch objectiveStruct.method
        case 1
            Fopt=NaN;
        case 2
            Fopt=NaN(size(load_exp)); %Squared differences
    end
    OPT_stats_out=[];
end

end


classdef VaEA < ALGORITHM
% <multi/many> <real/integer/label/binary/permutation>
% Vector angle based evolutionary algorithm

%------------------------------- Reference --------------------------------
% Y. Xiang, Y. Zhou, M. Li, and Z. Chen, A vector angle-based evolutionary
% algorithm for unconstrained many-objective optimization, IEEE
% Transactions on Evolutionary Computation, 2017, 21(1): 131-152.
%------------------------------- 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".
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Generate random population
            Population = Problem.Initialization();

            %% Optimization
            while Algorithm.NotTerminated(Population)
                MatingPool = randi(Problem.N,1,Problem.N);
                Offspring  = OperatorGA(Problem,Population(MatingPool));
                Population = EnvironmentalSelection([Population,Offspring],Problem.N);
            end
        end
    end
end


% SCRIPT TEST to view a manipulability ellipse

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
close all
fprintf('\nView manipulability ellipse and forces ellipse at q')

robot = load_robot('UR', 'UR10')

%q = [pi/2 pi/4 -pi/4 0.4 0.5 -pi/4]';
q = [0.15 0.15 0.15 0.15 0.15 0.15]';

%Plot manipulatiliby ellipse
drawrobot3d(robot, q)
hold on
%draw manipulability ellipse
plot_manipulability_ellipse(robot, q, 0.5)
%draw forces ellipse
%plot_forces_ellipse(robot, q, 0.1)


% This file is part of the following project:
% J. Zico Kolter. Tommi Jaakkola.
% Approximate Inference in Additive Factorial HMMs with Application to Energy Disaggregation.
% In: International Conference on Artificial Intelligence and Statistics (AISTATS). 2012.
% Code provided as supplementary material
% Copyright: J. Zico Kolter, MIT CSAIL, 2014.

% Modified by Romano Cicchetti, ETH Zurich, in the context of the NILM-Eval project

function [data] = kolter(days, setup, fid)

    % load parameters
    dataset = setup.dataset;
    interval = setup.granularity;
    household = setup.household;
    lratio = setup.filtLRatio;

    % set variables
    edgeThreshold = 5;
    plevelMinLength = 5;

    % get total power consumption
    evaluation_days = days{1};
    true_power = read_smartmeter_data(dataset, num2str(household, '%02d'), evaluation_days, interval, 'powerallphases');

    % total variation denoising
    lmax = tvdiplmax(true_power);
    [data, ~, ~] = tvdip(true_power, lmax*lratio, 1, 1e-3, 100);
    data = data';

    % generate power levels
    plevel = generatePlevels(edgeThreshold, plevelMinLength, data, true_power);

    % generate snippets from power levels
    snippets = generateSnippetsFromPlevels(plevel);

    % generate HMMs from snippets
    HMMs = generateHMMsFromSnippets(snippets);

    % calculate probability of one snippet generating another
    numOfSnippets = length(snippets.mean);
    probsBetweenAllSnippets = zeros(numOfSnippets, numOfSnippets);
    for i = 1:numOfSnippets
        log_trans = log(cell2mat(HMMs.transition(i)));
        mu = cell2mat(HMMs.mean(i));
        sigma = cell2mat(HMMs.std(i));
        numOfStates = length(cell2mat(snippets.mean(i))) + 1;
        for j = 1:numOfSnippets
            if numOfStates ~= length(cell2mat(snippets.mean(j))) + 1
                continue;
            end
            firstPlevelOfSnippet = snippets.start(j);
            lastPlevelOfSnippet = snippets.end(j);
            observation = data(plevel.startidx(firstPlevelOfSnippet):plevel.endidx(lastPlevelOfSnippet)) - ...
                plevel.mean(firstPlevelOfSnippet - 1);
            emit = zeros(numOfStates, length(observation));
            for state = 1:numOfStates
                emit(state, :) = normpdf(observation, mu(state), sigma(state));
            end
            log_emit = log(emit);

            maxProb = zeros(numOfStates, length(observation));
            maxProb(:,1) = log(0);
            maxProb(1,1) = log(1);
            for t = 2:length(observation)
                for k = 1:numOfStates
                    sum = maxProb(:,t-1) + log_trans(:,k) + log_emit(k,t);
                    maxProb(k,t) = max(sum);
                end
                if all(isinf(maxProb(:,t)))
                    break;
                end
            end
            probsBetweenAllSnippets(i,j) = max(maxProb(:,length(observation)));
        end
    end

    % build adjacency Matrix from k-nearest neighbor graph
    numOfStatesOfSnippets = cellfun(@length, HMMs.mean);
    mu = {};
    P = {};
    for numOfStates = 2:5
        idxOfHMMs = find(numOfStatesOfSnippets == numOfStates);
        meansOfHMMs = HMMs.mean(idxOfHMMs);
        transitionOfHMMs = HMMs.transition(idxOfHMMs);
        probsBetweenSelectedSnippets = probsBetweenAllSnippets(idxOfHMMs, idxOfHMMs);
        adjacencyMatrix = zeros(size(probsBetweenSelectedSnippets));
        for i = 1:length(idxOfHMMs)
            idx_nonzero = find(probsBetweenSelectedSnippets(:,i) ~= 0);
            numSnippets = length(idx_nonzero);
            if numSnippets == 0
                continue;
            end
             [~,sortedIdx] = sort(probsBetweenSelectedSnippets(idx_nonzero,i), 'descend');
            sortedIdx = sortedIdx(1:1+floor(numSnippets/4));
            adjacencyMatrix(idx_nonzero(sortedIdx),i) = 1;
        end

        numOfClusters = floor(sqrt(length(idxOfHMMs)/2));
        if numOfClusters == 0
            continue;
        end
        clusters = SpectralClustering(adjacencyMatrix, numOfClusters, 1);

        for cl = 1:numOfClusters
            idx = clusters(:,cl) == 1;
            selectedMeans = cell2mat(meansOfHMMs(idx)');
            mu{end+1} = mean(selectedMeans,2)';
            selectedTrans = transitionOfHMMs(idx);
            Trans = cat(3,selectedTrans{:});
            P{end+1} = mean(Trans,3);
        end
    end

    % write cluster centorids (HMMs) to text file
    fprintf(fid,'%20s\n', 'MEANS:');
    for i = 1:length(mu)
        fprintf(fid, '%f\t', cell2mat(mu(i)));
        fprintf(fid, '\n');
    end

    % run AFAMAP algorithm
    n = 1;
    paramsAfamap = struct;
    paramsAfamap.max_iter = 1;
    paramsAfamap.lambda = Inf;
    paramsAfamap.dlambda = Inf;
    paramsAfamap.dSig = var(diff(data));
    paramsAfamap.Sig = var(data);
    [X0,Z,G] = myAfamap(plevel.mean', mu, plevel.duration', P, paramsAfamap);

end


filename='test2d_micro';
ptype = 'MICRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'horizontal';
cost={'chomog_alphabeta'};
weights=[1 0.1];
constraint = {'volumeConstraint'};
optimizer = 'SLERP';
incrementFactor = 1;
designVariable = 'LevelSet';
filterType = 'P1';

nsteps = 1;
Vfrac_final = 0.5;
Perimeter_target=1;
optimality_final =1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-3;
constr_initial = 1e-3;

TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;

%Micro
epsilon_isotropy_initial=1e-1;
epsilon_isotropy_final = 1e-3;
micro.alpha =[1 1 0]';
micro.beta =[1 1 0]';

% For all tests
plotting = false;
printing = false;
monitoring = false;
maxiter = 3;


%+========================================================================+
%|                                                                        |
%|            This script uses the GYPSILAB toolbox for Matlab            |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal, Marc Bakry (c) 2017-2019.                 |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%|             marc.bakry@polytechnique.edu                               |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : nrtHmxVibroSlab.m                             |
%|    #    |   VERSION    : 0.55                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal & Marc Bakry                  |
%|  ( # )  |   CREATION   : 14.03.2019                                    |
%|  / 0 \  |   LAST MODIF :                                               |
%| ( === ) |   SYNOPSIS   :                                               |
%|  `---'  |                                                              |
%+========================================================================+

% Cleaning
clear all
close all
clc

% Gypsilab path
run('../../addpathGypsilab.m')

% Parameters
tol = 1e-3
X0  = [0 0 1]
e   = 0.5;
f   = 900;

% Exterior domain (water)
rho0 = 1000;         % density      (kg.m3)
c0   = 1500;         % celerity     (m.s-1)
k0   = 2*pi/c0*f;    % wave-number  (m-1)
lam0 = c0/f;         % wave-length  (m)

% Interior domain (different from water)
rhoS = 2*rho0;                                 % density                             (kg.m3)
cL   = 2*c0;                                   % celerity of longitudinal waves      (m.s-1)
kL   = 2*pi*f/cL;                              % wave-number of longitudinal waves   (m-1)
lamL = real(cL)/f;                             % wavelength of longitudinal waves    (m)
cT   = 0;                                      % celerity of transverse waves        (m.s-1)
kT   = 2*pi.*f/cT;                             % wave-number of transverse waves     (m-1)
lamT = real(cT)/f;                             % wavelength of transverse waves

% Minimum wavelength
tmp  = [lam0,lamL,lamT];
lmin = min(tmp(tmp>0));

% Slab mesh
L    = 20 * lmin;            % 20 wavelength to simulate infinite slab
nx   = ceil(L/lmin * 6)+1;   % 6 node per wavelength for L
ny   = ceil(L/lmin * 6)+1;   % 6 node per wavelength for L
nz   = ceil(e/lmin * 12)+1;  % 12 node per wavelength for e
N    = nx * ny * nz;         % Total number of nodes
mesh = mshCube(N,[L L e])

% Boundary
bound = mesh.bnd

% Admissible frequency
stp  = mesh.stp;
kmax = 1/stp(2);
if (k0 > kmax) || (kL > kmax/2) || ((kT>kmax/2) && ~isinf(kT))
%     error('frequency is too high for mesh discretization')
end

% Radiative mesh (fixed number of nodes)
radiatx     = mshSquare(1e3,[L L]);
radiaty     = radiatx;
radiatx.vtx = radiatx.vtx(:,[1 3 2]);
radiaty.vtx = radiaty.vtx(:,[3 1 2]);
radiat      = union(radiatx,radiaty);

% Measurement points for trans and refl coeff (1 wavelenth from bound)
Xmes = [0 0 -e/2-lmin ; 0 0 e/2+lmin];

% Cut-off function (50% full, 10% decrease)
cutoffx = vibsCutoff(1,L/5,L/10);
cutoffy = vibsCutoff(2,L/5,L/10);

% Green kernel function
Gxy         = @(X,Y) femGreenKernel(X,Y,'[exp(ikr)/r]',k0);
gradyGxy{1} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]1',k0);
gradyGxy{2} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]2',k0);
gradyGxy{3} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]3',k0);
G0          = '[1/r]';
gradyG0     = 'grady[1/r]';
cteGxy      =  1/(4*pi);
cteG0       =  1/(4*pi);

% Plane wave function
PW         = @(X) exp(1i*k0*X*X0') .* cutoffx(X) .* cutoffy(X);
gradxPW{1} = @(X) 1i*k0*X0(1) .* PW(X);
gradxPW{2} = @(X) 1i*k0*X0(2) .* PW(X);
gradxPW{3} = @(X) 1i*k0*X0(3) .* PW(X);

% Coupling coeff for Brackage-Werner simulation
beta = 1i*k0;

% Graphical representation
figure
plot(bound,'r')
hold on
plot(radiat,log(abs(PW(radiat.vtx))))
% plotNrm(bound,'w')
plot(msh(Xmes),'k')
axis equal
colorbar
view(45,10)

% Quadrature and finite elements (volumn)
omega = dom(mesh,4);
U     = fem(mesh,'P1');

% Quadrature and finite elements (boundary)
sigma = dom(bound,3);
u     = fem(bound,'P1');

% Left-hand side
tic
[A,B,C,D] = vibsHmxBlockOperator(omega,U,sigma,u,cL,cT,rhoS,c0,rho0,f,tol);
toc

% Add dirichlet condition to x and y unknows (penalization)
A(sub2ind(size(A),1:2*length(U),1:2*length(U))) = 1e15;

% Right-hand side
V    = cell(4,1);
V{1} = - integral(sigma,ntimes(U,1),PW);
V{2} = - integral(sigma,ntimes(U,2),PW);
V{3} = - integral(sigma,ntimes(U,3),PW);
V{4} = integral(sigma,ntimes(u),gradxPW);

% Resolution with Schur complement
Fa     = decomposition(A);
LHS    = @(V) D*V - C*(Fa \ (B.Ml*(B.Mr*V)) );
RHS    = V{end} - C*(Fa \ cell2mat(V(1:end-1)) );
mu     = mgcr(LHS,RHS,[],tol,100);
lambda = beta*mu;

% Measure of refexive and transmitted coeff
tic
Pmes = cteGxy .* integral(Xmes,sigma,Gxy,u)*lambda - ...
    cteGxy .* integral(Xmes,sigma,gradyGxy,ntimes(u))*mu;
Pmes(2) = Pmes(2) + PW(Xmes(2,:));
toc

% Finite element radiative operator --> \int_Sy G(x,y) psi(y) dy
tic
Srad = cteGxy .* integral(radiat.vtx,sigma,Gxy,u,tol);
Srad = Srad + cteG0 .* regularize(radiat.vtx,sigma,G0,u);

% Finite element radiative operator --> \int_Sy dnyG(x,y) psi(y) dy
Drad = cteGxy .* integral(radiat.vtx,sigma,gradyGxy,ntimes(u),tol);
Drad = Drad + cteG0 .* regularize(radiat.vtx,sigma,gradyG0,ntimes(u));
toc

% Domain solution
Psca = Srad*lambda - Drad*mu;
Pinc = PW(radiat.vtx);
Ptot = Psca + Pinc;

% Graphical repesentation
figure
subplot(1,2,1)
plot(bound)
hold on
plot(radiat,abs(Psca))
plot(msh(Xmes),abs(Pmes))
title('Scattered')
axis equal
colorbar
view(45,10)

subplot(1,2,2)
plot(bound)
hold on
plot(radiat,abs(Ptot))
plot(msh(Xmes),abs(Pmes))
title('Total')
axis equal
colorbar
view(45,10)

% Analytical solution
tic
c     = ones(length(f),1) * [c0 cL c0];
rho   = [rho0 rhoS rho0];
[R,T] = slabVibro(f,rho,c,e);
toc

% Comparison (db)
ref = 20*log10(abs([R ; T]));
sol = 20*log10(abs(Pmes));

norm(ref-sol)/norm(ref)

disp('~~> Michto gypsilab !')


function SR = superresolve_irwsr(slidingWindow, magFactor)

    model = SRModel;
    model.magFactor = magFactor;
    model.psfWidth = 0.4;
    model.motionParams = slidingWindow.flowToReference;
    model.SR = imresize(slidingWindow.referenceFrame, magFactor);

    model.imagePrior = SRPrior('function', @btvPriorWeighted, 'gradient', @btvPriorWeighted_grad, 'weight', [], 'parameters', {size(model.SR), 1, 0.5, []});
    reweightedOptimParams = getReweightedOptimizationParams;
    SR = superresolve(slidingWindow.frames, model, 'reweightedSR', reweightedOptimParams);


function  [value, isterminal, direction] = events_flight(~,Z,P)

LL = P.FullLength;
th = Z(1,:);
y0 = Z(3,:);
y2 = y0 + LL*cos(th);

%%%% CONSTRAINTS %%%%
% 1) y0 > 0
% 2) y2 > 0

% As a rule, each constraint will be satisfied if it's event function value
% is positive. This makes things easier at the FSM level.
n = length(th);
value = zeros(2,n);
isterminal = true(size(value));
direction = -ones(size(value));

value(1,:) = y0;
value(2,:) = y2;

end


function nlp = Step1Constraints(nlp, bounds, varargin)
    domain = nlp.Plant;

    %% virtual constraints
    nlp = CstrFcns.ImposeVirtualConstraint(nlp, bounds);

    %% initial position
    q0 = bounds.initialPos;
    dq0 = bounds.initialVel;

    updateVariableProp(nlp,'x','first','lb',q0,'ub',q0,'x0',q0);
    updateVariableProp(nlp,'dx','first','lb',dq0,'ub',dq0,'x0',dq0);

    %% torso

    torso = CoordinateFrame('Name','torso',...
        'Reference',nlp.Plant.Joints(end),...
        'Offset',[0,0,0.3],...
        'R',[0,0,0]);
    rpy = getEulerAngles(domain, torso);
    constr_fun = SymFunction(['torso_',domain.Name],rpy(2),{domain.States.x});
    lb = bounds.torso.lb;
    ub = bounds.torso.ub;
    addNodeConstraint(nlp,constr_fun,{'x'},'all', lb, ub,'Linear');
    %% initial position
    %     q0 = bounds.restPos;
    %     dq_minus = zeros(6,1);
    %     %     dq0 = bounds.restVel;
    %     %     ddq0 = bounds.restAcc;
    %     robot = nlp.Plant;
    %     D = robot.calcMassMatrix(q0);
    %     Jfoot = Jh_foot_acrobot(q0);
    %     Jimp = jac_push_acrobot(q0);
    %
    %     E = [D -Jfoot';Jfoot, zeros(3,3)];
    %     F = [D*dq_minus + Jimp'*bounds.push_force;
    %         zeros(3,1)];
    %
    %     y = E\F;
    %
    %     dq0 = y(1:6);
    %
    %
    %     updateVariableProp(nlp,'x','first','lb',q0,'ub',q0,'x0',q0);
    %     updateVariableProp(nlp,'dx','first','lb',dq0,'ub',dq0,'x0',dq0);
    %     updateVariableProp(nlp,'ddx','first','lb',ddq0,'ub',ddq0,'x0',ddq0);

    %     updateVariableProp(nlp,'x','last','lb',q0,'ub',q0,'x0',q0);
    %     updateVariableProp(nlp,'dx','last','lb',dq0,'ub',dq0,'x0',dq0);
    %     updateVariableProp(nlp,'ddx','last','lb',ddq0,'ub',ddq0,'x0',ddq0);

    %     D = nlp.Plant.Mmat;
    %     q = nlp.Plant.States.x;
    %     dq = nlp.Plant.States.dx;
    %
    %     f_push = CoordinateFrame('Name','push',...
    %         'Reference',nlp.Plant.Joints(end),...
    %         'Offset',[0,0,0.3],...
    %         'R',[0,0,0]);
    %
    %     %     p_push_sym = getCartesianPosition(robot,f_push);
    %     J_push_sym = getSpatialJacobian(nlp.Plant,f_push);
    %     J = J_push_sym([1,3],:);
    %
    %     F = SymVariable('f',[2,1]);
    %
    %     nlp.addVariable('fg','first','Name','fg','Dimension',3,...
    %         'lb',[-inf,0,-inf],'ub',[inf,inf,inf],'x0',[0,0,0]);
    %     Fg = SymVariable('fg',[3,1]);
    %     Jg = nlp.Plant.Gmap.ConstraintWrench.ffoot;
    %     constr = D*dq - J.'*F + Jg*Fg;
    %     constr_fun = SymFunction(['impact_',nlp.Plant.Name],constr,{q,dq,Fg},{F});
    %     addNodeConstraint(nlp,constr_fun,{'x','dx','fg'},'first',0,0,'Nonlinear',bounds.push_force);
end


%
% Stochastic function that generates new examples from the prior,
% given a type-level specification from generate_character().
%
% Input
%  template: MotorProgram class with R,ids,shapes_type,invscales_types
%    defined
%
% Output
%  M: fully-specified MotorProgram
function [M,template] = generate_exemplar(template,libclass)

    M = MotorProgram(template);

    % sample stroke parameters
    for i=1:M.ns
        if strcmp(M.S{i}.R.type,'mid')
            M.S{i}.R.eval_spot_token = CPD.sample_relation_token(libclass,M.S{i}.R.eval_spot_type);
        end
        M.S{i}.pos_token = CPD.sample_position(libclass,M.S{i}.R,M.S(1:i-1));
        M.S{i}.shapes_token = CPD.sample_shape_token(libclass,M.S{i}.shapes_type);
        M.S{i}.invscales_token = CPD.sample_invscale_token(libclass,M.S{i}.invscales_type);
    end

    % sample affine warp
    M.A = CPD.sample_affine(libclass);

    % set rendering parameters to minimum noise
    M.blur_sigma = template.parameters.min_blur_sigma;
    M.epsilon = template.parameters.min_epsilon;

    % sample rendering parameters
    %M.blur_sigma = CPD.sample_image_blur(template.parameters);
    %M.epsilon = CPD.sample_image_noise(template.parameters);

    % sample the image
    M.I = CPD.sample_image(M.pimg);

end


function [C,top, bottom, left, right] = imtrim(im,varargin)
  % IMTRIM auto-crop an image like Photoshop's Edit>Trim feature, as of yet only
  % grayscale imagse are supported
  %
  % C = imtrim(im)
  % [C,top,bottom,left,right] = imtrim(im,'ParamName',ParamValue, ...)
  %
  % Inputs:
  %   im  h by w by nc image
  %   Optional:
  %     'Location'  followed by {'NorthWest'} or 'SouthEast' specifying where
  %       to find trim color
  %     'Dims' followed by 2 long logic whether to use each dimension: [1 0]
  %       means only crop in h and not in w
  %     'Threshold' followed by distance threshold {0.1}
  %     'Map'  followed by colormap {[]}
  % Outputs:
  %   C  sh by wh cropped image
  %   top  index of top pixel in im
  %   bottom  index of bottom pixel in im
  %   left  index of left pixel in im
  %   righ  index of right pixel in im
  %
  %   Copyright Alec Jacobson, 2010
  %
  % Example:
  %  % Load in a one-way animation
  %  [X,M] = imread('bunny.gif');
  %  % Trim to animations extent
  %  Y = imtrim(X,'Map',M,'Threshold',0);
  %  writeGIF('output.gif',Y,M,0);
  %

  location = 'NorthWest';
  threshold = [];
  dims = [1 1];
  map = [];

  % parse optional input parameters
  v = 1;
  while v < numel(varargin)
    switch varargin{v}
    case 'Map'
      assert(v+1<=numel(varargin));
      v = v+1;
      map = varargin{v};
    case 'Location'
      assert(v+1<=numel(varargin));
      v = v+1;
      location = varargin{v};
    case 'Threshold'
      assert(v+1<=numel(varargin));
      v = v+1;
      threshold = varargin{v};
    case 'Dims'
      assert(v+1<=numel(varargin));
      v = v+1;
      dims = varargin{v};
    otherwise
      error('Unsupported parameter: %s',varargin{v});
    end
    v = v+1;
  end

  if isempty(threshold)
    % default threshold parameter, works equivalently with Photoshop's
    % hardcoded parameter
    threshold = 0.1;
  end

  im_in = im;

  if ~isempty(map)
    nf = size(im,4);
    im = cell2mat(permute(arrayfun(@(C) ind2rgb(im(:,:,:,C),map(:,:,min(end,C))),1:size(im,4),'UniformOutput',false),[1 4 3 2]));
  end

  if isempty(map) && ~isfloat(im)
    warning('converting input to double');
    im = im2double(im);
  end

  % gather corner value to which the image is compared
  if(strcmp(location,'NorthWest'))
    corner_value = im(1,1,:,1);
  elseif(strcmp(location,'SouthEast'))
    corner_value = im(1,1,:,1);
  else
    error([location ' is not a valid location']);
  end

  % get difference of image with corner value
  %difference = abs(im - corner_value)>0.1;
  % should work for any number of channels
  %difference = sqrt(sum((im - corner_value).^2,3)) > ...
  %  sqrt(threshold^2*size(im,3));
  difference = sqrt(sum(bsxfun(@minus,im,corner_value).^2,3)) > ...
    sqrt(threshold^2*size(im,3));
  difference = max(difference,[],4);

  if dims(2)
    [left_i,left] = ind2sub(size(difference),find(difference,1));
    [right_i,right] = ind2sub(size(difference),find(difference,1,'last'));
  else
    left = 1;
    right = size(im,2);
  end
  if dims(1)
    [top_j,top] = ind2sub(size(difference'),find(difference',1));
    [bottom_j,bottom] = ind2sub(size(difference'),find(difference',1,'last'));
  else
    top = 1;
    bottom = size(im,1);
  end
  C = im_in(top:bottom,left:right,:,:);
end


function y = stepLTEMIMOChan(in, prmLTEPDSCH)
%#codegen

% Get simulation params
numTx = prmLTEPDSCH.numTx;
numRx = prmLTEPDSCH.numRx;
chanSRate = prmLTEPDSCH.chanSRate;
SampleTime = 1/chanSRate;

EPAPathDelays = [0 30 70 90 110 190 410]*1e-9;
EPAPathGains  = [0 -1 -2 -3 -8 -17.2 -20.8];
EVAPathDelays = [0 30 150 310 370 710 1090 1730 2510]*1e-9;
EVAPathGains  = [0 -1.5 -1.4 -3.6 -0.6 -9.1 -7 -12 -16.9];
ETUPathDelays = [0 50 120 200 230 500 1600 2300 5000]*1e-9;
ETUPathGains  = [-1 -1 -1 0 0 0 -3 -5 -7];

switch prmLTEPDSCH.chanMdl
    case 'Static MIMO'
        PathDelays          = 0;
        AveragePathGains    = 0;
        MaximumDopplerShift = 0;
    case 'EPA 5Hz'
        PathDelays          = EPAPathDelays;
        AveragePathGains    = EPAPathGains;
        MaximumDopplerShift = 5;
    case 'EVA 5Hz'
        PathDelays          = EVAPathDelays;
        AveragePathGains    = EVAPathGains;
        MaximumDopplerShift = 5;
    case 'EVA 70Hz'
        PathDelays          = EVAPathDelays;
        AveragePathGains    = EVAPathGains;
        MaximumDopplerShift = 70;
    case 'ETU 70Hz'
        PathDelays          = ETUPathDelays;
        AveragePathGains    = ETUPathGains;
        MaximumDopplerShift = 70;
    case 'ETU 300Hz'
        PathDelays          = ETUPathDelays;
        AveragePathGains    = ETUPathGains;
        MaximumDopplerShift = 300;
end

% Create the MIMO Channel object
persistent chanObj;
if isempty(chanObj)
    chanObj = mimochan(numTx, numRx, SampleTime, ...
                       MaximumDopplerShift, PathDelays, AveragePathGains);
end

% Process data through MIMO Channel
y  = filter(chanObj, in);


% Creates input parameter structure array needed to run "treeqsm" function
% and "filtering" function.
% NOTE: use this code to define all the parameters but the PatchDiam and
% BallRad parameters can be conveniently defined by "define_input"
% function.
%
% Last update 11 May 2022

clear inputs

%% QSM reconstruction parameters
%%% THE THREE INPUT PARAMETERS TO BE OPTIMIZED.
% These CAN BE VARIED AND SHOULD BE OPTIMIZED
% One possibility to define these is to use "define_input" code
% (These can have multiple values given as vectors, e.g. [0.01 0.02]).
% Patch size of the first uniform-size cover:
inputs.PatchDiam1 = [0.08 0.12];
% Minimum patch size of the cover sets in the second cover:
inputs.PatchDiam2Min = [0.02 0.03];
% Maximum cover set size in the stem's base in the second cover:
inputs.PatchDiam2Max = [0.07 0.1];

%%% ADDITIONAL PATCH GENERATION PARAMETERS.
% The following parameters CAN BE VARIED BUT CAN BE USUALLY KEPT AS SHOWN
% (i.e. little bigger than PatchDiam parameters).
% One possibility to define these is to use "define_input" code
% Ball radius in the first uniform-size cover generation:
inputs.BallRad1 = inputs.PatchDiam1+0.015;
% Maximum ball radius in the second cover generation:
inputs.BallRad2 = inputs.PatchDiam2Max+0.01;

% The following parameters CAN BE USUALLY KEPT FIXED as shown.
% Minimum number of points in BallRad1-balls, generally good value is 3:
inputs.nmin1 = 3;
% Minimum number of points in BallRad2-balls, generally good value is 1:
inputs.nmin2 = 1;
% Does the point cloud contain points only from the tree (if 1, then yes):
inputs.OnlyTree = 1;
% Produce a triangulation of the stem's bottom part up to the first main
% branch (if 1, then yes):
inputs.Tria = 0;
% Compute the point-model distances (if 1, then yes):
inputs.Dist = 1;

%%% RADIUS CORRECTION OPTIONS FOR MODIFYING TOO LARGE AND TOO SMALL CYLINDERS.
% These parameters CAN BE USUALLY KEPT FIXED as shown.
% Traditional TreeQSM choices:
% Minimum cylinder radius, used particularly in the taper corrections:
inputs.MinCylRad = 0.0025;
% Radius correction based on radius of the parent. If 1, radii in a branch
% are always smaller than the radius of the parent in the parent branch:
inputs.ParentCor = 1;
% Parabola taper correction of radii inside branches. If 1, use the
% correction:
inputs.TaperCor = 1;

% Growth volume correction approach introduced by Jan Hackenberg,
% allometry: Radius = a*GrowthVol^b+c
inputs.GrowthVolCor = 0; % If 1, use growth volume (GV) correction
% fac-parameter of the GV-approach, defines upper and lower bound. When
% using GV-approach, consider setting TaperCorr = 0, ParentCorr = 0,
% MinCylinderRadius = 0.
inputs.GrowthVolFac = 1.5; % Defines the allowed radius:
%  1/fac*predicted_radius <= radius <= fac*predicted_radius
%  However, the radii of the branch tip cylinders are not increased.

%% Filtering parameters
% NOTE: These are all optional, but needed to run the "filtering" function.
% Statistical k-nearest neighbor distance outlier filtering, applied if
% filter.k > 0. The value filter.k is the number of nearest neighbors.
inputs.filter.k = 10;
% Statistical point density outlier filtering, applied if filter.radius > 0.
% The value filter.radius is the radius of the ball neighborhood. This is
% usually meant as alternative to the above knn-filtering.
inputs.filter.radius = 0.00;
% The value filter.nsigma is the multiplier of the standard deviation of
% the kth-nearest neighbor distance/point density and points whose
% kth-nearest neighbor distance/point density is larger/lower than the
% average +/- filter.nsigma * std are removed:
inputs.filter.nsigma = 1.5;
% Small component filtering is applied if filter.ncomp > 0. This filter is
% based on cover whose patches are defined by filter.PatchDiam1 and
% filter.BallRad1. The points which are included in components that have
% less than filter.ncomp patches are removed:
inputs.filter.PatchDiam1 = 0.05;
inputs.filter.BallRad1 = 0.075;
inputs.filter.ncomp = 2;
% Cubical downsampling is applied if filter.EdgeLength > 0.
% The value filter.EdgeLength is the length of the cube edges:
inputs.filter.EdgeLength = 0.004;
% Plot the filtering results automatically after the filtering if
% filter.plot > 0
inputs.filter.plot = 1;

%% Other inputs
% These parameters don't affect the QSM-reconstruction but define what is
% saved, plotted, and displayed and how the models are named/indexed
% Name string for saving output files and naming models:
inputs.name = 'tree';
% Tree index. If modelling multiple trees, then they can be indexed uniquely:
inputs.tree = 1;
% Model index, can separate models if multiple models with the same inputs:
inputs.model = 1;
% Save the output struct QSM as a matlab-file into \result folder.
% If name = 'pine', tree = 2, model = 5, the name of the saved file is
% 'QSM_pine_t2_m5.mat':
inputs.savemat = 1;
% Save the models in .txt-files (check "save_model_text.m"):
inputs.savetxt = 1;
% What are plotted during reconstruction process:
% 2 = plots the QSM, the segmentated point cloud and distributions,
% 1 = plots the QSM and the segmentated point cloud
% 0 = plots nothing
inputs.plot = 2;
% What are displayed during the reconstruction: 2 = display all;
% 1 = display name, parameters and distances; 0 = display only the name:
inputs.disp = 2;


clear;clc;close all;
path(path,'toolbox') ;

sk_filename ='../result/simplejoint_v4770_contract_t(4)_nn(30)_WL(9.621282)_WH(1.000000)_sl(3.000000)_skeleton.mat';

%%
SHOW_JOINTS = true;
SHOW_ROOT_JOINT = true;
SHOW_CYCLES = true;
SHOW_IRRELEVANT_EXTRAMA = true;

t1 = 0.1; % for inner branch nodes
a1 = pi*5.0/7.0; % for inner branch nodes,
t2 = 0.1; % for irrelevant extrama;
t3 = 6; % for small cycles;
sprintf('thresholds for remove 1)inner nodes: %f angle, %f length \n 2)irrelevant extrama: %f\n3)small cycles: %d \n',t1, a1, t2, t3)

load(sk_filename,'P');
figure;set(gcf,'color','white');movegui('northwest');set(gcf,'Renderer','OpenGL');view3d rot;
scatter3(P.pts(:,1),P.pts(:,2),P.pts(:,3),20,'.','MarkerEdgeColor', GS.PC_COLOR);hold on;
showoptions.sizep=600;showoptions.sizee=3;
plot_skeleton(P.spls,P.spls_adj,showoptions);
axis off; axis equal; camorbit(0,0,'camera'); axis vis3d; view(-90,0);
%% 0: find joints and NaN, first
[joints, segments] = find_joints(P.pts, P.spls, P.corresp, P.spls_adj, SHOW_JOINTS);
%% 1: removing small cycles measured by topological length
[P.spls, P.corresp, P.spls_adj, joints, segments] = remove_small_cycles(P.spls, P.corresp, P.spls_adj, joints, segments,t3, SHOW_CYCLES);
%% 2: find root joint, global distance relative to root_id, "size of skeleton"
[root_id, global_dist] = find_root_node(P.spls, P.spls_adj, joints, SHOW_ROOT_JOINT);
%% 3: unify joints if there are no branch node inbetween of them. The effect is bad for some cases.
if ~isempty(joints)
    [P.spls, P.corresp, P.spls_adj, joints, root_id] = merge_nearby_joints(P.spls, P.corresp, P.spls_adj, joints, root_id);
end
%% 4: removing irrelevant extrama
[P.spls, P.corresp, P.spls_adj, joints, segments] = remove_irrelevant_extrama(P.spls, P.corresp, P.spls_adj, joints, segments,global_dist, t2, SHOW_IRRELEVANT_EXTRAMA);
%% 5: removing inner branch nodes
[P.spls, P.corresp, P.spls_adj, joints, segments] = remove_inner_nodes(P.spls, P.corresp, P.spls_adj, joints, segments,global_dist,t1,a1);
%% 6: form hierarchy, and rebuild skel_adj remove null node.
P.root_id = root_id;% The first node is root node if there is no joints.
% [P, graph] = build_hierarchy(P,global_dist); % do not contain cycles
[P.spls,P.corresp,P.spls_adj, graph] = build_graph(P.spls,P.corresp,P.spls_adj);% may contain cycles
%% 7: move skeleton nodes to center of its correspondence point cloud
figure('Name','Build hierarchy','NumberTitle','off');set(gcf,'color','white');hold on; movegui('south');
plot_skeleton(P.spls,P.spls_adj);
%scatter3(P.pts(:,1),P.pts(:,2),P.pts(:,3),60,'.','MarkerEdgeColor', GS.PC_COLOR);hold on;
for i = 1:size(P.spls,1)
    verts = P.pts(P.corresp==i,:);
    if size(verts,1) == 1
        P.spls(i,:) = verts;
    else
        P.spls(i,:) = mean(verts);
    end
end
for i = 1:size(P.spls_adj,1)
    links = find( P.spls_adj(i,:)==1 );
    if length(links) == 2
        verts = P.spls(links,:);
        P.spls(i,:) = mean(verts);
    end
end
plot_embedded_graph(P.spls, graph, 'b', 'linewidth', 2);
axis off; axis equal; camorbit(0,0,'camera'); axis vis3d; view(-90,0);view3d rot;
%% finally
figure;set(gcf,'color','white');movegui('northwest');set(gcf,'Renderer','OpenGL');view3d rot;
scatter3(P.pts(:,1),P.pts(:,2),P.pts(:,3),20,'.','MarkerEdgeColor', GS.PC_COLOR);hold on;
plot_skeleton(P.spls, P.spls_adj);
axis off; axis equal; camorbit(0,0,'camera'); axis vis3d; view(0,90);
%%
% P.skel_dist = global_dist;
%P = rmfield(P, 'knn_idx');
%P = rmfield(P, 'knn_dist');

% write_cg();
fid = fopen([sk_filename(1:end-4), '.cg'],'w');
if( fid~=-1 )
    fprintf(fid, '# D:3 NV:%d NE:%d\n', size(P.spls,1), size(graph,1));
    fprintf(fid, 'v %f %f %f\n', P.spls');
    fprintf(fid, 'e %d %d\n', graph');
    fclose(fid);
end

% write_mapping();
fid = fopen([sk_filename(1:end-4), '.map'],'w');
if( fid~=-1 )
    fprintf(fid, '%d\n', P.corresp);
    fclose(fid);
end
save([sk_filename(1:end-4), '_r.mat'], 'P');


% J. C. Spall, August 1999
% Written in support of text, Introduction to Stochastic Search and Optimization, 2003
% This program runs a GA with the tournament parent selection scheme
% used.  Elitism is included.  Parent selection includes the elitist
% elements.  The standard binary bit
% form is used here.  As usual, code works in terms of fitness
% values (higher better); results, however, are reported for the loss
% values of actual interest.  This code does not work with constraints
% on theta values other than those directly associated with thetamin,
% thetamax.
%
% This code calls the functions 'bit2num','num2bit',and'fitpop'.
%
% The user is expected to set a variable 'expect_fn' representing the
% expected number of function evaluations allowed.  The main "while"
% loop in the code tests for when the expected number of function
% evaluations exceeds 'expect_fn' and terminates the iteration on the
% first generation that would require a no. of function evaluations
% exceeding this value.  The test in the "while" loop uses a formula
% in ISSO, Chap. 10; the "identical" variable in the formula is replaced
% here by an actual running calculation of the identical total.  Note
% that the code actually makes more function evaluations than the
% expected total due to the desire to avoid excessive bookkeeping and
% slowing of the code.  However, the IDEA of terminating when the
% expecting running total exceeds a threshold is consistent with the
% idea of placing a premium on possibly expensive function evaluations.
%
% This code handles noisy loss functions via the fact that the variables
% 'loss' and 'lossfinal' can call different functions.  The global
% variable 'sigma' is used to generate the scaling for noise in the
% loss measurements.
%
clear all
global p N thetamin thetamax
cases=15;         %no. of Monte Carlo cases
N = 40;           % population size
p=10;             % dimension of theta
P_c =.6;          % crossover rate
P_m =0.0001;      % mutation rate
elite=2;                % no. of population elements for "elitism"
                        % (should pick s.t. N-elite = even no.)
expect_fn=4000;         % expected no. of function evaluations
M=4*ones(p,1);          % no. of places to right of decimal
thetamin=-1.6383*ones(p,1);  %lower bounds on theta elements
thetamax=1.6384*ones(p,1);   %upper bounds: ditto
thetamin_0=-1.6383*ones(p,1);   %lower bounds on theta for initialization
thetamax_0=1.6384*ones(p,1);    %upper bounds: ditto
lossfinaleval ='loss4thorder';    % choice of loss function for final perf.
                              % evaluation
loss='loss4thnoise';
lossfinalsq=0;          %variable for cum.(over 'cases')squared loss values
lossfinal=0;            %variable for cum. loss values
rand('seed',31415927);
randn('seed',3111113)
global z sigma;       %declaration of random var. used for normal noise
                      %generation in loss fn. calls given seed above;
                      %also sigma in noise (noise is dependent on theta)
sigma=0;            %multiplier in loss noise (lower bd. to s.d.)
fitness=zeros(N,1);                %dim.-setting statement
theta=zeros(p,1);                  %dim.-setting statement
% User warning if invalid N-elite value
if (round((N-elite)/2)~=(N-elite)/2)
   disp('WARNING: N-elite should be even number');
else
end
%
% Determination of no. bits/theta element
bits=zeros(p,1);
for i=1:p
   b=1;
   while 10^M(i)*(thetamax(i)-thetamin(i))>2^b-1
      b=b+1;
   end
   bits(i)=b;
end
length=ones(1,p)*bits;              %no. of bits/population element
thetabit=zeros(N,length);  %dummy statement setting dimensions of matrix
                           %containing bit values of all chromosomes
thetabit_noelite=zeros(N-elite,length);%dummy statement setting
                                       %dimension of matrix used to temp.
                                       %store offspring
parent1=zeros(1,length); %dummy dimension statement
parent2=zeros(1,length); %ditto
% Begin outer loop of 'cases' Monte Carlo runs
%
navg=0;
for c=1:cases
   c
   %
   % Initial Random Population
   %
   % Statements below allow intialization of the search using a random
   % placement in natural theta space.  Points are placed uniformly dist.
   % on the hypercube defined by thetamin_0, thetamax_0.  The points are
   % converted to a bit representation for use by the GA.
   %
   for i=1:N
      theta=(thetamax_0-thetamin_0).*rand(p,1)+thetamin_0;
      pointer=1;
      for j=1:p
         thetabit(i,pointer:pointer+bits(j)-1)=num2bit(theta(j),bits(j),...
            thetamin(j),thetamax(j));
         pointer=pointer+bits(j);
      end
   end

%thetabit = rand(N,length) > 0.5;  % sets array of 0/1 for
                                  % all N chromosomes
%loop below is included for testing purposes and artifical insertion of
%known solution (to ensure that alg. properly handles this)
%for i=1:bits(1):p*bits(1)
%   thetabit(N,i:i+bits(1)-1)=[0,1,1,1,1,1,1,1,1,1,1,1,1,1,1];
%end
%zer=bit2num(thetabit(N,1:1+bits(1)-1),bits(1),M(1),thetamin(1),thetamax(1))
%upper = zeros(n, 1);              % fills vector for reporting loss
%average = zeros(n, 1);            % ditto
%lower = zeros(n, 1);              % ditto
   fitness=fitpop(thetabit,bits,loss);%N by 1 vector of fitness values
   [bestfitness,bestindex]=max(fitness);  bestindex
   % Loop below produces the theta associated with the best fitness value
   pointer=1;
   for j=1:p
      theta(j)=bit2num(thetabit(bestindex,pointer:pointer+bits(j)-1),...
            bits(j),thetamin(j),thetamax(j));
      pointer=pointer+bits(j);
   end
   %
   % Main loop of GA
   %
   index=zeros(elite,1);
   %cumfit=zeros(N-elite,1);
   identical=0;   %initialization of variable that keeps track of no.
                  %of identical parents in selection pairs (due to
                  %resampling)
   n=1;
   while n< 1+(expect_fn-N+identical*(1-(1-P_m)^length))/(P_c*(N-elite)...
         +(1-P_c)*(N-elite)*(1-(1-P_m)^length))
      % Evaluate obj. function for each individual (an N-dim.vector)
      %fitness = fitpop(thetabit,bits,loss); %higher better
      % Invoke elitism for 'elite' best chromosomes
      temp_fit=fitness;
      for j=1:elite
         [junk,index(j)]=max(temp_fit);
         temp_fit(index(j))=min(temp_fit);
      end
      for j=1:2:N-elite-1
         % Tournament selection below.  Uses tournament size = 2.
         for k=1:2
            cand=ceil(N*rand(2,1));
            if fitness(cand(1))>fitness(cand(2))
               parent1_idx=cand(1);
            else
               parent1_idx=cand(2);
            end
            cand=ceil(N*rand(2,1));
            if fitness(cand(1))>fitness(cand(2))
               parent2_idx=cand(1);
            else
               parent2_idx=cand(2);
            end
         end
      % Crossover with above two parents
         if rand < P_c
            cross_point=ceil(rand*(length-1)); %an integer 1,...,length-1
            parent1=thetabit(parent1_idx,:);
            %above picks off the chrom. for parent1_idx
            parent2=thetabit(parent2_idx,:);
            identical=identical+2*xor(1,norm(parent1-parent2));
            %above picks off the chrom. for parent2_idx
            child1=[parent1(1:cross_point),...
               parent2(cross_point+1:length)];   %merges parents1&2
            child2=[parent2(1:cross_point),...
               parent1(cross_point+1:length)];   %merges parents1&2
            thetabit_noelite(j,:)=child1;%assigns child1 to new chrom. matrix
            thetabit_noelite(j+1,:)=child2;%ditto for child2
         else
            parent1=thetabit(parent1_idx,:);
               %above picks off the chrom. for parent1_idx
            parent2=thetabit(parent2_idx,:);
               %above picks off the chrom. for parent2_idx
            thetabit_noelite(j,:)=parent1;%assigns parent1 to chrom. matrix
            thetabit_noelite(j+1,:)=parent2;%ditto for parent2
         end
      end
      %
      % Mutation operator applied to thetabit_noelite vector (elite
      % chromosomes not subject to mutation)
      %
      mutate=rand(N-elite,length)<P_m;
      thetabit_noelite=xor(thetabit_noelite,mutate); %exclusive-or operator
                                               %invokes mutation
      % Put elite chromosomes back in population
      %
      for j=1:elite
        thetabit(j,:)=thetabit(index(j),:);
      end
      thetabit(elite+1:N,:)=thetabit_noelite;
      fitness=fitpop(thetabit,bits,loss);
      n=n+1;
   end
      [bestfitness,bestindex]=max(fitness);
   pointer=1;
   for j=1:p
      theta(j)=bit2num(thetabit(bestindex,pointer:pointer+bits(j)-1),...
      bits(j),thetamin(j),thetamax(j));
      pointer=pointer+bits(j);
   end
   lossvalue=feval(lossfinaleval,theta);lossvalue
   lossfinalsq=lossfinalsq+lossvalue^2;
   lossfinal=lossfinal+lossvalue;
   navg=navg+n-1;   	%counter for # of iters. taken
end
navg/cases
% standard dev. of loss values
if cases ~= 1
	disp('standard deviation of mean loss value')
   sd=((cases/(cases-1))^.5)*(lossfinalsq/cases-(lossfinal/cases)^2)^.5;
   sd=sd/(cases^.5)
else
end
% normalized loss values
disp('mean loss value over "cases" runs')
mean=lossfinal/cases


function [cp, vs, i1, iN] = params_legacy_cost(om, name, idx)
%PARAMS_LEGACY_COST  Returns cost parameters for legacy user-defined costs.
%   CP = OM.PARAMS_LEGACY_COST()
%   CP = OM.PARAMS_LEGACY_COST(NAME)
%   CP = OM.PARAMS_LEGACY_COST(NAME, IDX_LIST)
%   [CP, VS] = OM.PARAMS_LEGACY_COST(...)
%   [CP, VS, I1, IN] = OM.PARAMS_LEGACY_COST(...)
%
%   With no input parameters, it assembles and returns the parameters
%   for the aggregate legacy user-defined cost from all legacy cost sets
%   added using ADD_LEGACY_COST. The values of these parameters are cached
%   for subsequent calls. The parameters are contained in the struct CP,
%   described below.
%
%   If a NAME is provided then it simply returns parameter struct CP for the
%   corresponding named set. Likewise for indexed named sets specified
%   by NAME and IDX_LIST.
%
%   An optional 2nd output argument VS indicates the variable sets used by
%   this cost set. The size of CP.N will be consistent with VS.
%
%   If NAME is provided, optional 3rd and 4th output arguments I1 and IN
%   indicate the starting and ending row indices of the corresponding
%   cost set in the full aggregate cost matrix CP.N.
%
%   Let X refer to the vector formed by combining the corresponding varsets VS,
%   and F_U(X, CP) be the cost at X corresponding to the cost parameters
%   contained in CP, where CP is a struct with the following fields:
%       N      - nw x nx sparse matrix (optional, identity matrix by default)
%       Cw     - nw x 1 vector
%       H      - nw x nw sparse matrix (optional, all zeros by default)
%       dd, mm - nw x 1 vectors (optional, all ones by default)
%       rh, kk - nw x 1 vectors (optional, all zeros by default)
%
%   These parameters are used as follows to compute F_U(X, CP)
%
%       R  = N*X - rh
%
%               /  kk(i),  R(i) < -kk(i)
%       K(i) = <   0,     -kk(i) <= R(i) <= kk(i)
%               \ -kk(i),  R(i) > kk(i)
%
%       RR = R + K
%
%       U(i) =  /  0, -kk(i) <= R(i) <= kk(i)
%               \  1, otherwise
%
%       DDL(i) = /  1, dd(i) = 1
%                \  0, otherwise
%
%       DDQ(i) = /  1, dd(i) = 2
%                \  0, otherwise
%
%       Dl = diag(mm) * diag(U) * diag(DDL)
%       Dq = diag(mm) * diag(U) * diag(DDQ)
%
%       w = (Dl + Dq * diag(RR)) * RR
%
%       F_U(X, CP) = 1/2 * w'*H*w + Cw'*w
%
%   See also OPT_MODEL, ADD_LEGACY_COST, EVAL_LEGACY_COST.

%   MATPOWER
%   Copyright (c) 2017, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

if nargin > 1       %% individual set
    if nargin < 3
        idx = {};
    end
    if isempty(idx)                 %% name, no index provided
        if numel(om.cost.idx.i1.(name)) == 1    %% simple named set
            N  = om.cost.data.N.( name);
            Cw = om.cost.data.Cw.(name);
            [nw, nx] = size(N);
            e1 = ones(nw, 1);
            e0 = zeros(nw, 1);
            cp = struct(...
                'N',    N, ...
                'Cw',   Cw, ...
                'H',    sparse(nw, nw), ... %% default => no quadratic term
                'dd',   e1, ...             %% default => linear
                'rh',   e0, ...             %% default => no shift
                'kk',   e0, ...             %% default => no dead zone
                'mm',   e1 );               %% default => no scaling
            if isfield(om.cost.data.H, name) && ~isempty(om.cost.data.H.(name))
                cp.H = om.cost.data.H.(name);
            end
            if isfield(om.cost.data.dd, name) && ~isempty(om.cost.data.dd.(name))
                cp.dd = om.cost.data.dd.(name);
            end
            if isfield(om.cost.data.rh, name) && ~isempty(om.cost.data.rh.(name))
                cp.rh = om.cost.data.rh.(name);
            end
            if isfield(om.cost.data.kk, name) && ~isempty(om.cost.data.kk.(name))
                cp.kk = om.cost.data.kk.(name);
            end
            if isfield(om.cost.data.mm, name) && ~isempty(om.cost.data.mm.(name))
                cp.mm = om.cost.data.mm.(name);
            end
            if nargout > 1
                vs = om.cost.data.vs.(name);
                if nargout > 3
                    i1 = om.cost.idx.i1.(name);     %% starting row index
                    iN = om.cost.idx.iN.(name);     %% ending row index
                end
            end
        else                                    %% indexing required
            error('@opt_model/params_legacy_cost: legacy cost set ''%s'' requires an IDX_LIST arg', name);
        end
    else                            %% indexed named set
        % (calls to substruct() are relatively expensive ...
        % s = substruct('.', name, '{}', idx);
        % ... so replace it with these more efficient lines)
        sc = struct('type', {'.', '{}'}, 'subs', {name, idx});
        N  = subsref(om.cost.data.N,  sc);
        Cw = subsref(om.cost.data.Cw, sc);
        [nw, nx] = size(N);
        e1 = ones(nw, 1);
        e0 = zeros(nw, 1);
        cp = struct(...
            'N',    N, ...
            'Cw',   Cw, ...
            'H',    sparse(nw, nw), ... %% default => no quadratic term
            'dd',   e1, ...             %% default => linear
            'rh',   e0, ...             %% default => no shift
            'kk',   e0, ...             %% default => no dead zone
            'mm',   e1 );               %% default => no scaling
        if isfield(om.cost.data.H, name) && ~isempty(subsref(om.cost.data.H, sc))
            cp.H = subsref(om.cost.data.H,  sc);
        end
        if isfield(om.cost.data.dd, name) && ~isempty(subsref(om.cost.data.dd, sc))
            cp.dd = subsref(om.cost.data.dd, sc);
        end
        if isfield(om.cost.data.rh, name) && ~isempty(subsref(om.cost.data.rh, sc))
            cp.rh = subsref(om.cost.data.rh, sc);
        end
        if isfield(om.cost.data.kk, name) && ~isempty(subsref(om.cost.data.kk, sc))
            cp.kk = subsref(om.cost.data.kk, sc);
        end
        if isfield(om.cost.data.mm, name) && ~isempty(subsref(om.cost.data.mm, sc))
            cp.mm = subsref(om.cost.data.mm, sc);
        end
        if nargout > 1
            vs = subsref(om.cost.data.vs, sc);
            if nargout > 3
                sn = sc; sn(2).type = '()';         %% num array field
                i1 = subsref(om.cost.idx.i1, sn);   %% starting row index
                iN = subsref(om.cost.idx.iN, sn);   %% ending row index
            end
        end
    end
else                %% aggregate
    cache = om.cost.params;
    if isempty(cache)       %% build the aggregate
        nx = om.var.N;          %% number of variables
        nw = om.cost.N;         %% number of cost rows
        Nt = sparse(nx, nw);    %% use N transpose for speed
        Cw = zeros(nw, 1);
        H = sparse(nw, nw);     %% default => no quadratic term
        dd = ones(nw, 1);       %% default => linear
        rh = Cw;                %% default => no shift
        kk = Cw;                %% default => no dead zone
        mm = dd;                %% default => no scaling

        %% fill in each piece
        for k = 1:om.cost.NS
            name = om.cost.order(k).name;
            idx  = om.cost.order(k).idx;
            [cp, vs, i1, iN] = om.params_legacy_cost(name, idx);
            [mk, nk] = size(cp.N);      %% size of Nk
            if mk
                Nkt_full = sparse(nx, nw);
                if isempty(vs)
                    if nk == nx     %% full size
                        Nkt_full(:, i1:iN) = cp.N';
                    else            %% vars added since adding this cost set
                        Nk_all_cols = sparse(mk, nx);
                        Nk_all_cols(:, 1:nk) = cp.N;
                        Nkt_full(:, i1:iN) = Nk_all_cols';
                    end
                else
                    jj = om.varsets_idx(vs);    %% indices for var set
                    Nk_all_cols = sparse(mk, nx);
                    Nk_all_cols(:, jj) = cp.N;
                    Nkt_full(:, i1:iN) = Nk_all_cols';
                end
                Nt = Nt + Nkt_full;
                Cw(i1:iN) = cp.Cw;
                H_all_cols = sparse(mk, nw);
                H_all_cols(:, i1:iN) = cp.H';
                H(:, i1:iN) = H_all_cols';
                dd(i1:iN) = cp.dd;
                rh(i1:iN) = cp.rh;
                kk(i1:iN) = cp.kk;
                mm(i1:iN) = cp.mm;
            end
        end

        %% cache aggregated parameters
        om.cost.params = struct( ...
            'N', Nt', 'Cw', Cw, 'H', H, 'dd', dd, 'rh', rh, 'kk', kk, 'mm', mm );
        cp = om.cost.params;
    else                    %% return cached values
        cp = cache;
    end
    if nargout > 1
        vs = {};
    end
end


function vn = backward_chain(w,tr,tc,la)
%
%Backward chain from goal of given world to find the initial states that
%are reachable
%
t.r = tr;
t.c = tc;
t.l = la;
q = qu_new;% start node queue
q = qu_enqu(q,t);% enqueue goal
v(1:size(w,1),1:size(w,2),1:la) = 0;
while qu_empty(q) ~= 1
    cu = qu_front(q);
    q = qu_dequ(q);% dequeue goal
    if (w(cu.r,cu.c,cu.l) ~= 1)&&(cu.l~=1)% check if obstacle or initial state
        if v(cu.r,cu.c,cu.l-1) ~= 1
            t.r = cu.r;
            t.c = cu.c;
            t.l = cu.l - 1;
            q = qu_enqu(q,t); % enqueue descendant
            v(cu.r,cu.c,cu.l-1) = 1;% mark descendant as visited
        end
        if (cu.r > 1)&&(v(cu.r-1,cu.c,cu.l-1) ~= 1)
            t.r = cu.r - 1;
            t.c = cu.c;
            t.l = cu.l - 1;
            q = qu_enqu(q,t);
            v(cu.r-1,cu.c,cu.l-1) = 1;
        end
        if (cu.c > 1)&&(v(cu.r,cu.c-1,cu.l-1) ~= 1)
            t.r = cu.r;
            t.c = cu.c - 1;
            t.l = cu.l - 1;
            q = qu_enqu(q,t);
            v(cu.r,cu.c-1,cu.l-1) = 1;
        end
        if (cu.r < size(w,1))&&(v(cu.r+1,cu.c,cu.l-1) ~= 1)
            t.r = cu.r + 1;
            t.c = cu.c;
            t.l = cu.l - 1;
            q = qu_enqu(q,t);
            v(cu.r+1,cu.c,cu.l-1) = 1;
        end
        if (cu.c < size(w,2))&&(v(cu.r,cu.c+1,cu.l-1) ~= 1)
            t.r = cu.r;
            t.c = cu.c + 1;
            t.l = cu.l - 1;
            q = qu_enqu(q,t);
            v(cu.r,cu.c+1,cu.l-1) = 1;
        end
    end
end
q = qu_free(q);% free queue space
vn = v; % return values
end


% Copyright 2017 Google Inc.
%
% Licensed under the Apache License, Version 2.0 (the "License");
% you may not use this file except in compliance with the License.
% You may obtain a copy of the License at
%
%     https://www.apache.org/licenses/LICENSE-2.0
%
% Unless required by applicable law or agreed to in writing, software
% distributed under the License is distributed on an "AS IS" BASIS,
% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
% See the License for the specific language governing permissions and
% limitations under the License.

% Returns set of parameters
% set light_mode = true to run the code in a sub optimal but faster mode
% set light_mode = false to obtain the results reported in the RED paper

function params = GetSuperResADMMParams(light_mode)

params = GetSuperResFPParams(light_mode);

% admm parameter
params.beta = 1e-3;

% number of denoising applications
params.inner_denoiser_iters = 1;

% relaxation parameter of ADMM
params.alpha = 1;

return


%% This is for testing the Homogeneous Transformation functions in the robotics Toolbox

function tests = PluckerTest
  tests = functiontests(localfunctions);

  clc
end

function teardownOnce(tc)
    close all
end

% Primitives
function constructor1_test(tc)

    % construct from 6-vector
    L = Plucker([1 2 3 4 5 6]);
    tc.verifyTrue( isa(L, 'Plucker') );
    tc.verifyEqual(L.v, [1 2 3]','absTol',1e-10);
    tc.verifyEqual(L.w, [4 5 6]','absTol',1e-10);

    % construct from object
    L2 = Plucker(L)
    tc.verifyTrue( isa(L, 'Plucker') );
    tc.verifyEqual(L2.v, [1 2 3]','absTol',1e-10);
    tc.verifyEqual(L2.w, [4 5 6]','absTol',1e-10);

    % construct from point and direction
    L = Plucker.pointdir([1 2 3], [4 5 6]);
    tc.verifyTrue(L.contains([1 2 3]'));
    tc.verifyEqual(L.uw, unit([4 5 6])', 'absTol', 1e-10);
end

function double_test(tc)
    % verify double
    L = Plucker([1 2 3 4 5 6]);
    tc.verifyEqual(double(L), [1 2 3 4 5 6]', 'absTol', 1e-10);
end

function constructor2_test(tc)
    % 2 point constructor
    P = [2 3 7]; Q = [2 1 0];
    L = Plucker(P, Q);
    tc.verifyEqual(L.w', P-Q,'absTol',1e-10);
    tc.verifyEqual(L.v', cross(P-Q,Q),'absTol',1e-10);

    % test all possible input shapes
    L2 = Plucker(P', Q);
    tc.verifyEqual(double(L2), double(L), 'absTol',1e-10);
    L2 = Plucker(P, Q');
    tc.verifyEqual(double(L2), double(L), 'absTol',1e-10);
    L2 = Plucker(P', Q');
    tc.verifyEqual(double(L2), double(L), 'absTol',1e-10);
    L2 = Plucker(P, Q);
    tc.verifyEqual(double(L2), double(L), 'absTol',1e-10);

    % planes constructor
    P = [10 11 12]'; w = [1 2 3];
    L = Plucker.pointdir(P, w);
    tc.verifyEqual(double(L), [cross(w,P) w]','absTol',1e-10); %FAIL
    L2 = Plucker.pointdir(P', w);
    tc.verifyEqual(double(L2), double(L), 'absTol',1e-10);
    L2 = Plucker.pointdir(P, w');
    tc.verifyEqual(double(L2), double(L), 'absTol',1e-10);
    L2 = Plucker.pointdir(P', w');
    tc.verifyEqual(double(L2), double(L), 'absTol',1e-10);
end

function pp_test(tc)
    % validate pp and ppd
    L = Plucker([-1 1 2], [1 1 2]);
    tc.verifyEqual(L.pp, [0 1 2]', 'absTol',1e-10);
    tc.verifyEqual(L.ppd, sqrt(5), 'absTol',1e-10);

    % validate pp
    tc.verifyTrue( L.contains(L.pp) );
end

function contains_test(tc)
    P = [2 3 7]; Q = [2 1 0];
    L = Plucker(P, Q);

    % validate contains
    tc.verifyTrue( L.contains([2 3 7]') );
    tc.verifyTrue( L.contains([2 1 0]') );
    tc.verifyFalse( L.contains([2 1 4]') );
end

function closest_test(tc)
    P = [2 3 7]; Q = [2 1 0];
    L = Plucker(P, Q);

    [p,d] = L.closest(P);
    tc.verifyEqual(p, P', 'absTol',1e-10);
    tc.verifyEqual(d, 0, 'absTol',1e-10);

     % validate closest with given points and origin
    [p,d] = L.closest(Q);
    tc.verifyEqual(p, Q', 'absTol',1e-10);
    tc.verifyEqual(d, 0, 'absTol',1e-10);

    L = Plucker([-1 1 2], [1 1 2]);
    [p,d] = L.closest([0 1 2]);
    tc.verifyEqual(p, [0 1 2]', 'absTol',1e-10);
    tc.verifyEqual(d, 0, 'absTol',1e-10);

    [p,d] = L.closest([5 1 2]);
    tc.verifyEqual(p, [5 1 2]', 'absTol',1e-10);
    tc.verifyEqual(d, 0, 'absTol',1e-10);

    [p,d] = L.closest([0 0 0]);
    tc.verifyEqual(p, L.pp, 'absTol',1e-10);
    tc.verifyEqual(d, L.ppd, 'absTol',1e-10);

    [p,d] = L.closest([5 1 0]);
    tc.verifyEqual(p, [5 1 2]', 'absTol',1e-10);
    tc.verifyEqual(d, 2, 'absTol',1e-10);
end

function plot_test(tc)

    P = [2 3 7]; Q = [2 1 0];
    L = Plucker(P, Q);

    axis(10*[-1 1 -1 1 -1 1]);
    plot(L, 'r', 'LineWidth', 2);
end

function eq_test(tc)
    w = [1 2 3];
    P = [-2 4 3];

    L1 = Plucker(P, P+w);
    L2 = Plucker(P+2*w, P+5*w);
    L3 = Plucker(P+[1 0 0], P+w);

    tc.verifyTrue(L1 == L2);
    tc.verifyFalse(L1 == L3);

    tc.verifyFalse(L1 ~= L2);
    tc.verifyTrue(L1 ~= L3);
end

function skew_test(tc)

    P = [2 3 7]; Q = [2 1 0];
    L = Plucker(P, Q);

    m = L.skew;

    tc.verifyEqual(size(m), [4 4]);
    tc.verifyEqual(m+m', zeros(4,4), 'abstol', 1e-10);
end

function mtimes_test(tc)
    P = [1 2 0]; Q = [1 2 10];  % vertical line through (1,2)
    L = Plucker(P, Q);

    % check pre/post multiply by matrix
    M = rand(4,10);

    a = L * M;
    tc.verifyEqual(a, L.skew*M);

    M = rand(10,4);
    a = M * L;
    tc.verifyEqual(a, M*L.skew);

    % check transformation by SE3

    L2 = SE3 * L;
    tc.verifyEqual(L.double, L2.double);

    L2 = SE3(2, 3, 1) * L; % shift line in the xy directions
    pxy = L2.intersect_plane([0 0 1 0]);
    tc.verifyEqual(pxy, [1+2 2+3 0]');

end

function parallel_test(tc)

    L1 = Plucker.pointdir([4 5 6], [1 2 3]);
    L2 = Plucker.pointdir([5 5 6], [1 2 3]);
    L3 = Plucker.pointdir([4 5 6], [3 2 1]);

    % L1 || L2 but doesnt intersect
    % L1 intersects L3

    tc.verifyTrue( isparallel(L1, L1) );
    tc.verifyTrue(L1 | L1);

    tc.verifyTrue( isparallel(L1, L2) );
    tc.verifyTrue(L1 | L2);
    tc.verifyTrue( isparallel(L2, L1) );
    tc.verifyTrue(L2 | L1);
    tc.verifyFalse( isparallel(L1, L3) );
    tc.verifyFalse(L1 | L3);
end

function intersect_test(tc)

    L1 = Plucker.pointdir([4 5 6], [1 2 3]);
    L2 = Plucker.pointdir([5 5 6], [1 2 3]);
    L3 = Plucker.pointdir( [4 5 6], [0 0 1]);
    L4 = Plucker.pointdir([5 5 6], [1 0 0]);

    % L1 || L2 but doesnt intersect
    % L3 intersects L4
    tc.verifyFalse( L1^L2 );

    tc.verifyTrue( L3^L4 );

end

function commonperp_test(tc)
    L1 = Plucker.pointdir([4 5 6], [0 0 1]);
    L2 = Plucker.pointdir([6 5 6], [0 1 0]);

    tc.verifyFalse( L1|L2 );
    tc.verifyFalse( L1^L2 );

    tc.verifyEqual( distance(L1, L2), 2);

    L = commonperp(L1, L2);  % common perp intersects both lines

    tc.verifyTrue( L^L1 );
    tc.verifyTrue( L^L2 );
end

function line_test(tc)

    % mindist
    % intersect
    % char
    % intersect_volume
    % mindist
    % mtimes
    % or
    % side
end

function point_test(tc)
    P = [2 3 7]; Q = [2 1 0];
    L = Plucker(P, Q);

    tc.verifyTrue( L.contains(L.point(0)) );
    tc.verifyTrue( L.contains(L.point(1)) );
    tc.verifyTrue( L.contains(L.point(-1)) );
end

function char_test(tc)
    P = [2 3 7]; Q = [2 1 0];
    L = Plucker(P, Q);

    s = char(L);
    tc.verifyClass(s, 'char');
    tc.verifyEqual(size(s,1), 1);

    s = char([L L]);
    tc.verifyClass(s, 'char');
    tc.verifyEqual(size(s,1), 2);
    s = char([L L]');
    tc.verifyClass(s, 'char');
    tc.verifyEqual(size(s,1), 2);
end

function plane_test(tc)

    xyplane = [0 0 1 0];
    xzplane = [0 1 0 0];
    L = Plucker.planes(xyplane, xzplane); % x axis
    tc.verifyEqual(double(L), [0 0 0 -1 0 0]','absTol',1e-10);

    L = Plucker([-1 2 3], [1 2 3]);  %line at y=2,z=3
    x6 = [1 0 0 -6]; % x = 6

    % plane_intersect
    [p,lambda] = L.intersect_plane(x6)
    tc.verifyEqual(p, [6 2 3]','absTol',1e-10);
    tc.verifyEqual(L.point(lambda), [6 2 3]','absTol',1e-10);

    x6s.n = [1 0 0];
    x6s.p = [6 0 0];
    [p,lambda] = L.intersect_plane(x6s);
    tc.verifyEqual(p, [6 2 3]','absTol',1e-10);
    tc.verifyEqual(L.point(lambda), [6 2 3]','absTol',1e-10);
end

function methods_test(tc)
    % intersection
    px = Plucker([0 0 0], [1 0 0]);  % x-axis
    py = Plucker([0 0 0], [0 1 0]);  % y-axis
    px1 = Plucker([0 1 0], [1 1 0]); % offset x-axis

    verifyEqual(tc, px.ppd, 0);
    verifyEqual(tc, px1.ppd, 1);
    verifyEqual(tc, px1.pp, [0 1 0]');

    px.intersects(px)
    px.intersects(py)
    px.intersects(px1)
end

% function intersect_test(tc)
%     px = Plucker([0 0 0], [1 0 0]);  % x-axis
%     py = Plucker([0 0 0], [0 1 0]);  % y-axis
%
%     plane.d = [1 0 0]; plane.p = 2; % plane x=2
%
%     px.intersect_plane(plane)
%     py.intersect_plane(plane)
% end


function demoDrawTubularMesh
%DEMODRAWTUBULARMESH Draw a tubular mesh using matGeom library
%
%   Example
%     demoDrawTubularMesh
%
%   See also
%     drawSoccerBall, demoRevolutionSurface, demoEquivalentEllipsoid
%

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2012-04-25,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2012 INRA - Cepia Software Platform.

%% Initialisation

% extract vertices, edges, and faces of soccerball polyhedron
[vertices, edges, faces] = createSoccerBall;

% prepare figure
figure(1); clf; hold on;
axis equal;
view(3);

% draw the polyhedron as basis
drawPolyhedron(vertices, faces);

%% Draw each edge as a green cylinder

figure; hold on;
axis equal;
view(3);

width = .05;
radius = .1;

for i = 1:size(edges, 1)
    p1 = vertices(edges(i, 1), :);
    p2 = vertices(edges(i, 2), :);

    drawCylinder([p1 p2 width], 'FaceColor', 'g');
end

%% Draw each vertex as a blue ball

for i = 1:size(vertices, 1)
    drawSphere([vertices(i, :) radius], 'FaceColor', 'b');
end

%% More setup on display

set(gcf, 'renderer', 'opengl')
light;
view([10 30]);


function showCoinSeg(img, coinseg, edgeColor, lineStyle)
% function showCoinSeg(CoinSeg, coinseg, edgeColor, lineStyle)
%
% purpose:
%   Utility to display segmentation parameters found by functions like
%   SegScaleAccHT on an image. A circle is draw at the segmentation.
% parameters:
%  IN:
%    img       image
%    coinseg   segmentation triplet [radius, centerY, centerX]
%    edgeColor color of the coircle to draw (optional)
%              default 'r' (red)
%    edgeColor line style of the coircle to draw (optional)
%              default '--'
%
% Example: (read coin image, segment coin and show segmentation)
%              img = imread('img1.png');
%              segdata = showCoinSeg(img, segScaleAccHT(img));
%

%
% (C) Christian Kotz 2007
%
% $Id: showCoinSeg.m,v 1.1 2008/04/25 17:39:40 ck Exp $
% author:  $Author: ck $
% date:    $Date: 2008/04/25 17:39:40 $
% version: $Revision: 1.1 $
% history:
% $Log: showCoinSeg.m,v $
% Revision 1.1  2008/04/25 17:39:40  ck
% added example
%
% Revision 1.2  2008/04/22 12:30:50  ck
% _
%
% Revision 1.1  2008/04/22 12:29:25  ck
% _
%
%

if nargin < 3
    edgeColor = 'r';
end

if nargin < 4
    lineStyle = '--';
end

r = coinseg(1);
y = coinseg(2);
x = coinseg(3);

imshow(img);
hold on;

rectangle('Position', [x-abs(r),y-abs(r),abs(2*r),abs(2*r)], 'Curvature', [1,1], ...
    'EdgeColor', edgeColor, 'LineStyle', lineStyle);
hold off


%  Figure 10.54      Feedback Control of Dynamic Systems, 6e
%                        Franklin, Powell, Emami
%
%  fig10_54a.m is a script to generate Fig. 10.54 (a)
% Kp=0.03333, and KI=0.01 corresponds to KpKs=2.0
clf;
[t,x,y,e]=sim('fig10_53and54a');

plot(t,e);
xlabel('Time (sec)');
ylabel('e');
title('Fig. 10.54(a)');
%grid
nicegrid