function fname_out = savecase(fname, varargin)
%SAVECASE  Saves a MATPOWER case file, given a filename and the data.
%   SAVECASE(FNAME, CASESTRUCT)
%   SAVECASE(FNAME, CASESTRUCT, VERSION)
%   SAVECASE(FNAME, BASEMVA, BUS, GEN, BRANCH)
%   SAVECASE(FNAME, BASEMVA, BUS, GEN, BRANCH, GENCOST)
%   SAVECASE(FNAME, BASEMVA, BUS, GEN, BRANCH, AREAS, GENCOST)
%   SAVECASE(FNAME, COMMENT, CASESTRUCT)
%   SAVECASE(FNAME, COMMENT, CASESTRUCT, VERSION)
%   SAVECASE(FNAME, COMMENT, BASEMVA, BUS, GEN, BRANCH)
%   SAVECASE(FNAME, COMMENT, BASEMVA, BUS, GEN, BRANCH, GENCOST)
%   SAVECASE(FNAME, COMMENT, BASEMVA, BUS, GEN, BRANCH, AREAS, GENCOST)
%
%   FNAME = SAVECASE(FNAME, ...)
%
%   Writes a MATPOWER case file, given a filename and data struct or list of
%   data matrices. The FNAME parameter is the name of the file to be created or
%   overwritten. If FNAME ends with '.mat' it saves the case as a MAT-file
%   otherwise it saves it as an M-file. Optionally returns the filename,
%   with extension added if necessary. The optional COMMENT argument is
%   either string (single line comment) or a cell array of strings which
%   are inserted as comments. When using a MATPOWER case struct, if the
%   optional VERSION argument is '1' it will modify the data matrices to
%   version 1 format before saving.

%   MATPOWER
%   Copyright (c) 1996-2016, Power Systems Engineering Research Center (PSERC)
%   by Carlos E. Murillo-Sanchez, PSERC Cornell & Universidad Nacional de Colombia
%   and 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.

%% define named indices into bus, gen, branch matrices
[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;
[PW_LINEAR, POLYNOMIAL, MODEL, STARTUP, SHUTDOWN, NCOST, COST] = idx_cost;

%% default arguments
if ischar(varargin{1}) || iscell(varargin{1})
    if ischar(varargin{1})
        comment = {varargin{1}};    %% convert char to single element cell array
    else
        comment = varargin{1};
    end
    [args{1:(length(varargin)-1)}] = deal(varargin{2:end});
else
    comment = {''};
    args = varargin;
end
mpc_ver = '2';              %% default MATPOWER case file version
if isstruct(args{1})        %% 1st real argument is a struct
    mpc = args{1};
    if length(args) > 1
        mpc.version = args{2};
        mpc_ver = mpc.version;
    end
    baseMVA = mpc.baseMVA;
    bus     = mpc.bus;
    gen     = mpc.gen;
    branch  = mpc.branch;
    if isfield(mpc, 'areas')
        areas   = mpc.areas;
    end
    if isfield(mpc, 'gencost')
        gencost = mpc.gencost;
    end
else                        %% 1st real argument is NOT a struct
    baseMVA = args{1};
    bus     = args{2};
    gen     = args{3};
    branch  = args{4};
    mpc.baseMVA = baseMVA;
    mpc.bus     = bus;
    mpc.gen     = gen;
    mpc.branch  = branch;
    if length(args) == 5
        gencost = args{5};
        mpc.gencost = gencost;
    end
    if length(args) == 6
        areas   = args{5};
        gencost = args{6};
        mpc.areas   = areas;
        mpc.gencost = gencost;
    end
end

%% modifications for version 1 format
if strcmp(mpc_ver, '1')
    %% remove extra columns of gen
    if size(gen, 2) >= MU_QMIN
        gen = gen(:, [1:PMIN, MU_PMAX:MU_QMIN]);
    else
        gen = gen(:, 1:PMIN);
    end
    %% use the version 1 values for column names
    shift = MU_PMAX - PMIN - 1;
    tmp = num2cell([MU_PMAX, MU_PMIN, MU_QMAX, MU_QMIN] - shift);
    [MU_PMAX, MU_PMIN, MU_QMAX, MU_QMIN] = deal(tmp{:});

    %% remove extra columns of branch
    if size(branch, 2) >= MU_ST
        branch = branch(:, [1:BR_STATUS, PF:MU_ST]);
    elseif size(branch, 2) >= QT
        branch = branch(:, [1:BR_STATUS, PF:QT]);
    else
        branch = branch(:, 1:BR_STATUS);
    end
    %% use the version 1 values for column names
    shift = PF - BR_STATUS - 1;
    tmp = num2cell([PF, QF, PT, QT, MU_SF, MU_ST] - shift);
    [PF, QF, PT, QT, MU_SF, MU_ST] = deal(tmp{:});
end

%% verify valid filename
[pathstr, fcn_name, extension] = fileparts(fname);
if isempty(extension)
    extension = '.m';
end
if regexp(fcn_name, '\W')
    old_fcn_name = fcn_name;
    fcn_name = regexprep(fcn_name, '\W', '_');
    fprintf('WARNING: ''%s'' is not a valid function name, changed to ''%s''\n', old_fcn_name, fcn_name);
end
fname = fullfile(pathstr, [fcn_name extension]);

%% open and write the file
if strcmp(upper(extension), '.MAT')     %% MAT-file
    if strcmp(mpc_ver, '1')
        if exist('areas', 'var') && exist('gencost', 'var')
            save(fname, 'baseMVA', 'bus', 'gen', 'branch', 'areas', 'gencost');
        else
            save(fname, 'baseMVA', 'bus', 'gen', 'branch');
        end
    else
        save(fname, 'baseMVA', 'mpc');
    end
else                                %% M-file
    %% open file
    [fd, msg] = fopen(fname, 'wt');     %% print it to an M-file
    if fd == -1
        error(['savecase: ', msg]);
    end

    %% function header, etc.
    if strcmp(mpc_ver, '1')
        if exist('areas', 'var') && exist('gencost', 'var') && ~isempty(gencost)
            fprintf(fd, 'function [baseMVA, bus, gen, branch, areas, gencost] = %s\n', fcn_name);
        else
            fprintf(fd, 'function [baseMVA, bus, gen, branch] = %s\n', fcn_name);
        end
        prefix = '';
    else
        fprintf(fd, 'function mpc = %s\n', fcn_name);
        prefix = 'mpc.';
    end
    if isempty(comment{1})
        comment{1} = sprintf('%s', upper(fcn_name));
    else
        comment{1} = sprintf('%s  %s', upper(fcn_name), comment{1});
    end
    for k = 1:length(comment)
        fprintf(fd, '%%%s\n', comment{k});
    end
    fprintf(fd, '\n%%%% MATPOWER Case Format : Version %s\n', mpc_ver);
    if ~strcmp(mpc_ver, '1')
        fprintf(fd, 'mpc.version = ''%s'';\n', mpc_ver);
    end
    fprintf(fd, '\n%%%%-----  Power Flow Data  -----%%%%\n');
    fprintf(fd, '%%%% system MVA base\n');
    fprintf(fd, '%sbaseMVA = %.9g;\n', prefix, baseMVA);

    %% bus data
    ncols = size(bus, 2);
    fprintf(fd, '\n%%%% bus data\n');
    fprintf(fd, '%%\tbus_i\ttype\tPd\tQd\tGs\tBs\tarea\tVm\tVa\tbaseKV\tzone\tVmax\tVmin');
    if ncols >= MU_VMIN             %% opf SOLVED, save with lambda's & mu's
        fprintf(fd, '\tlam_P\tlam_Q\tmu_Vmax\tmu_Vmin');
    end
    fprintf(fd, '\n%sbus = [\n', prefix);
    if ncols < MU_VMIN              %% opf NOT SOLVED, save without lambda's & mu's
        fprintf(fd, '\t%d\t%d\t%.9g\t%.9g\t%.9g\t%.9g\t%d\t%.9g\t%.9g\t%.9g\t%d\t%.9g\t%.9g;\n', bus(:, 1:VMIN).');
    else                            %% opf SOLVED, save with lambda's & mu's
        fprintf(fd, '\t%d\t%d\t%.9g\t%.9g\t%.9g\t%.9g\t%d\t%.9g\t%.9g\t%.9g\t%d\t%.9g\t%.9g\t%.4f\t%.4f\t%.4f\t%.4f;\n', bus(:, 1:MU_VMIN).');
    end
    fprintf(fd, '];\n');

    %% generator data
    ncols = size(gen, 2);
    fprintf(fd, '\n%%%% generator data\n');
    fprintf(fd, '%%\tbus\tPg\tQg\tQmax\tQmin\tVg\tmBase\tstatus\tPmax\tPmin');
    if ~strcmp(mpc_ver, '1')
        fprintf(fd, '\tPc1\tPc2\tQc1min\tQc1max\tQc2min\tQc2max\tramp_agc\tramp_10\tramp_30\tramp_q\tapf');
    end
    if ncols >= MU_QMIN             %% opf SOLVED, save with mu's
        fprintf(fd, '\tmu_Pmax\tmu_Pmin\tmu_Qmax\tmu_Qmin');
    end
    fprintf(fd, '\n%sgen = [\n', prefix);
    if ncols < MU_QMIN              %% opf NOT SOLVED, save without mu's
        if strcmp(mpc_ver, '1')
            fprintf(fd, '\t%d\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%d\t%.9g\t%.9g;\n', gen(:, 1:PMIN).');
        else
            fprintf(fd, '\t%d\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%d\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g;\n', gen(:, 1:APF).');
        end
    else
        if strcmp(mpc_ver, '1')
            fprintf(fd, '\t%d\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%d\t%.9g\t%.9g\t%.4f\t%.4f\t%.4f\t%.4f;\n', gen(:, 1:MU_QMIN).');
        else
            fprintf(fd, '\t%d\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%d\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.4f\t%.4f\t%.4f\t%.4f;\n', gen(:, 1:MU_QMIN).');
        end
    end
    fprintf(fd, '];\n');

    %% branch data
    ncols = size(branch, 2);
    fprintf(fd, '\n%%%% branch data\n');
    fprintf(fd, '%%\tfbus\ttbus\tr\tx\tb\trateA\trateB\trateC\tratio\tangle\tstatus');
    if ~strcmp(mpc_ver, '1')
        fprintf(fd, '\tangmin\tangmax');
    end
    if ncols >= QT                  %% power flow SOLVED, save with line flows
        fprintf(fd, '\tPf\tQf\tPt\tQt');
    end
    if ncols >= MU_ST               %% opf SOLVED, save with mu's
        fprintf(fd, '\tmu_Sf\tmu_St');
        if ~strcmp(mpc_ver, '1')
            fprintf(fd, '\tmu_angmin\tmu_angmax');
        end
    end
    fprintf(fd, '\n%sbranch = [\n', prefix);
    if ncols < QT                   %% power flow NOT SOLVED, save without line flows or mu's
        if strcmp(mpc_ver, '1')
            fprintf(fd, '\t%d\t%d\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%d;\n', branch(:, 1:BR_STATUS).');
        else
            fprintf(fd, '\t%d\t%d\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%d\t%.9g\t%.9g;\n', branch(:, 1:ANGMAX).');
        end
    elseif ncols < MU_ST            %% power flow SOLVED, save with line flows but without mu's
        if strcmp(mpc_ver, '1')
            fprintf(fd, '\t%d\t%d\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%d\t%.4f\t%.4f\t%.4f\t%.4f;\n', branch(:, 1:QT).');
        else
            fprintf(fd, '\t%d\t%d\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%d\t%.9g\t%.9g\t%.4f\t%.4f\t%.4f\t%.4f;\n', branch(:, 1:QT).');
       end
    else                            %% opf SOLVED, save with lineflows & mu's
        if strcmp(mpc_ver, '1')
            fprintf(fd, '\t%d\t%d\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%d\t%.4f\t%.4f\t%.4f\t%.4f\t%.4f\t%.4f;\n', branch(:, 1:MU_ST).');
        else
            fprintf(fd, '\t%d\t%d\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%.9g\t%d\t%.9g\t%.9g\t%.4f\t%.4f\t%.4f\t%.4f\t%.4f\t%.4f\t%.4f\t%.4f;\n', branch(:, 1:MU_ANGMAX).');
        end
    end
    fprintf(fd, '];\n');

    %% OPF data
    if (exist('areas', 'var') && ~isempty(areas)) || ...
        (exist('gencost', 'var') && ~isempty(gencost))
        fprintf(fd, '\n%%%%-----  OPF Data  -----%%%%');
    end
    if exist('areas', 'var') && ~isempty(areas)
        %% area data
        fprintf(fd, '\n%%%% area data\n');
        fprintf(fd, '%%\tarea\trefbus\n');
        fprintf(fd, '%sareas = [\n', prefix);
        if ~isempty(areas)
            fprintf(fd, '\t%d\t%d;\n', areas(:, 1:2).');
        end
        fprintf(fd, '];\n');
    end
    if exist('gencost', 'var') && ~isempty(gencost)
        %% generator cost data
        fprintf(fd, '\n%%%% generator cost data\n');
        fprintf(fd, '%%\t1\tstartup\tshutdown\tn\tx1\ty1\t...\txn\tyn\n');
        fprintf(fd, '%%\t2\tstartup\tshutdown\tn\tc(n-1)\t...\tc0\n');
        fprintf(fd, '%sgencost = [\n', prefix);
        if ~isempty(gencost)
            n1 = 2 * max(gencost(gencost(:, MODEL) == PW_LINEAR,  NCOST));
            n2 =     max(gencost(gencost(:, MODEL) == POLYNOMIAL, NCOST));
            n = max([n1; n2]);
            if size(gencost, 2) < n + 4
                error('savecase: gencost data claims it has more columns than it does');
            end
            template = '\t%d\t%.9g\t%.9g\t%d';
            for i = 1:n
                template = [template, '\t%.9g'];
            end
            template = [template, ';\n'];
            fprintf(fd, template, gencost(:, 1:n+4).');
        end
        fprintf(fd, '];\n');
    end

    if ~strcmp(mpc_ver, '1')
        %% generalized OPF user data
        if (isfield(mpc, 'A') && ~isempty(mpc.A)) || ...
                (isfield(mpc, 'N') && ~isempty(mpc.N))
            fprintf(fd, '\n%%%%-----  Generalized OPF User Data  -----%%%%');
        end

        %% user constraints
        if isfield(mpc, 'A') && ~isempty(mpc.A)
            %% A
            fprintf(fd, '\n%%%% user constraints\n');
            print_sparse(fd, sprintf('%sA', prefix), mpc.A);
            if isfield(mpc, 'l') && ~isempty(mpc.l) && ...
                    isfield(mpc, 'u') && ~isempty(mpc.u)
                fprintf(fd, 'lbub = [\n');
                fprintf(fd, '\t%.9g\t%.9g;\n', [mpc.l mpc.u].');
                fprintf(fd, '];\n');
                fprintf(fd, '%sl = lbub(:, 1);\n', prefix);
                fprintf(fd, '%su = lbub(:, 2);\n\n', prefix);
            elseif isfield(mpc, 'l') && ~isempty(mpc.l)
                fprintf(fd, '%sl = [\n', prefix);
                fprintf(fd, '\t%.9g;\n', mpc.l);
                fprintf(fd, '];\n\n');
            elseif isfield(mpc, 'u') && ~isempty(mpc.u)
                fprintf(fd, '%su = [\n', prefix);
                fprintf(fd, '\t%.9g;\n', mpc.u);
                fprintf(fd, '];\n');
            end
        end

        %% user costs
        if isfield(mpc, 'N') && ~isempty(mpc.N)
            fprintf(fd, '\n%%%% user costs\n');
            print_sparse(fd, sprintf('%sN', prefix), mpc.N);
            if isfield(mpc, 'H') && ~isempty(mpc.H)
                print_sparse(fd, sprintf('%sH', prefix), mpc.H);
            end
            if isfield(mpc, 'fparm') && ~isempty(mpc.fparm)
                fprintf(fd, 'Cw_fparm = [\n');
                fprintf(fd, '\t%.9g\t%d\t%.9g\t%.9g\t%.9g;\n', [mpc.Cw mpc.fparm].');
                fprintf(fd, '];\n');
                fprintf(fd, '%sCw    = Cw_fparm(:, 1);\n', prefix);
                fprintf(fd, '%sfparm = Cw_fparm(:, 2:5);\n', prefix);
            else
                fprintf(fd, '%sCw = [\n', prefix);
                fprintf(fd, '\t%.9g;\n', mpc.Cw);
                fprintf(fd, '];\n');
            end
        end

        %% user vars
        if isfield(mpc, 'z0') || isfield(mpc, 'zl') || isfield(mpc, 'zu')
            fprintf(fd, '\n%%%% user vars\n');
        end
        if isfield(mpc, 'z0') && ~isempty(mpc.z0)
            fprintf(fd, '%sz0 = [\n', prefix);
            fprintf(fd, '\t%.9g;\n', mpc.z0);
            fprintf(fd, '];\n');
        end
        if isfield(mpc, 'zl') && ~isempty(mpc.zl)
            fprintf(fd, '%szl = [\n', prefix);
            fprintf(fd, '\t%.9g;\n', mpc.zl);
            fprintf(fd, '];\n');
        end
        if isfield(mpc, 'zu') && ~isempty(mpc.zu)
            fprintf(fd, '%szu = [\n', prefix);
            fprintf(fd, '\t%.9g;\n', mpc.zu);
            fprintf(fd, '];\n');
        end

        %% (optional) generator unit types
        if isfield(mpc, 'gentype') && iscell(mpc.gentype)
            ng = length(mpc.gentype);
            if size(mpc.gen, 1) ~= ng
                warning('savecase: gentype field does not have the expected dimensions (length = %d, expected %d)', ng, size(mpc.gen, 1));
            end

            fprintf(fd, '\n%%%% generator unit type (see GENTYPES)\n');
            fprintf(fd, '%sgentype = {\n', prefix);
            for k = 1:ng
                fprintf(fd, '\t''%s'';\n', mpc.gentype{k});
            end
            fprintf(fd, '};\n');
        end

        %% (optional) generator fuel types
        if isfield(mpc, 'genfuel') && iscell(mpc.genfuel)
            ng = length(mpc.genfuel);
            if size(mpc.gen, 1) ~= ng
                warning('savecase: genfuel field does not have the expected dimensions (length = %d, expected %d)', ng, size(mpc.gen, 1));
            end

            fprintf(fd, '\n%%%% generator fuel type (see GENFUELS)\n');
            fprintf(fd, '%sgenfuel = {\n', prefix);
            for k = 1:ng
                fprintf(fd, '\t''%s'';\n', mpc.genfuel{k});
            end
            fprintf(fd, '};\n');
        end

        %% (optional) bus names
        if isfield(mpc, 'bus_name') && iscell(mpc.bus_name)
            nb = length(mpc.bus_name);
            if size(mpc.bus, 1) ~= nb
                warning('savecase: bus_name field does not have the expected dimensions (length = %d, expected %d)', nb, size(mpc.bus, 1));
            end

            fprintf(fd, '\n%%%% bus names\n');
            fprintf(fd, '%sbus_name = {\n', prefix);
            for k = 1:nb
                fprintf(fd, '\t''%s'';\n', strrep(mpc.bus_name{k}, '''', ''''''));
            end
            fprintf(fd, '};\n');
        end

        %% execute userfcn callbacks for 'savecase' stage
        if isfield(mpc, 'userfcn')
            run_userfcn(mpc.userfcn, 'savecase', mpc, fd, prefix);
        end
    end

    %% close file
    if fd ~= 1
        fclose(fd);
    end
end

if nargout > 0
    fname_out = fname;
end

function print_sparse(fd, varname, A)

[i, j, s] = find(A);
[m, n] = size(A);

if isempty(s)
    fprintf(fd, '%s = sparse(%d, %d);\n', varname, m, n);
else
    fprintf(fd, 'ijs = [\n');
    if m == 1           %% i, j, s are row vectors
        fprintf(fd, '\t%d\t%d\t%.9g;\n', [i; j; s]);
    else                %% i, j, s are column vectors
        fprintf(fd, '\t%d\t%d\t%.9g;\n', [i j s].');
    end
    fprintf(fd, '];\n');
    fprintf(fd, '%s = sparse(ijs(:, 1), ijs(:, 2), ijs(:, 3), %d, %d);\n', varname, m, n);
end


function hole_point = p00_hole_point ( test, hole_index, m )

%*****************************************************************************80
%
%% P00_HOLE_POINT returns a point inside a given hole.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer TEST, the index of the test problem
%
%    Input, integer HOLE_INDEX, the index of the hole.
%
%    Input, integer M, the spatial dimension.
%
%    Output, real HOLE_POINT(M), a point in the hole
%
  if ( test == 1 )
    hole_point = p01_hole_point ( hole_index, m );
  elseif ( test == 2 )
    hole_point = p02_hole_point ( hole_index, m );
  elseif ( test == 3 )
    hole_point = p03_hole_point ( hole_index, m );
  elseif ( test == 4 )
    hole_point = p04_hole_point ( hole_index, m );
  elseif ( test == 5 )
    hole_point = p05_hole_point ( hole_index, m );
  elseif ( test == 6 )
    hole_point = p06_hole_point ( hole_index, m );
  elseif ( test == 7 )
    hole_point = p07_hole_point ( hole_index, m );
  elseif ( test == 8 )
    hole_point = p08_hole_point ( hole_index, m );
  elseif ( test == 9 )
    hole_point = p09_hole_point ( hole_index, m );
  elseif ( test == 10 )
    hole_point = p10_hole_point ( hole_index, m );
  elseif ( test == 11 )
    hole_point = p11_hole_point ( hole_index, m );
  elseif ( test == 12 )
    hole_point = p12_hole_point ( hole_index, m );
  elseif ( test == 13 )
    hole_point = p13_hole_point ( hole_index, m );
  elseif ( test == 14 )
    hole_point = p14_hole_point ( hole_index, m );
  elseif ( test == 15 )
    hole_point = p15_hole_point ( hole_index, m );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'P00_HOLE_POINT - Fatal error!\n' );
    fprintf ( 1, '  Input value of TEST is out of range.\n' );
    error ( 'P00_HOLE_POINT - Fatal error!' );
  end

  return
end


function test_errors
%TEST_ERRORS tests error handling for the factorize object methods
%
% Example
%   test_errors
%
% See also test_all, factorize1, factorize.

% Copyright 2009, Timothy A. Davis, University of Florida

% the matrix A must be square for factorize1
fprintf ('\nTesting error-handling, error messages expected:\n') ;
ok = true ;
A = rand (3,2) ;
try
    F = factorize1 (A) ;                                               %#ok
    ok = false ;
catch me
    fprintf ('Expected error: [%s]\n', me.message) ;
end

% the matrix A must be 2D for factorize
A = ones (2,2,2) ;
try
    F = factorize (A) ;                                                %#ok
    ok = false ;
catch me
    fprintf ('Expected error: [%s]\n', me.message) ;
end

% the matrix A must be full-rank
A = ones (3) ;
try
    F = factorize (A) ;                                                %#ok
    ok = false ;
catch me
    fprintf ('Expected error: [%s]\n', me.message) ;
end

try
    F = factorize1 (A) ;                                               %#ok
    ok = false ;
catch me
    fprintf ('Expected error: [%s]\n', me.message) ;
end

A = ones (3,2) ;
try
    F = factorize (A) ;                                                %#ok
    ok = false ;
catch me
    fprintf ('Expected error: [%s]\n', me.message) ;
end

A = ones (2,3) ;
try
    F = factorize (A) ;                                                %#ok
    ok = false ;
catch me
    fprintf ('Expected error: [%s]\n', me.message) ;
end

% cannot use cell indexing
A = rand (3,2) ;
F = factorize (A) ;
try
    C = F {1} ;                                                        %#ok
    ok = false ;
catch me
    fprintf ('Expected error: [%s]\n', me.message) ;
end

% cannot use linear indexing of the inverse
S = inverse (A) ;
try
    C = S (1) ;                                                        %#ok
    ok = false ;
catch me
    fprintf ('Expected error: [%s]\n', me.message) ;
end

% invalid indexing
try
    C = F (1,1).L ;                                                    %#ok
    ok = false ;
catch me
    fprintf ('Expected error: [%s]\n', me.message) ;
end

% invalid indexing
try
    C = F.L (1,1).stuff ;                                              %#ok
    ok = false ;
catch me
    fprintf ('Expected error: [%s]\n', me.message) ;
end

% non-existent field
try
    C = F.junk ;                                                       %#ok
    ok = false ;
catch me
    fprintf ('Expected error: [%s]\n', me.message) ;
end

% can only update/downdate a dense Cholesky factorization
A = rand (2) ;
F = factorize (A) ;
w = rand (2,1) ;
try
    F = F + w ;
catch me
    fprintf ('Expected error: [%s]\n', me.message) ;
end
try
    F = F - w ;
catch me
    fprintf ('Expected error: [%s]\n', me.message) ;
end

if (~ok)
    error ('error-handling failed') ;
end
fprintf ('\nAll error-handing tests passed\n') ;


classdef WaveBuffer
    properties
        nCh = 1;            % number of channels
        BufferSize = 400;   % this is the buffer size, and also the data size we get from the buffer everytime we call Get().
        BlockSize = 160;    % this is the amount of data we push into the buffer everytime we call Push.
        FrameOverlap = 240; % this is the overlap between two consecutive frames.
        % Note that, the BlockSize = BufferSize - FrameOverlap. In this way,
        % we can gauarantee that the blockwise processing generate the same
        % Short-time Fourier Transform representation of the signal as in
        % the offline processing.
        precision = 'double';
        data = [];
    end

    methods
        function obj = WaveBuffer(nCh, BufferSize, FrameOverlap, precision)
            obj.nCh = nCh;
            obj.BufferSize = BufferSize;
            obj.FrameOverlap = FrameOverlap;
            obj.BlockSize = BufferSize - FrameOverlap;
            if nargin>=4
                obj.precision = precision;
            end

            % initialize the buffer with 0's
            obj.data = zeros(obj.nCh, obj.BufferSize, obj.precision);
        end

        function obj = Push(obj, data)  % push data into the buffer
            [nCh, nSample] = size(data);

            assert(obj.nCh == nCh, sprintf('Error: number of channel in data (%d) is different from number of channel in buffer (%d)', nCh, obj.nCh));
            assert(obj.BlockSize == nSample, sprintf('Error: number of samples in data (%d) is different from block size(%d)', nSample, obj.BlockSize));

            obj.data = [obj.data(:,obj.BlockSize+1:end) data];
        end

        function data = Get(obj)        % get the data from the buffer
            data = obj.data;
        end

        function obj = Reset(obj)       % reset the content of the buffer to 0
            obj.data(:) = 0;
        end
    end

end


function region = p18_region ( )

%*****************************************************************************80
%
%% P18_REGION returns the name of the integration region for problem 18.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 March 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, string REGION, the name of the integration region.
%
  region = 'box';

  return
end


function s = y_to_s_gregorian ( y )

%*****************************************************************************80
%
%% Y_TO_S_GREGORIAN writes a Gregorian year into a string.
%
%  Format:
%
%    YearNumber BC
%    YearNumber AD
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, the year.
%
%    Output, string S, a representation of the year.
%
  if ( y < 0 )
    s = sprintf ( 'BC %d', -y );
  elseif ( 0 < y )
    s = sprintf ( 'AD %d', y );
  end

  return
end


function print_progress(n, N)
%print_progress(n, N)
%
% print work progress of loops (tic, for n=1:N)

%% idea by stefan harmeling

cent= round(100*n/N);
togo= toc/n*(N-n);
fprintf('\r%3d%%, %ds to go    ', cent, floor(togo));
if n==N,
  fprintf('\r%40s\r','');
end


function [v, template] = struct2vector( s )

  if isnumeric(s)

    v = s(:);
    template = size(s);

  elseif iscell(s)

    [v, template] = cell2vector(s);

  else

    fields = fieldnames(s)';
    vs = {};
    shapes = {};
    for fi = 1:length(fields)
      x = getfield(s, fields{fi});
      if iscell(x)
        [vs{fi}, shapes{fi}] = cell2vector(x);
      elseif isstruct(x)
        [vs{fi}, shapes{fi}] = struct2vector(x);
      else
        vs{fi} = x(:);
        shapes{fi} = size(x);
      end
    end

    v = cat(1,vs{:});

    template.fields = fields;
    template.shapes = shapes;

  end
end


function res = set_partition(varargin)
% Obtain a subset of a data set, using an index set.
% Signal = set_partition(Signal, IndexSet, EpochBounds)
%
% A partitioning function (as required by the generic functions utl_crossval, utl_searchmodel,
% utl_nested_crossval) that takes a data set (either epoched or continuous), and index set (either
% of the raw samples, the target events, or of the epochs), and returns a reduced (subset) data set.
%
% In:
%   Signal   : Continuous or epoched input data set.
%
%   IndexSet : Either [] or a vector of indices;
%               * if [], the function returns the cardinality (highest allowed index) of the index set,
%               * if a vector of indices, the function returns the data set reduced to those indices;
%                 if the indices refer to target events, but the data is continuous, then the
%                 partition function tries to respect the implied epoch bounds when partitioning
%
%   EpochBounds : Only required when partitioning a continuous data set based on target events;
%                 this is done with the help of implied epoch bounds (as in set_makepos)
%
% Out:
%   Result : Reduced data set or index set cardinality.
%
% Examples:
%   % for an epoched data set or a continuous set with target markers, get the number of trials
%   numtrials = set_partition(eeg,[])
%
%   % for an epoched data set or a continuous set with target markers, retain only trials 20:40
%   eeg = set_partition(eeg,20:40)
%
%   % for a continuous data set without target markers, get the number of samples
%   numsamples = set_partition(eeg,[])
%
%   % for a continuous data set without target markers, retain only samples 1:10000 and 50000:100000
%   eeg = set_partition(eeg,[1:10000 50000:100000])
%
% See also:
%   set_makepos, set_selepos, set_selinterval, utl_crossval
%
%                                Christian Kothe, Swartz Center for Computational Neuroscience, UCSD
%                                2010-04-01
dp;

% set_partition_version<1.01> -- for the cache

if ~exp_beginfun('editing', 'memoize',0, 'fingerprint_create','passthrough') return; end

declare_properties('independent_channels',true,'independent_trials',false);

arg_define(varargin, ...
    arg_norep({'signal','Signal'}), ...
    arg({'idxset','IndexSet'},[],uint32([1 1000000]), 'Index set. This is either [] (indicating that the function should return the index set cardinality), or a vector of indices (indicating that the function shall partition the data set accordingly).','shape','row'), ...
    arg({'epoch_bounds','EpochBounds'},[],[], 'Epoch bounds. Only required when partitioning on a continuous data set based on target events; this is performed using the epoch bounds (as in set_makepos).','shape','row'));

% input validation
utl_check_fields(signal,{'data','srate','event'},'signal','signal'); %#ok<NODEF>
is_continuous = (~isfield(signal,'epoch') || isempty(signal.epoch)) && (isempty(signal.data) || size(signal.data,3) == 1);

if isempty(idxset) %#ok<NODEF>
    % --- calc index set size ---

    if ~is_continuous
        % epoched data set: return the number of epochs
        res = size(signal.data,3);
    else
        % continuous data set
        if ~isempty(signal.event) && isfield(signal.event,'target')
            targetmask = ~cellfun('isempty',{signal.event.target});
            if any(targetmask)
                % return the number of target events
                res = nnz(targetmask);
                exp_endfun; return;
            end
        end
        % otherwise return the number of samples
        res = size(signal.data,2);
    end

else
    % --- partition the data ---

    if ~is_continuous
        % set is epoched: strictly partition in terms of epochs
        res = exp_eval(set_selepos(signal,idxset));
    else
        % set is continuous
        if ~isempty(signal.event) && isfield(signal.event,'target')
            targetmask = ~cellfun('isempty',{signal.event.target});
            if any(targetmask)
                % ... and it has target events: strictly partition in terms of target events
                % this partitioning is perfectly consistent with what would happen if the data were epoched
                % (assuming that target markers with boundary-crossing epochs were already removed in
                %  set_targetmarkers, and assuming that the given epoch bounds are an upper bound of those
                %  used in set_makepos)
                if isempty(epoch_bounds) %#ok<NODEF>
                    disp_once('WARNING: Attempting to partition a continuous data set based on target events, but no EpochBounds argument is present. Assuming 1-sample epochs.');
                    ival = 0;
                else
                    % make sure that the epoch bounds include the event itself (and add a bit more slack
                    % to not lose it later even under resampling and rounding errors...)
                    epoch_bounds(1) = min(epoch_bounds(1),-0.25);
                    epoch_bounds(2) = max(epoch_bounds(2),+0.25);
                    ival = round(epoch_bounds(1)*signal.srate) : round(epoch_bounds(2)*signal.srate);
                end

                % check for sorting issues
                if ~issorted(idxset)
                    warn_once('Continuous-time data cannot reasonably be used with randomized partitioning; consider using chronological/blockwise partitioning! The index set will be sorted in the following.');
                    idxset = sort(idxset);
                end

                % build a logical mask that marks all retained intervals, starting with all false
                select_mask = false(1,size(signal.data,2));
                deselect_mask = false(1,size(signal.data,2));

                % find all target event latencies and the selected latencies
                if ~isfield(signal.event,'latency')
                    error('The given signal is lacking a .event.latency field.'); end
                latency_numels = cellfun('prodofsize',{signal.event.latency});
                if any(latency_numels == 0)
                    error('The signal contains events with an empty .latency field; this is not permitted.'); end
                if any(latency_numels ~= 1)
                    error('The signal contains events with a .latency value that is not a scalar; this is not permitted.'); end

                targlats = round([signal.event(targetmask).latency]);
                selected = targlats(idxset);

                % find the deselected latencies
                deselmask = true(size(targlats));
                deselmask(idxset) = false;
                deselected = targlats(deselmask);

                % add to the selected & deselected masks
                select_mask(min(length(select_mask),max(1,vec(bsxfun(@plus,ival,selected'))))) = true;
                deselect_mask(min(length(select_mask),max(1,vec(bsxfun(@plus,ival,deselected'))))) = true;

                % warn if the ranges overlap
                if any(select_mask & deselect_mask)
                    disp_once('Note: the epochs for some selected markers are overlapping with epochs of deselected markers. Make sure that you are using long enough safety margins if perfoming cross-validation.'); end

                % identify gaps in the select_mask
                gaps = find(diff([false select_mask false]));
                gaps = reshape(gaps(2:end-1),2,[])';
                gaps(:,2) = gaps(:,2)-1;

                % find all gaps that do not contain a deselected sample and fuse those...
                for g=1:size(gaps,1)
                    gap = gaps(g,1):gaps(g,2);
                    if ~any(deselect_mask(gap))
                        select_mask(gap) = true; end
                end

                % disable the now de-selected target events (settings their target flag back to empty)
                % (note that partitioning is strictly in terms of target markers)
                targetinds = find(targetmask);
                [signal.event(targetinds(deselmask)).target] = deal([]);

                % finally, re-extract the intervals and perform the actual partitioning
                retain_intervals = reshape(find(diff([false select_mask false])),2,[])';
                retain_intervals(:,2) = retain_intervals(:,2)-1;
                res = exp_eval(set_selinterval(signal,retain_intervals,'samples'));

                exp_endfun; return;
            end
        end

        % otherwise partition the raw data based on sample intervals

        % create a logical mask from the index set
        mask = false(1,size(signal.data,2)); mask(idxset) = true;
        % find the inclusive, 1-based intervals of non-zeros
        retain_intervals = reshape(find(diff([false mask false])),2,[])';
        retain_intervals(:,2) = retain_intervals(:,2)-1;
        res = exp_eval(set_selinterval(signal,retain_intervals,'samples'));
    end
end

exp_endfun;


% COPYRIGHT
% This file is part of ORCA: https://github.com/ayrna/orca
% Original authors: Pedro Antonio Gutiérrez, María Pérez Ortiz, Javier Sánchez Monedero
% Citation: If you use this code, please cite the associated paper http://www.uco.es/grupos/ayrna/orreview
% Copyright:
%     This software is released under the The GNU General Public License v3.0 licence
%     available at http://www.gnu.org/licenses/gpl-3.0.html
% RUN SINGLE TESTS
% This script runs single tests using the ORCA API. The toy dataset is used
% with a 1 -hold-out experimental. Reference performance is compared. To
% add an script just place it in the test/singletests folder following
% other examples
%
clear;
addpath Algorithms
addpath Measures
addpath Utils

tests_dir = ['tests' filesep 'singletests'];
eval(['addpath ' tests_dir])

files = dir([tests_dir filesep '*.m']);

% Load the different partitions of the dataset
load ../exampledata/1-holdout/toy/matlab/train_toy.0
load ../exampledata/1-holdout/toy/matlab/test_toy.0

% "patterns" refers to the input variables and targets to the output one
train.patterns = train_toy(:,1:end-1);
train.targets = train_toy(:,end);
test.patterns = test_toy(:,1:end-1);
test.targets = test_toy(:,end);

for i=1:length(files)
    % Clear (almost) all the variables to avoid issues with some mex files in Octave +4.2.2
    if (exist ('OCTAVE_VERSION', 'builtin') > 0)
      clear -x tests_dir files i train test
    else
		  clearvars -except tests_dir files i train test
    end
    fprintf('==============================================\n');
    cmd = files(i).name;
    fprintf('Processing %s...\n', cmd);
    eval(cmd(1:end-2))
end

% error is called in individual tests, so if code reach this point all the
% tests have been run successfully
fprintf('\nAll tests ended successfully\n')

rmpath Algorithms
rmpath Measures
rmpath Utils
eval(['rmpath ' tests_dir])


function[varargout]=ncload(varargin)
%NCLOAD  Load all variables from a NetCDF file and convert trajectories to cells.
%
%   NCLOAD(FILENAME) loads all variables from the NetCDF file FILENAME and
%   places them into a structure of the same name in the calling workspace.
%   The extension '.nc' in FILENAME is optional.
%
%   If FILENAME contains a full pathname, only the final portion after the
%   last '/' or '\' is used for the structure name.
%
%   Note that any hyphens '-' in the filename are replaced with underscores
%   in the structure name, as the former are not allowed in variable names.
%
%   For example, if FILENAME='/Home/data' contains variables 'num', 'lat',
%   and 'lon', then the result of calling NCLOAD('data') will be a
%   structure 'data' with fields 'data.num', 'data.lat', and 'data.lon'.
%
%   Then USE can be used to map these variables into the main workspace.
%
%   NCLOAD(FILENAME,VAR1,VAR2,...,VARN) will only load the variables with
%   the names VAR1, VAR2,...VARN.
%
%   NCLOAD will convert any variables that are numeric but not doubles to
%   doubles.  This is because experience has shown using other data types
%   in code that is expecting doubles can lead to errors in Matlab.
%   __________________________________________________________________
%
%   Convert trajectory data
%
%   In addition, if FILENAME has the global attribute
%
%         featureType    = 'trajectory'
%
%   then any variables in FILENAME are have 'Dimensions: obs' will be
%   interpreted as concatentated trajectory data, following the NetCDF
%   Climate and Forecast (CF) Metadata Conventions Appendix H.4.3
%
%         http://cfconventions.org/Data/cf-conventions/cf-conventions-1.8/cf-conventions.html
%
%   NCLOAD adopts the convention that a variable named 'ids', having
%   'Dimensions: obs', provides the id for all other variables.
%
%   All variables of dimension obs will be converted to cell arrays by
%   first calling COLBREAKS using ids as the first argument, then COL2CELL.
%
%   Alternatively, all variables with 'Dimensions: obs'  will be
%   interpreted as being in the NaN-separated column format used by
%   COL2CELL, which will be called to put then in cell array format. This
%   format is supported to provide for reverse compatibility; it is
%   recommended to use the CF trajectory convention instead.
%   __________________________________________________________________
%
%   The following formats also work:
%
%          ncload filename
%          ncload filename var1 var2 ... varN
%
%   In this format, the input strings have to be the actual names of the
%   file and variables, as opposed to variables containing those names.
%
%   This is basically a way to conveniently work with small NetCDF files as
%   if they were mat-files.  Typically, NetCDF files load much faster.
%
%   Usage: ncload(filename);
%          ncload filename
%          ncload(filename,'num','lat','lon');
%          ncload filename num lat lon
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2019--2020 J.M. Lilly --- type 'help jlab_license' for details

filename=varargin{1};
structname=filename;
ii=findstr(structname,'.');
if isempty(ii)
    filename=[filename '.nc'];
else
    structname=structname(1:ii-1);
end
index=findstr(structname,'/');
if ~isempty(index)
    structname=structname(index(end)+1:end);
end
index=findstr(structname,'\');
if ~isempty(index)
    structname=structname(index(end)+1:end);
end
structname(real(structname)==real('-'))='_';  %Replace minus signs with underscores

vars=varargin(2:end);
infostruct=ncinfo(filename);

%infostruct.Dimensions

for i=1:length(infostruct.Variables)
    allvars{i}=infostruct.Variables(i).Name;
    alldimensions{i}=infostruct.Variables(i).Dimensions.Name;
end

%alldimensions

dimensions=[];
if isempty(vars)
    vars=allvars;
    dimensions=alldimensions;
else
    for i=1:length(vars)
        for j=1:length(allvars)
            if strcmp(vars{i},allvars{j})
                 dimensions{i}=alldimensions{j};
            end
        end
    end
end

try
    attvalue=ncreadatt(filename,'/','featureType');
catch
    attvalue='nontrajectory';
end

convert=false;
if strcmp(attvalue,'trajectory')
    convert=true;
    isobs=false(length(dimensions),1);
    isids=false(length(dimensions),1);
    for i=1:length(isobs)
        isobs(i)=strcmp(dimensions{i},'obs');
        isids(i)=strcmp(vars{i},'ids');
    end
    if anyany(isobs)
        if ~anyany(isids)
            convert=false;
            disp('NCLOAD finding trajectory data but no "ids" variable, so not converting.')
        end
    end

end

if ~isempty(dimensions)
    for i=1:length(vars)
        if ~isempty(dimensions{i})
            %infostruct.Variables(i).Dimensions.Name
            if strcmp(dimensions{i},'column')
                %converting my older 'column' format
                str=[structname '.' vars{i} '=doubifnum_internal(col2cell_internal(ncread(''' filename ''',''' vars{i} ''')));'  ];
            else
                str=[structname '.' vars{i} '=doubifnum_internal(ncread(''' filename ''',''' vars{i} '''));'  ];
            end
            evalin('caller',str)
        end
    end
end

if convert
    strlist=[structname '.ids'];%make sure ids comes firsts
    for i=1:length(vars)
        if strcmp(dimensions{i},'obs')&&~strcmp(vars{i},'ids')
            strlist=[strlist ',' structname '.' vars{i} ];
        end
    end
    evalin('caller',['[' strlist ']=colbreaks_internal(' strlist ');'])
    evalin('caller',['[' strlist ']=col2cell_internal(' strlist ');'])
end

function[x]=doubifnum_internal(x)
%DOUBIFNUM  Converts a variable to double precision if it is numeric.
%
%   X=DOUBIFNUM
%
%   Usage: x=doubifnum(x);
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2020 J.M. Lilly --- type 'help jlab_license' for details

if isnumeric(x)
    x=double(x);
end

function[varargout]=colbreaks_internal(varargin)
%COLBREAKS  Insert NANs into discontinuties in a vector.
%
%   O1=COLBREAKS(I1), where I1 is a column vector, inserts NANs into I1 at
%   its discontinuities (and puts one at the end), for example:
%
%	  COLBREAKS([2;2;2;3;3])=[2;2;2;NAN;3;3;NAN];
%
%   [O1,O2,O3,...]=COLBREAKS(I1,I2,...), where I1,I2,... are all column
%   vectors of the same length, uses I1 as a reference for the other
%   vectors and inserts NANs into all the vectors at locations where I1 is
%   discontinous.
%
%   For instance, using station number for I1 will sort a column of CTD
%   data into a NAN-padded matrix.
%
%   Note that for complex-valued input arrays, a complex-valued NAN,
%   NAN+SQRT(-1)*NAN, is used to indicate the break locations.
%
%   MAT2=COLBREAKS(MAT1), where MAT1 and MAT2 are matrices of the same
%   size, also works.  In this case the first column of MAT1 is used as the
%   reference vector.
%
%   COLBREAKS, COL2MAT, and MAT2COL together form a system for moving data
%   with segments of nonuniform length rapidly back and forth between a
%   column format and a padded-matrix format. CTD or float data, for
%   instance, can be stored in the (usually much smaller) column format and
%   converted into the matrix format upon loading.
%
%   COLBREAKS(C1,C2,...); with no output arguments overwrites the original
%   input variables.
%
%   See also COL2MAT, MAT2COL, COL2CELL, CELL2COL, ORBITBREAKS.
%   _________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2000--2013 J.M. Lilly --- type 'help jlab_license' for details

bmat=0;
nargs=nargin;
ref=varargin{1};
if nargin==1 && size(ref,2)>1
    temp=ref;
    ref=temp(:,1);
    c=cell(size(temp,2),1);
    for i=1:size(temp,2)-1
        c{i}=temp(:,i+1);
    end
    nargs=size(temp,2);
    clear temp
    bmat=1;
else
    c=varargin(2:end);
end

ii=0;
index=find(diff(ref)~=0)+1;
bool=false(size(ref));
bool(index)=ones(size(index));
index=(1:length(ref))'+cumsum(bool);

while ii<nargs - 1
    ii=ii+1;
    col=c{ii};

    if isreal(col)
        colout=nan*ones(max(index),1);
        colout(index)=col;
        colout=[colout;nan];
    else
        colout=(nan+sqrt(-1)*nan)*ones(max(index),1);
        colout(index)=col;
        colout=[colout;(nan+sqrt(-1)*nan)];
    end

    varargout{ii+1}=colout;
end

if isreal(ref)
    refout=nan*ones(max(index),1);
    refout(index)=ref;
    refout=[refout;nan];
else
    refout=(nan+sqrt(-1)*nan)*ones(max(index),1);
    refout(index)=ref;
    refout=[refout;(nan+sqrt(-1)*nan)];
end

varargout{1}=refout;
if bmat
    for i=1:nargs-1
        varargout{1}(:,i+1)=varargout{i+1};
    end
    varargout=varargout(1);
end

function[varargout]=col2cell_internal(varargin)
%COL2CELL  Converts 'column-appended' data into cell arrays of numeric arrays.
%
%   X=COL2CELL(COL) converts the array COL, a 2D array having blocks of
%   data separated by rows of all NaNs, into a cell array, with each cell
%   containing one of the data blocks, excluding the trailing NaNs.
%
%   Since NANs mark the end of each block, missing or bad data in COL
%   should instead be indicated by INFs.
%
%   [X1,X2,...,XN]=COL2CELL(C1,C2,...,CN) also works, where the input
%   fields C1,...,CN are all the same size.  In this case the locations of
%   NANs in C1 are used to break all the other CN into cells.
%
%   COL2CELL(C1,C2,...,CN); with no output arguments overwrites the
%   original variables.
%
%   COL2CELL is inverted by CELL2COL.
%   __________________________________________________________________
%
%   See also CELL2COL, COL2MAT, MAT2COL, COLBREAKS.
%
%   'col2cell --t' runs a test.
%
%   Usage: x=col2cell(col);
%          [x1,x2,...,xN]=col2cell(c1,c2,...,cN);
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2008--2018 J.M. Lilly --- type 'help jlab_license' for details

i=0;
if ~isempty(varargin{1})
    while i<nargin
        i=i+1;
        col=varargin{i};
        if i==1
            %Note minor changes to make this work on matrices rather than
            %just columns.
            ia=[1;find(isnan(col(1:end-1,1)))+1];
            ib=find(isnan(col(:,1)))-1;
            %[L,ia,ib]=blocklen(col);
            ray=cell(length(ia),1);
        end
        for j=1:length(ia)
            if ~isempty(col)
                ray{j}=col(ia(j):ib(j),:);
            else
                ray{j}=[];
            end
        end
        varargout{i}=ray;
    end
else
    for i=1:nargout
        varargout{i}=varargin{1};
    end
end


function l4mat_print_some ( m, n, a, ilo, jlo, ihi, jhi, title )

%*****************************************************************************80
%
%% L4MAT_PRINT_SOME prints some of an L4MAT.
%
%  Discussion:
%
%    An L4MAT is an array of L4 values.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 November 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, logical A(M,N), an M by N matrix to be printed.
%
%    Input, integer ILO, JLO, the first row and column to print.
%
%    Input, integer IHI, JHI, the last row and column to print.
%
%    Input, string TITLE, a title.
%
  incx = 35;

  fprintf ( 1, '\n' );
  fprintf ( 1, '%s\n', title );

  for j2lo = max ( jlo, 1 ) : incx : min ( jhi, n )

    j2hi = j2lo + incx - 1;
    j2hi = min ( j2hi, n );
    j2hi = min ( j2hi, jhi );

    inc = j2hi + 1 - j2lo;

    fprintf ( 1, '\n' );

    if ( 100 <= j2hi )
      fprintf ( 1, '      ' );
      for j = j2lo : j2hi
        fprintf ( 1, ' %1d', floor ( j / 100 ) );
      end
      fprintf ( 1, '\n' );
    end

    if ( 10 <= j2hi )
      fprintf ( 1, '      ' );
      for j = j2lo : j2hi
        fprintf ( 1, ' %1d', mod ( floor ( j / 10 ), 10 ) );
      end
      fprintf ( 1, '\n' );
    end

    fprintf ( 1, '  Col ' );
    for j = j2lo : j2hi
      fprintf ( 1, ' %1d', mod ( j, 10 ) );
    end
    fprintf ( 1, '\n' );

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

    i2lo = max ( ilo, 1 );
    i2hi = min ( ihi, m );

    for i = i2lo : i2hi
      fprintf ( 1, '%5d:', i );
      for j = j2lo : j2hi
        fprintf ( 1, ' %1d', a(i,j) );
      end
      fprintf ( 1, '\n' );
    end

  end

  return
end


function f = imag(f)
%IMAG   Imaginary part of a CHEBTECH.
%   IMAG(F) is the imaginary part of F.
%
% See also REAL, ISREAL, CONJ.

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

% Compute imaginary part of the coefficients:
f.coeffs = imag(f.coeffs);

if ( ~any(f.coeffs(:)) )
    % Input was real, so output a zero CHEBTECH:
    f = f.make(zeros(1, size(f.coeffs, 2)));
    f.ishappy = 1;
end

end


%LINK Robot manipulator Link class
%
% A Link object holds all information related to a robot link such as
% kinematics parameters, rigid-body inertial parameters, motor and
% transmission parameters.
%
% Methods::
%  A             link transform matrix
%  RP            joint type: 'R' or 'P'
%  friction      friction force
%  nofriction    Link object with friction parameters set to zero
%  dyn           display link dynamic parameters
%  islimit       test if joint exceeds soft limit
%  isrevolute    test if joint is revolute
%  isprismatic   test if joint is prismatic
%  display       print the link parameters in human readable form
%  char          convert to string
%
% Properties (read/write)::
%
%  theta    kinematic: joint angle
%  d        kinematic: link offset
%  a        kinematic: link length
%  alpha    kinematic: link twist
%  sigma    kinematic: 0 if revolute, 1 if prismatic
%  mdh      kinematic: 0 if standard D&H, else 1
%  offset   kinematic: joint variable offset
%  qlim     kinematic: joint variable limits [min max]
%-
%  m        dynamic: link mass
%  r        dynamic: link COG wrt link coordinate frame 3x1
%  I        dynamic: link inertia matrix, symmetric 3x3, about link COG.
%  B        dynamic: link viscous friction (motor referred)
%  Tc       dynamic: link Coulomb friction
%-
%  G        actuator: gear ratio
%  Jm       actuator: motor inertia (motor referred)
%
% Examples::
%
%         L = Link([0 1.2 0.3 pi/2]);
%         L = Link('revolute', 'd', 1.2, 'a', 0.3, 'alpha', pi/2);
%         L = Revolute('d', 1.2, 'a', 0.3, 'alpha', pi/2);
%
% Notes::
% - This is a reference class object.
% - Link objects can be used in vectors and arrays.
%
% References::
% - Robotics, Vision & Control, Chap 7,
%   P. Corke, Springer 2011.
%
% See also Link, Revolute, Prismatic, SerialLink, RevoluteMDH, PrismaticMDH.

% Copyright (C) 1993-2015, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

classdef Link < handle
    properties
        % kinematic parameters
        theta % kinematic: link angle
        d     % kinematic: link offset
        alpha % kinematic: link twist
        a     % kinematic: link length
        sigma % revolute=0, prismatic=1
        mdh   % standard DH=0, MDH=1
        offset % joint coordinate offset
        name   % joint coordinate name
        flip   % joint moves in opposite direction

        % dynamic parameters
        m  % dynamic: link mass
        r  % dynamic: position of COM with respect to link frame (3x1)
        I  % dynamic: inertia of link with respect to COM (3x3)
        Jm % dynamic: motor inertia
        B  % dynamic: motor viscous friction (1x1 or 2x1)

        Tc % dynamic: motor Coulomb friction (1x2 or 2x1)
        G  % dynamic: gear ratio
        qlim % joint coordinate limits (2x1)
    end

    methods
        function l = Link(varargin)
            %LINK Create robot link object
            %
            % This the class constructor which has several call signatures.
            %
            % L = Link() is a Link object with default parameters.
            %
            % L = Link(LNK) is a Link object that is a deep copy of the link
            % object LNK.
            %
            % L = Link(OPTIONS) is a link object with the kinematic and dynamic
            % parameters specified by the key/value pairs.
            %
            % Options::
            % 'theta',TH    joint angle, if not specified joint is revolute
            % 'd',D         joint extension, if not specified joint is prismatic
            % 'a',A         joint offset (default 0)
            % 'alpha',A     joint twist (default 0)
            % 'standard'    defined using standard D&H parameters (default).
            % 'modified'    defined using modified D&H parameters.
            % 'offset',O    joint variable offset (default 0)
            % 'qlim',L      joint limit (default [])
            % 'I',I         link inertia matrix (3x1, 6x1 or 3x3)
            % 'r',R         link centre of gravity (3x1)
            % 'm',M         link mass (1x1)
            % 'G',G         motor gear ratio (default 1)
            % 'B',B         joint friction, motor referenced (default 0)
            % 'Jm',J        motor inertia, motor referenced (default 0)
            % 'Tc',T        Coulomb friction, motor referenced (1x1 or 2x1), (default [0 0])
            % 'revolute'    for a revolute joint (default)
            % 'prismatic'   for a prismatic joint 'p'
            % 'standard'    for standard D&H parameters (default).
            % 'modified'    for modified D&H parameters.
            % 'sym'         consider all parameter values as symbolic not numeric
            %
            % - It is an error to specify both 'theta' and 'd'
            % - The link inertia matrix (3x3) is symmetric and can be specified by giving
            %   a 3x3 matrix, the diagonal elements [Ixx Iyy Izz], or the moments and products
            %   of inertia [Ixx Iyy Izz Ixy Iyz Ixz].
            % - All friction quantities are referenced to the motor not the load.
            % - Gear ratio is used only to convert motor referenced quantities such as
            %   friction and interia to the link frame.
            %
            % Old syntax::
            % L = Link(DH, OPTIONS) is a link object using the specified kinematic
            % convention  and with parameters:
            %  - DH = [THETA D A ALPHA SIGMA OFFSET] where OFFSET is a constant displacement
            %    between the user joint angle vector and the true kinematic solution.
            %  - DH = [THETA D A ALPHA SIGMA] where SIGMA=0 for a revolute and 1 for a
            %    prismatic joint, OFFSET is zero.
            %  - DH = [THETA D A ALPHA], joint is assumed revolute and OFFSET is zero.
            %
            % Options::
            %
            % 'standard'    for standard D&H parameters (default).
            % 'modified'    for modified D&H parameters.
            % 'revolute'    for a revolute joint, can be abbreviated to 'r' (default)
            % 'prismatic'   for a prismatic joint, can be abbreviated to 'p'
            %
            % Examples::
            % A standard Denavit-Hartenberg link
            %        L3 = Link('d', 0.15005, 'a', 0.0203, 'alpha', -pi/2);
            % since 'theta' is not specified the joint is assumed to be revolute, and
            % since the kinematic convention is not specified it is assumed 'standard'.
            %
            % Using the old syntax
            %        L3 = Link([ 0, 0.15005, 0.0203, -pi/2], 'standard');
            % the flag 'standard' is not strictly necessary but adds clarity.  Only 4 parameters
            % are specified so sigma is assumed to be zero, ie. the joint is revolute.
            %
            %        L3 = Link([ 0, 0.15005, 0.0203, -pi/2, 0], 'standard');
            % the flag 'standard' is not strictly necessary but adds clarity.  5 parameters
            % are specified and sigma is set to zero, ie. the joint is revolute.
            %
            %        L3 = Link([ 0, 0.15005, 0.0203, -pi/2, 1], 'standard');
            % the flag 'standard' is not strictly necessary but adds clarity.  5 parameters
            % are specified and sigma is set to one, ie. the joint is prismatic.
            %
            % For a modified Denavit-Hartenberg revolute joint
            %        L3 = Link([ 0, 0.15005, 0.0203, -pi/2, 0], 'modified');
            %
            % Notes::
            % - Link object is a reference object, a subclass of Handle object.
            % - Link objects can be used in vectors and arrays.
            % - The parameter D is unused in a revolute joint, it is simply a placeholder
            %   in the vector and the value given is ignored.
            % - The parameter THETA is unused in a prismatic joint, it is simply a placeholder
            %   in the vector and the value given is ignored.
            % - The joint offset is a constant added to the joint angle variable before
            %   forward kinematics and subtracted after inverse kinematics.  It is useful
            %   if you  want the robot to adopt a 'sensible' pose for zero joint angle
            %   configuration.
            % - The link dynamic (inertial and motor) parameters are all set to
            %   zero.  These must be set by explicitly assigning the object
            %   properties: m, r, I, Jm, B, Tc.
            % - The gear ratio is set to 1 by default, meaning that motor friction and
            %   inertia will be considered if they are non-zero.
            %
            % See also Revolute, Prismatic.

            %TODO eliminate legacy dyn matrix

            if nargin == 0
                % create an 'empty' Link object
                % this call signature is needed to support arrays of Links

                %% kinematic parameters
                l.alpha = 0;
                l.a = 0;
                l.theta = 0;
                l.d = 0;
                l.sigma = 0;
                l.mdh = 0;
                l.offset = 0;
                l.flip = false;
                l.qlim = [];

                %% dynamic parameters
                % these parameters must be set by the user if dynamics is used
                l.m = 0;
                l.r = [0 0 0];
                l.I = zeros(3,3);

                % dynamic params with default (zero friction)
                l.Jm = 0;
                l.G = 1;
                l.B = 0;
                l.Tc = [0 0];
            elseif nargin == 1 && isa(varargin{1}, 'Link')
                % clone the passed Link object
                l = copy(varargin{1});

            else
                % Create a new Link based on parameters

                % parse all possible options
                opt.theta = [];
                opt.a = 0;
                opt.d = [];
                opt.alpha = 0;
                opt.G = 0;
                opt.B = 0;
                opt.Tc = [0 0];
                opt.Jm = 0;
                opt.I = zeros(3,3);
                opt.m = 0;
                opt.r = [0 0 0];
                opt.offset = 0;
                opt.qlim = [];
                opt.type = {'revolute', 'prismatic', 'fixed'};
                opt.convention = {'standard', 'modified'};
                opt.sym = false;
                opt.flip = false;

                [opt,args] = tb_optparse(opt, varargin);

                % return a parameter as a number of symbol depending on
                % the 'sym' option

                if isempty(args)
                    % This is the new call format, where all parameters are
                    % given by key/value pairs
                    %
                    % eg. L3 = Link('d', 0.15005, 'a', 0.0203, 'alpha', -pi/2);
                    if ~isempty(opt.theta)
                        % constant value of theta means it must be prismatic
                        l.theta = value( opt.theta, opt);
                        l.sigma = 1;
                    end

                    if ~isempty(opt.d)
                        % constant value of d means it must be revolute
                        l.d = value( opt.d, opt);
                        l.sigma = 0;
                    end
                    if ~isempty(opt.d) && ~isempty(opt.theta)
                        error('RTB:Link:badarg', 'specify only one of ''d'' or ''theta''');
                    end

                    l.a =     value( opt.a, opt);
                    l.alpha = value( opt.alpha, opt);

                    l.offset = value( opt.offset, opt);
                    l.flip = value( opt.flip, opt);

                    l.qlim =   value( opt.qlim, opt);

                    l.m = value( opt.m, opt);
                    l.r = value( opt.r, opt);
                    l.I = value( opt.I, opt);
                    l.Jm = value( opt.Jm, opt);
                    l.G = value( opt.G, opt);
                    l.B = value( opt.B, opt);
                    l.Tc = value( opt.Tc, opt);
                else
                    % This is the old call format, where all parameters are
                    % given by a vector containing kinematic-only, or
                    % kinematic plus dynamic parameters
                    %
                    % eg. L3 = Link([ 0, 0.15005, 0.0203, -pi/2, 0], 'standard');
                    dh = args{1};
                    if length(dh) < 4
                        error('must provide params (theta d a alpha)');
                    end

                    % set the kinematic parameters
                    l.theta = dh(1);
                    l.d = dh(2);
                    l.a = dh(3);
                    l.alpha = dh(4);

                    l.sigma = 0;
                    l.offset = 0;
                    l.flip = false;
                    l.mdh = 0;  % default to standard D&H

                    % optionally set sigma and offset
                    if length(dh) >= 5
                        l.sigma = dh(5);
                    end
                    if length(dh) == 6
                        l.offset = dh(6);
                    end

                    if length(dh) > 6
                        % legacy DYN matrix

                        l.sigma = dh(5);
                        l.mdh = 0;  % default to standard D&H
                        l.offset = 0;

                        % it's a legacy DYN matrix
                        l.m = dh(6);
                        l.r = dh(7:9).';     % a column vector
                        v = dh(10:15);
                        l.I = [ v(1) v(4) v(6)
                            v(4) v(2) v(5)
                            v(6) v(5) v(3)];
                        if length(dh) > 15
                            l.Jm = dh(16);
                        end
                        if length(dh) > 16
                            l.G = dh(17);
                        else
                            l.G = 1;
                        end
                        if length(dh) > 17
                            l.B = dh(18);
                        else
                            l.B = 0.0;
                        end
                        if length(dh) > 18
                            l.Tc = dh(19:20);
                        else
                            l.Tc = [0 0];
                        end
                        l.qlim = [];
                    else
                        % we know nothing about the dynamics
                        l.m = [];
                        l.r = [];
                        l.I = [];
                        l.Jm = [];
                        l.G = 0;
                        l.B = 0;
                        l.Tc = [0 0];
                        l.qlim = [];
                    end
                end

                % set the kinematic convention to be used
                if strcmp(opt.convention, 'modified')
                    l.mdh = 1;
                else
                    l.mdh = 0;
                end

            end

            function out = value(v, opt)
                if opt.sym
                    out = sym(v);
                else
                    out = v;
                end
            end

        end % link()

        function  tau = friction(l, qd)
            %Link.friction Joint friction force
            %
            % F = L.friction(QD) is the joint friction force/torque for link velocity QD.
            %
            % Notes::
            % - The returned friction value is referred to the output of the gearbox.
            % - The friction parameters in the Link object are referred to the motor.
            % - Motor viscous friction is scaled up by G^2.
            % - Motor Coulomb friction is scaled up by G.
            % - The appropriate Coulomb friction value to use in the non-symmetric case
            %   depends on the sign of the joint velocity, not the motor velocity.

            tau = l.B * abs(l.G) * qd;

            if issym(l)
                tau = tau + l.Tc;
            elseif qd > 0
                tau = tau + l.Tc(1);
            elseif qd < 0
                tau = tau + l.Tc(2);
            end
            tau = -abs(l.G) * tau;     % friction opposes motion
        end % friction()

        function tau = friction2(l, qd)

                    % experimental code
            qdm = qd / l.G;
            taum = -l.B * qdm;
            if qdm  > 0
                taum = taum - l.Tc(1);
            elseif qdm < 0
                taum = taum - l.Tc(2);
            end
            tau = taum * l.G;
        end

        function  l2 = nofriction(l, only)
            %Link.nofriction Remove friction
            %
            % LN = L.nofriction() is a link object with the same parameters as L except
            % nonlinear (Coulomb) friction parameter is zero.
            %
            % LN = L.nofriction('all') as above except that viscous and Coulomb friction
            % are set to zero.
            %
            % LN = L.nofriction('coulomb') as above except that Coulomb friction is set to zero.
            %
            % LN = L.nofriction('viscous') as above except that viscous friction is set to zero.
            %
            % Notes::
            % - Forward dynamic simulation can be very slow with finite Coulomb friction.
            %
            % See also SerialLink.nofriction, SerialLink.fdyn.

            l2 = Link(l);

            if nargin == 1
                only = 'coulomb';
            end

            switch only
                case 'all'
                    l2.B = 0;
                    l2.Tc = [0 0];
                case 'viscous'
                    l2.B = 0;
                case 'coulomb'
                    l2.Tc = [0 0];
            end
        end

        function v = RP(l)
            %Link.RP Joint type
            %
            % c = L.RP() is a character 'R' or 'P' depending on whether joint is revolute or
            % prismatic respectively.
            % If L is a vector of Link objects return a string of characters in joint order.
            v = '';
            for ll=l
                if ll.sigma == 0
                    v = strcat(v, 'R');
                else
                    v = strcat(v, 'P');
                end
            end
        end % RP()

        function set.r(l, v)
            %Link.r Set centre of gravity
            %
            % L.r = R sets the link centre of gravity (COG) to R (3-vector).
            %
            if isempty(v)
                return;
            end
            if length(v) ~= 3
                error('RTB:Link:badarg', 'COG must be a 3-vector');
            end
            l.r = v(:).';
        end % set.r()

        function set.Tc(l, v)
            %Link.Tc Set Coulomb friction
            %
            % L.Tc = F sets Coulomb friction parameters to [F -F], for a symmetric
            % Coulomb friction model.
            %
            % L.Tc = [FP FM] sets Coulomb friction to [FP FM], for an asymmetric
            % Coulomb friction model. FP>0 and FM<0.  FP is applied for a positive
            % joint velocity and FM for a negative joint velocity.
            %
            % Notes::
            % - The friction parameters are defined as being positive for a positive
            %   joint velocity, the friction force computed by Link.friction uses the
            %   negative of the friction parameter, that is, the force opposing motion of
            %   the joint.
            %
            % See also Link.friction.
            if isempty(v)
                return;
            end

            if isa(v,'sym') && ~isempty(findsym(v))
                l.Tc = sym('Tc');
            elseif isa(v,'sym') && isempty(findsym(v))
                v = double(v);
            end

            if length(v) == 1  ~isa(v,'sym')
                l.Tc = [v -v];
            elseif length(v) == 2 && ~isa(v,'sym')
                if v(1) < v(2)
                    error('RTB:Link:badarg', 'Coulomb friction is [Tc+ Tc-]');
                end
                l.Tc = v;
            else
                error('Coulomb friction vector can have 1 (symmetric) or 2 (asymmetric) elements only')
            end
        end % set.Tc()

        function set.I(l, v)
            %Link.I Set link inertia
            %
            % L.I = [Ixx Iyy Izz] sets link inertia to a diagonal matrix.
            %
            % L.I = [Ixx Iyy Izz Ixy Iyz Ixz] sets link inertia to a symmetric matrix with
            % specified inertia and product of intertia elements.
            %
            % L.I = M set Link inertia matrix to M (3x3) which must be symmetric.
            if isempty(v)
                return;
            end
            if all(size(v) == [3 3])
                if isa(v, 'double') && norm(v-v') > eps
                    error('inertia matrix must be symmetric');
                end
                l.I = v;
            elseif length(v) == 3
                l.I = diag(v);
            elseif length(v) == 6
                l.I = [ v(1) v(4) v(6)
                    v(4) v(2) v(5)
                    v(6) v(5) v(3) ];
            else
                error('RTB:Link:badarg', 'must set I to 3-vector, 6-vector or symmetric 3x3');
            end
        end % set.I()

        function v = islimit(l, q)
            %Link.islimit  Test joint limits
            %
            % L.islimit(Q) is true (1) if Q is outside the soft limits set for this joint.
            %
            % Note::
            % - The limits are not currently used by any Toolbox functions.
            if isempty(l.qlim)
                error('no limits assigned to link')
            end
            v = (q > l.qlim(2)) - (q < l.qlim(1));
        end % islimit()

        function v = isrevolute(L)
            %Link.isrevolute  Test if joint is revolute
            %
            % L.isrevolute() is true (1) if joint is revolute.
            %
            % See also Link.isprismatic.

            v = [L.sigma] == 0;
        end

        function v = isprismatic(L)
            %Link.isprismatic  Test if joint is prismatic
            %
            % L.isprismatic() is true (1) if joint is prismatic.
            %
            % See also Link.isrevolute.
            v = [L.sigma] == 1;
        end

        function T = A(L, q)
            %Link.A Link transform matrix
            %
            % T = L.A(Q) is the link homogeneous transformation matrix (4x4) corresponding
            % to the link variable Q which is either the Denavit-Hartenberg parameter THETA (revolute)
            % or D (prismatic).
            %
            % Notes::
            % - For a revolute joint the THETA parameter of the link is ignored, and Q used instead.
            % - For a prismatic joint the D parameter of the link is ignored, and Q used instead.
            % - The link offset parameter is added to Q before computation of the transformation matrix.
            sa = sin(L.alpha); ca = cos(L.alpha);
            if L.flip
                q = -q + L.offset;
            else
                q = q + L.offset;
            end
            if L.sigma == 0
                % revolute
                st = sin(q); ct = cos(q);
                d = L.d;
            else
                % prismatic
                st = sin(L.theta); ct = cos(L.theta);
                d = q;
            end

            if L.mdh == 0
                % standard DH

                T =    [    ct  -st*ca  st*sa   L.a*ct
                    st  ct*ca   -ct*sa  L.a*st
                    0   sa      ca      d
                    0   0       0       1];
            else
                % modified DH

                T =    [    ct      -st     0   L.a
                    st*ca   ct*ca   -sa -sa*d
                    st*sa   ct*sa   ca  ca*d
                    0       0       0   1];
            end

        end % A()

        function display(l)
            %Link.display Display parameters
            %
            % L.display() displays the link parameters in compact single line format.  If L is a
            % vector of Link objects displays one line per element.
            %
            % Notes::
            % - This method is invoked implicitly at the command line when the result
            %   of an expression is a Link object and the command has no trailing
            %   semicolon.
            %
            % See also Link.char, Link.dyn, SerialLink.showlink.
            loose = strcmp( get(0, 'FormatSpacing'), 'loose');
            if loose
                disp(' ');
            end
            disp([inputname(1), ' = '])
            disp( char(l) );
        end % display()

        function s = char(links, from_robot)
            %Link.char Convert to string
            %
            % s = L.char() is a string showing link parameters in a compact single line format.
            % If L is a vector of Link objects return a string with one line per Link.
            %
            % See also Link.display.

            % display in the order theta d a alpha
            if nargin < 2
                from_robot = false;
            end

            s = '';

            for j=1:length(links)
                l = links(j);

                conv = l.RP;
                if l.mdh == 0
                    conv = strcat(conv, ',stdDH');
                else
                    conv = strcat(conv, ',modDH');
                end
                if length(links) == 1
                    qname = 'q';
                else
                    qname = sprintf('q%d', j);
                end

                if from_robot
                    % invoked from SerialLink.char method, format for table
                    if l.sigma == 1
                        % prismatic joint
                        js = sprintf('|%3d|%11s|%11s|%11s|%11s|%11s|', ...
                            j, ...
                            render(l.theta), ...
                            qname, ...
                            render(l.a), ...
                            render(l.alpha), ...
                            render(l.offset));
                    else
                        % revolute joint
                        js = sprintf('|%3d|%11s|%11s|%11s|%11s|%11s|', ...
                            j, ...
                            qname, ...
                            render(l.d), ...
                            render(l.a), ...
                            render(l.alpha), ...
                            render(l.offset));
                    end
                else

                    if l.sigma == 1
                        % prismatic joint
                        js = sprintf(' theta=%s, d=%s, a=%s, alpha=%s, offset=%s (%s)', ...
                            render(l.theta), ...
                            qname, ...
                            render(l.a), ...
                            render(l.alpha), ...
                            render(l.offset), ...
                            conv);
                    else
                        % revolute
                        js = sprintf(' theta=%s, d=%s, a=%s, alpha=%s, offset=%s (%s)', ...
                            qname, ...
                            render(l.d), ...
                            render(l.a), ...
                            render(l.alpha), ...
                            render(l.offset), ...
                            conv);
                    end
                end
                if isempty(s)
                    s = js;
                else
                    s = char(s, js);
                end
            end

        end % char()

        function dyn(links)
            %Link.dyn Show inertial properties of link
            %
            % L.dyn() displays the inertial properties of the link object in a multi-line
            % format. The properties shown are mass, centre of mass, inertia, friction,
            % gear ratio and motor properties.
            %
            % If L is a vector of Link objects show properties for each link.
            %
            % See also SerialLink.dyn.

            for j=1:numel(links)
                l = links(j);
                if numel(links) > 1
                    fprintf('\nLink %d::', j);
                end
                fprintf('%s\n', l.char());
                if ~isempty(l.m)
                    fprintf('  m    = %s\n', render(l.m))
                end
                if ~isempty(l.r)
                    s = render(l.r);
                    fprintf('  r    = %s %s %s\n', s{:});
                end
                if ~isempty(l.I)
                    s = render(l.I(1,:));
                    fprintf('  I    = | %s %s %s |\n', s{:});
                    s = render(l.I(2,:));
                    fprintf('         | %s %s %s |\n', s{:});
                    s = render(l.I(3,:));
                    fprintf('         | %s %s %s |\n', s{:});
                end
                if ~isempty(l.Jm)
                    fprintf('  Jm   = %s\n', render(l.Jm));
                end
                if ~isempty(l.B)
                    fprintf('  Bm   = %s\n', render(l.B));
                end
                if ~isempty(l.Tc)
                    fprintf('  Tc   = %s(+) %s(-)\n', ...
                        render(l.Tc(1)), render(l.Tc(2)));
                end
                if ~isempty(l.G)
                    fprintf('  G    = %s\n', render(l.G));
                end
                if ~isempty(l.qlim)
                    fprintf('  qlim = %f to %f\n', l.qlim(1), l.qlim(2));
                end
            end
        end % dyn()

        % Make a copy of a handle object.
        % http://www.mathworks.com/matlabcentral/newsreader/view_thread/257925
        function new = copy(this)

            for j=1:length(this)
                % Instantiate new object of the same class.
                %new(j) = feval(class(this(j)));
                new(j) = Link();
                % Copy all non-hidden properties.
                p = properties(this(j));
                for i = 1:length(p)
                    new(j).(p{i}) = this(j).(p{i});
                end
            end
        end

        function links = horzcat(this, varargin)
            links = this.toLink();

            for i=1:length(varargin)
                links = cat(2, links, varargin{i}.toLink());
            end
        end

        function links = vertcat(this, varargin)
            links = this.horzcat(varargin{:});
        end

        function new = toLink(this)
            for j=1:length(this)
                new(j) = Link();

                % Copy all non-hidden properties.
                p = properties(this(j));
                for i = 1:length(p)
                    new(j).(p{i}) = this(j).(p{i});
                end
            end

        end

        function res = issym(l)
            %Link.issym Check if link is a symbolic model
            %
            % res = L.issym() is true if the Link L has symbolic parameters.

            res = isa(l.alpha,'sym');
        end

        function sl = sym(l)

            sl = Link(l);   % clone the link

            if ~isempty(sl.theta)
                sl.theta = sym(sl.theta);
            end
            if ~isempty(sl.d)
                sl.d = sym(sl.d);
            end
            sl.alpha = sym(sl.alpha);
            sl.a = sym(sl.a);
            sl.offset = sym(sl.offset);

            sl.I = sym(sl.I);
            sl.r = sym(sl.r);
            sl.m = sym(sl.m);

            sl.Jm = sym(sl.Jm);
            sl.G = sym(sl.G);
            sl.B = sym(sl.B);
            sl.Tc = sym(sl.Tc);

        end
    end % methods

end % class

function s = render(v, fmt)
    if nargin < 2
        fmt = '%11.4g';
    end
    if length(v) == 1
                if isa(v, 'double')
            s = sprintf(fmt, v);
        elseif isa(v, 'sym')
            s = char(v);
        else
            error('RTB:Link:badarg', 'Link parameter must be numeric or symbolic');
        end
    else

    for i=1:length(v)
        if isa(v, 'double')
            s{i} = sprintf(fmt, v(i));
        elseif isa(v, 'sym')
            s{i} = char(v(i));
        else
            error('RTB:Link:badarg', 'Link parameter must be numeric or symbolic');
        end
    end
    end
end


function b = isnull(x,eps)
% ckeck double == 0

if nargin == 1, eps = 1e-14; end

b = abs(x)<=eps;


function val = get(f, propName)
%GET   GET method for CHEBFUN3T class.
%   P = GET(F, PROP) returns the property P specified in the string PROP
%   from the CHEBFUN F. Valid entries for the string PROP are:
%    'COEFFS'
%    'VSCALE'
%    'DOMAIN'

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

% Loop through an array of CHEBFUN3 objects.
if ( numel(f) > 1 )
    val = cell(numel(f));
    for k = 1:numel(f)
        val{k} = get(f(k), propName);
    end
    return
end

% Get the properties.
switch ( propName )
    case 'coeffs'
        val = f.coeffs;
    case 'vscale'
        val = f.vscale;
    case 'domain'
        val = f.domain;
    otherwise
        error('CHEBFUN:CHEBFUN3T:get:propName', ...
            [propName,' is not a valid CHEBFUN3T property.'])
end


function x = cvx_basis( x )
x = sparse( reshape( x, 1, numel( x ) ) );

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


function h = imagesc(DEM,varargin)

%IMAGESC Scale data in GRIDobj and display as image object
%
% Syntax
%
%     h = imagesc(DEM,varargin)
%
% Description
%
%     see imagesc
%
%
% Author: Wolfgang Schwanghart (w.schwanghart[at]geo.uni-potsdam.de)
% Date: 8. January, 2013

[x,y] = refmat2XY(DEM.refmat,DEM.size);
ht = imagesc(x,y,DEM.Z,varargin{:});

axis xy
axis image

if nargout == 1;
    h = ht;
end


function [ word, done ] = word_next_read ( s, done )

%*****************************************************************************80
%
%% WORD_NEXT_READ "reads" words from a string, one at a time.
%
%  Special cases:
%
%    The following characters are considered to be a single word,
%    whether surrounded by spaces or not:
%
%      " ( ) { } [ ]
%
%    Also, if there is a trailing comma on the word, it is stripped off.
%    This is to facilitate the reading of lists.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 September 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S, a string, presumably containing words
%    separated by spaces.
%
%    Input, logical DONE.
%    TRUE, if we are beginning a new string;
%    FALSE, if we are continuing to process the current string.
%
%    Output, string WORD.
%    If DONE is FALSE, then WORD contains the "next" word read.
%    If DONE is TRUE, then WORD is blank, because there was no more to read.
%
%    Output, logical DONE.
%      FALSE if another word was read,
%      TRUE if no more words could be read.
%
  persistent lenc;
  persistent next;

  tab = char ( 9 );
%
%  We "remember" LENC and NEXT from the previous call.
%
%  An input value of DONE = TRUE signals a new line of text to examine.
%
  if ( done )

    next = 1;
    done = 0;
    lenc = s_len_trim ( s );

    if ( lenc <= 0 )
      done = 1;
      word = ' ';
      return
    end

  end
%
%  Beginning at index NEXT, search the string for the next nonblank,
%  which signals the beginning of a word.
%
  ilo = next;
%
%  ...S(NEXT:) is blank.  Return with WORD = ' ' and DONE = TRUE.
%
  while ( 1 )

    if ( lenc < ilo )
      word = ' ';
      done = 1;
      next = lenc + 1;
      return
    end
%
%  If the current character is blank, skip to the next one.
%
    if ( s(ilo) ~= ' ' & s(ilo) ~= tab )
      break
    end

    ilo = ilo + 1;

  end
%
%  ILO is the index of the next nonblank character in the string.
%
%  If this initial nonblank is a special character,
%  then that's the whole word as far as we're concerned,
%  so return immediately.
%
  if ( s(ilo) == '"' | ...
       s(ilo) == '(' | ...
       s(ilo) == ')' | ...
       s(ilo) == '{' | ...
       s(ilo) == '}' | ...
       s(ilo) == '[' | ...
       s(ilo) == ']' )

    word = s(ilo);
    next = ilo + 1;
    return

  end
%
%  Now search for the last contiguous character that is not a
%  blank, TAB, or special character.
%
  next = ilo + 1;

  while ( next <= lenc )

    if ( s(next) == ' ' )
      break;
    elseif ( s(next) == tab )
      break;
    elseif ( s(next) == '"' )
      break;
    elseif ( s(next) == '(' )
      break;
    elseif ( s(next) == ')' )
      break;
    elseif ( s(next) == '{' )
      break;
    elseif ( s(next) == '}' )
      break;
    elseif ( s(next) == '[' )
      break;
    elseif ( s(next) == ']' )
      break;
    end

    next = next + 1;

  end

  if ( s(next-1) == ',' )
    word = s(ilo:next-2);
  else
    word = s(ilo:next-1);
  end

  return
end


function y = apply_multiple_ouput(f, g, x)

% apply_multiple_ouput - compose multiple arguments functions
%
%   y = apply_multiple_ouput(f, g, x);
%
% compute y=f(g(x)) where f takes several input and g computes several outputs.
%
%   Copyright (c) 2010 Gabriel Peyre

if nargin(f)>=0 & nargout(g)>=0 & not(nargout(g)==nargin(f))
    error('one must have nargout(g)==nargin(f)');
end

switch nargin(f)
    case 1
        [a{1}] = g(x);
        y = f(a{1});
    case 2
        [a{1}, a{2}] = g(x);
        y = f(a{1}, a{2});
    case 3
        [a{1}, a{2}, a{3}] = g(x);
        y = f(a{1}, a{2}, a{3});
    case 4
        [a{1}, a{2}, a{3}, a{4}] = g(x);
        y = f(a{1}, a{2}, a{3}, a{4});
    otherwise
        error('Not implemented');
end


function exp = exp_not(exp)
% Negate the given expression.
% Output = Not(Input)
%
% In:
%   Input : An expression
%
% Out:
%   Output : The given expression, negated
%
%                                Christian Kothe, Swartz Center for Computational Neuroscience, UCSD
%                                2010-05-22

if ~exp_beginfun('default','fingerprint_check',0,'fingerprint_create',0,'memoize',0) return; end

exp = ~exp;

exp_endfun;


function [imageVol, lastfile] = GE_readVolume(baseFileName, volSize, depth, im_offset)
%
%GE_readVolume
%
% [imageVol, lastfile] = GE_readVolume(baseFileName, [nX nY nZ], depth, im_offset)
%
% reads the volume for passnum from the series which is stored
% starting in startDir and returns the name of the last file read
%
% Souheil J. Inati
% Dartmouth College
% May 2000
% souheil.inati@dartmouth.edu
%
% 2002.01.24 RFD: code needs comments! Apparently, this code assumes a particular dir
%   structure involving numbered 'run' directories. I simplified things by just assuming
%   all files are in the same dir.
%

% initialize some variables
nX = volSize(1);
nY = volSize(2);
nZ = volSize(3);
sliceSize = nX*nY;
imageVol = zeros(nX, nY, nZ);

for i = 1:nZ
    imNum = sprintf('.%.3d', i);
    imageFile = fullfile('', [baseFileName,imNum]);
    % Open the file
    [fid,message] = fopen(imageFile,'r','b');
    if (fid == -1)
        fprintf('Cannot Open %s.\n',imageFile);
        break
    end

    % Skip to the data
    fseek(fid,im_offset,-1);
    % Read the slice
    buffer = fread(fid,sliceSize,sprintf('int%d',depth));
    % append the slice to the imageSet
    imageVol(:,:,i) = reshape(buffer, nX, nY);

    % Close the file
    status = fclose(fid);

    % Set the lastfile
    lastfile = imageFile;
end

return


function varargout = ismember(varargin)
% (1) To make Octave's `ismember` behaviour match recent Matlab's: the second
% output gives the index of the first occurrence, not the last
% (2) Octave gives error when the first two inputs are char and
% numeric, whereas Matlab works. So I include here conversion from char to
% double in that case.
% (3) Octave's ismember treats complex inputs as their absolute value,
% which is incorrect (see https://savannah.gnu.org/bugs/index.php?52437).
% This seems to have been caused by a "reimplementation" of ismember. So if
% any input is complex we use unique to replace the inputs by unique labels
% (3):
if (isnumeric(varargin{1})&&~isreal(varargin{1})) || (isnumeric(varargin{2})&&~isreal(varargin{2}))
    a = varargin{1};
    b = varargin{2};
    [~, ~, u] = unique([a(:); b(:)]);
    a(:) = real(u(1:numel(a)));
    b(:) = real(u(numel(a)+1:end));
    varargin{1} = a;
    varargin{2} = b;
end
% (2):
if ischar(varargin{1})&&isnumeric(varargin{2})
   varargin{1} = double(varargin{1});
end
if isnumeric(varargin{1})&&ischar(varargin{2})
   varargin{2} = double(varargin{2});
end
% (1):
if nargout==1 % single output: no interception needed
    varargout{1} = builtin('ismember', varargin{:});
elseif nargin>=3 && strcmp(varargin{3},'rows') % two outputs, 'rows' case
    [aim, bim] = builtin('ismember', varargin{1}, varargin{2}(end:-1:1,:), 'rows');
    bim(aim) = size(varargin{2},1)+1-bim(aim);
    varargout = {aim, bim};
else % two outputs, no 'rows'
    [aim, bim] = builtin('ismember', varargin{1}, varargin{2}(end:-1:1));
    bim(aim) = numel(varargin{2})+1-bim(aim);
    varargout = {aim, bim};
end
end


function [model,rxnNames] = addDemandReactionOri(model,metaboliteNameList, addRxnGeneMat)
% addDemandReaction adds demand reactions for a set of metabolites
% The reaction names for the demand reactions will be DM_[metaboliteName]
%
% model = addDemandReaction(model,metaboliteNameList)
%
% INPUTS
% model                 COBRA model structure
% metaboliteNameList    List of metabolite names (cell array)
% addRxnGeneMat         Adds rxnGeneMat to model structure (default = true)
%
% OUTPUTS
% model                 COBRA model structure with added demand reactions
% rxnNames              List of added reactions
%
% Markus Herrgard 5/8/07
% Ines Thiele 03/09 - Corrected reaction coefficient for demand reaction
% Ines Thiele 08/03/2015, made rxnGeneMat optional
if ~exist('addRxnGeneMat','var')
    addRxnGeneMat = 1;
end

if (~iscell(metaboliteNameList))
    tmp = metaboliteNameList;
    clear metaboliteNameList;
    metaboliteNameList{1} = tmp;
end

for i = 1:length(metaboliteNameList)
    rxnName = ['DM_' metaboliteNameList{i}];
    rxnNames{i}=rxnName;
    metaboliteList = {metaboliteNameList{i}};
%     [model,rxnIDexists] = addReaction(model,rxnName,metaboliteList,stoichCoeffList,revFlag,lowerBound,upperBound,objCoeff,subSystem,grRule,geneNameList,systNameList,checkDuplicate, addRxnGeneMat)
    model = addReactionOri(model,rxnName,metaboliteList,-1,false,0,1000,0,'Demand','',[],[],0, addRxnGeneMat);
end


function vis_PlaneAxisProperties(ax,lattice,msize,pos)

% VIS_PLANEAXISPROPERTIES Set axis properties for SOM_CPLANE,
%                         SOM_PIEPLANE, SOM_BARPLANE and SOM_PLOTPLANE.
%
% vis_PlaneAxisProperties(ax,lattice,msize,pos)
%
%  Input arguments:
%   ax        (scalar) axis handle
%   lattice   (string) 'hexa', 'rect', 'hexaU' or 'rectU'
%             (matrix) defines the patch, see e.g. help vis_patch
%   msize     (vector) a 1x2 vector defining the grid size
%   pos       (vector) a 1x2 vector that determines position of
%                      origin or NaN which means default operation:
%                      origin to [1 1] and tighten axis limits
%                      according to the grid size.
%
% This is a subfunction for SOM_CPLANE, SOM_PIEPLANE, SOM_BARPLANE and
% SOM_PLOTPLANE. This subfunction sets the proper values for axis.

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

% Version 2.0beta Johan 060799

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

xdim=msize(1);ydim=msize(2);
set(ax,'Visible','off');
set(get(ax,'Title'),'Visible','on');
set(ax,'XaxisLocation','Top');            % axis orientation
set(ax,'xdir','normal');                  % = axis ij = matrix mode
set(ax,'ydir','reverse');

switch lattice
case {'rect', 'rectU'}
  lelim=-.51; rilim=.51; uplim=-.51; lolim=.51;  % axis limits
  set(ax,'DataAspectRatio', [1 1 1]);            % =axis equal
case {'hexa','hexaU'}
  lelim=-.51; rilim=1.01; uplim=-.67; lolim=.67; % axis limits
  set(ax,'DataAspectRatio',[0.9015 1 1]);        % this corrects hexagons
end

% Nan: default origin [1 1] & tighten the axis
if isnan(pos)
  set(ax,'XLim',[1+lelim ydim+rilim],'YLim',[1+uplim xdim+lolim], ...
      'XLimMode','manual','YLimMode','manual'); % tighten the axis
end