function pg = msTest(imsegs, data, maps, labelclassifier, segclassifier, normalize)
% [vacc, hacc, vcm, hcm] = testMultipleSegmentationsCV2(imsegs, labdata, segdata, maps, vclassifier, hclassifier, sclassifier, ncv)

if ~exist('normalize', 'var') || isempty(normalize)
    normalize = 1;
end

pg = cell(numel(imsegs), 1);
allcount = 0;

nclasses = size(labelclassifier.wcs,2);

for f = 1:numel(imsegs)

    %disp(num2str(f))

    nsp = imsegs(f).nseg;

    pg{f} = zeros(nsp, nclasses);

    segs = cell(nsp, 1);

    count = zeros(nsp, 1);

    if ~normalize
        sum_s = zeros(nsp, 1);
    end

    for k = 1:size(data, 2)
        if numel(data{f,k})>0

        isvalid = false(size(data{f, k}, 1), 1);
        for s = 1:size(data{f, k}, 1)
            [segs, ind] = checksegs(segs, maps{f}(:, k), s);
            isvalid(s) = ~isempty(ind);
        end

        yconf_all = test_boosted_dt_mc(labelclassifier, data{f, k}(isvalid, :));
        if exist('segclassifier','var')
        sconf_all = test_boosted_dt_mc(segclassifier, data{f, k}(isvalid, :));
        end

        isvalid = find(isvalid);
        for s = 1:numel(isvalid)

            ind = find(maps{f}(:, k)==isvalid(s));

            count(ind) = count(ind) + 1;

            yconf = yconf_all(s, :);
            yconf = 1 ./ (1+exp(-yconf));

            if normalize
                yconf = yconf / sum(yconf);
            end

            if exist('segclassifier','var')
            sconf = sconf_all(s, :);
            sconf = 1 ./ (1+exp(-sconf));
            pgs = yconf*sconf;
            else
              pgs = yconf;
            end

            if ~normalize
                sum_s(ind) = sum_s(ind) + sconf;
            end
            pg{f}(ind, :) = pg{f}(ind, :) + repmat(pgs, numel(ind), 1);

        end

        end
    end

    if normalize
        pg{f} = pg{f} ./ max(repmat(sum(pg{f}, 2), 1, size(pg{f}, 2)), 0.00001);
    else
        pg{f} = pg{f} ./ repmat(sum_s, [1 size(pg{f}, 2)]);
    end

    allcount = allcount + mean(count);

end

allcount = allcount / numel(imsegs);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [segs, ind] = checksegs(segs, map, s)
% Checks whether this segment has been seen before

ind = find(map==s);

if 0

if numel(ind)==1 % already accounted for by superpixels
    ind = [];
    return;
end

end

oldsegs = segs{ind(1)};

for k = 1:numel(oldsegs)
    if (numel(oldsegs{k})==numel(ind)) && oldsegs{k}(1)==ind(1) && all(oldsegs{k}==ind)
        ind = [];
        return;
    end
end

segs{ind(1)}{end+1} = ind;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% QPSK demonstration packet-based transceiver for Chilipepper
% Toyon Research Corp.
% http://www.toyon.com/chilipepper.php
% Created 10/17/2012
% embedded@toyon.com
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This file is the top level entry function for the receiver portion of the
% example. The entire receiver is designed to run at Rate=1 (one clock
% cycle per iteration of the core.
% We follow standard receive practice with frequency offset estimation,
% pulse-shape filtering, time estimateion, and correlation to determine
% tart of packet.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%#codegen

function [r_out, s_f_out, s_c_out, s_t_out, t_est_out, f_est_out] = ...
    qpsk_rx(i_in, q_in, mu_foc_in, mu_toc_in)

    % scale input data coming from the Chilipepper ADC to be purely
    % fractional to avoid scaling issues
    r_in = complex(i_in, q_in);

    % frequency offset estimation. Note that time constant is input as
    % integer
    [s_f_i, s_f_q, fe] = qpsk_rx_foc(i_in, q_in, mu_foc_in);

    % Square-root raised-cosine band-limited filtering
    [s_c_i, s_c_q] = qpsk_rx_srrc(s_f_i, s_f_q);

    % Time offset estimation. Output data changes at the symbol rate.
    [s_t_i, s_t_q, tauh] =  qpsk_rx_toc(s_c_i, s_c_q, mu_toc_in);

    % estimation and correlation values
    t_est_out = tauh;
    f_est_out = fe;

    % original signal out (real version)
    r_out = real(r_in);

    % incremental signal outputs after frequency estimation, filtering, and
    % timining estimation
    s_f_out = complex(s_f_i,s_f_q);
    s_c_out = complex(s_c_i,s_c_q);
    s_t_out = complex(s_t_i,s_t_q);
end


function [M,linearInd]=subImage(M,nSub)

% function M=subImage(M,nSub)
% ------------------------------------------------------------------------
%
%This function refines the input image M by splitting its voxels in half during nSub times
%which contains all folders and sub-folders within the folder specified by
%the input pathName.
%
% Kevin Mattheus Moerman
% kevinmoerman@hotmail.com
% 18/04/2013
%------------------------------------------------------------------------

%%
%TO DO: ADD WARNINGS AND INPUT PARSING HERE
% nSub=round(nSub);

%%

if numel(nSub)==1
    nSub=nSub.*ones(1,3);
end

stepSize_I=1/nSub(1);
stepSize_J=1/nSub(2);
stepSize_K=1/nSub(3);

%Define image coordinates of new voxel centres within image coordinate
%system of original coordinate mesh
I_range=linspace((0.5+(stepSize_I/2)),(size(M,1)-(stepSize_I/2))+0.5,size(M,1)*nSub(1));
J_range=linspace((0.5+(stepSize_J/2)),(size(M,2)-(stepSize_J/2))+0.5,size(M,2)*nSub(2));
K_range=linspace((0.5+(stepSize_K/2)),(size(M,3)-(stepSize_K/2))+0.5,size(M,3)*nSub(3));

[In,Jn,Kn]=ndgrid(I_range,J_range,K_range); %Coordinate matrices

%Rounding the coordinates so they "snap" to the mother voxel indices they
%are found in.
In=round(In); Jn=round(Jn); Kn=round(Kn);

%Convert these subscript indiced to linear indices in the image
linearInd =sub2ind(size(M),In,Jn,Kn);

%Get intensity values
Mc=M(:); %Image as column
M=Mc(linearInd); %The refined image

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


function m = size(t,idx)
%SIZE Size of a ttensor.
%
%   D = SIZE(T) returns the size of the tensor.
%
%   I = size(T,DIM) returns the size of the dimension specified by
%   the scalar DIM.
%
%   See also TTENSOR, TTENSOR/NDIMS.
%
%MATLAB Tensor Toolbox.
%Copyright 2010, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by Brett Bader and Tamara Kolda.
% http://csmr.ca.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2010) 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 tensor_toolbox/LICENSE.txt
% $Id: size.m,v 1.8 2010/03/19 23:46:31 tgkolda Exp $

if ndims(t) == 0
    m = [];
end

if exist('idx','var')
    m = size(t.u{idx}, 1);
else
    for i = 1 : ndims(t)
	m(i) = size(t.u{i}, 1);
    end
end


classdef RunningVademecumInParalel < handle

    properties (Access = private)
        samplePoints
        cellVar
        fileName
    end

    properties (Access = private)
        nCase
        nMx
        nMy
        nPhi
        phiMin
        phiMax
    end

    methods (Access = public)

        function obj = RunningVademecumInParalel(cParams)
            obj.init(cParams);
            obj.compute();
        end

    end

     methods (Access = private)

        function init(obj,nC)
            obj.nCase  = nC;
            obj.nMx    = 20;
            obj.nMy    = 20;
            obj.nPhi   = 10;
        end

        function compute(obj)
            obj.computeSameStressSign();
            obj.computeDifferentStressSign();
        end

        function computeSameStressSign(obj)
            obj.phiMin = 0;
            obj.phiMax = pi/4;
            obj.fileName = 'OptimalSuperEllipseSameStressSign';
            obj.computeVademecums();
        end

        function computeDifferentStressSign(obj)
            obj.phiMin = pi/2;
            obj.phiMax = 3*pi/4;
            obj.fileName = 'OptimalSuperEllipseDifferentStressSign';
            obj.computeVademecums();
        end

        function computeVademecums(obj)
            obj.createSamplePoints();
            obj.cellVar = cell(obj.nMx,obj.nMy,obj.nPhi);
            imx = obj.nCase;
            for imy = 1:obj.nMy
                obj.displayPercentatge(imx,imy);
                for iphi = 1:obj.nPhi
                    c = obj.computeOneVademecum(imx,imy,iphi);
                    obj.cellVar{imx,imy,iphi} = c;
                end
            end
            c = obj.cellVar;
            save([obj.fileName,num2str(imx)],'c');
        end

        function cellV = computeOneVademecum(obj,imx,imy,iphi)
            iter = obj.computeGlobalIteration(imx,imy);
            s.rho = obj.samplePoints.rhoV(iter);
            s.xi = obj.samplePoints.txiV(iter);
            s.phi = obj.samplePoints.phiV(iphi);
            s.nCase = [num2str(imx),num2str(imy),num2str(iphi)];
            v = VademecumCalculator(s);
            v.compute();
            %v.cellVariables = s.nCase;
            cellV = v.cellVariables;
        end

       function createSamplePoints(obj)
            s.type = 'FromMxMy';
            s.nMx  = obj.nMx;
            s.nMy  = obj.nMy;
            s.nPhi = obj.nPhi;
            s.phiMin = obj.phiMin;
            s.phiMax = obj.phiMax;
            sample = SamplePointsCreatorForOptimalExponentComputer.create(s);
            sample.compute();
            obj.samplePoints = sample;
        end

        function iter = computeGlobalIteration(obj,imx,imy)
            iter = (imy + obj.nMx*(imx -1));
        end

        function displayPercentatge(obj,imx,imy)
            perc = imy/(obj.nMy)*100;
            %disp([num2str(perc),'% done']);
            fid = fopen('Output.txt','a+');
            fprintf(fid,'%6.2f %%\n',perc);
            fclose(fid);
        end

     end

end


function opa=decimate_optimums(z,op,opv)
% z  zeros
% op optimums from d/dt=0
% opv optimums values
Lz=length(z);
Lop=length(op);
zop=[z; op];
zopv=[zeros(size(z)); opv]; % values
[zops ind]=sort(zop);
zopsv=zopv(ind);
isz=(ind<=Lz); % if zero in zops list
iszind=find(isz);
L1=length(iszind);
opa=[];
opva=[];
wasused=false(size(zops)); % what optimums was already used
for c=1:L1-1
    ind1=iszind(c);
    z1=zops(ind1);
%     if z1>1.47e4
%         'here'
%     end
    ind2=iszind(c+1);
    z2=zops(ind2);
    %ii=ind1+1:ind2-1;

    if (ind2+1)<=length(isz)
        inOnePlace=(~isz(ind2+1))&&(zops(ind2)==zops(ind2+1));
    else
        inOnePlace=false;
    end

    if inOnePlace
        % optimum and zero in one place
        ii=ind1+1:ind2+1;
    else
        ii=ind1+1:ind2;
    end
    wasused1=wasused(ii);

    ii=ii(~wasused1);

%     isz1=isz(ii);
%     isz2=~isz1; % what is optimums
%     isz3=isz2&(~wasused1); % optimums that was not used
%     %if all(isz(ii))
%     if any(isz3) % if any optimums that was not used
%
%     else
%         % case when zeros and maximum are same
%         ii=ind1+1:ind2+1;
%     end
    if ~isempty(ii)
        opt=zops(ii); % optimums between z1 and z2
        optv=zopsv(ii); % values
        [mx mxi]=max(abs(optv));
        opa=[opa; opt(mxi)];
        opva=[opva; optv(mxi)];
        wasused(ii(mxi))=true;
    end
end

mdf=0.005;

% detete optimums that to smaller in compare to bigest optimum:
bo=max(abs(opva));
indo=find(abs(opva)>=mdf*bo);
opa=opa(indo);

% delete repeated points:
opa=unique(opa);


function v = conj(v)
%CONJ Complex conjugate of a BALLFUNV.
%   CONJ(F) returns the complex conjugate of F. For a complex F, CONJ(F) =
%   REAL(F) - i*IMAG(F).
%
%   See also IMAG, REAL.

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

if isempty( v )
    return
end

% Take conjugate part of each component:
v = ballfunv( conj(v.comp{1}), conj(v.comp{2}), conj(v.comp{3}) );
end


function cPlot(x,y,varargin)
mode = 3;  % 1 = magnitude and phase,
           % 2 = real and imag
           % 3 = real and imag in one window
doHolding = false;
plotLegend = 0;
deleteInds = zeros(length(varargin),1);
parent = [];
if nargin == 1
    y = x;
    x = 1 : length(y);
end
if nargin > 2
    for i = 1 : floor(length(varargin)/2)
        option       = varargin{i*2-1};
        option_value = varargin{i*2};

        switch lower(option)
            case 'hold'
                doHolding  = option_value;
                deleteInds(i*2-1 : i*2) = [1;1];
            case 'mode'
                mode       = option_value;
                deleteInds(i*2-1 : i*2) = [1;1];
            case 'parent'
                parent       = option_value;
                deleteInds(i*2-1 : i*2) = [1;1];
        end
    end
end
varargin(deleteInds ~= 0) = [];

if isempty(parent)
    parent = gca;
end

if doHolding
    hold on
end

switch mode
    case 1
        subplot(parent,2,1,1)
        plot(x,abs(y),varargin{:});
        title('abs');
        subplot(parent,2,1,2)
        plot(x,angle(y),varargin{:});
        title('phase');
    case 2
        subplot(parent,2,1,1)
        plot(x,real(y),varargin{:});
        title('real');
        subplot(parent,2,1,2)
        plot(x,imag(y),varargin{:});
        title('imag');
    case 3
        % put parent option at the end of varargin vector to allow for a
        % preceeding cursor style option (e.g. "x-");
        varargin = [varargin,{'parent'},{parent}];

        plot(x,[real(y),imag(y)],varargin{:});
        if plotLegend
            legend('real','imag');
        end

end
hold off
end


function triangle_wandzura_rule_test01 ( )

%*****************************************************************************80
%
%% TRIANGLE_WANDZURA_RULE_TEST01 tests WANDZURA_RULE_NUM, WANDZURA_DEGREE, WANDZURA_ORDER_NUM.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 December 2006
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRIANGLE_WANDZURA_RULE_TEST01\n' );
  fprintf ( 1, '  WANDZURA_RULE_NUM returns the number of rules;\n' );
  fprintf ( 1, '  WANDZURA_DEGREE returns the degree of a rule;\n' );
  fprintf ( 1, '  WANDZURA_ORDER_NUM returns the order of a rule.\n' );

  rule_num = wandzura_rule_num ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of available rules = %d\n', rule_num );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      Rule    Degree     Order\n' );
  fprintf ( 1, '\n' );

  for rule = 1 : rule_num
    order_num = wandzura_order_num ( rule );
    degree = wandzura_degree ( rule );
    fprintf ( 1, '  %8d  %8d  %8d\n', rule, degree, order_num );
  end

  return
end


function [theta,rho,itheta,prho,rhomin] = getdata(S2G)
% return index of all points in a epsilon neighborhood of a vector
%
% Input
%  S2G     - @S2Grid
%
% Output
%  theta  - double
%  rho    - double
%  itheta - double
%  prho   - double

theta = double(S2G.thetaGrid);
rho = double(S2G.rhoGrid);
itheta = cumsum([0,GridLength(S2G.rhoGrid)]);
prho = S2G.rhoGrid(1).max;
rhomin = S2G.rhoGrid(1).min;
rhomax = S2G.rhoGrid(1).max;


%% Compare performance of gaimc to matlab_bgl
% While the |gaimc| library implements its graph routines in Matlab
% "m"-code, the |matlab_bgl| library uses graph algorithms from the Boost
% graph library in C++ through a mex interface.  Folklore has it that
% Matlab code with for loops like those required in the |gaimc| library is
% considerably slower.  This example examines this lore and shows that
% |gaimc| is typically within a factor of 2-4 of the mex code.

%%
% *This demo is unlikely to work on your own computer.*
% *They depend on having the MatlabBGL routines in one spot.*

%% Setup the environment
% We need MatlabBGL on the path
graphdir = '../graphs/';
matlabbgldir = '~/dev/matlab-bgl/4.0';
try
    addpath(matlabbgldir); % change this to your matlab_bgl path
    ci=components(sparse(ones(5)));
catch
    error('gaimc:performance_comparison','Matlab BGL is not working, halting...');
end

%%
% Check to make sure we are in the correct directory
cwd = pwd; dirtail = ['gaimc' filesep 'demo'];
if strcmp(cwd(end-length(dirtail)+1:end),dirtail) == 0
    error('%s should be executed from %s\n',mfilename,dirtail);
end

%%
% initalize the results structure
results=[];
mex_fast=0; mat_fast=0; mex_std=0; mat_std=0;

%% Depth first search
% To compare these functions, we have to make a copy and then delete it
copyfile(['..' filesep 'dfs.m'],'dfstest.m');
graphs = {'all_shortest_paths_example', 'clr-24-1', 'cs-stanford', ...
    'minnesota','tapir'};
nrep=30; ntests=100;
fprintf(repmat(' ',1,76));
for rep=1:nrep
    % Matlab needs 1 iteration to compile the function
    if nrep==2, mex_fast=0; mat_fast=0; mex_std=0; mat_std=0; end
    for gi=1:length(graphs)
        load([graphdir graphs{gi} '.mat']); n=size(A,1);
        At=A'; [rp ci ai]=sparse_to_csr(A); As.rp=rp; As.ci=ci; As.ai=ai;
        for ti=1:ntests
            fprintf([repmat('\b',1,76) '%20s rep=%3i graph=%-30s trial=%4i'], ...
                'dfs',rep,graphs{gi},ti);
            v=ceil(n*rand(1));
            tic; d1=dfs(A,v); mex_std=mex_std+toc;
            tic; d2=dfs(At,v,struct('istrans',1,'nocheck',1));
              mex_fast=mex_fast+toc;
            tic; d3=dfstest(A,v); mat_std=mat_std+toc;
            tic; d4=dfstest(As,v); mat_fast=mat_fast+toc;
            if any(d1 ~= d2) || any(d2 ~= d3) || any(d3 ~= d4)
                error('gaimc:dfs','incorrect results from dijkstra');
            end
        end
    end
end
fprintf('\n');
delete('dfstest.m');
results(end+1).name='dfs';
results(end).mex_fast = mex_fast;
results(end).mat_fast = mat_fast;
results(end).mex_std = mex_std;
results(end).mat_std = mat_std;

%% Connected components
% To evaluate the performance of the connected components algorithm, we use
% sets of random graphs.
nrep=30;
szs=[1 10 100 5000 10000 50000];
comp_results=[mex_fast mat_fast mex_std mat_std];
for szi=1:length(szs)
    fprintf('\n%20s size=%-5i     ', 'scomponents', szs(szi));
    % Matlab needs 1 iteration to compile the function
    if szi==2, mex_fast=0; mat_fast=0; mex_std=0; mat_std=0; end
    for rep=1:nrep
        fprintf('\b\b\b\b'); fprintf(' %3i', rep);
        A=sprand(szs(szi),szs(szi),25/szs(szi));
        At=A'; [rp ci ai]=sparse_to_csr(A); As.rp=rp; As.ci=ci; As.ai=ai;
        tic; cc1=components(A); mex_std=mex_std+toc;
        tic; cc2=components(At,struct('istrans',1,'nocheck',1));
            mex_fast=mex_fast+toc;
        tic; cc3=scomponents(A); mat_std=mat_std+toc;
        tic; cc4=scomponents(As); mat_fast=mat_fast+toc;
        cs1=accumarray(cc1,1,[max(cc1) 1]);
        cs2=accumarray(cc2,1,[max(cc2) 1]);
        cs3=accumarray(cc3,1,[max(cc3) 1]);
        cs4=accumarray(cc4,1,[max(cc4) 1]);
        if any(cs1 ~= cs2) || any(cs2 ~= cs3) || any(cs2 ~= cs4)
            error('gaimc:scomponents','incorrect results from scomponents');
        end
    end
    comp_results(end+1,:) = [mex_fast mat_fast mex_std mat_std];
end
comp_results=diff(comp_results);
results(end+1).name='scomponents';
results(end).mex_fast = mex_fast;
results(end).mat_fast = mat_fast;
results(end).mex_std = mex_std;
results(end).mat_std = mat_std;

%%
% Plot the data for connected components
% This plot isn't too meaningful, so I've omitted it.

% plot(szs, comp_results,'.-');
% legend('mex fast','mat fast','mex std','mat std','Location','Northwest');
% ylabel('time'); xlabel('graph size');

%% Dijkstra's algorithm
% To evaluate the performance of Dijkstra's algorithm, we pick
graphs = {'clr-25-2', 'clr-24-1', 'cs-stanford', ...
    'minnesota', 'tapir'};
nrep=30; ntests=100; mex_fast=0; mat_fast=0; mex_std=0; mat_std=0;
for rep=1:nrep
    for gi=1:length(graphs)
        load([graphdir graphs{gi} '.mat']); n=size(A,1);
        At=A'; [rp ci ai]=sparse_to_csr(A); As.rp=rp; As.ci=ci; As.ai=ai;
        for ti=1:ntests
            fprintf([repmat('\b',1,66) '%20s rep=%3i graph=%-20s trial=%4i'], ...
                'dijkstra',rep,graphs{gi},ti);
            v=ceil(n*rand(1));
            tic; d1=dijkstra_sp(A,v); mex_std=mex_std+toc;
            tic; d2=dijkstra_sp(At,v,struct('istrans',1,'nocheck',1));
              mex_fast=mex_fast+toc;
            tic; d3=dijkstra(A,v); mat_std=mat_std+toc;
            tic; d4=dijkstra(As,v); mat_fast=mat_fast+toc;
            if any(d1 ~= d2) || any(d2 ~= d3) || any(d3 ~= d4)
                error('gaimc:dijkstra','incorrect results from dijkstra');
            end
        end
    end
end
fprintf('\n');
results(end+1).name='dijkstra';
results(end).mex_fast = mex_fast;
results(end).mat_fast = mat_fast;
results(end).mex_std = mex_std;
results(end).mat_std = mat_std;

%% Directed Clustering coefficients
nrep=30; mex_fast=0; mat_fast=0; mex_std=0; mat_std=0;
comp_results=[mex_fast mat_fast mex_std mat_std];
szs=[1 10 100 5000 10000 50000];
for szi=1:length(szs)
    fprintf('\n%20s size=%-5i     ', 'dirclustercoeffs', szs(szi));
    % Matlab needs 1 iteration to compile the function
    if szi==2, mex_fast=0; mat_fast=0; mex_std=0; mat_std=0; end
    for rep=1:nrep
        fprintf('\b\b\b\b'); fprintf(' %3i', rep);
        A=sprand(szs(szi),szs(szi),25/szs(szi));
        At=A';
        [rp ci ai]=sparse_to_csr(A); As.rp=rp; As.ci=ci; As.ai=ai;
        [cp ri ati]=sparse_to_csr(At); As.cp=cp; As.ri=ri; As.ati=ati;
        tic; cc1=clustering_coefficients(A); mex_std=mex_std+toc;
        tic; cc2=clustering_coefficients(At,struct('istrans',1,'nocheck',1));
            mex_fast=mex_fast+toc;
        tic; cc3=dirclustercoeffs(A); mat_std=mat_std+toc;
        tic; cc4=dirclustercoeffs(As); mat_fast=mat_fast+toc;
    end
    comp_results(end+1,:) = [mex_fast mat_fast mex_std mat_std];
end
fprintf('\n');
comp_results=diff(comp_results);
results(end+1).name='dirclustercoeffs';
results(end).mex_fast = mex_fast;
results(end).mat_fast = mat_fast;
results(end).mex_std = mex_std;
results(end).mat_std = mat_std;

%% Minimum spanning tree
nrep=30; mex_fast=0; mat_fast=0; mex_std=0; mat_std=0;
comp_results=[];
szs=[10 100 5000 10000];
for szi=1:length(szs)
    fprintf('\n%20s size=%-5i     ', 'mst_prim', szs(szi));
    % Matlab needs 1 iteration to compile the function
    if szi==2, mex_fast=0; mat_fast=0; mex_std=0; mat_std=0; end
    for rep=1:nrep
        fprintf('\b\b\b\b'); fprintf(' %3i', rep);
        A=abs(sprandsym(szs(szi),25/szs(szi)));
        At=A';
        [rp ci ai]=sparse_to_csr(A); As.rp=rp; As.ci=ci; As.ai=ai;
        tic; T1=prim_mst(A); mex_std=mex_std+toc;
        tic; [t1i t1j t1v]=prim_mst(At,struct('istrans',1,'nocheck',1));
            mex_fast=mex_fast+toc;
        tic; T2=mst_prim(A,0); mat_std=mat_std+toc;
        tic; [t2i t2j t2v]=mst_prim(As,0); mat_fast=mat_fast+toc;
        T1f=sparse(t1i,t1j,t1v,size(A,1),size(A,2));
        T2f=sparse(t2i,t2j,t2v,size(A,1),size(A,2));
        if ~isequal(T1,T2) || ~isequal(T1f+T1f',T2f+T2f') || ...
                ~isequal(T1,T2f+T2f')
            keyboard
            warning('gaimc:mst_prim',...
                'incorrect results from mst_prim (%i,%i)', szi, rep);
        end
    end
    comp_results(end+1,:) = [mex_fast mat_fast mex_std mat_std];
end
fprintf('\n');
comp_results=diff(comp_results);
results(end+1).name='mst_prim';
results(end).mex_fast = mex_fast;
results(end).mat_fast = mat_fast;
results(end).mex_std = mex_std;
results(end).mat_std = mat_std;

%% Undirected Clustering coefficients
nrep=30; mex_fast=0; mat_fast=0; mex_std=0; mat_std=0;
undircc_results=[mex_fast mat_fast mex_std mat_std];
szs=[1 10 100 5000 10000 50000];
for szi=1:length(szs)
    fprintf('\n%20s size=%-5i     ', 'clustercoeffs', szs(szi));
    % Matlab needs 1 iteration to compile the function
    if szi==2, mex_fast=0; mat_fast=0; mex_std=0; mat_std=0; end
    for rep=1:nrep
        fprintf('\b\b\b\b'); fprintf(' %3i', rep);
        A=abs(sprandsym(szs(szi),25/szs(szi)));
        At=A';
        [rp ci ai]=sparse_to_csr(A); As.rp=rp; As.ci=ci; As.ai=ai;
        tic; cc1=clustering_coefficients(A,struct('undirected',1)); mex_std=mex_std+toc;
        tic; cc2=clustering_coefficients(At,struct('undirected',1,'istrans',1,'nocheck',1));
            mex_fast=mex_fast+toc;
        tic; cc3=clustercoeffs(A); mat_std=mat_std+toc;
        tic; cc4=clustercoeffs(As); mat_fast=mat_fast+toc;
    end
    undircc_results(end+1,:) = [mex_fast mat_fast mex_std mat_std];
end
%%
fprintf('\n');
undircc_results=diff(undircc_results);
results(end+1).name='clustercoeffs';
results(end).mex_fast = mex_fast;
results(end).mat_fast = mat_fast;
results(end).mex_std = mex_std;
results(end).mat_std = mat_std;

%% Summarize the results
% We are going to summarize the results in a bar plot based on the
% algorithm.  Each algorithm is a single bar
graphs = {'all_shortest_paths_example', 'clr-24-1', 'cs-stanford', ...
    'minnesota','tapir'};, where the performance of the
% mex code is 1.
nresults=length(results);
Ystd = zeros(nresults,1);
Yfast = zeros(nresults,1);
for i=1:nresults
    Ystd(i)=results(i).mat_std/results(i).mex_std;
    Yfast(i)=results(i).mat_fast/results(i).mex_fast;
end
bar(1:nresults,[Ystd Yfast]); set(gca,'XTickLabel',{results.name});
legend('Standard','Fast','Location','Northwest');


function state = psoplotswarm(options,state,flag,ijk)
% Plots the positions of particle swarm.
%
% ijk is an additional parameter, in the form of a 1x3 vector. It lists the
% dimension of the problem to be plotted, for a multidimensional problem.
%
% Updated this to conform with MATLAB GA toolbox specifications.

% We'll only plot the first two dimensions by default. User can
% set different dimensions to be plotted using ijk.

if nargin < 4 && size(state.Population,2) > 1 % Go to defaults
    ijk = [1,2] ; % k doesn't exist
elseif size(state.Population,2) == 1 % For 1-D problems
    ijk = 1 ;
else % Robust input check
    ijk = reshape(ijk,1,[]) ;
end

% Input checking
if size(ijk,2) > 3
    warning('PSO:Plotting:tooManyDimensions',...
        'Unable to plot %g dimensions. Plotting first 3 only',...
        length(ijk))
end % if length

if strcmpi(flag(1:4),'init') % Initialize
    delete(findobj(gca,'-regexp','Tag','*Locations'))
    initLoc = line(state.Population(:,ijk(1)),state.Population(:,ijk(2)),...
        'Color',0.75*ones(1,3),...
        'Marker','.',...
        'LineStyle','none',...
        'Tag','Initial Locations') ;

    % Set reasonable axes limits
    % ---------------------------------------------------------------------
    xlim([options.PopInitRange(1,ijk(1)) options.PopInitRange(2,ijk(1))])
    if size(ijk,2) > 1
        ylim([options.PopInitRange(1,ijk(2)) ...
            options.PopInitRange(2,ijk(2))])
        if size(ijk,2) > 2
            zlim([options.PopInitRange(1,ijk(3)) ...
                options.PopInitRange(2,ijk(3))])
        end % if size
    end % if size
    % ---------------------------------------------------------------------

    title('Swarm positions')
    set(gca,'Tag','Swarm Plot','NextPlot','add')

    % Initialize plots
    % ---------------------------------------------------------------------
    if size(ijk,2) == 1 %  One dimensional
        currentLoc = line(state.Population(:,ijk(1)),...
            zeros(size(state.Population,1),1)) ;
    elseif size(ijk,2) == 2 % Two dimensional
        currentLoc = line(state.Population(:,ijk(1)),...
            state.Population(:,ijk(2))) ;
    elseif size(ijk,2) == 3 % Three dimensional
        currentLoc = line(state.Population(:,ijk(1)),...
            state.Population(:,ijk(2)),...
            state.Population(:,ijk(3))) ;
    end % if size
    set(currentLoc,...
            'LineStyle','none',...
            'Marker','.',...
            'Color','blue',...
            'Tag','Swarm Locations') ;
    % ---------------------------------------------------------------------
elseif strcmpi(flag(1:4),'iter') % Iterate
    currentLoc = findobj(gca,'Tag','Swarm Locations','Type','line') ;
    if size(ijk,2) == 1 %  One dimensional
        set(currentLoc,'XData',state.Population(:,ijk(1)))
    elseif size(ijk,2) == 2 % Two dimensional
        set(currentLoc,...
            'XData',state.Population(:,ijk(1)),...
            'YData',state.Population(:,ijk(2)))
    elseif size(ijk,2) == 3 % Three dimensional
        set(currentLoc,...
            'XData',state.Population(:,ijk(1)),...
            'YData',state.Population(:,ijk(2)),...
            'ZData',state.Population(:,ijk(3)))
    end
end

if strcmpi(flag(1:4),'init')
    legend([initLoc,currentLoc],{'Initial Positions','Current Positions'})
end


function [] = plot_nodes(ax, parsed_osm, only_node_indices, show_id)
% plot (selected) nodes and label each with index and id
%
% usage
%   PLOT_NODES(ax, parsed_osm, only_node_indices, show_id)
%
% input
%   ax = axes object handle where to plot the nodes.
%   parsed_osm = MATLAB structure containing the OpenStreetMap XML data
%                after parsing, as returned by function
%                parse_openstreetmap.
%   only_node_indices = selected node indices in the global node matrix.
%   show_id = select whether to show or not the ID numbers of nodes as text
%             labes within the plot
%           = 0 (do not show labels) | 1 (show labels)
%
% 2012.04.24 (c) Ioannis Filippidis, jfilippidis@gmail.com
%
% See also PARSE_OPENSTREETMAP, ROUTE_PLANNER.

% do not show node id (default)
if nargin < 4
    show_id = 0;
end

nodes = parsed_osm.node;
node_ids = nodes.id;
node_xys = nodes.xy;

% which nodes to plot ?
n = size(node_xys, 2);
if nargin < 3
    only_node_indices = 1:n;
end

%% plot
held = takehold(ax);

% nodes selected exist ?
if n < max(only_node_indices)
    warning('only_node_indices contains node indices which are too large.')
    return
end

% plot nodes
xy = node_xys(:, only_node_indices);
plotmd(ax, xy, 'yo')

% label plots
for i=only_node_indices
    node_id_txt = num2str(node_ids(1, i) );
    if show_id
        curtxt = {['index=', num2str(i) ], ['id=', node_id_txt] }.';
    else
        curtxt = ['index=', num2str(i) ];
    end
    textmd(node_xys(:, i), curtxt, 'Parent', ax)
end

restorehold(ax, held)


function [idx,idy] = convertind(ind,m,n)
    if mod(ind,m) == 0
        idx = m;idy = ind/m;
    else
        idx = mod(ind,m);idy = floor(ind/m)+1;
    end
end


function HamilExp = DoublePendulumEnergy(in1,in2)
%DOUBLEPENDULUMENERGY
%    HAMILEXP = DOUBLEPENDULUMENERGY(IN1,IN2)

%    This function was generated by the Symbolic Math Toolbox version 8.2.
%    21-Sep-2019 22:29:59

dz1 = in2(:,1);
dz2 = in2(:,2);
z1 = in1(:,1);
z2 = in1(:,2);
HamilExp = cos(z1).*(2.09e2./2.0e2)+cos(z2).*3.268e-1+dz1.^2.*1.376e-2+dz2.^2.*3.272e-3+dz1.*dz2.*cos(z1-z2).*8.885000000000001e-3;


% LFCalRectifyLF - rectify a light field using a calibrated camera model, called as part of LFUtilDecodeLytroFolder
%
% Usage:
%     [LF, RectOptions] = LFCalRectifyLF( LF, CalInfo, RectOptions )
%     [LF, RectOptions] = LFCalRectifyLF( LF, CalInfo )
%
% This function is called by LFUtilDecodeLytroFolder to rectify a light field. It follows the
% rectification procedure described in:
%
% D. G. Dansereau, O. Pizarro, and S. B. Williams, "Decoding, calibration and rectification for
% lenslet-based plenoptic cameras," in Computer Vision and Pattern Recognition (CVPR), IEEE
% Conference on. IEEE, Jun 2013.
%
% Minor differences from the paper: light field indices [i,j,k,l] are 1-based in this
% implementation, and not 0-based as described in the paper.
%
% Note that a calibration should only be applied to a light field decoded using the same lenslet
% grid model. This is because the lenslet grid model forms an implicit part of the calibration,
% and changing it will invalidate the calibration.
%
% LFCalDispRectIntrinsics is useful for visualizing the sampling pattern associated with a requested
% intrinsic matrix.
%
% Inputs:
%
%         LF : The light field to rectify; should be floating point
%
%     CalInfo struct contains a calibrated camera model, see LFUtilCalLensletCam:
%            .EstCamIntrinsicsH : 5x5 homogeneous matrix describing the lenslet camera intrinsics
%            .EstCamDistortionV : Estimated distortion parameters
%
%    [optional] RectOptions struct (all fields are optional) :
%        .NInverse_Distortion_Iters : Number of iterations in inverse distortion estimation
%                        .Precision : 'single' or 'double'
%               .RectCamIntrinsicsH : Requests a specific set of intrinsics for the rectified light
%                                     field. By default the rectified intrinsic matrix is
%                                     automatically constructed from the calibrated intrinsic
%                                     matrix, but this process can in some instances yield poor
%                                     results: excessive black space at the edges of the light field
%                                     sample space, or excessive loss of scene content off the edges
%                                     of the space. This parameters allows you to fine-tune the
%                                     desired rectified intrinsic matrix.
%
% Outputs :
%
%     LF : The rectified light field
%     RectOptions : The rectification options as applied, including any default values employed.
%
% User guide: LFToolbox.pdf
% See also: LFCalDispRectIntrinsics, LFUtilCalLensletCam, LFUtilDecodeLytroFolder, LFUtilProcessWhiteImages

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

function [LF, RectOptions] = LFCalRectifyLF( LF, CalInfo, RectOptions )

%---Defaults---
RectOptions = LFDefaultField( 'RectOptions', 'Precision', 'single' );
RectOptions = LFDefaultField( 'RectOptions', 'NInverse_Distortion_Iters', 2 );
RectOptions = LFDefaultField( 'RectOptions', 'MaxUBlkSize', 32 );
LFSize = size(LF);
RectOptions = LFDefaultField( 'RectOptions', 'RectCamIntrinsicsH', LFDefaultIntrinsics( LFSize, CalInfo ) );

%---Build interpolation indices---
fprintf('Generating interpolation indices...\n');
NChans = LFSize(5);
LF = cast(LF, RectOptions.Precision);

%---chop up the LF along u---
fprintf('Interpolating...');
UBlkSize = RectOptions.MaxUBlkSize;
LFOut = LF;
for( UStart = 1:UBlkSize:LFSize(4) )
    UStop = UStart + UBlkSize - 1;
    UStop = min(UStop, LFSize(4));

    t_in=cast(1:LFSize(1), 'uint16'); % saving some mem by using uint16
    s_in=cast(1:LFSize(2), 'uint16');
    v_in=cast(1:LFSize(3), 'uint16');
    u_in=cast(UStart:UStop, 'uint16');
    [tt,ss,vv,uu] = ndgrid(t_in,s_in,v_in,u_in);

    % InterpIdx initially holds the index of the desired ray, and is evolved through the application
    % of the inverse distortion model to eventually hold the continuous-domain index of the undistorted
    % ray, and passed to the interpolation step.
    InterpIdx = [ss(:)'; tt(:)'; uu(:)'; vv(:)'; ones(size(ss(:)'))];
    DestSize = size(tt);
    clear tt ss vv uu

    InterpIdx = LFMapRectifiedToMeasured( InterpIdx, CalInfo, RectOptions );

    for( ColChan = 1:NChans )
        % todo[optimization]: use a weighted interpolation scheme to exploit the weight channel
        InterpSlice = interpn(squeeze(LF(:,:,:,:,ColChan)), InterpIdx(2,:),InterpIdx(1,:), InterpIdx(4,:),InterpIdx(3,:), 'linear');
        InterpSlice = reshape(InterpSlice, DestSize);
        LFOut(:,:,:,UStart:UStop,ColChan) = InterpSlice;
    end

    fprintf('.')
end
LF = LFOut;
clear LFOut;

%---Clip interpolation result, which sometimes rings slightly out of range---
LF(isnan(LF)) = 0;
LF = max(0, min(1, LF));

fprintf('\nDone\n');

end


function [ok, pns_norm, pns_comp, t_axis]=calcPNS(obj,hardware,doPlots)
% calculate PNS using safe model implementation by Szczepankiewicz and Witzel
% assumes safe_pns_prediction package has been downloaded and installed in
% Matlab path. See http://github.com/filip-szczepankiewicz/safe_pns_prediction
%
% returns pns levels due to respective axes (normalized to 1 and not to 100%)
%
% inputs:
%     hardware - hardware specifications. see safe_example_hw() from
%                the safe_pns_prediction package. Alternatively a text file
%                in the .asc format (Siemens) can be passed, e.g. for Prisma
%                it is MP_GPA_K2309_2250V_951A_AS82.asc (we leave it as an
%                exercise to the interested user to find were these files
%                can be acquired from);
%     doPlots  - optional parameter (defaluts to true)

if nargin < 3
    doPlots=true;
end

% acquire the entire gradient wave form
gw=obj.waveforms_and_times();
if doPlots
    figure;
    plot(gw{1}(1,:),gw{1}(2,:),gw{2}(1,:),gw{2}(2,:),gw{3}(1,:),gw{3}(2,:)); % plot the entire gradient shape
    title('gradient wave form, in T/m');
end

% find beginning and end times and resample GWs to a regular sampling raster
tf=[];
tl=[];
for i=1:3
    if size(gw{i},2)>0
        tf(end+1)=gw{i}(1,1);
        tl(end+1)=gw{i}(1,end);
    end
end
nt_min=floor(min(tf)/obj.gradRasterTime+eps);
nt_max=ceil(max(tl)/obj.gradRasterTime-eps);
% shift raster positions to the centers of the raster periods
nt_min = nt_min + 0.5;
nt_max = nt_max - 0.5;
if (nt_min<0.5)
    nt_min=0.5
end
t_axis=(nt_min:nt_max)*obj.gradRasterTime;
gwr=zeros(length(t_axis),3);
for i=1:3
    if size(gw{i},2)>0
        gwr(:,i)=interp1(gw{i}(1,:),gw{i}(2,:),t_axis,'linear',0);
    end
end

if ischar(hardware)
    % this loads the parameters from the provided text file
    asc=mr.Siemens.readasc(hardware);
    hardware=asc_to_hw(asc);
end

% use the Szczepankiewicz' and Witzel's implementation
[pns_comp,res]=safe_gwf_to_pns(gwr/obj.sys.gamma, NaN*ones(length(t_axis),1), obj.gradRasterTime, hardware); % the RF vector is unused in the code inside but it is zeropaded and exported ...
% use the exported RF vector to detect and undo zerpopadding
pns_comp=0.01*pns_comp(~isfinite(res.rf),:)';
% calc pns_norm and the final ok/not_ok
pns_norm=vecnorm(pns_comp);
ok=all(pns_norm<1);
% ready
if doPlots
    % plot results
    figure;safe_plot(pns_comp'*100, obj.gradRasterTime);
end

end

% local utility functions

function hw = asc_to_hw(asc)
% function hw = asc_to_hw(asc)
%
% SAFE model parameters for the asc structure as read from the asc file.
% See comments for units.
%
% Maxim Zaitsev 08/10/2019

if isfield(asc,'asCOMP') && isfield(asc.asCOMP,'tName')
    hw.name          = asc.asCOMP(1).tName;
else
    hw.name          = 'unknown';
end
%hw.look_ahead    =  1.0; % MZ: this is not a real hardware parameter but a coefficient, with which the final result is multiplied

hw.x.tau1        = asc.flGSWDTauX(1);  % ms
hw.x.tau2        = asc.flGSWDTauX(2);  % ms
hw.x.tau3        = asc.flGSWDTauX(3);  % ms
hw.x.a1          = asc.flGSWDAX(1);
hw.x.a2          = asc.flGSWDAX(2);
hw.x.a3          = asc.flGSWDAX(3);
hw.x.stim_limit  = asc.flGSWDStimulationLimitX;  % T/m/s
hw.x.stim_thresh = asc.flGSWDStimulationThresholdX;  % T/m/s
hw.x.g_scale     = asc.asGPAParameters.sGCParameters.flGScaleFactorX;

hw.y.tau1        = asc.flGSWDTauY(1);  % ms
hw.y.tau2        = asc.flGSWDTauY(2);  % ms
hw.y.tau3        = asc.flGSWDTauY(3);  % ms
hw.y.a1          = asc.flGSWDAY(1);
hw.y.a2          = asc.flGSWDAY(2);
hw.y.a3          = asc.flGSWDAY(3);
hw.y.stim_limit  = asc.flGSWDStimulationLimitY;  % T/m/s
hw.y.stim_thresh = asc.flGSWDStimulationThresholdY;  % T/m/s
hw.y.g_scale     = asc.asGPAParameters.sGCParameters.flGScaleFactorY;

hw.z.tau1        = asc.flGSWDTauZ(1);  % ms
hw.z.tau2        = asc.flGSWDTauZ(2);  % ms
hw.z.tau3        = asc.flGSWDTauZ(3);  % ms
hw.z.a1          = asc.flGSWDAZ(1);
hw.z.a2          = asc.flGSWDAZ(2);
hw.z.a3          = asc.flGSWDAZ(3);
hw.z.stim_limit  = asc.flGSWDStimulationLimitZ;  % T/m/s
hw.z.stim_thresh = asc.flGSWDStimulationThresholdZ;  % T/m/s
hw.z.g_scale     = asc.asGPAParameters.sGCParameters.flGScaleFactorZ;
end


clear
clc

addpath(genpath(pwd));
addpath('../../edges-master');
deep_model_path = 'model_deep_contour/';
deepModel.model_del_file = [deep_model_path 'deploy_cov4.prototxt'];
deepModel.model_file = [deep_model_path 'deep_contour_model'];
deepModel.mean_proto_file = [deep_model_path 'mean.binaryproto.txt'];
deepModel.use_gpu = 1;
modelDir = [deep_model_path 'models'];
use_gpu = deepModel.use_gpu;
gpu_id = 0;
[nBatch, nC, patchSize, ~, nOutputNum] = parse_del_file(deepModel.model_del_file);

PATCH_MEAN = single(dlmread(deepModel.mean_proto_file));

PATCH_MEAN = single(reshape(PATCH_MEAN, patchSize, patchSize, nC));

if exist('use_gpu', 'var')
  matcaffe_init(deepModel.use_gpu, deepModel.model_del_file, deepModel.model_file, gpu_id);
else
  matcaffe_init();
end
featSelecttion = false;

if featSelecttion
    if exist([modelDir '/feat_selection.mat'], 'file')
        load([modelDir '/feat_selection.mat']);
    else
        [selected_dims, ndim] = deepSparseFeatSelection([modelDir '/feat/'], 1e-3, 0.7);
        if(~isempty(ndim) && ~isempty(selected_dims))
            save([modelDir '/feat_selection.mat'], 'selected_dims', 'ndim');
        end
    end
else
    selected_dims = [];
end

dlPara.selected_dims = selected_dims;
dlPara.patch_mean = PATCH_MEAN;
dlPara.modelDir = modelDir;

edgesDLDemo(dlPara);


function quadrule_test ( )

%*****************************************************************************80
%
%% QUADRULE_FAST_TEST tests the QUADRULE_FAST library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 March 2007
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'QUADRULE_FAST_TEST\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Test the QUADRULE_FAST library.\n' );

  quadrule_fast_test01 ( );
  quadrule_fast_test02 ( );
  quadrule_fast_test03 ( );

  quadrule_fast_test04 ( );
  quadrule_fast_test05 ( );
  quadrule_fast_test06 ( );

  quadrule_fast_test07 ( );
  quadrule_fast_test08 ( );
  quadrule_fast_test09 ( );

  quadrule_fast_test10 ( );
  quadrule_fast_test11 ( );
  quadrule_fast_test12 ( );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'QUADRULE_FAST_TEST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


function x = emailFeatures(word_indices)
%EMAILFEATURES takes in a word_indices vector and produces a feature vector
%from the word indices
%   x = EMAILFEATURES(word_indices) takes in a word_indices vector and
%   produces a feature vector from the word indices.

% Total number of words in the dictionary
n = 1899;

% You need to return the following variables correctly.
x = zeros(n, 1);

% ====================== YOUR CODE HERE ======================
% Instructions: Fill in this function to return a feature vector for the
%               given email (word_indices). To help make it easier to
%               process the emails, we have have already pre-processed each
%               email and converted each word in the email into an index in
%               a fixed dictionary (of 1899 words). The variable
%               word_indices contains the list of indices of the words
%               which occur in one email.
%
%               Concretely, if an email has the text:
%
%                  The quick brown fox jumped over the lazy dog.
%
%               Then, the word_indices vector for this text might look
%               like:
%
%                   60  100   33   44   10     53  60  58   5
%
%               where, we have mapped each word onto a number, for example:
%
%                   the   -- 60
%                   quick -- 100
%                   ...
%
%              (note: the above numbers are just an example and are not the
%               actual mappings).
%
%              Your task is take one such word_indices vector and construct
%              a binary feature vector that indicates whether a particular
%              word occurs in the email. That is, x(i) = 1 when word i
%              is present in the email. Concretely, if the word 'the' (say,
%              index 60) appears in the email, then x(60) = 1. The feature
%              vector should look like:
%
%              x = [ 0 0 0 0 1 0 0 0 ... 0 0 0 0 1 ... 0 0 0 1 0 ..];
%
%
for i=1:size(word_indices)
    x(word_indices(i)) = 1;
end
% =========================================================================

end


function cs_demo3 (do_pause, matrixpath)
%CS_DEMO3 MATLAB version of the CSparse/Demo/cs_demo3.c program.
%   Cholesky update/downdate.
%
% Example:
%   cs_demo3
% See also: cs_demo

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

if (nargin < 2)
    matrixpath = [] ;
end

if (isempty (matrixpath))
    try
        % older versions of MATLAB do not have an input argument to mfilename
        p = mfilename ('fullpath') ;
        t = strfind (p, filesep) ;
        matrixpath = [ p(1:t(end)) '../../Matrix' ] ;
    catch
        % assume we are in the C*Sparse/MATLAB/CSparse/Demo directory
        matrixpath = '../../Matrix' ;
    end
end

matrices = { 'HB/bcsstk01', 'HB/bcsstk16' } ;

if (nargin < 1)
    do_pause = 1 ;
end

for i = 1:length(matrices)
    name = matrices {i} ;
    [C sym] = get_problem (matrixpath, name, 1e-14) ;
    demo3 (C, sym, name) ;
    if (do_pause)
        input ('Hit enter to continue: ') ;
    end
end


function [ gmm_c ] = compress_gmm(gmm,Xtrain,threashold,options)
%COMPRESS_GMM Compresses a GMM in which there are possibly duplicate
%parameters.
%
%   input ------------------------------------------------------
%
%       o gmm: strcut, gmm.Priors
%                      gmm.Mu
%                      gmm.Sigma
%
%       o Xtrain: (N x D), training data used to retrain the parameters of
%       the GMM after a few have been removed
%
%   output -----------------------------------------------------
%
%       o gmm_c: struct, GMM with less than or equal number of parameters
%       as gmm.
%

GMModel.ComponentProportion = gmm.Priors;
GMModel.mu                  = gmm.Mu;
GMModel.Sigma               = gmm.Sigma;

gmm_c = merge_gmm_components2(GMModel,Xtrain,threashold,options);

end


function varargout = squeeze(varargin)
%SQUEEZE   Squeeze a SPHEREFUN to one variable, if possible.
%   G = squeeze(F) returns a SPHEREFUN if F depends on LAMBDA and THETA in
%   spherical coordinates. If F depends only on the lambda-variable a row
%   CHEBFUN is returned and if it depends on just the theta-variable a
%   column CHEBFUN is returned.

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

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


function mm = minmax(data)
mm = zero_crossing(diff(data));


function [ dbCA, cones, dir, Q, P, dualized ] = eliminate( prob, destructive, can_dual )
if nargin < 3, can_dual = nargout >= 6; end
if nargin < 2, destructive = false; end

% For the problem
%
%    minimize c' * x + d
%    s.t.     y : A * x + b == 0
%             x \in K
%
% The Lagrangian is
%
%  L(x,y,z) = c' * x + d - y' * ( A x + b ) - z' * x
%
%                            [ - d  b' 0 ]   [ 1 ]
%            = - [ 1, x' ] * [ - c  A' I ] * [ y ]
%                                            [ z ]
%
% This function provides a smaller [ d, b' ; c, A' ] with no more nonzeros
% that solves an equivalent problem. The original x and y can be recovered
% from the reduced xx and yy by Q*[1;xx] and P*[1;-yy], respectively.

[ dbCA, cones, dir, Q, P ] = extract( prob, destructive );
dualized = false;
if size( dbCA, 1 ) == 1,
    return;
end

%
% Negate the objective so that the transformation matrices P and Q are
% properly formed.
%

dbCA(:,1) = -dbCA(:,1);
if ~issparse( dbCA ),
    dbCA = sparse( dbCA );
end

for pass = 1 : 2,

    if pass == 1 || dualized,
        n_tot = 0;
        nn  = size(dbCA,1);
        rsv = sparse( 1, 1, 1, nn, 1 );
        nng = sparse( nn, 1 );
        for k = 1 : length( cones ),
            temp = cones(k).indices;
            n_tot = n_tot + numel(temp);
            temp = sparse( temp, 1, 1, nn, 1 );
            rsv = rsv + temp;
            if isequal( cones(k).type, 'nonnegative' ),
                nng = nng + temp;
            elseif can_dual && strncmp( cones(k).type, 'i_', 2 ),
                can_dual = false;
            end
        end
        rsv   = full( rsv );
        nng   = full( nng );
        ndxs  = ( 1 : nn )';
        nold  = nn;
    end

    cc   = dbCA( :, 1 );
    rcnt = sum( dbCA ~= 0, 2 );

    % In the first pass, we don't eliminate columns which have inequality
    % structure to them, so that we can make the best decision as to
    % whether or not to convert the problem to dual standard form. Exempted
    % from this are columns with trivial xi = xj constraints, where xi is free.
    if pass == 1,
        trivs = sum( dbCA(rsv==0,:) ~= 0, 1 ) == 1 & sum( dbCA(rsv~=0,:) ~= 0, 1 ) - ( dbCA( 1, : ) ~= 0 ) == 1;
        ineqs = full(any(dbCA(rsv~=0&rcnt==1,:),1)) & full(~trivs);
        ineqs = +ineqs;
    else % if dualized,
        ineqs = zeros(1,size(dbCA,2));
    end
    ineqs(1) = 1;

    while true,

        success = false;

        %
        % STEP 1: Look for free or nonnegative variables that do not appear
        % in any constraints. Unconstrained variables that also appear in
        % the objective are unbounded, as are nonnegative variables that
        % appear there with the right sign. Otherwise their values might
        % as well be zero. If we have multiple unbounded variables, keep
        % all but one so that the solver can still see this happen.
        %
        % Eliminated for now. Frankly, my suspicion is that this happens
        % very infrequently, and this code seems to have been the source
        % of bugs in the past.
        %

        if 0,
            rows = ( rcnt == ( cc ~= 0 ) ) & ( ~rsv | nng );
            nnzr = nnz( rows );
            if nnzr > 0,
                csgn = 1 - 2 * dualized;
                celm = csgn * cc( rows, 1 );
                celm( nng(rows) & celm < 0 ) = 0;
                nnzc = nnz( celm );
                if nnzc > 1 || nnzr > nnzc,
                    success = true;
                    if nnzc,
                        cnrm = norm( celm );
                        ndxq = find( rows );
                        ndxq = ndxq( celm ~= 0 );
                        ndxq = ndxq( 1 );
                        Q( :, ndxq ) = Q( :, rows ) * ( celm / cnrm );
                        dbCA( ndxq, 1 ) = csgn * cnrm; %#ok
                        rows( ndxq ) = 0;
                    end
                    rowX = ~rows;
                    dbCA = dbCA( rowX, : );
                    rsv  = rsv ( rowX, : );
                    nng  = nng ( rowX, : );
                    ndxs = ndxs( rowX, : );
                    Q    =    Q( :, rowX );
                end
            end
        end

        %
        % STEP 2: Look for columns which differ only by a constant factor.
        % These correspond to redundant equality constraints. These occur
        % often enough as as consequence of our tranformation method, and
        % they cause problems in solvers, so we must eliminate them. Of
        % course, if there are more complex linear dependencies in the
        % equality constraints, we can't do anything about that.
        %

        [ xR, dbCA ] = cvx_bcompress( dbCA, 'full', 1 );
        if size( xR, 1 ) ~= size( xR, 2 ),
            success = true;
            P       = P * cvx_invert_structure( xR );
            ineqs   = ( xR * ineqs(:) )' ~= 0;
            ineqs   = +ineqs;
        end

        while true,

            %
            % STEP 3: Look for variables that we can eliminate without
            % increasing fill-in. This means looking for rows or columns
            % with only 1, 2, or (in some cases) 3 nonzeros.
            %

            [ rows, cols ] = cvx_eliminate_mex( dbCA, 1, rsv, ineqs );
            if ~any( rows ), break; end
            success = true;
            rows = rows ~= 0;
            cols = cols ~= 0;
            rowX = ~rows;
            colX = ~cols;

            %
            % [ x1^T x2^T ] [ C1 A11 A12 ] [ 1  ]
            %               [ C2 A21 A22 ] [ y1 ] = 0
            %                              [ y2 ]
            %
            % [ x1^T x2^T ] = x1^T [ I -A12*A22i ]
            %
            % [ G Y1^T Y2^T ] = [ G Y1^T ] [ I  0  -C2'*A22i'  ]
            %                              [ 0  I  -A21'*A22i' ]

            %
            A11  = dbCA( rowX, colX );
            A12  = dbCA( rowX, cols );
            A21  = dbCA( rows, colX );
            A22  = dbCA( rows, cols );
            if ( size( A22, 1 ) ~= size( A22, 2 ) || nnz( A22 ) ~= size( A22, 1 ) ),
                error( 'There seems to be an error in the CVX presolver routine.\nPlease report this to the authors; and if possible, include the\ncvx model and data that gave you this error.', 1 ); %#ok
            end
            [ ii, jj, vv ] = find( A22 );
            A22i  = sparse( jj, ii, 1.0 ./ vv );
            temp  = - A22i * A21;
            P     = P( :, colX ) + P( :, cols ) * temp;
            temp  = - A12 * A22i;
            Q     = Q( :, rowX ) + Q( :, rows ) * temp';
            dbCA  = A11 + temp * A21;
            rsv   =   rsv( rowX, : );
            nng   =   nng( rowX, : );
            ndxs  =  ndxs( rowX, : );
            ineqs = ineqs( :, colX );

        end

        if ~success,
            break;
        end

        cc   = dbCA( :, 1 );
        rcnt = sum( dbCA ~= 0, 2 );

    end

    if pass == 2 || isempty(cones) || ~can_dual,
        break;
    end

    %
    % Check to see if dualization will result in smaller problem
    %
    ineqs(1) = 0; rsv(1) = 0;
    n_save = nnz(cvx_eliminate_mex(dbCA,1,rsv,zeros(size(ineqs))));
    % n_save = nnz(sum(dbCA(:,ineqs~=0)~=0,1)==1+(dbCA(1,ineqs~=0)~=0));
    n_ineq = nnz(any(dbCA(rsv&rcnt==(cc~=0)+1,:)));
    rsv(1) = 1; ineqs(1) = 1; %#ok
    [n1,m1] = size(dbCA);
    m_pri = m1 - n_save - 1;
    n_pri = n1 - n_save - 1;
    m_dua = n1 - n_ineq - 1;
    n_dua = nnz( rsv ) + m1 - n_ineq - 1;
    if ( ( m_pri > n_pri ) || ( m_pri * m_pri * n_pri > m_dua * m_dua * n_dua ) ) && ( m_dua <= n_dua ),
        ndxs = full(sparse(ndxs,1,1:n1));
        PP = cell(2,length(cones));
        n_cur = m1;
        for k = 1 : length(cones),
            temp = cones(k).indices;
            [nn,nv] = size(temp);
            temp = reshape(ndxs(temp),size(temp));
            switch cones(k).type,
                case 'semidefinite',
                    nt = 0.5*(sqrt(8*nn+1)-1);
                    SS = 'symmetric';
                case 'hermitian-semidefinite',
                    nt = sqrt(nn);
                    SS = 'hermitian';
                case 'exponential',
                    SS = sparse(inv([0,-1,0;-1,0,0;0,0,exp(1)]));
                    SS = cvx_replicate_structure(SS,nv);
                otherwise,
                    SS = [];
            end
            PP{k} = sparse(1:numel(temp),max(temp,1),temp~=0,numel(temp),n1);
            if ~isempty(SS),
                if ischar(SS),
                    SS = cvx_create_structure([nt,nt,nv],SS);
                    SS = SS * SS';
                end
                PP{k} = SS * PP{k};
            end
            cones(k).indices = reshape(n_cur+1:n_cur+nn*nv,nn,nv);
            n_cur = cones(k).indices(end);
        end
        dbCA  = vertcat(dbCA',PP{:});
        dir   = -dir;
        tmp   = Q; Q = P; P = tmp;
        nold  = size(dbCA,1);
        Q(:,nold) = 0;
        dualized = true;
    end

end

%
% Return the objective back to normal.
%

if dualized,
    P = -P;
    P(:,1) = -P(:,1);
else
    dbCA(:,1) = -dbCA(:,1);
end

%
% Move the cone indices to their new locations
%

ndxs = full( sparse( ndxs, 1, 1 : length( ndxs ), nold, 1 ) );
tt = zeros(1,length(cones));
for k = 1 : length( cones ),
    temp = ndxs(cones(k).indices);
    if all(temp),
        temp = reshape( temp, size(cones(k).indices) );
    else
        temp = nonzeros(temp);
        temp = reshape( temp, 1, length(temp) );
    end
    tt(k) = isempty(temp);
    cones(k).indices = temp;
end
if any(tt),
    cones(tt~=0) = [];
end

% Copyright 2005-2016 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


clear all;close all;clc

M_dir = GetFishDirectories();

varList = {'CR_dtr','nCells','CInfo','anat_yx','anat_yz','anat_zx','ave_stack','fpsec','frame_turn'};

i_fish = 10;

%% load data
disp(['load fish ' num2str(i_fish)]);
tic
datadir = M_dir{i_fish};
load(fullfile(datadir,['Fish' num2str(i_fish) '_direct_load_nodiscard.mat']),varList{:});
toc

CR_dtr_full = CR_dtr;
CInfo_full = CInfo;
%% Test: discard 50% noisy cells based on std of zscore of baseline

% Compute std of dimest 10% of frames for each cell.
tic
prc = prctile(CR_dtr_full,10,2);
nCells = size(CR_dtr_full,1);
STD_full = zeros(nCells,1);
for i = 1:nCells,
    ix = find(CR_dtr_full(i,:)<prc(i));
    STD_full(i) = std(CR_dtr_full(i,ix));
end
toc
beep

%% Set threshold at 50%, i.e. discard 50% of all cells
perc_keep = 50;
thr = prctile(STD_full,perc_keep);

temp = find(STD_full>thr);
[~,I] = sort(STD_full(temp));
cIX = temp(I);

%% visualize cells to discard
% gIX = (round((1:length(cIX))/1000)+1)'; % option: view sorted index
gIX = round(cIX/1000)+1; % option: view anatomical z index
M = CR_dtr_full(cIX,1:1000);
[~,stim] = StimulusKeyPreprocessing(frame_turn,i_fish);
BasicPlotMaps(cIX,gIX,M,CInfo,stim,anat_yx,anat_yz,anat_zx);

% I_v: index of valid cells
I_v_Holder = ones(1,nCells);
I_v_Holder(cIX) = 0;
I_v = find(I_v_Holder);

CR_dtr = CR_dtr_full(I_v,:);
STD = STD_full(I_v);
nCells = length(I_v);
CInfo = CInfo_full(I_v);

%% visualize valid cells, sorted by noise
[~,I] = sort(STD);
cIX = I;
gIX = (round((1:length(cIX))/1000)+1)'; % sorted index
M = CR_dtr(I,1:1000);
BasicPlotMaps(cIX,gIX,M,CInfo_thr,stim,anat_yx,anat_yz,anat_zx);

%% Save new 'directload.mat'
temp = fullfile(datadir,['Fish' num2str(i_fish) '_direct_load_thr50.mat']);
varList = {'CR_dtr','nCells','CInfo','anat_yx','anat_yz','anat_zx','ave_stack','fpsec','frame_turn','perc_keep'};
% NOTE! not saving CR_raw in non-'_nodiscard.mat' again!
% should keep '_nodiscard.mat' so that this thresholding can be re-done
save(temp,varList{:},'-v7.3');


% Copyright (C) 2017-2018 Titus Cieslewski, RPG, University of Zurich,
%   Switzerland
%   You can contact the author at <titus at ifi dot uzh dot ch>
% Copyright (C) 2017-2018 Siddharth Choudhary, College of Computing,
%   Georgia Institute of Technology, Atlanta, GA, USA
% Copyright (C) 2017-2018 Davide Scaramuzza, RPG, University of Zurich,
%   Switzerland
%
% This file is part of dslam_open.
%
% dslam_open is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% dslam_open is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with dslam_open. If not, see <http://www.gnu.org/licenses/>.

function decentr_state = readDecentrStateFromOptG2oFiles(...
    g2o_dir, decentr_state, suffix)
% Inspired by Luca Carlone's
% https://bitbucket.org/lucacarlone/pgo3d-duality-opencode/src/ebb6e1b8cebaad7f2aaf581b1d0c0bad737faebb/lib/readG2oDataset3D.m?at=master&fileviewer=file-view-default

nr_robots = numel(decentr_state);

for robot_i = 1:nr_robots
    file_id = fopen(...
        [g2o_dir '/' num2str(robot_i - 1) suffix '.g2o'], 'r');

    while true
        line = fgets(file_id);
        data = textscan(line, '%s %d64 %f %f %f %f %f %f %f');
        if ~strcmp(data{1}, 'VERTEX_SE3:QUAT')
            break
        end
        [robot_i_val, frame_i] = gtsamFrameIdToIndices(data{2});
        assert(robot_i_val == robot_i);

        x = data{3}; y = data{4}; z = data{5};
        qx = data{6}; qy = data{7}; qz = data{8}; qw = data{9};

        Sim_W_C = eye(4);
        Sim_W_C(1:3, 4) = [x y z]';
        q = [qw, qx qy, qz]';
        if(abs(norm(q)-1) > 1e-3)
            norm(q)
            error('Quaternion has not unit norm');
        else
            q = q/norm(q); % we normalize anyway
        end

        Sim_W_C(1:3, 1:3) = fixR(quat2rot(q));

        decentr_state{robot_i}.Sim_O_C{frame_i} = ...
            Sim_W_C;
    end
end

end


% User-Specified Parameters
% ===============================================================

% Conditions and stimuli
% ---------------------------------------------------------------
GA.conditions = [1];
GA.freqConditions = [.5];
GA.scanLength = 480;
GA.ISI = 2;
GA.TR = 2;

% Genetic algorithm parameters
% ---------------------------------------------------------------
nmodels = 1;
GA.cbalColinPowerWeights = [1 1 1 0];	% 1 = cbal, 2 = eff, 3 = hrf shape, 4 = freq
GA.numGenerations = 2;
GA.sizeGenerations = 20;
GA.maxTime = 600;						% max time to run in s, or Inf
GA.alph = 2.1;
GA.plotFlag = 0;

% Filtering, counterbalancing, and design tolerance
% ---------------------------------------------------------------
GA.lowerLimit = [];
GA.HPlength = [];
GA.LPsmooth = [];
GA.maxOrder = 1;
GA.NumStimthresh = [];
GA.maxCbalDevthresh = []; 	% counterbalancing deviation
GA.maxFreqDevthresh = [];

% Contrast setup
% ---------------------------------------------------------------
GA.contrasts = [1];
GA.contrastweights = [1];	% or predictor weights, if no contrasts

% Autocorrelation and special options
% ---------------------------------------------------------------
AutocorrelationFileName = 'myscannerxc';
GA.restlength = [];
GA.restevery = [];
GA.trans2switch = 0;
GA.trans2block = 0;
GA.dofirst = 0;
GA.nonlinthreshold = [];

eval(['load ' AutocorrelationFileName]);
GA.xc = myscannerxc;

% =============================================
% * vary by parameter - set this in this script
% * if running multiple models, nmodels > 1
% ---------------------------------------------

varyparam = 0;
fieldname = 'alph';		% or 'freqConditions(end), etc.
incrementby = 0.3;
incrementevery = 5;		% every n models
contrastsofinterest = 1;% contrasts or predictors, if no cons specified

% =============================================

if varyparam
	eval(['paramvalue = GA.' fieldname ';'])
	disp(' ');disp('* *********************************************************************')
	disp('*  You have selected to vary a parameter across models in ga_gui_script')
	disp(['*  GA.' fieldname ' starting at ' num2str(paramvalue)])
	disp('* *********************************************************************')
end

for nm = 1:nmodels

   eval(['diary model' num2str(nm) '.log'])
   disp(['Starting Model ' num2str(nm)])
    % freqConditions

   M = optimize_rand_search(GA);

   Models(:,nm) = M.stimlist;
   MM{nm} = M;
   save GAworkspace

   if varyparam
   	if isfield(M,'consebeta'),
		Fit(1,nm) = mean(M.consebeta(contrastsofinterest));
	else
		Fit(1,nm) = mean(M.sebeta(contrastsofinterest));
	end
   	eval(['Fit(2,nm) = GA.' fieldname ';'])
   	FC(nm,:) = GA.freqConditions;
   	ALPH(nm,:) = GA.alph;
	str = ['if mod(nm,' num2str(incrementevery) ') == 0,GA.' fieldname ' = GA.' fieldname '+' num2str(incrementby) ';,end']
   	eval(str);
	eval(['paramvalue = GA.' fieldname ';'])
	disp('* *********************************************************************')
	disp('*  Varying parameter option on.')
	disp(['*  GA.' fieldname ' is now ' num2str(paramvalue)])
	disp('* *********************************************************************')
   end

   eval(['save model' num2str(nm) '_' M.date ' M'])
   save GAworkspace
   diary off
   fprintf('\n')
end


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

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

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


function [params, names] = rbfKernExtractParam(kern)

% RBFKERNEXTRACTPARAM Extract parameters from the RBF kernel structure.
% FORMAT
% DESC Extract parameters from the radial basis function kernel
% structure into a vector of parameters for optimisation.
% ARG kern : the kernel structure containing the parameters to be
% extracted.
% RETURN param : vector of parameters extracted from the kernel. If
% the field 'transforms' is not empty in the kernel matrix, the
% parameters will be transformed before optimisation (for example
% positive only parameters could be logged before being returned).
%
% FORMAT
% DESC Extract parameters and parameter names from the radial basis
% function kernel structure.
% ARG kern : the kernel structure containing the parameters to be
% extracted.
% RETURN param : vector of parameters extracted from the kernel. If
% the field 'transforms' is not empty in the kernel matrix, the
% parameters will be transformed before optimisation (for example
% positive only parameters could be logged before being returned).
% RETURN names : cell array of strings giving names to the parameters.
%
% SEEALSO rbfKernParamInit, rbfKernExpandParam, kernExtractParam, scg, conjgrad
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005, 2006

% KERN

params = [kern.inverseWidth kern.variance];
if nargout > 1
  names={'inverse width', 'variance'};
end


% hh = plot_onsets(d1, [color], [miny], [scalef], [duration])
%
% plots non-zero elements of an indicator vector or a list of onset times as vertical lines
%
% d1 is indicator vector; uses entries as scaling for height
%
% optional inputs:
% color, e.g., 'r'
% miny, shifts bars up/down to miny
% scalef, scales height of bars
%
% e.g.,
% plot_onsets(d1, 'k', -.5, .4)
%
%

function hh = plot_onsets(d1, varargin)
    if ~any(d1(2:end) == 0)    % we have an onset list, get indicator
        n = round(max(d1));
        z = zeros(n, 1);
        z(round(d1)+1) = 1;   % 0 is start of 1st onset, 1 of 1st element
        d1 = z;             % convert to indicator
        %wh = d1;
    end

    % make a row vector
    if size(d1, 1) > size(d1, 2)
        d1 = d1';
    end

    % indicator
    wh = find(d1);

    color = 'k';
    miny = 0;
    scalef = 1;
    dur = 0;

    if length(varargin) > 0, color = varargin{1}; end
    if length(varargin) > 1, miny = varargin{2}; end
    if length(varargin) > 2, scalef = varargin{3}; end
    if length(varargin) > 3, dur = varargin{4}; end

    % adjust by -1 so first element is time 0
    if dur == 0
        % events
        hh = plot([wh; wh]-1, [zeros(size(wh))+miny; miny+scalef.*d1(wh)], color);
    else
        % epochs
        hh = [];
        for i = 1:length(wh)
            hh(end+1) = drawbox(wh(i)-1, dur, miny, scalef.*d1(wh(i)), color);
        end
    end
end


function group = BF_ToGroup(groupIndices,maxLength)
% BF_ToGroup    convert from cell form to vector form of group labels
%
% Converts to a vector of group labels from a cell, where each element is a
% vector of indices for that group (or vice-versa).

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function is free software: you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation, either version 3 of the License, or (at your option) any later
% version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
% details.
%
% You should have received a copy of the GNU General Public License along with
% this program. If not, see <http://www.gnu.org/licenses/>.
% ------------------------------------------------------------------------------

if iscell(groupIndices) % Convert to vector of group indices

    if all(cellfun(@isempty,groupIndices)) % all empty -- rubbish
        error('Nothing in the group indices')
    end

    % Second input only important for cell -> vector transformation
    if nargin < 2 || isempty(maxLength)
        maxLength = max(cellfun(@max,groupIndices));
        % Make the length of output equal to the maximum index
    end

    group = zeros(maxLength,1);

    for i = 1:length(groupIndices)
        group(groupIndices{i}) = i;
    end

    if sum(cellfun(@length,groupIndices)) < length(group)
        warning('Group is missing some labels')
    end

else
    % Convert from vector of categorical group labels to cell of group indices:
    if iscategorical(groupIndices)
        classLabels = categories(groupIndices);
        numGroups = length(classLabels);
        group = cell(numGroups,1);
        for i = 1:numGroups
            group{i} = find(groupIndices==classLabels{i});
        end
    else
        numGroups = max(groupIndices);
        group = cell(numGroups,1);
        for i = 1:numGroups
            group{i} = find(groupIndices==i);
        end
    end
end

end


%% plot movement of vehicle

close all;
clearvars -except SimRealState input debug logsout

%% User Input

%% data source

   % 1: from simulation
   % 2: from script which loads data for simulink replay
   % 3: from debug file logged on vehicle; raw import with mat load

    i_datasource = 1;

% visualization settings

    % timestep of simulation in seconds
    sim_timestep = 0.004;       % in s

    % use every i-ith data point to reduce computational load
    i = 50;

    % timestep where to start visualization in seconds
    t_start = 0;

    % set replay speed: if > 1, replay is faster than reality and vice versa
    replay_speed_factor = 2;

% vehicle parameters
    length_front_m = 2;
    length_total_m = 4.5;

    width_m = 2;

%% convert data from different sources

if i_datasource == 1
    % from simulation
    dummy = logsout{1}.Values;
    VehicleData.x_m = dummy.x_m;
    VehicleData.y_m = dummy.y_m;
    VehicleData.psi_rad = dummy.psi_rad;

    VehicleData.vx_mps = dummy.vx_mps;
    VehicleData.vy_mps = dummy.vy_mps;

    VehicleData.ax_mps2 = dummy.ax_mps2;
    VehicleData.ay_mps2 = dummy.ay_mps2;

elseif i_datasource == 2

    % from script which loads data for simulink replay
    VehicleData = input{2};
    VehicleData.x_m = SimRealState.Pos.x_m;
    VehicleData.y_m = SimRealState.Pos.y_m;
    VehicleData.psi_rad = SimRealState.Pos.psi_rad;

elseif i_datasource == 2

    % from debug file logged on vehicle; raw import with mat load
    VehicleData.x_m = debug.debug_mvdc_state_estimation_debug_StateEstimate_Pos_x_m;
    VehicleData.y_m = debug.debug_mvdc_state_estimation_debug_StateEstimate_Pos_y_m;
    VehicleData.psi_rad = debug.debug_mvdc_state_estimation_debug_StateEstimate_Pos_psi_rad;

    VehicleData.vx_mps = debug.debug_mvdc_state_estimation_debug_StateEstimate_vx_mps;
    VehicleData.vy_mps = debug.debug_mvdc_state_estimation_debug_StateEstimate_vy_mps;

    VehicleData.ax_mps2 = debug.debug_mvdc_state_estimation_debug_StateEstimate_ax_mps2;
    VehicleData.ay_mps2 = debug.debug_mvdc_state_estimation_debug_StateEstimate_ay_mps2;

end

%% get recorded data and transform for use in visualization

    time = VehicleData.x_m.Time((t_start/sim_timestep)+1:i:end);

    x_m = VehicleData.x_m.Data((t_start/sim_timestep)+1:i:end);
    y_m = VehicleData.y_m.Data((t_start/sim_timestep)+1:i:end);

    psi_rad = VehicleData.psi_rad.Data((t_start/sim_timestep)+1:i:end);

    psi_rad = normalizeAngle(psi_rad) ;

    vx_mps = VehicleData.vx_mps.Data((t_start/sim_timestep)+1:i:end);
    vy_mps = VehicleData.vy_mps.Data((t_start/sim_timestep)+1:i:end);

    ax_mps2 = VehicleData.ax_mps2.Data((t_start/sim_timestep)+1:i:end);
    ay_mps2 = VehicleData.ay_mps2.Data((t_start/sim_timestep)+1:i:end);

    % calculate tangent to vehicle's CoG path
    diff_x_m = diff(x_m);
    diff_y_m = diff(y_m);

    heading_traj_rad = normalizeAngle(atan2(diff_y_m,diff_x_m)-pi/2);

%% main plotting

fig1 = figure;

% subplot 1 (top left)
    sp1 = subplot(3,2,1);
    title('vehicle velocity')
    grid on

  	xlabel('time in s')

    % set color of both y-axes to specific values matching the plot color
    ax_sp1 = gca;
  	yyaxis left
 	ax_sp1.YColor = [0, 0.4470, 0.7410];

    h1 = animatedline('Color',	[0, 0.4470, 0.7410]);
    ylabel('x-velocity in m/s')

    yyaxis right
    ax_sp1.YColor = [0.8500, 0.3250, 0.0980];

    h11 = animatedline('Color',	[0.8500, 0.3250, 0.0980]);
    ylabel('y-velocity in m/s')

% subplot 3 (middle left)
    sp3 = subplot(3,2,3);
    title('vehicle acceleration')
  	grid on

    h31 = animatedline('Color',	[0, 0.4470, 0.7410]);
    h32 = animatedline('Color',	[0.8500, 0.3250, 0.0980]);

    xlabel('time in s')
    ylabel('acceleration in m/s^2')

    legend('ax','ay')

% subplot 5 (bottom left)
    sp5 = subplot(3,2,5);
    title('vehicle location - global')

    xlabel('x-coordinate in m')
    ylabel('y-coordinate in m')

    xlim([min(x_m)-10,max(x_m)+10])
    ylim([min(y_m)-10,max(y_m)+10])

    hold on
    axis equal

    h5 = animatedline;

    plot(x_m(1),y_m(1),'+')

% subplot right side (subplot no. 2,4,6)
    sp246 = subplot(3,2,[2 4 6]);
    title('vehicle location - local')

    xlabel('x-coordinate in m')
    ylabel('y-coordinate in m')

    xlim([x_m(1)-50,x_m(1)+50])
    ylim([y_m(1)-50,y_m(1)+50])
    hold on
    axis equal

    h246 = animatedline;

    plot(x_m(1),y_m(1),'+')

    R = [cos(psi_rad) -sin(psi_rad) ;sin(psi_rad) cos(psi_rad)] ;

    g_veh = hgtransform;
    x = [-width_m/2 0 width_m/2 ];
    y = [length_front_m-length_total_m length_front_m length_front_m-length_total_m];
    vehicle_arrow = patch(x,y,'red');
    set(vehicle_arrow,'Parent',g_veh)

    g_head = hgtransform;
    heading_line = patch([0 0 0],[0 20 NaN],[0 0 0],'EdgeColor','red');
    set(heading_line,'Parent',g_head)

    g_traj_tangent = hgtransform;
    tangent_traj = patch([0 0 0],[0 20 NaN],[0 0 0],'EdgeColor','black');
    set(tangent_traj,'Parent',g_traj_tangent)

    h = zeros(2, 1);
    h(1) = plot(0,0,'r');
    h(2) = plot(0,0,'k');
    legend([h(1) h(2)], {'vehicle heading',"tangent to vehicle's CoG path"});

% for loop which updates every subplot
for timestep=1:size(time,1)-1

   	trans = makehgtform('translate',[x_m(timestep),y_m(timestep),0]);
    rotz = makehgtform('zrotate',psi_rad(timestep));

  	rotz_tangent = makehgtform('zrotate',heading_traj_rad(timestep));

    % update subplot 1
    sp1;

  	addpoints(h1,time(timestep), vx_mps(timestep));
  	addpoints(h11,time(timestep), vy_mps(timestep));

    % update subplot 3
    sp3;

  	addpoints(h31,time(timestep), ax_mps2(timestep));
  	addpoints(h32,time(timestep), ay_mps2(timestep));

    % update subplot 5
    sp5;

    addpoints(h5,x_m(timestep), y_m(timestep));

    % update subplot 246
  	sp246;

    xlim([x_m(timestep)-50,x_m(timestep)+50])
    ylim([y_m(timestep)-50,y_m(timestep)+50])

    set(g_veh,'Matrix',trans*rotz)
    set(g_head,'Matrix',trans*rotz)

    set(g_traj_tangent,'Matrix',trans*rotz_tangent)

    addpoints(h246,x_m(timestep), y_m(timestep));

    % update plot
    drawnow
    % pause to match specified visualization speed
    pause((sim_timestep*i)/replay_speed_factor)

end


function [sys,x0,str,ts] = ArticulatedVehicleSFunction(t,x,u,flag)
% This file is a s-function template for simulating the articulated vehicle
% model in Simulink.

% Choosing tire model
TireModel = VehicleDynamicsLateral.TirePacejka();
% Defining tire parameters
TireModel.a0        = 1;
TireModel.a1        = 2;
TireModel.a2        = 700;
TireModel.a3        = 5000;
TireModel.a4        = 80;
TireModel.a5        = 0;
TireModel.a6        = 0;
TireModel.a7        = 0.6;
TireModel.a8        = 0;
TireModel.a9        = 0;
TireModel.a10       = 0;
TireModel.a11       = 0;
TireModel.a12       = 0;
TireModel.a13       = 0;

% Choosing vehicle model
VehicleModel = VehicleDynamicsLateral.VehicleArticulatedNonlinear();
% Defining vehicle parameters
VehicleModel.mF0    = 5200;
VehicleModel.mR0    = 2400;
VehicleModel.mF     = 6000;
VehicleModel.mR     = 10000;
VehicleModel.mM     = 17000;
VehicleModel.IT     = 46000;
VehicleModel.IS     = 450000;
VehicleModel.lT     = 3.5;
VehicleModel.lS     = 7.7;
VehicleModel.c      = -0.3;
VehicleModel.nF     = 2;
VehicleModel.nR     = 4;
VehicleModel.nM     = 8;
VehicleModel.wT     = 2.6;
VehicleModel.wS     = 2.4;
VehicleModel.muy    = 0.8;
VehicleModel.deltaf = 0;
VehicleModel.Fxf    = 0;
VehicleModel.Fxr    = 0;
VehicleModel.Fxm    = 0;
VehicleModel.tire   = TireModel;

switch flag

  %%%%%%%%%%%%%%%%%%
  % Initialization %
  %%%%%%%%%%%%%%%%%%
  case 0
    [sys,x0,str,ts]=mdlInitializeSizes();

  %%%%%%%%%%%%%%%
  % Derivatives %
  %%%%%%%%%%%%%%%
  case 1,
    sys=mdlDerivatives(t,x,u,VehicleModel);

  %%%%%%%%%%%
  % Outputs %
  %%%%%%%%%%%
  case 3
    sys=mdlOutputs(t,x,u,VehicleModel);

  %%%%%%%%%%%%%%%%%%%
  % Unhandled flags %
  %%%%%%%%%%%%%%%%%%%
  case { 2, 4, 9 }
    sys = [];

  %%%%%%%%%%%%%%%%%
  % Vehicle model %
  %%%%%%%%%%%%%%%%%
  % Case 5 returns the vehicle model.
  case 5
    sys = VehicleModel;
    x0  =1; % Dummy
    str =1; % Dummy
    ts  =1; % Dummy

  %%%%%%%%%%%%%%%%%%%%
  % Unexpected flags %
  %%%%%%%%%%%%%%%%%%%%
  otherwise
    DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag));

end
% end csfunc

%
%=============================================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%=============================================================================
%

function [sys,x0,str,ts]=mdlInitializeSizes()

% Definitions
sizes = simsizes;
sizes.NumContStates  = 8;
sizes.NumDiscStates  = 0;
sizes.NumOutputs     = 8;
sizes.NumInputs      = 6;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;

sys = simsizes(sizes);

% Setting initial conditions
VEL0 = 20; % Initial velocity [m/s]

x0  = [0 0 0 0 VEL0 0 0 0];
str = [];
ts  = [0 0];

% end mdlInitializeSizes
%
%=============================================================================
% mdlDerivatives
% Return the derivatives for the continuous states.
%=============================================================================
%

function sys = mdlDerivatives(t,x,u,vehicle)

% Defining input
vehicle.deltaf      = u(1);
vehicle.deltar      = u(2);
vehicle.deltam      = u(3);
vehicle.Fxf         = u(4);
vehicle.Fxr         = u(5);
vehicle.Fxm         = u(6);

% Getting the vehicle model function (state equations)
ModelFunction       = @vehicle.Model;
% Getting the mass matrix function of the model
MassMatrixFunction  = @vehicle.MassMatrix;

% The derivatives are calculated with the inverse of the MassMatrix
sys = MassMatrixFunction(t,x)\ModelFunction(t,x,0);

% end mdlDerivatives
%
%=============================================================================
% mdlOutputs
% Return the block outputs.
%=============================================================================
%

function sys = mdlOutputs(~,x,~,~)

% Output all state variables
sys = x;

% end mdlOutputs


function [img_r1,img_r5,img_r10,img_med, img_map, txt_r1,txt_r5,txt_r10,txt_med,txt_map] = evaluate( ff1, ff2 ,img_id, txt_id)

parfor i = 1:size(ff1,1)
    %disp(i);
    tmp = ff1(i,:);
    score = tmp*(ff2)';
    [s, index] = sort(score, 'descend');
    good_index = find(txt_id==img_id(i));
    junk_index = [];
    [ap(i), CMC(i, :)] = compute_AP_rerank(good_index, junk_index, index);
end
CMC = mean(CMC);
rank = find(CMC>0.5);
img_r1 = CMC(1); img_map = mean(ap); img_med=rank(1);
img_r5 = CMC(5); img_r10 = CMC(10);

ap = [];
CMC = [];
% txt query
parfor i = 1:size(ff2,1)
    %disp(i);
    tmp = ff2(i,:);
    score = tmp*(ff1)';
    [s, index] = sort(score, 'descend');
    good_index = find(img_id==txt_id(i));
    %query_title = test_caption(i);
    junk_index = [];
    [ap(i), CMC(i, :)] = compute_AP_rerank(good_index, junk_index, index);
end
CMC = mean(CMC);
rank = find(CMC>0.5);
txt_r1 = CMC(1); txt_map = mean(ap); txt_med=rank(1);
txt_r5 = CMC(5); txt_r10 = CMC(10);

end


function [windows]=mvg_makeSuperpixelsWindows(superPix,maxNumConnectedSuperpix)

%% Default settings
if nargin<2
    maxNumConnectedSuperpix=3; % Defaults to triplets
end

%% Make sure superpxels are in labeled format
if size(superPix,3)>1
    superPix=mvg_numerizeLabels(superPix);
end
superPix=double(superPix);

%% Make single superpixel windows
superpixLabels=unique(superPix(:));
windows=zeros(length(superpixLabels),4);
for i=1:length(superpixLabels)
    [rw,cl]=find(superPix==superpixLabels(i));
    windows(i,:)=[min(cl),min(rw),max(cl),max(rw)];
end

%% Make tuplets if needed
if maxNumConnectedSuperpix>1

    %% Find neighboring superpixels (4-neighbors)
    neighborLabels=zeros(size(superPix,1),size(superPix,2),4);
    neighborLabels(:,:,1)=[superPix(:,1),superPix(:,1:end-1)]; % left
    neighborLabels(:,:,2)=[superPix(:,2:end),superPix(:,end)]; % right
    neighborLabels(:,:,3)=[superPix(1,:);superPix(1:end-1,:)]; % top
    neighborLabels(:,:,4)=[superPix(2:end,:);superPix(end,:)]; % bottom

    %% Find superpixel border pixels
    borderPairs=[neighborLabels(:),repmat(superPix(:),[4,1])];
    superPixNeighbors=unique(sort(borderPairs,2),'rows');

    %% Remove single superpixels
    superPixNeighbors(superPixNeighbors(:,1)==superPixNeighbors(:,2),:)=[];

    %% Make bounding boxes for all pairs
    numWindowsTuplets=size(superPixNeighbors,1);
    windowsAdd=zeros(numWindowsTuplets,4);
    for i=1:numWindowsTuplets
        [rw,cl]=find(superPix==superPixNeighbors(i,1) | superPix==superPixNeighbors(i,2));
        windowsAdd(i,:)=[min(cl),min(rw),max(cl),max(rw)];
    end

    %% Remove multiple windows
    windowsAdd=unique(windowsAdd,'rows');

    %% Add windows to existing ones
    windows=[windows;windowsAdd];

end

%% Add more connected components if needed
if maxNumConnectedSuperpix>2
    % Make superpixel pair matrix
    superpixelPairsMatrix=zeros(length(superpixLabels),length(superpixLabels));
    superpixelPairsMatrix(superPixNeighbors(:,1)+(superPixNeighbors(:,2)-1)*length(superpixLabels))=1;
    superpixelPairsMatrix=superpixelPairsMatrix+superpixelPairsMatrix';

    % Initialize
    superpixComb=superPixNeighbors;

    % Continue to add windows
    for i=3:maxNumConnectedSuperpix
        % Extend combination by one
        superpixComb=extendSupepixComb_(superpixelPairsMatrix,superpixComb);

        % Make new windows
        windowsAdd=zeros(size(superpixComb,1),4);
        for j=1:size(superpixComb,1)
            [rw,cl]=find(ismember(superPix,superpixComb(j,:)));
            windowsAdd(j,:)=[min(cl),min(rw),max(cl),max(rw)];
        end

        % Remove multiple windows
        windowsAdd=unique(windowsAdd,'rows');

        % Add windows to existing ones
        windows=[windows;windowsAdd];

    end

end

%% Remove any dublicate windows
windows=unique(windows,'rows');

%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Additional functions %%
%%%%%%%%%%%%%%%%%%%%%%%%%%

%%% Add new superpixel combination to the existing ones %%%
function [superpixExtComb]=extendSupepixComb_(superpixelPairsMatrix,superpixComb)

%% Initialize
superpixExtComb=[];

%% Loop over existing combination and extend
for i=1:size(superpixComb,1)
    % Find connected superpixels
    connSuperpix=superpixelPairsMatrix(:,superpixComb(i,:));
    connSuperpix=find(sum(connSuperpix,2)>eps);
    connSuperpix=setdiff(connSuperpix,superpixComb(i,:));

    % Add connected components to
    if ~isempty(connSuperpix)
        superpixExtComb=[superpixExtComb; [repmat(superpixComb(i,:),[length(connSuperpix),1]), connSuperpix(:)]];
    end

end

%% Remove possible duplicates
superpixExtComb=unique(sort(superpixExtComb,2),'rows');

%%% Numerize superpixel labels %%%
function N = numerizeSuperpixLabels_(S)

% converts the segmentation image S from true-color
% to ordered integer labels
%

N = zeros(size(S,1),size(S,2),'uint16');
col2n = zeros(256,256,256);
totcol = 0;
for x = 1:size(S,2)
    for y = 1:size(S,1)
        p = reshape(S(y,x,:),1,3)+1;
        cix = col2n(p(1),p(2),p(3));
        if cix > 0
          N(y,x) = cix;
        else
          totcol = totcol+1;
          col2n(p(1),p(2),p(3)) = totcol;
          N(y,x) = totcol;
        end
    end
end

% MVG version (faster)
% S=double(S);
% temp=S(:,:,1)+S(:,:,2)*256+S(:,:,3)*256^2;
% lbl=unique(temp(:));
% %lblCode=[lbl,(1:length(lbl))'];
% N=zeros(size(S,1),size(S,2),'uint16');
% for i=1:length(lbl)
%     N(temp==lbl(i))=i;
% end


% circpatch() - draw a dashed arc between two points
%
% Usage:
%   >> [handles] = circpatch( X, Y, curv, color, linewidth, segments, offset );
%
% Inputs:
%   X          - abscissae of the points (2 abscices for the 2 points)
%   Y          - ordinates of the points (2 ordinates for the 2 points)
%   curv       - curvature (0 = no curvature, 1 = round, -1 = round in
%                the other direction)
%   color      - line color (default black)
%   linewidth  - line thickness (same as line 'linewidth' property, default:1)
%   segments   - number of segments composing each continuous part of the arc (default:50)
%   offset     - change the offset of the dash (range: [0,1]) (default:0)
%   middle     - put a small line at the middle of the arc
%
% Outputs:
%   handles    - handles of all the objects composing the arc

% arno@salk.edu, Arnaud Delorme, CNL / Salk Institute, 2001

% This program is free software; you can redistribute it and/or modify it.
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.

function h = circpatch( X, Y, circfactor, color, thickness, segments, offset, middle );

PADSTRIPE  = 0.8; % percentage
SIZESTRIPE = 0.2; % percentage

if nargin < 3
	error('Not enough arguments. Type help circpatch');
	return;
end;
if nargin < 4
	color = 'k';
end;
if nargin < 5
	thickness = 1;
end;
if nargin < 6
	segments = 50;
end;
if nargin < 7
	offset = 0;
end;
if nargin < 8
	middle = 0;
end;

% middle of X and Y
% -----------------
middleXY = [(X(1)+X(2))/2 (Y(1)+Y(2))/2];

% normal to this line
% -------------------
diffXY = [ (X(2)-X(1))/2 (Y(2)-Y(1))/2];
normdiffXY = [diffXY(2) -diffXY(1)];

% third coordinate
% ----------------
fisrtPoint  = [ X(1) Y(1) ];
secondPoint = [ X(2) Y(2) ];
thirdPoint  = middleXY + normdiffXY*circfactor;

% compute the two lines equation
% ------------------------------
a1 = normdiffXY(2)/normdiffXY(1);
b1 = thirdPoint(2)-thirdPoint(1)*a1;

middle13 = [(fisrtPoint(1)+thirdPoint(1))/2 (fisrtPoint(2)+thirdPoint(2))/2];
a2 = [1 (fisrtPoint(2)-thirdPoint(2))/(fisrtPoint(1)-thirdPoint(1))];
a2 = [a2(2) -a2(1)]; % take the normal
a2 = a2(2)/a2(1);
b2 = middle13(2)-middle13(1)*a2;

% solve the equation (the center of the circle is the solution)
% -------------------------------------------------------------
centreX = (b1-b2)/(a2-a1);
centreY = a1*centreX + b1;
centreY = a2*centreX + b2;
radius  = sqrt((fisrtPoint(1)-centreX)^2+(fisrtPoint(2)-centreY)^2);

%circle( fisrtPoint(1), fisrtPoint(2), 0.05);
%circle( secondPoint(1), secondPoint(2), 0.05);
%circle( thirdPoint(1), thirdPoint(2), 0.05);

% compute the angle of the points
% -------------------------------
angle1 = atan( (fisrtPoint(2)-centreY)/(fisrtPoint(1)-centreX) )/pi*180;
angle2 = atan( (secondPoint(2)-centreY)/(secondPoint(1)-centreX) )/pi*180;
if ((fisrtPoint(1)-centreX) < 0) % negative cosinus
	angle1 = 180+angle1;
end;
if ((secondPoint(1)-centreX) < 0)
	angle2 = angle2+180;
end;
if angle1 < 0 angle1 = angle1+360; end;
if angle2 < 0 angle2 = angle2+360; end;

if circfactor<0 % which side
	tmp = angle1;
	angle1 = angle2;
	angle2 = tmp;
end;
if angle2 < angle1 angle2 = angle2+360; end;

% if offset exist, drawstripes
% ----------------------------
if offset ~= -1
	offsetshifted = mod(0.5 + offset + SIZESTRIPE/2,1);
	vectpad    = PADSTRIPE*abs(angle2-angle1);
	vectstripe = SIZESTRIPE*abs(angle2-angle1);
	nblines    = 1/(PADSTRIPE+SIZESTRIPE);
	currentangle = angle1;
	cont =1;
	h = [];
	firstpass = 1;
	while cont
		if firstpass == 1
			if offsetshifted/nblines < SIZESTRIPE
				currentangle = currentangle + vectstripe*(offsetshifted/nblines)/SIZESTRIPE;
				targetangle = currentangle + vectpad;
			else
				targetangle = currentangle + vectstripe*(offsetshifted/nblines-SIZESTRIPE)/SIZESTRIPE; %consider the offset from 0 to 1
			end;
			firstpass = 0;
		else
			targetangle  = currentangle + vectpad;
		end;
		if targetangle >= angle2
			targetangle = angle2;
			cont = 0;
		end;
		hh = circle( centreX, centreY, radius, 0, color, currentangle, targetangle, 0, thickness, ceil((targetangle-currentangle)/4*radius) );
		h = [h hh];

		currentangle = targetangle+vectstripe;
		if currentangle >= angle2
			cont = 0;
		end;
	end;
else
	h = circle( centreX, centreY, radius, 0, color, angle1, angle2, 0, thickness, segments );
end;

if middle
	x = centreX + cos((angle1+angle2)/2/180*pi)*radius;
	y = centreY + sin((angle1+angle2)/2/180*pi)*radius;
	hold on; h = plot( x, y, '*k', 'markersize', thickness/2);
end;

return;

clf;
for i=9:31
	fprintf('******************************* %d \n', mod(i/10, 1));
	circpatch( [ 0+i 1+i ] , [ 10 20 ], 0.5, 'b', 5, 10, mod(i/10, 1),1);
end;


%FEATSELB Trainable mapping for backward feature selection
%
%   [W,R] = FEATSELB(A,CRIT,K,T)
%   [W,R] = A*FEATSELB([],CRIT,K,T)
%   [W,R] = A*FEATSELB(CRIT,K,T)
%   [W,R] = FEATSELB(A,CRIT,K,N)
%   [W,R] = A*FEATSELB([],CRIT,K,N)
%   [W,R] = A*FEATSELB(CRIT,K,N)
%
% INPUT
%   A     Dataset
%   CRIT  String name of the criterion or untrained mapping
%         (optional; default: 'NN', i.e. 1-Nearest Neighbor error)
%   K     Number of features to select
%         (optional; default: return optimally ordered set of all features)
%   T     Tuning set (optional)
%   N     Number of cross-validations
%
% OUTPUT
%   W     Output feature selection mapping
%   R     Matrix with step-by-step results of the selection
%
% DESCRIPTION
% Backward selection of K features using the dataset A. CRIT sets the
% criterion used by the feature evaluation routine FEATEVAL. If the
% dataset T is given, it is used as test set for FEATEVAL. Alternatvely a
% a number of cross-validation N may be supplied. For K = 0, the optimal
% feature set (corresponding to the maximum value of FEATEVAL) is returned.
% The result W can be used for selecting features by B*W. In this case,
% features are ranked optimally.
% The selected features are stored in W.DATA and can be found by +W.
% In R, the search is reported step by step as:
%
% 	R(:,1) : number of features
% 	R(:,2) : criterion value
% 	R(:,3) : added / deleted feature
%
% SEE ALSO (PRTools Guide)
% MAPPINGS, DATASETS, FEATEVAL, FEATSELLR, FEATSEL,
% FEATSELO, FEATSELF, FEATSELI, FEATSELP, FEATSELM

% Copyright: R.P.W. Duin, r.p.w.duin@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

% $Id: featselb.m,v 1.6 2008/07/03 09:08:43 duin Exp $

function [w,r] = featselb(varargin)

  varargin = shiftargin(varargin,{'char','prmapping'});
  argin = setdefaults(varargin,[],'NN',0,[],[]);
  if mapping_task(argin,'definition')
    w = define_mapping(argin,'untrained','Backward FeatSel');
    return
  end

  [a,crit,ksel,t,fid] = deal(argin{:});
	[w,r] = featsellr(a,crit,ksel,0,1,t,fid);
  %DXD This is a patch: when the number of features has to be
  %optimized, and all features seem useful, when the list of
  %features is not reshuffled to reflect the relative importance of
  %the features:
  % (Obviously, this should be fixed in featsellr, but I don't
  % understand what is happening in there)
  dim = size(a,2);
  if (ksel==0) & (length(getdata(w))==dim)
    rr = -r(:,3); rr(1) = [];
    rr = [setdiff((1:dim)',rr) rr(end:-1:1)'];
    w = setdata(w,rr);
  end

return


function linpack_bench_backslash ( n, lda )

%*****************************************************************************80
%
%% LINPACK_BENCH_BACKSLASH drives the "backslash" LINPACK benchmark program.
%
%  Discussion:
%
%    This version of the program uses the MATLAB "backslash" operator
%    to factor and solve the linear system.
%
%    The standard LINPACK benchmark uses N = 1000, LDA = N + 1.
%
%  Modified:
%
%    22 May 2008
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.  If no value is supplied,
%    the default value of 1000 is used.
%
%    Input, integer LDA, the leading dimension of the matrix.
%    If no value is supplied, the default value of N+1 is used.
%
  timestamp ( );

  if ( nargin < 1 )
    n = 1000;
  end

  if ( nargin < 2 )
    lda = n + 1;
  end

  cray = 0.056;
  ops = ( 2 * n * n * n ) / 3.0 + 2.0 * n * n;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'LINPACK_BENCH_BACKSLASH\n' );
  fprintf ( 1, '  The LINPACK benchmark (using the MATLAB "backslash" operator).\n' );
  fprintf ( 1, '  Language: MATLAB\n' );
  fprintf ( 1, '  Datatype: Real Double Precision\n' );
  fprintf ( 1, '  Matrix order N        = %d\n', n );
  fprintf ( 1, '  Leading dimension LDA = %d\n', lda );

  a = r8_matgen ( n, n );

  a_max = max ( max ( a(1:n,1:n) ) );

  x_true(1:n,1) = 1.0;
  b(1:n,1) = a(1:n,1:n) * x_true(1:n,1);

  t1 = cputime;

  x = a \ b;

  t2 = cputime;
  time(1) = t2 - t1;
  time(2) = 0;

  total = time(1) + time(2);
%
%  Compute a residual to verify results.
%
  a = r8_matgen ( n, n );

  resid(1:n,1) = a(1:n,1:n) * x(1:n,1) - b(1:n,1);
  resid_max = max ( abs ( resid(1:n,1) ) );

  x_max = max ( abs ( x(1:n,1) ) );

  residn = resid_max /  ( n * a_max * x_max * eps );

  time(3) = total;
  time(4) = ops / ( 1000000 * total );
  time(5) = 2.0 / time(4);
  time(6) = total / cray;

  fprintf ( 1, '\n' );
  fprintf ( 1, '     Norm. Resid      Resid           MACHEP' );
  fprintf ( 1, '         X(1)          X(N)\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  %14e  %14e  %14e  %14f  %14f\n', ...
    residn, resid_max, eps, x(1,1), x(n,1) );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      Factor     Solve      Total     MFLOPS' );
  fprintf ( 1, '       Unit      Cray-Ratio\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  %9f  %9f  %9f  %9f  %9f  %9f\n', time(1:6) );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'LINPACK_BENCH_BACKSLASH\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function a = r8_matgen ( lda, n )

%*****************************************************************************80
%
%% R8_MATGEN generates a random matrix.
%
%  Modified:
%
%    08 June 2005
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Parameters:
%
%    Input, integer LDA, the leading dimension of the matrix.
%
%    Input, integer N, the order of the matrix.
%
%    Output, real A(LDA,N), the N by N matrix.
%
  a = zeros ( lda, n );

  init(1:4) = [ 1, 2, 3, 1325 ];

  for j = 1 : n
     for i = 1 : n
       [ a(i,j), init ] = r8_random ( init );
    end
  end

  a(1:n,1:n) = a(1:n,1:n) - 0.5;

  return
end
function [ value, seed ] = r8_random ( seed )

%*****************************************************************************80
%
%% R8_RANDOM returns a uniformly distributed random number between 0 and 1.
%
%  Discussion:
%
%    This routine uses a multiplicative congruential method with modulus
%    2**48 and multiplier 33952834046453.
%
%    48-bit integers are stored in 4 integer array elements with 12 bits
%    per element.  Hence the routine is portable across machines with
%    integers of 32 bits or more.
%
%  Modified:
%
%    08 June 2005
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    George Fishman,
%    Multiplicative congruential random number generators with modulus 2**b:
%    an exhaustive analysis for b = 32 and a partial analysis for b = 48,
%    Mathematics of Computation,
%    Volume 189, 1990, pages 331-344.
%
%  Parameters:
%
%    Input, integer SEED(4), the seed of the random number generator; the array
%    elements must be between 0 and 4095, and SEED(4) must be odd.
%
%    Output, real VALUE, the next pseudorandom number.
%
%    Output, integer SEED(4), the updated seed.
%
  ipw2 = 4096;
  m1 = 494;
  m2 = 322;
  m3 = 2508;
  m4 = 2549;
  one = 1.0;
  r = 1.0 / 4096.0;
%
%  Multiply the seed by the multiplier modulo 2**48.
%
  it4 = seed(4) * m4;
  it3 = floor ( it4 / ipw2 );
  it4 = it4 - ipw2 * it3;
  it3 = it3 + seed(3) * m4 + seed(4) * m3;
  it2 = floor ( it3 / ipw2 );
  it3 = it3 - ipw2 * it2;
  it2 = it2 + seed(2) * m4 + seed(3) * m3 + seed(4) * m2;
  it1 = floor ( it2 / ipw2 );
  it2 = it2 - ipw2 * it1;
  it1 = it1 + seed(1) * m4 + seed(2) * m3 + seed(3) * m2 + seed(4) * m1;
  it1 = mod ( it1, ipw2 );
%
%  Return updated seed
%
  seed(1) = it1;
  seed(2) = it2;
  seed(3) = it3;
  seed(4) = it4;
%
%  Convert 48-bit integer to a real number in the interval (0,1)
%
  value = ...
      r * ( it1  ...
    + r * ( it2  ...
    + r * ( it3  ...
    + r * ( it4 ) ) ) );

  return
end
function timestamp ( )

%*****************************************************************************80
%
%% TIMESTAMP prints the current YMDHMS date as a timestamp.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  t = now;
  c = datevec ( t );
  s = datestr ( c, 0 );
  fprintf ( 1, '%s\n', s );

  return
end


filename='RVE_Square_Triangle';
ptype = 'MICRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'circleInclusion';
cost={'enforceCh_CCstar_L2','perimeter'};%enforceCh_CCstar_L2
weights=[1, 0.1];
constraint = {'volumeConstraint'};
optimizer = 'MMA'; incrementFactor = 1;%mma CON 1E-4
filterType = 'P1';

nsteps = 1;
Vfrac_final = 0.6;
Perimeter_target=3;
optimality_final =1e-4;
constr_final =1e-3;

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

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

%Micro
epsilon_isotropy_initial=1e-1;
epsilon_isotropy_final = 1e-3;
selectiveC_Cstar = 'Vfrac06';