function [Fopt,OPT_stats_out]=obj_DEMO_FEBio_iFEA_uniaxial_transiso_02(Pn,objectiveStruct)

%% 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)]=parLimNat(objectiveStruct.Pb_struct.xx_c(q),objectiveStruct.Pb_struct.xxlim(q,:),P(q));
end

%% SETTING MATERIAL PARAMETERS

mat_struct.par_names={{'solid','Ogden unconstrained','c1'},...
     {'solid','Ogden unconstrained','m1'},...
    {'solid','Ogden unconstrained','c2'},...
     {'solid','Ogden unconstrained','m2'},...
    {'solid','Ogden unconstrained','cp'},...
    {'solid','ellipsoidal fiber distribution','ksi'},...
    {'solid','ellipsoidal fiber distribution','beta'},...
    };

%Acces material parameters
mat_struct.id=1;

c1=P(1);
m1=P(2);
ksi=[P(3) P(3) P(3)*P(4)];
beta=P(5)*ones(1,3);
cp=(2*c1+mean(ksi))*objectiveStruct.k_factor;

mat_struct.par_values={c1 m1 c1 -m1 cp ksi beta};

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

%Assign material parameters
docNode=set_mat_par_FEBIO(objectiveStruct.FEB_struct.run_filename,objectiveStruct.FEB_struct.run_filename,{mat_struct});

disp('Done')

%%

for q=1:1:2 %Direction cases

    disp('SETTING FIBRE DIRECTIONS...');

    switch q
        case 1
            alphaFib=0;
        case 2
            alphaFib=0.5*pi;
    end

    [R,~]=euler2DCM([0,alphaFib,0]);
    v_fib=(R*[0 0 1]')';
    V_fib=v_fib(ones(numel(objectiveStruct.FEB_struct.Geometry.ElementData.MatAxis.ElementIndices),1),:);

    %Adding fibre direction, construct local orthonormal basis vectors
    [a,d]=vectorOrthogonalPair(V_fib);
    VF_E=zeros(size(V_fib,1),size(V_fib,2),2);
    VF_E(:,:,1)=a; %a1 ~ e1 ~ X or first direction
    VF_E(:,:,2)=d; %a2 ~ e2 ~ Y or second direction
    %Vf_E %a3 ~ e3 ~ Z, third direction, or fibre direction
    objectiveStruct.FEB_struct.Geometry.ElementData.MatAxis.Basis=VF_E;
    docNode=addMatAxisFibreElementData_FEB(docNode,objectiveStruct.FEB_struct);
    write_XML_no_extra_lines(objectiveStruct.FEB_struct.run_filename,docNode)% Saving XML file
    disp('Done')

    %% START FEBio NOW

    [runFlag]=runMonitorFEBio(objectiveStruct.FEBioRunStruct);

    stretch_exp=objectiveStruct.stretch_exp;
    stress_cauchy_exp=objectiveStruct.stress_cauchy_exp(:,q);

    if runFlag==1

        %Importing stress
        [~,S_mat,~]=importFEBio_logfile(objectiveStruct.FEB_struct.run_output_names{2}); %Element Cauchy stresses
        S_mat=S_mat(:,2:end,:); %Final stress, no element labels
        S_mat=squeeze(mean(S_mat,1)); %Mean across elements
        stress_cauchy_sim=[0; S_mat(:)]; %Cauchy stress
        stress_cauchy_sim=stress_cauchy_sim.*1e3; %Scale to kPa

        %Import principal strains
        [~, E_mat,~]=importFEBio_logfile(objectiveStruct.FEB_struct.run_output_names{1}); %Nodal displacements
        E_mat=E_mat(:,2:end,:);

        %Derive x stretch
        EX_mat=E_mat(:,1,:);
        EX_mat=squeeze(mean(EX_mat,1)); %Mean across elements
        stretch_sim_X=sqrt(2*EX_mat+1);
        stretch_sim_X=[1; stretch_sim_X(:)];
        stretch_sim_X_end=(stretch_sim_X(end));

        %Derive y stretch
        EY_mat=E_mat(:,2,:);
        EY_mat=squeeze(mean(EY_mat,1)); %Mean across elements
        stretch_sim_Y=sqrt(2*EY_mat+1);
        stretch_sim_Y=[1; stretch_sim_Y(:)];
        stretch_sim_Y_end=(stretch_sim_Y(end));

        %Derive z stretch
        EZ_mat=E_mat(:,3,:);
        EZ_mat=squeeze(mean(EZ_mat,1)); %Mean across elements
        stretch_sim_Z=sqrt(2*EZ_mat+1);
        stretch_sim_Z=[1; stretch_sim_Z(:)];
        stretch_sim_Z_end=(stretch_sim_Z(end));

        %Interpolate experiment onto simulated points
        stress_cauchy_sim_exp = interp1(stretch_sim_Z,stress_cauchy_sim,stretch_exp,'pchip');

        %Derive Fopt
        stressDev=stress_cauchy_exp-stress_cauchy_sim_exp;
        Fopt_stress=mean(abs(stressDev)./max(abs(stress_cauchy_exp)));

        switch q
            case 1
                stretchDev=[1./(sqrt(0.7)) 1./(sqrt(0.7))]-[stretch_sim_X_end stretch_sim_Y_end];
            case 2
                stretchDev=[exp(log(stretch_sim_Z_end)*-0.36) exp(log(stretch_sim_Z_end)*-0.65)]-[stretch_sim_X_end stretch_sim_Y_end];
        end

        Fopt_stretch=mean(abs(stretchDev));

        [R_sq]=R_squared(stress_cauchy_sim_exp,stress_cauchy_exp);

    else %Output NaN
        stress_cauchy_sim=NaN(size(stretch_exp));
        stretch_sim_Z=NaN(size(stretch_exp));
        stressDev=NaN(size(stretch_exp));
        stretchDev=NaN(1,2);
        Fopt_stress=Nan;
        Fopt_stretch=NaN;
    end

    OPT_stats_out.stress_cauchy_sim{q}=stress_cauchy_sim;
    OPT_stats_out.stretch_sim{q}=stretch_sim_Z;
    OPT_stats_out.stretch_sim_end(q,:)=[stretch_sim_X_end stretch_sim_Y_end stretch_sim_Z_end];
    OPT_stats_out.stressDev{q}=stressDev;
    OPT_stats_out.stretchDev{q}=stretchDev;
    OPT_stats_out.Fopt(:,q)=[Fopt_stress Fopt_stretch];
    OPT_stats_out.R_sq(q)=R_squared(stress_cauchy_exp,stress_cauchy_sim_exp);

end

switch objectiveStruct.method
    case 1
        Fopt=OPT_stats_out.Fopt(1,1)+OPT_stats_out.Fopt(1,2)+OPT_stats_out.Fopt(2,2); %Scalar objective function
    case 2
        Fopt=[OPT_stats_out.Fopt(1,1) OPT_stats_out.Fopt(1,2) OPT_stats_out.Fopt(2,2)]; %Objective function vectors
end

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


function [xUpdate,SUpdate,innov,Szz,W]=sqrtEKFUpdate(xPred,SPred,z,SR,h,HJacob,innovTrans,stateTrans)
%SQRTEKFUPDATE Perform the measurement update step in a square-root version
%              of the first-order Extended Kalman Filter (EKF).
%
%INPUTS: xPred The xDim X 1 predicted target state.
%        SPred The xDim X xDim lower-triangular square root predicted state
%              covariance matrix.
%            z The zDim X 1 measurement vector.
%           SR The zDim X zDim lower-triangular square root of the
%              measurement covariance matrix in the native coordinate
%              system of the measurement.
%            h A function handle for the measurement function that takes
%              the state as its argument.
%       HJacob A function handle for the measurement Jacobian matrix that
%              takes the target state as a parameter. If not supplied or an
%              empty matrix is passed, then HJacob will be found using
%              numerical differentiation via the numDiff function with
%              default parameters.
%   innovTrans An optional function handle that computes and optionally
%              transforms the value of the difference between the
%              observation and any predicted points. This is called as
%              innovTrans(a,b) and the default if omitted or an empty
%              matrix is passed is @(a,b)bsxfun(@minus,a,b). This must be
%              able to handle sets of values. For a zDimX1 measurement,
%              either of the inputs could be zDimXN in size while one of
%              the inputs could be zDimX1 in size.  This only needs to be
%              supplied when a measurement difference must be restricted
%              to a certain range. For example, the innovation between two
%              angles will be 2*pi if one angle is zero and the other
%              2*pi, even though they are the same direction. In such an
%              instance, a function handle to the
%              wrapRange(bsxfun(@minus,a,b),-pi,pi) function with the
%              appropriate parameters should be passed for innovTrans.
%   stateTrans An optional function that takes a state estimate and
%              transforms it. This is useful if one wishes the elements of
%              the state to be bound to a certain domain. For example, if
%              an element of the state is an angle, one should generally
%              want to bind it to the region +/-pi.
%
%OUTPUTS: xUpdate The xDim X 1 updated state vector.
%         SUpdate The updated xDim X xDim lower-triangular square root
%                 state covariance matrix.
%      innov, Szz The zDimX1 innovation and the zDimXzDim square root
%                 innovation covariance matrix are returned in case one
%                 wishes to analyze the consistency of the estimator or use
%                 those values in gating or likelihood evaluation.
%               W The xDimXzDim gain used in the update. This can be
%                 useful when gating and using the function
%                 calcMissedGateCov.
%
%The first-order EKF is summarized in Figure 10.3.3-1 in Chapter 10.3.3 of
%[1]. The Joseph-form covariance update given in Chapter 5 of the same book
%is used for improved numerical stability. The mathematics behind the
%specific square root implementation used here are those used in the
%standard Kalman filter, which are described in [2] with a flow chart given
%in Appendix G.
%
%The optional parameter innovTrans is not described in the above
%reference, but allows for possible modifications to the filter as
%described in [3].
%
%The parameters have been added to allow the filter to be used with
%angular quantities. For example, if the measurement consisted of
%range and angle, z=[r;theta], then
%innovTrans=@(a,b)[bsxfun(@minus,a(1,:),b(1,:));
%                  wrapRange(bsxfun(@minus,a(2,:),b(2,:)),-pi,pi)];
%should be used to approximately deal with the circular nature of the
%measurements.
%
%REFERENCES:
%[1] Y. Bar-Shalom, X. R. Li, and T. Kirubarajan, Estimation with
%    Applications to Tracking and Navigation. New York: John Wiley and
%    Sons, Inc, 2001.
%[2] David F. Crouse , "Basic tracking using nonlinear 3D monostatic and
%    bistatic measurements," IEEE Aerospace and Electronic Systems
%    Magazine, vol. 29, no. 8, Part II, pp. 4-53, Aug. 2014.
%[3] David F. Crouse, "Cubature/ unscented/ sigma point Kalman filtering
%    with angular measurement models," in Proceedings of the 18th
%    International Conference on Information Fusion, Washington, D.C.,
%    6-9 Jul. 2015.
%
%March 2015 David Karnick, Naval Research Laboratory, Washington D.C.
%Updated with additional inputs, August 2015 David F. Crouse,
%                               Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

zDim=size(z,1);

if(nargin<8||isempty(stateTrans))
    stateTrans=@(x)x;
end

if(nargin<7||isempty(innovTrans))
    innovTrans=@(a,b)bsxfun(@minus,a,b);
end

if(nargin<6||isempty(HJacob))
    HJacob=@(x)numDiff(x,h,zDim);
end

zPred=h(xPred);

H=HJacob(xPred);
Pxz=SPred*SPred'*H';

Szz=tria([H*SPred,SR]);
W=(Pxz/Szz')/Szz;

innov=innovTrans(z,zPred);
xUpdate=stateTrans(xPred+W*innov);

temp=W*H;
SUpdate=tria([(eye(size(temp))-temp)*SPred,W*SR]);

end

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


function h = plotAngleDistribution(obj,varargin)
% plot axis distribution
%
% Syntax
%
%   plotAngleDistribution(mdf)
%   plotAngleDistribution(CS1,CS2)
%   plotAngleDistribution(grains.boundary.misorientation)
%
% Input
%  CS - @symmetry
%
% Options
%  resolution, xy*degree - resolution of the plots (given as angle)
%

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

% compute angles
plotType = 'line';
if isa(obj,'symmetry')
  maxOmega = maxAngle(obj,varargin{:});
else
  maxOmega = maxAngle(obj.CS,obj.SS);
  if ~isa(obj,'SO3Fun'), plotType = 'bar'; end
end

% seach for existing bar plots and adjust bar center
h = findobj(mtexFig.gca,'type','bar','-or','type','hgGroup');
h = flipud(h(:));

unit = '%';
if ~isempty(h)

  midPoints = ensurecell(get(h,'XData'));
  midPoints= midPoints{1}*degree;
  bins = [2*midPoints(1)-midPoints(2),midPoints,2*midPoints(end)-midPoints(end-1)];
  bins = (bins(1:end-1) + bins(2:end))/2;
  density = ensurecell(get(h,'YData'));
  density = cellfun(@(x) x(:),density,'UniformOutput',false);
  density = horzcat(density{:});
  lg = ensurecell(get(h,'DisplayName'));

  if strcmp(plotType,'bar')
    delete(h); % remove old bars

    % add a new column
    density(:,end+1) = 0;

    % maybe we have to enlarge bins
    if maxOmega > max(bins)
      bins = 0:(bins(2)-bins(1)):maxOmega + 0.01;
      density(end+1:length(bins)-1,:) = 0;
    end
  else
    faktor = 100 / size(density,1);
  end

else

  if strcmp(plotType,'bar')

    % bin size given?
    if max(obj.angle) < maxOmega/2, maxOmega = max(obj.angle);end
    nbins = max(15,round(maxOmega/get_option(varargin,'resolution',5*degree)));

    % compute bins
    bins = linspace(-eps,maxOmega+0.01,nbins);
    density = zeros(nbins-1,1);
    lg = {};
  elseif check_option(varargin,'percent')
    faktor = 100;
  else
    faktor = 1;
    unit = 'mrd';
  end
end

% compute angle distribution
if isa(obj,'symmetry') || isa(obj,'SO3Fun')
  [density,omega] = calcAngleDistribution(obj,varargin{:});
else
  d = histcounts(obj.angle,bins).';
  midPoints = 0.5*(bins(1:end-1) + bins(2:end));
  density(:,end) = 100 * d ./ sum(d);
end

% plot angle distribution
if strcmp(plotType,'bar')

  h = optiondraw(bar(midPoints/degree,density,'parent',mtexFig.gca),varargin{:});
  xlim(mtexFig.gca,[0,max(bins)/degree])

  % update legend
  lg = [lg;{[obj.CS.mineral '-' obj.SS.mineral]}];
  for i=1:length(h)
    set(h(i),'DisplayName',lg{i});
  end

else

  h = optiondraw(plot(omega/degree,faktor * max(0,density),...
    'parent',mtexFig.gca),'LineWidth',2,varargin{:});

end

% finish
if isNew
  xlabel(mtexFig.gca,'Misorientation angle (degrees)');
  ylabel(mtexFig.gca,['Frequency (' unit ')']);
  drawNow(mtexFig,varargin{:});
end

if nargout == 0, clear h; end


%% (Internal) Get the time samples of a (xx,yy) sequence below two restrictions (x_thr, y_thr)
%
% This function calculates the time samples (xx) of a two valued sequence
% (xx, yy) below two restrictions (x_thr, y_thr)
%
%       selected_segments = get_segments_from_sequence(yy, xx, y_thr, x_thr )
%
% Arguments:
%
%       xx, yy: xx and yy values.
%
%       x_thr, y_thr: the thresholds to search for within the sequence.
%
% Output:
%
%       selected_segments: a (n x 2) matrix with the segments start-end
%                          where n is the amount of segments found in the
%                          sequence
%
% Example
%
% See also RR_calculation, MedianFiltSequence
%
% Author: Mariano Llamedo Soria (llamedom at frba.utn.edu.ar)
% Version: 0.1 beta
% Birthdate  : 9/5/2017
% Last update: 9/5/2017
% Copyright 2008-2017
%
function selected_segments = get_segments_from_sequence(xx, yy, x_thr, y_thr )

    selected_segments = [];

    % sequence values below the thr
    bAux = yy <= y_thr;
    % indexes above the thr
    aux_idx = find(~bAux);

    aux_val = diff(xx(aux_idx));
    % starts of segments at least x_thr long that are below y_thr
    aux_idx3 = find(aux_val >= x_thr);
    below_idx = xx(bAux);
    above_idx = xx(~bAux);

% figure(3); plot(xx, yy ); xlims = xlim(); ylims = ylim(); ylims = ylims + [0.1 -0.1] * diff(ylims); hold on; plot(xlims, [y_thr y_thr] , '--r' ); hold off;

    for jj = 1:length(aux_idx3)
        start_idx = xx(aux_idx(aux_idx3(jj))+1);
        end_idx = find( above_idx > start_idx, 1,'first');
        if isempty(end_idx)
            continue
        end
        end_idx = below_idx( find( below_idx < above_idx(end_idx) ,1, 'last') );

        if( ~isempty(start_idx) && ~isempty(end_idx) )
            selected_segments = [selected_segments; [start_idx end_idx]];
        end
    end

end


% LORENZ system
% System identification: NARX

clear all, close all, clc
figpath = '../FIGURES/LORENZ/';
datapath = '../DATA/LORENZ/';
addpath('../utils');

SystemModel = 'LORENZ';

%% Generate Data
ONLY_TRAINING_LENGTH = 1;
InputSignalType = 'sphs'; % prbs; chirp; noise; sine2; sphs; mixed
getTrainingData

%% NARX: Training
ModelName = 'NARX';
Nvar = 3;
rng(2,'twister')
SUBSTRACT_MEAN = 0;
xmean = mean(x)';

if SUBSTRACT_MEAN == 1
    xtrain = x' - repmat(xmean',[1 size(x',2)]);
    utrain = u;
else
    xtrain = x';
    utrain = u;
end

% prepare training data
yt = con2seq(xtrain);
yi = con2seq(utrain);

% Neural network
stateDelays = 1;        % state delay vector
inputDelays = 1;        % input delay vector
hiddenSizes = [10];     % network structure (number of neurons per layer)

% Nonlinear autoregressive neural network
net = narxnet(inputDelays,stateDelays, hiddenSizes);

% Training parameters %nnstart
net.trainFcn = 'trainlm';
net.trainParam.min_grad = 1e-10;
net.trainParam.showCommandLine = 1;

% Prepares training data (shifting, copying feedback targets into inputs as needed, etc.)
[Us,Ui,Si,Ss] = preparets(net,yi,{},yt);

% Train net with prepared training data in open-loop
tic
net = train(net,Us,Ss,Ui,Si);
toc
% view(net)

% Close loop for recursive prediction
netc = closeloop(net);

%% Prediction over training phase
% Prepare validation data / Get initial state from training data
[Us,Ui,Si,So] = preparets(netc,yi,{},yt);

% Predict on validation data
predict = netc(Us,Ui,Si);
xNARX = cell2mat(predict)';

if SUBSTRACT_MEAN == 1
    xNARX = xNARX + repmat(xmean,[size(xNARX,1) 1]);
end

% Error
e = cell2mat(gsubtract(So,predict));

%% Show validation
clear ph
figure,box on,
ccolors = get(gca,'colororder');
ccolors_valid = [ccolors(1,:)-[0 0.2 0.2];ccolors(2,:)-[0.1 0.2 0.09];ccolors(3,:)-[0.1 0.2 0.09]];
for i = 1:Nvar
    ph(i) = plot(tspan,x(:,i),'-','Color',ccolors(i,:),'LineWidth',1); hold on
end
for i = 1:Nvar
    ph(Nvar+i) = plot(tspan(2:end),xNARX(:,i),'--','Color',ccolors_valid(i,:),'LineWidth',2);
end
xlim([0 (length(tspan)-1)*dt]), ylim([-25 50])
xlabel('Time')
ylabel('xi')
legend(ph([1,4]),'True',ModelName)
set(gca,'LineWidth',1, 'FontSize',14)
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto')
if SUBSTRACT_MEAN == 1
    print('-depsc2', '-loose', '-cmyk', [figpath,'EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_SUBSTRACT_MEAN.eps']);
else
    print('-depsc2', '-loose', '-cmyk', [figpath,'EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'.eps']);
end

%% Validation 3D
filename = ['EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_train'];
xModel = xNARX;
iModel = 3;
xTRUTH = x;
color_type = 'models';
VIZ_3D_MODELvsTRUTH

%% Prediction
% prepare validation data
if SUBSTRACT_MEAN == 1
    xvalid = xv' - repmat(xmean',[1 size(xv',2)]);
    uvalid = uv;
else
    xvalid = xv';
    uvalid = uv;
end

yt_valid = con2seq(xvalid);
yi_valid = con2seq(uvalid);
[Us,Ui,Si,So] = preparets(netc,yi_valid,{},yt_valid);

% Reference
xA      = xv;
tA      = tv;

% Predict on validation data
predict = netc(Us,Ui,Si);
xB = cell2mat(predict)';
tB = tA(2:end);

if SUBSTRACT_MEAN == 1
    xB = xB + repmat(xmean,[size(xB,1) 1]);
end
%% Show training and prediction
u = u'; uv = uv';
VIZ_SI_Validation

%% Validation 3D
filename = ['EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'_valid'];
xModel = xB;
xTRUTH = xA;
iModel = 3;
color_type = 'models';
VIZ_3D_MODELvsTRUTH

%% Save Data
Model.name = 'NARX';
Model.net = netc;
Model.xmean = xmean;
Model.stateDelays = stateDelays;
Model.inputDelays = inputDelays;
Model.hiddenSizes = hiddenSizes;
Model.SUBSTRACT_MEAN = SUBSTRACT_MEAN;
Model.dt = dt;
save(fullfile(datapath,['EX_',SystemModel,'_SI_',ModelName,'_',InputSignalType,'.mat']),'Model')


function out = SC_MMA(y,doOverlap,scaleRange,qRange)
% SC_MMA   Physionet implementation of multiscale multifractal analysis
%
% Scale-dependent estimates of multifractal scaling in a time series.

% ------------------------------------------------------------------------------
% Modified by Ben Fulcher for use in hctsa, 2015-05-12.
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
% ------------------------------------------------------------------------------

% ------------------------------------------------------------------------------
% Copyright (C) 2014 Jan Gieraltowski
% ------------------------------------------------------------------------------
%
% This program is free software; you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation; either version 2 of the License, or (at your option) any later
% version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT ANY
% WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
% PARTICULAR PURPOSE.  See the GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License along with
% this program; if not, write to the Free Software Foundation, Inc., 59 Temple
% Place - Suite 330, Boston, MA 02111-1307, USA.
%
% Author: Jan Gieraltowski
% Warsaw University of Technology
% Faculty of Physics
% gieraltowski@if.pw.edu.pl
% http://gieraltowski.fizyka.pw.edu.pl/
%
% Method was first proposed in:
% J. Gieraltowski, J. J. Zebrowski, and R. Baranowski,
% Multiscale multifractal analysis of heart rate variability recordings
% with a large number of occurrences of arrhythmia,
% Phys. Rev. E 85, 021915 (2012).
% http://dx.doi.org/10.1103/PhysRevE.85.021915
%
% Please cite the above publication when referencing this material,
% and also include the standard citation for PhysioNet:
% Goldberger AL, Amaral LAN, Glass L, Hausdorff JM, Ivanov PCh, Mark RG,
% Mietus JE, Moody GB, Peng C-K, Stanley HE.
% PhysioBank, PhysioToolkit, and PhysioNet: Components of a New Research Resource
% for Complex Physiologic Signals.
% Circulation 101(23):e215-e220
% [Circulation Electronic Pages; http://circ.ahajournals.org/cgi/content/full/101/23/e215]; 2000 (June 13).
% ------------------------------------------------------------------------------

% Generate a plot:
doPlot = false;

% Time-series length:
N = length(y);

% ------------------------------------------------------------------------------
% Check Inputs:
% ------------------------------------------------------------------------------
if nargin < 2 || isempty(doOverlap)
    doOverlap = false;
    % 0 - time series is partitioned into non overlapping windows of analysis,
    % 1- time series is partitioned into overlapping windows of analysis, step between consecutive windows is = 1 (much longer calculations)
end

if nargin < 3 || isempty(scaleRange)
    scaleRange = [10,round(N/40)];
    % minimal s scale used, when calculating Fq(s) functions family (default 10)
    % maximal s scale used, when calculating Fq(s) functions family, has to be multiple of 5 (default 600; in general should be near to N/50, where N is a time series length)
end
minScale = scaleRange(1);
maxScale = scaleRange(2);
if (maxScale/5) < minScale
    warning('Time-series (N=%u) too short for multiscale multifractal analysis',N);
    out = NaN;
    return
elseif rem(maxScale,5)~=0
    maxScale = round(maxScale/5)*5;
    fprintf(1,'adjusted maxScale to %u\n',maxScale);
end

if nargin < 4 || isempty(qRange)
    qRange = [-5,5];
    % minimal/maximal multifractal parameter q used (default -5)
end
qMin = qRange(1);
qMax = qRange(2);

qList = qMin:0.1:qMax;
qList(qList == 0) = 0.0001;

% if nargin < 5
%     precisionMode = 0;
%     % 0 (default) better looking plot, smaller files, faster calculations;
%     % set 1 for enhanced precision (smaller q and s steps)
% end

% ------------------------------------------------------------------------------

signal = y;

prof = cumsum(signal);
slength = size(prof);
fqs = [];

numIncrements = 20;
sListFull = unique(round(linspace(minScale,maxScale,numIncrements)));

timer = tic;
for s = sListFull

    if doOverlap
        vec = [0:s-1];
        ind = [1:slength-s+1]';
        coordinates = bsxfun(@plus, vec, ind);
    else
        ind2 = [1:size(prof,1)];
        coordinates = reshape(ind2(1:(size(prof,1)-mod(size(prof,1),s))),s,(size(prof,1)-mod(size(prof,1),s))/s)';
    end

    segments = prof(coordinates);
    xbase = [1:1:s];
    f2nis = [];

    for ni = 1:size(segments,1)
        seg = segments(ni,:);
        fit = polyfit(xbase,seg,2);
        variance = mean((seg - polyval(fit,xbase)).^2);
        f2nis(end+1) = variance;
    end

    for q = qList
        fqs = [fqs; q s (mean(f2nis.^(q/2)))^(1/q)];
    end
end
% fprintf(1,'Detrended fluctuations computed in %s\n',BF_TheTime(toc(timer)));

fqsll = [fqs(:,1) fqs(:,2) log(fqs(:,2)) log(fqs(:,3))];

% ------------------------------------------------------------------------------
% Now compute hurst exponents as the gradients of F(q) curves
% ------------------------------------------------------------------------------
hqs = [];

% if precisionMode == 0
%     % Take 11 points through the space
%     sspacing = ((maxScale/5)-minScale)/10;
%     sList = minScale:sspacing:(maxScale/5);
%     % Coarser sampling of q space
%     qList = qMin:1:qMax; qList(qList == 0) = 0.0001;
% else
    % sspacing = 1;
% sList = minScale:sspacing:(maxScale/5);

if sum(sListFull<=maxScale/5)>=10
    sList = sListFull(sListFull<=maxScale/5);
else
    % sample higher in the scale dimension:
    sspacing = ((maxScale/5)-minScale)/10;
    sList = minScale:sspacing:(maxScale/5);
end

% Coarser sampling of q space
qList = qMin:0.5:qMax; qList(qList == 0) = 0.0001;
% end

% sList = minScale:sspacing:(maxScale/5);

hqs = zeros(length(qList),length(sList));
for si = 1:length(sList)
    sit = sList(si);
    for qi = 1:length(qList)
        qit = qList(qi);

        fitTemp = fqsll(fqsll(:,1) == qit & fqsll(:,2) >= sit & fqsll(:,2) <= 5*sit,:);
        hTemp = polyfit(fitTemp(:,3),fitTemp(:,4),1);

        hqs(qi,si) = hTemp(1);
        % hqs = [hqs; qit 3*sit hTemp(1)];
    end
end

sListScaled = sList*3; % Not completely on top of the algorithm, but for some reason
                       % this was recorded as a multiple of 3 in the original algorithm

% hqsPlotData = reshape(hqs(:,3),size(qList,2),length(minScale:sspacing:(maxScale/5)));

% ------------------------------------------------------------------------------
% Plotting
% ------------------------------------------------------------------------------

if doPlot
    if max(max(hqs)) < 1.5
        hLim = 1.5;
    elseif max(max(hqs)) < 2.5
        hLim = 2.5;
    else
        hLim = ceil((max(max(hqs))*10))/10;
    end

    f = figure('color','w'); box('on');
    hqsplot = surf(sListScaled,qList,hqs);
    colormap(jet);
    colorbar;
    caxis([0,hLim]);
    set(gca,'YDir','reverse');
    view(-62,50);
    axis([sListScaled(1),sListScaled(end),qMin,qMax,0,hLim]);
    xlabel('scale')
    ylabel('q')
    zlabel('h')
end

% ------------------------------------------------------------------------------
% Output statistics:
% ------------------------------------------------------------------------------

% hqsPlotData (scale,q)

% Global properties:
allExponents = hqs(:);
out.meanHurstExponent = mean(allExponents);
out.stdHurstExponent = std(allExponents);
out.minHurstExponent = min(allExponents);
out.maxHurstExponent = max(allExponents);

% Changes with scale:
out.scaleHurstStd = std(mean(hqs,1));
out.scaleHurstTrend = GiveMeGradient(sListScaled,mean(hqs,1));

% Changes with q:
out.qHurstStd = std(mean(hqs,2));
out.qHurstTrend = GiveMeGradient(qList,mean(hqs,2));

% max/min points are where in scale/q space?
[qi,si] = find(hqs==max(hqs(:)),1);
out.maxHurstQ = qList(qi);
out.maxHurstScale = sListScaled(si);
[qi,si] = find(hqs==min(hqs(:)),1);
out.minHurstQ = qList(qi);
out.minHurstScale = sListScaled(si);

% phase transitions
% there is some peak or trough somewhere, so the standard deviation across
% scales or q is inconsistent
stdS = std(hqs,[],1);
stdQ = std(hqs,[],2);
out.stdStdHurstQ = std(stdQ); % will be large if variance changes alot with Q
out.stdStdHurstScale = std(stdS); % will be large if variance changes alot with scale

% saveas(hqsplot, [dirname filesep 'MMA_results' filesep 'MMA_' n1 '.jpg']);

% ------------------------------------------------------------------------------
function m = GiveMeGradient(xData,yData)
    if size(xData,1) ~= size(yData,1);
        yData = yData';
    end
    p = polyfit(xData,yData,1);
    m = p(1);
end
% ------------------------------------------------------------------------------

end


function feq = imequi(f, iscale, hgram)
%------------------------------------------------------------------------------
% imequi
% Under the assumption that f is of class double, we linearly transform f into
% an intensity image according to the definition of the Image Processing Toolbox
% meaning that the values are contained in the range [0, 1]. Moreover, the
% contrast is enhanced. In case the Image Processing Toolbox is available this
% is performed by histogram equalization, if this toolbox is not available it is
% performed by removing the outliers.
%
% Design and implementation by:
% Dr. Paul M. de Zeeuw <Paul.de.Zeeuw@cwi.nl>  http://homepages.cwi.nl/~pauldz/
% Last Revision: July 31, 2003.
%  2003 Stichting CWI, Amsterdam
%------------------------------------------------------------------------------
if nargin ~=2  && nargin ~=3
  error(' imequi - number of arguments should be either 2 or 3 ');
else
  if exist('histeq','file') ~= 2 && iscale == 3
%   Note: if histeq exists then imshow exists as well (in same toolbox).
    isca = 2;
    disp(' imequi - WARNING histeq does not exist ');
  else
    isca = iscale;
  end
end
%
if isca == 1
  feq = intensim(f);
elseif isca == 2
  showmean=mean(f(:));           showstd=std(f(:));
  showlow=showmean-2.0*showstd;  showhgh=showmean+2.0*showstd;
  supermin=min(min(f));          supermax=max(max(f));
  showlow=max(supermin,showlow); showhgh=min(showhgh,supermax);
  fs = (f < showlow)*showlow + (f > showhgh)*showhgh + ...
       (f >= showlow).*(f <= showhgh).*f;
  feq = intensim(fs);
elseif isca == 3
  if nargin == 3
    feq = histeq(intensim(f), hgram);
  else
    feq = histeq(intensim(f));
  end
else
  error(' imequi - value of argument iscale should be either 1 or 2 or 3 ');
end
%------------------------------------------------------------------------------


%EIMM_DEMO2 Bearings Only Tracking of a Manouvering Target demonstration
%
% Simple demonstration for non-linear IMM using the following models:
%  1. Standard Wiener process velocity model
%  2. Coordinated turn model
%
% The measurement model is non-linear bearings only model.
% See BOT-demo or the documentation for more details.
%
% Copyright (C) 2007-2008 Jouni Hartikainen
%
% This software is distributed under the GNU General Public
% Licence (version 2 or later); please refer to the file
% Licence.txt, included with the software, for details.

print_figures = 1;
save_figures = 1;

% Dimensionality of the state space
dims = 5;

% The number of models in use
nmodels = 2;

% Step size
dt = 0.1;

% Space for function handles and parameters
a_func = {};
ia_func = {};
a_param = {};
h_func = {};
h_param = {};
dh_dx_func = {};

a_func{1} = [];
a_func{2} = @f_turn;
ia_func{1} = [];
ia_func{2} = @f_turn_inv;
a_param{1} = [];
a_param{2} = {dt};

% Space for model parameters
ind = cell(1,nmodels);
F   = cell(1,nmodels);
L   = cell(1,nmodels);
Qc  = cell(1,nmodels);
A   = cell(1,nmodels);
Q   = cell(1,nmodels);
H   = cell(1,nmodels);
R   = cell(1,nmodels);

% Index vector of model 1
ind{1} = [1 2 3 4]';

% Transition matrix for the continous-time velocity model.
F{1} = [0 0 1 0;
        0 0 0 1;
        0 0 0 0;
        0 0 0 0];

% Noise effect matrix for the continous-time system.
L{1} = [0 0;
        0 0;
        1 0;
        0 1];

% Process noise variance
q1 = 0.01;
Qc{1} = diag([q1 q1]);

% Discretization of the continous-time system.
[A{1},Q{1}] = lti_disc(F{1},L{1},Qc{1},dt);

% Process noise variance for model 1 using EKF
EKF_q1 = .05;
EKF_Qc1 = diag([EKF_q1 EKF_q1]);

% Discretization of the continous-time system.
[EKF_A1,EKF_Q1] = lti_disc(F{1},L{1},EKF_Qc1,dt);

%%% Specification of the turning model

% System components. 5th parameter is the turning rate
ind{2} = [1 2 3 4 5]';

% Derivative of the dynamic function
A{2} = @f_turn_dx;
% Process noise for the turning rate
Qc{2} = 0.15;

% Noise effect matrix
L{2} = [0 0 0 0 1]';

% Process noise covariance
Q{2} = L{2}*Qc{2}*L{2}'*dt;

hdims = 2;

%mu_ip = [0.90 0.05 0.05];
mu_ip = [0.95 0.05];
mu_0j = mu_ip;
%p_ij = [0.65 0.35;
%        0.10 0.90];

p_ij = [0.90 0.10;
        0.10 0.90];

% Number of data points
n = 200;

% Space for real states and modes
X_r = zeros(dims,n);
mstate = zeros(1,n);

%%%%%%% Creation of trajectory %%%%%%%

% Start with constant velocity 1 toward right
mstate(1:40) = 1;
X_r(:,1) = [0 0 1 0 0]';

% At 4s make a turn left with rate 1
mstate(41:90) = 2;
X_r(5,40) = 1;

% At 9s move straight for 2 seconds
mstate(91:110) = 1;

% At 11s commence another turn right with rate -1
mstate(111:160) = 2;
X_r(5,110) = -1;

% At 16s move straight for 4 seconds
mstate(161:200) = 1;

% Generate object state values
for i = 2:n
   st = mstate(i);
   if isstr(a_func{st}) | strcmp(class(a_func{st}),'function_handle')
       X_r(ind{st},i) = feval(a_func{st},X_r(ind{st},i-1),a_param{st});
   else
       X_r(ind{st},i) = A{st}*X_r(ind{st},i-1);
   end
end

% Positions of sensors
% $$$ S1 = [-0.5; 3];
% $$$ S2 = [-0.5;-3];
% $$$ S3 = [   7;-3];
% $$$ S4 = [   7; 3];

S1 = [-0.5; 3.5];
S2 = [-0.5;-3.5];
S3 = [   7;-3.5];
S4 = [   7; 3.5];

s = [S1 S2 S3 S4];

% Handles to measurement models
H{1} = @bot_dh_dx;
H{2} = @bot_dh_dx;

h_func{1} = @bot_h;
h_func{2} = @bot_h;

h_param{1} = s;
h_param{2} = s;

% Standard deviation
sd = 0.1*ones(1,size(s,2));
R = {};
R{1} = diag(sd.^2);
R{2} = R{1};
% Generate measurements
Y = bot_h(X_r,s);
% Add noise
for i = 1:size(Y,2)
    st = mstate(i);
    for j = 1:size(Y,1)
        Y(j,i) = Y(j,i) + sqrt(R{st}(j,j)) * randn;
    end
end

% Print the trajectory
if print_figures
    h = plot(X_r(1,:),X_r(2,:),'-g',...
             s(1,:),s(2,:),'k^',...
             X_r(1,1),X_r(2,1),'ro','MarkerSize',12);
    legend('Real trajectory',...
           'Positions of sensors',...
           'Starting position', 'Location', 'North');
    set(h,'markersize',5);
    set(h,'linewidth',0.5);
    set(gca,'FontSize',8);
    xlim([-1 7.5])
    ylim([-4 4])
    if save_figures
        print('-depsc','eimm2_trajectory.eps');
    end
    pause
end

m = [0 0 0 -1 0]';
P = diag([10.1 10.1 1.1 1.1 1]);

%% Space for the estimates.

% EKF with model 1
EKF_MM = zeros(size(A{1},1), size(Y,2));
EKF_PP = zeros(size(A{1},1), size(A{1},1), size(Y,2));

% UKF with model 1
UKF_MM = zeros(size(A{1},1), size(Y,2));
UKF_PP = zeros(size(A{1},1), size(A{1},1), size(Y,2));

% EKF based IMM
EIMM_MM = zeros(size(m,1), size(Y,2));
EIMM_PP = zeros(size(m,1), size(m,1), size(Y,2));
EIMM_MM_i = cell(nmodels,n);
EIMM_PP_i = cell(nmodels,n);
EIMM_MU = zeros(nmodels,size(Y,2));

% UKF based IMM
UIMM_MM = zeros(size(m,1), size(Y,2));
UIMM_PP = zeros(size(m,1), size(m,1), size(Y,2));
UIMM_MM_i = cell(nmodels,n);
UIMM_PP_i = cell(nmodels,n);
UIMM_MU = zeros(nmodels,size(Y,2));

%%% Initial estimates %%%

% EKF with model 1
EKF_M = [0 0 0 -1]';
EKF_P = diag([1.1 1.1 0.1 0.1]);

% UKF with model 1
UKF_M = [0 0 0 -1]';
UKF_P = diag([1.1 1.1 0.1 0.1]);

% EKF based IMM
x_ip1{1} = [0 0 1 0]';
x_ip1{2} = [0 0 1 0 0]';
mu_ip1 = mu_ip;

P_ip1{1} = diag([0.1 0.1 0.1 0.1]);
P_ip1{2} = diag([0.1 0.1 0.1 0.1 1]);

% UKF based IMM
x_ip2{1} = [0 0 1 0]';
x_ip2{2} = [0 0 1 0 0]';
mu_ip2 = mu_ip;

P_ip2{1} = diag([0.1 0.1 0.1 0.1]);
P_ip2{2} = diag([0.1 0.1 0.1 0.1 1]);

% Filtering steps.
for i = 1:size(Y,2)
    % EKF with model 1
    [EKF_M,EKF_P] = kf_predict(EKF_M,EKF_P,EKF_A1,EKF_Q1);
    [EKF_M,EKF_P] = ekf_update1(EKF_M,EKF_P,Y(:,i),H{1},R{1},h_func{1},[],h_param{1});

    EKF_MM(:,i)   = EKF_M;
    EKF_PP(:,:,i) = EKF_P;

    % UKF with model 2
    [UKF_M,UKF_P] = kf_predict(UKF_M,UKF_P,EKF_A1,EKF_Q1);
    [UKF_M,UKF_P] = ukf_update1(UKF_M,UKF_P,Y(:,i),h_func{1},R{1},h_param{1});

    UKF_MM(:,i)   = UKF_M;
    UKF_PP(:,:,i) = UKF_P;

    % EKF based IMM
    [x_p1,P_p1,c_j1] = eimm_predict(x_ip1,P_ip1,mu_ip1,p_ij,ind,dims,A,a_func,a_param,Q);
    [x_ip1,P_ip1,mu_ip1,m1,P1] = eimm_update(x_p1,P_p1,c_j1,ind,dims,Y(:,i),H,h_func,R,h_param);
    EIMM_MM(:,i)   = m1;
    EIMM_PP(:,:,i) = P1;
    EIMM_MU(:,i)   = mu_ip1';
    EIMM_MM_i(:,i) = x_ip1';
    EIMM_PP_i(:,i) = P_ip1';

    % UKF based IMM
    [x_p2,P_p2,c_j2] = uimm_predict(x_ip2,P_ip2,mu_ip2,p_ij,ind,dims,A,a_func,a_param,Q);
    [x_ip2,P_ip2,mu_ip2,m2,P2] = uimm_update(x_p2,P_p2,c_j2,ind,dims,Y(:,i),H,h_func,R,h_param);

    UIMM_MM(:,i)   = m2;
    UIMM_PP(:,:,i) = P2;
    UIMM_MU(:,i)   = mu_ip2';
    UIMM_MM_i(:,i) = x_ip2';
    UIMM_PP_i(:,i) = P_ip2';

    % Plot the estimates so far
    if print_figures
        plot(EKF_MM(1,1:i),EKF_MM(2,1:i),'y-',...
             EIMM_MM(1,1:i),EIMM_MM(2,1:i),'r-',...
             UIMM_MM(1,1:i),UIMM_MM(2,1:i),'b-',...
             X_r(1,1:i),X_r(2,1:i),'g-');

        % Measurement directions
        hold on
        for k = 1:size(s,2)
            len = sqrt(sum((X_r(1:2,i)-s(:,k)).^2,1));
            dx = len*cos(Y(k,i));
            dy = len*sin(Y(k,i));

            plot([s(1,k);s(1,k)+dx], [s(2,k);s(2,k)+dy], 'k--')
        end
        plot(s(1,:),s(2,:),'k^')
        xlim([-1 7.5])
        ylim([-4 4])
        hold off
        drawnow
    end
end

% Smooth with EKF based IMM smoother
[SMI_1,SPI_1,SMI_i_1,SPI_i_1,MU_S_1] = eimm_smooth(EIMM_MM,EIMM_PP,EIMM_MM_i,EIMM_PP_i,EIMM_MU,p_ij,mu_0j,ind,dims,A,ia_func,a_param,Q,R,H,h_func,h_param,Y);

% Smooth with UKF based IMM smoother
[SMI_2,SPI_2,SMI_i_2,SPI_i_2,MU_S_2] = uimm_smooth(UIMM_MM,UIMM_PP,UIMM_MM_i,UIMM_PP_i,UIMM_MU,p_ij,mu_0j,ind,dims,A,ia_func,a_param,Q,R,H,h_func,h_param,Y);

% Smooth the EKF estimates with RTS smoother using model 1
[SM1,SP1] = rts_smooth(EKF_MM,EKF_PP,EKF_A1,EKF_Q1);

% Smooth the UKF estimates with RTS smoother using model 1
[SM2,SP2] = rts_smooth(UKF_MM,UKF_PP,EKF_A1,EKF_Q1);

% Calculate the MSEs
MSE_EKF1_1 = mean((X_r(1,:)-EKF_MM(1,:)).^2);
MSE_EKF1_2 = mean((X_r(2,:)-EKF_MM(2,:)).^2);
MSE_EKF1 = 1/2*(MSE_EKF1_1 + MSE_EKF1_2);

MSE_EKS1_1 = mean((X_r(1,:)-SM1(1,:)).^2);
MSE_EKS1_2 = mean((X_r(2,:)-SM1(2,:)).^2);
MSE_EKS1 = 1/2*(MSE_EKS1_1 + MSE_EKS1_2);

% Calculate the MSEs
MSE_UKF1_1 = mean((X_r(1,:)-UKF_MM(1,:)).^2);
MSE_UKF1_2 = mean((X_r(2,:)-UKF_MM(2,:)).^2);
MSE_UKF1 = 1/2*(MSE_UKF1_1 + MSE_UKF1_2);

MSE_UKS1_1 = mean((X_r(1,:)-SM2(1,:)).^2);
MSE_UKS1_2 = mean((X_r(2,:)-SM2(2,:)).^2);
MSE_UKS1 = 1/2*(MSE_UKS1_1 + MSE_UKS1_2);

MSE_EIMM1 = mean((X_r(1,:)-EIMM_MM(1,:)).^2);
MSE_EIMM2 = mean((X_r(2,:)-EIMM_MM(2,:)).^2);
MSE_EIMM = 1/2*(MSE_EIMM1 + MSE_EIMM2);

MSE_EIMMS1 = mean((X_r(1,:)-SMI_1(1,:)).^2);
MSE_EIMMS2 = mean((X_r(2,:)-SMI_1(2,:)).^2);
MSE_EIMMS = 1/2*(MSE_EIMMS1 + MSE_EIMMS2);

MSE_UIMM1 = mean((X_r(1,:)-UIMM_MM(1,:)).^2);
MSE_UIMM2 = mean((X_r(2,:)-UIMM_MM(2,:)).^2);
MSE_UIMM = 1/2*(MSE_UIMM1 + MSE_UIMM2);

MSE_UIMMS1 = mean((X_r(1,:)-SMI_2(1,:)).^2);
MSE_UIMMS2 = mean((X_r(2,:)-SMI_2(2,:)).^2);
MSE_UIMMS = 1/2*(MSE_UIMMS1 + MSE_UIMMS2);

fprintf('Mean square errors of position estimates:\n');
fprintf('EKF1-RMSE = %.4f\n',MSE_EKF1);
fprintf('EKS1-RMSE = %.4f\n',MSE_EKS1);
fprintf('UKF1-RMSE = %.4f\n',MSE_UKF1);
fprintf('UKS1-RMSE = %.4f\n',MSE_UKS1);
fprintf('EIMM-RMSE = %.4f\n',MSE_EIMM);
fprintf('EIMMS-RMSE = %.4f\n',MSE_EIMMS);
fprintf('UIMM-RMSE = %.4f\n',MSE_UIMM);
fprintf('UIMMS-RMSE = %.4f\n',MSE_UIMMS);

% Plot the final filtering and smoothing results
if print_figures
    h = plot(X_r(1,:),X_r(2,:),'g-',...
             EKF_MM(1,:),EKF_MM(2,:),'-k',...
             SM1(1,:),SM1(2,:),'--k',...
             EIMM_MM(1,:),EIMM_MM(2,:),'-r',...
             SMI_1(1,:),SMI_1(2,:),'--r',...
             UIMM_MM(1,:),UIMM_MM(2,:),'-b',...
             SMI_2(1,:),SMI_2(2,:),'--b');
    legend('True trajectory',...
           'EKF',...
           'ERTS',...
           'IMM-EKF',...
           'IMM-EKS',...
           'IMM-UKF',...
           'IMM-UKS');
    %title('Estimates produced by IMM-filter.')
    set(h,'markersize',2);
    set(h,'linewidth',0.5);
    set(gca,'FontSize',8);
    xlim([-1 7.5])
    ylim([-4 4])
    if save_figures
        print('-depsc','eimm2_1.eps');
    end
    pause

    % Determine the real model probabilities
    p_models = zeros(nmodels,n);
    I1 = find(mstate == 1);
    p_models(1,I1) = 1;
    I2 = find(mstate == 2);
    p_models(2,I2) = 1;

    % Plot model 2 probability for filters
    h = plot(1:n, p_models(2,:),'g--',...
             1:n,EIMM_MU(2,:)','-r',...
             1:n,MU_S_1(2,:)','--r',...
             1:n,UIMM_MU(2,:)','-b',...
             1:n,MU_S_2(2,:)','--b');
    %1:n,MU_S_2(2,:)','b-');
    legend('True',...
           'IMM-EKF',...
           'IMM-EKS',...
           'IMM-UKF',...
           'IMM-UKS');
    %title('Probability of model 2');
    ylim([-0.1,1.1]);
    set(h,'markersize',2);
    set(h,'linewidth',0.5);
    set(gca,'FontSize',8);
    if save_figures
        print('-depsc','eimm2_2.eps');
    end
    pause

    % Collect the turn rate estimates
    EMM_t = zeros(5,n);
    EMM_s = zeros(5,n);
    UMM_t = zeros(5,n);
    UMM_s = zeros(5,n);
    for i = 1:n
        EMM_t(:,i) = EIMM_MM_i{2,i};
        EMM_s(:,i) = SMI_i_1{2,i};
        UMM_t(:,i) = UIMM_MM_i{2,i};
        UMM_s(:,i) = SMI_i_2{2,i};
    end

    % Plot the filtered turn rates
    h = plot(1:n, X_r(5,:),'-g',...
             1:n,EMM_t(5,:),'-r',...
             1:n,EMM_s(5,:),'--r',...
             1:n,UMM_t(5,:),'-b',...
             1:n,UMM_s(5,:),'--b');
    %title('Turn rate estimates')
    legend('True',...
           'IMM-EKF',...
           'IMM-EKS',...
           'IMM-UKF',...
           'IMM-UKS');
    set(h,'markersize',2);
    set(h,'linewidth',0.5);
    set(gca,'FontSize',8);
    if save_figures
        print('-depsc','eimm2_3.eps');
    end

end


function varargout = fevalm(varargin)
%FEVALM   Evaluate a DISKFUN in polar coordinates.
%
%   Z = FEVALM(F, THETA, R) returns a matrix of values Z of size
%   length(R)-by-length(THETA). (R,THETA) are polar coordinates for the
%   evaluation points on the disk.  They should be vectors of doubles.
%   Calling this function as above is equivalent to making a meshgrid of
%   the vectors THETA and R and then using FEVAL to evaluate at that grid.
%
% See also DISKFUN/FEVAL.

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

[varargout{1:nargout}] = fevalm@separableApprox(varargin{:});

end


%% Lane Marking Identification
% One component of future luxury automobile safety
% systems is warning drivers that they are drifting
% between lanes.  To do this, it is first necessary
% to identify where the lanes are.  In this example,
% we will look at identifying the lanes from a image
% of a road.
%
% Copyright 2007-2013 MathWorks, Inc.
%

%% Setup
clear

%% Initialize Objects
% Video Reader

hvfr = vision.VideoFileReader('viplanedeparture.avi', ...
    'VideoOutputDataType', 'uint8');

%%
% Create a |ColorSpaceConverter| System object to
% convert the RGB image to an intensity image.
hColor = vision.ColorSpaceConverter( ...
               'Conversion', 'RGB to intensity');

%%
% Create Blob Detecter

hBlob = vision.BlobAnalysis(            ...
    'MinimumBlobAreaSource',     'Property', ...
    'MinimumBlobArea',           10        , ...
    'MajorAxisLengthOutputPort', true      , ...
    'MinorAxisLengthOutputPort', true      , ...
    'OrientationOutputPort',     true      , ...
    'CentroidOutputPort',        true      , ...
    'BoundingBoxOutputPort',     true      , ...
    'LabelMatrixOutputPort',     false     , ...
    'AreaOutputPort',            false     );

%% Create Shape Inserter
% Create a |ShapeInserter| System object to draw
% lanes on the original image.
hShapes = vision.ShapeInserter( ...
                'Shape', 'Lines', ...
                'BorderColor', 'Custom', ...
                'CustomBorderColor', [0 255 0], ...
                'Antialiasing', true);

%% Configure Video Players
hVidSource = vision.VideoPlayer;
hVidBlobs  = vision.VideoPlayer;
hVidLanes  = vision.VideoPlayer;

%% Setup Replay Loop
NumLoops = 3;
for loopIdx = 1:NumLoops

    % Loop through video frames
    while ~isDone(hvfr)
        % Read Frame
        frameRGB = step(hvfr);
        % and display
        step(hVidSource, frameRGB);

        % Convert to Intensity
        frameGray = step(hColor, frameRGB);

        % Thresholding image using Otsu's method
        level = graythresh(frameGray) * intmax(class(frameGray));
        binary = frameGray > level;

        % Perform blob detection
        [centroid, bbox, Major, Minor, Orientation] = step(hBlob, binary);
        step(hVidBlobs, binary);

        % Find lanes
        laneIndex = findlanes(bbox, Major, Minor);

        numLanes = sum(laneIndex);

        % Mark Lanes on original image
        laneVertices = getLaneVerticies(bbox(laneIndex, :), Orientation(laneIndex));
        laneVertices = int32(laneVertices);

        laneImage = step(hShapes, frameRGB, laneVertices);
        step(hVidLanes, laneImage);

    % End While
    end

    reset(hvfr);
% end loop
end

%% Release resources
release(hvfr   );
release(hColor );
release(hBlob  );
release(hShapes);


function T2 = sltensor_multiply(T, varargin)
%SLTENSOR_MULTIPLY Multiplies a tensor and a matrix
%
% $ Syntax $
%   - T2 = sltensor_multiply(T, M, k)
%   - T2 = sltensor_multiply(T, Ms)
%   - T2 = sltensor_multiply(T, Ms, ks)
%   - T2 = sltensor_multiply(T, M1, k1, M2, k2, ...)
%
% $ Description $
%   - T2 = sltensor_multiply(T, M, k) Computes the tensor multiplication between
%   a tensor and a 2D matrix along the k-th mode.
%
%   - T2 = sltensor_multiply(T, Ms) Sequentially multiplies the tensor with
%   the elements in Ms, which is a cell array from the mode 1 to n. n is
%   the number of matrices in Ms. It should be that n == ndims(T).
%
%   - T2 = sltensor_multiply(T, Ms, ks) Sequentially multiplies the tensor
%   with the matrices in the cell array Ms along the modes specified in
%   corresponding element in ks. ks should be an array with the same size
%   as Ms.
%
%   - T2 = sltensor_multiply(T, M1, k1, M2, k2, ...) Sequentially multiplies
%   the tensor with the matrices M1, M2, ... along the modes k1, k2, ...
%
% $ History $
%   - Created by Dahua Lin on Dec 17th, 2005
%

%% parse and verify input arguments
if nargin < 2
    raise_lackinput('sltensot_multiply', 2);
end
n0 = ndims(T);
if iscell(varargin{1});
    Ms = varargin{1};
    if nargin == 2
        n = numel(Ms);
        if n ~= n0
            error('sltoolbox:invalidarg', ...
                'For the case no specifying mode indices, it should be n == ndims(T)');
        end
        ks = 1:n;
    elseif nargin == 3
        ks = varargin{2};
        if ~isequal(size(Ms), size(k))
            error('sltoolbox:argmismatch', ...
                'The size of ks does not match that of Ms');
        end
    else
        error('sltoolbox:invalidarg', ...
            'Invalid input arguments');
    end
else
    if mod(length(varargin), 2) ~= 0
        error('sltoolbox:invalidarg', ...
            'Invalid input arguments');
    end
    Ms = varargin(1:2:end);
    ks = [varargin{2:2:end}];
    if numel(Ms) ~= numel(ks)
        error('sltoolbox:invalidarg', ...
            'Invalid input arguments');
    end
end
if any(ks(:) < 1)
    error('sltoolbox:invalidarg', ...
        'mode indices should all be positive integers');
end
maxk = max(ks(:));
dims = size(T);
if maxk > n0
    dims = [dims, ones(1, maxk-n0)];
end
nmat = numel(ks);

%% compute
if nmat == 1
    T2 = multiply_tensor_matrix(T, Ms{1}, ks, dims);
else
    T2 = T;
    for i = 1 : nmat
        T2 = multiply_tensor_matrix(T2, Ms{i}, ks(i), dims);
    end
end

%% compute (by converting to matrix product)

function T2 = multiply_tensor_matrix(T, M, k, dims)

Tk = sltensor_unfold(T, k);
T2 = M * Tk;
clear Tk;
dims(k) = size(M, 1);
T2 = sltensor_fold(T2, dims, k);


function [mmHg] = mbar2mmHg(mbar)
% Convert pressure from millibars to millimeters of mercury.
% Chad Greene 2012
mmHg = mbar*0.750062;


function [g] = spm_gx_SHC(x,v,P)
% maps to state of a SCH to a 2-D position in the world
% FORMAT [g] = spm_gx_SHC(x,v,P)
%
% x    - vector of hidden sates
% P.g  - state-space location associated with each hidden states
%
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_gx_SHC.m 3113 2009-05-11 15:25:13Z karl $

% expected position (coordinates in P.g)
%--------------------------------------------------------------------------
px     = exp(x(:));
px     = px/sum(px);
g      = P.g*px;


% LFHelperBuild4DFreq - Helper function used to construct 4D frequency-domain filters
%
% Much of the complexity in constructing MD frequency-domain filters, especially around including
% aliased components and controlling the filter rolloff, is common between filter shapes.  This
% function wraps much of this complexity.
%
% This gets called by the LFBuild4DFreq* functions.

% Copyright (c) 2013-2020 Donald G. Dansereau

function [H, FiltOptions] = LFHelperBuild4DFreq( LFSize, BW, FiltOptions, DistFunc )

FiltOptions = LFDefaultField('FiltOptions', 'Precision', 'single');
FiltOptions = LFDefaultField('FiltOptions', 'Rolloff', 'Gaussian'); %Gaussian or Butter
FiltOptions = LFDefaultField('FiltOptions', 'Aspect4D', 1);
FiltOptions = LFDefaultField('FiltOptions', 'Window', false);
FiltOptions = LFDefaultField('FiltOptions', 'Extent4D', 1.0);
FiltOptions = LFDefaultField('FiltOptions', 'IncludeAliased', false);

% avoid rounding error during comparisons
FiltOptions.Extent4D = cast(FiltOptions.Extent4D, FiltOptions.Precision);
FiltOptions.Aspect4D = cast(FiltOptions.Aspect4D, FiltOptions.Precision);

if( length(LFSize) == 1 )
	LFSize = LFSize .* [1,1,1,1];
end
if( length(LFSize) == 5 )
	LFSize = LFSize(1:4);
end
if( length(FiltOptions.Extent4D) == 1 )
	FiltOptions.Extent4D = FiltOptions.Extent4D .* [1,1,1,1];
end
if( length(FiltOptions.Aspect4D) == 1 )
	FiltOptions.Aspect4D = FiltOptions.Aspect4D .* [1,1,1,1];
end
ExtentWithAspect = FiltOptions.Extent4D.*FiltOptions.Aspect4D / 2;

t = LFNormalizedFreqAxis( LFSize(1), FiltOptions.Precision ) .* FiltOptions.Aspect4D(1);
s = LFNormalizedFreqAxis( LFSize(2), FiltOptions.Precision ) .* FiltOptions.Aspect4D(2);
v = LFNormalizedFreqAxis( LFSize(3), FiltOptions.Precision ) .* FiltOptions.Aspect4D(3);
u = LFNormalizedFreqAxis( LFSize(4), FiltOptions.Precision ) .* FiltOptions.Aspect4D(4);
[tt,ss,vv,uu] = ndgrid(cast(t,FiltOptions.Precision),cast(s,FiltOptions.Precision), ...
	cast(v,FiltOptions.Precision),cast(u,FiltOptions.Precision));
P = [tt(:),ss(:),vv(:),uu(:)]';
clear s t u v ss tt uu vv

if( ~FiltOptions.IncludeAliased )
	Tiles = [0,0,0,0];
else
	Tiles = ceil(ExtentWithAspect) % todo[optimization]: optimization possible for large extents
end

if( FiltOptions.IncludeAliased )
	AADist = inf(LFSize, FiltOptions.Precision);
end
WinDist = 0;

% Tiling proceeds only in positive directions, symmetry is enforced afterward, saving some computation overall
for( TTile = 0:Tiles(1) )
	for( STile = 0:Tiles(2) )
		for( VTile = 0:Tiles(3) )
			for( UTile = 0:Tiles(4) )
				Dist = inf(LFSize, FiltOptions.Precision);

				if( FiltOptions.Window )
					ValidIdx = ':';
					WinDist = bsxfun(@minus, abs(P)', ExtentWithAspect);
					WinDist = sum(max(0,WinDist).^2, 2);
					WinDist = reshape(WinDist, LFSize);
				else
					ValidIdx = find(all(bsxfun(@le, abs(P), ExtentWithAspect')));
				end

				Dist(ValidIdx) = DistFunc(P(:,ValidIdx), FiltOptions);
				Dist = Dist + WinDist;

				if( FiltOptions.IncludeAliased )
					AADist(:) = min(AADist(:), Dist(:));
				end

				P(4,:) = P(4,:) + FiltOptions.Aspect4D(4);
			end
			P(4,:) = P(4,:) - (Tiles(4)+1) .* FiltOptions.Aspect4D(4);
			P(3,:) = P(3,:) + FiltOptions.Aspect4D(3);
		end
		P(3,:) = P(3,:) - (Tiles(3)+1) .* FiltOptions.Aspect4D(3);
		P(2,:) = P(2,:) + FiltOptions.Aspect4D(2);
	end
	P(2,:) = P(2,:) - (Tiles(2)+1) .* FiltOptions.Aspect4D(2);
	P(1,:) = P(1,:) + FiltOptions.Aspect4D(1);
end

if( FiltOptions.IncludeAliased )
	Dist = AADist;
	clear AADist
end

H = zeros(LFSize, FiltOptions.Precision);

switch lower(FiltOptions.Rolloff)

	case 'gaussian'
		BW = BW.^2 / log(sqrt(2));
		H(:) = exp( -Dist / BW );  % Gaussian rolloff

	case 'butter'
		FiltOptions = LFDefaultField('FiltOptions', 'Order', 3);
		Dist = sqrt(Dist) ./ BW;
		H(:) = sqrt( 1.0 ./ (1.0 + Dist.^(2*FiltOptions.Order)) ); % Butterworth-like rolloff

	case 'sinc'
		Dist = sqrt(Dist) ./ BW;
		H(:) = sinc(Dist); % sinc-shaped rolloff

	otherwise
		error('unrecognized rolloff method');

end

H = ifftshift(H);

% force symmetric -- needed to visualize in 4D to get this one right
H = max(H, H(mod(LFSize(1):-1:1,LFSize(1))+1, mod(LFSize(2):-1:1,LFSize(2))+1, ...
	mod(LFSize(3):-1:1,LFSize(3))+1, mod(LFSize(4):-1:1,LFSize(4))+1));
H = max(H, H(:, mod(LFSize(2):-1:1,LFSize(2))+1, :, mod(LFSize(4):-1:1,LFSize(4))+1));
H = max(H, H(mod(LFSize(1):-1:1,LFSize(1))+1, :, mod(LFSize(3):-1:1,LFSize(3))+1, :));
% todo: confirm this is sufficient


% mlp_classify
% Copyright (C) 2011 KyungHyun Cho, Tapani Raiko, Alexander Ilin
%
%This program is free software; you can redistribute it and/or
%modify it under the terms of the GNU General Public License
%as published by the Free Software Foundation; either version 2
%of the License, or (at your option) any later version.
%
%This program is distributed in the hope that it will be useful,
%but WITHOUT ANY WARRANTY; without even the implied warranty of
%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%GNU General Public License for more details.
%
%You should have received a copy of the GNU General Public License
%along with this program; if not, write to the Free Software
%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
%
function [c, posterior] = mlp_classify(M, x0, Q0, raw)

if nargin < 3
    Q0 = [];
end

if nargin < 4
    raw = 0;
end

layers = M.structure.layers;
n_layers = length(layers);

posterior = x0;

if isfield(M, 'dbm') && M.dbm.use
    for l = 2:n_layers
        if M.dropout.use && l > 2
            posterior = posterior * bsxfun(@times, (M.W{l-1}), 1 - M.dropout.probs{l-1});
        else
            posterior = posterior * M.W{l-1};
        end

        if l < n_layers-1
            posterior = posterior + Q0{l+1} * (M.dbm.W{l})';
        end
        posterior = bsxfun(@plus, posterior, M.biases{l}');

        if l < n_layers
            posterior = sigmoid(posterior, M.hidden.use_tanh);
        end

        if l == n_layers && M.output.binary
            posterior = softmax(posterior);
        end
    end
else
    for l = 2:n_layers
        if M.dropout.use && l > 2
            posterior = bsxfun(@plus, posterior * (M.W{l-1}/2), M.biases{l}');
        else
            posterior = bsxfun(@plus, posterior * M.W{l-1}, M.biases{l}');
        end

        if l < n_layers
            posterior = sigmoid(posterior, M.hidden.use_tanh);
        end

        if l == n_layers && M.output.binary
            posterior = softmax(posterior);
        end
    end
end

if raw
    c = posterior;
else
    [maxp, c] = max(posterior, [], 2);
end


classdef StructuredMesh < handle

    properties (Access = public)
        nx
        ny
        x
        y
        mesh
    end

    properties (Access = private)
       xv
       yv
    end

    methods (Access = public)

        function obj = StructuredMesh(cParams)
            obj.init(cParams);
            s.coord  = obj.createCoordinates();
            s.connec = obj.createConnectivities();
            obj.createMesh(s);
        end

    end

    methods (Access = private)

        function init(obj,cParams)
            [obj.x,obj.y] = meshgrid(cParams.x,cParams.y);
            obj.nx = length(obj.x);
            obj.ny = length(obj.y);
        end

        function coord = createCoordinates(obj)
            coord(:,1) = reshape(obj.x',1,[]);
            coord(:,2) = reshape(obj.y',1,[]);
        end

        function connec = createConnectivities(obj)
            Nx = obj.nx;
            Ny = obj.ny;
            sw = obj.createVertex(1:Nx-1,1:Ny-1);
            se = obj.createVertex(2:Nx,1:Ny-1);
            ne = obj.createVertex(2:Nx,2:Ny);
            nw = obj.createVertex(1:Nx-1,2:Ny);
            connec = [sw se ne nw];
        end

        function v = createVertex(obj,xv,yv)
            Ny = obj.ny;
            v = bsxfun(@(x,y) x + Ny*(y-1),xv',yv);
            v = v(:);
        end

        function createMesh(obj,s)
            s.kFace = 0;
            obj.mesh = Mesh(s);
        end

    end

end


function F = stream2d(A,vector)
%% Stream function for any D2Qn Lattice
% There are only 4 basic movements and any other is a combination of the
% basic ones:

% Matlab equivalent solution:
% F = circshift(A,[y,x])

%% Evaluate input data

v = length(vector);

    y = vector(1);
    x = vector(2);

[m,n] = size(A);

    b = zeros(1,n);
    c = zeros(m,1);

%% Stream in XY plane

if x == 1       % x+ stream
    b = A(:,n);
    for i = n:-1:2;
        A(:,i) = A(:,i-1);
    end
    A(:,1) = b;
elseif x == -1  % x- stream
    b = A(:,1);
    for i = 1:n-1;
        A(:,i) = A(:,i+1);
    end
    A(:,n) = b;
else            % x=0 stream
    %do nothing
end

if y == 1       % y+ stream
    c = A(m,:);
    for j = m:-1:2;
        A(j,:) = A(j-1,:);
    end
    A(1,:) = c;
elseif y == -1  % y- stream
    c = A(1,:);
    for j = 1:m-1;
        A(j,:) = A(j+1,:);
    end
    A(m,:) = c;
else            % y=0 stream
    %do nothing
end

F = A;


function X = QLiftRec2(C, S, filtername)
%-----------------------------------------------------------------------------
% QLiftRec2
% Multilevel 2-D reconstruction by inverting the lifting scheme and using
% quincunx grids
%
% Syntax: X = QLiftRec2(C, S, filtername)
%
% QLiftRec2 performs the reconstruction of a two-dimensional signal (matrix,
% image) X by inverting the lifting scheme using prediction and update filters
% that are indicated by filtername.
% The reconstruction involves the vector of coefficients C and the bookkeeping
% matrix S. The structure and dimensions of C and S are supposed to be
% consistent with how they are produced by QLiftDec2.
% QLiftRec2 is the inverse function of QLiftDec2.
% filtername is supposed to be the same string as chosen for QLiftDec2.
%
% Output X is a two-dimensional signal (image).
%
% Calls for: QLiftRec2Nevill, QLiftRec2MaxMin, QLiftRec2MinMin, QLiftRec2MaxMax
%
% See also: QLiftDec2
%
% Design and implementation by:
% Dr. Paul M. de Zeeuw <Paul.de.Zeeuw@cwi.nl>  http://homepages.cwi.nl/~pauldz/
% Last Revision: February 17, 2003.
% (c) 1999-2003 Stichting CWI, Amsterdam
%-----------------------------------------------------------------------------
% Note: future argument list of this function might be extended with an
% argument that points to another level than 1, nargin could be checked
% for this.
if isempty(C)
  error(' QLiftRec2 - empty decomposition ');
else
  if isempty(S)
    error(' QLiftRec2 - empty bookkeeping ');
  else
    if strncmpi(filtername,'Neville',7)
       X = QLiftRec2Nevill(C,S,filtername);
    elseif strncmpi(filtername,'MaxMin',6)
       X = QLiftRec2MaxMin(C,S);
    elseif strncmpi(filtername,'MinMin',6)
       X = QLiftRec2MinMin(C,S);
    elseif strncmpi(filtername,'MaxMax',6)
       X = QLiftRec2MaxMax(C,S);
%   elseif strncmpi(filtername,'some',4)
%      stencilP = something;
%      centerP =  something;
%      stencilU = something;
%      centerU =  something;
%      X = QLiftRec2Custom(C, S, stencilP, centerP, stencilU, centerU);
    else
       error([' QLiftRec2 - unknown filter ' filtername]);
    end
  end
end
%
%-----------------------------------------------------------------------------


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% function [rho,drho] = rhoSpline(Tc,Rc,minT,maxT,nT,minR,maxR,nR,doDerivative)
%
% Parzen-Window Based density estimator for gray values of Tc and Rc, spline based.
%
% Input:
%  Tc, Rc          template and reference
%  minT, maxT, nT  discreted points in gray value range where splines are located
%  minR, maxR, nR  discreted points in gray value range where splines are located
%  doSerivative         FLAG for derivative compuatation
%
% Output:
%  rho          joint density estimator
%  drho         derivative of joint density estimator
%
% see also MIspline for an example.
%==============================================================================

function [rho,drho] = rhoSpline(Tc,Rc,minT,maxT,nT,minR,maxR,nR,doDerivative)

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

if ~exist('doDerivative','var'), doDerivative = 1;       end;
doDerivative = doDerivative & (nargout>1);

% prepare the output and organize input
rho  = []; drho = []; Tc = reshape(Tc,[],1); Rc = reshape(Rc,[],1);

widthT = (maxT-minT)/nT;
widthR = (maxR-minR)/nR;

Tt   = linspace(minT,maxT,nT);
Rt   = linspace(minR,maxR,nR);
rho  = zeros(length(Tt),length(Rt));
drho = spalloc(numel(rho),length(Tc),5*length(Tc));

for j=1:length(Tc), % run over all samples
  IT = find(abs(Tt-Tc(j))<widthT); % find locations of interest in Tc
  IR = find(abs(Rt-Rc(j))<widthR); % find locations of interest in Rc
  if ~isempty(IT) && ~isempty(IR),
    [KT,dKT] = spline1D(Tc(j) - Tt(IT),widthT,doDerivative);
    KR       = spline1D(Rc(j) - Rt(IR),widthR,0);
    rho(IT,IR) = rho(IT,IR) +KT*KR';
    if doDerivative,
      drhoj = zeros(size(rho));
      drhoj(IT,IR) = dKT*KR';
      drho(:,j) = drho(:,j) + sparse(drhoj(:));
    end;
  end;
end;

% normalize rho
fac = (Tt(2)-Tt(1))*(Rt(2)-Rt(1))/length(Tc);
rho  = fac*rho(:);
drho = fac*drho;

%------------------------------------------------------------------------------

% The Parzen Window Kernel is a spline function
%  function [s,ds] = spline1D(x,sigma,doDerivative);
% (c) Jan Modersitzki 2008/02/12, see FAIR.
% This evaluates a spline function s
% s(x,sigma) = 0 for x\notin[-sigma,sigma], \int s(x,sigma) dx = 1

function [s,ds] = spline1D(x,sigma,doDerivative);

if ~exist('doDerivative','var'), doDerivative = 1;       end;
doDerivative = doDerivative & (nargout>1);

% map [-sigm2,sigma] \to [-2,2]
x = 4/sigma*x;

if ~doDerivative,
  s  = 0*reshape(x,[],1);
  ds = [];
  J = find(x>=-2 & x<-1);   s(J)   = (x(J)+2).^3;
  J = find(x>=-1 & x< 0);   s(J)   = -x(J).^3 - 2*(x(J)+1).^3+6*(x(J)+1);
  J = find(x>= 0 & x< 1);   s(J)   = -2*(1-x(J)).^3 + x(J).^3 - 6*x(J) + 6;
  J = find(x>= 0 & x< 1);   s(J)   = -2*(1-x(J)).^3 + x(J).^3 - 6*x(J) + 6;
  J = find(x>= 1 & x< 2);   s(J)   = (2-x(J)).^3;
else
  s  = 0*reshape(x,[],1);
  ds = s;
  J = find(x>=-2 & x<-1);   s(J)   = (x(J)+2).^3;
                            ds(J)  = 3*(x(J)+2).^2;
  J = find(x>=-1 & x< 0);   s(J)   = -x(J).^3 - 2*(x(J)+1).^3+6*(x(J)+1);
                            ds(J)  = -3*x(J).^2 - 6*(x(J)+1).^2+6;
  J = find(x>= 0 & x< 1);   s(J)   = -2*(1-x(J)).^3 + x(J).^3 - 6*x(J) + 6;
                            ds(J)  = 6*(1-x(J)).^2 + 3*x(J).^2 - 6;
  J = find(x>= 1 & x< 2);   s(J)   = (2-x(J)).^3;
                            ds(J)  = -3*(2-x(J)).^2;
end;
% scale to make integral 1
s  = 2/(3*sigma)*s;
ds = 8/(3*sigma^2)*ds;
%==============================================================================


function [x,y,xy,xyp,ee] = q2p1grid(x,y,xy,mv,bound);
%q2p1grid   Q2-P1 element grid generator
%   [x,y,xy,xyp,ee] = q2p1grid(x,y,xy,mv,bound);
%   input
%          x          x coordinate vector
%          y          y coordinate vector
%          xy         nodal coordinate vector
%          mv         Q2 macroelement mapping matrix
%          bound      boundary vertex vector
%   output
%          xyp        centroid coordinate vector
%          ee         element edge connection matrix
%
%   IFISS function: DJS; 28 February 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
xx=xy(:,1); yy=xy(:,2); nvtx=length(xx);
nel=length(mv(:,1));
%% recompute mid-side points in the case of stretched grids
% y-direction
yv=yy; ny=length(y);
for k=2:2:ny;
yold=y(k); ynew=0.5*(y(k+1)+y(k-1));
l=find(yy==yold); yv(l)=ynew; y(k)=ynew;
end
% x-direction
xv=xx; nx=length(x);
for k=2:2:nx;
xold=x(k); xnew=0.5*(x(k+1)+x(k-1));
l=find(xx==xold); xv(l)=xnew; x(k)=xnew;
end
xy=[xv,yv];
% centroid coordinates
for ielem=1:nel
xc(ielem)=mean(xx(mv(ielem,1:4))); yc(ielem)=mean(yy(mv(ielem,1:4)));
end
xyp=[xc',yc'];
%
%% compute edge to edge connection array ee
      np=nel;
% initialise global matrices
      adj = sparse(nvtx,nvtx);
      ee = zeros(nel,4);
%
% evaluate element number on each edge in turn
% and assemble into adjacency matrix
%% nx= 0, ny=-1
		 adj=adj + sparse(mv(:,1),mv(:,2),1:np,nvtx,nvtx);
%% nx= 1, ny= 0
		 adj=adj + sparse(mv(:,2),mv(:,3),1:np,nvtx,nvtx);
%% nx= 0, ny= 1
		 adj=adj + sparse(mv(:,3),mv(:,4),1:np,nvtx,nvtx);
%% nx=-1, ny= 0
		 adj=adj + sparse(mv(:,4),mv(:,1),1:np,nvtx,nvtx);
%
       for el=1:nel
		   [ii,jj]=find(adj==el);
           ee(el,:)=diag(adj(jj,ii))';
	   end
       ee=ee(:,[2,4,3,1]);

%
% plotting of the grid
%if nel <=64,
adj=sparse(nvtx,nvtx);
mel=length(mv(:,1));
for i=1:nel
adj(mv(i,1),mv(i,2))=1;
adj(mv(i,2),mv(i,3))=1;
adj(mv(i,3),mv(i,4))=1;
adj(mv(i,4),mv(i,1))=1;
end
figure(30)
gplot(adj,xy,'b')
axis('square')
hold on
plot(xy(:,1),xy(:,2),'ro')
xybd=xy(bound,:);
plot(xybd(:,1),xybd(:,2),'ko')
plot(xyp(:,1),xyp(:,2),'k*',xyp(:,1),xyp(:,2),'ro')
hold off
title('Q2-P1 finite element subdivision')
drawnow
%end
return


function [c,relres,iter]=franaiter(F,f,varargin)
%FRANAITER  Iterative analysis
%   Usage:  c=franaiter(F,f);
%           [c,relres,iter]=franaiter(F,f,...);
%
%   Input parameters:
%         F       : Frame.
%         f       : Signal.
%         Ls      : Length of signal.
%   Output parameters:
%         c       : Array of coefficients.
%         relres  : Vector of residuals.
%         iter    : Number of iterations done.
%
%   `c=franaiter(F,f)` computes the frame coefficients *c* of the signal *f*
%   using an iterative method such that perfect reconstruction can be
%   obtained using |frsyn|. `franaiter` always works, even when |frana|
%   cannot generate perfect reconstruction coefficients.
%
%   `[c,relres,iter]=franaiter(...)` additionally returns the relative
%   residuals in a vector *relres* and the number of iteration steps *iter*.
%
%   **Note:** If it is possible to explicitly calculate the canonical dual
%   frame then this is usually a much faster method than invoking
%   `franaiter`.
%
%   `franaiter` takes the following parameters at the end of the line of
%   input arguments:
%
%     'tol',t      Stop if relative residual error is less than the
%                  specified tolerance. Default is 1e-9 (1e-5 for single precision)
%
%     'maxit',n    Do at most n iterations.
%
%     'pg'        Solve the problem using the Conjugate Gradient
%                  algorithm. This is the default.
%
%     'pcg'        Solve the problem using the Preconditioned Conjugate Gradient
%                  algorithm.
%
%     'print'      Display the progress.
%
%     'quiet'      Don't print anything, this is the default.
%
%   Examples
%   --------
%
%   The following example shows how to rectruct a signal without ever
%   using the dual frame:::
%
%      f=greasy;
%      F=frame('dgtreal','gauss',40,60);
%      [c,relres,iter]=franaiter(F,f,'tol',1e-14);
%      r=frsyn(F,c);
%      norm(f-r)/norm(f)
%      semilogy(relres);
%      title('Conversion rate of the CG algorithm');
%      xlabel('No. of iterations');
%      ylabel('Relative residual');
%
%   See also: frame, frana, frsyn, frsyniter

% AUTHORS: Peter L. Søndergaard

complainif_notenoughargs(nargin,2,'FRANAITER');
complainif_notvalidframeobj(F,'FRANAITER');

tolchooser.double=1e-9;
tolchooser.single=1e-5;

definput.keyvals.Ls=[];
definput.keyvals.tol=tolchooser.(class(f));
definput.keyvals.maxit=100;
definput.flags.alg={'cg','pcg'};
definput.keyvals.printstep=10;
definput.flags.print={'quiet','print'};

[flags,kv,Ls]=ltfatarghelper({'Ls'},definput,varargin);

%% ----- step 1 : Verify f and determine its length -------
% Change f to correct shape.
[f,~,Ls,W,dim,permutedsize,order]=assert_sigreshape_pre(f,[],[],upper(mfilename));

F=frameaccel(F,Ls);
L=F.L;

%% -- run the iteration

A=@(x) F.frsyn(F.frana(x));

% An explicit postpad is needed for the pcg algorithm to not fail
f=postpad(f,L);

if flags.do_pcg
    d=framediag(F,L);
    M=spdiags(d,0,L,L);

    [fout,flag,~,iter,relres]=pcg(A,f,kv.tol,kv.maxit,M);
else

    [fout,flag,~,iter,relres]=pcg(A,f,kv.tol,kv.maxit);
end;

c=F.frana(fout);

if nargout>1
    relres=relres/norm(fout(:));
end;

%% --- cleanup -----

permutedsize=[size(c,1),permutedsize(2:end)];

c=assert_sigreshape_post(c,dim,permutedsize,order);


function [flag] = point_visible(V,F,o,heuristic_ratio)
  % POINT_VISIBLE  test whether vertices of mesh are visible to a set of
  % points
  %
  % [flag] = point_visible(V,F,o)
  %
  % Input:
  %    V  #V by 3 list of vertex positions
  %    F  #F by 3 list of triangle indices
  %    o  row vector of position to test
  % Output:
  %    flag  #V by 1 list of bools (true) visible, (false) obstructed
  %

  dim = size(V,2);
  % Try to use accelerated bone_visible code
  if dim == 3 && 3==exist('bone_visible_embree','file')
    flag = bone_visible_embree(V,F,o,o);
  % otherwise try to use mex bone_visible code
  elseif dim ==3 && 3==exist('bone_visible','file')
    warning('Using non accelerated visibility test');
    flag = bone_visible(V,F,o,o);
  % otherwise use super slow matlab code
  else
    warning('Using non accelerated, pure matlab visibility test');

    assert(size(o,2) == dim);
    assert(size(F,2) == 3);
    % number of mesh vertices
    nv =  size(V,1);

    if dim == 2
      % get polygon edges of outline of 2D mesh
      O = outline(F);
    end

    if(exist('heuristic_ratio','var'))
      indices = randperm(nv);
      if(heuristic_ratio < 1)
        indices = indices(1:round(heuristic_ratio*nv));
      else
        indices = indices(1:round(heuristic_ratio));
      end
      flag = 0.5+eps + zeros(nv,1);
    else
      indices = 1:nv;
      flag = zeros(nv,1);
    end

    % loop over mesh vertices
    for jj_i = 1:numel(indices)
      progressbar(jj_i,numel(indices));
      jj = indices(jj_i);
      % query point, can o see q ?
      vjj = V(jj,:);

      dir = vjj-o;

      if dim == 3
        % extract triangles not containing jj
        Fmjj = F( all(F~=jj,2),:);
        %Fmjj = F;
        % shoot ray from o to q and see if any triangles not containing query get
        % hit
        %[hitc,tc] = mexray_mesh_intersect(o,dir,V,Fmjj);
        [hit,t] = ray_mesh_intersect(o,dir,V,Fmjj);
      elseif dim == 2
        Omjj = O( all(O~=jj,2),:);
        [hit,t] = ray_polygon_intersect(o,dir,V,Omjj);
      else
        error('Bad dimension');
      end

      %vjj-o
      %meshplot(V,F,'FC',repmat(hit,1,3))
      dir_mag =  sqrt(sum(dir.^2,2));
      flag(jj) = ~any(t(hit) < dir_mag);
    end

    if(exist('heuristic_ratio','var'))
      L = adjacency_matrix(F);
      L = L - diag(sum(L));
      i = 0;
      while(any(flag == (0.5+eps)))
        flag = flag+0.08*L*flag;
        flag(flag ~= (0.5+eps)) = round(flag(flag ~= (0.5+eps)));
        i = i +1;
      end
      i
      %L = cotmatrix(V,F);
      %I = speye(nv,nv);
      %L(indices,:) = I(indices,:);
      %flag = (L\flag)>0.5;
    end

  end
end


function [w,u,eqn,info] = biharmonicP2(node,elem,bdFlag,pde,option)
%%
%   [w,u] = biharmonicP1(node,elem,bdFlag,pde) produces the mixed cubic finite element
%   approximation of the biharmonic equation, where w = laplace u
%   See also biharmonicP1, biharmonicP2, biharmonicP3.
%
%   Created by Jie Zhou. Clean up is needed
%
%   Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if nargin<5, option = []; end
tic;
%% Construct Data Structure
[elem2dof,edge,bdDof] = dofP2(elem);
N = size(node,1);  NT = size(elem,1);  Ndof = N+size(edge,1);

%% Compute geometric quantities and gradient of local basis
[Dlambda,area] = gradbasis(node,elem);

%% Assemble stiffness matrix
% Since Dphi_i*Dphi_j is quadratic, numerical quadrature rule is used here
if ~isfield(option,'quadorder')
    option.quadorder = 4;   % default order
end
[lambda, weight] = quadpts(option.quadorder);
nQuad = size(lambda,1);
ii = zeros(21*NT,1); jj = zeros(21*NT,1); sA = zeros(21*NT,1);sB = zeros(21*NT,1);
index = 0;
for i = 1:6
    for j = i:6
        Bij = 0;
        Aij = 0;
        for p = 1:nQuad
                 Bij = Bij + weight(p)*dot(Dphi(p,i),Dphi(p,j),2);
                 Aij = Aij + weight(p)*dot(phi(p,i),phi(p,j),2);
        end
        Bij = Bij.*area;
        Aij = Aij.*area;

        ii(index+1:index+NT) = double(elem2dof(:,i));
        jj(index+1:index+NT) = double(elem2dof(:,j));
        sB(index+1:index+NT) = Bij;
        sA(index+1:index+NT) = Aij;
        index = index + NT;
    end
end
clear Aij Bij
diagIdx = (ii == jj);   upperIdx = ~diagIdx;
B = sparse(ii(diagIdx),jj(diagIdx),sB(diagIdx),Ndof,Ndof);
A = sparse(ii(diagIdx),jj(diagIdx),sA(diagIdx),Ndof,Ndof);
% A = spdiags(accumarray(ii(diagIdx),sA(diagIdx),[Ndof 1]),0,Ndof,Ndof);
BU = sparse(ii(upperIdx),jj(upperIdx),sB(upperIdx),Ndof,Ndof);
AU = sparse(ii(upperIdx),jj(upperIdx),sA(upperIdx),Ndof,Ndof);
B = B + BU + BU';
A = A + AU + AU';

%% boundary condition
%%
    fixedDof = [];
    isFixedDof = false(Ndof,1);
    if ~isempty(bdFlag)
        elem2edge = elem2dof(:,4:6)-N;
        isDirichlet(elem2edge(bdFlag(:)==1)) = true;
        isFixedDof(edge(isDirichlet,:)) = true;
        isFixedDof(N + find(isDirichlet')) = true;
        fixedDof = find(isFixedDof);
        freeDof = find(~isFixedDof);
    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunction Dphi
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function s = Dphi(p,i) % gradient of basis phi
    switch i
        case 1
            s = (4*lambda(p,1)-1).*Dlambda(:,:,1);
        case 2
            s = (4*lambda(p,2)-1).*Dlambda(:,:,2);
        case 3
            s = (4*lambda(p,3)-1).*Dlambda(:,:,3);
        case 4
            s = 4*(lambda(p,2)*Dlambda(:,:,3)+lambda(p,3)*Dlambda(:,:,2));
        case 5
            s = 4*(lambda(p,3)*Dlambda(:,:,1)+lambda(p,1)*Dlambda(:,:,3));
        case 6
            s = 4*(lambda(p,1)*Dlambda(:,:,2)+lambda(p,2)*Dlambda(:,:,1));
    end
    end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunction Dphi
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function s = phi(p,i) % gradient of basis phi
    switch i
        case 1
            s = (2*lambda(p,1)-1).*lambda(p,1);
        case 2
            s = (2*lambda(p,2)-1).*lambda(p,2);
        case 3
            s = (2*lambda(p,3)-1).*lambda(p,3);
        case 4
            s = 4*lambda(p,3).*lambda(p,2);
        case 5
            s = 4*lambda(p,1).*lambda(p,3);
        case 6
            s = 4*lambda(p,1).*lambda(p,2);
    end
    end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% Assemble right hand side by high order quadrature rule
% To reduce the effect of the error introduced by the numerical quadrature,
% the load term is computed using the 3rd order qudrature rule.
b = zeros(Ndof,1);
% u=zeros(Ndof,1);
w = zeros(Ndof,1);

if ~isfield(option,'fquadorder')
    option.fquadorder = 3;   % default order
end
if ~isfield(pde,'f') || (isreal(pde.f) && (pde.f==0))
    pde.f = [];
end
if ~isempty(pde.f)
    % quadrature points in the barycentric coordinate
    [lambda,w] = quadpts(option.fquadorder);
    nQuad = size(lambda,1);
%     phi(:,1) = lambda(:,1).*(2*lambda(:,1)-1);
%     phi(:,2) = lambda(:,2).*(2*lambda(:,2)-1);
%     phi(:,3) = lambda(:,3).*(2*lambda(:,3)-1);
%     phi(:,4) = 4*lambda(:,2).*lambda(:,3);
%     phi(:,5) = 4*lambda(:,3).*lambda(:,1);
%     phi(:,6) = 4*lambda(:,1).*lambda(:,2);
    bt = zeros(NT,6);
    for p = 1:nQuad
        % quadrature points in the x-y coordinate
        pxy = lambda(p,1)*node(elem(:,1),:) ...
            + lambda(p,2)*node(elem(:,2),:) ...
            + lambda(p,3)*node(elem(:,3),:);
        if isfield(pde,'f') && isnumeric(pde.f)
            fp = pde.f;        % piecewise constant
        else
            fp = pde.f(pxy);   % function handle
        end
        for j = 1:6
            bt(:,j) = bt(:,j) + w(p)*phi(p,j)*fp;
        end
    end
    bt = bt.*repmat(area,1,6);
    b = accumarray(elem2dof(:),bt(:),[Ndof 1]);
end

[b1,u] = getbdP2(b);
B(:,fixedDof)=[];
Nu=size(freeDof,1);

b(fixedDof)=[];

switch solver
    case 'direct'
        t = cputime;
        bigA = [A, B; ...
                B', sparse(Nu,Nu)];
        bigF = [b1; -b];
        bigu = bigA\bigF;
        w = bigu(1:Ndof);
        u(freeDof)=bigu(Ndof+1:Ndof+Nu);
        residual = norm(bigF-bigA*bigu);
        info = struct('solverTime',cputime - t,'itStep',0,'err',residual,'flag',2,'stopErr',residual);
    case 'none'
        info = struct('solverTime',[],'itStep',0,'err',[],'flag',3,'stopErr',[]);
end % end of four order.

%     soln = struct('u',u,'Du',Du);
    eqn = struct('A',A,'B',B','f',b1,'g',-b,'freeNode',freeNode,'Lap',A);
    info.assembleTime = assembleTime;

 function [b1,u] = getbdP2
    %% Boundary conditions for Poisson equation: P2 quadratic FEM.
    %
    % The set up of boundary condition consists of two parts:
    %

    %
    %  Modify the right hand side b. The Neumann boundary integral is added
    %  to b. For Dirichlet boundary ndoes, b(fixedDof) is the evaluation of
    % pde.g_D.
    %
    u = zeros(Ndof,1);

    %% Part 1: Find boundary edges and modify the load b
    % Neumann boundary condition
        if ~isfield(option,'gNquadorder')
            option.gNquadorder = 6;
        end
                idxN = (bdFlag(:) == 1);      % all Neumann edges in bdFlag
        Neumannidx = elem2edge(idxN ); % index of Neumann and Robin edges
        % since boundary integral is also needed for Robin edges
        Neumann   = edge(Neumannidx,:);
        b1 = zeros(Ndof,1);
        [lambdagN,weightgN] = quadpts1(option.gNquadorder);
        nQuadgN = size(lambdagN,1);
        % quadratic bases (1---3---2)
        bdphi = zeros(nQuadgN,3);
        bdphi(:,1) = (2*lambdagN(:,1)-1).*lambdagN(:,1);
        bdphi(:,2) = (2*lambdagN(:,2)-1).*lambdagN(:,2);
        bdphi(:,3) = 4*lambdagN(:,1).*lambdagN(:,2);
        % length of edge
        el = sqrt(sum((node(Neumann(:,1),:) - node(Neumann(:,2),:)).^2,2));
        ge = zeros(size(Neumann,1),3);
        for pp = 1:nQuadgN
            ppxy = lambdagN(pp,1)*node(Neumann(:,1),:) ...
                 + lambdagN(pp,2)*node(Neumann(:,2),:);
            gNu = pde.g_N(ppxy);
            ge(:,1) = ge(:,1) + weightgN(pp)*gNu*bdphi(pp,1);
            ge(:,2) = ge(:,2) + weightgN(pp)*gNu*bdphi(pp,2);
            ge(:,3) = ge(:,3) + weightgN(pp)*gNu*bdphi(pp,3); % interior bubble
        end
        % update RHS
        ge = ge.*repmat(el,1,3);
        b1(1:N) = accumarray(Neumann(:), [ge(:,1); ge(:,2)],[N,1]);
        b1(N+Neumannidx) = b1(N+Neumannidx) + ge(:,3);

    %% Part 2: Find Dirichlet boundary edges and compute the boundary value
    % Dirichlet boundary conditions

        isDirichlet(elem2edge(bdFlag(:)==1)) = true;
        % interpolation
                idx = (fixedDof > N);                         % index of edge nodes
        u(fixedDof(~idx)) = pde.g_D(node(fixedDof(~idx),:)); % bd value at vertex dofs
        bdEdgeIdx = fixedDof(idx) - N;
        bdEdgeMid = (node(edge(bdEdgeIdx,1),:) + node(edge(bdEdgeIdx,2),:))/2;
        u(fixedDof(idx)) = pde.g_D(bdEdgeMid);
        b1 = b1 - B*u;

    end % end of getbdP2

end                 % end of function biharmonicP2


% Fast Inter-Harmonic Reconstruction using the zero-phase version of the
% speech signal, as a pre-process for LPC-based spectral envelope estimation.
%
%
% Description
%  This technique reconstructs inter-harmonics of the voice signal,
% as a pre-process for an efficient spectral envelope estimation in
% high-pitched voices. The technique is fully described in [1].

%
% Inputs
%  wave            : [samples] [Nx1] input signal (speech signal)
%  Fs              : [Hz]      [1x1] sampling frequency
%  f0              : F0 estimate (a 0 value is for an unvoiced frame)
%  order           : Order used for the LP analysis
%
%
% Outputs
%  LP              : LP coefficients
%  Energy          : Energy of the frames
%
%
% Example
%  Please see the HOWTO_envelope.m example file.
%  Please see http://tcts.fpms.ac.be/~drugman/Toolbox/ for more details.
%
% References
%  [1] T.Drugman, Y. Stylianou, "Fast Inter-Harmonic Reconstruction for
%      Spectral Envelope Estimation in High-Pitched Voices", vol. 21, pp.
%      1418-1422, IEEE Signal Processing Letters, 2014.
%      http://tcts.fpms.ac.be/~drugman/files/SPL-FIHR.pdf
%
% Copyright (c) 2014 Toshiba Cambridge Research Laboratory
%
% License
%  This code will be part of the GLOAT toolbox with the following
%  licence:
%  This program is free software: you can redistribute it and/or modify
%  it under the terms of the GNU General Public License as published by
%  the Free Software Foundation, either version 3 of the License, or
%  (at your option) any later version.
%  This program is distributed in the hope that it will be useful,
%  but WITHOUT ANY WARRANTY; without even the implied warranty of
%  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%  GNU General Public License for more details.
%
% This function will also be part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Thomas Drugman thomas.drugman@umons.ac.be

function [LP, Energy] = env_fihrzp(Seg,Fs,F0_local,order)

F0_target=100;
% F0* in the paper. Here set to 100 Hz.

Energy=sum(Seg.^2);

if F0_local>0

    %Zero-phase version of the input signal
    Nfft=2048;
    ZP=real(ifft(abs(fft(Seg',Nfft))));
    ZP=[ZP(Nfft/2+1:end) ZP(1:Nfft/2)]';
    Seg=ZP(round(length(ZP)/2)-round(length(Seg)/2):round(length(ZP)/2)+round(length(Seg)/2)-1);

    % The rest is similar to the FIHR technique
    TmpSignal=ones(1,length(Seg));
    t_tmp=1:length(TmpSignal);

    I=ceil(F0_local/F0_target);
    for k=1:I-1
        W=(I-k)/I;
        % We use here a linear weighting function. Other functions are
        % possible, as long as they meet the properties mentioned in
        % the paper.
        TmpSignal=TmpSignal+2*W*cos(2*pi*(k/I)*F0_local/Fs*t_tmp);

        % Equation (2) in the paper
    end

    Seg2=Seg.*TmpSignal';
else
    Seg2=Seg;
end

LP=lpc(Seg2,order);


function pass = test_laplacian( pref )

% Grab some preferences
if ( nargin == 0 )
    pref = chebfunpref();
end
tol = 1e6*pref.techPrefs.chebfuneps;

% Test with different parity of m,p
% Example 1:
f = ballfun(@(x,y,z)x.^2+y.^2+z.^2);
g = laplacian(f);
exact = ballfun(@(x,y,z)6);
pass(1) = norm( g - exact ) < tol;

if (nargout > 0)
    pass = all(pass(:));
end
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Max manipulability index ALONG A LINE.
% Use stomp like to optimize along a surface/line
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function experiment2B
close all;
global robot
global parameters
global hfigures

%STOMP PARAMETERS
%conversion from cost to Prob factor
parameters.lambda = .4;
parameters.lambda_obstacles = .2;
%height of the obstacle
parameters.yo = 2.5;
%cost function starts at this distance
%must be below 0.3 for the 4 DOF robot
parameters.epsilon = 0.2;
%multiply noise by this facto
%parameters.noise_k = 5;
%parameters.noise_sigma_null_space = 0.01;
parameters.alpha=0.02;
parameters.time_step=0.01;

%number of waypoints
parameters.N = 12;
%number of particles
parameters.K = 20;
parameters.n_repeat = 30;
parameters.experiment_name = 'experiment2B_K20_N30.mat';

parameters.obstacles = [];

parameters.animate = 0;
close all
hfigures.hpaths = figure;
hfigures.hcosts = figure;
hfigures.hee = figure;
hfigures.htheta = figure;
hfigures.hbest_costs = figure;
hfigures.hdtheta = figure;

parameters.obstacles = [];
%LINE 1
x1 = -1.5;
y1 = .5; %m
x2 = 0;
y2 = 1.5; %m
phi = 3*pi/4;
p0 = [x1 y1 0]';
pf = [x2 y2 0]';
T0 = build_T_4dof(p0, phi);
parameters.obstacles{1}.line = [p0 pf];
parameters.obstacles{1}.T0 = T0;

%LINE 2
x1 = 0;
y1 = 1.5; %m
x2 = 1;
y2 = 1.5; %m
phi = pi/2;
p0 = [x1 y1 0]';
pf = [x2 y2 0]';
T0 = build_T_4dof(p0, phi);
parameters.obstacles{2}.line = [p0 pf];
parameters.obstacles{2}.T0 = T0;

%LINE 3
x1 = 1;
y1 = 1.5; %m
x2 = 2;
y2 = 0.5; %m
phi = pi/4;
p0 = [x1 y1 0]';
pf = [x2 y2 0]';
T0 = build_T_4dof(p0, phi);
parameters.obstacles{3}.line = [p0 pf];
parameters.obstacles{3}.T0 = T0;

%repeat the experiment E times
random_manips=[];
Gout = [];
for i=1:parameters.n_repeat
    close all
    [pk, final_manip] = stomp_null_space(robot);
    Gout{i}=pk;
    random_manips = [random_manips; final_manip];
    save(parameters.experiment_name)
end

function T = build_T_4dof(p, phi)
T = [cos(phi) -sin(phi) 0 p(1);
     sin(phi) cos(phi) 0 p(2);
     0            0     1  p(3);
     0             0    0   1];

 function T = build_T_sawyer(p, phi)
T = [1  -sin(phi) 0 p(1);
     0  cos(phi) 0 p(2);
     0            0     1  p(3);
     0             0    0   1];


% convert the trainable parameters in the graph into a vector such that we
% can call standard optimization packages to optimize the network
% parameters in batch mode.
%
function [cost, grad] = DNN_cost_wrapper(W, layer, data, para, mode)

% retrieve the weights from W and assign it to the correct layers
layer = NetWeights_vec2layer(W, layer, 0);

% compute the forward and backward passes and get the gradients
[cost_func, layer] = DNN_Cost10(layer, data, para, mode);
cost = cost_func.cost;

% retrieve the gradient from layer and store it into vector format
[grad] = NetWeights_layer2vec(layer, 1, para.useGPU);

cost = gather(cost);
grad = gather(grad);

end