function [K, Kc] = sdlfmXsdrbfKernCompute(sdlfmKern, sdrbfKern, t1, t2, type)

% SDLFMXSDRBFKERNCOMPUTE Cross kernel between a SDLFM and a SDRBF kernels.
% FORMAT
% DESC computes cross kernel terms between a switching dynamical
% LFM kernels and a switching dynamical RBF kernel for the multiple output
% kernel.
% ARG sdlfmKern : the kernel structure associated with the SDLFM
% kernel.
% ARG sdrbfKern : the kernel structure associated with the SDRBF
% kernel.
% ARG t : inputs for which kernel is to be computed.
% RETURN K : block of values from kernel matrix. K is a cell where the
% number of cells is related to the number of latent forces per interval
% RETURN Kc : the kernel matrices grouped in cells. Suitable form for the
% sparse approximations
%
% FORMAT
% DESC computes cross kernel terms between a SDLFM and a SDRBF kernels for
% the multiple output kernel.
% ARG sdlfmKern : the kernel structure associated with the SDLFM
% kernel.
% ARG sdrbfKern : the kernel structure associated with the SDRBF
% kernel.
% ARG t1 : row inputs for which kernel is to be computed.
% ARG t2 : column inputs for which kernel is to be computed.
% RETURN K : block of values from kernel matrix. K is a cell where the
% number of cells is related to the number of latent forces per interval
% RETURN Kc : the kernel matrices grouped in cells. Suitable form for the
% sparse approximations
%
% FORMAT
% DESC computes cross kernel terms a SDLFM and a SDRBF kernels for
% the multiple output kernel. The SDLFM kernel can correspond to Position
% (default), Velocity or Acceleration. The type of kernel to be computed
% is specified in 'type'.
% ARG sdlfmKern : the kernel structure associated with the SDLFM
% kernel.
% ARG sdrbfKern : the kernel structure associated with the SDRBF
% kernel.
% ARG t1 : row inputs for which kernel is to be computed.
% ARG t2 : column inputs for which kernel is to be computed.
% ARG type : specifies the type of kerne to be computed
% RETURN K : block of values from kernel matrix. K is a cell where the
% number of cells is related to the number of latent forces per interval
% RETURN Kc : the kernel matrices grouped in cells. Suitable form for the
% sparse approximations
%
% SEEALSO : sdlfmKernParamInit, sdlfmKernCompute, sdlfmKernParamInit
%
% COPYRIGHT : Mauricio A. Alvarez, 2010

% KERN

if nargin < 5
    type = 'Pos';
    if nargin < 4
        t2 = t1;
    end
end

if size(t1, 2) > 1 || size(t2, 2) > 1
    error('Input can only have one column');
end

% compareInverseWidth = sdlfmKern.inverseWidth == sdrbfKern.inverseWidth;
% if sum(sum(compareInverseWidth))~=(sdlfmKern.nIntervals*sdlfmKern.nlfPerInt)
%     error('Kernels cannot be cross combined if they have different inverse widths.')
% end
% compareSwitchingTimes = sdlfmKern.switchingTimes == sdrbfKern.switchingTimes;
% if sum(sum(compareSwitchingTimes))~=sdlfmKern.nIntervals
%     error('Kernels cannot be cross combined if they have different switching points.')
% end

switch type
    case 'Pos'
        fhandle = 'sdlfmXsdrbfKernComputeBlock';
    case 'Vel'
        fhandle = 'sdlfmvXsdrbfKernComputeBlock';
    case 'Accel'
        fhandle = 'sdlfmaXsdrbfKernComputeBlock';
end

fhandle = str2func(fhandle);

%Form the basic kernels
lfmKern = struct();
rbfKern = struct();

% Create structures that will make easy the computation of the kernels
%spVector = [sdlfmKern.switchingTimes t1(end)+0.1]; % For the last interval include the last point

spVector = [cumsum(sdlfmKern.switchingTimes) t1(end)+50];

dim1 = zeros(1, sdlfmKern.nIntervals); dim2 = zeros(1, sdrbfKern.nIntervals);

for i =1:sdlfmKern.nIntervals
    for j=1:sdlfmKern.nlfPerInt
        % Create the appropriate set of kernel structures
        lfmKern(i,j).mass = sdlfmKern.mass;
        lfmKern(i,j).spring = sdlfmKern.spring;
        lfmKern(i,j).damper = sdlfmKern.damper;
        lfmKern(i,j).inverseWidth = sdlfmKern.inverseWidth(j,i);
        lfmKern(i,j).sensitivity = sdlfmKern.sensitivity(j,i);
        rbfKern(i,j).variance = sdrbfKern.variance;
        rbfKern(i,j).inverseWidth = sdrbfKern.inverseWidth(j,i);
        lfmKern(i,j).limit = spVector(i+1) - spVector(i);
        rbfKern(i,j).limit = spVector(i+1) - spVector(i);
        lfmKern(i,j).isNormalised = sdlfmKern.isNormalised;
        rbfKern(i,j).isNormalised = sdrbfKern.isNormalised;
    end
    newt1 = t1(t1> spVector(i) & t1<spVector(i+1));
    newt2 = t2(t2> spVector(i) & t2<spVector(i+1));
    dim1(i) = length(newt1);
    dim2(i) = length(newt2);
end

if sum(dim1)~=length(t1) || sum(dim2)~=length(t2)
    error('A problem with the dimensions of the switching intervals occured')
end

% Compute some necessary constants

generalConst = sdlfmKernComputeConstant(sdlfmKern.nIntervals, ...
    lfmKern(1,1), lfmKern(1,1), spVector);

K = cell(sdrbfKern.nlfPerInt,1);
Kc = cell(sdrbfKern.nlfPerInt,1);

for q=1:sdrbfKern.nlfPerInt
    startValOne = 1;
    endValOne   = 0;
    tK = zeros(sum(dim1), sum(dim2));
    for i=1:sdlfmKern.nIntervals
        endValOne = endValOne + dim1(i);
        startValThree = 1;
        endValThree = 0;
        for j=1:i
            if i>j
                lfmKernLocal = lfmKern(j,q);
                rbfKernLocal = rbfKern(j,q);
            else
                lfmKernLocal = lfmKern(i,q);
                rbfKernLocal = rbfKern(i,q);
            end
            endValThree = endValThree + dim2(j);
            tK(startValOne:endValOne, startValThree:endValThree) = fhandle(lfmKernLocal, ...
                rbfKernLocal, t1(startValOne:endValOne) - spVector(i), ...
                t2(startValThree:endValThree) - spVector(j), i, j, generalConst);
            startValThree = endValThree + 1;
        end
        startValOne = endValOne + 1;
    end
    tK = real(tK);
    K{q} = tK;
end


function [r_j] = calcResidual(p_f_G, camStates, measurements)
%CALCRESIDUAL Calculates the residual for a feature position

% measurements is 2 x M_j
% camStates is a cell array of the camState structs for the states
%   included in measurements
    r_j = NaN(2*size(camStates,2), 1);
    for i = 1:size(camStates,2)

        C_CG = quatToRotMat(camStates{i}.q_CG);
        p_f_C = C_CG * (p_f_G - camStates{i}.p_C_G);

        zhat_i_j = p_f_C(1:2)/p_f_C(3);

        iStart = 2*(i-1)+1;
        iEnd = 2*i;
        r_j(iStart:iEnd) = measurements(:,i) - zhat_i_j;
    end

end


% LOTKA-VOLTERRA system

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

%% Load Models
InputSignalTypeModel = 'sphs'; % prbs; chirp; noise; sine2; sphs; mixed

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

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

% NARX
load(fullfile(datapath,['EX_LOTKA_SI_NARX','_',InputSignalTypeModel,'.mat'])) % Model
Models.NARX = Model;

%% Get training data
InputSignalType = InputSignalTypeModel;% prbs; chirp; noise; sine2; sphs; mixed
Ndelay = Models.DelayDMDc.Ndelay;
ONLY_TRAINING_LENGTH = 1;
getTrainingData

%% Get validation data
InputSignalType = 'sine2';% prbs; chirp; noise; sine2; sphs; mixed
Ndelay = Models.DelayDMDc.Ndelay;
getValidationData

%% FIGURE 1:  // Model comparison + Validation

% Reference
[tA,xA] = ode45(@(t,x)lotkacontrol(t,x,forcing(x,t),a,b,d,g),tspanV,x(end,1:2),options);   % true model

% DelayDMDc / DMDc
% Model
if Ndelay == 1
    x0      = [x(end,1:2)];
    Hunew   = [u(end),u_valid(1:end-1)];
    [xB,tB] = lsim(Models.DelayDMDc.sys,Hunew,tspanV,[x0-[xmean]]');
elseif Ndelay > 1
    x0      = [x(end-Ndelay+1,1:2),x(end,1:2)];
    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-[xmean]]');
    xB = xB(:,3:4); xB = xB + repmat(xmean,[size(xB,1) 1]);
end
xB = xB + repmat(xmean,[length(tB) 1]);

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

% NARX
Udummy = [u(end),u_valid(1:end)];
Hdummy = zeros(2,size(Udummy,2));
Hdummy(:,1) = x(end,1:2)';
[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,:)];

%%
clear ph
h = figure;
subplot(2,1,1), box on, hold on
plot([tB(1) tB(1)],[0 150],'--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 150])
% xlabel('Time','FontSize',13)
ylabel('Prey, x_1','FontSize',13)
set(gca,'FontSize',13)
subplot(2,1,2), box on, hold on
plot([tB(1) tB(1)],[0 60],'--k')
ph(1) = plot(t,x(:,2),'Color',[.4 .4 .4],'LineWidth',1.5);
ph(2) = plot(tA,xA(:,2),'k','LineWidth',1.5);
ph(3) = plot(tB,xB(:,2),'r-','LineWidth',1.5);
ph(4) = plot(tC,xC(:,2),'g--','LineWidth',1.5);
% ph(5) = plot(tB,xD(:,2),'-.','Color',[0.7,0.7,1],'LineWidth',1.5);
ph(5) = plot(tA(1:end),xD(:,2),'-.','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 60])
ylabel('Predator, x_2','FontSize',13)
set(gca,'FontSize',13)
xlabel('Time','FontSize',13)
set(gca,'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_LOTKA_SI_Comparison_Validation_',InputSignalTypeModel,'_',InputSignalType,'.eps'],'-r0');

%% Actuation signal
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 200])
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_LOTKA_SI_Comparison_Validation_',InputSignalTypeModel,'_',InputSignalType,'_Actuation.eps']);

%% MPC
% Parameters MPC
options = optimoptions('fmincon','Algorithm','sqp','Display','none', ...
    'MaxIterations',100);
N  = 10;                        % Control / prediction horizon (number of iterations)
Duration = 100;                 % Run for 'Duration' time units
Ton = 0;                        % Time units when control is turned on
Nvar = 2;
getMPCparams

x0n=xA(end,:)';                 % Initial condition
xref1 = [g/d;a/b];              % critical point
Tcontrol = tA(end);             % Time offset to combine training, prediction and control phase

% Parameters Models
ModelCollection = {ModelTypeDMDc, 'SINDYc', 'NARX'};
Nmodels = length(ModelCollection);

% Parameters True Model
p.a = a;
p.b = b;
p.d = d;
p.g = g;

%% Load Results
clear Results
load(fullfile(datapath,['MPC_Results_AllModels_',InputSignalTypeModel,'_',InputSignalType,'.mat']),'Results')

%% Unforced
[tU,xU] = ode45(@(t,x)lotkacontrol(t,x,0,a,b,d,g),Results(1).t,xA(end,1:2),options);   % true model

%% Show results
MODEL_COLOR = 1;
Ts = Models.DelayDMDc.dt;
ct = (Duration/Ts);
for iM = 1:Nmodels
    select_model = ModelCollection{iM};
    xHistory = Results(iM).x;
    uHistory = Results(iM).u;
    tHistory = Results(iM).t;
    VIZ_SI_Validation_MPC
end

%% Get cost // evaluate performance
% Data2 = Datal
Costs = zeros(length(Results(1).u),3,Nmodels);

for iM = 1:Nmodels
    [J] = evalObjectiveFCN(Results(iM).u,Results(iM).x,Results(iM).xref,diag(Q),R,Ru);
    Costs(:,1,iM) = J; %Costs(:,2,iM) = Js; Costs(:,3,iM) = Ju;
end

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

xlim([200 xlimval(2)])
l1=legend(ph,ModelCollection);
set(l1,'Location','SouthEast')
% ylim([0 5.5*10^4])
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_LOTKA_SI_Comparison_Validation_',InputSignalTypeModel,'_',InputSignalType,'_Cost.eps']);

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

xlim([200 xlimval(2)])
l1=legend(ph,ModelCollection);
set(l1,'Location','SouthEast')
% ylim([0 5.5*10^4])
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')
%%
iJ = 3;
clear ph
figure, hold on, box on
ph(1) = plot(Results(1).t,cumsum(Costs(:,iJ,1)),'-r','LineWidth',2);
ph(2) = plot(Results(1).t,cumsum(Costs(:,iJ,2)),'--g','LineWidth',2);
ph(3) = plot(Results(1).t,cumsum(Costs(:,iJ,3)),'-.','Color',[0.7,0.7,1],'LineWidth',2);

l1=legend(ph,ModelCollection);
set(l1,'Location','NorthEast')
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')


function prior = priorTest(priorType);

% PRIORTEST Run some tests on the specified prior.
% FORMAT
% DESC runs some checks on the specified prior (gradients etc.).
% ARG priorType : the prior type to check.
% RETURN prior : the prior structure that was created to test.
%
% SEEALSO : priorParamInit, priorExpandParam, priorExtractParam,
% priorLogProb, priorGradient
%
% COPYRIGHT : Neil D. Lawrence, 2003, 2004
%
% MODIFICATIONS : Antti Honkela, 2012, 2014

% PRIOR

prior.type = priorType;
prior = priorParamInit(prior);

% Set the parameters randomly.
params = priorExtractParam(prior);
params = randn(size(params))./sqrt(randn(size(params)).^2);
prior = priorExpandParam(prior, params);
if isfield(prior, 'isBounded') && prior.isBounded,
  prior.b = 1+9*rand(1);
  x = rand(1, 10) * (prior.b - prior.a) + prior.a;
else
  x = randn(1, 10);
end
epsilon = 1e-6;

if exist([priorType 'PriorGradientParams'])
  params = priorExtractParam(prior);
  origParams = params;
  for i = 1:length(params);
    params = origParams;
    params(i) = origParams(i) + epsilon;
    prior = priorExpandParam(prior, params);
    Lplus(i) = priorLogProb(prior, x);
    params(i) = origParams(i) - epsilon;
    prior = priorExpandParam(prior, params);
    Lminus(i) = priorLogProb(prior, x);
  end
  params = origParams;
  prior = priorExpandParam(prior, params);
  [void, names] = priorExtractParam(prior);
  gLDiff = .5*(Lplus - Lminus)/epsilon;
  g = priorGradient(prior, x);

  paramMaxDiff = max(max(abs(gLDiff-g)));
  if paramMaxDiff > 2*epsilon
    l = 0;
    for i = 1:length(names)
      if l < length(names{i})
        l = length(names{i});
      end
    end

    fprintf([char(repmat(32, 1, l)) '\tanalytic   diffs     delta\n']);
    for i = 1:length(names)
      spaceLen = l - length(names{i});
      space = char(repmat(32, 1, spaceLen));
      fprintf([space names{i} ':\t%4.6f\t%4.6f\t%4.6f\n'], ...
              g(i), gLDiff(i), gLDiff(i) - g(i));
    end
  end
end
if exist([priorType 'PriorGradient'])
  origX = x;
  for i = 1:length(x);
    x = origX;
    x(i) = origX(i) + epsilon;
    Lplus(i) = priorLogProb(prior, x);
    x(i) = origX(i) - epsilon;
    Lminus(i) = priorLogProb(prior, x);
  end
  x = origX;
  gLDiff = .5*(Lplus - Lminus)/epsilon;
  g = priorGradient(prior, x);

  paramMaxDiff = max(max(abs(gLDiff-g)));
%  if paramMaxDiff > 2*epsilon

    fprintf([ '\tanalytic   diffs     delta\n']);
    for i = 1:length(x)
      fprintf([num2str(i) ':\t%4.6f\t%4.6f\t%4.6f\n'], ...
              g(i), gLDiff(i), gLDiff(i) - g(i));
    end
%  end
end


function [ishappy, cutoff] = strictCheck(f, values, data, pref)
%STRICTCHECK   Attempt to trim trailing Chebyshev coefficients in a
%   CHEBTECH. [ISHAPPY, CUTOFF] = STRICTCHECK(F, VALUES, DATA) returns an
%   estimated location CUTOFF at which the CHEBTECH F could be truncated to
%   maintain an accuracy of the default CHEBTECH CHEBFUNEPS preference
%   relative to DATA.VSCALE and DATA.HSCALE. ISHAPPY is TRUE if
%   CUTOFF < MIN(LENGTH(F.COEFFS), 2) or VSCALE(F)=0, and FALSE otherwise.
%
%   [ISHAPPY, CUTOFF] = STRICTCHECK(F, VALUES, DATA, PREF) allows
%   additional preferences to be passed. In particular, one can adjust the
%   target accuracy with PREF.CHEBFUNEPS. The VALUES field is ignored, but
%   included for consistency with other happiness checks.
%
%   STRICTCHECK tests to see if the absolute values of the entries in the
%   tail of coeffs, i.e., f.coeffs(1:TESTLENGTH,:), where
%       TESTLENGTH = n,             for n = 1:4
%       TESTLENGTH = 5,             for n = 5:44
%       TESTLENGTH = round((n-1)/8) for n > 44
%   all lie below the value in PREF.CHEBFUNEPS.  CUTOFF is the location of
%   the first entry above PREF.CHEBFUNEPS in absolute value.
%
%   STRICKCHECK differs from CLASSICCHECK() in that the tolerance
%   PREF.CHEBFUNEPS is not relaxed by the length of the representation of F
%   or by any finite difference approximation of the gradient of F.
%
% See also STRICTCHECK, LOOSECHECK.

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

% Deal with special cases ------------------------------------------------------

% Determine n (the length of the input):
n = length(f);

% Assume we're not happy:
ishappy = false;

% Grab some preferences:
if ( nargin == 1 )
    pref = f.techPref();
    epslevel = pref.chebfuneps;
elseif ( isnumeric(pref) )
    epslevel = pref;
else
    epslevel = pref.chebfuneps;
end

% Convert scalar epslevel/tolerance inputs into vectors.
if ( isscalar(epslevel) )
    epslevel = repmat(epslevel, 1, size(f.coeffs, 2));
end

% Deal with the trivial case:
if ( n < 2 )  % (Can't be simpler than a constant.)
    cutoff = n;
    return
end

if ( max(data.vscale) == 0 )
    % This is the zero function, so we must be happy.
    ishappy = true;
    cutoff = 1;
    return
elseif ( any(isinf(data.vscale(:))) )
    % Inf located. No cutoff.
    cutoff = n;
    return
end

% NaNs are not allowed.
if ( any(isnan(f.coeffs(:))) )
    error('CHEBFUN:CHEBTECH:strictCheck:nanEval', ...
        'Function returned NaN when evaluated.')
end

% Check for convergence and chop location --------------------------------------
testLength = min(n, max(5, round((n-1)/8)));
ac = bsxfun(@rdivide, abs(f.coeffs), data.vscale);
f.coeffs(bsxfun(@le, ac, epslevel)) = 0;
tail = f.coeffs(end-testLength+1:end,:);
if ( ~any(tail(:)) )
    cutoff = find(max(f.coeffs, [], 2) > 0, 1, 'last');
    ishappy = true;
else
    cutoff = n;
end

end


function [ abd, ipvt, info ] = zgbfa ( abd, lda, n, ml, mu )

%*****************************************************************************80
%
%% ZGBFA factors a complex band matrix by elimination.
%
%  Discussion:
%
%    ZGBFA is usually called by ZGBCO, but it can be called
%    directly with a saving in time if RCOND is not needed.
%
%  Band storage:
%
%    If A is a band matrix, the following program segment
%    will set up the input.
%
%      ml = (band width below the diagonal)
%      mu = (band width above the diagonal)
%      m = ml + mu + 1
%      do j = 1, n
%        i1 = max ( 1, j - mu )
%        i2 = min ( n, j + ml )
%        do i = i1, i2
%          k = i - j + m
%          abd(k,j) = a(i,j)
%        end do
%      end do
%
%    This uses rows ML+1 through 2*ML+MU+1 of ABD.
%    In addition, the first ML rows in ABD are used for
%    elements generated during the triangularization.
%    The total number of rows needed in ABD is 2*ML+MU+1.
%    The ML+MU by ML+MU upper left triangle and the
%    ML by ML lower right triangle are not referenced.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 April 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%
%  Parameters:
%
%    Input, complex ABD(LDA,N), the matrix in band storage.  The columns of
%    the matrix are stored in the columns of ABD and the diagonals of the
%    matrix are stored in rows ML+1 through 2*ML+MU+1 of ABD.
%
%    Input, integer LDA, the leading dimension of ABD.
%    LDA must be at least 2*ML+MU+1.
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer ML, the number of diagonals below the main diagonal.
%    0 <= ML < N.
%
%    Input, integer MU, the number of diagonals above the main diagonal.
%    0 <= MU < N.  More efficient if ML <= MU.
%
%    Output, complex ABD(LDA,N), an upper triangular matrix
%    in band storage and the multipliers which were used to obtain it.
%    The factorization can be written A = L*U where L is a product of
%    permutation and unit lower triangular matrices and U is upper triangular.
%
%    Output, integer IPVT(N), the pivot indices.
%
%    Output, integer INFO.
%    0, normal value.
%    K, if U(K,K) == 0.0.  This is not an error condition for this
%    subroutine, but it does indicate that ZGBSL will divide by zero if
%    called.  Use RCOND in ZGBCO for a reliable indication of singularity.
%
  m = ml + mu + 1;
  info = 0;
%
%  Zero initial fill-in columns.
%
  j0 = mu + 2;
  j1 = min ( n, m ) - 1;

  for jz = j0 : j1
    i0 = m + 1 - jz;
    for i = i0 : ml
      abd(i,jz) = 0.0;
    end
  end

  jz = j1;
  ju = 0;
%
%  Gaussian elimination with partial pivoting.
%
  for k = 1 : n-1
%
%  Zero next fill-in column
%
    jz = jz + 1;
    if ( jz <= n )
      abd(1:ml,jz) = 0.0;
    end
%
%  Find L = pivot index.
%
    lm = min ( ml, n - k );
    l = izamax ( lm+1, abd(m:m+lm,k), 1 ) + m - 1;
    ipvt(k) = l + k - m;
%
%  Zero pivot implies this column already triangularized.
%
    if ( zabs1 ( abd(l,k) ) == 0.0 )
      info = k;
      continue
    end
%
%  Interchange if necessary.
%
    if ( l ~= m )
      t        = abd(l,k);
      abd(l,k) = abd(m,k);
      abd(m,k) = t;
    end
%
%  Compute multipliers.
%
    t = - 1.0 / abd(m,k);
    abd(m+1:m+lm,k) = abd(m+1:m+lm,k) * t;
%
%  Row elimination with column indexing.
%
    ju = min ( max ( ju, mu + ipvt(k) ), n );
    mm = m;

    for j = k+1 : ju
      l = l - 1;
      mm = mm - 1;
      t = abd(l,j);
      if ( l ~= mm )
        abd(l,j) = abd(mm,j);
        abd(mm,j) = t;
      end
      abd(mm+1:mm+lm,j) = abd(mm+1:mm+lm,j) + t * abd(m+1:m+lm,k);
    end

  end

  ipvt(n) = n;

  if ( zabs1 ( abd(m,n) ) == 0.0 )
    info = n;
  end

  return
end


function D = partsof(D,name,parts,thresh)
% Finds the parts of an object and modifies the database structure D so
% that parts point to the object to which it belongs.  To find the parts
% of a "car", do the following:
%
% D = partsof(D,'car');
%
% If object j in image i is a part of a car, then
% D(i).annotation.object(j).partof will be set to the ID of the car to
% which it belongs in the image.

if nargin < 4
    thresh = 0.5;
end

[~,jj] = LMquery(D,'object.name',[name ',' parts]);

for ii = 1:length(jj)
    jo = LMobjectindex(D(jj(ii)).annotation,name);
    jp = LMobjectindex(D(jj(ii)).annotation,parts);
    M = zeros(length(jo),length(jp));
    for kk = 1:length(jo)
        for ll = 1:length(jp)
            M(kk,ll) = 0;
            [XXo,YYo] = getLMpolygon(D(jj(ii)).annotation.object(jo(kk)).polygon);
            [XXp,YYp] = getLMpolygon(D(jj(ii)).annotation.object(jp(ll)).polygon);

            ro = [min(XXo) min(YYo) max(XXo) max(YYo)];
            rp = [min(XXp) min(YYp) max(XXp) max(YYp)];

            if rect_intersect(ro,rp) >= thresh

                Ao = polyarea(XXo,YYo);
                Ap = polyarea(XXp,YYp);
                if Ao > Ap
                    M(kk,ll) = int_area(XXo,YYo,XXp,YYp)/Ap;
                end
            end
        end
    end
    [vals,inds] = max(M,[],1);

    %
    %
    for kk = 1:length(inds)
        if vals(kk) >= thresh
            [ids,jj_id] = getID(D(jj(ii)).annotation);
            id_ind = find(jj_id==jo(inds(kk)));

            object_id = ids(id_ind);
            part_id =
            D(jj(ii)).annotation = addpart_i_to_object_j(D(jj(ii)).annotation, ids(id_ind), j);

            if ~isempty(id_ind)
                D(jj(ii)).annotation.object(jp(kk)).partof = num2str(ids(id_ind));
            elseif ~isempty(ids)
                D(jj(ii)).annotation.object(jo(inds(kk))).id = num2str(max(ids)+1);
                D(jj(ii)).annotation.object(jp(kk)).partof = num2str(max(ids)+1);
            else
                D(jj(ii)).annotation.object(jo(inds(kk))).id = '0';
                D(jj(ii)).annotation.object(jp(kk)).partof = '0';
            end
            D(jj(ii)).annotation.object(jp(kk)).partofobject = ...
                D(jj(ii)).annotation.object(jo(inds(kk))).name;
        end
    end
end

function annotation = addpart_i_to_object_j(annotation,i,j)

annotation.object(j).parts.hasparts = i;
annotation.object(i).parts.ispartof = j;

function [v,j] = getID(annotation)

if isfield(annotation,'object') && isfield(annotation.object(1),'id')
    v = str2double({annotation.object(:).id});
    j = find(~isnan(v));
    v = v(j);
    if length(v) ~= length(unique(v))
        disp('WARNING: getID(): There are duplicate IDs!');
    end
else
    v = [];
    j = [];
end

function tt = rect_intersect(ro,rp)
% min_x,min_y,max_x,max_y

tt = 0;
wo = ro(3)-ro(1);
ho = ro(4)-ro(2);
wp = rp(3)-rp(1);
hp = rp(4)-rp(2);
if (wp*hp) > 0
    tt = rectint([ro(1) ro(2) wo ho],[rp(1) rp(2) wp hp]) / (wp*hp);
end


function test_bug1529

% MEM 2gb
% WALLTIME 00:10:00
% DEPENDENCY ft_plot_ortho

[ftver, ftpath] = ft_version;

%% Load the SPMtemplate
mritem = ft_read_mri([ftpath '/external/spm8/templates/T1.nii']);
% mrichecked = ft_determine_coordsys(mritem);
mritem.coordsys = 'spm'; % define the coordinate system (why is this not part of ft_read_mri?)

%% Display the Anatomy via plot_ortho (defaults)
figure('name','MRI TEM');
ft_plot_ortho(mritem.anatomy);

%% Display the Anatomy via plot_ortho (at a certain location )
location_voxel = [46 64 37];
location_str   = sprintf('x: %.0f y: %.0f z:%.0f',location_voxel(1:3));
figure('name',['MRI TEM VOX' location_str ]);
ft_plot_ortho(mritem.anatomy,'location',location_voxel(1:3));

%% Display the Anatomy via plot_ortho (at the same certain location, using external transform )
location_mri = [0 0 0];
location_voxel = mritem.transform\[location_mri 1]';
location_str   = sprintf('x: %.0f y: %.0f z:%.0f',location_voxel(1:3));
figure('name',['MRI TEM MRI EXTERN ' location_str]);
ft_plot_ortho(mritem.anatomy,'location',location_voxel(1:3));

%% Display the Anatomy via plot_ortho (at the same certain location, using internal transform )
location_mri = [0 0 0];
location_str   = sprintf('x: %.0f y: %.0f z:%.0f',location_mri(1:3));
figure('name',['MRI TEM MRI INTERN ' location_str]);
ft_plot_ortho(mritem.anatomy,'location',location_mri(1:3),'transform',mritem.transform);

%% Display the Anatomy via plot_ortho (at the same certain location, using inverse internal transform )
location_mri = [0 0 0];
location_str   = sprintf('x: %.0f y: %.0f z:%.0f',location_mri(1:3));
figure('name',['MRI TEM MRI INTERN ' location_str]);
ft_plot_ortho(mritem.anatomy,'location',location_mri(1:3),'transform',inv(mritem.transform));


function L = pureShear(exp,comp,e)
% defines velocityGradientTensor representing pure shear
%
% Syntax
%   L = velocityGradientTensor.uniaxialCompression(exp,comp)
%
% Input
%  exp  - @vector3d expansion direction
%  comp - @vector3d compression direction
%  e - strain rate
%
% Output
%  L - @velocityGradientTensor
%

if nargin == 2, e = 1; end

v1 = normalize(exp - comp);
v2 = normalize(exp + comp);

L = 2*velocityGradientTensor(e .* (dyad(v1,v2) + dyad(v1,v2)'));

end


function [ t_start, y_start ] = p17_start ( neqn )

%*****************************************************************************80
%
%% P17_START returns the starting point for problem p17.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 February 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NEQN, the number of equations.
%
%    Output, real T_START, Y_START(NEQN,1), the initial data.
%
  y_start = zeros ( neqn, 1 );

  t_start = 0.0;
  delta = p17_param ( 'GET', 'DELTA', [] );
  y_start(1) = 1.0 - delta;
  y_start(2) = 0.0;
  y_start(3) = 0.0;
  y_start(4) = sqrt ( ( 1.0 + delta ) / ( 1.0 - delta ) );

  return
end


  function ob = block_fatrix(blocks, varargin)
%|function ob = block_fatrix(blocks, options)
%|
%| Construct block_fatrix object, a meta-Fatrix composed of Fatrix blocks,
%| such as block_diag(A_1, A_2, ..., A_M)
%| See block_fatrix_test.m for example usage.
%|
%| in
%|	blocks	{cell}	cell array of the blocks
%|
%| options
%|	'type'	char	options:
%|				'diag' for block diagonal (default)
%|				'col' for [A_1; A_2; ...; A_M]
%|				'kron' for kron(eye(Mkron), blocks{1})
%|				'row' for [A1, A2, ..., A_M]
%|				'sum' for A1 + A2 + ... + A_M
%|	'Mkron'	int	required for 'kron' type
%|	'tomo'	0|1	special support for tomography-type objects (default: 0)
%|
%| out
%|	ob [nd np]	diag,row: np = sum_m ncol(A_m)
%|
%| Copyright 05-5-12, Jeff Fessler, University of Michigan

if nargin < 1, ir_usage, end
if nargin == 1 && streq(blocks, 'test')
	run_mfile_local block_fatrix_test
return
end

arg.blocks = blocks;

% defaults
arg.type = 'diag';
arg.chat = 0;
arg.tomo = 0;
arg.Mkron = [];

% options
arg = vararg_pair(arg, varargin);

% initialize
if ~iscell(blocks)
	fail('blocks must be cell array, not "%s"', class(blocks))
end

switch arg.type
case 'col'
	ob = block_fatrix_col(blocks, arg);
case 'diag'
	ob = block_fatrix_diag(blocks, arg);
case 'kron'
	ob = block_fatrix_kron(blocks, arg);
case 'row'
	ob = block_fatrix_row(blocks, arg);
case 'sum'
	ob = block_fatrix_sum(blocks, arg);
otherwise
	fail('unknown block type "%s"', arg.type)
end

%
% block_fatrix_col()
%
function ob = block_fatrix_col(blocks, arg)

MM = length(blocks);
arg.dims = zeros(MM, 2);
for ii=1:MM
	arg.dims(ii,:) = size(blocks{ii});
	if arg.dims(ii,2) ~= arg.dims(1,2)
		error 'all blocks must have same #cols for "col"'
	end
end
% start/end indices for selecting parts of x and y
arg.istart = cumsum([1; arg.dims(1:end-1,1)]);
arg.iend = arg.istart + arg.dims(:,1) - 1;

arg.dim = [sum(arg.dims(:,1)) arg.dims(1,2)];

if arg.tomo % prep for mat2cell later
	for ii=1:MM
		odim = arg.blocks{ii}.arg.odim;
		arg.tomo_ndim = length(odim);
		arg.tomo_dim = arg.tomo_ndim; % insist on last dimension
		if ii==1
			for id = 1:arg.tomo_ndim
				if id ~= arg.tomo_dim
					arg.mat2cell_arg{id} = odim(id);
				end
			end
		end
		arg.mat2cell_arg{arg.tomo_dim}(ii) = odim(arg.tomo_dim);
	end
end

% build Fatrix object
ob = Fatrix(arg.dim, arg, 'caller', 'block_fatrix(col)', ...
	'forw', @block_fatrix_col_forw, 'back', @block_fatrix_col_back, ...
	'free', @block_fatrix_free, 'gram', @block_fatrix_col_gram);

%
% block_fatrix_col_forw(): y = A * x
%
function y = block_fatrix_col_forw(arg, x)

MM = length(arg.blocks);
y = cell(MM,1);
for ii=1:MM
	y{ii} = arg.blocks{ii} * x;
end

if ~arg.tomo || ncol(y{1}) == 1 ...
	|| (arg.tomo && arg.dim(2) == size(x,1)) % [np (L)]
	y = cat(1, y{:});
else % handle 'tomo' case where x is not a single column
	y = cat(arg.tomo_dim, y{:});
end

%
% block_fatrix_col_back(): x = A' * y
%
function x = block_fatrix_col_back(arg, y)

MM = length(arg.blocks);
if arg.tomo
	if arg.dim(1) == size(y,1) % [nd (L)]
		arg.tomo = 0;
	else % [(odim) (L)] split into separate parts, undoing cat(?, y{:})
		tmp = size(y);
		if length(tmp) == arg.tomo_ndim
			yc = mat2cell(y, arg.mat2cell_arg{:});
		elseif length(tmp) > arg.tomo_ndim
			yc = mat2cell(y, arg.mat2cell_arg{:}, ...
				tmp((arg.tomo_ndim+1):end)); % handle (L)
		else
			error 'bug'
		end
	end
end

x = 0;
for ii=1:MM
	if ~arg.tomo % [nd (L)]
		jj = arg.istart(ii):arg.iend(ii);
		yi = y(jj,:);
	else
		yi = yc{ii};
	end
	t = arg.blocks{ii}' * yi;
	x = x + t;
end

%
% block_fatrix_col_gram()
%
function [T, reuse] = block_fatrix_col_gram(ob, W, reuse, varargin)

blocks = ob.arg.blocks;
T = cell(size(blocks));
for ii=1:length(blocks)
	A = blocks{ii};
	if isnumeric(A)
		if isempty(W)
			T{ii} = A' * A;
		else
			warn 'todo: this may not work, need piece of W!'
			T{ii} = A' * W * A;
		end
	else
		if isempty(W)
			T{ii} = build_gram(A, [], reuse, varargin{:});
		else
			if isvar('W.arg.blocks{ii}')
				T{ii} = build_gram(A, W.arg.blocks{ii}, ...
					reuse, varargin{:});
			else
				fail('block_fatrix_col_gram needs block diag W')
			end
		end
	end
end
T = fatrix_plus(T{:});

%
% block_fatrix_row()
% trick: just reuse col via transpose!
%
function ob = block_fatrix_row(blocks, arg)

for ii=1:length(blocks)
	blocks{ii} = blocks{ii}'; % trick: transpose
end

ob = block_fatrix(blocks, 'type', 'col')'; % trick: transpose

%
% block_fatrix_diag()
%
function ob = block_fatrix_diag(blocks, arg)

arg.dims = zeros(length(blocks), 2);
for ii=1:length(blocks)
	arg.dims(ii,:) = size(blocks{ii});
end
% start/end indices for selecting parts of x and y
arg.istart = cumsum([1; arg.dims(1:end-1,1)]);
arg.iend = arg.istart + arg.dims(:,1) - 1;
arg.jstart = cumsum([1; arg.dims(1:end-1,2)]);
arg.jend = arg.jstart + arg.dims(:,2) - 1;

arg.dim = sum(arg.dims, 1);

% build Fatrix object
ob = Fatrix(arg.dim, arg, 'caller', 'block_fatrix(diag)', ...
	'forw', @block_fatrix_diag_forw, 'back', @block_fatrix_diag_back, ...
	'free', @block_fatrix_free, 'gram', @block_fatrix_diag_gram, ...
	'mtimes_block', @block_fatrix_diag_mtimes_block);

%
% block_fatrix_diag_forw(): y = A * x
%
function y = block_fatrix_diag_forw(arg, x)

if nrow(x) ~= arg.dim(2)
	error('x size=%d vs dim(2)=%d', nrow(x), arg.dim(2))
end
y = [];
for ii=1:length(arg.blocks)
	t = arg.blocks{ii} * x([arg.jstart(ii):arg.jend(ii)], :);
	y = [y; t];
end

%
% block_fatrix_diag_back(): x = A' * y
% full backprojection
%
function x = block_fatrix_diag_back(arg, y)

if nrow(y) ~= arg.dim(1), error 'bad y size', end
x = [];
for ii=1:length(arg.blocks)
	t = arg.blocks{ii}' * y([arg.istart(ii):arg.iend(ii)], :);
	x = [x; t];
end

%
% block_fatrix_diag_mtimes_block()
% caution: it is quite unclear whether these are useful!
%
function y = block_fatrix_diag_mtimes_block(arg, is_transpose, x, istart, nblock)
if is_transpose
	y = block_fatrix_diag_block_back(arg, x, istart, nblock);
else
	y = block_fatrix_diag_block_forw(arg, x, istart, nblock);
end

%
% block_fatrix_diag_block_forw()
%
function y = block_fatrix_diag_block_forw(arg, x, istart, nblock)

if nrow(x) ~= arg.dim(2)
	error('x size=%d vs dim(2)=%d', nrow(x), arg.dim(2))
end
y = [];
for ii=istart:nblock:length(arg.blocks)
	t = arg.blocks{ii} * x([arg.jstart(ii):arg.jend(ii)], :);
	y = [y; t];
end

%
% block_fatrix_diag_block_back()
%
function x = block_fatrix_diag_block_back(arg, y, istart, nblock)

if nrow(y) ~= arg.dim(1), error 'bad y size', end
x = [];
for ii=istart:nblock:length(arg.blocks)
	t = arg.blocks{ii}' * y([arg.istart(ii):arg.iend(ii)], :);
	x = [x; t];
end

%
% block_fatrix_diag_gram()
%
function [T, reuse] = block_fatrix_diag_gram(ob, W, reuse, varargin)

blocks = ob.arg.blocks;
T = cell(size(blocks));
for ii=1:length(blocks)
	A = blocks{ii};
	if isnumeric(A)
		if isempty(W)
			T{ii} = A' * A;
		else
			T{ii} = A' * W * A;
		end
	else
		T{ii} = build_gram(A, W, reuse, varargin{:});
	end
end
T = block_fatrix(T, 'type', 'diag');

%
% block_fatrix_kron()
%
function ob = block_fatrix_kron(blocks, arg)

if isempty(arg.Mkron), error 'Mkron required', end
if length(blocks) ~= 1, error 'kron expects exactly one block', end

arg.dims = repmat(size(blocks{1}), [arg.Mkron 1]);
% start/end indices for selecting parts of x and y
arg.istart = cumsum([1; arg.dims(1:end-1,1)]);
arg.iend = arg.istart + arg.dims(:,1) - 1;
arg.jstart = cumsum([1; arg.dims(1:end-1,2)]);
arg.jend = arg.jstart + arg.dims(:,2) - 1;
arg.dim = sum(arg.dims,1);

% build Fatrix object
if isa(blocks{1}, 'Fatrix')
	tmp = ['F:' blocks{1}.caller];
else
	tmp = class(blocks{1});
end
tmp = sprintf('block_fatrix(kron, %s)', tmp);
ob = Fatrix(arg.dim, arg, 'caller', tmp, ...
	'forw', @block_fatrix_kron_forw, 'back', @block_fatrix_kron_back, ...
	'block_setup', @block_fatrix_kron_block_setup, ...
	'mtimes_block', @block_fatrix_kron_mtimes_block, ...
	'free', @block_fatrix_free);

%
% block_fatrix_kron_forw(): y = A * x
%
function y = block_fatrix_kron_forw(arg, x)

if nrow(x) ~= arg.dim(2)
	fail('x size=%d vs dim(2)=%d', nrow(x), arg.dim(2))
end
y = [];
for ii=1:arg.Mkron
	t = arg.blocks{1} * x([arg.jstart(ii):arg.jend(ii)], :);
	y = [y; t];
end

%
% block_fatrix_kron_back(): x = A' * y
% full backprojection
%
function x = block_fatrix_kron_back(arg, y)

if nrow(y) ~= arg.dim(1), error 'bad y size', end
x = [];
for ii=1:arg.Mkron
	t = arg.blocks{1}' * y([arg.istart(ii):arg.iend(ii)], :);
	x = [x; t];
end

%
% block_fatrix_kron_block_setup()
% apply Gblock to the base block, to prepare it for later
%
function ob = block_fatrix_kron_block_setup(ob, varargin)
ob.arg.blocks = {Gblock(ob.arg.blocks{1}, ob.nblock)};
% todo: probably need to set up some other internal variables here
% for use inside block_fatrix_kron_mtimes_block()

%
% block_fatrix_kron_mtimes_block(): y = A{ib} * x
%
function y = block_fatrix_kron_mtimes_block(arg, is_transpose, x, iblock, nblock)

bl1 = arg.blocks{1}; % base block, already put through Gblock
warn 'todo: size check not done'

if ~is_transpose % forw
%	if nrow(x) ~= arg.dim(2)
%		fail('x size=%d vs dim(2)=%d', nrow(x), arg.dim(2))
%	end
	y = [];
	for ii=1:arg.Mkron
		t = bl1{iblock} * x([arg.jstart(ii):arg.jend(ii)], :);
		y = [y; t];
	end

else % back
	y = x;
%	if nrow(y) ~= arg.dim(1), error 'bad y size', end
	x = [];
	for ii=1:arg.Mkron
		tmp = [arg.istart(ii):arg.iend(ii)]; % i list (if all data)
		% todo: we need a certain subset of that list
		fail('todo: kron subset backprojector not done')
		t = bl1{iblock}' * y(tmp, :);
		x = [x; t];
	end
	y = x;
end

%
% block_fatrix_sum()
%
function ob = block_fatrix_sum(blocks, arg)

dim = size(blocks{1});
for ii=1:length(blocks)
	if ~isequal(size(blocks{ii}), dim)
		error 'blocks must have same size for "sum"'
	end
end

% build Fatrix object
ob = Fatrix(dim, arg, 'caller', 'block_fatrix(sum)', ...
	'forw', @block_fatrix_sum_forw, 'back', @block_fatrix_sum_back, ...
	'free', @block_fatrix_free);

%
% block_fatrix_sum_forw(): y = A * x
%
function y = block_fatrix_sum_forw(arg, x)

y = 0;
for ii=1:length(arg.blocks)
	y = y + arg.blocks{ii} * x;
end

%
% block_fatrix_sum_back(): x = A' * y
% full backprojection
%
function x = block_fatrix_sum_back(arg, y)

x = 0;
for ii=1:length(arg.blocks)
	x = x + arg.blocks{ii}' * y;
end

%
% block_fatrix_free()
%
function block_fatrix_free(arg)
if arg.chat
	printm 'freeing block_fatrix object static memory'
end
for ii=1:length(arg.blocks)
	try
		free(blocks{ii})
	catch
	end
end

%
% block_fatrix_update()
%
function out = block_fatrix_update(ob, varargin)
% todo: figure out how to update, e.g., new_zmap(s), ...


% Wrapper for vl_nnsplit block
% inputs{1} :   X     : 1 x 1 x 16 x b
% outputs{1}:   y     : 1 x 1 x 10 x b
% outputs{1}:   r     : 1 x 1 x 3 x b
% outputs{1}:   t     : 1 x 1 x 2 x b
% outputs{1}:   s     : 1 x 1 x 1 x b

classdef split < dagnn.Layer

    methods
        function outputs = forward(~, inputs, ~)

            useGPU = isa(inputs{1}, 'gpuArray');
            if useGPU
            [y,r,t,s] = vl_nnsplit(gather(inputs{1}));
            outputs = {gpuArray(y),gpuArray(r),gpuArray(t),gpuArray(s)};
            else
            [y,r,t,s] = vl_nnsplit(inputs{1});
            outputs = {y,r,t,s};
            end

        end

        function [derInputs, derParams] = backward(~, inputs, ~, derOutputs)

            useGPU = isa(inputs{1}, 'gpuArray');
            if useGPU
            derInputs{1} = gpuArray( vl_nnsplit(gather(inputs{1}),gather(derOutputs{1}), gather(derOutputs{2}), gather(derOutputs{3}), gather(derOutputs{4})) );
            else
            derInputs{1} = vl_nnsplit(inputs{1},derOutputs{1},derOutputs{2},derOutputs{3},derOutputs{4});
            end

            derParams = {};
        end

        function outputSizes = getOutputSizes(~, inputSizes)
            outputSizes = {1 1 inputSizes{1}(3) inputSizes{1}(4)} ;
        end

        function obj = split(varargin)
            obj.load(varargin);
        end
    end
end


function [A,varargout] = selectMaxbyColumn(A,varargin)
% find maximum in each row

% find maximum values
ind = A == repmat(max(A,[],1),size(A,1),1);

% select only the first maximum
ind = ind & ind == cumsum(ind,1);

% return results
A = A(ind);

for i = 1:nargout-1
  varargout{i} = varargin{i}(ind);
end


function [y, m ] = month_borrow_common ( y, m )

%*****************************************************************************80
%
%% MONTH_BORROW_COMMON borrows a year of months on the Common calendar.
%
%  Discussion:
%
%    If the month index is legal, nothing is done.  If the month index
%    is too small, then one or more years are "cashed in" to bring the
%    month index up to a legal value.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 June 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input/output, integer Y, M, the YM date.
%    On input, M might be negative.  On output, Y should have decreased by
%    one, and M gone up by the number of months in the year that we
%    "cashed in".  The routine knows there was no year 0.
%
  while ( m <= 0 )

    months = year_length_months_common ( y );

    m = m + months;
    y = y - 1;

    if ( y == 0 )
      y = - 1;
    end

  end

  return
end


function A = meanOne(hyp, x, i)

% One mean function. The mean function does not have any parameters.
%
% m(x) = 1
%
% Copyright (c) by Carl Edward Rasmussen and Hannes Nickisch, 2010-08-04.
%
% See also MEANFUNCTIONS.M.

if nargin<2, A = '0'; return; end             % report number of hyperparameters
if nargin==2
  A = ones(size(x,1),1);                                         % evaluate mean
else
  A = zeros(size(x,1),1);                                           % derivative
end


 function [mass_atten, kev, mtype, file] = ...
		xray_read_atten(mtype, kev_in, varargin)
%function [mass_atten, kev, mtype, file] = ...
%|		xray_read_atten(mtype, kev_in, [options])
%|
%| Read mass attenuation coefficients for a given material type.
%| Optionally interpolate onto desired energies.
%|
%| in
%|	mtype			'aluminum', 'copper', 2, '2', '02-helium', ...
%|				See xray_material_file_name.m
%|		(Optionally mtype can be a cell array of several materials.
%|		If so, kev_in is mandatory and the output will be an array.)
%|	kev_in		[N 1]	optional desired energies [in keV]
%|
%| options
%|	'units'		cm | mm	default: cm
%|	'interp'	{}	interpolator type.  default {'pchip', 'extrap'}
%|	shortfile	0|1	return short file name instead of full path
%|
%| out
%|	mass_atten	[L 1]	mass attenuation coefficients [cm^2/g],
%|				If "energy" input is provided, then [N L].
%|	kev		[N 1]	at these corresponding energies.
%|	mtype		{L}	material type
%|	file		{L}	file name(s) for each material
%|
%| Copyright 2004-05-1, Jeff Fessler, University of Michigan

% default is to show example
if nargin < 1, ir_usage, end
if nargin == 1 && streq(mtype, 'test'), xray_read_atten_test, return, end
if ~isvar('kev_in'), kev_in = []; end

if isnumeric(mtype) && length(mtype) > 1
	mtype = num2cell(mtype);
end

if iscell(mtype)
	if isempty(kev_in), error 'kev_in required', end
	mass_atten = zeros(length(kev_in), length(mtype)); % array
	for ll=1:length(mtype)
		[mass_atten(:,ll) dum mtype{ll} file{ll}] = ...
			xray_read_atten(mtype{ll}, kev_in, varargin{:});
	end
	kev = kev_in;
return
end

% defaults
arg.units = 'cm';
%arg.interp = {'linear', 'extrap'};
%arg.interp = {'spline', 'extrap'};
arg.interp = {'pchip', 'extrap'};
arg.shortfile = false;

arg = vararg_pair(arg, varargin);

is_mm = 0;
if streq(arg.units, 'mm')
	is_mm = 1;
elseif ~streq(arg.units, 'cm')
	error 'bad units'
end

file = xray_material_file_name(mtype);
tmp = load_ascii_skip_header(file); % read uncommented lines
kev = tmp(:,1) * 1000;	% keV
mass_atten = tmp(:,2);	% mass atten. coeff. [cm^2/g]

if arg.shortfile
	file = regexprep(file, '.*\/', '');
end

if 0 % look at k-edges
	jump = find(diff(kev) == 0);
	if ~isempty(jump)
		file = regexprep(file, '.*\/', '');
		jump = num2str(kev(jump)', '%5.1f');
		printf('%s %s', file, jump)
	end
end

% if input energies are specified, then interpolate onto those.
% trick: allow for the k-edge jumps!
if isvar('kev_in') && ~isempty(kev_in)
	mass_atten = log(mass_atten); % interpolate on a log scale
	mass_atten = interp1_jump(kev, mass_atten, kev_in, ...
		'monodown', arg.interp{:});
	mass_atten = exp(mass_atten);
	kev = kev_in;
end

if is_mm
	mass_atten = mass_atten * 100;
end

% xray_read_atten_test()
% example usage
function xray_read_atten_test
mtypes = {'lead', 'aluminum', 'water', 'lexan'};
mtypes = {'water', 'bone'};
mtypes = {'iodine', 'cesium', 'csi'};
arg1 = {};
kev = [10:510]';
for ll=1:length(mtypes)
	mtype = mtypes{ll};
	[mac1 kev1] = xray_read_atten(mtype);
	ie = min(kev) <= kev1 & kev1 <= max(kev);
	arg1 = {arg1{:}, kev1(ie), mac1(ie), 'o'};
end
mac2 = xray_read_atten(mtypes, kev); % test multiple types (cell)
if im
	clf, semilogy(arg1{:})
	legend(mtypes{:})
	hold on, semilogy(kev, mac2, '-'), hold off
	xlabel 'KeV', ylabel 'mass atten. coef.', axis tight
end


function [EToE,EToF]= tiConnect2D(EToV)

% function [EToE,EToF]= tiConnect2D(EToV)
% Purpose: triangle face connect algorithm due to Toby Isaac

Nfaces=3;
K = size(EToV,1);
Nnodes = max(max(EToV));

% create list of all faces 1, then 2, & 3
fnodes = [EToV(:,[1,2]);EToV(:,[2,3]);EToV(:,[3,1])];
fnodes = sort(fnodes,2)-1;

% set up default element to element and Element to faces connectivity
EToE= (1:K)'*ones(1,Nfaces); EToF= ones(K,1)*(1:Nfaces);

% uniquely number each set of three faces by their node numbers
id = fnodes(:,1)*Nnodes + fnodes(:,2)+1;
spNodeToNode=[id, (1:Nfaces*K)', EToE(:), EToF(:)];

% Now we sort by global face number.
sorted=sortrows(spNodeToNode,1);

% find matches in the sorted face list
[indices,dummy]=find( sorted(1:(end-1),1)==sorted(2:end,1) );

% make links reflexive
matchL = [sorted(indices,:)   ;sorted(indices+1,:)];
matchR = [sorted(indices+1,:) ;sorted(indices,:)];

% insert matches
EToE(matchL(:,2)) = matchR(:,3); EToF(matchL(:,2)) = matchR(:,4);
return;


function kam = KAM(ebsd,varargin)
% intragranular average misorientation angle per orientation
%
% Syntax
%
%   plot(ebsd,ebsd.KAM ./ degree)
%
%   % ignore misorientation angles > threshold
%   kam = KAM(ebsd,'threshold',10*degree);
%   plot(ebsd,kam./degree)
%
%   % ignore grain boundary misorientations
%   [grains, ebsd.grainId] = calcGrains(ebsd)
%   plot(ebsd, ebsd.KAM./degree)
%
%   % consider also second order neigbors
%   kam = KAM(ebsd,'order',2);
%   plot(ebsd,kam./degree)
%
% Input
%  ebsd - @EBSD
%
% Options
%  threshold - ignore misorientation angles larger then threshold
%  order     - consider neighbors of order n
%  max       - take not the mean but the maximum misorientation angle
%
% See also
% grain2d.GOS

%weights = get_option(varargin,'weights',ones(3));

if check_option(varargin,'max')
  fun = @(a,b) nanmax(a,[],b);
else
  fun = @(a,b) nanplus(a,b);
end

weights = get_option(varargin,'weights',[]);

if isempty(weights)

  % get order
  order = get_option(varargin,'order',1);
  [i,j] = meshgrid(-order:order,-order:order);
  weights = (abs(i) + abs(j)) <= order;

  psi = get_option(varargin,'kernel');
  if ~isempty(psi)
    weights = psi(sqrt(ebsd.dy^2 * i.^2 + ebsd.dx^2 * j.^2));
  end
else

  order = (min(size(weights)) -1)/2;

end

% set center to zero
weights(order+1,order+1) = 0;

% get threshold
threshold = get_option(varargin,'threshold',inf);

% prepare the result
kam = zeros(size(ebsd));

% for taking the mean count the non nan values
count = zeros(size(kam));

% extract grainIds and make them a bit larger
if isfield(ebsd.prop,'grainId')
  grainId = nan(size(ebsd)+2*order);
  grainId(order+1:end-order,order+1:end-order) = ebsd.grainId;
end

% for all phases
for id = ebsd.indexedPhasesId

  % extract orientations and make them a bit larger
  rot = ebsd.rotations;
  rot(ebsd.phaseId ~= id) = NaN;
  ori = orientation.nan(size(ebsd)+2*order,ebsd.CSList{id});
  ori(order+1:end-order,order+1:end-order) = rot;

  % take the mean
  for i = -order:order
    for j = -order:order

      if  weights(order+1+i,order+1+j) == 0, continue; end

      % compute misorientation angles
      omega = angle(ori(order+1:end-order,order+1:end-order),...
        ori((order+1:end-order)+i,(order+1:end-order)+j));

      % apply angle threshold
      omega(omega > threshold) = NaN;

      % avoid grain boundaries
      if isfield(ebsd.prop,'grainId')
        omega( grainId(order+1:end-order,order+1:end-order) ~= ...
          grainId((order+1:end-order)+i,(order+1:end-order)+j) ) = NaN;
      end

      kam = fun(kam, omega .* weights(order+1+i,order+1+j));
      count = fun(count,~isnan(omega) .* weights(order+1+i,order+1+j));

    end
  end

end

kam = kam ./ count;


filename='CantileverBeam_Triangle_Linear_Fine';
ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance';'perimeter'};
weights = [1 0.1];
constraint = {'volumeConstraint'};
optimizer = 'SLERP';
incrementFactor = 1;
designVariable = 'LevelSet';
filterType = 'P1';

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

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

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


function aa=lpcdl2aa(dl)
%LPCDL2AA dct of log area to area coefficients AA=(DL)

% we do not correct for sinc errors

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: lpcdl2aa.m,v 1.4 2007/05/04 07:01:38 dmb Exp $
%
%   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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[nf,p]=size(dl);
aa=[zeros(nf,1) exp(irdct(dl.').') ones(nf,1)];


function cleanimg=deislands2d(img,sizelim)
%
% cleanimg=deislands2d(img,sizelim)
%
% remove isolated islands on a 2D image below speicified size limit
%
% author: Qianqian Fang (q.fang at neu.edu)
%
% input:
% 	img: a 2D binary image
% 	sizelim: a integer as the maximum pixel size of a isolated region
%
% output:
% 	cleanimg: a binary image after removing islands below sizelim
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

img=squeeze(img);
maxisland=-1;
if(nargin==2) maxisland=sizelim; end

islands={};

cleanimg=zeros(size(img));
if(sum(img(:)))
    img=imclose(img, strel('disk',3));
    islands=bwislands(img);
end

if(length(islands))
    % remove small islands of the foreground
    maxblock=-1;
    maxblockid=-1;
    if(maxisland<0)
      for i=1:length(islands)
        if(length(islands{i})>maxblock)
            maxblockid=i;
            maxblock=length(islands{i});
        end
      end
      if(maxblock>0)
          cleanimg(islands{maxblockid})=1;
      end
    else
      for i=1:length(islands)
        if(length(islands{i})>maxisland)
            cleanimg(islands{i})=1;
        end
      end
    end

    % remote small islands of the background

    cleanimg=imfill(cleanimg,'holes');
end


function final_result = check_yale_faces(num_subjects, solver, solver_name, solver_params)
if nargin < 4
    solver_params = struct;
end

% Create the directory for storing results
spx.fs.ensure_dir('bin');
yf = spx.data.image.ChongYaleFaces();
num_total_subjects = yf.num_subjects;

% R = yf.num_experiments_of_n_subjects(num_subjects);
if num_subjects == 38
    R = 1;
else
    R = 20;
end
fprintf('Number of subject sets: %d\n', R);

% We will carry out one experiment for each signal density level
test_result.failed_examples = [];
trials = cell(1, R);

for r=1:R
    trial = struct();
    rng(r * 38 + num_subjects);
    subject_list = randperm(num_total_subjects, num_subjects) % select #nCluster subjects
    fprintf('Subject set (%d): ', r);
    fprintf('%d ', subject_list);
    fprintf('\n');
    % [Y, cluster_sizes, true_labels] = yf.experiment_data(num_subjects, r);
    [Y, cluster_sizes, true_labels] = yf.data_for_subjects(subject_list);
    % Normalize the columns
    Y = spx.norm.normalize_l2(Y);
    % Ambient space dimension and number of data points
    [trial.M, trial.S] = size(Y);
    % Number of subspaces
    trial.K = num_subjects;
    % maximum dimension for each subspace
    trial.D = 5;
    trial.cluster_sizes = cluster_sizes;
    % Solve the sparse subspace clustering problem
    tstart = tic;
    try
        clustering_result = solver(Y, trial.D, trial.K, solver_params);
    catch ME
        % we will move on to next one
        fprintf('Problem in processing this example. %s: %s\n', ME.identifier, ME.message);
        % add to the list of failed examples
        test_result.failed_examples = [test_result.failed_examples r];
        % move on to next example
        throw(ME)
        continue;
    end
    trial.elapsed_time = toc (tstart);
    % graph connectivity
    trial.connectivity = clustering_result.connectivity;
    % estimated number of clusters
    trial.estimated_num_subspaces = clustering_result.num_clusters;
    % Time to compare the clustering
    cluster_labels = clustering_result.labels;
    comparsion_result = spx.cluster.clustering_error_hungarian_mapping(cluster_labels, true_labels, trial.K);
    trial.clustering_error_perc = comparsion_result.error_perc;
    trial.clustering_acc_perc = 100 - comparsion_result.error_perc;
    % Compute the statistics related to subspace preservation
    spr_stats = spx.cluster.subspace.subspace_preservation_stats(clustering_result.Z, trial.cluster_sizes);
    trial.spr_error = spr_stats.spr_error;
    trial.spr_flag = spr_stats.spr_flag;
    trial.spr_perc = spr_stats.spr_perc;
    fprintf('\nclustering error: %0.2f %% , clustering accuracy: %0.2f %%\n, mean spr error: %0.4f preserving : %0.2f %%\n, connectivity: %0.2f, elapsed time: %0.2f sec', trial.clustering_error_perc, trial.clustering_acc_perc, spr_stats.spr_error, spr_stats.spr_perc, trial.connectivity, trial.elapsed_time);
    fprintf('\n\n');
    trials{r} = trial;
end
% clear some variables no more required.
clear trial;
clear yf;
clear clustering_result;
clear comparison_result;
%  save rest of variables in file.
filepath = sprintf('bin/yale_faces_%d_subjects_test_%s.mat', num_subjects, solver_name);
save(filepath);
final_result = merge_results(num_subjects, solver_name);
end


function [km] = ly2km(ly)
% Convert length from light years to kilometers.
% Chad A. Greene 2012
km = ly*9460730472581;


function tests = test_spm_dcm_peb_bmc_fam
% Unit Tests for test_spm_dcm_peb_bmc
%__________________________________________________________________________
% Copyright (C) 2016 Wellcome Trust Centre for Neuroimaging

% $Id: test_spm_dcm_peb_bmc_fam.m 6946 2016-11-23 15:26:29Z peter $

tests = functiontests(localfunctions);

% -------------------------------------------------------------------------
function test_peb_bmc(testCase)

data_path = get_data_path();

% Load
GCM = load(fullfile(data_path,'models','GCM_simulated.mat'));
PEB = load(fullfile(data_path,'PEB_test.mat'));
GCM = GCM.GCM;
PEB = PEB.PEB;

% Model comparison
[BMA,BMR] = spm_dcm_peb_bmc(PEB(1),GCM(1,:));

% Family 1: model 1 (full) and model 2 (fwd modulation)
% Family 2: model 3 (bwd) and model 4 (bwd modulation)
families = [1 1 2 2];

% Model comparison under families
[BMA,fam] = spm_dcm_peb_bmc_fam(BMA,BMR,families);

% -------------------------------------------------------------------------
% Should be equal priors as families were balanced
Nm = size(GCM,2);
testCase.assertEqual([0.5 0.5], fam.family.prior)
testCase.assertTrue(all(fam.model.prior(:) == 1/(Nm*Nm)));

% Family 1 and model 2 should win
testCase.assertTrue(fam.family.post(1,1) > 0.9);
testCase.assertTrue(fam.model.Pw(2) > 0.9);
testCase.assertTrue(fam.model.Px(2) > 0.9);

% -------------------------------------------------------------------------
% Run again with unbalanced families
families = [1 1 1 2];
[BMA,fam] = spm_dcm_peb_bmc_fam(BMA,BMR,families);

% Families should have equal priors
testCase.assertEqual([0.5 0.5], fam.family.prior);

% Model joint prior matrix should sum to 1
testCase.assertEqual(1, sum(fam.model.prior(:)));

% Models within each family should have same total prior probability
family_total_prior = [];
for f = 1:2
    p = (fam.model.prior(families == f,families == f));
    family_total_prior(f) = sum(p(:));
end
testCase.assertTrue(...
    all(family_total_prior(:) - family_total_prior(1) < 1e-10));

% -------------------------------------------------------------------------
function data_path = get_data_path()

data_path = fullfile( spm('Dir'), 'tests', ...
    'data', 'fMRI', 'simulated_2region');


function tabledata = highlight_best_rows(tabledata, columns, varargin)
% highlight_best_rows Adds highlight to the best three cells in a given
% row.
%
% The function modifies the input matrix by converting it to cell array
% and augmenting the cells in individual columns with class labels for
% first three places acording to specified ordering. The resulting array
% can be then handed over to matrix2html function.
%
% Input:
% - tabledata (matrix): <description>
% - columns (cell): An array of strings that denote type of ordering for
% individual columns.
%     - Decreasing: Highlight in decreasing order.
%     - Increasing: Highlight in increasing order.
%     - None: Do not highlight.
%
% Output:
% - tabledata (cell): Modified array.
%

if size(tabledata, 2) ~= numel(columns)
    error('Number of columns does not match the sorting instructuions.');
end;

labels = {'first', 'second', 'third'};

for i = 1:numel(columns)

    switch lower(columns{i})
        case {'descending', 'descend', 'high'}
            values = cell2mat(tabledata(:, i));
            usable = find(~isnan(values));
            levels = sort(unique(values(usable)), 'descend');

        case {'ascending', 'ascend', 'low'}
            values = cell2mat(tabledata(:, i));
            usable = find(~isnan(values));
            levels = sort(unique(values(usable)), 'ascend');

        otherwise
            levels = [];
    end;

    if isempty(levels)
        continue;
    end;

    for j = 1:min(numel(labels), numel(levels))
        tabledata(values == levels(j), i) = cellfun(...
            @(x) struct('text', x, 'class', labels{j}), tabledata(values == levels(j), i), 'UniformOutput', false);

    end
end;


function ns2de_gnuplot ( header, n, x, y, u, v, s )

%*****************************************************************************80
%
%% NS2DE_GNUPLOT writes the Navier-Stokes velocity field to files for GNUPLOT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 January 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string HEADER, a header to be used to name the files.
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real X(N), Y(N), the coordinates of the evaluation points.
%
%    Input, real U(N), V(N), the velocity components.
%
%    Input, real S, a scale factor for the velocity vectors.
%

%
%  Write the data file.
%
  data_filename = strcat ( header, '_data.txt' );

  data_unit = fopen ( data_filename, 'wt' );

  for i = 1 : n
    fprintf ( data_unit, '  %10.4f  %10.4f  %10.4f  %10.4f\n', ...
      x(i), y(i), s * u(i), s * v(i) );
  end

  fclose ( data_unit );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Data written to "%s".\n', data_filename );
%
%  Write the command file.
%
  command_filename = strcat ( header, '_commands.txt' );
  plot_filename = strcat ( header, '.png' );

  command_unit = fopen ( command_filename, 'wt' );

  fprintf ( command_unit, '#  %s\n', command_filename );
  fprintf ( command_unit, '#\n' );
  fprintf ( command_unit, 'set term png\n' );
  fprintf ( command_unit, 'set output "%s"\n', plot_filename );
  fprintf ( command_unit, '#\n' );
  fprintf ( command_unit, '#  Add titles and labels.\n' );
  fprintf ( command_unit, '#\n' );
  fprintf ( command_unit, 'set xlabel "<--- X --->"\n' );
  fprintf ( command_unit, 'set ylabel "<--- Y --->"\n' );
  fprintf ( command_unit, 'set title "Navier-Stokes velocity field"\n' );
  fprintf ( command_unit, 'unset key\n' );
  fprintf ( command_unit, '#\n' );
  fprintf ( command_unit, '#  Add grid lines.\n' );
  fprintf ( command_unit, '#\n' );
  fprintf ( command_unit, 'set grid\n' );
  fprintf ( command_unit, 'set size ratio -1\n' );
  fprintf ( command_unit, '#\n' );
  fprintf ( command_unit, '#  Timestamp the plot.\n' );
  fprintf ( command_unit, '#\n' );
  fprintf ( command_unit, 'set timestamp\n' );
  fprintf ( command_unit, 'plot "%s" using 1:2:3:4 with vectors \\\n', ...
    data_filename );
  fprintf ( command_unit, '  head filled lt 2 linecolor rgb "blue"\n' );
  fprintf ( command_unit, 'quit\n' );

  fclose ( command_unit );

  fprintf ( 1, '  Commands written to "%s".\n', command_filename );

  return
end


function [parameters, stderrors, LLF, ht, resids, summary] = garchfind(data, models, distributions, ar, ma, p, q, options)
%{
[parameters, stderrors, LLF, ht, resids, summary] = garch(data, model, distr, ar, ma, x, p, q, y,startingvalues, options)
-----------------------------------------------------------------------
 PURPOSE:
 Finds the combination of models and distributions that better fits the
 data based on a set of criteria (i.e.  largest log likelihood value
 and the smallest AIC and BIC criteria).
----------------------------------------------------------------------
 USAGE:
 garchfind(data, models, distributions, ar, ma, p, q)

 INPUTS:
 data:          (T x 1) vector of data
 models:        a character vector with models to be estimated
 distributions: a character vector with distributions
 ar:            length of AR
 ma:            length of MA
 p:             length of ARCH
 q:             length of GARCH
 options:       set of options

 OUTPUTS based on the estimation of the best model:
 parameters:   a vector of parameters a0, a1, b0, b1, b2, ...
 stderrors:    standard errors estimated by the inverse Hessian (fmincon)
 LFF:          the value of the Log-Likelihood Function
 ht:           vector of conditional variances
 resids:       vector of residuals
 summary:      summary of results including:
                -model specification, distribution and statistics
                -optimization options
                -t-statistics
                -robust standard errors: (HESSIAN^-1)*cov(scores)*(HESSIAN^-1)
                -scores: numerical scores for M testing
-----------------------------------------------------------------------
Author:
 Alexandros Gabrielsen, a.gabrielsen@city.ac.uk
 Date: 08/2011
-----------------------------------------------------------------------
%}
if nargin == 0
    error('Data, GARCH Models, Distribution, AR, MA, ARCH, GARCH, Options')
end

if size(data,2) > 1
   error('Data vector should be a column vector')
end

if (length(ar) > 1) | (length(ma) > 1) | ar < 0 | ma < 0
    error('AR and MA should be positive scalars')
end

if (length(p) > 1) | (length(q) > 1) | p < 0 | q < 0
    error('P and Q should be positive scalars')
end

Criteria=[];
index=1;
for i = 1:size(models,1)
    for j = 1:size(distributions,1)
        for a=1:ar
            for b=1:ma
                for c=1:p
                    for d=1:q
                         fprintf('Progress: ARMA(%1.0f,%1.0f,%1.0f)-%s(%1.0f,%1.0f,%1.0f) - %s distribution\n', a, b, 0, strcat(models(i,:)), c, d, 0, strcat(distributions(j,:)))
                         eval(['[parameters, stderrors, LLF_temp] = garch(data,''' strcat(models(i,:)),''',''', strcat(distributions(j,:)), ''',a,b,0,c,d,0,[],options);']);
                         Criteria(index,:) = [LLF_temp, -2*LLF_temp+2*size(parameters,1), -2*LLF_temp+size(parameters,1)*log(size(data,1)), i, j, a, b, c,d];
                         index=index+1;
                    end
                end
            end
        end
    end
end

% This estimates a vector of results as long as the criteria are satisfied
holder=sortrows(Criteria,1);
z=size(holder,1)+1;
fprintf('\n The top models are: \n')
for i = 1:size(holder,1)
    if  find(holder == max(holder(1:z-i,1))) == find(holder == min(holder(1:z-i,2))) - size(holder(1:z-i,1),1) & find(holder == max(holder(1:z-i,1))) == find(holder == min(holder(1:z-i,3))) - 2*size(holder(1:z-i,1),1)
        fprintf('ARMA(%1.0f,%1.0f,%1.0f)-%s(%1.0f,%1.0f,%1.0f) - %s distribution\n', holder(z-i,6), holder(z-i,7), 0, strcat(models(holder(z-i,4),:)), holder(z-i,8), holder(z-i,9), 0, strcat(distributions(holder(z-i,5),:)))
    end
end

% Finds the best model so as to estimate the model
if find(Criteria == max(Criteria(:,1))) == find(Criteria == min(Criteria(:,2))) - size(Criteria,1) & find(Criteria == max(Criteria(:,1))) == find(Criteria == min(Criteria(:,3))) - 2*size(Criteria,1)
[parameters, stderrors, LLF, ht, resids, summary] = garch(data, strcat(models(Criteria(find(Criteria == max(Criteria(:,1))),4),:)), ...
    strcat(distributions(Criteria(find(Criteria == max(Criteria(:,1))),5),:)), ...
    Criteria(find(Criteria == max(Criteria(:,1))),6),...
    Criteria(find(Criteria == max(Criteria(:,1))),7),0, ...
    Criteria(find(Criteria == max(Criteria(:,1))),8), ...
    Criteria(find(Criteria == max(Criteria(:,1))),9),0,[],options);
end
end


function tetra_node2 = tet_mesh_order10_to_order4_compute ( tetra_num1, ...
  tetra_node1, tetra_num2 )

%*****************************************************************************80
%
%% TET_MESH_ORDER10_TO_ORDER4_COMPUTE linearizes a quadratic tet mesh.
%
%  Discussion:
%
%    A quadratic tet mesh is assumed to consist of 10-node
%    tetrahedrons.
%
%    This routine rearranges the information so as to define a 4-node
%    tet mesh.
%
%    The same nodes are used, but there are 8 times as many
%    tetrahedrons.
%
%    The node ordering for the quadratic tetrahedron is somewhat
%    arbitrary.  In the current scheme, the vertices are listed
%    first, followed by the 6 midside nodes.  Each midside node
%    may be identified by the two vertices that bracket it.  Thus,
%    the node ordering may be suggested by:
%
%      1  2  3  4 (1+2) (1+3) (1+4) (2+3) (2+4) (3+4)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 January 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Anwei Liu, Barry Joe,
%    Quality Local Refinement of Tetrahedral Meshes Based
%    on 8-Subtetrahedron Subdivision,
%    Mathematics of Computation,
%    Volume 65, Number 215, July 1996, pages 1183-1200.
%
%  Parameters:
%
%    Input, integer TETRA_NUM1, the number of tetrahedrons in the quadratic
%    tet mesh.
%
%    Input, integer TETRA_NODE1(10,TETRA_NUM1), the quadratic tet mesh.
%
%    Input, integer TETRA_NUM2, the number of tetrahedrons in the linear
%    tet mesh.  TETRA_NUM2 = 8 * TETRA_NUM1.
%
%    Output, integer TETRA_NODE2(4,TETRA_NUM2), the linear tet mesh.
%
  tetra2 = 0;

  for tetra1 = 1 : tetra_num1

    n1 = tetra_node1(1,tetra1);
    n2 = tetra_node1(2,tetra1);
    n3 = tetra_node1(3,tetra1);
    n4 = tetra_node1(4,tetra1);
    n5 = tetra_node1(5,tetra1);
    n6 = tetra_node1(6,tetra1);
    n7 = tetra_node1(7,tetra1);
    n8 = tetra_node1(8,tetra1);
    n9 = tetra_node1(9,tetra1);
    nx = tetra_node1(10,tetra1);

    tetra2 = tetra2 + 1;
    tetra_node2(1:4,tetra2 ) = [ n1, n5, n6, n7 ]';
    tetra2 = tetra2 + 1;
    tetra_node2(1:4,tetra2 ) = [ n2, n5, n8, n9 ]';
    tetra2 = tetra2 + 1;
    tetra_node2(1:4,tetra2 ) = [ n3, n6, n8, n9 ]';
    tetra2 = tetra2 + 1;
    tetra_node2(1:4,tetra2 ) = [ n4, n7, n9, nx ]';
    tetra2 = tetra2 + 1;
    tetra_node2(1:4,tetra2)  = [ n5, n6, n7, n9 ]';
    tetra2 = tetra2 + 1;
    tetra_node2(1:4,tetra2 ) = [ n5, n6, n8, n9 ]';
    tetra2 = tetra2 + 1;
    tetra_node2(1:4,tetra2 ) = [ n6, n7, n9, nx ]';
    tetra2 = tetra2 + 1;
    tetra_node2(1:4,tetra2 ) = [ n6, n8, n9, nx ]';

  end

  return
end


% GRAVLOAD Joint loads due to gravity
%
% Uses either the rne MEX file, from RTB, or the grav function, from
% pHRIWARE, depending on being able to use the MEX file and if the
% Jacobian is requested to be returned
%
% Copyright (C) Bryan Moutrie, 2013-2014
% Licensed under the GNU Lesser General Public License
% see full file for full statement
%
% This file requires file(s) from The Robotics Toolbox for MATLAB (RTB)
% by Peter Corke (www.petercorke.com), see file for statement
%
% Syntax:
%  (1) tauB = robot.gravload(q)
%  (2) [tauB, J] = robot.grav(q)
%  (3) ... = robot.grav(q, grav)
%
%  (2) is as per (1) but also returns world-frame Jacobian - pHRIWARE
%  (3) is as per others but with explicit gravity vector
%
% Outputs:
%  tauB : Generalised joint force/torques
%  J    : Jacobian in world frame
%
% Inputs:
%  q     : Joint row vector, or trajectory matrix of joint row vectors
%  grav  : Gravity _reaction_ vector (as RTB default is [0 0 +9.81]')
%
% See also grav, rne

% LICENSE STATEMENT:
%
% This file is part of pHRIWARE.
%
% pHRIWARE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as
% published by the Free Software Foundation, either version 3 of
% the License, or (at your option) any later version.
%
% pHRIWARE 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 Lesser General Public
% License along with pHRIWARE.  If not, see <http://www.gnu.org/licenses/>.
%
% RTB LIBRARY:
%
% Copyright (C) 1993-2014, by Peter I. Corke
% http://www.petercorke.com
% Released under the GNU Lesser General Public license

function varargout = gravload(robot, varargin)

if robot.fast && ~robot.issym() && nargout <= 1
   qz = zeros(size(varargin{1}));
   rne_input = {varargin(1), qz, qz, varargin(2:end)};
   varargout = robot.frne(rne_input{:});
else
   pHRIWARE('c');
   varargout = robot.grav(varargin{:});
end

end


function result = cvRsimpls(x,y,kmax,rmsecv,h,k)

%CVRIMPLS calculates the robust RMSECV (root mean squared error of cross-validation) curve
% for RSIMPLS or the robust RMSEP(root mean squared error of prediction) value in a fast way.
% The R-RMSECV curve can be used to make a selection of the optimal number of
% components to include in the regression model. The function is used in rsimpls.m.
%
% Input arguments:
%    x          : the explanatory variables
%    y          : the rsponse variables
%    kmax       : maximal number of variables to include in the model.
%    rmsecv     : Optional. If equal to 1 (default), the rmsecv is computed.
%                 Else, rmsecv = 0 and then the rss and R2 are computed.
%    h          : optional input argument.
%    k          : optional input argument. If k = 0 (default) then RMSECV is calculated. Else the RMSEP wil be computed.
%
% Output:
%  if RMSECV is computed:
%   result.rmsecv     : the R-RMSECV values (obtained with the minimum weights).
%  if RMSEP is computed:
%   result.rmsep      : the R-RMSEP values
%   result.rss        : the RSS values for every k.
%   result.R2         : the coefficient of determination for every k.
%   result.residu     : the residuals for every k = 1,...,kmax
%   result.outWeights : the weights used to compute the robust R-RMSEP values.
%
% This function is part of LIBRA: the Matlab Library for Robust Analysis,
% available at:
%              http://wis.kuleuven.be/stat/robust.html
%
% Written by Sanne Engelen
% Last Update: 08/10/2004, 03/07/2006
% Last Revision: 03/07/2006

% some initialisations
n = size(x,1);
p = size(x,2);
q = size(y,2);
r = rank(x);
rz = rank([x,y]);
teller_if_lus = 0;
cutoffWeights = sqrt(chi2inv(0.975,q));

if nargin < 4
    alfa = 0.75;
    kmaxr=min([kmax+q,rz]);
    h=floor(2*floor((n+kmaxr+1)/2)-n+2*(n-floor((n+kmaxr+1)/2))*alfa);
    k = 0;
    rmsecv = 1;
elseif nargin == 4
    alfa = 0.75;
    kmaxr=min([kmax+q,rz]);
    h=floor(2*floor((n+kmaxr+1)/2)-n+2*(n-floor((n+kmaxr+1)/2))*alfa);
    k = 0;
elseif nargin == 5
    k = 0;
end

outWeights = weightcvRsimpls(x,y,kmax,h,k,cutoffWeights);

if rmsecv
    if k == 0
        w_min = outWeights.w_min;
    end
    rob = outWeights.ResRob;

    % Assigning the input variables
    if size(rob.Hsubsets.H0,2)==1
        rob.Hsubsets.H0=rob.Hsubsets.H0';
    end
    Hsets = [rob.Hsubsets.H0;rob.Hsubsets.H1;rob.Hsubsets.Hfreq];
    same.value = 0;
    data = [x,y];

    for i = 1:n
        disp(['observation ',num2str(i),' is left out'])
        X_min_i = removal(x,i,0);
        Y_min_i = removal(y,i,0);
        data_min_i = removal(data,i,0);

        same.value = 0;
        if isempty(find(rob.Hsubsets.H0 == i))
            if teller_if_lus >= 1
                same.value = 1;
            end
            teller_if_lus = teller_if_lus + 1;
        end

        % constructing Hsets of right size: h - 1.
        Hsets_min_i = RemoveObsHsets(Hsets,i);

        if k == 0
            res = removeObsRobpca(data,i,kmax + q,Hsets_min_i,same,1);
        else
            res = removeObsRobpca(data,i,k + q,Hsets_min_i,same);
        end

        if isempty(find(rob.Hsubsets.H0 == i))
            same.res = res;
        end

        Prob_min_i = res.Pk_min_i;
        Lrob_min_i = res.Lk_min_i;
        murob_min_i = res.muk_min_i;
        Trob_min_i = (data_min_i - repmat(murob_min_i,n-1,1))*Prob_min_i;

        % Computing weights corresponding with the ROBPCA results.
        sdrob_min_i = sqrt(libra_mahalanobis(Trob_min_i,zeros(1,size(Trob_min_i,2)),'invcov',1./Lrob_min_i))';

        if k == 0
            cutoff.sd=sqrt(chi2inv(0.975,kmax));
        else
            cutoff.sd = sqrt(chi2inv(0.975,k));
        end

        % Orthogonal distances to robust PCA subspace
        XRc=data_min_i-repmat(murob_min_i,n-1,1);
        Xtilde=Trob_min_i*Prob_min_i';
        Rdiff=XRc-Xtilde;
        for j=1:(n-1)
            odrob_min_i(j,:)=norm(Rdiff(j,:));
        end
        % Robust cutoff-value for the orthogonal distance
        if k == 0
            test_k = kmax;
        else
            test_k = k;
        end

        if test_k~=r
            [m,s]=unimcd(odrob_min_i.^(2/3),h);
            cutoff.od = sqrt(norminv(0.975,m,s).^3);
            wrob_min_i = (odrob_min_i<=cutoff.od)&(sdrob_min_i<=cutoff.sd);
        else
            cutoff.od=0;
            wrob_min_i = (sdrob_min_i<=cutoff.sd);
        end

        % start the deflation:
        xycentr_min_i = [];
        sigmax_min_i = [];
        xcentr_min_i = [];
        sigmaxy_min_i = [];
        sigmayx_min_i = [];

        xycentr_min_i = murob_min_i;
        sigmax_min_i = Prob_min_i(1:p,:)*diag(Lrob_min_i)*Prob_min_i(1:p,:)';
        xcentr_min_i = X_min_i - repmat(murob_min_i(1:p),n-1,1);
        sigmaxy_min_i = Prob_min_i(1:p,:)*diag(Lrob_min_i)*Prob_min_i(p+1:p+q,:)';
        sigmayx_min_i = sigmaxy_min_i';

        % calculation of the scores.
        nScores = 1;
        R_min_i = [];
        T_min_i = [];
        P_min_i = [];
        V_min_i = [];

        if k == 0
            countScores = kmax;
        else
            countScores = k;
        end

        while nScores <= countScores
            if q == 1
                qq_min_i = 1;
            else
                [QQ,LL] = eig(sigmayx_min_i*sigmaxy_min_i);
                [LL,I] = greatsort(diag(LL));
                qq_min_i = QQ(:,I(1));
            end

            rr_min_i = sigmaxy_min_i*qq_min_i;
            rr_min_i = rr_min_i/norm(rr_min_i);
            tt_min_i = xcentr_min_i*rr_min_i;
            pp_min_i = sigmax_min_i*rr_min_i/(rr_min_i'*sigmax_min_i*rr_min_i);
            vv_min_i = pp_min_i;

            if nScores > 1
                vv_min_i = vv_min_i - V_min_i*(V_min_i'*pp_min_i);
            end
            vv_min_i = vv_min_i./norm(vv_min_i);
            sigmaxy_min_i = sigmaxy_min_i - vv_min_i*(vv_min_i'*sigmaxy_min_i);

            V_min_i(:,nScores) = vv_min_i;
            T_min_i(:,nScores) = tt_min_i;
            R_min_i(:,nScores) = rr_min_i;
            P_min_i(:,nScores) = pp_min_i;

            nScores = nScores + 1;
        end

        if k == 0
            outRegr = runRegr(x,y,i,T_min_i,Y_min_i,R_min_i,murob_min_i,wrob_min_i,kmax,cutoffWeights);
            for j = 1:kmax
                Tk_min_i = T_min_i(:,1:j);
                geg = [Tk_min_i,Y_min_i];

                outRegr.Mu = outRegr.center';
                outRegr.Sigma = outRegr.sigma;
                [Bk,intk,sigmayykmaxrew_k,sigmattkmaxrew_k] = extractmcdregres(outRegr,Tk_min_i,Y_min_i,kmax,n-1,q,j,h-1,cutoffWeights);
                coeffk = [Bk;intk];
                b_min_i = R_min_i(:,1:j)*coeffk(1:j,:);
                int_min_i = coeffk(j+1,:)  - murob_min_i(1:p)*R_min_i(:,1:j)*coeffk(1:j,:);
                Yhat_min_i = x(i,:)*b_min_i + int_min_i;
                resid_min_i(i,(j-1)*q + 1:j*q) = y(i,:) - Yhat_min_i;

                % calculation of the resd:
                rewE2=sigmayykmaxrew_k- coeffk(1:j,1:q)'*sigmattkmaxrew_k*coeffk(1:j,1:q);

                if q > 1
                    cov = rewE2;
                    cen=zeros(q,1);
                    resd(i,j)=sqrt(libra_mahalanobis(resid_min_i(i,(j-1)*q + 1:j*q),cen','cov',cov))'; %robust distances of residuals
                else
                    scale = sqrt(rewE2);
                    resd(i,j) = resid_min_i(i,(j-1)*q + 1:j*q)/scale;
                end
            end
        else
            outRegr = runRegr(x,y,i,T_min_i,Y_min_i,R_min_i,murob_min_i,wrob_min_i,k,cutoffWeights);
            resid_min_i(i,:) = outRegr.resid_min_i;
            resd(i,:) = outRegr.resd';
        end
    end

    if k == 0
        for j = 1:kmax
            resk = resid_min_i(:,(j-1)*q + 1:j*q);
            if q == 1
                rmsecv(j) = sqrt(1/sum(w_min)*w_min*(resk).^2);
            else
                rmsecv(j) = sqrt(1/sum(w_min)*w_min*(mean((resk').^2))');
            end
        end
        result.rmsecv = rmsecv;
        result.residu = resid_min_i;
    else
        weights = outWeights.weightsk;
        if q == 1
            rmsep = sqrt(1/sum(weights)*weights'*(resid_min_i).^2);
        else
            rmsep = sqrt(1/sum(weights)*weights'*(mean((resid_min_i').^2))');
        end
        result.rmsep = rmsep;
        result.residu = resid_min_i;
    end
end

result.outWeights = outWeights;
result.rss = outWeights.rss;
result.R2 = outWeights.R2;

%------------------------------------------------------------------
function out = runRegr(x,y,i,T_min_i,Y_min_i,R_min_i,mukmax_min_i,wkmax_min_i,k,cutoffWeights)

[n,p] = size(x);
[n,q] = size(y);

% perform the robpca regression:
breg = [];
b_min_i = [];
int_min_i= [];
Yhat_min_i = [];
robpcareg = robpcaregres(T_min_i(:,1:k),Y_min_i,wkmax_min_i',cutoffWeights);
out.center = robpcareg.center;
out.sigma = robpcareg.sigma;
breg = robpcareg.coeffs(1:k,:);
b_min_i = R_min_i(:,1:k)*breg;
int_min_i = robpcareg.coeffs(k+1,:) - mukmax_min_i(1:p)*R_min_i(:,1:k)*breg;
Yhat_min_i = x(i,:)*b_min_i + int_min_i;
resid_min_i = y(i,:) - Yhat_min_i;

% calculation of the resd:
if q > 1
    cov = robpcareg.cov;
    cen=zeros(q,1);
    resd=sqrt(libra_mahalanobis(resid_min_i,cen','cov',cov))'; %robust distances of residuals
else
    scale = sqrt(robpcareg.cov);
    resd = resid_min_i/scale;
end

out.resid_min_i = resid_min_i;
out.resd = resd;
%----------------------------------------------------------------------------------------
function out = weightcvRsimpls(x,y,kmax,h,k,cutoffWeights)

% Computes the weights for the robust RMSECV/RMSEP values.
%
% input:
%   x    : the independent variables.
%   y    : the response variables.
%   kmax : the maximal number of components to be considered.
%   h    : the number of observations on which the calculations are based.
%   k    : if equal to zero, robpca is performed on kmax components (case RMSECV). (default).
%          Else, robpca is performed for a certain number of components (case RMSEP)
%
% output:
%   out.w_min    : the weights obtained by taking the minimum over all k
%   out.weightsk : the weights for all observations and all k (n x kmax)
%   out.resrob   : the results of robpca on [x,y].
%   out.R2       : the weighted Rsquared for each value of k
%   out.rss      : the weighted rss for each value of k

n = size(x,1);
p = size(x,2);
q = size(y,2);
r = rank(x);

if nargin < 5
    k = 0;
end

if k == 0
    ResRob = robpca([x,y],'plots',0,'k',kmax + q,'kmax',kmax + q,'h',h);
else
    ResRob = robpca([x,y],'plots',0,'k',k + q,'kmax',kmax + q,'h',h);
end

Trob = ResRob.T;
Prob = ResRob.P;
Lrob = ResRob.L;
murob = ResRob.M;
wrob = ResRob.flag.all;

%deflation

xycentr = [];
sigmax = [];
xcentr = [];
sigmaxy = [];
sigmayx = [];

xycentr = murob;
sigmax = Prob(1:p,:)*diag(Lrob)*Prob(1:p,:)';
xcentr = x - repmat(murob(1:p),n,1);
sigmaxy = Prob(1:p,:)*diag(Lrob)*Prob(p+1:p+q,:)';
sigmayx = sigmaxy';

% calculation of the scores.
nScores = 1;
R = [];
T = [];
P = [];
V = [];

if k == 0
    countScores = kmax;
else
    countScores = k;
end

while nScores <= countScores
    if q == 1
        qq = 1;
    else
        [QQ,LL] = eig(sigmayx*sigmaxy);
        [LL,I] = greatsort(diag(LL));
        qq = QQ(:,I(1));
    end

    rr = sigmaxy*qq;
    rr = rr/norm(rr);
    tt = xcentr*rr;
    pp = sigmax*rr/(rr'*sigmax*rr);
    vv = pp;

    if nScores > 1
        vv = vv - V*(V'*pp);
    end
    vv = vv./norm(vv);
    sigmaxy = sigmaxy - vv*(vv'*sigmaxy);

    V(:,nScores) = vv;
    T(:,nScores) = tt;
    R(:,nScores) = rr;
    P(:,nScores) = pp;

    nScores = nScores + 1;
end

if k == 0
    outRobRegr = robpcaregres(T(:,1:kmax),y,wrob');

    for j = 1:kmax
        outRobRegr.Mu = outRobRegr.center';
        outRobRegr.Sigma = outRobRegr.sigma;
        [Bk,intk,sigmayykmaxrew_k,sigmattkmaxrew_k] = extractmcdregres(outRobRegr,T(:,1:j),y,kmax,n,q,j,h,cutoffWeights);
        coeffk = [Bk;intk];
        finalB = R(:,1:j)*coeffk(1:j,:);
        finalInt = coeffk(j+1,:)  - murob(1:p)*R(:,1:j)*coeffk(1:j,:);
        Yhat = x*finalB + repmat(finalInt,n,1);
        resid(:,(j-1)*q+1:j*q) = y - Yhat;

        % calculation of the rd:
        rewE2=sigmayykmaxrew_k- coeffk(1:j,1:q)'*sigmattkmaxrew_k*coeffk(1:j,1:q);

        if q > 1
            cov = rewE2;
            cen=zeros(q,1);
            resd = sqrt(libra_mahalanobis(resid(:,(j-1)*q+1:j*q),cen','cov',cov))'; %robust distances of residuals
            weightsk(:,j) = (abs(resd)<=cutoffWeights);
        else
            scale = sqrt(rewE2);
            resd = resid(:,(j-1)*q+1:j*q)/scale;
            weightsk(:,j) = (abs(resd)<=cutoffWeights);
        end
    end
else
    outRobRegr = robpcaregres(T(:,1:k),y,wrob');

    % robust residual distance:
    if q==1
        resd=outRobRegr.resids/sqrt(outRobRegr.cov);
    else
        resd=sqrt(libra_mahalanobis(outRobRegr.resids,zeros(1,q),'cov',outRobRegr.cov))';
    end

    weightsk = (abs(resd)<=cutoffWeights);
end

if k == 0
    if kmax == 1
        w_min = weightsk';
    else
        w_min = min(weightsk');
    end

    out.w_min = w_min;
    out.weightsk = weightsk;

    yw = mean(y(w_min == 1,:));
    y2=(y-repmat(yw,n,1)).^2;
    R = resid.^2;
    D=sum(y2(w_min==1,:));
    for j = 1:kmax
        R1=R(w_min==1,(j-1)*q+1:j*q);
        rss(j) = sum(sum(R1));
        R2(j)=1-rss(j)/sum(D);
    end

    out.rss = 1/(q*sum(w_min))*rss;
    out.R2 = R2;
else
    out.weightsk = weightsk;

    s=sum(weightsk);
    yw=sum(y(weightsk==1,:))/s;
    y2=(y-repmat(yw,n,1)).^2;
    R = outRobRegr.resids.^2;
    D=sum(y2(weightsk==1,:));
    R1=R(weightsk==1,:);
    rss = sum(sum(R1));
    R2=1-rss/sum(D);

    out.rss = 1/(q*sum(weightsk))*rss;
    out.R2 = R2;
end
out.ResRob = ResRob;
%---------------------------------------------------------------------------------------
function Hsets_min_i = RemoveObsHsets(Hsets,i)

% removes the right index from the $h$-subsets in Hsets to
% obtain (h - 1)-subsets.
% every h-set is put as a row in Hsets.
% i is the index of the observation that is removed from the whole data.

for r = 1:size(Hsets,1)
    if ~isempty(find(Hsets(r,:)== i))
        Hsets_min_i(r,:) = removal(Hsets(r,:),0,find(Hsets(r,:) == i));
    else
        Hsets_min_i(r,:) = Hsets(r,1:(end-1));
    end

    for j = 1:length(Hsets_min_i(r,:))
        if Hsets_min_i(r,j) > i
            Hsets_min_i(r,j) = Hsets_min_i(r,j) - 1;
        end
    end
end


% This functions, when given the detectionsQ and detectionsT
% (after propagation step) gives the 3D3D and 3D2D intesection
% area score
function [scoreMatrix_3D3D, scoreMatrix_3D2D] = generateScoreMatrices(detectionsQ, detectionsT)

    % score_3D2D   = get3D2DMatchScore(detectionsT(1).bbox, detectionsQ(1).bvolume_proj);
    % score_3D3D = get3D3DMatchScore(detectionsT(2).bvolume, detectionsQ(1).bvolume);

    scoreMatrix_3D3D = zeros(length(detectionsQ), length(detectionsT));
    scoreMatrix_3D2D = zeros(length(detectionsQ), length(detectionsT));

    % 3D 3D score
    for i = 1:length(detectionsQ)
        for j = 1:length(detectionsT)

            % remember all the shapes are in F2, and we are matching shapes
            % i.e. 3D bvolumes and their bboxes in F2 with their
            % corresponding search bounding volumes and bounding volume
            % projections which are the propagated volumes of cars in F1.
            % Hence here Q is T and vice-versa
            scoreMatrix_3D3D(i,j) = get3D3DMatchScore(detectionsT{j}.bvolume, detectionsQ{i}.bvolume);
            scoreMatrix_3D2D(i,j) = get3D2DMatchScore(detectionsT{j}.bbox, detectionsQ{i}.bvolume_proj);
        end
    end

end


function subpak_test33 ( )

%*****************************************************************************80
%
%% TEST33 tests TVEC_EVEN_BRACKET, TVEC_EVEN_BRACKET2, and TVEC_EVEN_BRACKET3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST33\n' );
  fprintf ( 1, '  For evenly spaced angles between THETA1 and THETA2:\n' );
  fprintf ( 1, '  TVEC_EVEN_BRACKET\n' );
  fprintf ( 1, '  TVEC_EVEN_BRACKET2.\n' );
  fprintf ( 1, '  TVEC_EVEN_BRACKET3.\n' );
  fprintf ( 1, '\n' );

  nt = 4;
  theta1 = 30.0;
  theta2 = 90.0;

  t = tvec_even_bracket ( nt, theta1, theta2 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  TVEC_EVEN_BRACKET\n' );
  fprintf ( 1, '    NT = %d\n', nt );
  fprintf ( 1, '    THETA1 = %f\n', theta1 );
  fprintf ( 1, '    THETA2 = %f\n', theta2 );
  fprintf ( 1, '\n' );
  for i = 1 : nt
    fprintf ( 1, '  %f\n', t(i) );
  end

  nt = 5;

  t = tvec_even_bracket2 ( nt, theta1, theta2 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  TVEC_EVEN_BRACKET2\n' );
  fprintf ( 1, '    NT = %d\n', nt );
  fprintf ( 1, '    THETA1 = %f\n', theta1 );
  fprintf ( 1, '    THETA2 = %f\n', theta2 );
  fprintf ( 1, '\n' );
  for i = 1 : nt
    fprintf ( 1, '  %f\n', t(i) );
  end

  nt = 3;

  t = tvec_even_bracket3 ( nt, theta1, theta2 );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TVEC_EVEN_BRACKET3\n' );
  fprintf ( 1, '    NT = %d\n', nt );
  fprintf ( 1, '    THETA1 = %f\n', theta1 );
  fprintf ( 1, '    THETA2 = %f\n', theta2 );
  fprintf ( 1, '\n' );
  for i = 1 : nt
    fprintf ( 1, '  %f\n', t(i) );
  end

  return
end


function [tau] = ReasTaucalc(xk,EqMag,xmeff,bgdiff,P)
	% routine to claculate the look ahead time for clustered events gives tau back
    %
    %   xk:    factor used in xmeff
    %   xmeff: "effective" lower magnitude cutoff for the catalog.
    %   bgdiff:  difference in time
    %   P :    confidence that you are observing the next event in  the sequence (default is 0.95)
    %

	%ORIGINAL FROM ZMAP: NEEDS WORK FOR MAPSEIS
	%SUBFUNCTION: ReasenbergDeclus.m MIGHT NOT BE NEEDED
	%---------------------------------------------------

	%Adopted for MapSeis (small changes)

	%tauclac.m                                         A.Allmann

	deltam = (1-xk)*EqMag-xmeff;        %delta in magnitude
    if deltam<0
        deltam=0;
    end

	denom  = 10^((deltam-1)*2/3);              %expected rate of aftershocks
	top    = -log(1-P)*bgdiff;
	tau    = top/denom;                        %equation out of Raesenberg paper

end


%%%  ModeSwitch_HIL
clear
path( './icon/',path);
load vehicle_local_position.mat;
%Constant value
RAD2DEG = 57.2957795;
DEG2RAD = 0.0174533;
%throttle when UAV is hovering
THR_HOVER = 0.609;

%% control parameter
%Attitude PID parameters
Kp_PITCH_ANGLE = 6.5;
Kp_PITCH_AngleRate = 0.1;
Ki_PITCH_AngleRate = 0.02;
Kd_PITCH_AngleRate = 0.001;
Kp_ROLL_ANGLE = 6.5;
Kp_ROLL_AngleRate = 0.1;
Ki_ROLL_AngleRate = 0.02;
Kd_ROLL_AngleRate = 0.001;

Kp_YAW_ANGLE = 3;
Kp_YAW_AngleRate = 0.3;
Ki_YAW_AngleRate = 0.001;
Kd_YAW_AngleRate = 0.00;
%Position PID parameters
Kpxp = 1.0;
Kpyp = 1.0;
Kpzp = 4.0;
Kvxp = 2.5; Kvxi = 0.4; Kvxd = 0.01;
Kvyp = 2.5; Kvyi = 0.4; Kvyd = 0.01;
Kvzp = 0.45; Kvzi = 0.01; Kvzd = 0.005;
%integral saturation
Saturation_I_RP_Max = 0.3;
Saturation_I_RP_Min = -0.3;
Saturation_I_Y_Max = 0.2;
Saturation_I_Y_Min = -0.2;
Saturation_I_ah = 3.43;
Saturation_I_az = 5;

%max control angle,default 35deg
MAX_CONTROL_ANGLE_ROLL = 35;
MAX_CONTROL_ANGLE_PITCH  = 35;
MAX_CONTROL_ANGLE_NAV_ROLL = 15;
MAX_CONTROL_ANGLE_NAV_PITCH  = 15;
%max control angle rate,rad/s
MAX_CONTROL_ANGLE_RATE_PITCH = 220;
MAX_CONTROL_ANGLE_RATE_ROLL = 220;
MAX_CONTROL_ANGLE_RATE_Y = 200;
%Maximum control speed, m/s
MAX_CONTROL_VELOCITY_XY = 5;
MAX_CONTROL_VELOCITY_Z = 3;
%Throttle amplitude
MAX_MAN_THR = 0.9;
MIN_MAN_THR = 0.05;
%% run simulink model
ModeSwitch_HIL


function [z,flag] = formationP2z(polytope,zd,range,lc,lm)
max_aw = 1;
min_aw = 0.45;
polynum = length(polytope);
% get the A and b from polytope
Ap = []; bp = [];
for i=1:polynum
    Ap = [Ap;polytope(i).A];
    bp = [bp;polytope(i).b];
end
if length(zd)==12
    % constraints al<=a<=au and thl<=th<=thu just use lb and ub
    lb = [range.lb;-pi;min_aw*ones(4,1);-pi/4*ones(4,1);min_aw]';
    ub = [range.ub;pi;max_aw*ones(4,1);pi/4*ones(4,1);max_aw]';
    % calculate z
    [z,~,flag] = fmincon(@(z) (z-zd)*(z-zd)',zd,[],[],[],[],lb,ub,@(z) mycon(z,Ap,bp,lc,lm));
else
    % constraints al<=a<=au and thl<=th<=thu just use lb and ub
    lb = [range.lb;min_aw*ones(2,1)]';
    ub = [range.ub;max_aw*ones(2,1)]';
    % calculate z
    [z,~,flag] = fmincon(@(z) (z-zd)*(z-zd)',zd,[],[],[],[],lb,ub,@(z) mycon2(z,Ap,bp,lc,lm));
end
end

function [c,ceq] = mycon(z,A,b,lc,lm)
import rvctools.*
max_aw = 1;
min_aw = 0.45;
xr = [z(1:2),0,0,0,z(3)]; a = z(4:7); th = z(8:11); w = z(12);
xm = xr2m(xr,a,th,lc,lm);
for i=1:size(xm,1)
    plat(i) = pkgMechanics.RigidCuboid(1,xm(i,:),lm);
    pm_v(:,:,i) = plat(i).vertices(1:4,:)';
    Rm(:,:,i) = rpy2r(xm(i,4:end));
    pw_m(:,i) = xm(i,1:3)';
    pw_v(:,4*i-3:4*i) = pw_m(:,i)+Rm(:,:,i)*pm_v(:,:,i);
end
for i=1:size(pw_v,2)
    c1(:,i) = A*pw_v(1:2,i)-b;
end
c2 = (a.^2+w.^2-max_aw^2)';
c3 = -(a.^2+w.^2-min_aw^2)';
c = [c1(:);c2(:);c3(:)];
ceq = 0;
end

function [c,ceq] = mycon2(z,A,b,lc,lm)
import rvctools.*
max_aw = 1;
min_aw = 0.45;
xr = [z(1:2),0,0,0,0]; a = z(3)*ones(1,4); th = zeros(1,4); w = z(4);
xm = xr2m(xr,a,th,lc,lm);
for i=1:size(xm,1)
    plat(i) = pkgMechanics.RigidCuboid(1,xm(i,:),lm);
    pm_v(:,:,i) = plat(i).vertices(1:4,:)';
    Rm(:,:,i) = rpy2r(xm(i,4:end));
    pw_m(:,i) = xm(i,1:3)';
    pw_v(:,4*i-3:4*i) = pw_m(:,i)+Rm(:,:,i)*pm_v(:,:,i);
end
for i=1:size(pw_v,2)
    c1(:,i) = A*pw_v(1:2,i)-b;
end
c2 = (a.^2+w.^2-max_aw^2)';
c3 = -(a.^2+w.^2-min_aw^2)';
c = [c1(:);c2(:);c3(:)];
ceq = 0;
end


function c = reverse_cumsum(v)

c = reverse_vector(cumsum(reverse_vector(v)));