function [label, centroid, dis] = fkmeans(X, k, options)
% FKMEANS Fast K-means with optional weighting and careful initialization.
% [L, C, D] = FKMEANS(X, k) partitions the vectors in the n-by-p matrix X
% into k (or, rarely, fewer) clusters by applying the well known batch
% K-means algorithm. Rows of X correspond to points, columns correspond to
% variables. The output k-by-p matrix C contains the cluster centroids. The
% n-element output column vector L contains the cluster label of each
% point. The k-element output column vector D contains the residual cluster
% distortions as measured by total squared distance of cluster members from
% the centroid.
%
% FKMEANS(X, C0) where C0 is a k-by-p matrix uses the rows of C0 as the
% initial centroids instead of choosing them randomly from X.
%
% FKMEANS(X, k, options) allows optional parameter name/value pairs to
% be specified. Parameters are:
%
%   'weight' - n-by-1 weight vector used to adjust centroid and distortion
%              calculations. Weights should be positive.
%   'careful' - binary option that determines whether "careful seeding"
%               as recommended by Arthur and Vassilvitskii is used when
%               choosing initial centroids. This option should be used
%               with care because numerical experiments suggest it may
%               be counter-productive when the data is noisy.
%
% Notes
% (1) The careful seeding procedure chooses the first centroid at random
% from X, and each successive centroid from the remaining points according
% to the categorical distribution with selection probabilities proportional
% to the point's minimum squared Euclidean distance from the already chosen
% centroids. This tends to spread the points out more evenly, and, if the
% data is made of k well separated clusters, is likely to choose an initial
% centroid from each cluster. This can speed convergence and reduce the
% likelihood of getting a bad solution [1]. However, in experiments where
% 5% uniformly distributed noise data was added to such naturally clustered
% data the results were frequently worse then when centroids were chosen at
% random.
% (2) If, as is possible, a cluster is empty at the end of an iteration,
% then there may be fewer than k clusters returned. In practice this seems
% to happen very rarely.
% (3) Unlike the Mathworks KMEANS this implementation does not perform a
% final, slow, phase of incremental K-means ('onlinephase') that guarantees
% convergence to a local minimum.
%
% References
% [1] "k-means++: The Advantages of Careful Seeding", by David Arthur and
% Sergei Vassilvitskii, SODA 2007.

n = size(X,1);

% option defaults
weight = 0; % uniform unit weighting
careful = 0;% random initialization

if nargin == 3
    if isfield(options, 'weight')
        weight = options.weight;
    end
    if isfield(options,'careful')
        careful = options.careful;
    end
end

% If initial centroids not supplied, choose them
if isscalar(k)
    % centroids not specified
    if careful
        k = spreadseeds(X, k);
    else
        k = X(randsample(size(X,1),k),:);
    end
end

% generate initial labeling of points
[~,label] = max(bsxfun(@minus,k*X',0.5*sum(k.^2,2)));
k = size(k,1);

last = 0;

if ~weight
    % code defactoring for speed
    while any(label ~= last)
        % remove empty clusters
        [~,~,label] = unique(label);
        % transform label into indicator matrix
        ind = sparse(label,1:n,1,k,n,n);
        % compute centroid of each cluster
        centroid = (spdiags(1./sum(ind,2),0,k,k)*ind)*X;
        % compute distance of every point to each centroid
        distances = bsxfun(@minus,centroid*X',0.5*sum(centroid.^2,2));
        % assign points to their nearest centroid
        last = label;
        [~,label] = max(distances);
    end
    dis = ind*(sum(X.^2,2) - 2*max(distances)');
else
    while any(label ~= last)
        % remove empty clusters
        [~,~,label] = unique(label);
        % transform label into indicator matrix
        ind = sparse(label,1:n,weight,k,n,n);
        % compute centroid of each cluster
        centroid = (spdiags(1./sum(ind,2),0,k,k)*ind)*X;
        % compute distance of every point to each centroid
        distances = bsxfun(@minus,centroid*X',0.5*sum(centroid.^2,2));
        % assign points to their nearest centroid
        last = label;
        [~,label] = max(distances);
    end
    dis = ind*(sum(X.^2,2) - 2*max(distances)');
end
label = label';

% Code below this line reused from the file exchange submission K-means++
% (http://www.mathworks.com/matlabcentral/fileexchange/28901-k-means) in
% accordance with the license:
% vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv
% Copyright (c) 2010, Michael Chen
% All rights reserved.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:
%
%     * Redistributions of source code must retain the above copyright
%       notice, this list of conditions and the following disclaimer.
%     * Redistributions in binary form must reproduce the above copyright
%       notice, this list of conditions and the following disclaimer in the
%       documentation and/or other materials provided with the distribution
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
% IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
% THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
% PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
% CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
% EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
% PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
% PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
% LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
% NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
% SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
% ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
%
function D = sqrdistance(A, B)
% Square Euclidean distances between all sample pairs
% A:  n1 x d data matrix
% B:  n2 x d data matrix
% WB: n2 x 1 weights for matrix B
% D: n2 x n1 pairwise square distance matrix
%    D(i,j) is the squared distance between A(i,:) and B(j,:)
% Written by Michael Chen (sth4nth@gmail.com). July 2009.
n1 = size(A,1); n2 = size(B,2);
m = (sum(A,1)+sum(B,1))/(n1+n2);
A = bsxfun(@minus,A,m);
B = bsxfun(@minus,B,m);
D = full((-2)*(A*B'));
D = bsxfun(@plus,D,full(sum(B.^2,2))');
D = bsxfun(@plus,D,full(sum(A.^2,2)))';
end

function [S, idx] = spreadseeds(X, k)
% X: n x d data matrix
% k: number of seeds
% reference: k-means++: the advantages of careful seeding.
% by David Arthur and Sergei Vassilvitskii
% Adapted from softseeds written by Mo Chen (mochen@ie.cuhk.edu.hk),
% March 2009.
[n,d] = size(X);
idx = zeros(k,1);
S = zeros(k,d);
D = inf(n,1);
idx(1) = ceil(n.*rand);
S(1,:) = X(idx(1),:);
for i = 2:k
    D = min(D,sqrdistance(S(i-1,:),X));
    idx(i) = find(cumsum(D)/sum(D)>rand,1);
    S(i,:) = X(idx(i),:);
end
end

end


function result = spsym (A, quick)					    %#ok
%SPSYM determine if a sparse matrix is symmetric, Hermitian, or skew-symmetric.
%   If so, also determine if its diagonal has all positive real entries.
%   A must be sparse.
%
%   Example:
%   result = spsym (A) ;
%   result = spsym (A,quick) ;
%
%   If quick = 0, or is not present, then this routine returns:
%
%       1: if A is rectangular
%       2: if A is unsymmetric
%       3: if A is symmetric, but with one or more A(j,j) <= 0
%       4: if A is Hermitian, but with one or more A(j,j) <= 0 or with
%           nonzero imaginary part
%       5: if A is skew symmetric (and thus the diagonal is all zero as well)
%       6: if A is symmetric with real positive diagonal
%       7: if A is Hermitian with real positive diagonal
%
%   If quick is nonzero, then the function can return more quickly, as soon as
%   it finds a diagonal entry that is <= 0 or with a nonzero imaginary part.
%   In this case, it returns 2 for a square matrix, even if the matrix might
%   otherwise be symmetric or Hermitian.
%
%   Regardless of the value of "quick", this function returns 6 or 7 if A is
%   a candidate for sparse Cholesky.
%
%   For an MATLAB M-file function that computes the same thing as this
%   mexFunction (but much slower), see the get_symmetry function by typing
%   "type spsym".
%
%   This spsym function does not compute the transpose of A, nor does it need
%   to examine the entire matrix if it is unsymmetric.  It uses very little
%   memory as well (just size-n workspace, where n = size (A,1)).
%
%   Examples:
%       load west0479
%       A = west0479 ;
%       spsym (A)
%       spsym (A+A')
%       spsym (A-A')
%       spsym (A+A'+3*speye(size(A,1)))
%
%   See also mldivide.

%       function result = get_symmetry (A,quick)
%       %GET_SYMMETRY: does the same thing as the spsym mexFunction.
%       % It's just a lot slower and uses much more memory.  This function
%       % is meant for testing and documentation only.
%       [m n] = size (A) ;
%       if (m ~= n)
%           result = 1 ;            % rectangular
%           return
%       end
%       if (nargin < 2)
%           quick = 0 ;
%       end
%       d = diag (A) ;
%       posdiag = all (real (d) > 0) & all (imag (d) == 0) ;
%       if (quick & ~posdiag)
%           result = 2 ;            % Not a candidate for sparse Cholesky.
%       elseif (~isreal (A) & nnz (A-A') == 0)
%           if (posdiag)
%               result = 7 ;        % complex Hermitian, with positive diagonal
%           else
%               result = 4 ;        % complex Hermitian, nonpositive diagonal
%           end
%       elseif (nnz (A-A.') == 0)
%           if (posdiag)
%               result = 6 ;        % symmetric with positive diagonal
%           else
%               result = 3 ;        % symmetric, nonpositive diagonal
%           end
%       elseif (nnz (A+A.') == 0)
%           result = 5 ;            % skew symmetric
%       else
%           result = 2 ;            % unsymmetric
%       end

% With additional outputs, spsym computes the following for square matrices:
% (in this case "quick" is ignored, and set to zero):
%
% [result xmatched pmatched nzoffdiag nnzdiag] = spsym(A)
%
%   xmatched is the number of nonzero entries for which A(i,j) = conj(A(j,i)).
%   pmatched is the number of entries (i,j) for which A(i,j) and A(j,i) are
%   both in the pattern of A (the value doesn't matter).  nzoffdiag is the
%   total number of off-diagonal entries in the pattern.  nzdiag is the number
%   of diagonal entries in the pattern.  If the matrix is rectangular,
%   xmatched, pmatched, nzoffdiag, and nzdiag are not computed (all of them are
%   returned as zero).  Note that a matched pair, A(i,j) and A(j,i) for i != j,
%   is counted twice (once per entry).

%   Copyright 2006-2007, Timothy A. Davis
%   http://www.cise.ufl.edu/research/sparse

error ('spsym mexFunction not found') ;


% Automatic flight control of F8 aircraft using model predictive control
% and various models

% MPC execution time for sine3 models
% 83.5243, 188.2009, 2.1348e+03
% 79.5731, 179.1829, 2.1387e+03

clear all, close all, clc

SystemModel = 'F8';

figpath = ['../FIGURES/',SystemModel,'/'];mkdir(figpath)
datapath = ['../DATA/',SystemModel,'/'];mkdir(datapath)
addpath('../utils');

%% Load Models
Nvar = 3;
InputSignalTypeModel = 'sine3';             % Use actuation to collect training data
InputSignalTypeModel_Validation = 'sine2';  % Use actuation to collect validation data to test for generalization

% DelayDMDc
ModelTypeDMDc = 'DMDc'; % Model: DelayDMDc , DMDc
load(fullfile(datapath,['EX_',SystemModel,'_SI_',ModelTypeDMDc,'_',InputSignalTypeModel,'.mat']))
Models.DelayDMDc = Model;

% SINDYc
load(fullfile(datapath,['EX_',SystemModel,'_SI_SINDYc','_',InputSignalTypeModel,'.mat']))
Models.SINDYc = Model;

% NARX
NARX_SUBSTRACT_MEAN = 0;
load(fullfile(datapath,['EX_',SystemModel,'_SI_NARX','_',InputSignalTypeModel,'.mat']))
Models.NARX = Model;

%% Get training data
ENSEMBLE_DATA = 0;
ONLY_TRAINING_LENGTH = 1;
InputSignalType = InputSignalTypeModel;
Ndelay = Models.DelayDMDc.Ndelay;
getTrainingData

%% Get validation data: Model comparison
InputSignalType = InputSignalTypeModel_Validation;
Ndelay = Models.DelayDMDc.Ndelay;
tspanV =[tspan(end):dt:150];

% Forcing
if strcmp(InputSignalTypeModel_Validation,'sine2')
    forcing = @(x,t) [(.05* (sin(0.7*t).*sin(.1*t).*sin(.2*t).*sin(.05*t)) )];
elseif strcmp(InputSignalTypeModel_Validation,'sine3')
    forcing = @(x,t) (.5*sin(5*t).*sin(.5*t)+0.1).^3;
end

% Reference
[tA,xA] = ode45(@(t,x)F8Sys(t,x,forcing(x,t)),tspanV,x(end,1:3),options);   % true model
u_valid = forcing(0,tA)';

% DelayDMDc / DMDc
if Ndelay == 1
    x0      = [x(end,1:Nvar)];
    Hunew   = [u(end),u_valid(1:end-1)];
    [xB,tB] = lsim(Models.DelayDMDc.sys,Hunew,tspanV,[x0-[Models.DelayDMDc.xmean]]');
elseif Ndelay > 1
    x0      = [x(end-Ndelay+1,1:Nvar),x(end,1:Nvar)];
    Hunew   = [ u(end-Ndelay+1:end),u_valid(1:end-Ndelay);
        u(end),u_valid(1:end-1)];
    [xB,tB] = lsim(Models.DelayDMDc.sys,Hunew,tspanV,[x0-[Models.DelayDMDc.xmean]]');
    xB = xB(:,Nvar+1:2*Nvar); xB = xB + repmat(xmean,[size(xB,1) 1]);
end
xB = xB + repmat(Models.DelayDMDc.xmean,[length(tB) 1]);

% SINDYc
[tC,xC]=ode45(@(t,x)sparseGalerkinControl(t,x,forcing(x,t),Models.SINDYc.Xi(:,1:Nvar),Models.SINDYc.polyorder,Models.SINDYc.usesine),tspanV,x(end,1:Nvar),options);  % approximate

% NARX
Udummy = [u(end),u_valid(1:end)];
Hdummy = zeros(Nvar,size(Udummy,2));
if NARX_SUBSTRACT_MEAN == 1
    NARX_xmean = Models.NARX.xmean;
else
    NARX_xmean = zeros(size(Models.NARX.xmean'));
end

Hdummy(:,1) = x(end,1:Nvar)'-NARX_xmean';
[Us,Ui,Si] = preparets(Models.NARX.net,con2seq(Udummy),{},con2seq(Hdummy));
xD = Models.NARX.net(Us,Ui,Si); % Predict on validation data
xD = cell2mat(xD)'; xD = [x0;xD(1:end-1,:)];
xD(2:end,:) = xD(2:end,:) + repmat(NARX_xmean,[size(xD,1)-1 1]);

%% Show results
clear ph
h = figure;
subplot(3,1,1), box on, hold on
plot([tB(1) tB(1)],[-100 100],'--k')
plot(t,x(:,1),'Color',[.4 .4 .4],'LineWidth',1.5);
plot(tA,xA(:,1),'k','LineWidth',1.5);
plot(tB,xB(:,1),'r-','LineWidth',1.5);
plot(tC,xC(:,1),'g--','LineWidth',1.5);
plot(tA(1:end),xD(:,1),'-.','Color',[0.7,0.7,1],'LineWidth',1.5);
grid on
ylim([-0.4 0.3])
ylabel('x_1','FontSize',13)
set(gca,'FontSize',13)

subplot(3,1,2), box on, hold on
plot([tB(1) tB(1)],[-100 100],'--k')
plot(t,x(:,2),'Color',[.4 .4 .4],'LineWidth',1.5);
plot(tA,xA(:,2),'k','LineWidth',1.5);
plot(tB,xB(:,2),'r-','LineWidth',1.5);
plot(tC,xC(:,2),'g--','LineWidth',1.5);
plot(tA(1:end),xD(:,2),'-.','Color',[0.7,0.7,1],'LineWidth',1.5);
ylim([-4 0.1])
set(gca,'FontSize',13)
ylabel('x_2','FontSize',13)

subplot(3,1,3), box on, hold on
plot([tB(1) tB(1)],[-100 100],'--k')
ph(1) = plot(t,x(:,3),'Color',[.4 .4 .4],'LineWidth',1.5);
ph(2) = plot(tA,xA(:,3),'k','LineWidth',1.5);
ph(3) = plot(tB,xB(:,3),'r-','LineWidth',1.5);
ph(4) = plot(tC,xC(:,3),'g--','LineWidth',1.5);
ph(5) = plot(tA(1:end),xD(:,3),'-.','Color',[0.7,0.7,1],'LineWidth',1.5);
l1=legend(ph,'Training','Validation',ModelTypeDMDc,'SINDYc','NARX');
set(l1,'Location','NorthWest')
grid on
ylim([-0.9 0.5])
ylabel('x_3','FontSize',13)
set(gca,'FontSize',13)
xlabel('Time','FontSize',13)

set(h,'Units','Inches');
set(gcf,'Position',[1 1 6. 5.5])
pos = get(h,'Position');
set(h,'PaperPositionMode','Auto','PaperSize',[pos(3), pos(4)])
print(h,'-painters','-depsc2',  '-loose','-cmyk', [figpath,'EX_',SystemModel,'_SI_Comparison_Validation_',InputSignalTypeModel,'_',InputSignalType,'.eps'],'-r0');

%% Actuation signal
t_valid = tspanV;
clear ph
figure,box on, hold on,
ccolors = get(gca,'colororder');
plot([tA(1),tA(1)],[-15 260],':','Color',[0.4,0.4,0.4],'LineWidth',1.5)
plot(t,u,'-k','LineWidth',1);
plot(t_valid,u_valid,'-k','LineWidth',1);
grid off
ylim([min([u u_valid])+0.05*min([u u_valid]) max([u u_valid])+0.05*max([u u_valid])])
xlim([0 t_valid(end)])
xlabel('Time')
ylabel('Input')
set(gca,'LineWidth',1, 'FontSize',14)
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto')
print('-depsc2', '-loose','-cmyk', [figpath,'EX_',SystemModel,'_SI_Comparison_Validation_',InputSignalTypeModel,'_',InputSignalType,'_Actuation.eps']);

%% True SINDYc model parameters
Xi0 = zeros(size(Models.SINDYc.Xi));
Xi0([2,4,5,6,8,10,16,18,19,25,35],1) = [-0.877,1,-0.215,0.47,-0.088,-0.019,3.846,-1,0.28,0.47,0.63];
Xi0([4],2) = 1;
Xi0([2,4,5,6,16,19,25,35],3) = [-4.208,-0.396,-20.967,-0.47,-3.564,6.265,46,61.4];

%% MPC
% Parameters MPC
options = optimoptions('fmincon','Algorithm','sqp','Display','none', ...
    'MaxIterations',100);
Duration = 6;                   % Run for 'Duration' time units
Ton = 0;                        % Time units when control is turned on
getMPCparams

x0n=[0.1 0 0]'; %xA(end,:)';    % Initial condition
Tcontrol = tA(end);             % Time offset to combine training, prediction and control phase

% Parameters Models
ModelCollection = {ModelTypeDMDc, 'SINDYc', 'NARX'}; % 51.8924, 46.0083, 1.4662e+03 execution times
Nmodels = length(ModelCollection);

clear Results
Results(1:Nmodels) = struct('x',[], 'u', [], 't', [], 'xref', [], 'J', [], 'elapsed_time', []);

Ts = Models.SINDYc.dt;

%%
for iM = 1:Nmodels
    select_model = ModelCollection{iM}; % DelayDMDc; DMDc; NARX ; SINDYc

    % Prepare variables
    Nt = (Duration/Ts)+1;
    uopt0    = 0;
    xhat     = x0n;
    uopt     = uopt0.*ones(Nu,1);
    xHistory = zeros(Nvar,Nt); xHistory(:,1) = xhat;
    uHistory = zeros(1,Nt); uHistory(1)   = uopt(1);
    tHistory = zeros(1,Nt); tHistory(1)   = Tcontrol;
    rHistory = zeros(1,Nt);

    % Parameters Model
    switch select_model
        case 'DelayDMDc'
            pest.dt     = Models.DelayDMDc.dt;
            pest.sys    = Models.DelayDMDc.sys;
            pest.xmean  = xmean';
            pest.udelay = zeros(1,N);
            pest.xdelay = [x(end-Ndelay+1,1:2)]';
            pest.Nxlim  = size(x,1);
        case 'DMDc'
            pest.dt = Models.DelayDMDc.dt;
            pest.sys = Models.DelayDMDc.sys;
            pest.xmean = Models.DelayDMDc.xmean';
            pest.Nxlim = size(x,1);
        case 'SINDYc'
            pest.ahat = Models.SINDYc.Xi(:,1:Nvar); % Replace with Xi0 to test true model
            pest.polyorder = Models.SINDYc.polyorder;
            pest.usesine = Models.SINDYc.usesine;
            pest.dt = Models.SINDYc.dt;
        case 'NARX'
            pest.net = Models.NARX.net;
            pest.Ndelay = 1;
            pest.udelay = zeros(1,Ndelay);
            pest.xdelay = zeros(2,length(pest.udelay));
            pest.xdelay(:,1:Ndelay) = [x(end-Ndelay+1:end,1:2)]';
            pest.Nxlim = size(x,1);
    end

    % Start simulation
    fprintf('Simulation started.  It might take a while...\n')
    tic
    for ct = 1:(Duration/Ts)

        % Set references over prediction horizon
        tref = (ct:ct+N-1).*Ts;
        xref = [xrefFUN(tref); zeros(1,N); zeros(1,N)];

        % NMPC with full-state feedback
        COSTFUN = @(u) ObjectiveFCN_models(u,xhat,N,Nu,xref,uHistory(:,ct),pest,diag(Q),R,Ru,select_model);
        CONSFUN = @(u) ConstraintFCN_models(u,uHistory(:,ct),xhat,N,LBo,UBo,LBdu,UBdu,pest,select_model);
        uopt = fmincon(COSTFUN,uopt,[],[],[],[],LB,UB,CONSFUN,options);

        % Run without constraints
%         uopt = fmincon(COSTFUN,uopt,[],[],[],[],[],[],[],options);

        % Integrate system
        xhat = rk4u(@F8Sys,xhat,uopt(1),Ts/10,10,[],0);
        xHistory(:,ct+1) = xhat;
        uHistory(:,ct+1) = uopt(1);
        tHistory(:,ct+1) = ct*Ts+Tcontrol;
        rHistory(:,ct+1) = xref(1,2);

    end
    tElapsed = toc
    fprintf('Simulation finished!\n')

    % Collect results
    Results(iM).eval_time = tElapsed;
    Results(iM).xref = rHistory;
    Results(iM).x = xHistory;
    Results(iM).u = uHistory;
    Results(iM).t = tHistory;
    Results(iM).J = evalObjectiveFCN(Results(iM).u,Results(iM).x,Results(iM).xref,diag(Q),R,Ru);
    Results(iM).elapsed_time = tElapsed;

    VIZ_SI_Validation_MPC

    if iM==Nmodels % Plot ensemble training for NN / NARX
       VIZ_SI_Validation_MPC_ensemble(ct,t_valid,u_valid,xHistory,tHistory,uHistory,Results,t,tA,xA,xB,xC,xD,select_model,SystemModel,figpath,N,InputSignalTypeModel,Nmodels,ModelCollection)
    end

end

% MPC Execution times using N=10: 120.9128, 96.8967, 2.1755e+03

%% Unforced
[tU,xU] = ode45(@(t,x)F8Sys(t,x,0,[]),tHistory,xA(end,1:3),options);   % true model

%% Show cost
clear ph
figure, hold on, box on
ph(1) = plot(Results(1).t,cumsum(Results(1).J),'-r','LineWidth',2);
ph(3) = plot(Results(1).t,cumsum(Results(3).J),'-.','Color',[0.7,0.7,1],'LineWidth',2);
ph(2) = plot(Results(1).t,cumsum(Results(2).J),'--g','LineWidth',2);

xlim([Results(1).t(1) Results(1).t(end)])
ylabel('Cost','FontSize',14)
xlabel('Time','FontSize',14)
set(gca,'LineWidth',1, 'FontSize',14)
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto')

print('-depsc2', '-loose','-cmyk', [figpath,'EX_',SystemModel,'_SI_Comparison_Validation_',InputSignalTypeModel,'_',InputSignalType,'_Cost','_N_',num2str(N),'.eps']);

%% Show control stage separate
clear ph
figure, hold on, box on
ph(4) = plot(Results(1).t(2:end),(Results(1).xref(2:end)),'-k','LineWidth',2);
ph(1) = plot(Results(1).t,(Results(1).x(1,:)),'-r','LineWidth',2);
ph(3) = plot(Results(1).t,(Results(3).x(1,:)),'-.','Color',[0.7,0.7,1],'LineWidth',2);
ph(2) = plot(Results(1).t,(Results(2).x(1,:)),'--g','LineWidth',2);

l1=legend(ph,ModelCollection,'Ref');
set(l1,'Location','NorthEast')
xlim([Results(1).t(1) Results(1).t(end)])
ylim([-0.25 .25])
ylabel('AoA','FontSize',14)
xlabel('Time','FontSize',14)
set(gca,'LineWidth',1, 'FontSize',14)
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto')

print('-depsc2', '-loose','-cmyk', [figpath,'EX_',SystemModel,'_SI_Comparison_',InputSignalTypeModel,'_',InputSignalType,'_AoA','_N_',num2str(N),'.eps']);

clear ph
figure, hold on, box on
ph(1) = plot(Results(1).t,(Results(1).u(:)),'-r','LineWidth',2);
ph(3) = plot(Results(1).t,(Results(3).u(:)),'-.','Color',[0.7,0.7,1],'LineWidth',2);
ph(2) = plot(Results(1).t,(Results(2).u(:)),'--g','LineWidth',2);

xlim([Results(1).t(1) Results(1).t(end)])
ylabel('Input','FontSize',14)
xlabel('Time','FontSize',14)
set(gca,'LineWidth',1, 'FontSize',14)
set(gcf,'Position',[100 100 300 200])
set(gcf,'PaperPositionMode','auto')

print('-depsc2', '-loose','-cmyk', [figpath,'EX_',SystemModel,'_SI_Comparison_',InputSignalTypeModel,'_',InputSignalType,'_Input','_N_',num2str(N),'.eps']);

%% Save results
save(fullfile(datapath,['MPC_',SystemModel,'_Results_AllModels_',InputSignalTypeModel,'_',InputSignalType,'_N_',num2str(N),'.mat']),'Results')
save(fullfile(datapath,['MPC_',SystemModel,'_Results_AllModels_',InputSignalTypeModel,'_',InputSignalType,'_All','_N_',num2str(N),'.mat']))


function Ne = som_neighborhood(Ne1,n)

%SOM_NEIGHBORHOOD Calculate neighborhood matrix.
%
% Ne = som_neighborhood(Ne1,n)
%
%  Ne = som_neighborhood(Ne1);
%  Ne = som_neighborhood(som_unit_neighs(topol),2);
%
%  Input and output arguments ([]'s are optional):
%   Ne1       (matrix, size [munits m]) a sparse matrix indicating
%                      the units in 1-neighborhood for each map unit
%   [n]       (scalar) maximum neighborhood which is calculated, default=Inf
%
%   Ne        (matrix, size [munits munits]) neighborhood matrix,
%                      each row (and column) contains neighborhood
%                      values from the specific map unit to all other
%                      map units, or Inf if the value is unknown.
%
% For more help, try 'type som_neighborhood' or check out online documentation.
% See also SOM_UNIT_NEIGHS, SOM_UNIT_DISTS, SOM_UNIT_COORDS, SOM_CONNECTION.

%%%%%%%%%%%%% DETAILED DESCRIPTION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% som_neighborhood
%
% PURPOSE
%
% Calculate to which neighborhood each map unit belongs to relative to
% each other map unit, given the units in 1-neighborhood of each unit.
%
% SYNTAX
%
%  Ne = som_neighborhood(Ne1);
%  Ne = som_neighborhood(Ne1,n);
%
% DESCRIPTION
%
% For each map unit, finds the minimum neighborhood to which it belongs
% to relative to each other map unit. Or, equivalently, for each map
% unit, finds which units form its k-neighborhood, where k goes from
% 0 to n.
%
% The neighborhood is calculated iteratively using the reflexivity of
% neighborhood.
%     let  N1i  be the 1-neighborhood set a unit i
% and let  N11i be the set of units in the 1-neighborhood of any unit j in N1i
%     then N2i  (the 2-neighborhood set of unit i) is N11i \ N1i
%
% Consider, for example, the case of a 5x5 map. The neighborhood in case of
% 'rect' and 'hexa' lattices (and 'sheet' shape) for the unit at the
% center of the map are depicted below:
%
%   'rect' lattice           'hexa' lattice
%   --------------           --------------
%   4  3  2  3  4            3  2  2  2  3
%   3  2  1  2  3             2  1  1  2  3
%   2  1  0  1  2            2  1  0  1  2
%   3  2  1  2  3             2  1  1  2  3
%   4  3  2  3  4            3  2  2  2  3
%
% Because the iterative procedure is rather slow, the neighborhoods
% are calculated upto given maximal value. The uncalculated values
% in the returned matrix are Inf:s.
%
% REQUIRED INPUT ARGUMENTS
%
%  Ne1   (matrix) Each row contains 1, if the corresponding unit is adjacent
%                 for that map unit, 0 otherwise. This can be calculated
%                 using SOM_UNIT_NEIGHS. The matrix can be sparse.
%                 Size munits x munits.
%
% OPTIONAL INPUT ARGUMENTS
%
%  n     (scalar) Maximal neighborhood value which is calculated,
%                 Inf by default (all neighborhoods).
%
% OUTPUT ARGUMENTS
%
%  Ne    (matrix) neighborhood values for each map unit, size is
%                 [munits, munits]. The matrix contains the minimum
%                 neighborhood of unit i, to which unit j belongs,
%                 or Inf, if the neighborhood was bigger than n.
%
% EXAMPLES
%
%  Ne = som_neighborhood(Ne1,1);    % upto 1-neighborhood
%  Ne = som_neighborhood(Ne1,Inf);  % all neighborhoods
%  Ne = som_neighborhood(som_unit_neighs(topol),4);
%
% SEE ALSO
%
%  som_unit_neighs   Calculate units in 1-neighborhood for each map unit.
%  som_unit_coords   Calculate grid coordinates.
%  som_unit_dists    Calculate interunit distances.
%  som_connection    Connection matrix.

% Copyright (c) 1999-2000 by the SOM toolbox programming team.
% http://www.cis.hut.fi/projects/somtoolbox/

% Version 1.0beta juuso 141097
% Version 2.0beta juuso 101199

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Check arguments

error(nargchk(1, 2, nargin));

if nargin<2, n=Inf; end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Action

% initialize
if issparse(Ne1), Ne = full(Ne1); else Ne = Ne1; end
clear Ne1
[munits dummy] = size(Ne);
Ne(find(Ne==0)) = NaN;
for i=1:munits, Ne(i,i)=0; end

% Calculate neighborhood distance for each unit using reflexsivity
% of neighborhood:
%   let  N1i be the 1-neighborhood set a unit i
%   then N2i is the union of all map units, belonging to the
%        1-neighborhood of any unit j in N1i, not already in N1i
k=1;
if n>1,
  fprintf(1,'Calculating neighborhood: 1 ');
  N1 = Ne;
  N1(find(N1~=1)) = 0;
end
while k<n && any(isnan(Ne(:))),
  k=k+1;
  fprintf(1,'%d ',k);
  for i=1:munits,
    candidates = isnan(Ne(i,:));              % units not in any neighborhood yet
    if any(candidates),
      prevneigh = find(Ne(i,:)==k-1);         % neighborhood (k-1)
      N1_of_prevneigh = any(N1(prevneigh,:)); % union of their N1:s
      Nn = find(N1_of_prevneigh & candidates);
      if length(Nn), Ne(i,Nn) = k; Ne(Nn,i) = k; end
    end
  end
end
if n>1, fprintf(1,'\n'); end

% finally replace all uncalculated distance values with Inf
Ne(find(isnan(Ne))) = Inf;

return;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% faster version?

l = size(Ne1,1); Ne1([0:l-1]*(l+1)+1) = 1; Ne = full(Ne1); M0 = Ne1; k = 2;
while any(Ne(:)==0), M1=(M0*Ne1>0); Ne(find(M1-M0))=k; M0=M1; k=k+1; end
Ne([0:l-1]*(l+1)+1) = 0;


%% Copyright (C) 2016-2017 Lagu
%% Copyright (C) 2017, 2019, 2022 Colin B. Macdonald
%%
%% This file is part of OctSymPy.
%%
%% OctSymPy 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 software 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 software; see the file COPYING.
%% If not, see <http://www.gnu.org/licenses/>.

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defmethod @@sym ellipticK (@var{m})
%% Complete elliptic integral of the first kind.
%%
%% The complete elliptic integral of the first kind
%% with parameter @var{m} is defined by:
%% @example
%% @group
%% syms m
%% ellipticK (m)
%%   @result{} ans = (sym) K(m)
%% @end group
%%
%% @group
%% rewrite (ans, 'Integral')         % doctest: +SKIP
%%   @result{} ans = (sym)
%%       π
%%       ─
%%       2
%%       ⌠
%%       ⎮         1
%%       ⎮ ────────────────── dα
%%       ⎮    _______________
%%       ⎮   ╱          2
%%       ⎮ ╲╱  1 - m⋅sin (α)
%%       ⌡
%%       0
%% @end group
%% @end example
%%
%% Examples:
%% @example
%% @group
%% diff (ellipticK (m), m)
%%   @result{} (sym)
%%       -(1 - m)⋅K(m) + E(m)
%%       ────────────────────
%%           2⋅m⋅(1 - m)
%% @end group
%%
%% @group
%% vpa (ellipticK (sym (pi)/4))
%%   @result{} (sym) 2.2252536839853959577044373301346
%% @end group
%% @end example
%%
%% There are other conventions for the inputs of elliptic integrals,
%% @pxref{@@sym/ellipticF}.
%%
%% @seealso{@@sym/ellipke, @@sym/ellipticF, @@sym/ellipticE, @@sym/ellipticPi}
%% @end defmethod

function y = ellipticK (m)
  if (nargin > 1)
    print_usage ();
  end

  % y = ellipticF (sym (pi)/2, m);
  y = elementwise_op ('elliptic_k', m);

end

%!error ellipticK (sym(1), 2)

%!assert (isequal (ellipticK (sym (0)), sym (pi)/2))
%!assert (isequal (ellipticK (sym (-inf)), sym (0)))

%!assert (double (ellipticK (sym (1)/2)), 1.854074677, 10e-10)
%!assert (double (ellipticK (sym (pi)/4)), 2.225253684, 10e-10)
%!assert (double (ellipticK (sym (-55)/10)), 0.9324665884, 10e-11)

%!test
%! % compare to double ellipke
%! m = 1/5;
%! ms = sym(1)/5;
%! [K, E] = ellipke (m);
%! assert (double (ellipticK (ms)), K, -1e-15)
%! assert (double (ellipticE (ms)), E, -1e-15)

%!test
%! % compare to double ellipke
%! if (exist ('OCTAVE_VERSION', 'builtin'))
%! m = -10.3;
%! ms = -sym(103)/10;
%! [K, E] = ellipke (m);
%! assert (double (ellipticK (ms)), K, -1e-15)
%! assert (double (ellipticE (ms)), E, -1e-15)
%! end

%!test
%! % compare to Maple
%! us = vpa (ellipticK (sym (7)), 40);
%! % > evalf(EllipticK(sqrt(7)), 40);
%! maple = vpa ('0.6168027921799632674669917683443602673441', 40) - ...
%!         vpa ('0.9114898734184488922164103102629560336918j', 40);
%! assert (abs (double (maple - us)), 0, 1e-39)


function out=errorplot(x,y,dy,plottypes,dx);
%
%  2 dimensional plot with error bars
%
%	By: P.S.Basran		6/17/03
%
%       Toronto-Sunnybrook Cancer Centre
%       Dept. Medical Physics
%       Parminder.Basran@sw.ca
%
%
%
%	Function:
%		This function produces a two dimensional plot with error bars.
%
%	Syntax:
% 		out=errorplot(x,y,dy,plottypes,dx)
%
%	Inputs:
%		    x - is a [1 x n] dimensional vector
%           y - is a [m x n] dimensional matrix for multidimensional plots
%          dy - is a [m x n] dimensional matrix whose dimensions must match those of y
%   plottypes - is a [1 x 4] dimensional vector whose entries specify the plottype in the
%               same fashion as the variable 's' in the function plot(x,y,s).
%               NOTE: current version of this requires the plottype to be specified in
%               all instances, and to be 4 characters in length.
%          dx - is a real number that specifices the dimensions of the horizontal tics
%               for each error bar. The default value is 0.25 the dimension of x.
%
%	Outputs:
%		   - plot with legend, labeled '1', '2', etc.
%
%   Example:
%
%           %For a single plot:
%
%           x=[-10:1:10];
%           y1=x.^2;
%           dy1=0.3*y1.*rand(size(x)); %
%           plottype1=[':   '];
%           errorplot(x,y1,dy1,plottype1);
%
%           % For multiple plots:
%
%           y2=0.5*x.^2;
%           dy2=0.2*y2.*rand(size(x)); %
%           plottype2=['o-- '];
%           y=[y1; y2];
%           dy=[dy1; dy2];
%           errorplot(x,y,dy,[plottype1 plottype2],1);
%

%   Modification Log:
%       June 17 - 2003: alpha version
%       March 11 - 2004: fix for the plottypes ....thanks to Sean Verret
%

error(nargchk(2,5,nargin));

if nargin <5, dx=(x(2)-x(1))/4;
    if nargin < 4, plottypes='';
        if nargin < 3, dy='';
            [a,b]=size(y);
            for i=1:a, plot(x,y(i,:)); end % default to a regular plot for 2 inputs
        end, end, end

%Provide plot and legend
[a,b]=size(y);
if a>b,
    y=y';
    [a,b]=size(y);
end

tx=[];
for i=1:a,
    plot(x,y(i,:),plottypes(4*(i-1)+1:4*i));;
    tx=[tx; 'plot ' num2str(i)];
    hold on;
end
legend(tx);

%Now the error bars
for i=1:a,
    for j=1:b,
        %horizontal lines
        line([x(j) x(j)], [y(i,j)-dy(i,j) y(i,j)+dy(i,j)]);
        %top vertical line
        line([x(j)-dx x(j)+dx], [y(i,j)+dy(i,j) y(i,j)+dy(i,j)]);
        %bottom vertical line
        line([x(j)-dx x(j)+dx], [y(i,j)-dy(i,j) y(i,j)-dy(i,j)]);
     end
end

hold off


%compute sumspinerelangle

function [data,units]=compute_sumspinerelangle(trx,n)

larvae=trx.exp2flies{n};
numlarvae=numel(larvae);
spineangles=cell(1,numlarvae);
spinerelangle=cell(1,numlarvae);
sumspinerelangle=cell(1,numlarvae);
for i=1:numlarvae
    larva=larvae(i);
    spineangles{1,i}=bsxfun(@atan2,trx(larva).yspine_mm(1:end-1,:)-trx(larva).yspine_mm(2:end,:),trx(larva).xspine_mm(1:end-1,:)-trx(larva).xspine_mm(2:end,:));
    % spinerelangle{1,i}=bsxfun(@atan2,sin(spineangles{1,i}(2:end,:))-sin(spineangles{1,i}(1:end-1,:)),cos(spineangles{1,i}(2:end,:))-cos(spineangles{1,i}(1:end-1,:)));
    % KB: this method of measuring the difference between angles didn't
    % make sense to me.
    spinerelangle{1,i} = modrange(-diff(spineangles{i},1,1),-pi,pi);
    sumspinerelangle{1,i}=sum(spinerelangle{1,i},1);
end

units=parseunits('rad');
data=sumspinerelangle;


function [x, infos] = div_mu_partial_nmf(V, rank, in_options)
%
% This file is part of NMFLibrary
%
% Created by H.Kasai on Feb. 16, 2017
%
% Change log:
%
%       June 16, 2022 (Hiroyuki Kasai): Initial version.
%
%       Jul. 12, 2022 (Hiroyuki Kasai): Modified code structures.
%

    % set dimensions and samples
    [m, n] = size(V);

    % set local options
    local_options = [];
    local_options.alg           = 'mu';
    local_options.norm_h        = 0;
    local_options.norm_w        = 1;
    local_options.alpha         = 2;
    local_options.delta         = 0.1;
    local_options.metric_type   = 'kl-div'; % 'kl-div' (default)
    local_options.d_alpha       = -1; % for alpha divergence
    local_options.d_beta        = 0; % for beta divergence
    local_options.myeps         = 1e-16;

    % check input options
    if ~exist('in_options', 'var') || isempty(in_options)
        in_options = struct();
    end
    % merge options
    options = mergeOptions(get_nmf_default_options(), local_options);
    options = mergeOptions(options, in_options);

    if ~strcmp(options.alg, 'mu')
        fprintf('Invalid algorithm: %s. Therfore, we use mu (i.e., multiplicative update).\n', options.alg);
        options.alg = 'mu';
    end

    % initialize factors
    init_options = options;
    [init_factors, ~] = generate_init_factors(V, rank, init_options);
    W = init_factors.W;
    H = init_factors.H;

    % initialize
    method_name = sprintf('MU-Partial (%s:%s)', options.alg, options.metric);
    epoch = 0;
    grad_calc_count = 0;

    if options.verbose > 0
        fprintf('# %s: started ...\n', method_name);
    end

    % store initial info
    clear infos;
%     if strcmp(options.metric, 'alpha-div')
%         metric_param = options.d_alpha;
%     elseif strcmp(options.metric, 'beta-div')
%         metric_param = options.d_beta;
%     end
    [infos, f_val, optgap] = store_nmf_info(V, W, H, [], options, [], epoch, grad_calc_count, 0);

    if options.verbose > 1
        fprintf('MU-Partial (%s:%s): Epoch = 0000, cost = %.16e, optgap = %.4e\n', options.alg, options.metric, f_val, optgap);
    end

    % set start time
    start_time = tic();

    % main loop
    while true

        % check stop condition
        [stop_flag, reason, max_reached_flag] = check_stop_condition(epoch, infos, options);
        if stop_flag
            display_stop_reason(epoch, infos, options, method_name, reason, max_reached_flag);
            break;
        end

        if strcmp(options.alg, 'mu')
            if strcmp(options.metric, 'euc')

                % update H
                H = H .* (W' * V) ./ (W' * W * H);
                H = H + (H<options.myeps) .* options.myeps;

                % update W
                W(:, options.updateW) = W(:, options.updateW) .* (V * H(options.updateW, :)') ...
                    ./ (W * (H * H(options.updateW, :)'));
                W = W + (W<options.myeps) .* options.myeps;

            elseif strcmp(options.metric, 'kl-div')

                % update W
                W(:, options.updateW) = W(:, options.updateW) .* ...
                    ((V./(W*H + options.myeps))*H(options.updateW, :)')./(ones(m,1)*sum(H(options.updateW, :)'));
                if options.norm_w ~= 0
                    W = normalize_W(W, options.norm_w);
                end

                % update H
                H = H .* (W'*(V./(W*H + options.myeps)))./(sum(W)'*ones(1,n));
                if options.norm_h ~= 0
                    H = normalize_H(H, options.norm_h);
                end

            elseif strcmp(options.metric, 'alpha-div')

                % update W
                W(:, options.updateW) = W(:, options.updateW) .* ...
                    ( ((V+options.myeps) ./ (W*H+options.myeps)).^options.d_alpha * H(options.updateW, :)').^(1/options.d_alpha);
                if options.norm_w ~= 0
                    W = normalize_W(W, options.norm_w);
                end
                W = max(W, options.myeps);

                % update H
                H = H .* ( (W'*((V+options.myeps)./(W*H+options.myeps)).^options.d_alpha) ).^(1/options.d_alpha);
                if options.norm_h ~= 0
                    H = normalize_H(H, options.norm_h);
                end
                H = max(H, options.myeps);

            elseif strcmp(options.metric, 'beta-div')

                WH = W * H;

                % update W
                W(:, options.updateW) = W(:, options.updateW) .* ...
                    ( ((WH.^(options.d_beta-2) .* V)*H(options.updateW, :)') ./ ...
                    max(WH.^(options.d_beta-1)*H(options.updateW, :)', options.myeps) );

                if options.norm_w ~= 0
                    W = normalize_W(W, options.norm_w);
                end

                WH = W * H;

                % update H
                H = H .* ( (W'*(WH.^(options.d_beta-2) .* V)) ./ max(W'*WH.^(options.d_beta-1), options.myeps) );
                if options.norm_h ~= 0
                    H = normalize_H(H, options.norm_h);
                end
            else
                error('Invalid metric.')
            end

        end

        % measure elapsed time
        elapsed_time = toc(start_time);

        % measure gradient calc count
        grad_calc_count = grad_calc_count + m*n;

        % update epoch
        epoch = epoch + 1;

        % store info
        infos = store_nmf_info(V, W, H, [], options, infos, epoch, grad_calc_count, elapsed_time);

        % display info
        display_info(method_name, epoch, infos, options);

    end

    x.W = W;
    x.H = H;

end


function x=v_usasi(n,fs)
%V_USASI generates N samples of USASI noise at sample frequency FS X=(N,FS)

% This routine is based on the USASI noise defined in [1] which was later
% reissued as [2]. USASI noise is intended to simulate the long-term average
% of typical audio program material. The routine does not currently implement
% the pulsation at 2.5Hz 12.5% duty cycle that is recommended by the standard.
% Also it should probably be scaled to a well-defined power.
%
%  [1] NRSC AM Reemphasis, Deemphasize, and Broadcast Audio Transmission Bandwidth Specifications,
%      EIA-549 Standard, Electronics Industries Association , July 1988.
%  [2] NRSC AM Reemphasis, Deemphasize, and Broadcast Audio Transmission Bandwidth Specifications,
%      NRSC-1-A Standard, Sept 2007, Online: http://www.nrscstandards.org/SG/NRSC-1-A.pdf

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: v_usasi.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin<2 fs=8000; end
b=[1 0 -1];
a=poly(exp(-[100 320]*2*pi/fs));

x=v_randfilt(b,a,n);


function r = mpower(a,b)
%MPOWER       Implements  a ^ b  for intervals
%
% either a and b are (interval) scalar or, b is scalar integer
%

% written  10/16/98     S.M. Rump
% modified 12/30/98     S.M. Rump  improved performance and even exponent
% modified 05/20/02     S.M. Rump  interval integer exponent checked
% modified 04/04/04     S.M. Rump  set round to nearest for safety
%                                    NaN corrected
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
%

  % for both a and b scalars, use .^ with improved diameter for even exponent
  if ( prod(size(a))==1 ) & ( prod(size(b))==1 )
    r = a .^ b ;
    return
  end

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  % Check integer exponent
  if isa(b,'intval') & all(inf(b)==sup(b))
    b = b.inf;
    if ~isreal(b)
      error('invalid call of intval mpower ^')
    end
    a = intval(a);
  end

  if isa(b,'double') & isreal(b) & prod(size(b))==1 & b==round(b)
    [m n] = size(a);
    if m~=n
      error('intval mpower of non-square matrix')
    end
    if b==0
      if issparse(a)
        r = intval(speye(size(a)));
      else
        r = intval(eye(size(a)));
      end
      index = isnan(a) & ( b==0 );
      if any(index(:))
        %VVVV  r(index) = NaN;
        s.type = '()'; s.subs = {index}; r = subsasgn(r,s,NaN);
        %AAAA  Matlab bug fix
      end
    else                        % b is integer
      b_is_negative = b<0;
      b = abs(b) - 1;           % abs(b) is at least 1
      r = a;
      while b>0
        if mod(b,2)==1
          r = r*a;
        end
        b = floor(b/2);
        if b~=0
          a = a*a;
        end
      end
      if b_is_negative
        r = inv(r);
      end
    end
  else
    error('invalid call of intval mpower ^')
  end

  if rndold
    setround(rndold)
  end


function areasmooth = LowPassFilterArea(area,filterorder,maxfreq)

f = fdesign.lowpass('N,F3db',filterorder,maxfreq);
h = design(f,'butter');
h.PersistentMemory = true;

h.filter(fliplr(area));
areasmooth = h.filter(area);


function w = clean(w)
% CLEAN Clean up waveform object(s)
%   w = clean(w) will detrend waveforms (in a smart way, aware of NaN
%   values marking missing values), then use fillgaps to mark bounded NaNs
%   with linear-interpolated values, and also mark bounded NaNs with zeroes
%   (now okay, since trend has been removed, so no weird startup effects).
%   It then removes non-linear trends using a 20-s (0.05 Hz) highpass
%   filter.

% Glenn Thompson March 28, 2017

    for c=1:numel(w)

        if ~isempty(w(c))

            % remove spikes of length 1
            w(c) = medfilt1(w(c), 3);

            % smart detrend
            w(c) = detrend(w(c));

            % % fill gaps to get rid of NaNs marking missing values, so we can filter
            w(c) = fillgaps(w(c), 'interp');

            % highpass data at 20s - to remove non-linear trends
            f = filterobject('h',0.05,2);
            w(c) = filtfilt(f,w(c));

        end
    end

end


classdef StressNormSuperEllipseComputer < handle

    properties (Access = private)
        microProblem
        fName
        outputFolder
    end

    properties (Access = private)
        mx
        my
        q
        phi
        pNorm
        print
        fileName
        hasToCaptureImage
        testName
        iter
        meshBackground
        mesh
        hMesh
    end

    methods (Access = public)

        function obj = StressNormSuperEllipseComputer(cParams)
            obj.init(cParams);
        end

        function sPnorm = compute(obj)
            obj.createMicroProblem()
            sPnorm = obj.computePstressNorm();
        end

        function var = computeCellVariables(obj)
            obj.compute();
            mProblem = obj.microProblem;
            var.Ctensor = mProblem.variables.varFromCh.Chomog;
            var.tstress = mProblem.variables.varFromCh.tstress;
            var.tstrain = mProblem.variables.varFromCh.tstrain;
            var.displ   = mProblem.variables.varFromCh.tdisp;
            var.volume  = mProblem.mesh.computeVolume();
            var.mesh    = mProblem.mesh;

            quad = Quadrature.set(var.mesh.geometryType);
            quad.computeQuadrature('CONSTANT');
            volume = var.mesh.computeDvolume(quad);
            var.integrationVar.dV    = volume;
            var.integrationVar.nstre = size(var.tstress,1);
            var.integrationVar.ngaus = size(var.tstress,2);
            var.integrationVar.geoVol = obj.meshBackground.computeVolume();
        end

        function printImage(obj)
            outputName = [obj.fileName,'Print'];
            if obj.print
                    s.mesh       = obj.mesh;
                    s.outPutName = outputName;
                    printer = SuperEllipsePrinter(s);
                    printer.print();
                if obj.hasToCaptureImage
                    printer.captureImage()
                end
            end
        end

        function printStress(obj)
            if obj.print
                microP = obj.microProblem;
                outputName = [obj.fileName,'Print'];
                dI.mesh    =  microP.mesh;
                dI.outName = outputName;
                dI.pdim    = '2D';
                dI.ptype   = 'MICRO';
                ps = PostProcessDataBaseCreator(dI);
                dB = ps.getValue();
                postCase = 'ElasticityMicro';
                postProcess = Postprocess(postCase,dB);
                d.fields = microP.variables;
                d.quad   = microP.element.quadrature;
                postProcess.print(obj.iter,d);
            end
        end

    end

    methods (Access = private)

        function init(obj,cParams)
            obj.mx       = cParams.mx;
            obj.my       = cParams.my;
            obj.q        = cParams.q;
            obj.phi      = cParams.phi;
            obj.pNorm    = cParams.pNorm;
            obj.print    = cParams.print;
            obj.fileName = cParams.fileName;
            obj.iter     = cParams.iter;
            obj.hMesh    = cParams.hMesh;
            obj.hasToCaptureImage = cParams.hasToCaptureImage;
            obj.computeFileNameAndOutputFolder();
        end

        function computeFileNameAndOutputFolder(obj)
            obj.fName = [obj.fileName];
            obj.outputFolder = fullfile(pwd,'Output',obj.fName);
        end

        function sPnorm2 = computePstressNorm(obj)
            stress = [sin(obj.phi) cos(obj.phi) 0];
            p = obj.pNorm;
            sPnorm2 = obj.microProblem.computeStressPnorm(stress,p);
        end

        function ls = createLevelSet(obj)
            sM.coord  = obj.meshBackground.coord;
            sM.connec = obj.meshBackground.connec;
            s.mesh = Mesh_Total(sM);

            s.widthH = obj.mx;
            s.widthV = obj.my;
            s.pnorm  = obj.q;
            s.type = 'smoothRectangle';

            s.levelSetCreatorSettings = s;
            s.type = 'LevelSet';

            s.scalarProductSettings.epsilon = 1;
            levelSet = LevelSet(s);
            ls = levelSet.value;
        end

        function createBackgroundMesh(obj)
            obj.testName = 'RVE_Square_Triangle_FineFine';
            %obj.testName = 'RVE_Square_Triangle_Fine';
            s.testName = obj.testName;
            obj.meshBackground = Mesh().createFromFile(s);
        end

        function createMesh(obj)
            s.fileName = obj.fileName;
            s.levelSet = obj.createLevelSet();
            s.meshBackground = obj.meshBackground;
            s.hMesh = obj.hMesh;
            mCreator = MeshCreatorFromLevelSetWithMMG(s);
            obj.mesh = mCreator.create();
        end

        function createMicroProblem(obj)
            obj.createBackgroundMesh();
            obj.createMesh();
            femSolver = Elastic_Problem_Micro.create(obj.testName);
            femSolver.setMesh(obj.mesh);
            props.kappa = .75;
            props.mu    = .375;
            femSolver.setMatProps(props);
            obj.microProblem = femSolver;
        end

    end

end


% AS-RPCA: Active Subspace: Towards Scalable Low-Rank Learning (Liu and Yan, 2012)
% process_video('RPCA', 'AS-RPCA', 'dataset/demo.avi', 'output/demo_AS-RPCA.avi');
lambda = 1/sqrt(min(size(M)));
[L,S] = as_rpca(M,lambda);


function [ n_data, a, b, x, fx ] = von_mises_cdf_values ( n_data )

%*****************************************************************************80
%
%% VON_MISES_CDF_VALUES returns some values of the von Mises CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 December 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Kanti Mardia and Peter Jupp,
%    Directional Statistics,
%    Wiley, 2000, QA276.M335
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real A, B, the parameters of the function.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 23;

  a_vec = [ ...
     0.0E+00, ...
     0.0E+00, ...
     0.0E+00, ...
     0.0E+00, ...
     0.0E+00, ...
     0.1E+01, ...
     0.1E+01, ...
     0.1E+01, ...
     0.1E+01, ...
     0.1E+01, ...
     0.1E+01, ...
    -0.2E+01, ...
    -0.1E+01, ...
     0.0E+01, ...
     0.1E+01, ...
     0.2E+01, ...
     0.3E+01, ...
     0.0E+00, ...
     0.0E+00, ...
     0.0E+00, ...
     0.0E+00, ...
     0.0E+00, ...
     0.0E+00 ];

  b_vec = [ ...
     0.1E+01, ...
     0.1E+01, ...
     0.1E+01, ...
     0.1E+01, ...
     0.1E+01, ...
     0.2E+01, ...
     0.2E+01, ...
     0.2E+01, ...
     0.2E+01, ...
     0.2E+01, ...
     0.2E+01, ...
     0.3E+01, ...
     0.3E+01, ...
     0.3E+01, ...
     0.3E+01, ...
     0.3E+01, ...
     0.3E+01, ...
     0.0E+00, ...
     0.1E+01, ...
     0.2E+01, ...
     0.3E+01, ...
     0.4E+01, ...
     0.5E+01 ];

  fx_vec = [ ......
    0.2535089956281180E-01, ...
    0.1097539041177346E+00, ...
    0.5000000000000000E+00, ...
    0.8043381312498558E+00, ...
    0.9417460124555197E+00, ...
    0.5000000000000000E+00, ...
    0.6018204118446155E+00, ...
    0.6959356933122230E+00, ...
    0.7765935901304593E+00, ...
    0.8410725934916615E+00, ...
    0.8895777369550366E+00, ...
    0.9960322705517925E+00, ...
    0.9404336090170247E+00, ...
    0.5000000000000000E+00, ...
    0.5956639098297530E-01, ...
    0.3967729448207649E-02, ...
    0.2321953958111930E-03, ...
    0.6250000000000000E+00, ...
    0.7438406999109122E+00, ...
    0.8369224904294019E+00, ...
    0.8941711407897124E+00, ...
    0.9291058600568743E+00, ...
    0.9514289900655436E+00 ];

  x_vec = [ ......
    -0.2617993977991494E+01, ...
    -0.1570796326794897E+01, ...
     0.0000000000000000E+00, ...
     0.1047197551196598E+01, ...
     0.2094395102393195E+01, ...
     0.1000000000000000E+01, ...
     0.1200000000000000E+01, ...
     0.1400000000000000E+01, ...
     0.1600000000000000E+01, ...
     0.1800000000000000E+01, ...
     0.2000000000000000E+01, ...
     0.0000000000000000E+00, ...
     0.0000000000000000E+00, ...
     0.0000000000000000E+00, ...
     0.0000000000000000E+00, ...
     0.0000000000000000E+00, ...
     0.0000000000000000E+00, ...
     0.7853981633974483E+00, ...
     0.7853981633974483E+00, ...
     0.7853981633974483E+00, ...
     0.7853981633974483E+00, ...
     0.7853981633974483E+00, ...
     0.7853981633974483E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    a = 0.0;
    b = 0.0;
    x = 0.0;
    fx = 0.0;
  else
    a = a_vec(n_data);
    b = b_vec(n_data);
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function besi0_test ( )

%*****************************************************************************80
%
%% BESI0_TEST tests R4_BESI0 and R8_BESI0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    John Burkardt
%
  addpath ( '../test_values' );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'BESI0_TEST:\n' );
  fprintf ( 1, '  Test BESI0_VALUES, R4_BESI0, R8_BESI0\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '             X       BESI0(X)\n' );
  fprintf ( 1, '                  R4_BESI0(X)         Diff\n' );
  fprintf ( 1, '                  R8_BESI0(X)         Diff\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx1 ] = bessel_i0_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fx2 = r4_besi0 ( single ( x ) );
    fx3 = r8_besi0 ( x );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  %14.4f  %14.6g\n', x, fx1 );
    fprintf ( 1, '                  %14.6g  %14.6g\n', fx2, abs ( fx1 - fx2 ) );
    fprintf ( 1, '                  %14.6g  %14.6g\n', fx3, abs ( fx1 - fx3 ) );

  end

  rmpath ( '../test_values' );

  return
end


function r8vec_split_test ( )

%*****************************************************************************80
%
%% R8VEC_SPLIT_TEST tests R8VEC_SPLIT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  n = 25;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8VEC_SPLIT_TEST\n' );
  fprintf ( 1, '  R8VEC_SPLIT splits a vector into\n' );
  fprintf ( 1, '  entries less than and greater than a\n' );
  fprintf ( 1, '  splitting value.\n' );

  b = 0.0;
  c = 10.0;
  seed = 123456789;

  [ a, seed ] = r8vec_uniform_ab ( n, b, c, seed );

  a(1:n) = round ( a(1:n) ) / 2.0;

  split = 0.5 * ( a(1) + a(n) );

  r8vec_print ( n, a, '  The array:' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Splitting value is %f\n', split );

  [ a, isplit ] = r8vec_split ( n, a, split );

  r8vec_print ( n, a, '  The split array:' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Array entries <= SPLIT up to index %d\n', isplit );

  return
end


function [startingvals,nu,lambda,LLs,outputParameters]=igarch_starting_values(startingvals,epsilon,fepsilon,p,q,T,errorType,igarchType,constant)
% Perform a grid search to find decent starting values for IGARCH(P,Q)
% esimtation.  If starting values are user supplied (and thus nonempty), reformats
% starting values depending on ERRORTYPE.
%
% USAGE:
%   [STARTINGVALS,NU,LAMBDA,LLS,OUTPUTPARAMETERS] = ...
%        igarch_starting_values(STARTINGVALS,EPSILON,EPSILON2,P,Q,T,ERRORTYPE,IGARCHTYPE,CONSTANT)
%
% INPUTS:
%   STARTINGVALS     - A vector of starting values or empty to perform a grid search
%   EPSILON          - A column of mean zero data
%   FEPSILON         - Either abs(EPSILON) or EPSILON.^2, depending on IGARCHTYPE
%   P                - Positive, scalar integer representing the number of
%                      symmetric innovations
%   Q                - Non-negative, scalar integer representing the number
%                      of lags of conditional variance (0 for ARCH)
%   T                - Length of EPSILON
%   ERRORTYPE        - [OPTIONAL] The error distribution used, valid types are:
%                       'NORMAL'    - Gaussian Innovations [DEFAULT]
%                       'STUDENTST' - T distributed errors
%                       'GED'       - Generalized Error Distribution
%                       'SKEWT'     - Skewed T distribution
%   IGARCHTYPE       - [OPTIONAL] The type of variance process, either
%                        1 - Model evolves in absolute values
%                        2 - Model evolves in squares [DEFAULT]
%   CONSTANT         - [OPTIONAL] Logical value indicating whether model
%                       should include a constant.  Default is true (include).
%
% OUTPUTS:
%   STARTINGVALS     - A vector of starting values (CONSTANT+p+q) by 1
%   NU               - Distribution kurtosis parameter, empty if not applicable
%   LAMBDA           - Distribution asymmetry parameter, empty if not applicable
%   LLS              - A vector of log likelihoods corresponding to OUTPUTPARAMETERS
%   OUTPUTPARAMETERS - A matrix of alternative starting values, sorted by log likelihood
%
% COMMENTS:
%   See also IGARCH

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 1    Date: 7/12/2009

%Initialize variables
LLs=[];
outputParameters=[];

%No starting values provided
if isempty(startingvals)
    nu=[];
    lambda=[];

    %Procedure is to find best starting values, using a grid search find values for normal, then

    %Possible starting values based on commonly estimated values
    a=[.05 .1 .2];
    la=length(a);
    ab=1;
    lb=length(ab);

    %Many outputParameters and LLs
    outputParameters=zeros(la*lb,1+p+q-1);
    LLs=zeros(la,1);

    %Adjustment is needed to the intercept.  Assumes normality
    if igarchType==1
        adjFactor=sqrt(2/pi);
        backCast=mean(abs(epsilon));
    else
        adjFactor=1;
        backCast=cov(epsilon);
    end

    %Use an index to count
    index=1;

    for i=1:la
        %Loop over a
        alpha=a(i);
        tempAlpha=alpha*ones(p,1)/p;
        %Beta must satisfy the unit root
        beta=1-sum(tempAlpha);
        %Pick omega to match the unconditional
        if constant
            omega=backCast*.01*adjFactor;
        else
            omega = [];
        end
        %Build the parameter vector

        if q==0
            parameters=[omega; tempAlpha];
        else
            parameters=[omega; tempAlpha; beta*ones(q-1,1)/q];
        end
        %Set the output parameters
        outputParameters(index,:)=parameters';
        %Set the log likelihoods
        LLs(index)=igarch_likelihood(parameters, epsilon, fepsilon, p, q, 1, igarchType, constant, backCast, T, false);
        %Increment
        index=index+1;
    end
    %Sort the LLs so the best (lowest, since we minimize the -1*LL)
    [LLs,index]=sort(LLs);
    %Order the starting values
    startingvals=outputParameters(index(1),:)';
    %Order the ouputParameters
    outputParameters=outputParameters(index,:);
    %Use generic values for nu and lambda if needed
    if errorType==2
        nu=8;
    elseif errorType==3
        nu=1.9;
    elseif errorType==4
        nu=8;
        lambda=-.1;
    end
else
    %Values provided, only parse them
    nu=[];
    lambda=[];
    if errorType==2 || errorType==3 || errorType==4
        nu=startingvals(constant+p+q);
    end
    if errorType==4
        lambda=startingvals(constant+p+q+1);
    end
    startingvals=startingvals(1:constant+p+q-1);
end


function [ relpos, psize3, delta_midy ] = gen2dOffset_v2( box1, box2, issplitting )
%GEN2DOFFSET_V2 compute the rotation and offsets between two sibling boxes
% INPUT: box1, box2 - 3d box of base box with absolute position
%
% OUTPUT: relpos - 13d vector (rp of box2 relative to box1), including:
%                    (1) relative orientation - 1 bit degree
%                    (3) offsets - 2 bits
%                    (4) 16 classes (4 classes along each axis) - 8 bits
%                    (5) attachments (4 possible cases) - 4 bits
%                    (6) alignments (aligned along each axis) - 2 bit

% box1 info
pcent = box1(1:3);
pfront = box1(4:6);
pup = box1(7:9);
psize = box1(10:12);
paxes = cross(pfront,pup);
paxes = paxes/norm(paxes,2);

% box2 info
ccent = box2(1:3);
cfront = box2(4:6);
cup = box2(7:9);
csize = box2(10:12);
caxes = cross(cfront,cup);
caxes = caxes/norm(caxes,2);

transform = genTransMat(pfront,pup);
ncent = ccent-pcent;
ncent = transform*ncent; % child's center position in parent's local frame
nfront = cfront;
nfront = transform*nfront; % child's front direction in parent's local frame
nfront = nfront/norm(nfront,2);
nup = cup;
nup = transform*nup; % child's up direction in parent's local frame
nup = nup/norm(nup,2);
naxes = cross(nfront,nup);
naxes = naxes/norm(naxes,2);

% 4 corner points of parent box under its own local frame
plfront = [1;0;0];
plaxes = [0;0;1];
ppoints(:,1) = psize(1)*plfront/2 + psize(3)*plaxes/2;
ppoints(:,2) = psize(1)*plfront/2 - psize(3)*plaxes/2;
ppoints(:,3) = -psize(1)*plfront/2 - psize(3)*plaxes/2;
ppoints(:,4) = -psize(1)*plfront/2 + psize(3)*plaxes/2;

% 4 corner points of child box under its parent's local frame
cpoints(:,1) = ncent + csize(1)*nfront/2 + csize(3)*naxes/2;
cpoints(:,2) = ncent + csize(1)*nfront/2 - csize(3)*naxes/2;
cpoints(:,3) = ncent - csize(1)*nfront/2 - csize(3)*naxes/2;
cpoints(:,4) = ncent - csize(1)*nfront/2 + csize(3)*naxes/2;

% the relative positions
pmaxx = max(ppoints(1,:));
pmaxy = max(ppoints(3,:));
pminx = min(ppoints(1,:));
pminy = min(ppoints(3,:));
cmaxx = max(cpoints(1,:));
cmaxy = max(cpoints(3,:));
cminx = min(cpoints(1,:));
cminy = min(cpoints(3,:));

delta_midy = 0;
if(issplitting==1)
    pmidy = (pmaxy+pminy)/2;
    cmidy = (cmaxy+cminy)/2;
    delta_midy = pmidy-cmidy;
    pmaxy = cmaxy;
    pminy = cminy;
end
psize3 = pmaxy-pminy;

% relative position class along x axis
delta_x = [pminx-cmaxx;pminx-cminx;pmaxx-cmaxx;pmaxx-cminx];
[~,classxindex] = min(abs(delta_x));
offsets(1) = delta_x(classxindex);

% relative position class along y axis
delta_y = [pminy-cmaxy;pminy-cminy;pmaxy-cmaxy;pmaxy-cminy];
[~,classyindex] = min(abs(delta_y));
offsets(2) = delta_y(classyindex);

% alignment info
attachindex = 1;
attacheps = 0.011; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if(abs(offsets(1))<attacheps)
    attachindex = attachindex + 1;
end
if(abs(offsets(2))<attacheps)
    attachindex = attachindex + 2;
end
attachvec = zeros(4,1);
attachvec(attachindex) = 1;

% form the relpos vector
rot_eps = 0.01;
alignment = [1;0;0;0;0];
if(abs(nfront(1)-1)<rot_eps)
    alignment = [0;1;0;0;0]; % nfront = [1,0,0];
end
if(abs(nfront(1)+1)<rot_eps)
    alignment = [0;0;1;0;0]; % nfront = [-1,0,0];
end
if(abs(nfront(3)-1)<rot_eps)
    alignment = [0;0;0;1;0]; % nfront = [0,0,1];
end
if(abs(nfront(3)+1)<rot_eps)
    alignment = [0;0;0;0;1]; % nfront = [0,0,-1];
end

if(nfront(1)==0)
    if(nfront(3)>0)
        degree = -0.5;
    else
        degree = 0.5;
    end
else
    degree = atand(nfront(3)/nfront(1));
    if(nfront(1)<0)
        if(degree<0)
            degree = degree-180;
        else
            degree = degree+180;
        end
    end
    degree = mod(degree,360)/180-1;
end
if(abs(degree)>1)
        o=0;
    end

classindex = classxindex + (classyindex-1)*4;
classvec = zeros(16,1);
classvec(classindex) = 1;
relpos = [degree;offsets(1);offsets(2);classvec;attachvec;alignment];

% tmpcent = pcent;
% tmpfsize = (max(cmaxx,pmaxx)-min(cminx,pminx));
% tmpasize = (max(cmaxy,pmaxy)-min(cminy,pminy));
% tmpobb = [tmpcent;plfront;0;1;0;tmpfsize;1;tmpasize];
% draw3dOBB_v2(box1,'r');
% draw3dOBB_v2(box2,'g');
% draw3dOBB_v2(tmpobb,'y');

end


function kSpaceLow = updownsample( kSpaceHigh, lInputImage, out_size, lAAfilter)
%
% updownsample - up-sample or down-sample an input series using fourier domain
%                input series needs to be continuous of a high degree
%
%
% input:    in_m                - input matrix for up/down sampling, in fourier domain
%
%           out_size            - desired number of pixels in the output image

%
% output:   out_m               - up/down sampled image
%
% NOTE: it is important to specify if the image is REAL or COMPLEX, since
%       if the image is REAL -> we have to use ABS() on the inverse fourier
%       transform (because of roundoff errors of the transform).
%
% NOTE: since a desired amount of pixels is needed at the output, there is
%       no attempt to use matrices which are in size of power of 2. this
%       optimization can not be used in this case
%
% NOTE: input series needs to be CONTINUOUS OF A HIGH DEGREE, since the
%       upsampling is done in the frequency domain, which samples the output
%       grid with SINE-like (harmonic) functions
%
%
%
% Theory:   the upsampling is done by zero-padding in the input domain BETWEEN the samples,
%           then at the fourier domain, taking the single spectrum (out of the repetition of spectrums)
%           i.e. low pass with Fcutoff=PI/upsample_factor, zeroing the rest of the spectrum
%           and then doing ifft to the distribution.
%           since we have a zero padding operation in time, we need to multiply by the fourier gain.
%
%              +-----------+     +-------+     +---------+     +--------+     +--------+
%   y[n,m] --> | up-sample | --> |  FFT  | --> |   LPF   | --> | * Gain | --> |  IFFT  | --> interpolated
%              | factor M  |     +-------+     | Fc=PI/M |     +--------+     +--------+
%              +-----------+                   +---------+
%
%           this operation is the same as the following one (which has less operations):
%
%              +-------+     +--------+     +--------------+     +--------+
%   y[n,m] --> |  FFT  | --> | * Gain | --> | Zero Padding | --> |  IFFT  | --> interpolated
%              +-------+     +--------+     +--------------+     +--------+
%
%           NOTE THAT, the zero-padding must be such that the D.C. ferquency remains the D.C. frequency
%           and that the zero padding is applied to both positive and negative frequencies.
%           The zero padding actually condences the frequency -> which yields a longer series in the
%           image domain, but without any additional information, thus the operation must be an interpolation.

if(nargin < 4)
    lAAfilter = true;
end

% ==============================================
% get input image size, and calculate the gain
% ==============================================
out_x_sz = out_size(1); out_y_sz = out_size(2); out_z_sz = out_size(3);
[in_x_sz,in_y_sz, in_z_sz, nCha, nTime] = size( kSpaceHigh );
gain_x = out_x_sz/in_x_sz;
gain_y = out_y_sz/in_y_sz;
gain_z = out_z_sz/in_z_sz;

% upsample or downsample as needed
% ==================================
if(lInputImage)
    % image -> kSpace
    kSpaceHigh = fftnshift(kSpaceHigh,1:3);
end

% check if up/down sampling is needed at all
if(gain_x == 1 && gain_y == 1 && gain_z == 1)
    kSpaceLow = kSpaceHigh;
    dImgLow = ifftnshift(kSpaceLow,1:3);
    return;
end

% if downsampling -> apply anti-aliasing filter
if(gain_x < 1 && gain_y < 1 && gain_z < 1 && lAAfilter)
    kSpaceHigh = kSpaceHigh .* repmat(windowND(@hamming, [size(kSpaceHigh,1), size(kSpaceHigh,2), size(kSpaceHigh,3)]),[1 1 1 size(kSpaceHigh,4) size(kSpaceHigh,5)]);
end

% build grid vectors for the up/down sampling
% ============================================
% if the input is even & output is odd-> use floor for all
% if the output is even & input is odd -> use ceil for all
% other cases - don't care
% for downsampling -> the opposite
if (~mod( in_x_sz,2 ) && (out_x_sz>in_x_sz)) || (mod( in_x_sz,2 ) && (out_x_sz<in_x_sz))
    x_output_space  = max(floor((out_x_sz-in_x_sz)/2),0) + [1:min(in_x_sz,out_x_sz)];
    x_input_space   = max(floor((in_x_sz-out_x_sz)/2),0) + [1:min(in_x_sz,out_x_sz)];
else
    x_output_space  = max(ceil((out_x_sz-in_x_sz)/2),0) + [1:min(in_x_sz,out_x_sz)];
    x_input_space   = max(ceil((in_x_sz-out_x_sz)/2),0) + [1:min(in_x_sz,out_x_sz)];
end
if (~mod( in_y_sz,2 ) && (out_y_sz>in_y_sz)) || (mod( in_y_sz,2 ) && (out_y_sz<in_y_sz))
   y_output_space  = max(floor((out_y_sz-in_y_sz)/2),0) + [1:min(in_y_sz,out_y_sz)];
   y_input_space   = max(floor((in_y_sz-out_y_sz)/2),0) + [1:min(in_y_sz,out_y_sz)];
else
   y_output_space  = max(ceil((out_y_sz-in_y_sz)/2),0) + [1:min(in_y_sz,out_y_sz)];
   y_input_space   = max(ceil((in_y_sz-out_y_sz)/2),0) + [1:min(in_y_sz,out_y_sz)];
end
if (~mod( in_z_sz,2 ) && (out_z_sz>in_z_sz)) || (mod( in_z_sz,2 ) && (out_z_sz<in_z_sz))
   z_output_space  = max(floor((out_z_sz-in_z_sz)/2),0) + [1:min(in_z_sz,out_z_sz)];
   z_input_space   = max(floor((in_z_sz-out_z_sz)/2),0) + [1:min(in_z_sz,out_z_sz)];
else
   z_output_space  = max(ceil((out_z_sz-in_z_sz)/2),0) + [1:min(in_z_sz,out_z_sz)];
   z_input_space   = max(ceil((in_z_sz-out_z_sz)/2),0) + [1:min(in_z_sz,out_z_sz)];
end

% perform the up/down sampling
kSpaceLow  = zeros( out_x_sz, out_y_sz, out_z_sz, size(kSpaceHigh,4), size(kSpaceHigh,5) );
% kSpaceHigh = fftshift(fftshift(fftshift(kSpaceHigh,1),2),3); % already zero-centered
kSpaceLow(x_output_space,y_output_space, z_output_space, :, :) = kSpaceHigh(x_input_space,y_input_space, z_input_space, :, :);

end


function w = padua_weights ( l )

%*****************************************************************************80
%
%% PADUA_WEIGHTS returns quadrature weights for Padua points.
%
%  Discussion:
%
%    The order of the weights corresponds to the ordering used
%    by the companion function padua_points().
%
%    Caliari, de Marchi and Vianello supplied a MATLAB code pdwtsMM
%    which carries out this same computation in a way that makes
%    more efficient use of MATLAB's vector and matrix capabilities.
%    This version of the computation was painfully rewritten to display
%    the individual scalar computations, so that it could be translated
%    into other languages.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Marco Caliari, Stefano de Marchi, Marco Vianello,
%    Bivariate interpolation on the square at new nodal sets,
%    Applied Mathematics and Computation,
%    Volume 165, Number 2, 2005, pages 261-274.
%
%  Parameters:
%
%    Input, integer L, the level of the set.
%    0 <= L
%
%    Output, real W((L+1)*(L+2)/2), the quadrature weights.
%
  if ( l == 0 )
    w = 4.0;
    return
  end
%
%  Relatives of L/2:
%
  lp1h = floor ( ( l + 1 ) / 2 );
  lp2h = floor ( ( l + 2 ) / 2 );
  lp3h = floor ( ( l + 3 ) / 2 );
%
%  TE1, TE2, TO1, TO2:
%  Even and odd Chebyshev polynomials on subgrids 1 and 2.
%
  te1 = zeros(lp2h,lp2h);
  for j = 1 : lp2h
    for i = 1 : lp2h
      te1(i,j) = cos ( 2 * ( i - 1 ) * ( 2 * j - 2 ) * pi / l );
    end
  end
  te1(2:lp2h,1:lp2h) = te1(2:lp2h,1:lp2h) * sqrt ( 2.0 );

  to1 = zeros(lp2h,lp1h);
  for j = 1 : lp1h
    for i = 1 : lp2h
      to1(i,j) = cos ( 2 * ( i - 1 ) * ( 2 * j - 1 ) * pi / l );
    end
  end
  to1(2:lp2h,1:lp1h) = to1(2:lp2h,1:lp1h) * sqrt ( 2.0 );

  te2 = zeros(lp2h,lp3h);
  for j = 1 : lp3h
    for i = 1 : lp2h
      te2(i,j) = ...
        cos ( 2 * ( i - 1 ) * ( 2 * j - 2 ) * pi / ( l + 1 ) );
    end
  end
  te2(2:lp2h,1:lp3h) = te2(2:lp2h,1:lp3h) * sqrt ( 2.0 );

  to2 = zeros(lp2h,lp2h);
  for j = 1 : lp2h
    for i = 1 : lp2h
      to2(i,j) = ...
        cos ( 2 * ( i - 1 ) * ( 2 * j - 1 ) * pi / ( l + 1 ) );
    end
  end
  to2(2:lp2h,1:lp2h) = to2(2:lp2h,1:lp2h) * sqrt ( 2.0 );
%
%  MOM: Moments matrix for even * even pairs.
%
  mom = zeros(lp2h,lp2h);

  for j = 1 : lp2h
    mj = 2.0 * sqrt ( 2.0 ) / ( 1.0 - ( 2 * j - 2 ) ^2 );
    for i = 1 : lp2h + 1 - j
      mi = 2.0 * sqrt ( 2.0 ) / ( 1.0 - ( 2 * i - 2 ) ^2 );
      mom(i,j) = mi * mj;
    end
  end

  mom(1,1:lp2h) = mom(1,1:lp2h) / sqrt ( 2.0 );
  mom(1:lp2h,1) = mom(1:lp2h,1) / sqrt ( 2.0 );

  if ( mod ( l, 2 ) == 0 )
    mom(lp2h,1) = mom(lp2h,1) / 2.0;
  end
%
%  TMTOE and TMTEO: matrix products.
%
  tmtoe = zeros(lp2h,lp2h);

  for j2 = 1 : lp2h
    for i2 = 1 : lp2h+1-j2
      for j = 1 : lp2h
        for i = 1 : lp2h
          tmtoe(i,j) = tmtoe(i,j) + to2(i2,i) * mom(j2,i2) * te1(j2,j);
        end
      end
    end
  end

  tmteo = zeros(lp3h,lp1h);

  for j2 = 1 : lp2h
    for i2 = 1 : lp2h+1-j2
      for j = 1 : lp1h
        for i = 1 : lp3h
          tmteo(i,j) = tmteo(i,j) + te2(i2,i) * mom(j2,i2) * to1(j2,j);
        end
      end
    end
  end
%
%  W1 and W2: Interpolation weight matrices.
%
  w1 = 2.0 * ones(lp2h,lp2h) / ( l * ( l + 1 ) );

  w1(1:lp2h,1) = w1(1:lp2h,1) / 2.0;

  if ( mod ( l, 2 ) == 0 )
    w1(1:lp2h,lp2h) = w1(1:lp2h,lp2h) / 2.0;
    w1(lp2h,1:lp2h) = w1(lp2h,1:lp2h) / 2.0;
  end

  w2 = 2.0 * ones(lp3h,lp1h) / ( l * ( l + 1 ) );
  w2(1,1:lp1h) = w2(1,1:lp1h) / 2.0;

  if ( mod ( l, 2 ) == 1 )
    w2(lp3h,1:lp1h) = w2(lp3h,1:lp1h) / 2.0;
    w2(1:lp3h,lp1h) = w2(1:lp3h,lp1h) / 2.0;
  end
%
%  Cubature weights as matrices on the subgrids.
%
  for j = 1 : lp2h
    for i = 1 : lp2h
      w1(i,j) = w1(i,j) * tmtoe(i,j);
    end
  end

  for j = 1 : lp1h
    for i = 1 : lp3h
      w2(i,j) = w2(i,j) * tmteo(i,j);
    end
  end
%
%  Pack weight matrices W1 and W2 into the vector W.
%
  n = ( ( l + 1 ) * ( l + 2 ) ) / 2;
  w = zeros(n,1);

  if ( mod ( l, 2 ) == 0 )

    for i = 1 : lp2h
      for j = 1 : lp2h
        w(i+(2*j-2)*lp2h) = w1(i,j);
      end
    end

    for i = 1 : lp3h
      for j = 1 : lp1h
        w(i+(2*j-1)*lp2h) = w2(i,j);
      end
    end

  else

    for j = 1 : lp1h
      for i = 1 : lp2h
        w(i+(j-1)*(l+2)) = w1(i,j);
      end
    end

    for j = 1 : lp1h
      for i = 1 : lp3h
        w(i+lp2h+(j-1)*(l+2)) = w2(i,j);
      end
    end

  end

  return
end


function [R, movement, verbose] = tapas_physio_create_movement_regressors(...
    movement, verbose)
% Reads realignment parameters, creates derivative/squared & outlier regressor
%
% [R, movement, verbose] = ...
%       tapas_physio_create_movement_regressors(movement, verbose)
%
% The 6 realignment parameters can be augmented by their derivatives (in
% total 12 parameters) + the squares of parameters and derivatives (in
% total 24 parameters), leading to a Volterra expansion as in
%
%               Friston KJ, Williams S, Howard R, Frackowiak
%               RS, Turner R. Movement-related effects in fMRI
%               time-series. Magn Reson Med. 1996;35:346?355.)
%
% IN
%   movement    physio.model.movement
%   verbose     physio.verbose
% OUT
%   R                   [nScans, (6|12|24)+nOutliers] regressor matrix
%                       from movement modeling
%   movement            structure (as defined in tapas_physio_new) with the
%                       following fields
%   rp                  [nScans, 6] realignment parameters
%   dRp                 [nScans, 6] temporal difference of realignment
%                       parameters, prepending one line of zeros (for 1st
%                       scan)
%   quality_measures.   structure holding the suggested quality control
%                       measures for subject motion of Power et al., 2014,
%                       Fig. 2; all rotational parameters are transformed
%                       into translations by multiplying with rHead (arc
%                       length)
%       FD              framewise displacement (FD)
%       absTransDisplacement
%                       sum of absolute values of translational (x,y,z)
%                       realignment estimates, reflecting absolute
%                       displacement of the head
%       absRotDisplacement
%                       sum of absolute values of rotational (rotation
%                       around x,y,z-axis = pitch/roll/yaw)
%                       realignment estimates, reflecting absolute
%                       displacement of the head on its surface (arc
%                       length)
%       rmsdTrans       root mean squared framewise translational displacement (over x,y,z)
%                       equals to euclidean distance of center of
%                       mass of head between consecutive volumes
%       rmsdRot         root mean squared framewise rotational displacement
%                       (rotation around x,y,z axis)
%                       equals to euclidean distance of between angles
%                       between consecutive volumes
%       meanFD          mean (over volumes) of framewise displacement
%                       summary measure for subject (or session)
%       rmsMovement     root mean square (over scans) of detrended
%                       realignment estimates
%                       summary measure for subject (or session)
%
%   censoring.          structure with censoring information (detected outliers,
%                       computed quality values for censoring method maxval/fd:
%       nOutliers       number of detected outliers
%       R_outlier       stick/spike regressors indicating location of
%                       detected outliers
%       iOutlierTrans   volume indices of translation-related outliers
%       iOutlierRot     volume indices of rotation-related outliers
%       iOutlierArray   volume indices of all detected outliers
%
%
% EXAMPLE
%   tapas_physio_create_movement_regressors
%
%   See also tapas_physio_new tapas_physio_main_create_regressors

% Author: Lars Kasper
% Created: 2015-07-10
% Copyright (C) 2015 TNU, Institute for Biomedical Engineering,
%                    University of Zurich and ETH Zurich.
%
% This file is part of the TAPAS PhysIO Toolbox, which is released under the terms of the GNU General Public
% License (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

rHead = 50; % head radius in mm for FD computation (Power et al., 2012)

[fp, fn, ext] = fileparts(movement.file_realignment_parameters);

if ~exist(movement.file_realignment_parameters, 'file')
    verbose = tapas_physio_log('No input multiple regressors found', verbose, 1);
    R = [];
else
    tmp = load(movement.file_realignment_parameters);
    if strcmp('.txt', ext) % text-file
        rp = tmp;
    else % mat file
        rp = tmp.R;
    end

    %% Motion Quality control

     [quality_measures, dRp] = ...
         tapas_physio_get_movement_quality_measures(rp, rHead);

    %% Motion 6/12/24 model
    R = rp;

    % Include derivatives
    if movement.order > 6
        R = [rp, dRp];
    end

    % Include squared regressors/derivatives
    if movement.order > 12
        R = [R, R.^2];
    end

    % Sanity check if order misspecified
    movement.order = size(R, 2);

    %% Motion Censoring
    % Include outlier movements exceeding thresholds as stick regressors
    % Euclidean distance used!
    iOutlierArray = [];
    iOutlierTrans = [];
    iOutlierRot = [];

    switch lower(movement.censoring_method)
        case 'none' % done
        case 'fd' % framewise displacement
            iOutlierArray = find(quality_measures.FD > ...
                movement.censoring_threshold);
        case 'maxval'   % tresholds for max abrupt translation/rotation,
                        % even per axis
            switch numel(movement.censoring_threshold)
                case {1,2}
                    outlier_translation_mm = movement.censoring_threshold(1);
                    outlier_rotation_deg = movement.censoring_threshold(end);
                    iOutlierTrans = find(quality_measures.rmsdTrans > outlier_translation_mm);
                    iOutlierRot = find(quality_measures.rmsdRot > outlier_rotation_deg/180*pi);
                case 6
                    ct = movement.censoring_threshold;
                    ct(4:6) = ct/180*pi;
                    iOutlierTrans = dRp(:,1) > ct(1) || dRp(:,2) > ct(2) || dRp(:,3) > ct(3);
                    iOutlierRot = dRp(:,4) > ct(4) || dRp(:,5) > ct(5) || dRp(:,6) > ct(6);
                otherwise
                    error('censoring threshold has to be 1,2 or 6 element vector. See tapas_physio_new');
            end

            iOutlierArray = unique([iOutlierTrans; iOutlierRot]);
        case 'dvars' %DVARS, as in Power et al, 2012
            % TODO
    end

    %% Construct censoring regressor matrix
    nOutliers = numel(iOutlierArray);

    nScans = size(rp,1);
    R_outlier = zeros(nScans, nOutliers);
    for iOutlier = 1:nOutliers
        R_outlier(iOutlierArray(iOutlier), iOutlier) = 1;
    end

    censoring = struct('nOutliers', nOutliers, 'R_outlier', R_outlier, ...
        'iOutlierTrans', iOutlierTrans, 'iOutlierRot', iOutlierRot, ...
        'iOutlierArray', iOutlierArray);

    if verbose.level >= 2
        switch lower(movement.censoring_method)
            case 'fd'
                verbose.fig_handles(end+1) = tapas_physio_plot_movement_outliers_fd(rp, ...
                    quality_measures, censoring, movement.censoring_threshold);
            case 'maxval'
                verbose.fig_handles(end+1) = tapas_physio_plot_movement_outliers_maxval(rp, ...
                    quality_measures, censoring, movement.censoring_threshold);
        end
    end

    %% Gather return values
    movement.censoring = censoring;
    movement.rp = rp;
    movement.dRp = dRp;
    movement.quality_measures = quality_measures;

    R = [R, R_outlier];

end


function [res, inds] = polylineSubcurve(poly, t0, t1)
%POLYLINESUBCURVE Extract a portion of a polyline.
%
%   POLY2 = polylineSubcurve(POLYLINE, POS0, POS1)
%   Create a new polyline, by keeping vertices located between positions
%   POS0 and POS1, and adding points corresponding to positions POS0 and
%   POS1 if they are not already vertices.
%
%   [POLY2, INDS] = polylineSubcurve(POLYLINE, POS0, POS1)
%   Also returns the indices of the original polyline that were selected.
%   The size of the array INDS may be smaller than the array POLY, due to
%   the addition of new vertices at the extremities.
%
%   Example
%     Nv = 100;
%     poly = circleAsPolygon([10 20 30], Nv);
%     poly2 = polylineSubcurve(poly, 15, 65);
%     drawCurve(poly2);
%
%   See also
%     polygons2d, polygonSubCurve
%

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2009-04-30, using Matlab 7.7.0.471 (R2008b)
% Copyright 2009-2022 INRAE - Cepia Software Platform

% number of vertices
Nv = size(poly, 1);

if t0 < t1
    % format positions
    t0 = max(t0, 0);
    t1 = min(t1, Nv-1);
end

% indices of extreme vertices inside subcurve
ind0 = ceil(t0)+1;
ind1 = floor(t1)+1;

% get the portion of polyline between 2 extremities
if t0 < t1
    inds = ind0:ind1;
else
    inds = [ind0:Nv 1:ind1];
end

res = poly(inds, :);

% add first point if it is not already a vertex
if t0 ~= ind0-1
    res = [polylinePoint(poly, t0); res];
end

% add last point if it is not already a vertex
if t1 ~= ind1-1
    res = [res; polylinePoint(poly, t1)];
end


%GRIDROTATE Rotate grid.
%
%   [ GRID ] = GRIDROTATE( GRID, TH, AX ) Applies rotation angle TH
%   (radians) to grid points around the origin (in 3D around axis AX:
%   1 = x-axis (default), 2 = y-axis, 3 = z-axis). The input argument
%   GRID can either be a grid struct or simply an array of
%   coordinates. Normals will also be recalculated in the case of a
%   grid struct with boundary information.
%
%   See also GRIDEXTRUDE, GRIDMERGE, GRIDREVOLVE, GRIDSCALE

% Copyright 2013-2022 Precise Simulation, Ltd.


%% OPTI Toolbox Global Nonlinear Programming Examples
%
% This file contains a number of Global NLP problems and demonstrates how
% to solve them using the OPTI Toolbox. You should read and complete
% BasicUsage.m & NonlinearProgramming.m BEFORE running the below examples.
%
%   Copyright (C) 2014 Jonathan Currie (IPL)

% There is also a page on the Wiki which supplements this example:
web('https://www.inverseproblem.co.nz/OPTI/index.php/Probs/GNLP');

%% Determing which Solver to Use
% OPTI Toolbox comes with a number of NLP solvers, thus to determine which
% ones are available on your system you can type:
clc
optiSolver('NLP')

% Note the columns DR and GL. A cross in DR indicates the solver requires
% 1st (and perhaps 2nd) derivatives, while a cross in GL indicates the
% solver can solve Global Optimization problems. For noisy problems unless
% you have exact derivatives, avoid solvers with a cross in DR.

%% Typical Global Optimization Problems
% Global optimization problems result from any of the following circumstances:
%
%       - Objectives containing noise / a stochastic element
%       - Non-convex functions (functions which are not a bowl / hill in 2D)
%       - Objectives that include periodic functions (sin, cos)
%       - Parameter estimation of ODEs solved with adaptive step integrators
%       - Any problem that contains multiple local minima.
%
% An extreme example from Wolfram is shown below:
clc
%Objective
fun = @(x) norm([x(1) - sin(2*x(1) + 3*x(2)) - cos(3*x(1) - 5*x(2));
          x(2) - sin(x(1) - 2*x(2)) + cos(x(1) + 3*x(2))]);
lb = [-4;-4]; ub = [4;4];
x0 = [-4;-4];

%Plot
n = 1e2;
x = linspace(-4,4,n); y = linspace(-4,4,n); Z = zeros(n,n);
for i = 1:n
    for j = 1:n
        Z(j,i) = fun([x(i),y(j)]);
    end
end
surfc(x,y,Z)
colormap summer; shading interp; lighting phong; view(-38,58);
xlabel('x1'); ylabel('x2'); zlabel('obj'); title('Wolfram Global Optimization Problem');

%% Example 1 - Basic Setup
% The main difference when solving a Global NLP is that OPTI treats Global
% and Local NLP problems identically, and therefore you will have to specify
% a Global solver. For this example we will build 4 OPTI objects with 4
% different solvers:
clc
%Build OPTI Problem
prob = optiprob('fun',fun,'bounds',lb,ub);
%Choose Global Solver
opts1 = optiset('solver','nomad');
opts2 = optiset('solver','pswarm');
opts3 = optiset('solver','nlopt','solverOpts',nloptset('algorithm','GN_DIRECT'));
opts4 = optiset('solver','ipopt','warnings','off');

%Pass to OPTI Constructor for Error Checking + Setup
Opt1 = opti(prob,opts1);
Opt2 = opti(prob,opts2);
Opt3 = opti(prob,opts3);
Opt4 = opti(prob,opts4);

% Call solve to solve the problem. Check the plot for a comparison of the
% solution points. Note NOMAD and NLOPT are deterministic with the current
% settings, PSWARM includes random elements, and IPOPT is for comparison of
% a local solution.

[x1,fval1] = solve(Opt1,x0);
[x2,fval2] = solve(Opt2,x0);
[x3,fval3] = solve(Opt3,x0);
[x4,fval4] = solve(Opt4,x0);

view(0,90); hold on;
plot3(x0(1),x0(2),10,'kx','markersize',10);
plot3(x1(1),x1(2),10,'ro'); text(x1(1)+0.1,x1(2)+0.1,10,sprintf('NOMAD: %f',fval1));
plot3(x2(1),x2(2),10,'ro'); text(x2(1)+0.1,x2(2)-0.1,10,sprintf('PSWARM: %f',fval2));
plot3(x3(1),x3(2),10,'ro'); text(x3(1)+0.1,x3(2)+0.2,10,sprintf('NLOPT: %f',fval3));
plot3(x4(1),x4(2),10,'ro'); text(x4(1)+0.1,x4(2)+0.1,10,sprintf('IPOPT: %f',fval4));
hold off;

%% Problem 2 - Quartic
% Includes Linear and Nonlinear Constraints
clc
%Problem
fun = @(x) x(1)^4 - 14*x(1)^2 + 24*x(1) - x(2)^2;

%Linear Constraints
A = [-1 1]; b = 8;
%Nonlinear Constraints
nlcon = @(x) (-x(1)^2) - 2*x(1) + x(2);
nlrhs = -2;
nle = -1;
%Bounds + Starting Guess
lb = [-8;0];
ub = [10;10];
x0 = [0;0];

% Solving a constrained global optimization problem. Note linear
% constraints will be converted to nonlinear ones for this solver
opts = optiset('solver','nomad','solverOpts',nomadset('direction_type','lt 2n'));
Opt = opti('fun',fun,'ineq',A,b,'nlmix',nlcon,nlrhs,nle,'bounds',lb,ub,'options',opts)

% This may take a few seconds...
[x,fval,exitflag,info] = solve(Opt,x0)

%% Example 3 - Solving with PSwarm
% Note x0 is on the local minima side of the saddle
clc
%Problem
fun = @(x) -2*x(1)*x(2);

%Constraints
lb = [-0.5;-0.5];
ub = [1;1];
x0 = [-0.3;-0.3];

%Plot
n = 1e2;
x = linspace(-0.5,1,n); y = linspace(-0.5,1,n); Z = zeros(n,n);
for i = 1:n
    for j = 1:n
        Z(j,i) = fun([x(i),y(j)]);
    end
end
surfc(x,y,Z); hold on; plot3(x0(1),x0(2),fun(x0),'r.','markersize',20); hold off;
colormap winter; shading flat; lighting gouraud; view(18,28);
xlabel('x1'); ylabel('x2'); zlabel('obj'); title('Saddle Point Optimization Problem');

% PSwarm solves bounded and linearly constrained global problems
Opt = opti('fun',fun,'bounds',lb,ub,'options',optiset('solver','pswarm'))

[x,fval,exitflag,info] = solve(Opt,x0)
plot(Opt,3)

%% Example 4 - White Box Quartic
% The following problem is the same as problem 2, however this time we are
% going to solve it using a white box solver (SCIP). The SCIP interface
% will parse the following functions into an algebraic description,
% allowing SCIP to find a global solution to this problem.
clc
%Problem
fun = @(x) x(1)^4 - 14*x(1)^2 + 24*x(1) - x(2)^2;

%Linear Constraints
A = [-1 1]; b = 8;
%Nonlinear Constraints
nlcon = @(x) (-x(1)^2) - 2*x(1) + x(2);
nlrhs = -2;
nle = -1;
%Bounds + Starting Guess
lb = [-8;0];
ub = [10;10];
x0 = [0;0];

% SCIP solves a subset of nonlinear and mixed integer problems, provided
% the problem is deterministics and constains a subset of allowable functions.
Opt = opti('fun',fun,'ineq',A,b,'nlmix',nlcon,nlrhs,nle,'bounds',lb,ub,...
           'options',optiset('solver','scip'))

[x,fval,exitflag,info] = solve(Opt,x0)
plot(Opt)

%% Example 5 - OPTI's Multi-Start Solver
% For simple, low dimensional problems, OPTI's mulit-start is a naive
% algorithm for searching the problem space for a global solution. See the
% following page for more examples:

web('https://www.inverseproblem.co.nz/OPTI/index.php/Advanced/MultiSolve');

clc
Opt = opti(Opt,'solver','ipopt');
[x,fval,exitflag,info] = multisolve(Opt,x0)
multiplot(Opt)

%% Summary
% While Global Optimization solvers may take longer, many real engineering
% problems result in noisy or non-convex objectives and local solvers will
% often struggle to return a result, or fall into the closest local
% optima. OPTI provides a range of competitive blackbox global optimization
% solvers which can return much better results, as well as a white box
% solver for academic users which guarantees a global solution.


classdef SimpAllInterpolationImplicit2D < SimpAllInterpolationImplicit

   methods  (Access = public)

        function obj = SimpAllInterpolationImplicit2D(cParams)
            obj.init(cParams);
            obj.computeNstre();
            obj.computeSymbolicInterpolationFunctions();
            obj.dmu0 = obj.computeDmu0();
            obj.dmu1 = obj.computeDmu1();
            obj.dk0  = obj.computeDKappa0();
            obj.dk1  = obj.computeDKappa1();
        end

   end

    methods  (Access = protected)

        function [pMu,pKappa] = computePolarizationTensorAsMuKappa(eMatrix,eInclusion,nuMatrix,nuInclusion)
            coef = obj.compute2Dcoefficients(eMatrix,eInclusion,nuMatrix,nuInclusion);
            [p1,p2] = obj.computeP1P2(coef);
            [pMu,pKappa] = obj.computePkappaPmu(p1, p2);
        end

        function dmu0 = computeDmu0(obj)
            E1  = obj.matProp.E1;
            E0  = obj.matProp.E0;
            nu1 = obj.matProp.nu1;
            nu0 = obj.matProp.nu0;
            mu0 = obj.matProp.mu0;
            [pMu0,~] = obj.computePolarizationTensorAsMuKappa(E0,E1,nu0,nu1);
            dmu0     = -mu0*pMu0;
        end

        function dmu1 = computeDmu1(obj)
            E1  = obj.matProp.E1;
            E0  = obj.matProp.E0;
            nu1 = obj.matProp.nu1;
            nu0 = obj.matProp.nu0;
            mu1 = obj.matProp.mu1;
            [pMu1,~] = obj.computePolarizationTensorAsMuKappa(E1,E0,nu1,nu0);
            dmu1     = mu1*pMu1;
        end

        function dkappa0 = computeDKappa0(obj)
            E1     = obj.matProp.E1;
            E0     = obj.matProp.E0;
            nu1    = obj.matProp.nu1;
            nu0    = obj.matProp.nu0;
            kappa0 = obj.matProp.kappa0;
            [~,pKappa0] = obj.computePolarizationTensorAsMuKappa(E0,E1,nu0,nu1);
            dkappa0     = -kappa0*pKappa0;
        end

        function dkappa1 = computeDKappa1(obj)
            E1     = obj.matProp.E1;
            E0     = obj.matProp.E0;
            nu1    = obj.matProp.nu1;
            nu0    = obj.matProp.nu0;
            kappa1 = obj.matProp.kappa1;
            [~,pKappa1] = obj.computePolarizationTensorAsMuKappa(E1,E0,nu1,nu0);
            dkappa1     = kappa1*pKappa1;
        end

    end

    methods  (Access = protected, Static)

        function coef = compute2Dcoefficients(eMatrix,eInclusion,nuMatrix,nuInclusion)
            coef.a    = (1 + nuMatrix)/(1 - nuMatrix);
            coef.b    = (3 - nuMatrix)/(1 + nuMatrix);
            coef.gam  = eInclusion/eMatrix;
            coef.tau1 = (1 + nuInclusion)/(1 + nuMatrix);
            coef.tau2 = (1 - nuInclusion)/(1 - nuMatrix);
            coef.tau3 = (nuInclusion*(3*nuMatrix - 4) + 1)/(nuMatrix*(3*nuMatrix - 4) + 1);
        end

        function [p1,p2] = computeP(s)
            a    = s.a;
            b    = s.b;
            gam  = s.gam;
            tau1 = s.tau1;
            tau2 = s.tau2;
            tau3 = s.tau3;
            p1   = 1/(b*gam+tau1)*(1+b)*(tau1-gam);
            p2   = 0.5*(a-b)/(b*gam+tau1)*(gam*(gam-2*tau3)+tau1*tau2)/(a*gam+tau2);
        end

        function [pMu,pKappa] = computePKappaMu(p1, p2)
            pMu    = p1;
            pKappa = 2*p2 + p1;
        end

    end

end


clear, clc;

% This is an example for running the function altra
%
%  Problem:
%
%  min  1/2 || x - v||^2 + z * sum_j w_j ||x_{G_j}||
%
%  G_j's are nodes with tree structure
%
%  The tree structured group information is contained in
%  opts.ind, which is a 3 x nodes matrix, where nodes denotes the number of
%  nodes of the tree.
%
%  opts.ind(1,:) contains the starting index
%  opts.ind(2,:) contains the ending index
%  opts.ind(3,:) contains the corresponding weight (w_j)
%
%  Note:
%  1) If each element of x is a leaf node of the tree and the weight for
%  this leaf node are the same, we provide an alternative "efficient" input
%  for this kind of node, by creating a "super node" with
%  opts.ind(1,1)=-1; opts.ind(2,1)=-1; and opts.ind(3,1)=the common weight.
%
%  2) If the features are well ordered in that, the features of the left
%  tree is always less than those of the right tree, opts.ind(1,:) and
%  opts.ind(2,:) contain the "real" starting and ending indices. That is to
%  say, x( opts.ind(1,j):opts.ind(2,j) ) denotes x_{G_j}. In this case,
%  the entries in opts.ind(1:2,:) are within 1 and n.
%
%
%  If the features are not well ordered, please use the input opts.G for
%  specifying the index so that
%   x( opts.G ( opts.ind(1,j):opts.ind(2,j) ) ) denotes x_{G_j}.
%  In this case, the entries of opts.G are within 1 and n, and the entries of
%  opts.ind(1:2,:) are within 1 and length(opts.G).
%
%% Related papers
%
% [1] Jun Liu and Jieping Ye, Moreau-Yosida Regularization for
%     Grouped Tree Structure Learning, NIPS 2010
%
%%

cd ..
cd ..

root=cd;
addpath(genpath([root '/SLEP']));
                     % add the functions in the folder SLEP to the path

% change to the original folder
cd Examples/tree;

n=10000;
v=randn(n,1);
ind=[ [-1, -1, 0.5]'; [1, 3000, 0.2]'; [3001, 6000, 0.2]'; [6001, 10000, 0.2]';...
    [1, 10000, 2]'];

nodes=size(ind,2);

tic;
x=altra(v, n, ind, nodes);
toc;


function [x,fval,exitflag,info,Opt] = opti_bintprog(f,A,b,Aeq,beq,x0,opts)
%OPTI_BINTPROG Solve a BIP using an OPTI BIP Solver (Matlab Overload)
%
%   [x,fval,exitflag,info] = opti_bintprog(f,A,b,Aeq,beq,x0) solves the
%   linear program min f'x where A,b are the inequality constraints,
%   Aeq,beq are the equality constraints and x0 is the initial guess. All
%   decision variables are binary only.
%
%   [x,fval,exitflag,info] = opti_bintprog(f,...,x0,opts) allows the user
%   to specify optiset options. This includes specifying a solver via the
%   'solver' field of optiset.
%
%   [x,...,info,Opt] = opti_bintprog(f,...) returns the internally built
%   OPTI object.

%   Copyright (C) 2011 Jonathan Currie (I2C2)

% Handle missing arguments
if nargin < 7, opts = optiset; end
if nargin < 6, x0 = []; end
if nargin < 5, beq = []; end
if nargin < 4, Aeq = []; end
if nargin < 3, error('You must supply at least 3 arguments to opti_bintprog'); end

%Build OPTI Object
Opt = opti('f',f,'ineq',A,b,'eq',Aeq,beq,'x0',x0,'int',repmat('B',size(f)),'options',opts);

%Solve
[x,fval,exitflag,info] = solve(Opt);