function t_opf_dc_glpk(quiet)
%T_OPF_DC_GLPK  Tests for DC optimal power flow using GLPK solver.

%   MATPOWER
%   Copyright (c) 2004-2021, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

if nargin < 1
    quiet = 0;
end

algs  = [1; 1; 2];
alg_names = {
    'primal simplex',
    'dual simplex',
    'interior',
};
if have_feature('octave')
    if have_feature('octave', 'vnum') < 3.007
        dual = [0; 1; 1];
    else
        dual = [1; 2; 1];
    end
else
    dual = [0; 2; 1];
end
num_tests = 43 * length(algs);

t_begin(num_tests, quiet);

[PQ, PV, REF, NONE, BUS_I, BUS_TYPE, PD, QD, GS, BS, BUS_AREA, VM, ...
    VA, BASE_KV, ZONE, VMAX, VMIN, LAM_P, LAM_Q, MU_VMAX, MU_VMIN] = idx_bus;
[GEN_BUS, PG, QG, QMAX, QMIN, VG, MBASE, GEN_STATUS, PMAX, PMIN, ...
    MU_PMAX, MU_PMIN, MU_QMAX, MU_QMIN, PC1, PC2, QC1MIN, QC1MAX, ...
    QC2MIN, QC2MAX, RAMP_AGC, RAMP_10, RAMP_30, RAMP_Q, APF] = idx_gen;
[F_BUS, T_BUS, BR_R, BR_X, BR_B, RATE_A, RATE_B, RATE_C, ...
    TAP, SHIFT, BR_STATUS, PF, QF, PT, QT, MU_SF, MU_ST, ...
    ANGMIN, ANGMAX, MU_ANGMIN, MU_ANGMAX] = idx_brch;

casefile = 't_case9_opf';
if quiet
    verbose = 0;
else
    verbose = 0;
end
if have_feature('octave')
    if have_feature('octave', 'vnum') >= 4
        file_in_path_warn_id = 'Octave:data-file-in-path';
    else
        file_in_path_warn_id = 'Octave:load-file-in-path';
    end
    s1 = warning('query', file_in_path_warn_id);
    warning('off', file_in_path_warn_id);
end

mpopt = mpoption('out.all', 0, 'verbose', verbose);
mpopt = mpoption(mpopt, 'opf.dc.solver', 'GLPK');

%% run DC OPF
if have_feature('glpk')
  for k = 1:length(algs)
    mpopt = mpoption(mpopt, 'glpk.opts.lpsolver', algs(k), 'glpk.opts.dual', dual(k));
    if algs(k) == 2     %% interior point sometimes fails on GLPK 4.6x w/o this
        mpopt = mpoption(mpopt, 'glpk.opts.scale', 1);
    end
    t0 = sprintf('DC OPF (GLPK %s): ', alg_names{k});

    %% set up indices
    ib_data     = [1:BUS_AREA BASE_KV:VMIN];
    ib_voltage  = [VM VA];
    ib_lam      = [LAM_P LAM_Q];
    ib_mu       = [MU_VMAX MU_VMIN];
    ig_data     = [GEN_BUS QMAX QMIN MBASE:APF];
    ig_disp     = [PG QG VG];
    ig_mu       = (MU_PMAX:MU_QMIN);
    ibr_data    = (1:ANGMAX);
    ibr_flow    = (PF:QT);
    ibr_mu      = [MU_SF MU_ST];
    ibr_angmu   = [MU_ANGMIN MU_ANGMAX];

    %% get solved DC power flow case from MAT-file
    load soln9_dcopf;       %% defines bus_soln, gen_soln, branch_soln, f_soln

    %% run OPF
    t = t0;
    [baseMVA, bus, gen, gencost, branch, f, success, et] = rundcopf(casefile, mpopt);
    t_ok(success, [t 'success']);
    t_is(f, f_soln, 3, [t 'f']);
    t_is(   bus(:,ib_data   ),    bus_soln(:,ib_data   ), 10, [t 'bus data']);
    t_is(   bus(:,ib_voltage),    bus_soln(:,ib_voltage),  3, [t 'bus voltage']);
    t_is(   bus(:,ib_lam    ),    bus_soln(:,ib_lam    ),  3, [t 'bus lambda']);
    t_is(   bus(:,ib_mu     ),    bus_soln(:,ib_mu     ),  2, [t 'bus mu']);
    t_is(   gen(:,ig_data   ),    gen_soln(:,ig_data   ), 10, [t 'gen data']);
    t_is(   gen(:,ig_disp   ),    gen_soln(:,ig_disp   ),  3, [t 'gen dispatch']);
    t_is(   gen(:,ig_mu     ),    gen_soln(:,ig_mu     ),  3, [t 'gen mu']);
    t_is(branch(:,ibr_data  ), branch_soln(:,ibr_data  ), 10, [t 'branch data']);
    t_is(branch(:,ibr_flow  ), branch_soln(:,ibr_flow  ),  3, [t 'branch flow']);
    t_is(branch(:,ibr_mu    ), branch_soln(:,ibr_mu    ),  2, [t 'branch mu']);

    %%-----  test OPF with angle difference limits  -----
    t = [t0 'w/angle diff lims : '];
    mpc = loadcase(casefile);
    mpc.branch(4, ANGMAX) = 3;
    mpc.branch(7, ANGMIN) = -4.5;
    r = rundcopf(mpc, mpopt);
    [bus, gen, branch, f, success] = deal(r.bus, r.gen, r.branch, r.f, r.success);
    t_ok(success, [t 'success']);
    t_is(   f, 6456.7213, 3, [t 'f']);
    t_is(   bus(:,ib_data   ),    bus_soln(:,ib_data   ), 10, [t 'bus data']);
    t_is(   gen(:,ig_data   ),    gen_soln(:,ig_data   ), 10, [t 'gen data']);
    t_is(   gen(:,PG        ),    [99.98497;89.35133;125.66371], 4, [t 'gen dispatch']);
    t_is(branch(:,ibr_data  ), mpc.branch(:,ibr_data   ), 10, [t 'branch data']);
    e = zeros(size(branch, 1), 1);
    e(4) = 297.83776;
    e(7) = -26.94788;
    t_is(branch(:,MU_ANGMAX )-branch(:,MU_ANGMIN ), e, 4, [t 'branch ang diff mu']);

    t = [t0 'w/ignored angle diff lims : '];
    mpopt1 = mpoption(mpopt, 'opf.ignore_angle_lim', 1);
    r = rundcopf(mpc, mpopt1);
    [bus, gen, branch, f, success] = deal(r.bus, r.gen, r.branch, r.f, r.success);
    t_ok(success, [t 'success']);
    t_is(f, f_soln, 3, [t 'f']);
    t_is(   bus(:,ib_data   ),    bus_soln(:,ib_data   ), 10, [t 'bus data']);
    t_is(   bus(:,ib_voltage),    bus_soln(:,ib_voltage),  3, [t 'bus voltage']);
    t_is(   bus(:,ib_lam    ),    bus_soln(:,ib_lam    ),  3, [t 'bus lambda']);
    t_is(   bus(:,ib_mu     ),    bus_soln(:,ib_mu     ),  2, [t 'bus mu']);
    t_is(   gen(:,ig_data   ),    gen_soln(:,ig_data   ), 10, [t 'gen data']);
    t_is(   gen(:,ig_disp   ),    gen_soln(:,ig_disp   ),  3, [t 'gen dispatch']);
    t_is(   gen(:,ig_mu     ),    gen_soln(:,ig_mu     ),  3, [t 'gen mu']);
    t_is(branch(:,ibr_data  ), mpc.branch(:,ibr_data   ), 10, [t 'branch data']);
    t_is(branch(:,ibr_flow  ), branch_soln(:,ibr_flow  ),  3, [t 'branch flow']);
    t_is(branch(:,ibr_mu    ), branch_soln(:,ibr_mu    ),  2, [t 'branch mu']);

    %%-----  run OPF with extra linear user constraints & costs  -----
    %% two new z variables
    %%      0 <= z1, P3 - P1 <= z1
    %%      0 <= z2, P3 - P2 <= z2
    %% with A and N sized for DC opf
    mpc = loadcase(casefile);
    mpc.A = sparse([1;1;1;2;2;2],[10;12;13;12;11;14],[-1;1;-1;1;-1;-1],2,14);
    mpc.u = [0; 0];
    mpc.l = [-Inf; -Inf];
    mpc.zl = [0; 0];

    mpc.N = sparse([1;2], [13;14], [1;1], 2, 14);   %% new z variables only
    mpc.fparm = ones(2,1) * [1 0 0 1];              %% w = r = z
    mpc.H = sparse(2,2);                            %% no quadratic term
    mpc.Cw = [1000;1];

    t = [t0 'w/extra constraints & costs 1 : '];
    [r, success] = rundcopf(mpc, mpopt);
    t_ok(success, [t 'success']);
    t_is(r.gen(1, PG), 116.15974, 5, [t 'Pg1 = 116.15974']);
    t_is(r.gen(3, PG), 116.15974, 5, [t 'Pg3 = 116.15974']);
    t_is(r.var.val.z, [0; 0.3348], 4, [t 'user vars']);
    t_is(r.cost.usr, 0.3348, 4, [t 'user costs']);

    %% with A and N sized for AC opf
    mpc = loadcase(casefile);
    mpc.A = sparse([1;1;1;2;2;2],[19;21;25;21;20;26],[-1;1;-1;1;-1;-1],2,26);
    mpc.u = [0; 0];
    mpc.l = [-Inf; -Inf];
    mpc.zl = [0; 0];

    mpc.N = sparse([1;2], [25;26], [1;1], 2, 26);   %% new z variables only
    mpc.fparm = ones(2,1) * [1 0 0 1];              %% w = r = z
    mpc.H = sparse(2,2);                            %% no quadratic term
    mpc.Cw = [1000;1];

    t = [t0 'w/extra constraints & costs 2 : '];
    [r, success] = rundcopf(mpc, mpopt);
    t_ok(success, [t 'success']);
    t_is(r.gen(1, PG), 116.15974, 5, [t 'Pg1 = 116.15974']);
    t_is(r.gen(3, PG), 116.15974, 5, [t 'Pg3 = 116.15974']);
    t_is(r.var.val.z, [0; 0.3348], 4, [t 'user vars']);
    t_is(r.cost.usr, 0.3348, 4, [t 'user costs']);

    t = [t0 'infeasible : '];
    %% with A and N sized for DC opf
    mpc = loadcase(casefile);
    mpc.A = sparse([1;1], [10;11], [1;1], 1, 14);   %% Pg1 + Pg2
    mpc.u = Inf;
    mpc.l = 600;
    [r, success] = rundcopf(mpc, mpopt);
    t_ok(~success, [t 'no success']);

    %% OPF with all buses isolated
    t = [t0 'all buses isolated : '];
    mpc = loadcase(casefile);
    mpc.bus(:, BUS_TYPE) = NONE;
    try
        r = rundcopf(mpc, mpopt);
        t_is(r.success, 0, 12, [t 'success = 0']);
    catch
        t_ok(0, [t 'unexpected fatal error']);
    end
  end
else
    t_skip(num_tests, 'GLPK not available');
end

if have_feature('octave')
    warning(s1.state, file_in_path_warn_id);
end

t_end;


function [superpixels, clusters, Z] = doAmodalCompletion(imName, paths, ucm2, pc, amodalParam)
	ucmThresh = amodalParam.ucmThresh;
	thresh = amodalParam.thresh;

	superpixels = bwlabel(ucm2 < amodalParam.ucmThresh);
	superpixels = superpixels(2:2:end, 2:2:end);
	[clusters, Z] = amodalCompletion(superpixels, pc, thresh);

	fileName = fullfile(paths.amodalDir, strcat(imName, '.mat'));
	save(fileName, 'ucmThresh', 'superpixels', 'thresh', 'clusters', 'Z');
end


function test_bug921

% MEM 2gb
% WALLTIME 00:10:00
% DEPENDENCY ft_selectdata ft_selectdata_old ft_selectdata_new

% See also bug 798 that was reported by Yoni, from which I am reusing the data to test this bug

datadir = (dccnpath('/home/common/matlab/fieldtrip/data/test/bug798'));
load(fullfile(datadir,'t2_subj1'));
load(fullfile(datadir,'t2_subj1_null'));
load(fullfile(datadir,'t2_subj2'));
load(fullfile(datadir,'t2_subj2_null'));

argin{1} = ft_checkdata(t2_subj1, 'datatype', 'freq');
argin{2} = ft_checkdata(t2_subj1_null, 'datatype', 'freq');
argin{3} = ft_checkdata(t2_subj2, 'datatype', 'freq');
argin{4} = ft_checkdata(t2_subj2_null, 'datatype', 'freq');

% the following should select and concatenate only the powspctrm field
% but not the prob and stat fields
%data = ft_selectdata(argin{:}, 'param', 'powspctrm');

%assert(~isfield(data, 'stat'));
%assert(~isfield(data, 'prob'));
%assert(strcmp(data.dimord, 'rpt_chan_freq'));

data = cell(1,numel(argin));
[data{:}] = ft_selectdata([],argin{:});

cfg = [];
cfg.parameter = 'powspctrm';
cfg.appenddim = 'rpt';
data = ft_appendfreq(cfg, data{:});

assert(size(data.powspctrm,1)==4);
assert(size(data.powspctrm,2)==274);
assert(size(data.powspctrm,3)==1);


classdef UnilateralConstraint < handle
    % UnilateralConstraint represents a scalar or vector inequality
    % constraints should be imposed on the continuous dynamical systems
    %
    % @author ayonga @date 2017-04-20
    %
    % Copyright (c) 2016-2017, AMBER Lab
    % All right reserved.
    %
    % Redistribution and use in source and binary forms, with or without
    % modification, are permitted only in compliance with the BSD 3-Clause
    % license, see
    % http://www.opensource.org/licenses/bsd-license.php

    methods

        function obj = UnilateralConstraint(model, h, name, deps, varargin)
            % The class constructor function
            %
            % Parameters:
            % model: the dynamical system model in which the virtual
            % constraints are defined @type DynamicalSystem
            % h: the symbolic expression of the constraints
            % name: the name of the virtual constraints @type char
            % deps: the dependent variables (could be states or inputs) @type
            % cellstr
            % @type SymExpression
            % varargin: optional parameters. In details
            %  AuxData:
            %  ConstrLabel:

            if nargin == 0
                return;
            end

            % validate (model) argument
            validateattributes(model, {'DynamicalSystem'},...
                {'scalar'},...
                'UnilateralConstraint','model');

            % validates (name) argument
            validateName(obj, name);
            obj.Name = name;

            % validate (ya) argument
            validateattributes(h,{'SymExpression'},...
                {'nonempty','vector'},...
                'UnilateralConstraint','h');
            if isrow(h) % convert to column vector if it is a row vector
                h = vertcat(h(:));
            end
            dim = length(h);
            obj.Dimension = dim;

            % parse the input options
            args = struct(varargin{:});
            assert(isscalar(args),...
                'The values of optional properties are must be scalar data.');

            % validate and assign the label
            if isfield(args, 'ConstrLabel')
                obj.setConstrLabel(args.ConstrLabel);
            end

            if isfield(args, 'AuxData')
                auxdata = args.AuxData;
            else
                auxdata = [];
            end

            %% set the unilateral constraints
            if ~iscell(deps), deps = {deps}; end
            n_deps = length(deps);

            % get the variable group of each dependent variable
            var_group = cellfun(@(x)model.validateVarName(x), deps, 'UniformOutput', false);
            vars = cell(1,n_deps);

            % assume it is not input-dependent
            input_dep = false;
            for i=1:n_deps
                tmp = var_group{i};
                % check if it is input-dependent
                if strcmp(tmp{1},'Inputs')
                    input_dep = true;
                    vars{i} = model.(tmp{1}).(tmp{2}).(deps{i});
                else
                    vars{i} = model.(tmp{1}).(deps{i});
                end
            end
            obj.InputDependent = input_dep;

            if isa(h, 'SymFunction')
                % validate the dependent variables
                assert(length(h.Vars)==n_deps,...
                    ['The constraint SymFunction (h) must have the same',...
                    'number of dependent variables as specified in the argument (deps).']);
                for i=1:n_deps
                    assert(h.Vars{i} == vars{i},...
                        'The %d-th dependent variable must be the same as the variable %s.',i,deps{i});
                end
                obj.h_ = h;
                obj.DepLists = deps;

                if ~isempty(h.Params)
                    if isempty(auxdata)
                        error('The constraint function requires auxilary constant parameters.');
                    else
                        if ~iscell(auxdata), auxdata = {auxdata}; end
                        assert(length(h.Params) == length(auxdata),...
                            'The number of required auxilaray data (%d) and the provided auxilary data (%d) does not match.\n',...
                            length(h.Params),length(auxdata));
                        for i=1:numel(auxdata)
                            assert(numel(auxdata{i})== prod(size(h.Params{i})),...
                                'The length %d-th auxiliary parameters variable does not match the function definition.',i); %#ok<PSIZE>
                        end

                        obj.AuxData = auxdata;
                    end

                else
                    obj.AuxData = [];
                end

            elseif isa(h, 'SymExpression')
                % create a SymFunction object if the input is a SymExpression

                obj.h_ = SymFunction(['u_' name '_' model.Name], h, vars);
                obj.DepLists = deps;
                if ~isempty(auxdata)
                    error('If the constraint depends on auxilary constant parameters, please provide it as a SymFunction object directly.');
                end
                obj.AuxData = [];
            else
                error('The constraint expression must be given as an object of SymExpression or SymFunction.');
            end

        end

    end

    % properties determined internally
    properties (SetAccess=protected, GetAccess=public)
        % The dimension of the virtual constraints
        %
        % @type integer
        Dimension

        % An indicator whether the unilateral constraints depend on the
        % input variables of the dynamical system. If the constraints are
        % input-dependent, then it will requires to execute the
        % calcDynamics function to compute the required input signals.
        %
        % @type logical
        InputDependent
    end

    % properties must be determined by the users
    properties (SetAccess=protected, GetAccess=public)
        % The name of the virtual constraints
        %
        % @type char
        Name

        % The label of the holonomic constraint
        %
        % @type char
        ConstrLabel

        % The name list of dependent variables in the associated dynamical
        % system model
        %
        % @type cellstr
        DepLists

        % The list of auxiliary data to be used to call the function
        %
        % @type cell
        AuxData

    end

    properties (Dependent)
        % The holonomic constraint expression
        %
        % @type SymFunction
        ConstrExpr

    end

    methods
        function cstr = get.ConstrExpr(obj)
            cstr = obj.h_;
        end

    end

    properties (Access = protected)

        % The holonomic constraint expression
        %
        % @type SymFunction
        h_

    end

    methods
        function export(obj, export_path, varargin)
            % export the symbolic expressions of the constraints and
            % compile as MEX files.
            %
            % Parameters:
            %  export_path: the path to export the file @type char
            %  varargin: variable input parameters @type varargin
            %   Vars: a list of symbolic variables @type SymVariable
            %   File: the (full) file name of exported file @type char
            %   ForceExport: force the export @type logical
            %   BuildMex: flag whether to MEX the exported file @type logical
            %   Namespace: the namespace of the function @type char

            export(obj.h_,export_path, varargin{:});

        end

        function nlp = imposeNLPConstraint(obj, nlp)
            % impose holonomic objaints as NLP objaints in the trajectory
            % optimization problem 'nlp' of the dynamical system
            %
            %
            % Parameters:
            % nlp: the trajectory optimization NLP @type TrajectoryOptimization

            if ~isempty(obj.AuxData)
                nlp = addNodeConstraint(nlp, obj.h_, obj.DepLists, 'all',...
                    0, inf, 'Nonlinear', obj.AuxData);
            else
                nlp = addNodeConstraint(nlp, obj.h_, obj.DepLists, 'all',...
                    0, inf, 'Nonlinear');
            end

        end

        function f = calcConstraint(obj, varargin)
            % calculate the unilateral constraints
            %
            % Parameters:
            % varargin: input variable depends on the object
            %
            % Return values:
            % f: the value of the unilateral constraints
            if isempty(obj.AuxData)
                f = feval(obj.h_.Name, varargin{:});
            else
                f = feval(obj.h_.Name, varargin{:}, obj.AuxData{:});
            end
        end

    end

    % set functions
    methods

        % OutputLabel
        function obj = setConstrLabel(obj, label)
            % sets the naming labels of outputs
            %
            % Parameters:
            % label: the cell array of labels @type cellstr

            validateattributes(label,{'cell'},...
                {'nonempty','numel',obj.Dimension,'row'},...
                'UnilateralConstraint','ConstrLabel');
            cellfun( @(x) validateattributes(...
                x, {'char'},{}), label);

            obj.ConstrLabel = label;
        end

        % Name
        function name = validateName(~, name)
            validateattributes(name, {'char'},...
                {'nonempty','scalartext'},...
                'UnilateralConstraint','Name');

            assert(isempty(regexp(name, '\W', 'once')) || ~isempty(regexp(name, '\$', 'once')),...
                'UnilateralConstraint:invalidSymbol', ...
                'Invalid symbol string, can NOT contain special characters.');

            assert(isempty(regexp(name, '_', 'once')),...
                'UnilateralConstraint:invalidSymbol', ...
                'Invalid symbol string, can NOT contain ''_''.');

        end
    end
end


function test_bug2444

% MEM 1gb
% WALLTIME 00:10:00
% DEPENDENCY ft_selectdata ft_selectdata_new

%% make some data

comp = [];
comp.label = {'comp1', 'comp2'};
comp.topolabel = {'chan1', 'chan2'};
comp.time = {1:1000, 1:1000};
comp.trial = {randn(2,1000), randn(2,1000)};
comp.topo = eye(2);
comp.unmixing = eye(2);

%% select some data

cfg = [];
cfg.latency = [10 20];
selected = ft_selectdata(cfg, comp);

end


%

% @
% Copyright (C) 2017 Jonas Ruesch
%
% 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/>.
% @

load('../output/coefficients');

plotCoefficients(alpha, beta, CX, CY, CZ, Cl, Cm, Cn, CX_d, CY_d, CZ_d, Cl_d, Cm_d, Cn_d, ...
    CX_P, CY_P, CZ_P, Cl_P, Cm_P, Cn_P, CX_Q, CY_Q, CZ_Q, Cl_Q, Cm_Q, Cn_Q, CX_R, CY_R, CZ_R, Cl_R, Cm_R, Cn_R);


%Autocalibration.
%
%This add on allows to compute the hand to eye calibration based ont he
%calibration toolbox from Jean-Yves Bouguet
%see http://www.vision.caltech.edu/bouguetj/calib_doc/
%more information can be found here:
%http://www.vision.ee.ethz.ch/~cwengert/calibration_toolbox.php
%
%It performs the calibration 100% automatically, but uses a different
%pattern for calibration which can be found here:
%http://www.vision.ee.ethz.ch/~cwengert/calibration_toolbox.php
%
%See also "Fully Automatic Endoscope Calibration for Intraoperative Use,
%Wengert C., Reeff M., Cattin P., Szekely G."
%
%You can set up some variables to change the behavior of the code:
%doShow	[default=0]
%Set doShow = 1 if you want to see what the calibration is doing
%doRefineGrid [default = 0]
%Set doRefineGrid = 1 if the grid should be refined after detection
%(does not really change much)
%isStronglyDistorted [default = 0]
%Set isStronglyDistorted = 1, if your images show severe distortion, such
%as from an endoscope or extremely wide optics
%doIterateCalibration [default = 1]
%Set doIterateCalibration = 1, if you want the system to give the best
%results. This will try to detect wrong correspondences and calibrate
%again, set to 0 if you dont want to use this feature.
%
%Christian Wengert
%Computer Vision Laboratory
%ETH Zurich
%Sternwartstrasse 7
%CH-8092 Zurich
%www.vision.ee.ethz.ch/cwengert
%wengert@vision.ee.ethz.ch

%Needed for the gridextractor, depending on your images, change this
minArea =15;maxArea = 14000;
%maximum backprojkection error
MAX_ERROR = 4000;

%With the doShow flag you can show the results
if(~exist('doShow'))
    doShow = 0;
end

if(~exist('dX_default'))
	dX_default = 2;
end

if(~exist('dY_default'))
	dY_default = 2;
end

if(~exist('dX'))
    dX = dX_default;
end
if(~exist('dY'))
    dY = dY_default;
end

%Give best accuracy
if(~exist('doIterateCalibration'))
    doIterateCalibration =0;
end
%Check the flag for refining the grid
if(~exist('doRefineGrid'))
    doRefineGrid = 0;
end
%Set it to zero anyway, it is not yet ready!
doRefineGrid = 0;

%Check the distortion flag
if(~exist('isStronglyDistorted'))
    isStronglyDistorted = 0;
end

%Check whether we have images
if(n_ima<=0)
    disp(['autocalibrate:: No images available']);
    return
end
    %CORNER EXTRACTION PART
    active_images = zeros(1,n_ima);
    for i=1:n_ima
        disp(['autocalibrate:: Processing image ' num2str(i)]);
        %Needs to be done for toolbox
        eval(['x_' num2str(i) ' = NaN*ones(2,1);']);
        eval(['X_' num2str(i) ' = NaN*ones(3,1);']);
        eval(['im = I_' num2str(i) ';'])
        if(doShow)  %Show if necessary
            figure;hold on;title(['Image ' num2str(i)]);
        end
        %Extract the grid points
        [success, stats, cnt, id0, id1, searchPoints] = gridextractor(im, doShow, minArea, maxArea);

        %Sometimes if we can find the two main marks, we dont process this
        %image
        if(success & cnt==2 & length(stats)>0)
            %Fit the grid
            if(isStronglyDistorted)
                [success, x_, X_, searchPoints] = fitgridDistorted(stats, searchPoints, id0, id1, dX, dY, doShow);
            else
                [success, x_, X_, searchPoints, err] = fitgrid(stats, searchPoints, id0, id1, dX, dY, doShow);
            end
            %Grid was fitted succesfully
            if(success)
                %Refine grid if wanted
                if(doRefineGrid)
                    refineGrid(im, success, stats, cnt, id0, id1, searchPoints);
                end
                %Update calib toollbox
                active_images(i) = i;
                eval(['X_' num2str(i) '=X_;']);
                eval(['x_' num2str(i) '=x_;']);

                %Check for error
                if(exist('err')==1)
                    if(err>MAX_ERROR)
                        active_images(i) = 0;
                        disp(['autocalibrate:: Image ' num2str(i) ' has rather huge gridprojection error. Setting it to inactive.']);
                    end
                end
            end
        else
            disp(['autocalibrate:: Not enough image information in image ' num2str(i) '. Setting it to inactive.']);
        end
    end
    %Make a backup
    old_active_images = active_images;
    %NOW WE START THE CAMERA CALIBRATION
    %Do this for calibtoolbox compatibility
    [ny,nx,bpp] = size(im);
    center_optim=1;
    est_aspect_ratio = 1;
    est_alpha=0;
    est_dist = [1 1 1 1 0]';
    init_intrinsic_param;
    %Calibrate camera
    try
        while(~exist('err_std'))
            go_calib_optim_iter;
        end
    catch
        disp(['autocalibrate:: Could not calibrate']);
        s = lasterror;
        disp(['autocalibrate:: ' s.message ' in function ' s.stack.name ' in ' s.stack.file ' @line ' num2str(s.stack.line)])
    end

    if(doIterateCalibration)
        %Improve
        maxIter=10;
        iter=0;

        %Max Error to eliminate points which have a backprojectionerror bigger than
        %that
        BP_ERROR = 1.5;
        while(norm(err_std)>0.5 & iter<maxIter)
            for i=active_images
                if(i>0)
                    eval(['error = ex_' num2str(i) ';']);
                    ix=find(error(1,:)>BP_ERROR);
                    iy=find(error(2,:)>BP_ERROR);
                    ixx = union(ix,iy);
                    eval(['x_' num2str(i) '(:,ixx) = [];']);
                    eval(['X_' num2str(i) '(:,ixx) = [];']);
                end

            end
            go_calib_optim_iter;
           iter=iter+1;
        end
    end


classdef SimplePeakDetector < Algorithm

    properties (Access = public)
        minPeakHeight = single(0.8);
        minPeakDistance  = int32(100);
    end

    methods  (Access = public)

        function obj = SimplePeakDetector(minPeakHeight, minPeakDistance)
            if nargin > 0
                obj.minPeakHeight = minPeakHeight;
                obj.minPeakDistance = minPeakDistance;
            end
            obj.name = 'simplePeakDet';
            obj.inputPort = DataType.kSignal;
            obj.outputPort = DataType.kEvent;
        end

        function str = toString(obj)
            str = sprintf('%s_%.3f_%d',obj.name,obj.minPeakHeight,obj.minPeakDistance);
        end

        function editableProperties = getEditableProperties(obj)
            minPeakHeightProperty = Property('minPeakHeight',obj.minPeakHeight,0,1);
            minPeakDistanceProperty = Property('minPeakDistance',obj.minPeakDistance,50,200);
            editableProperties = [minPeakHeightProperty,minPeakDistanceProperty];
        end

        function events = compute(obj,signal)
            eventLocations = obj.detectEvents(signal);
            events = Event.EventsArrayWithEventLocations(eventLocations);
        end

        function metrics = computeMetrics(~,input)
            flops = size(input,1) * 4;
            memory = 1;
            outputSize = 0;
            metrics = Metric(flops,memory,outputSize);
        end
    end

    methods (Access = private)

        function peakLocations = detectEvents(obj,signal)

            signal = single(signal);
            lastPeakLocation = -1000;
            lastPeak = 0;

            [peakLocations, ~,~] = obj.findPeakLocations(signal,obj.minPeakHeight,obj.minPeakDistance,lastPeakLocation,lastPeak);
            peakLocations = uint32(peakLocations);
        end

        function [peakLocations, lastPeakLocation, lastPeakValue] = findPeakLocations(~,magnitude,...
                minPeakHeight, minPeakDistance, lastPeakLocation, lastPeakValue)
            numPeakLocations = uint8(0);
            peakLocations = int32(zeros(1,floor(length(magnitude) / minPeakDistance)));

            for i = 1 : length(magnitude)
                sample = magnitude(i);

                if sample >= minPeakHeight
                    if sample > lastPeakValue || i >= (lastPeakLocation + minPeakDistance)
                        lastPeakValue = sample;
                        lastPeakLocation = int32(i);
                    end
                end

                if lastPeakValue > single(0.00001) && i >= lastPeakLocation + minPeakDistance
                    lastPeakValue = single(0);
                    numPeakLocations = numPeakLocations + 1;
                    peakLocations(numPeakLocations) = lastPeakLocation;
                end
            end
            peakLocations = peakLocations(1:numPeakLocations);
        end
    end

end


function out = feval(F, x, varargin)
%FEVAL   Evaluate a CHEBFUN.
%   FEVAL(F, X) evaluates a CHEBFUN F at the points in X.  If F is a quasimatrix
%   with columns F1, ..., FN, then the result will be [F1(X), ..., FN(X)], the
%   horizontal concatenation of the results of evaluating each column at the
%   points in X.
%
%   FEVAL(F, 'left'), FEVAL(F, 'start'), and FEVAL(F, '-') return the value of F
%   at the left endpoint of its domain.  FEVAL(F, 'right'), FEVAL(F, 'end'), and
%   FEVAL(F, '+') do the same for the right endpoint.
%
%   FEVAL(F, X, 'left') and FEVAL(F, X, '-') evaluate F at the points in X,
%   using left-hand limits to evaluate F at any breakpoints. FEVAL(F, X,
%   'right') and FEVAL(F, X, '+') do the same but using right-hand limits.
%
%   F(X), F('left'), F(X, 'left'), etc, are equivalent syntaxes.
%
%   Example:
%     f = chebfun(@(x) 1./(1 + 25*x.^2));
%     y = feval(f, linspace(-1, 1, 100).');
%
% See also SUBSREF.

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

% If F or x are empty, there's nothing to do.
if ( isempty(F) )
    out = [];
    return
elseif ( isempty(x) )
    % Return empty matrix with dimensions of the appropriate size.
    out = zeros(size(x));
    return
end

% Deal with FEVAL(FUNCTION_HANDLE, CHEBFUN) type calls.
if ( isa(F, 'function_handle') )
    out = F(x, varargin{:});
    return
end

% Deal with FEVAL(CHEBFUN, CHEBFUN) type calls.
if ( isa(x, 'chebfun') )
    out = compose(x, F);
    return
end

%% LEFT / RIGHT / END VALUES:
% Support for feval(f, 'left') and feval(f, 'end'), etc.
if ( ischar(x) )
    if ( any(strcmpi(x, {'left', 'start' ,'-'})) )
        out = F.pointValues(1,:);
    elseif ( any(strcmpi(x, {'right', 'end', '+'})) )
        out = F.pointValues(end,:);
    else
        error('CHEBFUN:CHEBFUN:feval:strInput', ...
            'Unknown input argument "%s".', x);
    end
    if ( F(1).isTransposed )
        out = out.';
    end
    return
end

%% EVALUATE EACH COLUMN:
if ( numel(F) == 1 )
    out = columnFeval(F, x, varargin{:});
else
    % Deal with quasimatrices:
    out = cell(1, numel(F));
    for k = 1:numel(F)
        out{k} = columnFeval(F(k), x, varargin{:});
    end
    out = cell2mat(out);
end

%% DEAL WITH ROW CHEBFUNS:
if ( F(1).isTransposed )
    % We got a passed a row CHEBFUN. If X had more than two dimensions, we can't
    % simply transpose the output from above, instead, we need to use permute.
    ndimsx = ndims(x);
    if ( ndimsx <= 2 )
        out = out.';
    else
        % We define "transposition" in this case to mean the switching of the
        % first two dimensions.
        out = permute(out, [2 1 3:ndimsx]);
    end
end

end

function out = columnFeval(f, x, varargin)
% Act on each column. Note that columnFeval ignores the transpose state of F.

%% INITIALISE:

% Reshape x to be a column vector.
sizex = size(x);
ndimsx = ndims(x);
x = x(:);

% Initialise output:
numFuns = numel(f.funs);

funs = f.funs;
dom = f.domain;
isTrans = f.isTransposed;

%% LEFT AND RIGHT LIMITS:
% Deal with feval(f, x, 'left') and feval(f, x, 'right'):
leftFlag = 0; rightFlag = 0;
if ( nargin > 2 )
    lr = varargin{1};
    leftFlag = any(strcmpi(lr, {'left', '-'}));
    rightFlag = any(strcmpi(lr, {'right', '+'}));
    if ( ~(leftFlag || rightFlag) )
        if ( ischar(lr) )
            error('CHEBFUN:CHEBFUN:feval:leftRightChar',...
                'Unknown input argument "%s".', lr);
        else
            error('CHEBFUN:CHEBFUN:feval:leftRight', 'Unknown input argument.');
        end
    end
end

%% VALUES FROM FUNS:

if ( numFuns == 1 )

    % Things are simple when there is only a single FUN:
    out = feval(funs{1}, x(:), varargin{:});
    numCols = size(out, 2);

else

    % For multiple FUNs we must determine which FUN corresponds to each x.

    % Initialise output matrix:
    numCols = numColumns(f);
    out = zeros(numel(x), numCols);

    % Replace the first and last domain entries with +/-inf. (Since we want to
    % use FUN{1} if real(x) < dom(1) and FUN{end} if real(x) > dom(end)).
    domInf = [-inf, dom(2:end-1), inf];

    % Loop over each FUN. If real(x) is in [dom(k) dom(k+1)] then use FUN{k}.
    xReal = real(x);
    for k = 1:numFuns
        I = ( xReal >= domInf(k) ) & ( xReal < domInf(k+1) );
        if ( any(I(:)) )
            % Evaluate the appropriate fun:
            out(I,:) = feval(funs{k}, x(I), varargin{:});
        end
    end

end

%% POINTVALUES:
% If the evaluation point corresponds to a breakpoint, we get the value from

% f.pointValues. However, if the 'left' or 'right' flag is given, we first
% modify the entry in point values to take either the left- or right-sided
% limit, respectively.

[xAtBreaks, domIndices] = ismember(x, dom);
if ( any(xAtBreaks) )
    if ( leftFlag )
        % Note that for leftFlag we use 'rval-local', which corresponds to the
        % function value at the right part of the subdomain.
        rvals = get(f, 'rval-local'); if ( isTrans ), rvals = rvals.'; end
        pointValues = [f.pointValues(1,:); rvals];
    elseif ( rightFlag )
        % Similarly rightFlag uses lval-local.
        lvals = get(f, 'lval-local'); if ( isTrans ), lvals = lvals.'; end
        pointValues = [lvals; f.pointValues(end,:)];
    else
        pointValues = f.pointValues;
    end
    % Set the output values for any values of x at the breakpoints.
    out(xAtBreaks,:) = pointValues(domIndices(xAtBreaks),:);
end

%% RESHAPE FOR OUTPUT:
% Reshape fx, which is a column vector or horizontal concatenation of column
% vectors, to be of the appropriate size, and handle transposition.
sizefx = sizex;
sizefx(2) = numCols*sizex(2);
if ( ndimsx == 2 )
    % If x was just a matrix or vector, the reshape is straightforward.
    out = reshape(out, sizefx);
else
    % If x had more than two dimensions, we have to be more careful.  The
    % cell2mat(mat2cell(...).') effects a transpose which keeps certain
    % columnar blocks of the fx matrix intact, putting the entries in the
    % correct order for reshape().
    blockLength = sizex(1)*sizex(2);
    blocksPerCol = prod(sizex(3:end));
    out = reshape(cell2mat(mat2cell(out, blockLength*ones(1, blocksPerCol), ...
        ones(1, numCols)).'), sizefx);
end

end


classdef ChongYaleFaces < handle
% This class is a wrapper over the dataset created by
% Chong You, 2016
% % C. You, D. Robinson, R. Vidal, Scalable Sparse Subspace Clustering by
% Orthogonal Matching Pursuit, CVPR 2016.
% [ExtendedYaleB.mat, https://sites.google.com/site/chongyou1987/] .
% The images are already  cropped.
properties
end
properties(SetAccess=private)
    % the data file from which everything is loaded.
    file_path
    % the raw data loaded from file
    data
    % image height
    image_height
    % image width
    image_width
    % number of subjects in database
    num_subjects
    %  The data set
    Y
    % The labels
    labels
    % The cluster sizes
    cluster_sizes
    % positions for individual clusters
    start_indices
    end_indices
end

properties(Access=private)
    subject_sets_map
end

methods
    function self = ChongYaleFaces()
        root = spx.data_dir;
        self.file_path = fullfile(root, 'faces', 'ExtendedYaleB.mat');
        data = load(self.file_path);
        self.image_height = 48;
        self.image_width = 42;
        self.num_subjects = 38;
        self.labels = data.EYALEB_LABEL;
        self.Y = data.EYALEB_DATA;
        self.cluster_sizes = spx.cluster.cluster_sizes_from_labels(self.labels);
        [self.start_indices, self.end_indices] = spx.cluster.start_end_indices(self.cluster_sizes);
        self.subject_sets_map = cell(1, 10);
    end

    function result = num_total_faces(self)
        result = sum(self.cluster_sizes);
    end

    function result = subject_faces_as_matrices(self, i)
        error('Not implemented.');
        % Returns all faces of a subject as matrices
        result = self.data.I(:, :, i, :);
    end

    function result = subject_faces_as_columns(self, i)
        % Returns all faces of a subject as column vectors
        s= self.start_indices(i);
        e = self.end_indices(i);
        result = self.Y(:, s:e);
    end

    function result = sets_of_n_subjects(self, n)
        if n < 1 || n > 10
            error('Number of subjects must be between 1 and 10');
        end
        if ~isempty(self.subject_sets_map{n})
            result = self.subject_sets_map{n};
            return;
        end
        result1 = nchoosek(1:10, n);
        result2 = nchoosek(11:20, n);
        result3 = nchoosek(21:30, n);
        result = [result1; result2; result3];
        if n <= 8
            result4 = nchoosek(31:38, n);
            result = [result; result4];
        end
        self.subject_sets_map{n} = result;
    end

    function result = num_experiments_of_n_subjects(self, n)
        sets = self.sets_of_n_subjects(n);
        result = size(sets, 1);
    end

    %% experiment_data: Returns data for a particular experiment
    function [Y, cluster_sizes, labels] = experiment_data(self, n, i)
        sets = self.sets_of_n_subjects(n);
        nn  = size(sets, 1);
        if i < 1 || i > nn
            error('Invalid experiment number.');
        end
        indices = sets(i, :);
        Y = [];
        cluster_sizes = self.cluster_sizes(indices);
        for k=1:n
            subject_index = indices(k);
            Y = [Y self.subject_faces_as_columns(subject_index)];
        end
        labels = spx.cluster.labels_from_cluster_sizes(cluster_sizes);
    end

    %% Generates data for specified subjects
    function [Y, cluster_sizes, labels] = data_for_subjects(self, subject_list)
        cluster_sizes = self.cluster_sizes(subject_list);
        Y = [];
        for k=1:numel(subject_list)
            subject_index = subject_list(k);
            Y = [Y self.subject_faces_as_columns(subject_index)];
        end
        labels = spx.cluster.labels_from_cluster_sizes(cluster_sizes);
    end

    function [faces, cluster_sizes, labels] = all_faces(self)
        faces = self.Y;
        cluster_sizes = self.cluster_sizes;
        labels = self.labels;
    end

    function canvas = create_random_canvas(self, rows, cols)
        n = self.num_subjects;
        if nargin < 2
            rows = 4;
        end
        if nargin < 3
            cols = rows;
        end
        k  = rows * cols;
        indices = randperm(n, k);
        faces = self.all_faces;
        % pick first image for each subject
        indices = self.start_indices(indices);
        images = faces(:, indices);
        canvas = spx.graphics.canvas.create_image_grid(images, rows, cols, ...
            self.image_height, self.image_width);
        canvas = uint8(canvas);
    end

end

end


function [bs] = A2bs(A)
% Convert length from angstroms to beard-seconds.
% Chad A. Greene 2012
bs = A/100;


function y_bytearray = int32_2_int24(u)
  % returns a three byte array containing uint8 values which represent together a
  % signed int24 value
  % reverse engineered from roborace int24 to int32 cast
  % y_int24 = bitshift(uint32(bitshift(u, 8)), -8);
  y_int24_separate = typecast(u, 'uint8');
  % extract bytes
  y_bytearray = y_int24_separate(1:3);
end


function [ event ] = read_ah5_markers(hdr, filename )

if isempty(hdr)
    orig = read_ahdf5_hdr(filename);
    hdr.orig = orig;
    hdr.Fs = orig.channels(1).samplingRate;
    hdr.nChans = numel(orig.channels);
    hdr.nSamples = orig.numberOfSamples;
    hdr.nTrials = orig.numberOfBlocks;
    hdr.nSamplesPre = 0;
    hdr.label = orig.label;
end

marks = h5read(filename, '/Blocks/1/markers');
labels = marks.label';
positions = marks.position';
durations = marks.duration';
values = marks.value';

for i=1:size(labels, 1)
    event(i).type = strcat(labels(i, 1:256));
    event(i).value = values(i);
    event(i).sample = positions(i) * hdr.Fs;
    event(i).duration = durations(i) * hdr.Fs;
end
end