function colors = distinguishable_colors(n_colors, bg, func)
% DISTINGUISHABLE_COLORS: pick colors that are maximally perceptually distinct
%
% When plotting a set of lines, you may want to distinguish them by color.
% By default, Matlab chooses a small set of colors and cycles among them,
% and so if you have more than a few lines there will be confusion about
% which line is which. To fix this problem, one would want to be able to
% pick a much larger set of distinct colors, where the number of colors
% equals or exceeds the number of lines you want to plot. Because our
% ability to distinguish among colors has limits, one should choose these
% colors to be "maximally perceptually distinguishable."
%
% This function generates a set of colors which are distinguishable
% by reference to the "Lab" color space, which more closely matches
% human color perception than RGB. Given an initial large list of possible
% colors, it iteratively chooses the entry in the list that is farthest (in
% Lab space) from all previously-chosen entries. While this "greedy"
% algorithm does not yield a global maximum, it is simple and efficient.
% Moreover, the sequence of colors is consistent no matter how many you
% request, which facilitates the users' ability to learn the color order
% and avoids major changes in the appearance of plots when adding or
% removing lines.
%
% Syntax:
%   colors = distinguishable_colors(n_colors)
% Specify the number of colors you want as a scalar, n_colors. This will
% generate an n_colors-by-3 matrix, each row representing an RGB
% color triple. If you don't precisely know how many you will need in
% advance, there is no harm (other than execution time) in specifying
% slightly more than you think you will need.
%
%   colors = distinguishable_colors(n_colors,bg)
% This syntax allows you to specify the background color, to make sure that
% your colors are also distinguishable from the background. Default value
% is white. bg may be specified as an RGB triple or as one of the standard
% "ColorSpec" strings. You can even specify multiple colors:
%     bg = {'w','k'}
% or
%     bg = [1 1 1; 0 0 0]
% will only produce colors that are distinguishable from both white and
% black.
%
%   colors = distinguishable_colors(n_colors,bg,rgb2labfunc)
% By default, distinguishable_colors uses the image processing toolbox's
% color conversion functions makecform and applycform. Alternatively, you
% can supply your own color conversion function.
%
% Example:
%   c = distinguishable_colors(25);
%   figure
%   image(reshape(c,[1 size(c)]))
%
% Example using the file exchange's 'colorspace':
%   func = @(x) colorspace('RGB->Lab',x);
%   c = distinguishable_colors(25,'w',func);

% Copyright 2010-2011 by Timothy E. Holy

% Parse the inputs
if (nargin < 2)
    bg = [1 1 1];  % default white background
else
    if iscell(bg)
        % User specified a list of colors as a cell aray
        bgc = bg;
        for i = 1:length(bgc)
            bgc{i} = parsecolor(bgc{i});
        end
        bg = cat(1,bgc{:});
    else
        % User specified a numeric array of colors (n-by-3)
        bg = parsecolor(bg);
    end
end

% Generate a sizable number of RGB triples. This represents our space of
% possible choices. By starting in RGB space, we ensure that all of the
% colors can be generated by the monitor.
n_grid = 30;  % number of grid divisions along each axis in RGB space
x = linspace(0,1,n_grid);
[R,G,B] = ndgrid(x,x,x);
rgb = [R(:) G(:) B(:)];
if (n_colors > size(rgb,1)/3)
    error('You can''t readily distinguish that many colors');
end

% Convert to Lab color space, which more closely represents human
% perception
if (nargin > 2)
    lab = func(rgb);
    bglab = func(bg);
else
    C = makecform('srgb2lab');
    lab = applycform(rgb,C);
    bglab = applycform(bg,C);
end

% If the user specified multiple background colors, compute distances
% from the candidate colors to the background colors
mindist2 = inf(size(rgb,1),1);
for i = 1:size(bglab,1)-1
    dX = bsxfun(@minus,lab,bglab(i,:)); % displacement all colors from bg
    dist2 = sum(dX.^2,2);  % square distance
    mindist2 = min(dist2,mindist2);  % dist2 to closest previously-chosen color
end

% Iteratively pick the color that maximizes the distance to the nearest
% already-picked color
colors = zeros(n_colors,3);
lastlab = bglab(end,:);   % initialize by making the "previous" color equal to background
for i = 1:n_colors
    dX = bsxfun(@minus,lab,lastlab); % displacement of last from all colors on list
    dist2 = sum(dX.^2,2);  % square distance
    mindist2 = min(dist2,mindist2);  % dist2 to closest previously-chosen color
    [~,index] = max(mindist2);  % find the entry farthest from all previously-chosen colors
    colors(i,:) = rgb(index,:);  % save for output
    lastlab = lab(index,:);  % prepare for next iteration
end

%-------------------------------------------------------------------------
function c = parsecolor(s)
if ischar(s)
    c = colorstr2rgb(s);
elseif isnumeric(s) && size(s,2) == 3
    c = s;
else
    error('MATLAB:InvalidColorSpec','Color specification cannot be parsed.');
end

%-------------------------------------------------------------------------
function c = colorstr2rgb(c)
% Convert a color string to an RGB value.
% This is cribbed from Matlab's whitebg function.
% Why don't they make this a stand-alone function?
rgbspec = [1 0 0;0 1 0;0 0 1;1 1 1;0 1 1;1 0 1;1 1 0;0 0 0];
cspec = 'rgbwcmyk';
k = find(cspec==c(1));
if isempty(k)
    error('MATLAB:InvalidColorString','Unknown color string.');
end
if k~=3 || length(c)==1,
    c = rgbspec(k,:);
elseif length(c)>2,
    if strcmpi(c(1:3),'bla')
        c = [0 0 0];
    elseif strcmpi(c(1:3),'blu')
        c = [0 0 1];
    else
        error('MATLAB:UnknownColorString', 'Unknown color string.');
    end
end


function [ element_node, element_att ] = triangle_element_data_example ( ...
  element_num, element_order, element_att_num )

%*****************************************************************************80
%
%% TRIANGLE_ELEMENT_DATA_EXAMPLE returns the element information for the example.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 October 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer ELEMENT_NUM, the number of elements.
%
%    Input, integer ELEMENT_ORDER, the order of the elements.
%
%    Input, integer ELEMENT_ATT_NUM, the number of element
%    attributes.
%
%    Output, integer ELEMENT_NODE(ELEMENT_ORDER,ELEMENT_NUM),
%    the indices of the nodes that make up each element.
%
%    Output, real ELEMENT_ATT(ELEMENT_ATT_NUM,ELEMENT_NUM),
%    the attributes of each element.
%
  element_att = [];

  element_node = [ ...
    1,  2,  6; ...
    7,  6,  2; ...
    2,  3,  7; ...
    8,  7,  3; ...
    3,  4,  8; ...
    9,  8,  4; ...
    4,  5,  9; ...
   10,  9,  5; ...
    6,  7, 11; ...
   12, 11,  7; ...
    7,  8, 12; ...
   13, 12,  8; ...
    8,  9, 13; ...
   14, 13,  9; ...
    9, 10, 14; ...
   15, 14, 10; ...
   11, 12, 16; ...
   17, 16, 12; ...
   12, 13, 17; ...
   18, 17, 13; ...
   16, 17, 19; ...
   20, 19, 17; ...
   17, 18, 20; ...
   21, 20, 18 ]';

  return
end


function varargout = multiwaveplot(varargin)
% Plot stacked waves from a multichannel matrix
%
%   multiwaveplot(wave)
%   multiwaveplot(x,y,wave)
%   multiwaveplot(...,gain)
%   multiwaveplot(...,mode)
%   h = multiwaveplot(...)
%
%   Multiwaveplot draws a series of stacked waves (one on
%   top of the other) contained in the rows of an input 2-D
%   matrix. Each wave has a designated row on the plot; the
%   first row is plotted at the bottom of the plot.
%
%   multiwaveplot(wave) draws a series of waves
%   contained in the rows of wave.
%
%   multiwaveplot(x,y,wave) plot the waves against the data
%   in x and y, such that x specifies the common x data, and
%   y determines the vertical position of each row. Hence,
%   length(x)==size(wave,2) and length(y)==size(wave,1).
%
%   multiwaveplot(...,gain) scales the height of each wave
%   according to gain. With gain=1 (default), the height of
%   the tallest wave will be limited so as not to encroach
%   on the adjacent wave; all other waves are scaled
%   accordingly. This can be overridden by specifying gain >
%   1.
%
%   multiwaveplot(...,mode) plots the data with the
%   specified mode. The default mode depends on gain: for
%   gain <= 1, mode defaults to 'plot' and plots lines for
%   each row of wave; for gain > 1, mode defaults to 'fill'
%   and instead plots white patch objects for each row of
%   wave, covering the area under each wave, such that waves
%   with a lower row index mask those with a higher row
%   index. This behaviour can be overridden by specifying
%   MODE as a string (either 'plot' or 'fill').
%
%   h = multiwaveplot(...) returns a vector of handles to
%   patch (for mode = 'fill') or lineseries (for mode =
%   'plot') graphics objects, one handle per wave.
%
%   See also FILL, PATCH, PLOT, IMAGESC.

% !---
% ==========================================================
% Last changed:     $Date: 2013-06-24 17:45:41 +0100 (Mon, 24 Jun 2013) $
% Last committed:   $Revision: 249 $
% Last changed by:  $Author: ch0022 $
% ==========================================================
% !---

%% Derive inputs

input_spec = 'X and Y must be vectors, WAVE must be a matrix, GAIN must be a scalar and MODE must be a string';

% MODE
mode = find_inputs(@ischar,varargin,...
    ['Unknown parameter specified. ' input_spec]);

% GAIN
gain = find_inputs(@(x)(isscalar(x) & ~ischar(x)),varargin,...
    ['Unknown parameter specified. ' input_spec]);

if isempty(gain)
    gain = 1;
end

% Condtionally set MODE depending on GAIN
if isempty(mode)
    if gain > 1
        mode = 'fill';
    else
        mode = 'plot';
    end
end

% WAVE
wave = find_inputs(@(x)(isnumeric(x) & all(numel(x)>size(x)) & length(size(x))<3),varargin,...
    ['Both X and Y must be specified. ' input_spec]);

assert(~isempty(wave),'Please specify WAVE (which should be a matrix)')

[r,c] = size(wave);

% X and Y
[x,y] = find_inputs(@(x)(~isscalar(x) & isvector(x) & ~ischar(x)),varargin,...
    ['Both X and Y must be specified. ' input_spec]);

if isempty(x)
    x = 1:c;
    y = 1:r;
end

%% Plot

if min(wave(:))>=0
    adjust = 1; % for positive data (e.g. correlograms)
else
    adjust = 0.5; % for wave data varying on zero
end

for n = 1:r
    % just in case all values are zero
    if ~all(wave(n,:)==0)
        wave(n,:) = (adjust/max(abs(wave(:)))).*wave(n,:);
    end
end

scale = zeros(r,1); % this parameter allows for non-linear y-values, scaling each channel accordingly
h = zeros(r,1);

axis_min = y(1)-(adjust*(y(2)-y(1)));

hold on;

for n = r:-1:1
    try % calculate scaling factor
        scale(n) = y(n+1)-y(n);
    catch
        scale(n) = y(n)-y(n-1);
    end
    wave(n,:) = wave(n,:).*scale(n);
    switch mode % plot
        case 'plot'
            h(n) = plot(x,gain.*wave(n,:)+y(n),'k');
        case 'fill'
            xa=[x(1) x(1:c) x(c) x(1)];
            ya=[axis_min gain.*wave(n,:)+y(n) axis_min axis_min];
            h(n) = fill(xa,ya,'w');
        otherwise
            error('Unknown MODE specified: must be ''fill'' or ''plot''')
    end
end

hold off
axis([x(1) x(c) axis_min max(y(r)+(gain.*wave(r,:)))]); % set axis limits
set(gca,'layer','top') % move axis to top layer
box on

varargout(1:nargout) = {h};

% end of multiwaveplot()

% ----------------------------------------------------------
% Local functions:
% ----------------------------------------------------------

% ----------------------------------------------------------
% find_inputs: validate and provide inputs (if any)
% ----------------------------------------------------------
function varargout = find_inputs(fhandle,input,msg)

indices = cellfun(fhandle,input);

if any(indices) % inputs are specified
    if sum(indices)~=nargout
        error(msg) % return error message
    end
    varargout(1:nargout) = input(indices);
else % unspecified returns empty matrix
    varargout(1:nargout) = {[]};
end

% [EOF]


function har=arrow(x,y,z,vx,vy,vz,th,lr,c,hpar)
% har=arrow(x,y,z,vx,vy,vz,th,lr,c)
% har - cell array of handles to objects
% draw 3d arrow
% (x,y,z) - point of arrow begin
% (vx,vy,vz) - vector for arrow
% th - tip height
% lr - line radiuse
% c - color
% hpar - parent

% xyz=[x;y;z];
v=[vx;vy;vz];
vl=sqrt(v'*v); % lenght of arrow, each size will be normalazed to vl
if vl~=0
    %warning('length of arrow equal to 0');

    n=20; % discretization for circle in tip (tip is cone)
    dfi=2*pi/n;
    r=0.3*th; % radius in tip
    h=th; % height of tip
    v0=v*((vl-h))/vl; % rest part vector

    % draw tip of arrow

    % find some vector-perpendicular to v:
    if (vx==0)&&(vy==0)
        vp=[0;vz;-vy];
    else
        vp=[vy;-vx;0];
    end
    vp=r*vp/sqrt(vp'*vp);

    vpa=vp; % vectors for draw circle

    for fi=dfi:dfi:2*pi
        vcr=cross(v,vp);
        vcrn=vcr/sqrt(vcr'*vcr);
        vp=vp*cos(dfi)+r*sin(dfi)*vcrn;
        vpa=[vpa,vp];
    end

    X=x+v0(1)+vpa(1,:);
    Y=y+v0(2)+vpa(2,:);
    Z=z+v0(3)+vpa(3,:);

    % hold off
    % plot3(X,Y,Z);

    % add vertex of tip:
    X=[X;(x+vx)*ones(size(X))];
    Y=[Y;(y+vy)*ones(size(Y))];
    Z=[Z;(z+vz)*ones(size(Z))];

    hs=surf(X,Y,Z,'parent',hpar);
    har{1}=hs;
    set(hs,'FaceColor',c,'EdgeColor','none');
    har{2}=fill3(X(1,:),Y(1,:),Z(1,:),c,'parent',hpar);

    % draw line:
    vpal=lr*vpa/r;
    Xl=x+vpal(1,:);
    Yl=y+vpal(2,:);
    Zl=z+vpal(3,:);

    Xl=[Xl;x+v0(1)+vpal(1,:)];
    Yl=[Yl;y+v0(2)+vpal(2,:)];
    Zl=[Zl;z+v0(3)+vpal(3,:)];

    hs=surf(Xl,Yl,Zl,'parent',hpar);
    har{3}=hs;
    set(hs,'FaceColor',c,'EdgeColor','none');
    har{4}=fill3(Xl(1,:),Yl(1,:),Zl(1,:),c,'parent',hpar);
    har{5}=fill3(Xl(2,:),Yl(2,:),Zl(2,:),c,'parent',hpar);

else % zero length

    n=20; % discretization for circle in tip (tip is cone)
    dfi=2*pi/n;
    r=0.3*th; % radius in tip
    h=th; % height of tip

    % instead of tip of arrow:
    har{1}=spherei(x,y,z,r,c,1,hpar);
    crc=circle(0,0,r);
    har{2}=fill3(x+crc(:,1),y+crc(:,2),z+zeros(length(crc(:,1))),c,'parent',hpar,'EdgeColor','none');

    % insrtead of line:
    har{3}=spherei(x,y,z,r,c,1,hpar);
    har{4}=fill3(x+crc(:,1),y+crc(:,2),z+zeros(length(crc(:,1))),c,'parent',hpar,'EdgeColor','none');
    har{5}=fill3(x+crc(:,1),y+crc(:,2),z+zeros(length(crc(:,1))),c,'parent',hpar,'EdgeColor','none');
end


function [net, stats] = cnn_toy_data(varargin)
% CNN_TOY_DATA
% Minimal demonstration of MatConNet training of a CNN on toy data.
%
% It also serves as a short tutorial on creating and using a custom imdb
% (image database).
%
% The task is to distinguish between images of triangles, squares and
% circles.

% Copyright (C) 2017 Joao F. Henriques.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

run([fileparts(mfilename('fullpath')) '/../../matlab/vl_setupnn.m']) ;

% Parameter defaults. You can add any custom parameters here (e.g.
% opts.alpha = 1), and change them when calling: cnn_toy_data('alpha', 2).
opts.train.batchSize = 200 ;
opts.train.numEpochs = 10 ;
opts.train.continue = true ;
opts.train.gpus = [] ;
opts.train.learningRate = 0.01 ;
opts.train.expDir = [vl_rootnn '/data/toy'] ;
opts.dataDir = [vl_rootnn '/data/toy-dataset'] ;
[opts, varargin] = vl_argparse(opts, varargin) ;

opts.imdbPath = [opts.train.expDir '/imdb.mat'] ;
opts = vl_argparse(opts, varargin) ;

% --------------------------------------------------------------------
%                                                         Prepare data
% --------------------------------------------------------------------

% Generate images if they don't exist (this would be skipped for real data)
if ~exist(opts.dataDir, 'dir')
  mkdir(opts.dataDir) ;
  cnn_toy_data_generator(opts.dataDir) ;
end

% Create image database (imdb struct). It can be cached to a file for speed
if exist(opts.imdbPath, 'file')
  disp('Reloading image database...')
  imdb = load(opts.imdbPath) ;
else
  disp('Creating image database...')
  imdb = getImdb(opts.dataDir) ;
  mkdir(fileparts(opts.imdbPath)) ;
  save(opts.imdbPath, '-struct', 'imdb') ;
end

% Create network (see HELP VL_SIMPLENN)
f = 1/100 ;
net.layers = {} ;
net.layers{end+1} = struct('type', 'conv', ...
                           'weights', {{f*randn(5,5,1,5, 'single'), zeros(1, 5, 'single')}}) ;
net.layers{end+1} = struct('type', 'pool', ...
                           'method', 'max', ...
                           'pool', [2 2], ...
                           'stride', 2) ;
net.layers{end+1} = struct('type', 'conv', ...
                           'weights', {{f*randn(5,5,5,10, 'single'),zeros(1,10,'single')}}) ;
net.layers{end+1} = struct('type', 'pool', ...
                           'method', 'max', ...
                           'pool', [2 2], ...
                           'stride', 2) ;
net.layers{end+1} = struct('type', 'conv', ...
                           'weights', {{f*randn(5,5,10,3, 'single'),  zeros(1,3,'single')}}) ;
net.layers{end+1} = struct('type', 'softmaxloss') ;

% Fill in any values we didn't specify explicitly
net = vl_simplenn_tidy(net) ;

% --------------------------------------------------------------------
%                                                                Train
% --------------------------------------------------------------------

use_gpu = ~isempty(opts.train.gpus) ;

% Start training
[net, stats] = cnn_train(net, imdb, @(imdb, batch) getBatch(imdb, batch, use_gpu), ...
  'train', find(imdb.set == 1), 'val', find(imdb.set == 2), opts.train) ;

% Visualize the learned filters
figure(3) ; vl_tshow(net.layers{1}.weights{1}) ; title('Conv1 filters') ;
figure(4) ; vl_tshow(net.layers{3}.weights{1}) ; title('Conv2 filters') ;
figure(5) ; vl_tshow(net.layers{5}.weights{1}) ; title('Conv3 filters') ;

% --------------------------------------------------------------------
function [images, labels] = getBatch(imdb, batch, use_gpu)
% --------------------------------------------------------------------
% This is where we return a given set of images (and their labels) from
% our imdb structure.
% If the dataset was too large to fit in memory, getBatch could load images
% from disk instead (with indexes given in 'batch').

images = imdb.images(:,:,:,batch) ;
labels = imdb.labels(batch) ;

if use_gpu
  images = gpuArray(images) ;
end

% --------------------------------------------------------------------
function imdb = getImdb(dataDir)
% --------------------------------------------------------------------
% Initialize the imdb structure (image database).
% Note the fields are arbitrary: only your getBatch needs to understand it.
% The field imdb.set is used to distinguish between the training and
% validation sets, and is only used in the above call to cnn_train.

% The sets, and number of samples per label in each set
sets = {'train', 'val'} ;
numSamples = [1500, 150] ;

% Preallocate memory
totalSamples = 4950 ;  % 3 * 1500 + 3 * 150
images = zeros(32, 32, 1, totalSamples, 'single') ;
labels = zeros(totalSamples, 1) ;
set = ones(totalSamples, 1) ;

% Read all samples
sample = 1 ;
for s = 1:2  % Iterate sets
  for label = 1:3  % Iterate labels
    for i = 1:numSamples(s)  % Iterate samples
      % Read image
      im = imread(sprintf('%s/%s/%i/%04i.png', dataDir, sets{s}, label, i)) ;

      % Store it, along with label and train/val set information
      images(:,:,:,sample) = single(im) ;
      labels(sample) = label ;
      set(sample) = s ;
      sample = sample + 1 ;
    end
  end
end

% Show some random example images
figure(2) ;
montage(images(:,:,:,randperm(totalSamples, 100))) ;
title('Example images') ;

% Remove mean over whole dataset
images = bsxfun(@minus, images, mean(images, 4)) ;

% Store results in the imdb struct
imdb.images = images ;
imdb.labels = labels ;
imdb.set = set ;


% Digital circuit sizing example (GP)
% Boyd, Kim, Patil, and Horowitz, "Digital circuit optimization
% via geometric programming"
% Written for CVX by Almir Mutapcic 02/08/06
%
% Solves the problem of choosing gate scale factors x_i to give
% minimum ckt delay, subject to limits on the total area and power.
% Uses max gate arrival time T formulation that avoids evaluation
% of the delay over all paths in the circuit.
%
%   minimize   T_bar
%       s.t.   T_j <= T_bar      for j an output gate
%              T_j + d_i <= T_i  for j in FI(i)
%              P <= Pmax, A <= Amax
%              x >= 1
%
% where variables are x and T.
%
% We use the circuit topology presented in figure 1 (page 902),
% where we take gates 1, 3 and 6 to be inverters (INV),
% gates 2 and 7 to be three input NANDs (NAND3),
% and gates 4 and 5 to be two input NORs (NOR2).

%********************************************************************
% user specified data (specify problem constant and ckt topology)
%********************************************************************
m = 7;        % number of gates
Vdd = 5;      % supply voltage
Amax = 250;   % maximum area spec

% gate specs
INV   = struct('Cin',3, 'Cint',3, 'Rdrv',0.48, 'A',3,  'Ileak',0.006);
NAND3 = struct('Cin',4, 'Cint',6, 'Rdrv',0.48, 'A',8,  'Ileak',0.007);
NOR2  = struct('Cin',5, 'Cint',6, 'Rdrv',0.48, 'A',10, 'Ileak',0.009);

clear gates;
gates([1 3 6]) = INV;
gates([2 7])   = NAND3;
gates([4 5])   = NOR2;

% primary inputs and primary outputs labels (start with m+1)
primary_inputs = [8 9 10];
primary_outputs = [11 12];
M = m + length( primary_inputs ) + length( primary_outputs );

% fan-in cell array
FI = cell(M,1);
FI{1} = 8;
FI{2} = [8 9 10];
FI{3} = 10;
FI{4} = [1 2];
FI{5} = [2 3];
FI{6} = 4;
FI{7} = [3 4 5];
FI{8} = [];
FI{9} = [];
FI{10} = [];
FI{11} = 6;
FI{12} = 7;

% primary output has Cin capacitance (but has no Cload)
Cin_po = sparse(M,1);
Cin_po(primary_outputs) = [10 10];

% primary input has Cload capacitance (but has no Cin)
Cload_pi = sparse(M,1);
Cload_pi(primary_inputs) = [10 10 10];

% activity frequency of gates and primary inputs
f_gates = 0.001*ones(m,1);
f_pi = sparse(M,1);
f_pi(primary_inputs) = 0.001*[10 10 10];

%********************************************************************
% derived problem data (computed from user inputs)
%********************************************************************
% fan-out cell array (compute it from the fan-in cell array)
FO = cell(M,1);
for gate = [1:m primary_outputs]
  preds = FI{gate};
  for k = 1:length(preds)
    FO{preds(k)}(end+1) = gate;
  end
end

% input and internal capacitance of gates, and driving resistance
Cin_norm  = [gates.Cin]';
Cint_norm = [gates.Cint]';
Rdrv_norm = [gates.Rdrv]';

% area specification for each gate with unit scaling
A_norm = [gates.A]';

% leakage current of gate i with unit scaling
Ileak_norm = [gates.Ileak]';

%********************************************************************
% optimization (with tradeoff curve generation)
%********************************************************************
% objective is the upper bound on the overall delay
% and that is the max of arrival times for output gates
output_gates = [FI{primary_outputs}];

% varying parameters for the tradeoff curve
N = 25;
Pmax = linspace(10,20,N);
min_delay = zeros(N,1);
disp('Generating the optimal tradeoff curve...')
for n = 1:N
  fprintf('Pmax = %6.2f: ',Pmax(n));
  cvx_begin gp quiet
    % optimization variables
    variable x(m)                 % scale factor
    variable T(m)                 % arrival times

    % input capacitance is an affine function of sizes
    Cin  = Cin_norm.*x;
    Cint = Cint_norm.*x;

    % driving resistance is inversily proportional to sizes
    R = Rdrv_norm./x;

    % gate delay is the product of its driving resistance and load cap.
    Cload = cvx( zeros(m,1) );
    for gate = 1:m
      if ~ismember( FO{gate}, primary_outputs )
        Cload(gate) = sum( Cin(FO{gate}) );
      else
        Cload(gate) = Cin_po( FO{gate} );
      end
    end

    % delay
    D = 0.69*ones(m,1).*R.*( Cint + Cload );

    % total area
    area = A_norm'*x;

    % total power calculation
    Pdyn = Vdd^2*sum( f_pi(primary_inputs).*Cload_pi(primary_inputs) ) + ...
           Vdd^2*(f_gates'*(Cint + Cload));
    Pstat = Vdd*Ileak_norm'*x;
    power = Pdyn + Pstat;

    minimize( max( T(output_gates) ) )
    subject to
      % constraints
      x >= 1; %#ok
      area <= Amax; %#ok
      power <= Pmax(n); %#ok

      % create timing constraints
      for gate = 1:m
        if ~ismember( FI{gate}, primary_inputs )
          for j = FI{gate}
            % enforce T_j + D_j <= T_i over all gates j that drive i
            D(gate) + T(j) <= T(gate); %#ok
          end
        else
          % enforce D_i <= T_i for gates i connected to primary inputs
          D(gate) <= T(gate); %#ok
        end
      end
  cvx_end
  fprintf( 'delay = %3.2f\n', cvx_optval );
  min_delay(n) = cvx_optval;
end

% plot the tradeoff curve
figure, clf
plot(Pmax,min_delay);
xlabel('Pmax'); ylabel('Dmin');
title(['Tradeoff curve for Amax = ' num2str(Amax)])
disp('Optimal tradeoff curve plotted.')


function stepplot

rr=findobj(gcf,'Tag','edit1');
r1=get(rr,'string');% get the value of numenator of G(s)
rr=findobj(gcf,'Tag','edit2');
r2=get(rr,'string');% get the value of denomenator of G(s

numg=str2num(r1); %numenator of G(s)
deng=str2num(r2); %denomenator of G(s)

rr=findobj(gcf,'Tag','edit3');
r4=get(rr,'string');% get the value of numenator of H(s)
rr=findobj(gcf,'Tag','edit4');
r5=get(rr,'string');% get the value of denomenator of H(s)

numh=str2num(r4);%numenator of H(s)
denh=str2num(r5);%denomenator of H(s)
h=tf(numh,denh); % H(s)

rr=findobj(gcf,'Tag','slider1');
r6=get(rr,'value');% get the value of proportional controller
rr=findobj(gcf,'Tag','slider2');
r7=get(rr,'value');% get the value of integrater controller
rr=findobj(gcf,'Tag','slider3');
r8=get(rr,'value');% get the value of defferentiatr controller

Kp=r6;
Ki=r7;
Kd=r8;

numcf=[Kd Kp Ki];         % numinator of PID
dencf=[1 0];              % denomenator of PID
numf=conv(numcf,numg);
denf=conv(dencf,deng);
fsys=tf(numf,denf); % forward transfare function.
csys=feedback(fsys,h);
[y,t] = step(csys); % step response
plot(t,y)


classdef PathVertexesToBoundaryComputer < handle

    properties (Access = public)

    end

    properties (Access = private)
       mesh
       singularityCoord
    end

    properties (Access = private)
       boundaryPointCoord
       pathVector
       closestVertex
       boundaryNodes
       connectedVertex
       pathVertexes
       pathCells
    end

    methods (Access = public)

        function obj = PathVertexesToBoundaryComputer(cParams)
            obj.init(cParams)
        end

        function v = compute(obj)
            obj.computeBoundaryPointCoord();
            obj.createStraightPathVector();
            obj.computeClosestVertex();
            obj.computeBoundaryNodes();
            obj.computeMeshEdges();
            obj.computePath();
            v = obj.pathVertexes;
        end

        function plot(obj)
            figure()
            hold on
            obj.plotMesh();
            obj.plotStraightPath();
            obj.plotClosestVertex();
            obj.plotVerticesPath();
        end

    end

    methods (Access = private)

        function init(obj,cParams)
            obj.mesh               = cParams.mesh;
            obj.singularityCoord   = cParams.singularityCoord;
        end

        function computeBoundaryPointCoord(obj)
        %    obj.computeToyBoundaryPoint();
            obj.computeBenchmarkBoundaryPoint();
        end

        function computeToyBoundaryPoint(obj)
            obj.boundaryPointCoord = [0 150];
        end

        function computeBenchmarkBoundaryPoint(obj)
           obj.boundaryPointCoord(:,2) = obj.singularityCoord(:,2);
           obj.boundaryPointCoord(:,1) = max(obj.mesh.coord(:,1));
        end

        function createStraightPathVector(obj)
            OA = obj.singularityCoord;
            OB = obj.boundaryPointCoord;
            AB = OB - OA;
            obj.pathVector = AB ;
        end

        function computeClosestVertex(obj)
            xy  = obj.mesh.coord;
            xyS = obj.singularityCoord;
            xD  = xy(:,1) - xyS(:,1);
            yD  = xy(:,2) - xyS(:,2);
            dist = sqrt(xD.^2 + yD.^2);
            [~,iD] = min(dist);
            obj.closestVertex = iD;
        end

        function computeBoundaryNodes(obj)
            coord  = obj.mesh.coord;
            nodesB = boundary(coord);
            obj.boundaryNodes = nodesB;
        end

        function computeMeshEdges(obj)
            obj.mesh.computeEdges();
        end

        function itIs = isInBoundary(obj,node)
            nodesB = obj.boundaryNodes;
            itIs = any(node == nodesB);
        end

        function computePath(obj)
            i = 1;
            vertex       = obj.closestVertex;
            pVertexes    = vertex;
            while ~obj.isInBoundary(vertex)
                otherVertex = obj.mesh.computeConnectedVertex(vertex);
                iD          = obj.computeOptimalVertex(vertex,otherVertex);
                newVertex   = otherVertex(iD);
                i = i + 1;
                vertex       = newVertex;
                pVertexes(i) = vertex;
            end
            obj.pathVertexes = pVertexes;
        end

        function iD = computeOptimalVertex(obj,currentVertex,trialVertexes)
            s.currentVertexCoord  = obj.mesh.coord(currentVertex,:);
            s.trialVertexesCoord  = obj.mesh.coord(trialVertexes,:);
            s.boundaryVertexCoord = obj.boundaryPointCoord;
            s.lineVector          = obj.pathVector;
            n = NextVertexToBoundaryComputer(s);
            iD = n.compute();
        end

        function plotMesh(obj)
            obj.mesh.plot();
        end

        function plotStraightPath(obj)
            xP = [obj.singularityCoord(:,1),obj.boundaryPointCoord(:,1)];
            yP = [obj.singularityCoord(:,2),obj.boundaryPointCoord(:,2)];
            plot(xP,yP,'+-')
        end

        function plotClosestVertex(obj)
            cV = obj.closestVertex;
            x = obj.mesh.coord(cV,1);
            y = obj.mesh.coord(cV,2);
            plot(x,y,'r+')
        end

        function plotVerticesPath(obj)
            cV = obj.pathVertexes;
            x = obj.mesh.coord(cV,1);
            y = obj.mesh.coord(cV,2);
            plot(x,y,'g-','LineWidth',5)
        end

    end

end


function [soln,eqn,info] = DivCurl3PDWG(node,elem,bdFlag,pde,option,varargin)
%% DivCurl3PDWG div-curl system equation:
%        primal-dual weak-Galerkin in 3-D to solve the following system.
%       div(epsilon*u)=f  in \Omega,
%       curl(u) = g       in \Omega,
%       Dirichlet boundary condition epsilon*u \cdot n =g_n on \Gamma_0,
%
% The code is vectorized with no loops. Data structure follows iFEM tradition.
%
% Reference: A New Numerical Method for Div-Curl Systems with Low Regularity Assumptions
% S. Cao, C. Wang, J. Wang, https://arxiv.org/abs/2101.03466
%
% Copyright (C)  Long Chen. See COPYRIGHT.txt for details.

if ~exist('option','var'), option = []; end
if ~isfield(option,'inverseh'), option.inverseh = true; end
if ~isfield(option,'dquadorder'), option.dquadorder = 3; end

[lambda,w] = quadpts3(option.dquadorder); % quadrature order is 1 or 2
nQuad = size(lambda,1);

if ~exist('bdFlag','var')
    bdFlag = setboundary3(node,elem,'Dirichlet');
end

%% Mesh related geometric quantities

% local edge to face indices, first two edges' local indices on a face
loc_e2f = [4 5; 2 3; 1 3; 1 2];
% local face to vertices indices, positive oriented
loc_f2v = [2 3 4; 1 4 3; 1 2 4; 1 3 2];

loc_e2v = [1 2; 1 3; 1 4; 2 3; 2 4; 3 4];

[elem2face,face] = dof3face(elem);
NT = size(elem,1);
NF = size(face,1);
elem2intdof = reshape(1:3*NT, 3, NT)';

[Dphi,volume] = gradbasis3(node,elem);

hK = (6*volume).^(1/3);

elem2ve = zeros(NT,3,6);
for e = 1:6 % six edges
   elem2ve(:,:,e) = node(elem(:,loc_e2v(e,2)),:)-node(elem(:,loc_e2v(e,1)),:);
end

elem2ve = elem2ve./repmat(sqrt(sum(elem2ve.^2,2)),1,3); % normalize for basis

faceNormal = cross(node(face(:,2),:) - node(face(:,1),:),...
    node(face(:,3),:) - node(face(:,2),:),2);
faceArea = 0.5*sqrt(sum(faceNormal.^2,2));
faceArea2elem = faceArea(elem2face);
faceArea2elemTotal = sum(faceArea2elem,2);

center = (node(elem(:,1),:) + node(elem(:,2),:) + ...
    node(elem(:,3),:) + node(elem(:,4),:))/4;

%% permeability tensor
t = cputime;  % record assembling time

if ~isfield(pde,'Eps'), pde.Eps = []; end

if ~isempty(pde.Eps) && isnumeric(pde.Eps)
    Eps2elem = repmat(pde.Eps, [1,1,NT]);   % Eps is a constant matrix
    Eps2elem = permute(Eps2elem,[3,1,2]); % switch the element idx to the 1st dim
end

if ~isempty(pde.Eps) && ~isnumeric(pde.Eps)
    Eps2elem = zeros(NT,3,3);  %#ok<PREALL>
    % Eps2elem is a NTx3x3 array so that Eps2elem(i,:,:) is the tensor at the
    % center of i-th element
    Epstmp = arrayfun(@(rowidx) pde.Eps(center(rowidx,:)), ...
        1:size(center,1), 'UniformOutput',0);
    Eps2elem = cat(3,Epstmp{:});
    Eps2elem = permute(Eps2elem,[3,1,2]); % switch the element idx to the 1st dim
end

%% compute eps \nabla_w phi
EpsDphi = zeros(NT,3,4);
for i = 1:4
    EpsDphi(:,:,i) = sum(bsxfun(@times, Eps2elem, Dphi(:,:,i)), 2);
end

%% assembling
% DoF order: # of Dofs
% \lambda_h: NT + NF, local 1+4
% u_h: 3*NT
% s_h: s_0 + s_b;  NT + NF (simply-connected case), local 1+4
% q_h: q_0 + q_b; 3*NT + 2*NF (for H(curl) vector field)
% assembling indices following locally within each group

%% equation 1 for u_h and lambda_h

blockUlambda = sparse(4*NT+NF,4*NT+NF);  %#ok<NASGU>
% (u_h, \epsilon\nabla_w phi)
% + \sum h_T^{-1} <\lambda_0 - \lambda_b, \varphi_0 - \varphi_b>_{boundary T}

%% (u_h, \epsilon\nabla_w phi_{face})
% locally the basis for P_0(T)^3 is edge [1, 2], [1, 3], and [1, 4]
% size (NT+NF, 3*NT)
C = sparse(NF,3*NT); %(u_h, \epsilon\nabla_w phi_{face})

for j = 1:3 % constant vector space spanned by 3 edge vectors
    for i = 1:4 % face 1 to 4
        % Cij = (q_j, eps \nabla_w \phi_{F_i})
        % \nabla_w \phi_{F_i} = -3\nabla \phi_i

        Cij = dot(elem2ve(:,:,j),-3*EpsDphi(:,:,i),2).*volume;

        ii = double(elem2face(:,i));
        jj = elem2intdof(:,j);
        C = C + sparse(ii,jj,Cij,NF,3*NT);
    end
end

Ugradphi = [sparse(NT, 3*NT); C];

%% \sum h_T^{-1} <\lambda_0 - \lambda_b, \varphi_0 - \varphi_b>_{boundary T}
% this is essetially a mass matrix stab
Stab_Mh = sparse(NT+NF,NT+NF); %#ok<NASGU>

L0ij = faceArea2elemTotal./hK;
Lambda00 = sparse(1:NT, 1:NT, L0ij, NT, NT); %% <\lambda_0, \lambda_0>

Lambda0b = sparse(NT, NF); %% <\lambda_b, \lambda_0>
Lambdabb = sparse(NF, NF); %% <\lambda_b, \lambda_b>
for j = 1:4 % face 1 to 4
    jj = double(elem2face(:,j));
    Lbj = faceArea2elem(:,j)./hK; % = <\lambda_{F_j}, \lambda_0>
    Lambda0b = Lambda0b + sparse(1:NT, jj, Lbj, NT, NF);
    Lambdabb = Lambdabb + sparse(jj, jj, Lbj, NF, NF);
end

Stab_Mh = [Lambda00 -Lambda0b; -Lambda0b' Lambdabb];
% Stablization on M_h should have null space dimension 1 (global constant)

%% \sum h_T <s_0 - s_b, r_0 - r_b>_{boundary T}

Stab_Sh = sparse(NT+NF,NT+NF); %#ok<NASGU>

S0ij = faceArea2elemTotal.*hK;
S00 = sparse(1:NT, 1:NT, S0ij, NT, NT); %% <s_0, s_0>

Sb0 = sparse(NT, NF); %% <s_b, s_0>
Sbb = sparse(NF, NF); %% <s_b, s_b>
for j = 1:4 % face 1 to 4
    jj = double(elem2face(:,j));
    Sbj = faceArea2elem(:,j).*hK; % = <\lambda_{F_j}, \lambda_0>
    Sb0 = Sb0 + sparse(1:NT, jj, Sbj, NT, NF);
    Sbb = Sbb + sparse(jj, jj, Sbj, NF, NF);
end

Stab_Sh = [S00 -Sb0; -Sb0' Sbb];
% Stablization on s_h should have null space dimension 1 (global constant)

%% Equation 1 for u_h and q_h = {q_0, q_b}

%%  (u_h, \epsilon\nabla_w \times psi_{face})
% locally the basis for P_0(T)^3 is the edge vector representing
% the edge [1, 2], [1, 3], and [1, 4] (local indexing of iFEM tradition)
% locally the basis for the tangential component are
% the edge 1 and 2 vectors for the triangular face (local indexing on this face)
% size: (3*NT+2*NF, 3*NT)

D = sparse(2*NF,3*NT);
%(u_h, \nabla_w \times psi_{face}) with 2 basis vec at each face

for i = 1:4 % face 1 to 4
    for j = 1:3 % constant vector space spanned by 3 edge vectors
        for e = 1:2 % two basis vectors on each face
            % Dije = (q_j, eps \nabla_w \times \psi_{F_i,e})
            % \nabla_w \times [0, \psi_{F_i}] =
            % (rotation of \psi_{F_i} counter-clockwise by pi/2)*|F_i|/|K|
            % = 3 \psi_{F_i} \times \nabla phi_i

            curlpsiFe = 3*mycross(elem2ve(:,:,loc_e2f(i,e)), Dphi(:,:,i));
            Dije = dot(elem2ve(:,:,j),curlpsiFe,2).*volume;

            % k-th (global face indexing) face has 2 DoFs in W_h: 2k-1, and 2k
            ii = 2*double(elem2face(:,i)) - e + 1;
            jj = elem2intdof(:,j);
            D = D + sparse(ii,jj, Dije, 2*NF,3*NT);
        end
    end
end

Ucurlpsi = [sparse(3*NT, 3*NT); D];

%%  (\psi, \epsilon\nabla_w s_h)
Psigrads = sparse(3*NT+2*NF, NT+NF);

E = sparse(3*NT,NF);
for i = 1:3 % constant vector space spanned by 3 edge vectors W_{h,0}
    for j = 1:4 % face 1 to 4
        % Eij = (\psi_{K,i}, eps \nabla_w \phi_{F_j})
        % \nabla_w \phi_{F_j} = -3\nabla \phi_j

        Eij = dot(elem2ve(:,:,i),-3*EpsDphi(:,:,j),2).*volume;

        ii = elem2intdof(:,i);
        jj = double(elem2face(:,j));
        E = E + sparse(ii,jj,Eij,3*NT,NF);
    end
end

%%% E is C's transpose, the code above is for booking-keeping
E = C';
Psigrads(1:3*NT,NT+1:end) = E;

%% \sum h_T^{-1} <(q_0-q_b)\times n, (\psi_0-\psi_b)\times n>_{boundary T}

% \sum h_T^{-1} <q_0\times n, \psi_0 \times n>_{boundary T}
WS00 = sparse(3*NT, 3*NT);
for i = 1:3 % constant vector space spanned by 3 edge vectors W_{h,0}
    for j = 1:3 % constant vector space spanned by 3 edge vectors W_{h,0}
        S00ijface = 0;
        for f = 1:4
            psi0crossn = mycross(elem2ve(:,:,i), Dphi(:,:,f)); % test
            q0crossn = mycross(elem2ve(:,:,j), Dphi(:,:,f)); % trial

            S00ijface = S00ijface + 9*dot(q0crossn,psi0crossn,2)...
                .*volume.^2./faceArea2elem(:,f);
        end
        S00ijface = S00ijface./hK;
        ii = elem2intdof(:,i);
        jj = elem2intdof(:,j);
        WS00 = WS00 + sparse(ii,jj,S00ijface,3*NT, 3*NT);
    end
end

% \sum h_T^{-1} <q_b\times n, \psi_0 \times n>_{boundary T}
WSb0 = sparse(3*NT, 2*NF);
for i = 1:3 % constant vector space spanned by 3 edge vectors W_{h,0}
    for j = 1:4 % 4 face for W_{h,b} each basis has support only on F_j
        for e = 1:2 % each face has 2 DoFs
            psi0crossn = mycross(elem2ve(:,:,i), Dphi(:,:,j)); % test
            qbcrossn = mycross(elem2ve(:,:,loc_e2f(j,e)), Dphi(:,:,j)); % trial

            Sb0ijface = 9*dot(qbcrossn,psi0crossn,2)...
                .*volume.^2./faceArea2elem(:,j)./hK;

            ii = elem2intdof(:,i);
            % k-th (global face indexing) face has 2 DoFs
            jj = 2*double(elem2face(:,j)) - e + 1;
            WSb0 = WSb0 + sparse(ii,jj,Sb0ijface,3*NT,2*NF);
        end
    end
end

% \sum h_T^{-1} <q_b\times n, \psi_b \times n>_{boundary T}
WSbb = sparse(2*NF, 2*NF);
for f = 1:4
    % this term is non-zero only when both trial and test
    % are associated with the same face
    for i = 1:2 % each face has 2 DoFs
        for j = 1:2
            psibcrossn = mycross(elem2ve(:,:,loc_e2f(f,i)), Dphi(:,:,f));
            qbcrossn = mycross(elem2ve(:,:,loc_e2f(f,j)), Dphi(:,:,f));

            Sbbijface = 9*dot(qbcrossn,psibcrossn,2)...
                .*volume.^2./faceArea2elem(:,f)./hK;

            % k-th (global face indexing) face has 2 DoFs
            ii = 2*double(elem2face(:,f)) - i + 1;
            jj = 2*double(elem2face(:,f)) - j + 1;
            WSbb = WSbb + sparse(ii,jj,Sbbijface,2*NF,2*NF);
        end
    end
end

Stab_Wh = [WS00 -WSb0; -WSb0' WSbb];
% Stab_Wh should have null space dimension
% = span{gradient of continuous P_1 Langrange} = # node -1

%% overall stiffness matrix for the system
blockUlambda = [Stab_Mh Ugradphi; Ugradphi' sparse(3*NT,3*NT)];
blockOffDiag = sparse(4*NT+3*NF, 4*NT+NF);
blockOffDiag(NT+NF+1:end, NT+NF+1:end) = Ucurlpsi;
blockQs = [-Stab_Sh Psigrads'; Psigrads Stab_Wh];
bigA = [blockUlambda blockOffDiag'; blockOffDiag blockQs];

%% right hand sides

%% -(f,\varphi_0)
ft = zeros(NT,1);

for p = 1:nQuad
		% quadrature points in the x-y-z coordinate
		pxyz = lambda(p,1)*node(elem(:,1),:) ...
			 + lambda(p,2)*node(elem(:,2),:) ...
			 + lambda(p,3)*node(elem(:,3),:) ...
             + lambda(p,4)*node(elem(:,4),:);
		fp = pde.f(pxyz);
        ft = ft + w(p)*fp;
end

Fphi0 = -accumarray((1:NT)',ft.*volume,[NT 1]);

%% \sum_{T} <eps u\cdot n, \varphi_b>_{\partial T} near \partial \Omega
GNphib = zeros(NF,1); % <eps u\cdot n, \varphi_F>_{\partial T}
% update: face quadorder increased

if ~isfield(option,'gNquadorder')
    option.gNquadorder = 5;   % default order exact for linear gN
end
[lambdagN, weightgN] = quadpts(option.gNquadorder);               % linear bases
nQuadgN = size(lambdagN,1);

for i = 1:4 % 4 faces
    idxBdElem = find(bdFlag(:,i) > 0); % if an element has a boundary face

    if ~isempty(idxBdElem)
        bdNormal = -3*repmat(volume(idxBdElem,:),1,3).*Dphi(idxBdElem,:,:);
        % scaled normal with face area built-in
        loc_face = loc_f2v(i,:);

        EpsUdotN = zeros(size(idxBdElem,1),1);

        for pp = 1:nQuadgN
            % quadrature points in the x-y coordinate
            ppxyz = lambdagN(pp,1)*node(elem(idxBdElem,loc_face(1)),:) ...
                + lambdagN(pp,2)*node(elem(idxBdElem,loc_face(2)),:) ...
                + lambdagN(pp,3)*node(elem(idxBdElem,loc_face(3)),:);

            u_bd = pde.exactu(ppxyz);
            EpsU = sum(bsxfun(@times, Eps2elem(idxBdElem,:,:), u_bd), 2);
            EpsUdotN = EpsUdotN + ...
                weightgN(pp)*dot(squeeze(EpsU), bdNormal(:,:,i), 2);

        end

        GNphib = GNphib + accumarray(elem2face(idxBdElem,i), EpsUdotN, [NF 1]);
    end
end

%% (g, \psi_0)
Gpsi0 = zeros(3*NT,1);

for i = 1:3 % 3 vector basis on each element for \psi_0
    gtpsi0 = zeros(NT,1);
    for p = 1:nQuad
        % quadrature points in the x-y-z coordinate
        pxyz = lambda(p,1)*node(elem(:,1),:) ...
            + lambda(p,2)*node(elem(:,2),:) ...
            + lambda(p,3)*node(elem(:,3),:) ...
            + lambda(p,4)*node(elem(:,4),:);
        gp = pde.g(pxyz);

        gtpsi0 = gtpsi0 + w(p)*dot(gp, elem2ve(:,:,i), 2);
    end

    Gpsi0 = Gpsi0 + accumarray(elem2intdof(:,i), gtpsi0.*volume, [3*NT 1]);
end
%% RHS
Rhs = [Fphi0; GNphib]; % test: phi_0, phi_b
Rhs = [Rhs; zeros(3*NT, 1)]; % test: v
Rhs = [Rhs; zeros(NT, 1)]; % test: r_0
Rhs = [Rhs; zeros(NF, 1)]; % test: r_b
Rhs = [Rhs; Gpsi0]; % test: psi_0
Rhs = [Rhs; zeros(2*NF, 1)]; % test: psi_b

%% set up free dof
% DoF order: # of Dofs
% prefix with "idx": global indices
% the DoF variable itself is boolean (fastest on MATLAB)
% \lambda_h: NT + NF, local 1+4 (no BC)
% u_h: 3*NT (no BC)
% s_h: s_0 + s_b;  NT + NF (simply-connected case), s_b = 0 on Gamma_0
% q_h: q_0 + q_b; 3*NT + 2*NF (for H(curl) vector field) q_b\cross n =0
% assembling indices following locally within each group

isBdFace = false(NF,1);
isBdFace(elem2face(bdFlag(:) > 0)) = true;
idxBdFace = find(isBdFace);
idxBdFaceVec = [2*idxBdFace-1 2*idxBdFace]';
idxBdFaceVec = idxBdFaceVec(:);

freeDof = true(8*NT+4*NF,1);
idxBdSb = idxBdFace + 5*NT+NF; % boundary DoF indices for s_b
idxBdQb = idxBdFaceVec + 8*NT+2*NF; % boundary DoF indices for q_b

%%%%% fixing 1 in lambda_0 is enough, the others are optional %%%%%
%%% current config according to the paper: fixing 1 in lambda_0
%%% bd dofs of s and q
freeDof([1; idxBdSb; idxBdQb]) = false;

%%%%% fixing bd dofs for q
% freeDof([1; idxBdQb]) = false;

%%%%% fixing bd dofs for s
% freeDof([1; idxBdSb]) = false;

idxIntBdFace = [];
% old way of handling cavity (second betti number not zero)
if any(elem2face(bdFlag(:) == 2)) && false
    disp('Modifying cavity dofs')
    isIntBdFace = false(NF,1);
    isIntBdFace(elem2face(bdFlag(:) == 2)) = true;
    idxIntBdFace = find(isIntBdFace);
    idxIntBdSb = idxIntBdFace + 5*NT+NF;  % boundary DoF indices for s_b on cavity
    freeDof(idxIntBdSb) = true;
end

%% Record assembling time
assembleTime = cputime - t;
if ~isfield(option,'printlevel'), option.printlevel = 1; end
if option.printlevel >= 2
    fprintf('Time to assemble matrix equation %4.2g s\n',assembleTime);
end

%% direct solve
NDof = 8*NT+4*NF;

bigSoln = zeros(NDof,1);

[~,sys] = memory;
if sum(freeDof) > 2e6 && sys.PhysicalMemory.Available < 3e10
    % 2 million DoFs need about 27GB free memory to be solved
    % 3 million DoFs need about 39GB free memory
    fprintf('Number of DoF %d exceeds estimated max dofs allowed in memory, stop. \n',sum(freeDof));
    return
end
bigSoln(freeDof) = bigA(freeDof, freeDof)\Rhs(freeDof);

%% post-processing for cavity
% TO-DO: current code allows only 1 cavity
if any(elem2face(bdFlag(:) == 2))
    disp('Post-processing cavity dofs')
    intBdFaceVec = false(size(freeDof,1), 1);
    isIntBdFace = false(NF,1);
    isIntBdFace(elem2face(bdFlag(:) == 2)) = true;
    idxIntBdFace = find(isIntBdFace);
    idxIntBdSb = idxIntBdFace + 5*NT+NF;  % boundary DoF indices for s_b on cavity
    intBdFaceVec(idxIntBdSb) = true;

    AintBdFaceVec = bigA*intBdFaceVec;
    sbConst = dot((Rhs - bigA*bigSoln),AintBdFaceVec)/dot(AintBdFaceVec,AintBdFaceVec);
    fprintf('s_b on the interior boundary is %6.4g \n',sbConst);
    bigSoln = bigSoln + sbConst*intBdFaceVec;
end

%% computing error
errL2U = zeros(NT,1);

idxUDoF = (NT+NF+1:NT+NF+3*NT)';
uh = bigSoln(idxUDoF);
uhElem = zeros(NT,3);

idxq0DoF = (5*NT+2*NF+1:5*NT+2*NF+3*NT)';
qh0 = bigSoln(idxq0DoF);
qh0Elem = zeros(NT,3);

idxqbDoF = (8*NT+2*NF+1:8*NT+4*NF)';
qhb = bigSoln(idxqbDoF);

idxqDoF = (5*NT+2*NF+1:8*NT+4*NF)';
qh = bigSoln(idxqDoF);

idxlambdaDoF = (1:NT+NF)';
lambdah = bigSoln(idxlambdaDoF);

lambdah0 = bigSoln((1:NT)');
lambdahb = bigSoln((NT+1:NT+NF)');

idxsDoF = (4*NT+NF+1:5*NT+2*NF)';
sh = bigSoln(idxsDoF);
sh0 = bigSoln((4*NT+NF+1:5*NT+NF)');
shb = bigSoln((5*NT+NF+1:5*NT+2*NF)');

for k = 1:3 % three bases in each K
    uhElem = uhElem + repmat(uh(elem2intdof(:,k)),1,3).*elem2ve(:,:,k);
    qh0Elem = qh0Elem + repmat(qh0(elem2intdof(:,k)),1,3).*elem2ve(:,:,k);
end

[lambda,w] = quadpts3(option.dquadorder+2); % quadrature order is 1 or 2
nQuad = size(lambda,1);
for p = 1:nQuad
    pxyz = lambda(p,1)*node(elem(:,1),:) ...
        + lambda(p,2)*node(elem(:,2),:) ...
        + lambda(p,3)*node(elem(:,3),:) ...
        + lambda(p,4)*node(elem(:,4),:);

    up = pde.exactu(pxyz);
    EpsUminusUh = sum(bsxfun(@times, Eps2elem, up - uhElem), 2);
    errL2U = errL2U + w(p)*dot(squeeze(EpsUminusUh), up - uhElem, 2);
end

errL2U = sqrt(errL2U.*volume);
errL2UTotal = norm(errL2U);

% idxfreeFace = find(~isBdFace);
% idxFreeFaceVec = [2*idxfreeFace-1 2*idxfreeFace]';
% idxFreeFaceVec = idxFreeFaceVec(:);
% errorStabTotal = sqrt(qh(idxFreeFaceVec)'*...
%     Stab_Wh(idxFreeFaceVec,idxFreeFaceVec)*qh(idxFreeFaceVec)...
%     +lambdah'*Stab_Mh*lambdah);
errorStabTotal = sqrt(qh'*Stab_Wh*qh+lambdah'*Stab_Mh*lambdah);
errorsTotal = sqrt(sh'*Stab_Sh*sh);

%% Output information
if nargout == 1
    soln = bigSoln;
else
    soln = struct('u',uh,...
                  'uh2elem', uhElem,...
                  'q0',qh0,...
                  'qb',qhb,...
                  'lambda0',lambdah0,...
                  'lambdab',lambdahb,...
                  's0',sh0,...
                  'sb',shb);
    eqn = struct('A',bigA,'b',Rhs,...
                'face',face,...
                'freeDof',freeDof,...
                'bdDofScalar',idxBdFace,...
                'bdDofVec',idxBdFaceVec,...
                'bdDofCavity',idxIntBdFace,...
                'errorU',errL2UTotal, ...
                'errorqlambda', errorStabTotal,...
                'errorS', errorsTotal,...
                'errorUelem',errL2U,...
                'NFace',NF);
    info.assembleTime = assembleTime;
end

%% end of function
end


function confidences = test_boosted_dt_mc(classifier, features)
% confidences = test_boosted_dt_mc(classifier, features)
%
% Returns a log likelihod ratio for each class in the classifier
%
% Input:
%  classifier: boosted decision tree classifier
%  features:   classifier features (ndata, nvariables)
% Output:
%   confidences(ndata, nclasses):
%      P(class=k|features) \propto 1./(1+exp(-confidences(k)))

npred = classifier.wcs(1).dt.npred;
if size(features, 2)~=npred
    error('Incorrect number of attributes')
end

wcs = classifier.wcs;
nclasses = size(wcs, 2);

ntrees = size(wcs, 1);

confidences = zeros(size(features, 1), nclasses);
for c = 1:nclasses
    for t = 1:ntrees
        if ~isempty(wcs(t,c).dt)
            if 1
                dt = wcs(t,c).dt;
%                 dt = tree_getNewVersion(dt);
                [var, cut, children, catsplit] = tree_getParameters(dt);
                nodes = treevalc(int32(var), cut, int32(children(:, 1)), ...
                        int32(children(:, 2)), catsplit(:, 1), features');
                %disp(num2str(nodes));
            else
                [class_indices, nodes, classes] = treeval(wcs(t, c).dt, features);
            end
            confidences(:, c) = confidences(:, c) + wcs(t, c).confidences(nodes);
        end
    end
    confidences(:, c) = confidences(:, c) + classifier.h0(c);
end


function [] = Example_ABOD()
    fprintf('In this demo, we will show you how to use ABOD and fastABOD.\n');
    fprintf('\n');
    fprintf('Behold, red "+" expresses anomaly points and blue "+" expresses normal points.\n');
    fprintf('\n');
    fprintf('ABOD with baseline dataset demo will start.\n');
    fprintf('\n');
    fprintf('Baseline.mat contains 6 points.\n');
    fprintf('\n');
    fprintf(' 1  0\n');
    fprintf(' 0  1\n');
    fprintf(' 0  0\n');
    fprintf('-1  0\n');
    fprintf(' 0 -1\n');
    fprintf(' 5  5\n');
    fprintf('\n');
    fprintf('First 5 points are very close to each other and aroind the origin,\n');
    fprintf('and 6th point is far away from origin.\n');
    fprintf('\n');
    fprintf('So obviously 6th will be treat as anomaly.\n');
    fprintf('\n');

    baseline = [1 0;0 1;0 0;-1 0;0 -1;5 5];
    [suspicious_index abof]=ABOD(baseline);
    anomaly=suspicious_index(1);
    normal=suspicious_index(2:end);
    plot(baseline(anomaly,1),baseline(anomaly,2),'r +',baseline(normal,1),baseline(normal,2),'b +');
    axis([-2, 6, -2, 6]);

    fprintf('\n');
    fprintf('\n');
    fprintf('\n');
    fprintf('Press Enter before continuing\n');
    pause;

    fprintf('Loading Iris dataset from UCI\n');
    fprintf('\n');

    load('iris.mat', '-mat');
    fprintf('Processing...\n');
    fprintf('\n');
    fprintf('\n');
    fprintf('\n');

    [suspicious_index abof]=ABOD(iris(:,[1;2;5]));
    anomaly=suspicious_index(1:15,1);
    normal=suspicious_index(16:end,1);
    plot3(iris(anomaly(:,1),1),iris(anomaly(:,1),2),iris(anomaly(:,1),5),'r +',iris(normal(:,1),1),iris(normal(:,1),2),iris(normal(:,1),5),'b +');

    fprintf('ABOD with iris dataset demo(1) will start.\n');
    fprintf('\n');
    fprintf('In iris demo(1), only sepal length, sepal width, class will be considered.\n');
    fprintf('\n');
    fprintf('Other attributes will be used in demo(2), so that we can plot points in 3d graph for you.\n');
    fprintf('\n');
    fprintf('Because this dataset contains 3 class, you will see 3 groups in graph.\n');
    fprintf('\n');
    fprintf('And each group has its anomaly.\n');
    fprintf('\n');

    fprintf('\n');
    fprintf('\n');
    fprintf('\n');
    fprintf('Press Enter before ABOD with iris dataset demo(2).\n');
    pause;

    fprintf('Processing...\n');
    fprintf('\n');
    fprintf('\n');
    fprintf('\n');

    [suspicious_index abof]=ABOD(iris(:,[3;4;5]));
    anomaly=suspicious_index(1:15,1);
    mormal=suspicious_index(16:end,1);
    plot3(iris(anomaly(:,1),3),iris(anomaly(:,1),4),iris(anomaly(:,1),5),'r +',iris(normal(:,1),1),iris(normal(:,1),3),iris(normal(:,1),3),'b +');

    fprintf('In iris demo(2), only petal length, petal width, class will be considered.\n');
    fprintf('\n');

    fprintf('Now you already know how to use ABOD.\n');
    fprintf('\n');
    fprintf('fastABOD is a approximation version of ABOD, so basicly they are same thing.\n');
    fprintf('\n');
    fprintf('You can find all detail information in ABOD.m and fastABOD.m\n');
    fprintf('\n');
    fprintf('Enjoy! :)\n');
    fprintf('\n');


function [ordered_alpha,index_alpha]=order_subsets(angles,blocksize, mode)
index_alpha=1:size(angles,2);

block_alpha=mat2cell(angles     ,size(angles,1),[repmat(blocksize,1,floor(size(angles,2)/blocksize)) mod(size(angles,2),blocksize)]);
index_alpha=mat2cell(index_alpha,1,[repmat(blocksize,1,floor(size(angles,2)/blocksize)) mod(size(angles,2),blocksize)]);

block_alpha=block_alpha(~cellfun('isempty',block_alpha));
index_alpha=index_alpha(~cellfun('isempty',index_alpha));

if strcmp(mode,'ordered')
    ordered_alpha=block_alpha;
    return;
end
if strcmp(mode,'random')
    neworder=randperm(length(block_alpha));
    ordered_alpha=block_alpha(neworder);
    index_alpha=index_alpha(neworder);
    return;
end
%% not finished

if strcmp(mode,'angularDistance')
    % we need them sorted, so we need to recompute the blocks, but sorted
    [angles,sortindex] = sortrows(angles.',1);
    angles=angles.';sortindex=sortindex.';
    index_alpha=1:size(angles,2);
    index_alpha=index_alpha(sortindex);

    block_alpha=mat2cell(angles      ,size(angles,1),[repmat(blocksize,1,floor(size(angles,2)/blocksize)) mod(size(angles,2),blocksize)]);
    index_alpha=mat2cell(index_alpha,1,[repmat(blocksize,1,floor(size(angles,2)/blocksize)) mod(size(angles,2),blocksize)]);

    block_alpha=block_alpha(~cellfun('isempty',block_alpha));
    index_alpha=index_alpha(~cellfun('isempty',index_alpha));

    avrg=cellfun(@mean,block_alpha);
    used_avrg=[];
    % start from the beggining
    ordered_alpha{1}=block_alpha{1};
    auxindex_alpha=index_alpha;
    index_alpha{1}=auxindex_alpha{1};
    used_avrg(end+1)=avrg(1);
    for ii=2:length(block_alpha)
        dist=[];
        for jj=1:length(used_avrg)
            dist(jj,:)=abs(mod((avrg- used_avrg(jj))+pi,2*pi)-pi);
        end
        dist=bsxfun(@times,dist,all(dist,1));
        [~,midx]=max(dist(:));
        [~,avrgindx]=ind2sub(size(dist),midx);
        index_alpha{ii}=auxindex_alpha{avrgindx};
        ordered_alpha{ii}=block_alpha{avrgindx};
        used_avrg(end+1)=avrg(avrgindx);
    end
end

end


%% ------ monetary policy variance and coefs switch independently ------ %%
%
% Both coefficients and variances in monetary policy equation
% change with two independent Markov processes

%% housekeeping
clear
close all
clc()

%% Create dataset
clc

do_plot=true;

scale=100;

[db,varlist0]=create_dataset(scale,do_plot);

varlist=fieldnames(varlist0);

%% set up Markov chains

markov_chains=struct('name','mpcoef',...
    'number_of_states',2,...
    'controlled_parameters',{{'c(1)','a0(1)','a1(1)','a2(1)'}},...
    'endogenous_probabilities',[],...
    'probability_parameters',[]);

markov_chains(2)=struct('name','mpvol',...
    'number_of_states',3,...
    'controlled_parameters',{{'s(1)'}},...
    'endogenous_probabilities',[],...
    'probability_parameters',[]);

%% Create the VAR

clc

nlags=2;

exog={};

constant=true;

panel=[];

sv0=svar(varlist,exog,nlags,constant,panel,markov_chains);

%% set up restrictions

% syntax is alag(eqtn,vname,chain_name,state)
%------------------------------------------------
lin_restr=cell(0,1);

nonlin_restr={'a0(3,FFR)>=0'};

which_chain=strcmp('mpcoef',{markov_chains.name});
% first equation or "FFR" equation:
%----------------------------------
for istate=1:markov_chains(which_chain).number_of_states

    mystate=int2str(istate);

    lin_restr=[lin_restr
        {
        ['a1(1,pi,mpcoef,',mystate,')=0']
        ['a2(1,pi,mpcoef,',mystate,')=0']
        ['a1(1,ygap,mpcoef,',mystate,')=0']
        ['a2(1,ygap,mpcoef,',mystate,')=0']
        ['a2(1,FFR,mpcoef,',mystate,')=0']
        }
        ]; %#ok<AGROW>

    nonlin_restr=[nonlin_restr
        {
        ['a1(1,FFR,mpcoef,',mystate,')>=0']
        ['a1(1,FFR,mpcoef,',mystate,')<=1']
        }
        ]; %#ok<AGROW>
end

lin_restr=[lin_restr
    {
    % second equation or "pi" equation
    %----------------------------------
    'a0(2,FFR)=0'
    'a1(2,FFR)=0'
    'a2(2,FFR)=0'
    'a1(2,ygap)=0'
    'a2(2,ygap)=0'
    % third equation or "ygap" equation
    %-----------------------------------
    'a1(3,FFR)=0'
    'a2(3,FFR)=0'
    'a1(3,pi)=0'
    'a2(3,pi)=0'
    'a0(3,pi)+a0(3,FFR)=0'
    }
    ];

restrictions=[lin_restr;nonlin_restr];

%% set priors

% priors for the VAR coefficients
%--------------------------------
var_prior=svar.prior_template();
var_prior.type='sz';
% priors for the mpcoef transition probabilities
%------------------------------------------------
switch_prior=struct();
switch_prior.mpcoef_tp_1_2={0.5,0.1,0.3,'beta'};
switch_prior.mpcoef_tp_2_1={0.5,0.1,0.3,'beta'};
% priors for the mpvol transition probabilities
%------------------------------------------------
switch_prior.dirichlet_1={0.1,'mpvol_tp_1_2',0.2,'mpvol_tp_1_3',0.2};
switch_prior.dirichlet_2={0.1,'mpvol_tp_2_1',0.2,'mpvol_tp_2_3',0.2};
switch_prior.dirichlet_3={0.1,'mpvol_tp_3_1',0.2,'mpvol_tp_3_2',0.2};

prior=struct();

prior.var=var_prior;

prior.nonvar=switch_prior;

%% Find posterior mode
clc

sv=sv0;

sv=estimate(sv,db,{'1960Q1','2015Q2'},prior,restrictions);

%% estimates
clc

pmode=posterior_mode(sv)

%% Printing estimates
clc

print_structural_form(sv)

%% Printing solution
clc

print_solution(sv)

%% plot smoothed state and regime probabilities
clc

close all

plot_probabilities(sv)

%% plots probabilities against data
close all

plot_data_against_probabilities(sv,'regime')

%% Impulse responses

myirfs=irf(sv);

%% Posterior sampling

%% Marginal data density

%% Out-of sample forecasts

%% Conditional forecast


function distplot(y,cutoff,class,nid)

%DISTPLOT plots the vector y versus the index.
% At the height of the cutoff value a red vertical line is plotted.
%
% Required input arguments:
%       y  : the vector to be plotted
%  cutoff  : a cutoff value
%
% Optional input arguments:
%   class  : the class of the y-vector
%     nid  : number of points to be identified (Default value: 3)
%
% I/O: distplot(y,cutoff,class,nid)
%
% This function is part of LIBRA: the Matlab Library for Robust Analysis,
% available at:
%              http://wis.kuleuven.be/stat/robust.html
%
% Last update: 24/11/2003

set(gcf,'Name', 'Index plot of the distances', 'NumberTitle', 'off');
n=length(y);
if nargin==2
    laby='Distance';
    nid=3;
elseif nargin==3
    nid=3;
end
ymax=max([max(y),cutoff,2.5])*1.05;
plot(1:n,y,'o')
box on
xlabel('Index')
if strcmp(class,'MCDCOV')
    laby='Robust distance';
elseif strcmp(class,'COV')
    laby='Mahalanobis distance';
end
ylabel(laby)
xlim([-0.025*n,n*1.05]);
ylim([-0.025*ymax,ymax]);
plotnumbers(1:n,y,0,nid,1)
line([-0.025*n,n*1.05],repmat(max([cutoff,2.5]),1,2),'Color','r');
title([class]);


function aamax = ivec_amax ( n, a )

%*****************************************************************************80
%
%% IVEC_AMAX returns the largest magnitude in an I4VEC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, integer A(N), the vector to be searched.
%
%    Output, integer AAMAX, the value of the entry of largest magnitude.
%
  aamax = max ( abs ( a(1:n) ) );

  return
end


%
%  T = build_toep( c, d, n );
%
%  Given:
%    c - the nonzero part of a central column of a banded Toeplitz
%        matrix
%    d - index of c containing the diagonal element of T
%    n - dimension of the banded Toeplitz matrix
%
%  The banded Toeplitz matrix is constructed explicitly.
%
function T = build_toep( c, d, n )

m = length( c );

col = zeros(n,1);
row = col';
col(1:m-d+1,1) = c(d:m);
row(1,1:d) = c(d:-1:1)';
T = toeplitz( col, row );


function Mij = get_block(M, i, j, row_range, col_range)
	%% ================== File info ==========================
	% Author		: Tiep Vu (http://www.personal.psu.edu/thv102/)
	% Time created	: 1/27/2016 2:46:04 AM
	% Last modified	: 1/27/2016 2:46:09 AM
	% Description	: Get a block matrix of a big matrix X.
	% 	INPUT
	%		M        : the big matrix.
	%			M = [	M11, M12, ..., M1m]
	% 					M21, M22, ..., M2m]
	%					..................
	% 					Mn1, Mn2, ..., Mnm]
	%		i        : row block index
	%		j        : column block index
	%		row_range: a vector storing the last index of each block. row_range(1) = 0.
	%					i-th block is indexed by row_range(i)+1: row_range(i+1).
	%		col_range: a vector storing the last index of each block. row_range(1) = 0.
	%					i-th block is indexed by col_range(i)+1: col_range(i+1).
	% 	OUTPUT
	%		Mi: output block matrix
	%
	%% ================== end File info ==========================
	range1 = row_range(i) + 1: row_range(i + 1);
	range2 = col_range(j) + 1: col_range(j + 1);
	Mij = M(range1, range2);
end


function edge = clipLine3d(line, box)
%CLIPLINE3D Clip a line with a box and return an edge.
%
%   EDGE = clipLine3d(LINE, BOX);
%   Clips the line LINE with the bounds given in BOX, and returns the
%   corresponding edge.
%
%   If the line lies totally outside of the box, returns a 1-by-6 row array
%   containing only NaN's.
%
%   If LINE is a N-by-6 array, with one line by row, returns the clipped
%   edge coresponding to each line in a N-by-6 array.
%
%   See also
%   lines3d, edges3d, createLine3d, clipRay3d
%

% ------
% Author: David Legland
% E-mail: david.legland@inra.fr
% Created: 2008-10-30, from drawLine3d
% Copyright 2008-2022 INRA - TPV URPOI - BIA IMASTE

% get box limits
xmin = box(1); xmax = box(2);
ymin = box(3); ymax = box(4);
zmin = box(5); zmax = box(6);

% extreme corners of the box
p000 = [xmin ymin zmin];
p111 = [xmax ymax zmax];

% main vectors
ex   = [1 0 0];
ey   = [0 1 0];
ez   = [0 0 1];

% box faces parallel to Oxy
planeZ0 = [p000 ex ey];
planeZ1 = [p111 ex ey];

% box faces parallel to Oxz
planeY0 = [p000 ex ez];
planeY1 = [p111 ex ez];

% box faces parallel to Oyz
planeX0 = [p000 ey ez];
planeX1 = [p111 ey ez];

% number of lines
nLines = size(line, 1);

% allocate memory for result
edge = zeros(nLines, 6);

% iterate over lines to clip
for i = 1:nLines

    % compute intersection point with each plane
    ipZ0 = intersectLinePlane(line(i,:), planeZ0);
    ipZ1 = intersectLinePlane(line(i,:), planeZ1);
    ipY0 = intersectLinePlane(line(i,:), planeY0);
    ipY1 = intersectLinePlane(line(i,:), planeY1);
    ipX1 = intersectLinePlane(line(i,:), planeX1);
    ipX0 = intersectLinePlane(line(i,:), planeX0);

    % concatenate resulting points
    points  = [ipX0;ipX1;ipY0;ipY1;ipZ0;ipZ1];

    % compute position of each point on the line
    pos     = linePosition3d(points, line(i,:));

    % keep only defined points
    ind     = find(~isnan(pos));
    pos     = pos(ind);
    points  = points(ind,:);

    % sort points with respect to their position
    [pos, ind] = sort(pos); %#ok<ASGLU>
    points  = points(ind, :);

    % keep median points wrt to position. These points define the limit of
    % the clipped edge.
    nv      = length(ind)/2;

    % create resulting edge.
    edge(i,:)   = [points(nv, :) points(nv+1, :)];
end

% check that middle point of the edge is contained in the box
midX = mean(edge(:, [1 4]), 2);
xOk  = xmin <= midX & midX <= xmax;
midY = mean(edge(:, [2 5]), 2);
yOk  = ymin <= midY & midY <= ymax;
midZ = mean(edge(:, [3 6]), 2);
zOk  = zmin <= midZ & midZ <= zmax;

% if one of the bounding condition is not met, set edge to NaN
edge (~(xOk & yOk & zOk), :) = NaN;


function printXferOrbitToTextbox(hXfrOrbitText, xferOrbitTextHeader, xfrOrbit, form, paddLen)
%printXferOrbitToTextbox Summary of this function goes here
%   Detailed explanation goes here

    hRule = getHRule();

    xferOrbitText = xferOrbitTextHeader;
    xferOrbitText{end+1} = hRule;
    xferOrbitText{end+1} = [paddStr('Semi-major Axis = ',paddLen), num2str(xfrOrbit(1), form), ' km'];
    xferOrbitText{end+1} = [paddStr('Eccentricity = ', paddLen), num2str(xfrOrbit(2))];
    xferOrbitText{end+1} = [paddStr('Inclination = ',paddLen), num2str(rad2deg(AngleZero2Pi(xfrOrbit(3))), form), ' deg'];
    xferOrbitText{end+1} = [paddStr('Right Ascension of AN = ',paddLen), num2str(rad2deg(AngleZero2Pi(xfrOrbit(4))), form), ' deg'];
    xferOrbitText{end+1} = [paddStr('Argument of Periapse = ',paddLen), num2str(rad2deg(AngleZero2Pi(xfrOrbit(5))), form), ' deg'];
    xferOrbitText{end+1} = '---------------------';
    xferOrbitText{end+1} = [paddStr('Departure True Anomaly = ',paddLen), num2str(rad2deg(AngleZero2Pi(xfrOrbit(6))), form), ' deg'];
    xferOrbitText{end+1} = [paddStr('Arrival True Anomaly = ',paddLen), num2str(rad2deg(AngleZero2Pi(xfrOrbit(7))), form), ' deg'];
    set(hXfrOrbitText,'String',xferOrbitText);
end


function [T_request_Nm, OverallRatio] = control_ForceRequestConverter(F_x_request_N, ...
                        tire_radius_m, i_c, i_f, i_r, i_gearset, ...
                        gear_in)
%__________________________________________________________________________
%% Documentation
%
% Author:       Dominik St�rk (dominik.staerk@tum.de)
%
% Start Date:   21.12.2020
%
% Description:
% This function converts the requested overall longitudinal force
% in a torque request for the engine. The effects of gear-ratios and
% -efficiencies as well as differentials and acceleration-losses are taken
% into account.
%
% Inputs:
%   F_x_request_N   - requested overall longitudinal force [N]
%   tire_radius_m   - tire`s radius [m]
%   i_c [2x1]       - central differential ratio and efficiency [-, -]
%   i_f [2x1]       - front differential ratio and efficiency [-, -]
%   i_r [2x1]       - rear differential ratio and efficiency [-, -]
%   i_gearset [4x7] - gear information
%   gear_in         - currently selected gear
%
% Outputs:
%   T_request_Nm    - requested engine torque [Nm]

p_torque_rear = 1;

%% request engine torque
T_Nm = F_x_request_N * tire_radius_m;

if gear_in ~= 0
    T_request_Nm = T_Nm / (i_c(1)*i_c(2) * ...
      ((1-p_torque_rear)*i_f(1)*i_f(2) + p_torque_rear*i_r(1)*i_r(2)) * ...
      i_gearset(2, gear_in+1)*i_gearset(4, gear_in+1));
    OverallRatio = (i_c(1)*i_c(2) * ...
      ((1-p_torque_rear)*i_f(1)*i_f(2) + p_torque_rear*i_r(1)*i_r(2)) * ...
      i_gearset(2, gear_in+1)*i_gearset(4, gear_in+1))/tire_radius_m;
else
    % use first gear even if in neutral
    T_request_Nm = T_Nm / (i_c(1)*i_c(2) * ...
      ((1-p_torque_rear)*i_f(1)*i_f(2) + p_torque_rear*i_r(1)*i_r(2)) * ...
      i_gearset(2, gear_in+2)*i_gearset(4, gear_in+1));
    % no brake torque when in neutral
    OverallRatio = 0;
end


Network ncf_gemm {
Layer GEMM0 {
Type: CONV
Dimensions { K: 128, C: 2048, Y: 256, X: 1, R: 1, S: 1 }

}
Layer GEMM1 {
Type: CONV
Dimensions { K: 64, C: 2048, Y: 128, X: 1, R: 1, S: 1 }

}
Layer GEMM2 {
Type: CONV
Dimensions { K: 256, C: 2048, Y: 256, X: 1, R: 1, S: 1 }

}
Layer GEMM3 {
Type: CONV
Dimensions { K: 256, C: 256, Y: 2048, X: 1, R: 1, S: 1 }

}
Layer GEMM4 {
Type: CONV
Dimensions { K: 256, C: 256, Y: 2048, X: 1, R: 1, S: 1 }

}
Layer GEMM5 {
Type: CONV
Dimensions { K: 128, C: 256, Y: 2048, X: 1, R: 1, S: 1 }

}
Layer GEMM6 {
Type: CONV
Dimensions { K: 256, C: 128, Y: 2048, X: 1, R: 1, S: 1 }

}
Layer GEMM7 {
Type: CONV
Dimensions { K: 128, C: 64, Y: 2048, X: 1, R: 1, S: 1 }

}
Layer GEMM8 {
Type: CONV
Dimensions { K: 64, C: 128, Y: 2048, X: 1, R: 1, S: 1 }

}
Layer GEMM9 {
Type: CONV
Dimensions { K: 1, C: 2048, Y: 128, X: 1, R: 1, S: 1 }

}
Layer GEMM10 {
Type: CONV
Dimensions { K: 1, C: 128, Y: 2048, X: 1, R: 1, S: 1 }

}
Layer GEMM11 {
Type: CONV
Dimensions { K: 128, C: 1, Y: 2048, X: 1, R: 1, S: 1 }

}
}


Network MobileNetV2 {
	Layer CONV1 {
		Type: CONV
		Stride { X: 2, Y: 2 }
		Dimensions { K: 32, C: 3, R: 1, S: 1, Y:224, X:224 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

//Bottleneck 1
//BottleneckID_repeat_operatorID

	Layer Bottleneck1_1_1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 32, C: 32, R: 1, S: 1, Y:112, X:112 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck1_1_2 {
		Type: DSCONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 1, C: 32, R: 3, S: 3, Y:112, X:112 }
		Dataflow {
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck1_1_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 16, C: 32, R: 1, S: 1, Y:112, X:112 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck1_1_Residual {
		Type: DSCONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 1, C: 16, R: 1, S: 1, Y:112, X:112 }
		Dataflow {
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

// Bottleneck 2
	Layer Bottleneck2_1_1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 96, C: 16, R: 1, S: 1, Y:112, X:112 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck2_1_2 {
		Type: DSCONV
		Stride { X: 2, Y: 2 }
		Dimensions { K: 1, C: 96, R: 3, S: 3, Y:112, X:112 }
		Dataflow {
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck2_1_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 96, C: 96, R: 1, S: 1, Y:56, X:56 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck2_2_1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 96, C: 96, R: 1, S: 1, Y:56, X:56 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck2_2_2 {
		Type: DSCONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 1, C: 96, R: 3, S: 3, Y:56, X:56 }
		Dataflow {
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck2_2_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 24, C: 96, R: 1, S: 1, Y:56, X:56 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

//Bottleneck 3

	Layer Bottleneck3_1_1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 144, C: 24, R: 1, S: 1, Y:56, X:56 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck3_1_2 {
		Type: DSCONV
		Stride { X: 2, Y: 2 }
		Dimensions { K: 1, C: 144, R: 3, S: 3, Y:56, X:56 }
		Dataflow {
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck3_1_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 144, C: 144, R: 1, S: 1, Y:28, X:28 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck3_2_1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 144, C: 144, R: 1, S: 1, Y:28, X:28 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck3_2_2 {
		Type: DSCONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 1, C: 144, R: 3, S: 3, Y:28, X:28 }
		Dataflow {
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck3_2_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 144, C: 144, R: 1, S: 1, Y:28, X:28 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck3_2_Residual {
		Type: DSCONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 1, C: 144, R: 1, S: 1, Y:28, X:28 }
		Dataflow {
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck3_3_1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 144, C: 144, R: 1, S: 1, Y:28, X:28 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck3_3_2 {
		Type: DSCONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 1, C: 144, R: 3, S: 3, Y:28, X:28 }
		Dataflow {
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck3_3_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 32, C: 144, R: 1, S: 1, Y: 28, X: 28 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

//Bottleneck 4

	Layer Bottleneck4_1_1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 192, C: 32, R: 1, S: 1, Y: 28, X: 28 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck4_1_2 {
		Type: DSCONV
		Stride { X: 2, Y: 2 }
		Dimensions { K: 1, C: 192, R: 3, S: 3, Y: 28, X: 28 }
		Dataflow {
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck4_1_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 192, C: 192 R: 1, S: 1, Y: 14, X: 14 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck4_2_1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 192, C: 192, R: 1, S: 1, Y: 14, X: 14 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck4_2_2 {
		Type: DSCONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 1, C: 192, R: 3, S: 3, Y: 14, X: 14 }
		Dataflow {
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck4_2_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 192, C: 192 R: 1, S: 1, Y: 14, X: 14 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck4_2_Residual {
		Type: DSCONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 1, C: 192, R: 1, S: 1, Y: 14, X: 14 }
		Dataflow {
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck4_3_1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 192, C: 192, R: 1, S: 1, Y: 14, X: 14 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck4_3_2 {
		Type: DSCONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 1, C: 192, R: 3, S: 3, Y: 14, X: 14 }
		Dataflow {
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck4_3_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 192, C: 192 R: 1, S: 1, Y: 14, X: 14 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck4_3_Residual {
		Type: DSCONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 1, C: 192, R: 1, S: 1, Y: 14, X: 14 }
		Dataflow {
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck4_4_1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 192, C: 192, R: 1, S: 1, Y: 14, X: 14 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck4_4_2 {
		Type: DSCONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 1, C: 192, R: 3, S: 3, Y: 14, X: 14 }
		Dataflow {
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck4_4_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 64, C: 192 R: 1, S: 1, Y: 14, X: 14 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

// Bottleneck 5

	Layer Bottleneck5_1_1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 384, C: 64, R: 1, S: 1, Y: 14, X: 14 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck5_1_2 {
		Type: DSCONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 1, C: 384, R: 3, S: 3, Y: 14, X: 14 }
		Dataflow {
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck5_1_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 384, C: 384 R: 1, S: 1, Y: 14, X: 14 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck5_2_1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 384, C: 384, R: 1, S: 1, Y: 14, X: 14 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck5_2_2 {
		Type: DSCONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 1, C: 384, R: 3, S: 3, Y: 14, X: 14 }
		Dataflow {
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck5_2_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 96, C: 384 R: 1, S: 1, Y: 14, X: 14 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

// Bottleneck 6

	Layer Bottleneck6_1_1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 576, C: 96, R: 1, S: 1, Y: 14, X: 14 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck6_1_2 {
		Type: DSCONV
		Stride { X: 2, Y: 2 }
		Dimensions { K: 1, C: 576, R: 3, S: 3, Y: 14, X: 14 }
		Dataflow {
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck5_2_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 576, C: 576 R: 1, S: 1, Y: 7, X: 7 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck6_2_1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 576, C: 576, R: 1, S: 1, Y: 7, X: 7 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck6_2_2 {
		Type: DSCONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 1, C: 576, R: 3, S: 3, Y: 7, X: 7 }
		Dataflow {
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck6_2_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 576, C: 576 R: 1, S: 1, Y: 7, X: 7 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck6_2_Residual {
		Type: DSCONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 1, C: 576 R: 1, S: 1, Y: 7, X: 7 }
		Dataflow {
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck6_3_1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 576, C: 576, R: 1, S: 1, Y: 7, X: 7 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck6_3_2 {
		Type: DSCONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 1, C: 576, R: 3, S: 3, Y: 7, X: 7 }
		Dataflow {
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck6_3_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 160, C: 576 R: 1, S: 1, Y: 7, X: 7 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

// Bottleneck 7

	Layer Bottleneck7_1_1 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 960, C: 160, R: 1, S: 1, Y: 7, X: 7 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck7_1_2 {
		Type: DSCONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 1, C: 960, R: 3, S: 3, Y: 7, X: 7 }
		Dataflow {
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer Bottleneck7_1_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 320, C: 960, R: 1, S: 1, Y: 7, X: 7 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

// CONV2D

	Layer CONV2D_2 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 1280, C: 320, R: 1, S: 1, Y: 7, X: 7 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

	Layer CONV2D_3 {
		Type: CONV
		Stride { X: 1, Y: 1 }
		Dimensions { K: 1000, C: 1280, R: 1, S: 1, Y: 1, X: 1 }
		Dataflow {
			TemporalMap (1,1) K;
			TemporalMap (1,1) C;
			TemporalMap (Sz(R),1) Y;
			SpatialMap (Sz(S),1) X;
			TemporalMap (Sz(R),Sz(R)) R;
			TemporalMap (Sz(S),Sz(S)) S;
		}
	}

}


function [numDataSym] = offline(cfgNonHT)

mcsTable = wlan.internal.getRateTable(cfgNonHT);
Ntail = 6; Nservice = 16;
numDataSym = ceil((8*cfgNonHT.PSDULength + Nservice + Ntail)/mcsTable.NDBPS);


function binnedmatrix = toy_simulation(Network_opts,Assembly_opts)

% creating matrix
binnedmatrix=poissrnd(Network_opts.meanspikebin,[Network_opts.nneurons Network_opts.nbins]);

allassemblyneurons = unique([Assembly_opts.assembly_neurons{:}]);
nonassemblyneurons = setdiff(1:Network_opts.nneurons,allassemblyneurons);
nassemblies = length(Assembly_opts.assembly_neurons);

% add activations of all assemblies in random bins
for assemblyindex=1:nassemblies

    assemblysize = length(Assembly_opts.assembly_neurons{assemblyindex});

    % drawing activations
    activation_bins = randi([1,Network_opts.nbins],1,Assembly_opts.number_of_activations);

    % introducting activations
    binnedmatrix(Assembly_opts.assembly_neurons{assemblyindex},activation_bins) = ...
        poissrnd(Assembly_opts.meanspikerate_activations,...
        assemblysize,Assembly_opts.number_of_activations);

end


function xdot=Duffing(t,x)
global Gamma;
xdot(1)=x(2);
xdot(2)=x(1)-0.1*x(2)-(x(1))^3+Gamma*cos(1.25*t);
xdot=[xdot(1);xdot(2)];


function varargout = min2(varargin)
%MIN2   Global minimum of a DISKFUN.
%   M = MIN2(F) returns the global minimum of F over its domain.
%
%   [M, LOC] = MIN2(F) returns the global minimum in M and its location in
%   LOC.
%
%  This command may be faster if the OPTIMIZATION TOOLBOX is installed.
%
% See also DISKFUN/MAX2, DISKFUN/MINANDMAX2.

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

f = varargin{1};

if ( isempty(f) )
 varargout= [];
 return
end

% Give cdr to separableApprox.
f = cart2pol(f, 'cdr');
[out{1:nargout}] = min2@separableApprox(f);
if numel(out) == 1
    varargout = out;
else
    % Return loc in Cartesian coords.
    [val, loc] = out{:};
    t = loc(1);
    loc(1) = loc(2).*cos(t);
    loc(2) = loc(2).*sin(t);
    varargout = {val, loc};
end

end


clear all

dt = 0.1;
t  = 0:dt:10;

Nsamples = length(t);

Xsaved   = zeros(Nsamples, 2);
DeXsaved = zeros(Nsamples, 2);
Zsaved   = zeros(Nsamples, 1);

for k=1:Nsamples
  z = GetPos();
  [pos vel]   = DvKalman(z);
  [dpos dvel] = DeDvKalman(z);

  Xsaved(k, :)   = [pos vel];
  DeXsaved(k, :) = [dpos dvel];
  Zsaved(k)      = z;
end

figure
hold on
plot(t, Xsaved(:, 1), 'ro')
plot(t, DeXsaved(:,1))

figure
hold on
plot(t, Xsaved(:, 2), 'ro')
plot(t, DeXsaved(:,2))


% Test file for trigtech/imag.m

function pass = test_imag(pref)

% Get preferences.
if ( nargin < 1 )
    pref = trigtech.techPref();
end

testclass = trigtech();

%%
% Test a scalar-valued function.
f = testclass.make(@(x) cos(pi*x) + 1i*sin(pi*x), [], pref);
g = testclass.make(@(x) sin(pi*x), [], pref);
h = imag(f);
g = prolong(g, length(h));
pass(1) = norm(h.coeffs - g.coeffs, inf) < 10*vscale(h)*eps;

%%
% Test an array-valued function.
f = testclass.make(@(x) [cos(sin(pi*x)) + 1i*sin(cos(pi*x)), -exp(1i*pi*x)], [], pref);
g = testclass.make(@(x) [sin(cos(pi*x)), -imag(exp(1i*pi*x))], [], pref);
h = imag(f);
g = prolong(g, length(h));
pass(2) = norm(h.coeffs - g.coeffs, inf) < 100*max(vscale(h)*eps);

%%
% Test a real function.
f = testclass.make(@(x) cos(pi*x), [], pref);
g = imag(f);
pass(3) = numel(g.coeffs) == 1;

%%
% Test an array-valued real function.
f = testclass.make(@(x) [cos(pi*x), sin(pi*x)], [], pref);
g = imag(f);
pass(4) = all(size(g.coeffs) == [1, 2]) && all(g.coeffs == 0);

end


function test_ft_channelnormalise

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

fs = 500;
nchan = 32;
start_time = -1; % seconds
end_time = 2.5; % seconds
nsamples = (end_time - start_time) * fs + 1;

data = [];
data.time{1} = linspace(start_time, end_time, nsamples);
data.trial{1} = randn(nchan,nsamples);
data.label = cellstr(num2str((1:nchan).'));

cfg = [];
dataout = ft_channelnormalise(cfg, data);


function c = ttv(a,v,dims)
%TTV Tensor times vector for ktensor.
%
%   Y = TTV(X,A,N) computes the product of Kruskal tensor X with a
%   (column) vector A.  The integer N specifies the dimension in X
%   along which A is multiplied.  If size(A) = [I,1], then X must have
%   size(X,N) = I.  Note that ndims(Y) = ndims(X) - 1 because the N-th
%   dimension is removed.
%
%   Y = TTV(X,{A1,A2,...}) computes the product of tensor X with a
%   sequence of vectors in the cell array.  The products are computed
%   sequentially along all dimensions (or modes) of X. The cell array
%   contains ndims(X) vectors.
%
%   Y = TTV(X,{A1,A2,...},DIMS) computes the sequence tensor-vector
%   products along the dimensions specified by DIMS.
%
%   See also TENSOR/TTV, KTENSOR, KTENSOR/TTM.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

%%%%%%%%%%%%%%%%%%%%%%
%%% ERROR CHECKING %%%
%%%%%%%%%%%%%%%%%%%%%%

% Check the number of arguments
if (nargin < 2)
    error('TTV requires at least two arguments.');
end

% Check for 3rd argument
if ~exist('dims','var')
    dims = [];
end

% Check that 2nd argument is cell array. If not, recall with v as a
% cell array with one element.
if ~iscell(v)
    c = ttv(a,{v},dims);
    return;
end

% Get sorted dims and index for multiplicands
[dims,vidx] = tt_dimscheck(dims,ndims(a),numel(v));

% Check that each multiplicand is the right size.
for i = 1:numel(dims)
    if ~isequal(size(v{vidx(i)}),[size(a,dims(i)) 1])
        error('Multiplicand is wrong size');
    end
end

% Figure out which dimensions will be left when we're done
remdims = setdiff(1:ndims(a),dims);

% Collapse dimensions that are being multiplied out
newlambda = a.lambda;
for i = 1:numel(dims)
    newlambda = newlambda .* ( a.u{dims(i)}' * v{vidx(i)} );
end

% Create final result
if isempty(remdims)
    c = sum(newlambda);
else
    c = ktensor(newlambda,a.u{remdims});
end


% XOVMP.m                Multi-point crossover
%
%       Syntax: NewChrom =  xovmp(OldChrom, Px, Npt, Rs)
%
%       This function takes a matrix OldChrom containing the binary
%       representation of the individuals in the current population,
%       applies crossover to consecutive pairs of individuals with
%       probability Px and returns the resulting population.
%
%       Npt indicates how many crossover points to use (1 or 2, zero
%       indicates shuffle crossover).
%       Rs indicates whether or not to force the production of
%       offspring different from their parents.
%

% Author: Carlos Fonseca, 	Updated: Andrew Chipperfield
% Date: 28/09/93,		Date: 27-Jan-94

function NewChrom = xovmp(OldChrom, Px, Npt, Rs);

% Identify the population size (Nind) and the chromosome length (Lind)
[Nind,Lind] = size(OldChrom);

if Lind < 2, NewChrom = OldChrom; return; end

if nargin < 4, Rs = 0; end
if nargin < 3, Npt = 0; Rs = 0; end
if nargin < 2, Px = 0.7; Npt = 0; Rs = 0; end
if isnan(Px), Px = 0.7; end
if isnan(Npt), Npt = 0; end
if isnan(Rs), Rs = 0; end
if isempty(Px), Px = 0.7; end
if isempty(Npt), Npt = 0; end
if isempty(Rs), Rs = 0; end

Xops = floor(Nind/2);
DoCross = rand(Xops,1) < Px;
odd = 1:2:Nind-1;
even = 2:2:Nind;

% Compute the effective length of each chromosome pair
Mask = ~Rs | (OldChrom(odd, :) ~= OldChrom(even, :));
Mask = cumsum(Mask')';

% Compute cross sites for each pair of individuals, according to their
% effective length and Px (two equal cross sites mean no crossover)
xsites(:, 1) = Mask(:, Lind);
if Npt >= 2,
        xsites(:, 1) = ceil(xsites(:, 1) .* rand(Xops, 1));
end
xsites(:,2) = rem(xsites + ceil((Mask(:, Lind)-1) .* rand(Xops, 1)) ...
                                .* DoCross - 1 , Mask(:, Lind) )+1;

% Express cross sites in terms of a 0-1 mask
Mask = (xsites(:,ones(1,Lind)) < Mask) == ...
                        (xsites(:,2*ones(1,Lind)) < Mask);

if ~Npt,
        shuff = rand(Lind,Xops);
        [ans,shuff] = sort(shuff);
        for i=1:Xops
          OldChrom(odd(i),:)=OldChrom(odd(i),shuff(:,i));
          OldChrom(even(i),:)=OldChrom(even(i),shuff(:,i));
        end
end

% Perform crossover
NewChrom(odd,:) = (OldChrom(odd,:).* Mask) + (OldChrom(even,:).*(~Mask));
NewChrom(even,:) = (OldChrom(odd,:).*(~Mask)) + (OldChrom(even,:).*Mask);

% If the number of individuals is odd, the last individual cannot be mated
% but must be included in the new population
if rem(Nind,2),
  NewChrom(Nind,:)=OldChrom(Nind,:);
end

if ~Npt,
        [ans,unshuff] = sort(shuff);
        for i=1:Xops
          NewChrom(odd(i),:)=NewChrom(odd(i),unshuff(:,i));
          NewChrom(even(i),:)=NewChrom(even(i),unshuff(:,i));
        end
end


function [enums,Matrices] = mpt_enumerate_binary(Matrices)

binary_var_index = Matrices.binary_var_index;
notbinary_var_index = setdiff(1:Matrices.nu,binary_var_index);

% Detect and extract pure binary equalities. These are used
% to detect SOS constraints, and for pruning
nbin = length(binary_var_index);
only_binary = ~any(Matrices.Aeq(:,notbinary_var_index),2);
Aeq_bin = Matrices.Aeq(find(only_binary),binary_var_index);
%Beq_bin = Beq(find(only_binary),binary_var_index);
beq_bin = Matrices.beq(find(only_binary),:);

% Keep the rest
Matrices.Aeq = Matrices.Aeq(find(~only_binary),:);
Matrices.Beq = Matrices.Beq(find(~only_binary),:);
Matrices.beq = Matrices.beq(find(~only_binary),:);

% Dummy pre-solve to avoid problems
if length(beq_bin)>0
    [ii,jj,kk] = unique([Aeq_bin beq_bin],'rows');
    Aeq_bin = Aeq_bin(jj,:);
    beq_bin = beq_bin(jj,:);
end

% Normalize...
neg_b = find(beq_bin < 0);
Aeq_bin(neg_b,:) = -Aeq_bin(neg_b,:);
beq_bin(neg_b,:) = -beq_bin(neg_b,:);

% Detect and extract simple binary LP constraints
only_binary_lp = find(~any([Matrices.G(:,notbinary_var_index) Matrices.E],2));
Abin = Matrices.G(only_binary_lp,binary_var_index);
bbin = Matrices.W(only_binary_lp);

% Remove pure binary constraints
Matrices.G(only_binary_lp,:) = [];
Matrices.W(only_binary_lp,:) = [];
Matrices.E(only_binary_lp,:) = [];

% if ~isempty(Matrices.binary_var_index)
%     fixed_up = find(Matrices.lb(Matrices.binary_var_index) == 1);
%     fixed_down = find(Matrices.ub(Matrices.binary_var_index) == 0);
%     fixed = [fixed_up fixed_down];
%     if ~isempty(fixed)
%         % Ok, some of the binary are fixed.
%         % Do not enumerate over these
%         % Fix them and correct the constraints that we use
%         % for enumeration and pruning
%         bbin = bbin - Abin(:,fixed)*Matrices.lb(Matrices.binary_var_index(fixed));
%         Abin(:,fixed) = [];
%         beq_bin = beq_bin - Aeq_bin(:,fixed)*Matrices.lb(Matrices.binary_var_index(fixed));
%         Aeq_bin(:,fixed) = [];
%         Matrices.binary_var_index(fixed) = [];
%         binary_var_index = Matrices.binary_var_index;
%         notbinary_var_index = setdiff(1:Matrices.nu,binary_var_index);
%     end
% end

% Detect groups with constraints sum(d_i) == d_j
SOS = [];
variables_in_sos = [];
for i = 1:size(Aeq_bin,1)
    if beq_bin(i) == 0
        [ix,jx,sx] = find(Aeq_bin(i,:));
        if all(abs(sx) == 1)
            j = find(sx == -1);
            if length(j) == 1 & length(jx)>2
                % Aha, we have sum binary(i) == binary(j)
                j = jx(j);
                jx = setdiff(jx,j);
                this_sos = sparse(1:length(jx),jx,1,length(jx),length(binary_var_index));
                this_sos(1:end,j)= 1;
                this_sos(end+1,1)=0;
                if isempty(SOS)
                    SOS = this_sos;
                else
%                    new_sos  = [];
                    SOS = kron(ones(size(SOS,1),1),this_sos) | kron(SOS,ones(size(this_sos,1),1));
%                     for k = 1:size(SOS,1)
%                         for r = 1:size(this_sos,1)
%                             new_sos = [new_sos;this_sos(r,:) | SOS(k,:)];
%                         end
%                     end
%                     if ~isequal(new_sos,new_sos2)
%                         error
%                     end
%                    SOS = new_sos;
                end
                variables_in_sos = [variables_in_sos jx j];
            end
        end
    end
end

% Detect groups with constraints sum(d_i) == 1
for i = 1:size(Aeq_bin,1)
    if beq_bin(i) == 1
        [ix,jx,sx] = find(Aeq_bin(i,:));
        if all(sx == 1)
            % Aha, we have sum binary(jx) == 1
            this_sos = sparse(1:length(jx),jx,1,length(jx),length(binary_var_index));
            if isempty(SOS)
                SOS = this_sos;
            else
               % new_sos  = [];
                SOS = kron(ones(size(SOS,1),1),this_sos) | kron(SOS,ones(size(this_sos,1),1));
%                 for k = 1:size(SOS,1)
%                     for r = 1:size(this_sos,1)
%                         new_sos = [new_sos;this_sos(r,:) | SOS(k,:)];
%                     end
%                 end
%                 SOS = new_sos;
            end
            variables_in_sos = [variables_in_sos jx];
        end
    end
end

variables_not_in_sos = setdiff(1:length(binary_var_index),variables_in_sos);
if ~isempty(variables_not_in_sos)
    % we need to add some more binaries
    n_left = length(variables_not_in_sos);
    all_perms = dec2decbin(0:2^n_left-1,n_left);
    [ix,jx,sx] = find(all_perms);jx = variables_not_in_sos(jx);
    this_sos = sparse(ix,jx,sx,size(all_perms,1),length(binary_var_index));
    if isempty(SOS)
        SOS = this_sos;
    else
        new_sos  = [];
        for k = 1:size(SOS,1)
            for r = 1:size(this_sos,1)
                new_sos = [new_sos;this_sos(r,:) | SOS(k,:)];
            end
        end
        SOS = new_sos;
    end
end

enums = unique(SOS,'rows')';

% FIX :FULL, F**N Linux 6.5 repmat bug
feasible = find(~any(Aeq_bin*enums-repmat(full(beq_bin),1,size(enums,2)),1));
enums = enums(:,feasible);

% Remove binary vertices violating these
if ~isempty(Abin)
    remove = find(any(Abin*enums - repmat(bbin,1,size(enums,2)) >0,1));
%    keep = setdiff(1:size(enums,2),remove);
%    mapper = find(ismember(1:size(enums,2),keep));
%    keep(remove)=0;keep2 = 1:size(enums,2);keep2(find(keep)) = find(keep);
%     map = find(keep);
%     remove = find(ismember(j,remove));
%     i(remove) = [];
%     j(remove) = [];
%     k(remove) = [];
%     enums = sparse(i,
%
%     [i,j,k] = find(enums);
    enums(:,remove)=[];
end