function [solution, LPProblem] = solveCobraLPCPLEX(LPProblem, printLevel, basisReuse, conflictResolve, contFunctName, minNorm, interface)
% Calls CPLEX to solve an LP problem
% By default, use the matlab interface to cplex written by TOMLAB, in
% preference to the one written by ILOG.
%
% USAGE:
%
%    [solution, LPProblem] = solveCobraLPCPLEX(LPProblem, printLevel, basisReuse, conflictResolve, contFunctName, minNorm, interface)
%
% INPUT:
%
%    LPProblem:          Structure containing the following fields describing the LP problem to be solved
%
%                          * .A - LHS matrix
%                          * .b - RHS vector
%                          * .c - Objective coeff vector
%                          * .lb - Lower bound vector
%                          * .ub - Upper bound vector
%                          * .osense - Objective sense (-1 max, +1 min)
%                          * .rxns - (optional) cell array of reaction abbreviations (necessary for
%                            making a readable confilict resolution file).
%                          * .csense - (optional) Constraint senses, a string containting the constraint sense for
%                            each row in `A` ('E', equality, 'G' greater than, 'L' less than).
%                          * .LPBasis - (optional) Basis from previous solution of similar LP problem.
%                            See `basisReuse`
% OPTIONAL INPUTS:
%    printLevel:         Printing level in the CPLEX m-file and CPLEX C-interface.
%
%                          * 0 - Silent
%                          * 1 - Warnings and Errors
%                          * 2 - Summary information (Default)
%                          * 3 - More detailed information
%                          * > 10 - Pause statements, and maximal printing (debug mode)
%    basisReuse:         0 - Use this for one of solution of an LP (Default);
%                        1 - Returns a basis for reuse in the next LP i.e. outputs `model.LPBasis`
%    conflictResolve:    0 (Default);
%                        1 If LP problem is proven to be infeasible by CPLEX,
%                        it will print out a 'conflict resolution file',
%                        which indicates the irreducible infeasible set of
%                        equaltiy & inequality constraints that together,
%                        combine to make the problem infeasible. This is
%                        useful for debugging an LP problem if you want to
%                        try to resolve a constraint conflict
%    contFunctName:      structure or function with parameters (only for `tomlab_cplex` or `ILOGcomplex`)
%
%                        - when using the `tomlab_cplex` interface
%
%                              1. contFunctName = [] Use all default CLPEX control parameters, (Default);
%                              2. contFunctName = someString e.g. 'someFunctionName'
%                                 uses the user specified control parameters defined
%                                 in `someFunctionName.m` (see template function CPLEXParamSet for details).
%                              3. contFunctName = `cpxControl` structure (output from a file like `CPLEXParamSet.m`)
%
%                        - when using the `ILOGcomplex` interface (parameter structure for Cplex). The full set of parameters can be obtained by calling `Cplex().Param`. For example:
%
%                              - `[solverParams.simplex.display, solverParams.tune.display, solverParams.barrier.display, solverParams.sifting.display, solverParams.conflict.display] = deal(0);`
%                              - `[solverParams.simplex.tolerances.optimality, solverParams.simplex.tolerances.feasibility] = deal(1e-9, 1e-8);`
%
%    minNorm:            {(0), 1 , `n x 1` vector} If not zero then, minimise the Euclidean length
%                        of the solution to the LP problem. Gives the same objective,
%                        but minimises the square of flux. `minNorm` ~1e-6 should be
%                        high enough for regularisation yet keep the same objective
%    interface:          {'ILOGcomplex', 'ILOGsimple', 'tomlab_cplex'}
%                        Default is the `tomlab_cplex` interface
%
% OUTPUT:
%    solution:           Structure containing the following fields describing a LP solution:
%
%                          * .full:               Full LP solution vector
%                          * .obj:                Objective value
%                          * .rcost:              Lagrangian multipliers to the simple inequalties (Reduced costs)
%                          * .dual:               Lagrangian multipliers to the equalities
%                          * .nInfeas:            Number of infeasible constraints
%                          * .sumInfeas:          Sum of constraint violation
%                          * .stat:               COBRA Standardized solver status code:
%
%                            * 1 - Optimal solution
%                            * 2 - Unbounded solution
%                            * 0 - Infeasible
%                            * -1 - No solution reported (timelimit, numerical problem etc)
%                          * .origStat:           CPLEX status code. Use `cplexStatus(solution.origStat)` for more information from the CPLEX solver
%                          * .solver              solver used by `cplex`
%                          * .time                time taken to solve the optimization problemtime taken to solve the optimization problem
%
% OPTIONAL OUTPUT:
%    LPProblem:          with field:
%
%                          * .LPBasis:            When input `basisReuse = 1`, we return a basis for reuse in the next LP
%
% CPLEX consists of 4 different LP solvers which can be used to solve sysbio optimization problems
% you can control which of the solvers, e.g. simplex vs interior point solver using the
% CPLEX control parameter cpxControl.LPMETHOD. At the moment, the solver is
% automatically chosen for you
%
% .. Note:
%       ILOG CPLEX parameters
%       https://www.ibm.com/support/knowledgecenter/SSSA5P_12.6.3/ilog.odms.studio.help/pdf/paramcplex.pdf
%
%       TOMLAB Cplex parameters
%       http://tomwiki.com/CPLEX_Parameter_Table
%
% .. Author: - Ronan Fleming

if ~exist('printLevel','var')
    printLevel=0;
end
if ~exist('basisReuse','var')
    basisReuse=0;
end
if ~exist('conflictResolve','var')
    conflictResolve=0;
end
if ~exist('interface','var')
    interface='tomlab_cplex';
end
if strcmp(interface,'tomlab_cplex')
    if ~exist('contFunctName','var')
        cpxControl=[];
    else
        if isstruct(contFunctName)
            cpxControl=contFunctName;
        else
            if ~isempty(contFunctName)
                %calls a user specified function to create a CPLEX control structure
                %specific to the users problem. A TEMPLATE for one such function is
                %CPLEXParamSet
                cpxControl=eval(contFunctName);
            else
                cpxControl=[];
            end
        end
    end
end
if ~exist('minNorm','var')
    minNorm=0;
end

if basisReuse
    if isfield(LPProblem, 'LPBasis')
        basis = LPProblem.LPBasis;
        % use advanced starting information when optimization is initiated.
        cpxControl.advance = 1;
        cpxControl.ADVIND = 1;
    else
        basis=[];
    end
else
    basis=[];
    % do not use advanced starting information when optimization is initiated.
    cpxControl.advance = 0;
    cpxControl.ADVIND = 0;
end

if ~isfield(LPProblem,'A')
    if ~isfield(LPProblem,'S')
        error('Equality constraint matrix must either be a field denoted A or S.')
    end
    LPProblem.A=LPProblem.S;
end

if ~isfield(LPProblem,'csense')
    nMet=size(LPProblem.A);
    if printLevel>0
        fprintf('%s\n','Assuming equality constraints, i.e. S*v=b');
    end
    %assuming equality constraints
    LPProblem.csense(1:nMet,1)='E';
end

if ~isfield(LPProblem,'osense')
    %assuming maximisation
    LPProblem.osense=-1;
    if printLevel>0
        fprintf('%s\n','Assuming maximisation of objective');
    end
end

if size(LPProblem.A,2)~=length(LPProblem.c)
    error('dimensions of A & c are inconsistent');
end

if size(LPProblem.A,2)~=length(LPProblem.lb) || size(LPProblem.A,2)~=length(LPProblem.ub)
    error('dimensions of A & bounds are inconsistent');
end

%get data
[c,x_L,x_U,b,csense,osense] = deal(LPProblem.c,LPProblem.lb,LPProblem.ub,LPProblem.b,LPProblem.csense,LPProblem.osense);
%modify objective to correspond to osense
c=full(c*osense);

%cplex expects it dense
b=full(b);
%Conflict groups descriptor (cpxBuildConflict can be used to generate the input). Set this if
%conflict refinement is desired in the case that infeasibility is detected
%by CPLEX.
if conflictResolve
    % set to deterministic mode to get reproducible conflict resolve file
    if (isfield(cpxControl,'PARALLEL') && cpxControl.PARALLEL ~=1) || (isfield(cpxControl,'PARALLELMODE') && cpxControl.PARALLELMODE ~=1)
        fprintf('PARALLEL / PARALLELMODE Parameter was changed to 1 to ensure a reproducible log file\n');
        cpxControl.PARALLEL = 1;
        cpxControl.PARALLELMODE = 1;
    end
    [m_lin,n]=size(LPProblem.A);
    m_quad=0;
    m_sos=0;
    m_log=0;
    %determines how elaborate the output is
    mode='full';%'minimal';
    fprintf('%s\n%s\n','Building Structure for Conflict Resolution...','...this slows CPLEX down so should not be used for repeated LP');
    confgrps = cpxBuildConflict(n,m_lin,m_quad,m_sos,m_log,mode);
    prefix=pwd;
    suffix='LP_CPLEX_conflict_file.txt';
    conflictFile=[prefix filesep suffix];
else
    confgrps=[]; conflictFile=[];
end

%Name of file to write the CPLEX log information to. If empty, no log is
%written.
logfile=[];

%Name of a file to save the CPLEX problem object (Used for submitting
%possible bugs in CPLEX to ILOG)
savefile=[]; savemode=[];
% savefile='C:\CPLEX_possible_bug.txt';

% vector defining which callbacks to use in CPLEX. If the ith entry of the logical vector
% callback is set, the corresponding callback is defined. The callback calls the m-file specified
% in Table 7 below. The user may edit this file, or make a new copy, which is put in a directory
% that is searched before the cplex directory in the Matlab path.
callback=[]; %I'm not really sure what this option means as yet

%this is not a tomlab problem so this is not needed
Prob=[];

% variables not used in LP problems
IntVars=[]; PI=[]; SC=[]; SI=[]; sos1=[]; sos2=[];

%quadratic constraint matrix, size n x n
if sum(minNorm)~=0
    if length(minNorm)==1
        % same weighting of min norm for all variables
        F=speye(length(c))*minNorm;
    else
        if length(minNorm)~=length(c)
            error('Either minNorm is a scalar, or is an n x 1 vector')
        else
            % individual weighting of min norm for all variables
            F=spdiags(minNorm,0,length(c),length(c));
        end
    end
else
    F=[];
end
%Structure array defining quadratic constraints
qc=[];

%Structure telling whether and how you want CPLEX to perform a sensitivity analysis (SA).
%This may be useful in future but probably will have more meaning with an
%additional term in the objective
saRequest =[];

%Vector with MIP starting solution, if known
xIP=[];

%Logical constraints, i.e. an additional set of single-sided linear constraints that are controlled
%by a binary variable (switch) in the problem
logcon=[];

%Report of incompatibility R2016b - ILOGcomplex interface
verMATLAB = version('-release');
if str2num(verMATLAB(1:end-1)) >= 2016 && strcmp(interface, 'ILOGcomplex')
    error(['MATLAB ',verMATLAB, ' and the ILOGcomplex interface are not compatible. Select ILOGsimple or tomlab_cplex as a CPLEX interface.'])
end

%call cplex
tic;
switch interface
    case 'ILOGcomplex'
        %complex ibm ilog cplex interface
        if ~isempty(csense)
            %set up constant vectors for CPLEX
            b_L(csense == 'E',1) = b(csense == 'E');
            b_U(csense == 'E',1) = b(csense == 'E');
            b_L(csense == 'G',1) = b(csense == 'G');
            b_U(csense == 'G',1) = Inf;
            b_L(csense == 'L',1) = -Inf;
            b_U(csense == 'L',1) = b(csense == 'L');
        else
            b_L = b;
            b_U = b;
        end

        % Initialize the CPLEX object
        try
            ILOGcplex = Cplex('fba');
        catch ME
            error('CPLEX not installed or licence server not up')
        end

        ILOGcplex.Model.sense = 'minimize';

        % Now populate the problem with the data
        ILOGcplex.Model.obj   = c;
        ILOGcplex.Model.lb    = x_L;
        ILOGcplex.Model.ub    = x_U;
        ILOGcplex.Model.A     = LPProblem.A;
        ILOGcplex.Model.lhs   = b_L;
        ILOGcplex.Model.rhs   = b_U;

        if ~isempty(F)
            %quadratic constraint matrix, size n x n
            ILOGcplex.Model.Q=F;
        end

        if ~exist('contFunctName','var')
            cpxControl= struct();
        else
            %Read ILOG cplex parameters
            ILOGcplex = setCplexParam(ILOGcplex, cpxControl, printLevel);
        end

        if printLevel==0
            ILOGcplex.DisplayFunc=[];
        else
            %print level
            ILOGcplex.Param.barrier.display.Cur = printLevel;
            ILOGcplex.Param.simplex.display.Cur = printLevel;
            ILOGcplex.Param.sifting.display.Cur = printLevel;
        end

        % Optimize the problem
        ILOGcplex.solve();
        %http://www-01.ibm.com/support/knowledgecenter/SSSA5P_12.2.0/ilog.odms.cplex.help/Content/Optimization/Documentation/CPLEX/_pubskel/CPLEX1210.html
        if ILOGcplex.Solution.status == 1
            solution.obj        = osense*ILOGcplex.Solution.objval;
            solution.full       = ILOGcplex.Solution.x;
            solution.rcost      = ILOGcplex.Solution.reducedcost;
            solution.dual       = ILOGcplex.Solution.dual;
            solution.nInfeas    = NaN;
            solution.sumInfeas  = NaN;
            %solution.stat       = ILOGcplex.Solution.
            solution.origStat   = ILOGcplex.Solution.status;
            solution.solver     = ILOGcplex.Solution.method;
            solution.time       = ILOGcplex.Solution.time;
            solution.kappa      = ILOGcplex.Solution.quality.kappa.value;
        else
            warning(['IBM CPLEX STATUS = ' int2str(ILOGcplex.Solution.status) ', see: http://www-01.ibm.com/support/knowledgecenter/SSSA5P_12.2.0/ilog.odms.cplex.help/Content/Optimization/Documentation/CPLEX/_pubskel/CPLEX1210.html'])
            solution.origStat   = ILOGcplex.Solution.status;
            solution.full       = NaN;
            solution.obj        = NaN;
            solution.rcost      = NaN;
            solution.dual       = NaN;
            solution.nInfeas    = NaN;
            solution.sumInfeas  = NaN;
            solution.solver     = NaN;
            solution.time       = NaN;
        end
    case 'ILOGsimple'
        try
            ILOGcplex = Cplex('fba');
        catch ME
            error('CPLEX not installed or licence server not up')
        end
        %simple ibm ilog cplex interface
        options = cplexoptimset;
        if printLevel == 0
            options = cplexoptimset(options,'Display','off');
        else
            options = cplexoptimset(options,'Display','on');
        end

        if ~isempty(csense)
            if sum(minNorm)~=0
                Aineq = [LPProblem.A(csense == 'L',:); - LPProblem.A(csense == 'G',:)];
                bineq = [b(csense == 'L',:); - b(csense == 'G',:)];
                %             min      0.5*x'*H*x+f*x or f*x
                %             st.      Aineq*x     <= bineq
                %             Aeq*x    = beq
                %             lb <= x <= ub
                [x,fval,exitflag,output,lambda] = cplexqp(F,c,Aineq,bineq,LPProblem.A(csense == 'E',:),b(csense == 'E',1),x_L,x_U,[],options);
            else
                Aineq = [LPProblem.A(csense == 'L',:); - LPProblem.A(csense == 'G',:)];
                bineq = [b(csense == 'L',:); - b(csense == 'G',:)];
                %        min      c*x
                %        st.      Aineq*x <= bineq
                %                 Aeq*x    = beq
                %                 lb <= x <= ub
                [x,fval,exitflag,output,lambda] = cplexlp(c,Aineq,bineq,LPProblem.A(csense == 'E',:),b(csense == 'E',1),x_L,x_U,[],options);
            end
            %primal
            solution.obj=osense*fval;
            solution.full=x;
            %this is the dual to the equality constraints but it's not the chemical potential
            solution.dual=lambda.eqlin;
        else
            Aineq=[];
            bineq=[];
            if sum(minNorm)~=0
                [x,fval,exitflag,output,lambda] = cplexqp(F,c,Aineq,bineq,LPProblem.A,b,x_L,x_U,[],options);
            else
                [x,fval,exitflag,output,lambda] = cplexlp(c,Aineq,bineq,LPProblem.A,b,x_L,x_U,[],options);
            end
            solution.obj=osense*fval;
            solution.full=x;
            %this is the dual to the equality constraints but it's not the chemical potential
            solution.dual=sparse(size(LPProblem.A,1),1);
            solution.dual(csense == 'E')=lambda.eqlin;
            %this is the dual to the inequality constraints but it's not the chemical potential
            solution.dual(csense == 'L')=lambda.ineqlin(1:nnz(csense == 'L'),1);
            solution.dual(csense == 'G')=lambda.ineqlin(nnz(csense == 'L')+1:end,1);
        end
        %this is the dual to the simple ineequality constraints : reduced costs
        solution.rcost=lambda.lower-lambda.upper;
        solution.nInfeas = [];
        solution.sumInfeas = [];
        solution.origStat = output.cplexstatus;
    case 'tomlab_cplex'
        %tomlab cplex interface
        if ~isempty(csense)
            %set up constant vectors for CPLEX
            b_L(csense == 'E',1) = b(csense == 'E');
            b_U(csense == 'E',1) = b(csense == 'E');
            b_L(csense == 'G',1) = b(csense == 'G');
            b_U(csense == 'G',1) = Inf;
            b_L(csense == 'L',1) = -Inf;
            b_U(csense == 'L',1) = b(csense == 'L');
        else
            b_L = b;
            b_U = b;
        end

        %tomlab cplex interface
        %   minimize   0.5 * x'*F*x + c'x     subject to:
        %      x             x_L <=    x   <= x_U
        %                    b_L <=   Ax   <= b_U
        [x, slack, v, rc, f_k, ninf, sinf, Inform, basis] = cplex(c, LPProblem.A, x_L, x_U, b_L, b_U, ...
            cpxControl, callback, printLevel, Prob, IntVars, PI, SC, SI, ...
            sos1, sos2, F, logfile, savefile, savemode, qc, ...
            confgrps, conflictFile, saRequest, basis, xIP, logcon);

        solution.full=x;
        %this is the dual to the equality constraints but it's not the chemical potential
        solution.dual=v*osense;%negative sign Jan 25th
        %this is the dual to the simple ineequality constraints : reduced costs
        solution.rcost=rc*osense;%negative sign Jan 25th
        if Inform~=1
            solution.obj = NaN;
        else
            if minNorm==0
                solution.obj=f_k*osense;
            else
                solution.obj=c'*x*osense;
            end
            %     solution.obj
            %     norm(x)
        end
        solution.nInfeas = ninf;
        solution.sumInfeas = sinf;
        solution.origStat = Inform;
    otherwise
        error([interface ' is not a recognised solveCobraLPCPLEX interface'])
end
solution.time=toc;
Inform = solution.origStat;

if Inform~=1 && conflictResolve ==1
    switch interface
        case {'ILOGcomplex','ILOGsimple'}
            if isfield(LPProblem,'mets') && isfield(LPProblem,'rxns')
                %this code reads the conflict resolution file and replaces the
                %arbitrary names with the abbreviations of metabolites and reactions
                [nMet,nRxn]=size(LPProblem.A);
                totAbbr=nMet+nRxn;
                conStrFind=cell(nMet+nRxn,1);
                conStrReplace=cell(nMet+nRxn,1);
                %only equality constraint rows
                for m=1:nMet
                    conStrFind{m,1}=['c' int2str(m) ':'];
                    conStrReplace{m,1}=[LPProblem.mets{m} ':  '];
                end
                %reactions
                for n=1:nRxn
                    conStrFind{nMet+n,1}=['x' int2str(n) ' '];
                    conStrReplace{nMet+n,1}=[LPProblem.rxns{n} ' '];
                end
                fid1 = fopen(suffix);
                fid2 = fopen(['COBRA_' suffix], 'w');
                while ~feof(fid1)
                    tline{1}=fgetl(fid1);
                    %replaces all occurrences of the string str2 within string str1
                    %with the string str3.
                    %str= strrep(str1, str2, str3)
                    for t=1:totAbbr
                        tline= strrep(tline, conStrFind{t}, conStrReplace{t});
                    end
                    fprintf(fid2,'%s\n', tline{1});
                end
                fclose(fid1);
                fclose(fid2);
                %delete other file without replacements
                %         delete(suffix)
                fprintf('%s\n',['Conflict resolution file written to: ' prefix '\COBRA_' suffix]);
                fprintf('%s\n%s\n','The Conflict resolution file gives an irreducible infeasible subset ','of constraints which are making this LP Problem infeasible');
            else
                warning('Need reaction and metabolite abbreviations in order to make a readable conflict resolution file');
            end
    end
else
    if printLevel>0 && Inform~=1
        fprintf('%s\n','No conflict resolution file. Consider to set conflictResolve = 1 next time.');
    end
end

if strcmp(interface, 'tomlab_cplex')
    % Try to give back COBRA Standardized solver status:
    %           1   Optimal solution
    %           2   Unbounded solution
    %           0   Infeasible
    %           -1  No solution reported (timelimit, numerical problem etc)
    if Inform==1
        solution.stat = 1;
        if printLevel>0
            %use tomlab code to print out exit meassage
            [ExitText,ExitFlag] = cplexStatus(Inform);
            solution.ExitText=ExitText;
            solution.ExitFlag=ExitFlag;
            fprintf('\n%s%g\n',[ExitText ', Objective '],  c'*solution.full*osense);
        end
    else
        if Inform==2
            solution.stat = 2;
            %use tomlab code to print out exit meassage
            [ExitText,ExitFlag] = cplexStatus(Inform);
            solution.ExitText=ExitText;
            solution.ExitFlag=ExitFlag;
            fprintf('\n%s%g\n',[ExitText ', Objective '],  c'*solution.full*osense);
        else
            if Inform==3
                solution.stat = 0;
            else
                %this is a conservative view
                solution.stat = -1;
                %use tomlab code to print out exit meassage
                [ExitText,ExitFlag] = cplexStatus(Inform);
                solution.ExitText=ExitText;
                solution.ExitFlag=ExitFlag;
                fprintf('\n%s%g\n',[ExitText ', Objective '],  c'*solution.full*osense);
            end
        end
    end
end

%return basis
if basisReuse
    LPProblem.LPBasis=basis;
end

if sum(minNorm)~=0
    fprintf('%s\n','This objective corresponds to a flux with minimum Euclidean norm.');
    fprintf('%s%d%s\n','The weighting for minimising the norm was ',minNorm,'.');
    fprintf('%s\n','Check that the objective is the same without minimising the norm.');
end


classdef ConicalSensor < AbstractSensor
    %ConicalSensor Summary of this class goes here
    %   Detailed explanation goes here

    properties
        name(1,:) char = 'Untitled Sensor';

        %Initial sensor properties (can be changed in sensor state later)
        angle(1,1) double = deg2rad(10) %rad
        range(1,1) double = 1000;       %km
        origin AbstractGeometricPoint
        steeringModel AbstractSensorSteeringModel
        initActiveTf(1,1) logical = true;

        %LVD Data
        lvdData LvdData

        %drawing properties
        color(1,1) ColorSpecEnum = ColorSpecEnum.Green;
        alpha(1,1) double = 0.3;
        showMeshEdges(1,1) logical = false;
    end

    properties(Constant)
        typeEnum = SensorEnum.ConicalSensor;
    end

    methods
        function obj = ConicalSensor(name, angle, range, origin, steeringModel, lvdData)
            arguments
                name(1,:) char
                angle(1,1) double {mustBeGreaterThanOrEqual(angle,0)}
                range(1,1) double {mustBeGreaterThan(range, 0)}
                origin(1,1) AbstractGeometricPoint
                steeringModel(1,1) AbstractSensorSteeringModel
                lvdData(1,1) LvdData
            end

            if(angle > pi/2)
                angle = pi/2;
            end

            obj.name = name;
            obj.angle = angle;
            obj.range = range;
            obj.origin = origin;
            obj.steeringModel = steeringModel;
            obj.lvdData = lvdData;
        end

        function [V,F] = getSensorMesh(obj, sensorState, scElem, dcm, inFrame)
            arguments
                obj(1,1) ConicalSensor
                sensorState(1,1) ConicalSensorState
                scElem(1,1) CartesianElementSet
                dcm(3,3) double
                inFrame(1,1) AbstractReferenceFrame
            end

            active = sensorState.getSensorActiveState();
            if(active)
                sensorSteering = sensorState.getSensorSteeringMode();
                time = scElem.time;
                sensorRange = sensorState.getSensorMaxRange();
                sensorAngle = sensorState.getSensorAngle();

%                 theta = linspace(pi/2, pi/2 + sensorAngle, 10);
%                 xPts = sensorRange*cos(theta);
%                 yPts = sensorRange*sin(theta);
%
%                 xPts = [0, 0, xPts];
%                 yPts = [0, sensorRange/2, yPts];
%
%                 v = normVector([xPts(end); yPts(end)]);
%                 xPts = [xPts 0.5*v(1)*sensorRange];
%                 yPts = [yPts 0.5*v(2)*sensorRange];
%
%                 xPts = [xPts, 0];
%                 yPts = [yPts, 0];
%
%                 PV = [xPts(:), yPts(:)];
%
%                 [X,Y,Z] = revolutionSurface(PV, 15, [0,0, 0, 1]);
%                 fvc = surf2patch(X,Y,Z);
%                 fvc.faces = triangulateFaces(fvc.faces);

                rVectSensorOrigin = obj.getOriginInFrame(time, scElem, inFrame);

                MM = sensorSteering.getSensorDcmToInertial(time, scElem, dcm, inFrame);
                r = rotm2axang(MM*roty(90));
                M = makehgtform('translate',rVectSensorOrigin(:)', 'axisrotate',r(1:3),r(4));

%                 fvc = transformPoint3d(fvc, M);
%                 [V, F] = meshVertexClustering(fvc, 1);

                S = [0,0,0, sensorRange];
                sphere = sphereMesh(S, 'nTheta', ceil(5*2*pi/sensorAngle), 'nPhi', 16);
                sPtsRaw = unique(sphere.vertices,'rows');
                sPtsAngs = dang(repmat([0;0;1],1,size(sPtsRaw,1)), sPtsRaw');
                sPts = sPtsRaw(sPtsAngs <= sensorAngle+1E-10, :);

                sPts2 = transformPoint3d(sPts(:,1), sPts(:,2), sPts(:,3), M);

                V = vertcat(rVectSensorOrigin(:)', sPts2);
                F = convhull(V);

                [V, F] = meshVertexClustering(V,F, 0.001);
            else
                V = [];
                F = [];
            end
        end

        function boreDir = getSensorBoresightDirection(~, sensorState, time, scElem, dcm, inFrame)
            boreDir = sensorState.getSensorSteeringMode().getBoresightVector(time, scElem, dcm, inFrame);
        end

        function sensorDcm = getSensorDcmToInertial(obj, sensorState, scElem, dcm, inFrame)
            time = scElem.time;
            sensorDcm = sensorState.getSensorSteeringMode().getSensorDcmToInertial(time, scElem, dcm, inFrame);
        end

        function rVectOrigin = getOriginInFrame(obj, time, scElem, inFrame)
            newCartElem = obj.origin.getPositionAtTime(time, scElem, inFrame);
            rVectOrigin = [newCartElem.rVect];
        end

        function tf = isVehDependent(obj, sensorState)
            tf = obj.origin.isVehDependent() || sensorState.getSensorSteeringMode().isVehDependent();
        end

        function listboxStr = getListboxStr(obj)
            listboxStr = obj.name;
        end

        function color = getMeshColor(obj)
            color = obj.color;
        end

        function alpha = getMeshAlpha(obj)
            alpha = obj.alpha;
        end

        function tf = getDisplayMeshEdges(obj)
            tf = obj.showMeshEdges;
        end

        function name = getName(obj)
            name = obj.name;
        end

        function tf = isInUse(obj, lvdData)
            tf = lvdData.usesSensor(obj);
        end

        function useTf = openEditDialog(obj)
            output = AppDesignerGUIOutput({false});
            lvd_EditConicalSensorGUI_App(obj, obj.lvdData, output);
            useTf = output.output{1};
        end

        function state = getInitialState(obj)
            state = ConicalSensorState(obj, obj.initActiveTf, obj.steeringModel, obj.angle, obj.range);
        end

        function tf = usesGeometricPoint(obj, point)
            tf = obj.origin == point;
        end
    end
end


%run this test case with the command
%results = runtests('fillGapsSmallTests.m')
function tests = fillGapsSmallTests
tests = functiontests(localfunctions);
end

function testSmallGlpk(testCase)
%Test using small model
sourceDir = fileparts(which(mfilename));
load([sourceDir,'/test_data/ecoli_textbook.mat'], 'model');
modelDB=model; % Keep as database with reactions
try
    oldSolver=getpref('RAVEN','solver');
catch
end
setRavenSolver('glpk');

%Remove first 10 reactions
model=removeReactions(modelDB,(1:10));
modelDB.id='DB';
try
    evalc('[newConnected,cannotConnect,addedRxns,model,exitFlag]=fillGaps(model,modelDB)');
catch
    try
        setRavenSolver(oldSolver);
    catch
        rmpref('RAVEN','solver');
    end
    error('Solver not working')
end
sol=solveLP(model);
try
    setRavenSolver(oldSolver);
catch
    rmpref('RAVEN','solver');
end
%Should give non-zero flux
verifyTrue(testCase,-sol.f>0);
end

function testSmallGurobi(testCase)
if exist('gurobi','file')~=3
    error('Gurobi not installed or cannot be found in MATLAB path, test skipped')
end
%Test using small model
sourceDir = fileparts(which(mfilename));
load([sourceDir,'/test_data/ecoli_textbook.mat'], 'model');
modelDB=model; % Keep as database with reactions
try
    oldSolver=getpref('RAVEN','solver');
catch
end
setRavenSolver('gurobi');

%Remove first 10 reactions
model=removeReactions(modelDB,(1:10));

modelDB.id='DB';
try
    evalc('[newConnected,cannotConnect,addedRxns,model,exitFlag]=fillGaps(model,modelDB)');
catch
    try
        setRavenSolver(oldSolver);
    catch
        rmpref('RAVEN','solver');
    end
    error('Solver not working')
end
sol=solveLP(model);
try
    setRavenSolver(oldSolver);
catch
    rmpref('RAVEN','solver');
end
%Expect at least 5% of the original growth
verifyTrue(testCase,-sol.f>0);
end

function testSmallCobra(testCase)
if exist('initCobraToolbox.m','file')~=2
    error('COBRA Toolbox not installed or cannot be found in MATLAB path, test skipped')
end
%Test using small model
sourceDir = fileparts(which(mfilename));
load([sourceDir,'/test_data/ecoli_textbook.mat'], 'model');
modelDB=model; % Keep as database with reactions
try
    oldSolver=getpref('RAVEN','solver');
catch
end
setRavenSolver('cobra');

%Remove first 10 reactions
model=removeReactions(modelDB,(1:10));
modelDB.id='DB';
try
    evalc('[newConnected,cannotConnect,addedRxns,model,exitFlag]=fillGaps(model,modelDB)');
catch
    try
        setRavenSolver(oldSolver);
    catch
        rmpref('RAVEN','solver');
    end
    error('Solver not working')
end
sol=solveLP(model);
try
    setRavenSolver(oldSolver);
catch
    rmpref('RAVEN','solver');
end
%Expect at least 5% of the original growth
verifyTrue(testCase,-sol.f>0);
end


function x = power( x, y )

% POWER     Matrix power, z = x.^y

if isa(y,'tfocs_tuple')
	x.value_ = cellfun( @power, x.value_, y.value_, 'UniformOutput', false );
elseif isscalar(y)
	x.value_ = cellfun( @power, x.value_, {y}, 'UniformOutput', false );
else
	x.value_ = cellfun( @power, x.value_, {y}, 'UniformOutput', false );
end

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


function a = transpose(a)
%TRANSPOSE    Implements  a.'  for gradients
%
%   c = a.'
%

% written  10/16/98     S.M. Rump
% modified 11/03/03     S.M. Rump  improved performance
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
%

  [m n] = size(a.x);
  a.x = a.x.';
  if m*n~=1
    index = reshape( 1:(m*n) , m , n )';
    a.dx = a.dx( index , : );
  end


function r = ldivide(a,b)
%LDIVIDE      Taylor elementwise left division a .\ b
%

% written  05/21/09     S.M. Rump
%

  r = b ./ a;


function uu = VBA_getU(u,options,dim,flag)
% reshape micro-resolution driving input u
% function uu = VBA_getU(u,options,dim,flag)
% IN:
%   - u: macro-/micro- resolution input
%   - options: the options structure (see VBA_check.m)
%   - dim: the dim structure (see VBA_check.m)
%   - flag: gives the transformation direction (from micro- to macro- or
%   back...)
% OUT:
%   - uu: reshaped micro-resolution driving input

if options.microU && ~isequal(options.decim,1)
    switch flag
        case '2macro'
            u0 = u;
            nut = dim.u*options.decim;
            uu = zeros(nut,dim.n_t);
            for t=1:dim.n_t
                uu(:,t) = VBA_vec(u0(:,(t-1)*options.decim+1:t*options.decim));
            end
        case 'back2micro'
            u0 = u;
            uu = zeros(dim.u,options.decim*dim.n_t);
            for t=1:dim.n_t
                ut = reshape(u0(:,t),dim.u,options.decim);
                uu(:,(t-1)*options.decim+1:t*options.decim) = ut;
            end
    end
else % do not change input
    uu = u;
end


function [columnNames, data] = tableRead(fileName, separator)

% TABLEREAD Read in data which has column titles in the first line and separated values in each other line.
% FORMAT
% DESC reads in data from a file that has column titles in the first line and
% separated values in every other line.
% ARG fileName : file name in which the data is stored.
% ARG separator : separator between the columns (default ',').
% RETURN columnNames : the names of the columns taken from the
% first line.
% RETURN data : the data, taken from the remaining lines.
%
% SEEALSO : fopen, fgetl
%
% COPYRIGHT : Neil D. Lawrence, 2004

% NDLUTIL

if nargin < 2
  separator = ',';
end

fid = fopen(fileName);
i = 0;
while 1
  i = i + 1;
  lin=fgetl(fid);
  if ~ischar(lin), break, end
end
numLines = i;

fid = fopen(fileName);
lin = fgetl(fid);
columnNames = stringSplit(lin, separator);
numCol = length(columnNames);
i = 0;
data = zeros(numLines - 1, numCol);
while 1
  i = i+1;
  lin=fgetl(fid);
  if ~ischar(lin), break, end
  split = stringSplit(lin, separator);
  if length(split) ~= numCol
    error(['Error at line ' num2str(i) ' of file ' fileName ': wrong ' ...
                        'number of columns'])
  end
  for j = 1:length(split);
    data(i, j) = num2str(split{j});
  end
end
fclose(fid);


function varargout = process_ft_mtmconvol( varargin )
% PROCESS_FT_MTMCONVOL: Call FieldTrip function ft_mtmconvol.
%
% REFERENCES:
%     - http://www.fieldtriptoolbox.org/tutorial/timefrequencyanalysis

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Authors: Francois Tadel, 2017-2021

eval(macro_method);
end

%% ===== GET DESCRIPTION =====
function sProcess = GetDescription() %#ok<DEFNU>
    % Description the process
    sProcess.Comment     = 'FieldTrip: ft_mtmconvol (Multitaper)';
    sProcess.Category    = 'Custom';
    sProcess.SubGroup    = 'Frequency';
    sProcess.Index       = 506;
    sProcess.Description = 'http://www.fieldtriptoolbox.org/tutorial/timefrequencyanalysis';
    % Definition of the input accepted by this process
    sProcess.InputTypes  = {'import', 'data'};
    sProcess.OutputTypes = {'import', 'data'};
    sProcess.nInputs     = 1;
    sProcess.nMinFiles   = 1;
    % Definition of the input accepted by this process
    sProcess.InputTypes  = {'data', 'results', 'matrix'};
    sProcess.OutputTypes = {'timefreq', 'timefreq', 'timefreq'};
    sProcess.nInputs     = 1;
    sProcess.nMinFiles   = 1;
    % Options: Time window
    sProcess.options.timewindow.Comment = 'Time window:';
    sProcess.options.timewindow.Type    = 'timewindow';
    sProcess.options.timewindow.Value   = [];
    sProcess.options.timewindow.Group   = 'input';
    % Options: Sensor types
    sProcess.options.sensortypes.Comment = 'Sensor types or names (empty=all): ';
    sProcess.options.sensortypes.Type    = 'text';
    sProcess.options.sensortypes.Value   = 'MEG, EEG';
    sProcess.options.sensortypes.InputTypes = {'data'};
    sProcess.options.sensortypes.Group   = 'input';
    % Options: Scouts
    sProcess.options.clusters.Comment = '';
    sProcess.options.clusters.Type    = 'scout_confirm';
    sProcess.options.clusters.Value   = {};
    sProcess.options.clusters.InputTypes = {'results'};
    sProcess.options.clusters.Group   = 'input';
    % Options: Scout function
    sProcess.options.scoutfunc.Comment    = {'Mean', 'Max', 'PCA', 'Std', 'All', 'Scout function:'};
    sProcess.options.scoutfunc.Type       = 'radio_line';
    sProcess.options.scoutfunc.Value      = 1;
    sProcess.options.scoutfunc.InputTypes = {'results'};
    sProcess.options.scoutfunc.Group   = 'input';

    % Options: Taper
    sProcess.options.mt_taper.Comment = 'Taper: ';
    sProcess.options.mt_taper.Type    = 'combobox_label';
    sProcess.options.mt_taper.Value   = {'dpss', {'dpss', 'hanning', 'rectwin', 'sine'; ...
                                                  'dpss', 'hanning', 'rectwin', 'sine'}};
    % Options: Frequencies
    sProcess.options.mt_frequencies.Comment = 'Frequencies (start:step:stop): ';
    sProcess.options.mt_frequencies.Type    = 'text';
    sProcess.options.mt_frequencies.Value   = '1:2:120';
    % Options: Frequency resolution
    sProcess.options.mt_freqmod.Comment = 'Modulation factor: ';
    sProcess.options.mt_freqmod.Type    = 'value';
    sProcess.options.mt_freqmod.Value   = {10, ' ', 0};
    % Options: Time resolution
    sProcess.options.mt_timeres.Comment = 'Time resolution: ';
    sProcess.options.mt_timeres.Type    = 'value';
    sProcess.options.mt_timeres.Value   = {1, 'ms', []};
    % Options: Time step
    sProcess.options.mt_timestep.Comment = 'Time step: ';
    sProcess.options.mt_timestep.Type    = 'value';
    sProcess.options.mt_timestep.Value   = {0.1, 'ms', []};
    % === CORRESPONDANCE WITH FIELDTRIP
    sProcess.options.mt_label.Comment = ['
' ...
        'Correspondance with FieldTrip inputs:
' ...
        ' - timeoi = tw(1)+tr/2 : time_step : tw(2)-tr/2-1/sfreq
' ...
        '        tr=time_resolution, tw=time_window
' ...
        ' - tapsmofrq = frequencies / modulation_factor
' ...
        ' - timwin = repmat(time_resolution, 1, length(frequencies))'];
    sProcess.options.mt_label.Type    = 'label';

    % === MEASURE
    sProcess.options.measure.Comment = {'Power', 'Magnitude', 'Measure: '; ...
                                        'power', 'magnitude', ''};
    sProcess.options.measure.Type    = 'radio_linelabel';
    sProcess.options.measure.Value   = 'power';
    sProcess.options.measure.Group   = 'output';
    % === AVERAGE OUTPUT FILES
    sProcess.options.avgoutput.Comment = 'Save average power across files
(do not save one new file per input file)';
    sProcess.options.avgoutput.Type    = 'checkbox';
    sProcess.options.avgoutput.Value   = 1;
    sProcess.options.avgoutput.Group   = 'output';
end

%% ===== FORMAT COMMENT =====
function Comment = FormatComment(sProcess) %#ok<DEFNU>
    Comment = sProcess.Comment;
end

%% ===== RUN =====
function OutputFiles = Run(sProcess, sInputs) %#ok<DEFNU>
    OutputFiles = {};
    % Initialize FieldTrip or SPM: ft_specest_mtmconvol is in both
    [isInstalled, errMsg] = bst_plugin('InstallMultipleChoice', {'fieldtrip', 'spm12'});
    if ~isInstalled
        bst_report('Error', sProcess, [], errMsg);
        return;
    end
    bst_plugin('SetProgressLogo', 'fieldtrip');
    % Check for Signal Processing Toolbox when using DPSS
    if iscell(sProcess.options.mt_taper.Value)
        mt_taper = sProcess.options.mt_taper.Value{1};
    else
        mt_taper = sProcess.options.mt_taper.Value;
    end
    if strcmpi(mt_taper, 'dpss') && ~exist('dpss', 'file')
        bst_report('Error', sProcess, [], 'The option "dpss" requires the Signal Processing Toolbox.');
        return;
    end
    % Call TIME-FREQ process
    OutputFiles = process_timefreq('Run', sProcess, sInputs);
    % Remove logo
    bst_plugin('SetProgressLogo', []);
end


function diaphony ( input_filename )

%*****************************************************************************80
%
%% MAIN is the main program for DIAPHONY.
%
%  Discussion:
%
%    DIAPHONY reads a table dataset and applies the diaphony test.
%
%  Usage:
%
%    diaphony ( 'table_file' )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 January 2012
%
%  Author:
%
%    John Burkardt
%
  verbose = 0;

  if ( verbose )
    timestamp ( );
    fprintf ( 1, '\n' );
    fprintf ( 1, 'DIAPHONY:\n' );
    fprintf ( 1, '  MATLAB version\n' );
    fprintf ( 1, '  Compute the diaphony of a point set.\n' );
  end
%
%  Get the filename.
%
  if ( nargin < 1 )
    fprintf ( 1, '\n' );
    input_filename = input ( 'Enter the name of the input file.' );
  end
%
%  Get the data size.
%
  [ dim_num, point_num ] = r8mat_header_read ( input_filename );

  if ( verbose )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  The spatial dimension is %d\n', dim_num );
    fprintf ( 1, '  The number of points is %d\n', point_num );
  end
%
%  Read the data.
%
  points = r8mat_data_read ( input_filename, dim_num, point_num );

  if ( min ( min ( points ) ) < 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'DIAPHONY - Fatal error!\n' );
    fprintf ( 1, '  At least one coordinate of a point is less than 0!\n' );
    error ( 'DIAPHONY - Fatal error!' )
  elseif ( 1.0 < max ( max ( points ) ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'DIAPHONY - Fatal error!\n' );
    fprintf ( 1, '  At least one coordinate of a point is greater than 1!\n' );
    error ( 'DIAPHONY - Fatal error!' )
  end
%
%  Analyze the data.
%
  d = diaphony_compute ( dim_num, point_num, points );

  e = 1.0 / sqrt ( point_num );
  de = d / e;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  File  M  N  Diaphony  1/sqrt(N)  D/sqrt(N)\n' );
  fprintf ( 1, '  %s  %2d  %5d  %14.6g  %14.6g  %14.6g\n', ...
    input_filename, dim_num, point_num, d, e, de );
%
%  Terminate.
%
  if ( verbose )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'DIAPHONY:\n' );
    fprintf ( 1, '  Normal end of execution.\n' );
    fprintf ( 1, '\n' );
    timestamp ( )
  end

  return
end
function d = diaphony_compute ( dim_num, point_num, x )

%*****************************************************************************80
%
%% DIAPHONY_COMPUTE evaluates the diaphony of a N-dimensional point set.
%
%  Discussion:
%
%    The diaphony is analogous to, and related to, the discrepancy,
%    and is a measure of how well spread a set of point is.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 May 2003
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Peter Heelekalek, Harald Niederreiter,
%    The Weighted Spectral Test: Diaphony,
%    ACM Transactions on Modeling and Computer Simulation,
%    Volume 8, Number 1, January 1998, pages 43-60.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, real X(DIM_NUM,POINT_NUM), the point set, which is
%    presumed to lie in the DIM_NUM dimensional unit hypercube.
%
%    Output, real D, the value of the diaphony.
%
  d = 0.0;

  for i = 1 : point_num
    for j = 1 : point_num
      z(1:dim_num) = x(1:dim_num,i) - x(1:dim_num,j);
      for k = 1 : dim_num
        z(k) = r8_modp ( z(k), 1.0 );
      end
      d = d - 1.0 + prod ( ...
        ( 1.0 + 2.0 * pi^2 * ( z(1:dim_num).^2 - z(1:dim_num) + 1.0 / 6.0 ) ) );
    end
  end

  bot = point_num^2 * ( ( 1.0 + pi^2 / 3.0 )^dim_num - 1.0 );

  d = d / bot;

  d = sqrt ( d );

  return
end
function column_num = file_column_count ( input_file_name )

%*****************************************************************************80
%
%% FILE_COLUMN_COUNT counts the columns in the first line of a file.
%
%  Discussion:
%
%    The file is assumed to be a simple text file.
%
%    Most lines of the file are presumed to consist of COLUMN_NUM words,
%    separated by spaces.  There may also be some blank lines, and some
%    comment lines, which have a "#" in column 1.
%
%    The routine tries to find the first non-comment non-blank line and
%    counts the number of words in that line.
%
%    If all lines are blanks or comments, it goes back and tries to analyze
%    a comment line.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 February 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILE_NAME, the name of the file.
%
%    Output, integer COLUMN_NUM, the number of columns in the file.
%
  FALSE = 0;
  TRUE = 1;
%
%  Open the file.
%
  input_unit = fopen ( input_file_name );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_COLUMN_COUNT - Error!\n' );
    fprintf ( 1, '  Could not open the file "%s".\n', input_file_name );
    error ( 'FILE_COLUMN_COUNT - Error!' );
  end
%
%  Read one line, but skip blank lines and comment lines.
%  Use FGETL so we drop the newline character!
%
  got_one = FALSE;

  while ( 1 )

    line = fgetl ( input_unit );

    if ( line == -1 )
      break;
    end

    if ( s_len_trim ( line ) == 0 )

    elseif ( line(1) == '#' )

    else
      got_one = TRUE;
      break;
    end

  end

  fclose ( input_unit );

  if ( got_one == FALSE )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_COLUMN_COUNT - Warning!\n' );
    fprintf ( 1, '  The file does not seem to contain any data.\n' );
    column_num = -1;
    return
  end

  column_num = s_word_count ( line );

  return
end
function row_num = file_row_count ( input_file_name )

%*****************************************************************************80
%
%% FILE_ROW_COUNT counts the number of row records in a file.
%
%  Discussion:
%
%    Each input line is a "RECORD".
%
%    The records are divided into three groups:
%
%    * BLANK LINES (nothing but blanks)
%    * COMMENT LINES (begin with a '#')
%    * DATA RECORDS (anything else)
%
%    The value returned by the function is the number of data records.
%
%    By the way, if the MATLAB routine FGETS is used, instead of
%    FGETL, then the variable LINE will include line termination
%    characters, which means that a blank line would not actually
%    have zero characters.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 February 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILE_NAME, the name of the input file.
%
%    Output, integer ROW_NUM, the number of rows found.
%
  input_unit = fopen ( input_file_name );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_ROW_COUNT - Error!\n' );
    fprintf ( 1, '  Could not open the file "%s".\n', input_file_name );
    error ( 'FILE_ROW_COUNT - Error!' );
  end

  blank_num = 0;
  comment_num = 0;
  row_num = 0;

  record_num = 0;

  while ( 1 )

    line = fgetl ( input_unit );

    if ( line == -1 )
      break;
    end

    record_num = record_num + 1;
    record_length = s_len_trim ( line );

    if ( record_length <= 0 )
      blank_num = blank_num + 1;
    elseif ( line(1) == '#' )
      comment_num = comment_num + 1;
    else
      row_num = row_num + 1;
    end

  end

  fclose ( input_unit );

  return
end
function value = r8_modp ( x, y )

%*****************************************************************************80
%
%% R8_MODP returns the nonnegative remainder of real division.
%
%  Discussion:
%
%    If
%      REM = R8_MODP ( X, Y )
%      RMULT = ( X - REM ) / Y
%    then
%      X = Y * RMULT + REM
%    where REM is always nonnegative.
%
%    The MOD function computes a result with the same sign as the
%    quantity being divided.  Thus, suppose you had an angle A,
%    and you wanted to ensure that it was between 0 and 360.
%    Then mod(A,360.0) would do, if A was positive, but if A
%    was negative, your result would be between -360 and 0.
%
%    On the other hand, R8_MODP(A,360.0) is between 0 and 360, always.
%
%  Example:
%
%        I         J     MOD R8_MODP  R8_MODP Factorization
%
%      107        50       7       7    107 =  2 *  50 + 7
%      107       -50       7       7    107 = -2 * -50 + 7
%     -107        50      -7      43   -107 = -3 *  50 + 43
%     -107       -50      -7      43   -107 =  3 * -50 + 43
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 January 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the number to be divided.
%
%    Input, real Y, the number that divides X.
%
%    Output, real VALUE, the nonnegative remainder when X is divided by Y.
%
  if ( y == 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_MODP - Fatal error!\n' );
    fprintf ( 1, '  R8_MODP ( X, Y ) called with Y = %f\n', y );
    error ( 'R8_MODP - Fatal error!' );
  end

  value = mod ( x, y );

  if ( value < 0.0 )
    value = value + abs ( y );
  end

  return
end
function table = r8mat_data_read ( input_filename, m, n )

%*****************************************************************************80
%
%% R8MAT_DATA_READ reads data from an R8MAT file.
%
%  Discussion:
%
%    An R8MAT is an array of R8's.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 February 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Input, integer M, N, the number of rows and columns of data.
%
%    Output, real TABLE(M,N), the point coordinates.
%

%
%  Build up the format string for reading M real numbers.
%
  string = ' ';

  for i = 0 : m
    string = strcat ( string, ' %f' );
  end

  input_unit = fopen ( input_filename );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_DATA_READ - Error!\n' );
    fprintf ( 1, '  Could not open the file.\n' );
    error ( 'R8MAT_DATA_READ - Error!' );
  end

  table = zeros(m,n);

  i = 0;

  while ( i < n )

    line = fgets ( input_unit );

    if ( line == -1 )
      break;
    end

    if ( line(1) == '#' )

    elseif ( s_len_trim ( line ) == 0 )

    else

      [ x, count ] = sscanf ( line, string );

      if ( count == m )
        i = i + 1;
        table(1:m,i) = x(1:m);
      end

    end

  end

  fclose ( input_unit );

  return
end
function [ m, n ] = r8mat_header_read ( input_filename )

%*****************************************************************************80
%
%% R8MAT_HEADER_READ reads the header from an R8MAT file.
%
%  Discussion:
%
%    An R8MAT is an array of R8's.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Output, integer M, the spatial dimension.
%
%    Output, integer N, the number of points.
%
  m = file_column_count ( input_filename );

  if ( m <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data columns in\n' );
    fprintf ( 1, '  the file %s.\n', input_filename );
  end

  n = file_row_count ( input_filename );

  if ( n <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data rows in\n' );
    fprintf ( 1, '  the file %s\n', input_filename );
  end

  return
end
function len = s_len_trim ( s )

%*****************************************************************************80
%
% S_LEN_TRIM returns the length of a character string to the last nonblank.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 June 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S, the string to be measured.
%
%    Output, integer LENGTH, the length of the string up to the last nonblank.
%
  len = length ( s );

  while ( 0 < len )
    if ( s(len) ~= ' ' )
      return
    end
    len = len - 1;
  end

  return
end
function word_num = s_word_count ( s )

%*****************************************************************************80
%
%% S_WORD_COUNT counts the number of "words" in a string.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S, the string to be examined.
%
%    Output, integer WORD_NUM, the number of "words" in the string.
%    Words are presumed to be separated by one or more blanks.
%
  FALSE = 0;
  TRUE = 1;

  word_num = 0;
  s_length = length ( s );

  if ( s_length <= 0 )
    return;
  end

  blank = TRUE;

  for i = 1 : s_length

    if ( s(i) == ' ' )
      blank = TRUE;
    elseif ( blank == TRUE )
      word_num = word_num + 1;
      blank = FALSE;
    end

  end

  return
end
function timestamp ( )

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

  return
end


%CODEGENERATION.GENMFUNINERTIA Generate M-function for robot inertia matrix
%
% cGen.genmfuninertia() generates a robot-specific M-function to compute
% robot inertia matrix.
%
% Notes::
% - Is called by CodeGenerator.geninertia if cGen has active flag genmfun
% - The inertia matrix is stored row by row to avoid memory issues.
% - The generated M-function recombines the individual M-functions for each row.
% - Access to generated function is provided via subclass of SerialLink
%   whose class definition is stored in cGen.robjpath.
%
% Author::
%  Joern Malzahn, (joern.malzahn@tu-dortmund.de)
%
% See also CodeGenerator.CodeGenerator, CodeGenerator.gencoriolis.

% Copyright (C) 2012-2014, by Joern Malzahn
%
% 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
%
% The code generation module emerged during the work on a project funded by
% the German Research Foundation (DFG, BE1569/7-1). The authors gratefully
% acknowledge the financial support.

function [] = genmfuninertia(CGen)

%% Does robot class exist?
if ~exist(fullfile(CGen.robjpath,[CGen.getrobfname,'.m']),'file')
    CGen.logmsg([datestr(now),'\tCreating ',CGen.getrobfname,' m-constructor ']);
    CGen.createmconstructor;
    CGen.logmsg('\t%s\n',' done!');
end

%%
CGen.logmsg([datestr(now),'\tGenerating m-function for the robot inertia matrix row' ]);

q = CGen.rob.gencoords;
nJoints = CGen.rob.n;

for kJoints = 1:nJoints
    CGen.logmsg(' %s ',num2str(kJoints));
    symname = ['inertia_row_',num2str(kJoints)];
    fname = fullfile(CGen.sympath,[symname,'.mat']);

    if exist(fname,'file')
        tmpStruct = load(fname);
    else
        error ('genmfuninertia:SymbolicsNotFound','Save symbolic expressions to disk first!')
    end

    funfilename = fullfile(CGen.robjpath,[symname,'.m']);

    matlabFunction(tmpStruct.(symname),'file',funfilename,...              % generate function m-file
        'outputs', {'Irow'},...
        'vars', {'rob',[q]});
    hStruct = createHeaderStructRow(CGen.rob,kJoints,symname);                 % replace autogenerated function header
    replaceheader(CGen,hStruct,funfilename);

end
CGen.logmsg('\t%s\n',' done!');

CGen.logmsg([datestr(now),'\tGenerating full inertia matrix m-function']);

    funfilename = fullfile(CGen.robjpath,'inertia.m');
    hStruct = createHeaderStructFullInertia(CGen.rob,funfilename);

    fid = fopen(funfilename,'w+');

    fprintf(fid, '%s\n', ['function I = inertia(rob,q)']);                 % Function definition
    fprintf(fid, '%s\n',constructheaderstring(CGen,hStruct));                   % Header

    fprintf(fid, '%s \n', 'I = zeros(length(q));');                        % Code
    for iJoints = 1:nJoints
        funcCall = ['I(',num2str(iJoints),',:) = ','rob.inertia_row_',num2str(iJoints),'(q);'];
        fprintf(fid, '%s \n', funcCall);
    end

    fclose(fid);

    CGen.logmsg('\t%s\n',' done!');
end

function hStruct = createHeaderStructRow(rob,curJointIdx,fName)
[~,hStruct.funName] = fileparts(fName);
hStruct.shortDescription = ['Computation of the robot specific inertia matrix row for corresponding to joint ', num2str(curJointIdx), ' of ',num2str(rob.n),'.'];
hStruct.calls = {['Irow = ',hStruct.funName,'(rob,q)'],...
    ['Irow = rob.',hStruct.funName,'(q)']};
hStruct.detailedDescription = {'Given a full set of joint variables this function computes the',...
                               ['inertia matrix row number ', num2str(curJointIdx),' of ',num2str(rob.n),' for ',rob.name,'.']};
hStruct.inputs = { ['rob: robot object of ', rob.name, ' specific class'],...
                   ['q:  ',int2str(rob.n),'-element vector of generalized'],...
                   '     coordinates',...
                   'Angles have to be given in radians!'};
hStruct.outputs = {['Irow:  [1x',int2str(rob.n),'] row of the robot inertia matrix']};
hStruct.references = {'1) Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
    '2) Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
    '3) Introduction to Robotics, Mechanics and Control - Craig',...
    '4) Modeling, Identification & Control of Robots - Khalil & Dombre'};
hStruct.authors = {'This is an autogenerated function!',...
    'Code generator written by:',...
    'Joern Malzahn',...
    '2012 RST, Technische Universitaet Dortmund, Germany',...
    'http://www.rst.e-technik.tu-dortmund.de'};
hStruct.seeAlso = {'coriolis'};
end

function hStruct = createHeaderStructFullInertia(rob,fname)
[~,hStruct.funName] = fileparts(fname);
hStruct.shortDescription = ['Inertia matrix for the ',rob.name,' arm.'];
hStruct.calls = {['I = ',hStruct.funName,'(rob,q)'],...
    ['I = rob.',hStruct.funName,'(q)']};
hStruct.detailedDescription = {'Given a full set of joint variables the function computes the',...
                               'inertia Matrix of the robot.'};
hStruct.inputs = { ['rob: robot object of ', rob.name, ' specific class'],...
                   ['q:  ',int2str(rob.n),'-element vector of generalized'],...
                   '     coordinates',...
                   'Angles have to be given in radians!'};
hStruct.outputs = {['I:  [',int2str(rob.n),'x',int2str(rob.n),'] inertia matrix']};
hStruct.references = {'1) Robot Modeling and Control - Spong, Hutchinson, Vidyasagar',...
    '2) Modelling and Control of Robot Manipulators - Sciavicco, Siciliano',...
    '3) Introduction to Robotics, Mechanics and Control - Craig',...
    '4) Modeling, Identification & Control of Robots - Khalil & Dombre'};
hStruct.authors = {'This is an autogenerated function!',...
    'Code generator written by:',...
    'Joern Malzahn',...
    '2012 RST, Technische Universitaet Dortmund, Germany',...
    'http://www.rst.e-technik.tu-dortmund.de'};
hStruct.seeAlso = {'coriolis'};
end


%% This function will generate the ODE function based on calculation result of the sybolic expression
% Last Update: 2019/04/22
% Coded By: K
%(Modified so that matrix demension match)

function Generate_ODE_RHS(Sindy_ODEs,var_num_state,var_num_control)
z=sym('z',[1,var_num_state]);
u=sym('u',[1,var_num_control]);
syms t
f= matlabFunction(Sindy_ODEs,'File','Sindy_ODE_RHS','Optimize',true,'Vars',{t,z,u});


function plotem()

figure(1); clf; hold on;
pr = load('pb/bgtg_0.01_0.02_64/pr.txt');
plot(pr(:,2),pr(:,3),'co-');
pr = load('pb/bgtg/pr.txt');
plot(pr(:,2),pr(:,3),'bx-');
pr = load('pb/cgtg/pr.txt');
plot(pr(:,2),pr(:,3),'rx-');
prplot('');
legend('bgtg zone=2','bgtg zone=10','cgtg');
return

figure(1); clf; hold on;
prplot('all');
pr = load('pb/bgtg_0.01_0.02_64/pr.txt');
plot(pr(:,2),pr(:,3),'c-');
pr = load('pb/bgtg_hys_0.1/pr.txt');
plot(pr(:,2),pr(:,3),'rx-');
pr = load('pb/bgtg_hys_0.3/pr.txt');
plot(pr(:,2),pr(:,3),'ro-');
pr = load('pb/bgtg_hys/pr.txt');
plot(pr(:,2),pr(:,3),'mo-');
pr = load('pb/bgtg_hys_0.5/pr.txt');
plot(pr(:,2),pr(:,3),'rv-');
pr = load('pb/bgtg_hys_0.7/pr.txt');
plot(pr(:,2),pr(:,3),'bx-');
pr = load('pb/bgtg_hys_0.9/pr.txt');
plot(pr(:,2),pr(:,3),'bo-');
legend('base','0.1','0.3','0.4','0.5','0.7','0.9');
return

figure(1); clf; hold on;
pr = load('pb/random/pr.txt');
plot(pr(:,2),pr(:,3),'cx-');
pr = load('pb/gm_2/pr.txt');
plot(pr(:,2),pr(:,3),'mx-');
pr = load('pb/canny_2/pr.txt');
plot(pr(:,2),pr(:,3),'mo-');
pr = load('pb/gm_2_4/pr.txt');
plot(pr(:,2),pr(:,3),'mv-');
pr = load('pb/2mm_2/pr.txt');
plot(pr(:,2),pr(:,3),'gx-');
pr = load('pb/bgtg_0.01_0.02_64/pr.txt');
plot(pr(:,2),pr(:,3),'bx-');
pr = load('pb/bgtg_hys/pr.txt');
plot(pr(:,2),pr(:,3),'bo-');
prplot('all');
legend('random','gm(2)','canny(2)','gm(2,4)','2mm(2)','bgtg','bgtg(hys)');
return

figure(2); clf; hold on;
pr = load('pb/gm_1/pr.txt');
plot(pr(:,2),pr(:,3),'bx-');
pr = load('pb/gm_2/pr.txt');
plot(pr(:,2),pr(:,3),'bv-');
pr = load('pb/gm_4/pr.txt');
plot(pr(:,2),pr(:,3),'b^-');
pr = load('pb/gm_8/pr.txt');
plot(pr(:,2),pr(:,3),'bo-');
pr = load('pb/gm_16/pr.txt');
plot(pr(:,2),pr(:,3),'bs-');
prplot('GM, Single Scale');
legend('gm-1','gm-2','gm-4','gm-8','gm-16');
return

figure(3); clf; hold on;
pr = load('pb/2mm_1/pr.txt');
plot(pr(:,2),pr(:,3),'rx-');
pr = load('pb/2mm_2/pr.txt');
plot(pr(:,2),pr(:,3),'rv-');
pr = load('pb/2mm_4/pr.txt');
plot(pr(:,2),pr(:,3),'r^-');
pr = load('pb/2mm_8/pr.txt');
plot(pr(:,2),pr(:,3),'ro-');
pr = load('pb/2mm_16/pr.txt');
plot(pr(:,2),pr(:,3),'rs-');
prplot('2MM, Single Scale');
legend('2mm-1','2mm-2','2mm-4','2mm-8','2mm-16');

figure(4); clf; hold on;
pr = load('pb/gm_4/pr.txt');
plot(pr(:,2),pr(:,3),'rx-');
pr = load('pb/gm_1_2/pr.txt');
plot(pr(:,2),pr(:,3),'bx-');
pr = load('pb/gm_2_4/pr.txt');
plot(pr(:,2),pr(:,3),'bv-');
pr = load('pb/gm_4_8/pr.txt');
plot(pr(:,2),pr(:,3),'b^-');
prplot('GM, Multi-Scale');
legend('gm-4','gm-1-2','gm-2-4','gm-4-8');

figure(5); clf; hold on;
pr = load('pb/2mm_2/pr.txt');
plot(pr(:,2),pr(:,3),'rx-');
pr = load('pb/2mm_1_2/pr.txt');
plot(pr(:,2),pr(:,3),'bx-');
pr = load('pb/2mm_2_4/pr.txt');
plot(pr(:,2),pr(:,3),'bv-');
prplot('2MM, Multi-Scale');
legend('2mm-2','2mm-1-2','2mm-2-4');

function prplot(ti)
axis([0 1 0 1]);
axis square;
box on;
xlabel('Recall');
ylabel('Precision');
title(ti);


function ppgOnsets = qppg_fast(data,fs,from,to)

% This function is rewriten from wabp_pleth_new.c and wabp.c
% /* file wabp.c          Wei Zong       23 October 1998
%    			Last revised:   9 April 2010 (by G. Moody)
% -----------------------------------------------------------------------------
% wabp: beat detector for arterial blood presure (ABP) signal
% Copyright (C) 1998-2010 Wei Zong
%
% This program is free software; you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation; either version 2 of the License, or (at your option) any later
% version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT ANY
% WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
% PARTICULAR PURPOSE.  See the GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License along with
% this program; if not, write to the Free Software Foundation, Inc., 59 Temple
% Place - Suite 330, Boston, MA 02111-1307, USA.
%
% You may contact the author by e-mail (wzong@mit.edu) or postal mail
% (MIT Room E25-505, Cambridge, MA 02139, USA).  For updates to this software,
% please visit PhysioNet (http://www.physionet.org/).
% ------------------------------------------------------------------------------
%
% This program detects heart beats (pulse waveforms) in a continuous arterial
% blood pressure (ABP) signal.  This version of wabp works best with ABP signals
% sampled at 125 Hz, but it can analyze ABPs sampled at any frequency
% using on-the-fly resampling provided by the WFDB library.  'wabp' has been
% optimized for adult human  ABPs. For other ABPs, it may be necessary to
% experiment with the input sampling frequency and the time constants indicated
% below.
%
% `wabp' can process records containing any number of signals, but it uses only
% one signal for ABP pulse detection (by default, the lowest-numbered signal
% labelled `ABP', `ART', or `BP';  this can be changed using the `-s' option, see
% below).
%
% To compile this program under GNU/Linux, MacOS/X, MS-Windows, or Unix, use gcc:
%         gcc -o wabp wabp.c -lwfdb
% You must have installed the WFDB library, available at
%         http://www.physionet.org/physiotools/wfdb.shtml
% gcc is standard with GNU/Linux and is available for other platforms from:
%         http://www.gnu.org/             (sources and Unix binaries)
%         http://fink.sourceforge.net     (Mac OS/X only)
%         http://www.cygwin.com/          (MS-Windows only)
%
% For a usage summary, see the help text at the end of this file.  The input
% record may be in any of the formats readable by the WFDB library, and it may
% be anywhere in the WFDB path (in a local directory or on a remote web or ftp
% server).  The output of 'wabp' is an annotation file named RECORD.wabp (where
% RECORD is replaced by the name of the input record).  Within the output
% annotation file, the time of each NORMAL annotation marks an ABP pulse wave
% onset.
%
%
%
% input :   data: PPG data
%           fs:   sampling frequency, default 125 Hz
%           from: begin point to analysis
%           to  : end point to analysis
% output:   ppgOnsets: onset position of PPG beats in samples
%
%
%
% CHANGE LOG (should be moved to GitHub as commit messages...)
%
% 03 Aug 2010:  by Qiao Li
%   1) sample() function sometimes get WFDB_INVALID_SAMPLE value even when data is good.
%   2) Changed to mysample(), using isigsettime() and getvec() instead, but slow!!!
%   3) Add a mysetbuf() function, read the data to a databuf first, then mysample()
%      get data from the buf. It's much more fast than before.
%
%
% 04 Jul 2011 by Qiao Li
%   1) Set eye-closing period for PPG 0.34s, slope width 0.17s
%   2) Remove physical units to ADC units transform //Tm = physadu((unsigned)sig, Tm);
%   3) Add maxmin_2_3_threshold to modify the logic of eye-closing in order to minimize
%      the double beats
%	4) Change t += EyeClosing; to t = tpq+EyeClosing;
%
%
% 19 Jul 2011 by Qiao Li
%
%   1) add: (before reading data,at line 480)
%      isigsettime(from);
%      dataL=from;
%      to read data from 'from' setting
% 	2) Changed: (after learning period, at line 544)
%      (void)sample(sig, tpq);
%      if (sample_valid() == 0) break;
%       to:
%      if (dataend) break;
%
% 18 Oct 2016 by Qiao Li
%   1) add: input parameter fs for different sampling frequency data
%	2) add: re-scale data to ~ +/- 2000
%   3) add: find valley from the original data around 0.25s of tpq
%
% 03 Mar 2017 by Adriana Vest
%   Changed name of function to qppg to avoid confusion with wabp
%	Previous name: wabp_pleth_new.m
%
% 12 Sep 2018 by Giulia Da Poian
%   Changed output variable name to ppgOnsets
%
% 22 Dec 2018 by Giulia Da Poian
%   Start from the qppg.m code, replace use of global variables and use
%   function arguments, rename the function qppg_fast

if nargin<3
    from=1;
    to=length(data);
end

if nargin<2
    error('Wrong Number of Input Arguments: Sampling Frequency is required')
end

ppgOnsets=[];
beat_n=1;

sps=fs; % Sampling Frequency

BUFLN = 4096;           % /* must be a power of 2, see slpsamp() */
EYE_CLS = 0.34;         % /* eye-closing period is set to 0.34 sec (340 ms) for PPG */
LPERIOD  = sps*8;       % /* learning period is the first LPERIOD samples */
SLPW = 0.17;            % /* Slope width (170ms) for PPG */
NDP = 2.5;              % /* adjust threshold if no pulse found in NDP seconds */
TmDEF = 5;              % /* minimum threshold value (default) */
Tm = TmDEF;

BUFLN2 = (BUFLN*2);

INVALID_DATA=-32768;
if data(1)<=INVALID_DATA+10
    data(1)=mean(data);
end
inv=find(data<=INVALID_DATA+10);
for i=1:length(inv)
    data(inv(i))=data(inv(i)-1);
end

% re-scale data to ~ +/- 2000
if length(data)<5*60*sps
    data=(data-min(data))./(max(data)-min(data)).*4000-2000;
else
% find max/min every 5 minute for re-scaling data
    n=1;
    for i=1:5*60*sps:length(data)
        max_data(n)=max(data(i:min(i+5*60*sps-1,length(data))));
        min_data(n)=min(data(i:min(i+5*60*sps-1,length(data))));
        n=n+1;
    end
    data=(data-median(min_data))./(median(max_data)-median(min_data)).*4000-2000;
end

samplingInterval = 1000.0/sps;
spm = 60 * sps;
EyeClosing = round(sps * EYE_CLS);   % /* set eye-closing period */
ExpectPeriod = round(sps * NDP);	  % /* maximum expected RR interval */
SLPwindow = round(sps * SLPW);       % /* slope window size */
timer=0;

ebuf(1:BUFLN)=0;
lbuf=ebuf;
if from>BUFLN
    tt_2=from-BUFLN;
else
    tt_2=0;
end
aet=0;

t1=8*sps;
t1 = t1+from;
T0 = 0;
n=0;
for t = from:t1
   [temp,ebuf,lbuf,tt_2, aet] = slpsamp(t,data,BUFLN,ebuf,lbuf,tt_2, aet,SLPwindow);
    if temp > INVALID_DATA+10
        T0 = T0+temp;
        n=n+1;
    end
end
T0 = T0/n; % T0=T0/(t1-from);
Ta = 3 * T0;

learning=1;

%    /* Main loop */
t = from;
while t <= to

    if (learning)
        if (t > from + LPERIOD)
    		learning = 0;
        	T1 = T0;
            t = from;	% /* start over */
        else
            T1 = 2*T0;
        end
    end

	[temp,ebuf,lbuf,tt_2, aet] = slpsamp(t,data,BUFLN,ebuf,lbuf,tt_2, aet,SLPwindow);

    if (temp > T1)    % /* found a possible ABP pulse near t */
	    timer = 0;
            % /* used for counting the time after previous ABP pulse */
	    maxd = temp;
        mind = maxd;
        tmax=t;
        for (tt = t + 1: t + EyeClosing-1)
            [temp2 ,ebuf,lbuf,tt_2, aet] = slpsamp(tt,data,BUFLN,ebuf,lbuf,tt_2, aet,SLPwindow);
            if temp2 > maxd
                maxd=temp2;
                tmax=tt;
            end
        end
        if (maxd == temp)
            t=t+1;
            continue;
        end

        for tt = tmax :-1: (t - EyeClosing / 2 +1)
            [temp2 ,ebuf,lbuf,tt_2, aet] = slpsamp(tt,data,BUFLN,ebuf,lbuf,tt_2, aet,SLPwindow);
            if temp2< mind
                mind=temp2;
            end
        end
        if maxd>mind+10
            onset=(maxd-mind)/100+2;
            tpq=t-round(0.04*fs);
            maxmin_2_3_threshold=(maxd-mind)*2.0/3;
            for tt=tmax:-1:t-EyeClosing/2+1
                [temp2, ebuf,lbuf,tt_2, aet] = slpsamp(tt,data,BUFLN,ebuf,lbuf,tt_2, aet,SLPwindow);
                if temp2<maxmin_2_3_threshold
                    break;
                end
            end
            for tt=tt:-1:t - EyeClosing / 2 + round(0.024*fs)
                [temp2 ,ebuf,lbuf,tt_2, aet] = slpsamp(tt,data,BUFLN,ebuf,lbuf,tt_2, aet,SLPwindow);
                [temp3 ,ebuf,lbuf,tt_2, aet] = slpsamp(tt-round(0.024*fs),data,BUFLN,ebuf,lbuf,tt_2, aet,SLPwindow);
                if temp2-temp3<onset
                    tpq=tt-round(0.016*fs);
                    break;
                end
            end

            % find valley from the original data around 0.25s of tpq
            valley_v = round(tpq);
            for valley_i=round(max(2,tpq-round(0.20*fs))):round(min(tpq+round(0.05*fs),length(data)-1))

                % If vally is too low, it cannot serve as an index, so move to the next time.
                if valley_v <= 0
                    t = t + 1;
                    continue;
                end

                if data(valley_v)>data(valley_i) && data(valley_i)<=data(valley_i-1) && data(valley_i)<=data(valley_i+1)
                    valley_v=valley_i;
                end
            end

            if (~learning)

                % If we are looking for the first peak
                if beat_n == 1

                    % If the proposed peak index > 0
                    if round(valley_v) > 0
                        ppgOnsets(beat_n) = round(valley_v);
                        beat_n = beat_n + 1;
                    end
                else
                    % Check if rounded valley_v is greater than the prior beat index
                    if round(valley_v) > ppgOnsets(beat_n-1)
                        ppgOnsets(beat_n) = round(valley_v);
                        beat_n = beat_n + 1;
                    end
                end
            end

            % /* Adjust thresholds */
            Ta = Ta + (maxd - Ta)/10;
            T1 = Ta / 3;

            % /* Lock out further detections during the eye-closing period */
            t = tpq+EyeClosing;
        end
    else
        if (~learning)
	    % /* Once past the learning period, decrease threshold if no pulse
	    %   was detected recently. */
            timer = timer+1;
            if (timer > ExpectPeriod && Ta > Tm)
                Ta=Ta-1;
                T1 = Ta / 3;
            end
        end
    end

    t=t+1;

end

% Discard first beat because algorithm always finds first minimum value, so trace-back logic
% will find a fir

end

function [beat1,ebuf,lbuf,tt_2, aet] = slpsamp(t,data,BUFLN,ebuf,lbuf,tt_2, aet,SLPwindow)

    while (t > tt_2)
        prevVal=0;

        if (tt_2>0) && (tt_2-1>0) && (tt_2<length(data)) && (tt_2-1<length(data))
            val2=data(tt_2 - 1);
            val1=data(tt_2);
        else
            val2=prevVal;
            val1=val2;
        end
    	prevVal=val2;
        dy =  val1-val2;
        if (dy < 0)
            dy = 0;
        end
        tt_2=tt_2+1;
        M=round(mod(tt_2,(BUFLN-1))+1);
        et=dy;
        ebuf(M)=et;
%         M2=round(mod(tt_2-SLPwindow,(BUFLN-1))+1);
%         aet=aet+et-ebuf(M2);
        aet=0;
        for i=0:SLPwindow-1
            p=M-i;
            if p<=0
                p=p+BUFLN;
            end
            aet=aet+ebuf(p);
        end
        lbuf(M) = aet;

    end
    M3=round(mod(t,(BUFLN-1))+1);
    beat1=lbuf(M3);
end


function []=panel2hddisp(N,n,T,Units,endo,hd_estimates,stringdates1,decimaldates1,pref)

% plot the figure
if pref.plot
hd=figure('Tag','BEARresults');
set(hd,'Color',[0.9 0.9 0.9]);
set(hd,'name','historical decomposition');
% initiate the count
count=0;
% loop over units
for ii=1:N
   % loop over endogenous variables
   for jj=1:n
      % loop over shocks
      for kk=1:n+1
      % increment count
      count=count+1;
      % then plot
      subplot(N*n,n+1,count)
      temp=hd_estimates{jj,kk,ii};
      hold on
      Xpatch=[decimaldates1' fliplr(decimaldates1')];
      Ypatch=[temp(1,:) fliplr(temp(3,:))];
      HDpatch=patch(Xpatch,Ypatch,[0.7 0.78 1]);
      set(HDpatch,'facealpha',0.5);
      set(HDpatch,'edgecolor','none');
      plot(decimaldates1,temp(2,:)','Color',[0.4 0.4 1],'LineWidth',2);
      plot([decimaldates1(1,1),decimaldates1(end,1)],[0 0],'k--');
      hold off
      set(gca,'XLim',[decimaldates1(1,1) decimaldates1(end,1)],'FontName','Times New Roman');
         % top labels
         if (jj==1 && kk<=n)
         title([Units{ii,1} '\_' endo{kk,1}],'FontWeight','normal');
         elseif (jj==1 && kk==n+1)
         title('Exogenous','FontWeight','normal');
         end
         % side labels
         if kk==1
         ylabel([Units{ii,1} '\_' endo{jj,1}],'FontWeight','normal');
         end
      end
   end
end
% top supertitle
ax=axes('Units','Normal','Position',[.11 .075 .85 .88],'Visible','off');
set(get(ax,'Title'),'Visible','on')
title('Contribution of shock:','FontSize',11,'FontName','Times New Roman','FontWeight','normal');
% side supertitle
ylabel('Fluctuation of:','FontSize',12,'FontName','Times New Roman','FontWeight','normal');
set(get(ax,'Ylabel'),'Visible','on')
end

% save on Excel

% create the cell that will be saved on excel
hdcell={};
% build preliminary elements: space between the tables
vertspace=repmat({''},T+3,1);
horzspace=repmat({''},2,5*(n+1));
% loop over units
for ii=1:N
   % loop over endogenous variables
   for jj=1:n
   % initiate the cell of results
   endocell={};
      % loop over shocks
      for kk=1:n
      % create a header
      header=[{['fluctuation of ' Units{ii,1} '_' endo{jj,1} ' due to ' Units{ii,1} '_' endo{kk,1} ' shocks']} {''} {''} {''};{''} {''} {''} {''};{''} {'lower bound'} {'median'} {'upper bound'}];
      % complete the cell
      tempcell=[[header;stringdates1 num2cell((hd_estimates{jj,kk,ii}(1,:))') num2cell((hd_estimates{jj,kk,ii}(2,:))') num2cell((hd_estimates{jj,kk,ii}(3,:))')] vertspace];
      % concatenate to the previous parts of unitcell
      endocell=[endocell tempcell];
      end
   % exogenous contribution
   header=[{['fluctuation of ' Units{ii,1} '_' endo{jj,1} ' due to exogenous']} {''} {''} {''};{''} {''} {''} {''};{''} {'lower bound'} {'median'} {'upper bound'}];
   % complete the cell
   tempcell=[[header;stringdates1 num2cell((hd_estimates{jj,n+1,ii}(1,:))') num2cell((hd_estimates{jj,n+1,ii}(2,:))') num2cell((hd_estimates{jj,n+1,ii}(3,:))')] vertspace];
   % concatenate to the previous parts of unitcell
   endocell=[endocell tempcell];
   % concatenate to the previous parts of irfcell
   hdcell=[hdcell;horzspace;endocell];
   end
hdcell=[hdcell;horzspace];
end
% trim
hdcell=hdcell(3:end-2,1:end-1);
% write in excel
if pref.results==1
    bear.xlswritegeneral(fullfile(pref.results_path, [pref.results_sub '.xlsx']),hdcell,'hist decomposition','B2');
end


% op_concatAverages.m
% Jamie Near, McGill University 2014.
%
% USAGE:
% out=op_concatAverages(in1,in2);
%
% DESCRIPTION:
% Concatenate two scans along the averages dimension.  Two scans with 50
% averages each will now look like a single scan with 100 averages.  For
% looping, it is possible to enter a blank structure as the first input.
% In this case, the function will simply return the second input.
%
% INPUTS:
% in1    = first input in matlab structure format.
% in2    = second input in matlab structure format.
%
% OUTPUTS:
% out    = Output following concatenation of inputs along the
%           averages dimension.

function out=op_concatAverages(in1,in2);

if isempty(fieldnames(in1)) %Check if the first argument to be a null string
    out=in2; %If so, return in2;
else
    if in1.dims.averages~=0 && in2.dims.averages~=0 && in1.dims.averages ~= in2.dims.averages
        %RULE:  Averages dimension must be the same for both inputs UNLESS one of
        %the inputs has an averages dimension of zero:
        error('averages dimensions must be the same for both inputs');
    end

    if in1.dims.averages==0 && in2.dims.averages==0
        dim=2;
    elseif in1.dims.averages==0
        dim=in2.dims.averages;
    elseif in2.dims.averages==0;
        dim=in1.dims.averages;
    else
        dim=in1.dims.averages;
    end

    fids=cat(dim,in1.fids,in2.fids);
    specs=cat(dim,in1.specs,in2.specs);
    sz=size(fids);

    %FILLING IN DATA STRUCTURE
    out=in1;
    out.fids=fids;
    out.specs=specs;
    out.sz=sz;
    out.averages=in1.averages+in2.averages;
    out.rawAverages=in1.rawAverages+in2.rawAverages;
    if in1.dims.averages==0
        out.dims.averages=dim;
    end

    %FILLING IN THE FLAGS
    out.flags=in1.flags;
    out.flags.writtentostruct=1;
    out.flags.averaged=0;
end


function [sol,fit,popo,fits] = gaevolve(varargin)
% GAEVOLVE  Evolution of a population
%
% function [sol,fit,popo] = gaevolve(pop,funcs,fparams,params,maxiters)
%
%  This function controls the evolution of a population via simple
% genetical crossing, mutations and selection of the units. The initial
% population is given and the functions to compute the fit value, the
% crossing and the mutation are given. The maximum number of iterations is
% defined. An hooking function can be defined to hook the single evolution
% iteration step before update.
%
%  Params
%  ------
% IN:
%  pop      = The initial population: a (column) cell of units.
%  funcs    =  A cell array containing the function handlers for the
%             following functions:
%       ffit  = The fit function: recives a unit and compute its fit value.
%       fcros =  The crossing function: recives two units and cross them
%               generating a new unit.
%       fmut  =  The mutation function: recives a unit and mutate it into a
%               new unit.
%       fhook = The hooking function called before iterations: recives the
%              actual output values; returns a boolean value that says if
%              the updating process must stop.
%  fparams  = The cell of function aditional parameters for the funcs.
%  params   =  An array containing the parameters the following of the
%             updating process:
%       psel  = The selection percentage.
%       prmut = The mutation probability.
%  maxiters = The maximum number of iterations. (def=1000)
% OUT:
%  sol      = The final solution unit.
%  fit      = The final fit.
%  popo     = The final population.
%  fits     = All the actual population fits.

% Parsing of params:
[pop,ffit,fcros,fmut,fhook,pfit,pcros,pmut,phook,psel,prmut,maxiters] = ...
    ParseParams(varargin{:});

% Init:
iters = 0;
szpop = numel(pop);
nsel = round(psel*szpop);

% Computing the initial solution:
[sol,fit,fits] = ComputeSolution(pop,ffit,pfit);
popo = pop;

% Iterating:
while iters<maxiters && ~feval(fhook,sol,fit,pop,fits,phook{:})

    % ------------   Init    ------------

    % The new population:
    popo = cell(szpop,1);

    % The actual vacant element:
    actual = 1;

    % ------------ Selection ------------

    % Selection on the fits:
    [sfits,sinds] = sort(fits);
    popo(1:nsel) = pop(sinds(szpop-nsel+1:szpop));
    actual = nsel + 1;

    % ------------ Crossing  ------------

    % Computing the probabilities:
    totFit = sum(fits);
    if totFit==0
        probs = ones([1,szpop])/szpop;
    else
        probs = fits/totFit;
    end

    % The cumulative probs:
    for ind=2:szpop
        probs(ind) = probs(ind-1) + probs(ind);
    end

    % Iterating on all the other units:
    while actual<=szpop
        % Selecting the first unit:
        u1p = rand; u1 = 1;
        while probs(u1)<u1p && u1<szpop u1 = u1+1; end

        % Selecting the second unit:
        u2 = u1;
        while u1==u2 && szpop>1
            % A random unit selection:
            u2p = rand; u2 = 1;
            while probs(u2)<u2p && u2<szpop u2 = u2+1; end
        end

        % Generating a new unit:
        popo{actual} = feval(fcros,pop{u1},pop{u2},pcros{:});

        % Next:
        actual = actual + 1;
    end

    % ------------ Mutation  ------------

    % Iterating on all the units:
    for ind=1:szpop
        % Must be mutated?
        if rand<=prmut
            % Mutating:
            popo{ind} = feval(fmut,popo{ind},pmut{:});
        end
    end

    % ------------  Saving   ------------

    % The population:
    pop = popo;

    % Computing the solution:
    [sol,fit,fits] = ComputeSolution(pop,ffit,pfit);

    % A new iteration:
    iters = iters + 1;
end

% -----------------------------------------------------------------

% Computing the solution given a population:
function [sol,fit,fits] = ComputeSolution(pop,ffit,pfit)

% Computing the fits:
nel = numel(pop);
fits = zeros(1,nel);
for ind=1:nel
    % Computing a fit:
    fits(ind) = feval(ffit,pop{ind},pfit{:});
end

% Localizing and extracting the solution:
[fit,pos] = max(fits);
sol = pop(pos);

% ------------------------ LOCAL FUNCTIONS ------------------------

% Parsing of parameters:
function [pop,ffit,fcros,fmut,fhook, ...
          pfit,pcros,pmut,phook,psel,prmut,maxiters] = ...
    ParseParams(varargin)

% Minimum number of parameters is 1 (the image)
if nargin<4 error('Too few parameters'); end
if nargin>5 error('Too many parameters'); end

% Set up variables
varnames = {'pop','funcs','fparams','params','maxiters'};
for ind=1:nargin
    eval([varnames{ind} ' = varargin{ind} ;']);
end

% Default parameters
if nargin<5 maxiters=1000; end

% Checking types:
if ~isa(funcs,'cell') || numel(funcs)>4 || numel(funcs)<3
    error('The functions array must contain 3 or 4 function_handles!');
end
if ~isa(fparams,'cell') || size(fparams,1)>4
    error('The function parameter cell must contain at least 4 rows!');
end
if ~isa(params,'double') || numel(params)~=2
    error('The double parameters array must contain psel and pmut values!');
end

% Extracting the functions:
ffit = funcs{1};
fcros = funcs{2};
fmut = funcs{3};
if numel(funcs)>3
    fhook = funcs{4};
else
    fhook = @(sol,fit,popo)(false);
end

% Extracting function params:
npar = size(fparams,2);
if npar>=1 pfit=fparams{1}; else pfit={}; end
if npar>=2 pcros=fparams{2}; else pcros={}; end
if npar>=3 pmut=fparams{3}; else pmut={}; end
if npar>=4 phook=fparams{4}; else phook={}; end

% Extracting parameters:
psel = params(1);
prmut = params(2);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Spherical Harmonic Modeling and Analysis Toolkit (SPHARM-MAT) is a 3D
% shape modeling and analysis toolkit.
% It is a software package developed at Shenlab in Center for Neuroimaging,
% Indiana University (SpharmMat@gmail.com, http://www.iupui.edu/~shenlab/)
% It is available to the scientific community as copyright freeware
% under the terms of the GNU General Public Licence.
%
% Copyright 2009, 2010, ShenLab, Center for Neuroimaging, Indiana University
%
% This file is part of SPHARM-MAT.
%
% SPHARM-MAT is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% SPHARM-MAT is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with SPHARM-MAT. If not, see <http://www.gnu.org/licenses/>.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function patch_overlay(vertices, faces, signal, threshold, type, vtnorm, cmap,windowHandle, fname)

figure(windowHandle);
set(gcf,'PaperPositionMode','auto');
colormap(cmap);

patch_lighta(vertices, faces);
maxSG = max(signal);
minSG = min(signal);

FValpha = ones(size(vertices,1),1);
switch type
    case 'p-value'
        idx = find(abs(signal)>(threshold));
        FValpha(idx) = 0;

        hold on;
        patch('faces', faces,'vertices',vertices, 'FaceVertexCData',signal, 'EdgeColor', 'none', ...
            'VertexNormals',vtnorm,'FaceVertexAlpha',FValpha,'FaceColor','interp', 'FaceAlpha','interp');
        hold off;
        colorbar;

    case 't-map'
        idx = find(abs(signal)<threshold);
        FValpha(idx) = 0;

        hold on;
        patch('faces', faces,'vertices',vertices, 'FaceVertexCData',signal, 'EdgeColor', 'none', ...
            'VertexNormals',vtnorm,'FaceVertexAlpha',FValpha,'FaceColor','interp', 'FaceAlpha','interp');
        hold off;
        colorbar('location', 'EastOutside');
end

title(sprintf('%s.mat\n%s',strrep(fname,'_','-'),type));

return;


function C = ojw_bsxfun(func, A, B)
%OJW_BSXFUN  Provides backwards compatibility for Matlab's bsxfun
%
%   C = ojw_bsxfun(func, A, B)
%
% Each dimension of A and B must be equal to each other, or equal to one.
% Whenever a dimension of one of A or B is singleton (equal to 1), the
% array is virtually replicated along that dimension to match the other
% array (or diminished if the corresponding dimension of the other array is
% 0). The two resized arrays are then passed, as vectors, to func(), which
% must be able to accept as input either two column vectors of the same
% size, or one column vector and one scalar, and return as output a column
% vector of the same size as the input(s).
%
% IN:
%    func - Function handle.
%    A - Input array.
%    B - Input array of same type as A.
%
% OUT:
%    C - max(size(A),size(B)).*(size(A)>0 & size(B)>0) sized output array.
%
% See also BSXFUN.

% $Id: ojw_bsxfun.m,v 1.1 2007/12/07 11:27:52 ojw Exp $

if exist('bsxfun', 'builtin')
    % Do the obvious - Matlab versions below 7.3 don't have it though
    C = bsxfun(func, A, B);
else
    % Backwardly compatible method
    if isscalar(A)
        C = reshape(func(A, B(:)), size(B));
        return
    elseif isscalar(B)
        C = reshape(func(A(:), B), size(A));
        return
    end
    sA = size(A);
    sB = size(B);
    % Enlarge the smaller of the two sizes
    s = numel(sA) - numel(sB);
    if s < 0
        sA = [sA ones(1, -s)];
    elseif s > 0
        sB = [sB ones(1, s)];
    end
    % Calculate the output array size
    sMax = max(sA, sB) .* (sA > 0 & sB > 0);
    % Make sure arrays have same size of dimension or size 1
    a = sA ~= sMax;
    b = sB ~= sMax;
    if any(a & sA ~= 1) || any(b & sB ~= 1)
        error('A and B must have equal or singleton dimensions');
    end
    if ~all(sMax)
        % Some entries are zero, so array is empty
        C = zeros(sMax);
        return
    end
    % Resize the arrays to have the same size, sMax
    if any(a)
        A = repmat(A, sMax ./ sA);
    end
    if any(b)
        B = repmat(B, sMax ./ sB);
    end
    % Apply the function
    C = reshape(func(A(:), B(:)), sMax);
end


clear

scrsz = get(0,'ScreenSize');
figure1 = figure('Position',[20 50 3*scrsz(3)/4 0.9*scrsz(4)]);

set(figure1,'Units','Inches');
pos = get(figure1,'Position');
set(figure1,'PaperPositionMode','Auto','PaperUnits','Inches','PaperSize',[pos(3), pos(4)])

% Create axes
axes1 = axes('Parent',figure1,'FontSize',40,'FontName','Helvetica');

line_width = 6;
hold on;

load('../../matlab_version/experiments_300VW/results/cfss_errors.mat');
[error_x, error_y] = cummErrorCurve(cfss_error_66_cat_1);
plot(error_x, error_y, 'DisplayName', 'CFSS', 'LineWidth',line_width);

load('../../matlab_version/experiments_300VW/results/cfan_errors.mat');
[error_x, error_y] = cummErrorCurve(cfan_error_66_cat_1);
plot(error_x, error_y, 'DisplayName', 'CFAN', 'LineWidth',line_width);
%
load('../../matlab_version/experiments_300VW/results/iccr_errors.mat');
[error_x, error_y] = cummErrorCurve(iccr_error_66_cat_1);
plot(error_x, error_y, 'DisplayName', 'iCCR', 'LineWidth',line_width);

load('results/300VW_OpenFace.mat');
[error_x, error_y] = cummErrorCurve(clnf_error_66_cat_1);
plot(error_x, error_y, 'DisplayName', 'OpenFace - CLNF', 'LineWidth',line_width);

% Make sure CE-CLM is drawn on top
[error_x, error_y] = cummErrorCurve(ceclm_error_66_cat_1);
plot(error_x, error_y, 'r', 'LineWidth',line_width, 'DisplayName', 'OpenFace - CE-CLM');

% Make it looks nice and print to a pdf
set(gca,'xtick',[0.01:0.01:0.08])
xlim([0.01,0.08]);
xlabel('IOD normalized MAE','FontName','Helvetica');
ylabel('Proportion of images','FontName','Helvetica');
grid on
ax=legend('show', 'Location', 'SouthEast');
ax.FontSize = 30;

print -dpdf results/300VWres_66_cat1.pdf

%%
clear

scrsz = get(0,'ScreenSize');
figure1 = figure('Position',[20 50 3*scrsz(3)/4 0.9*scrsz(4)]);

set(figure1,'Units','Inches');
pos = get(figure1,'Position');
set(figure1,'PaperPositionMode','Auto','PaperUnits','Inches','PaperSize',[pos(3), pos(4)])

% Create axes
axes1 = axes('Parent',figure1,'FontSize',40,'FontName','Helvetica');

line_width = 6;
hold on;

load('../../matlab_version/experiments_300VW/results/cfss_errors.mat');
[error_x, error_y] = cummErrorCurve(cfss_error_66_cat_2);
plot(error_x, error_y, 'DisplayName', 'CFSS', 'LineWidth',line_width);

load('../../matlab_version/experiments_300VW/results/cfan_errors.mat');
[error_x, error_y] = cummErrorCurve(cfan_error_66_cat_2);
plot(error_x, error_y, 'DisplayName', 'CFAN', 'LineWidth',line_width);
%
load('../../matlab_version/experiments_300VW/results/iccr_errors.mat');
[error_x, error_y] = cummErrorCurve(iccr_error_66_cat_2);
plot(error_x, error_y, 'DisplayName', 'iCCR', 'LineWidth',line_width);

load('results/300VW_OpenFace.mat');
[error_x, error_y] = cummErrorCurve(clnf_error_66_cat_2);
plot(error_x, error_y, 'DisplayName', 'OpenFace - CLNF', 'LineWidth',line_width);

% Make sure CE-CLM is drawn on top
[error_x, error_y] = cummErrorCurve(ceclm_error_66_cat_2);
plot(error_x, error_y, 'r', 'LineWidth',line_width, 'DisplayName', 'OpenFace - CE-CLM');

% Make it looks nice and print to a pdf
set(gca,'xtick',[0.01:0.01:0.08])
xlim([0.01,0.08]);
xlabel('IOD normalized MAE','FontName','Helvetica');
ylabel('Proportion of images','FontName','Helvetica');
grid on
ax=legend('show', 'Location', 'SouthEast');
ax.FontSize = 30;

print -dpdf results/300VWres_66_cat2.pdf

%%
clear

scrsz = get(0,'ScreenSize');
figure1 = figure('Position',[20 50 3*scrsz(3)/4 0.9*scrsz(4)]);

set(figure1,'Units','Inches');
pos = get(figure1,'Position');
set(figure1,'PaperPositionMode','Auto','PaperUnits','Inches','PaperSize',[pos(3), pos(4)])

% Create axes
axes1 = axes('Parent',figure1,'FontSize',40,'FontName','Helvetica');

line_width = 6;
hold on;

load('../../matlab_version/experiments_300VW/results/cfss_errors.mat');
[error_x, error_y] = cummErrorCurve(cfss_error_66_cat_3);
plot(error_x, error_y, 'DisplayName', 'CFSS', 'LineWidth',line_width);

load('../../matlab_version/experiments_300VW/results/cfan_errors.mat');
[error_x, error_y] = cummErrorCurve(cfan_error_66_cat_3);
plot(error_x, error_y, 'DisplayName', 'CFAN', 'LineWidth',line_width);
%
load('../../matlab_version/experiments_300VW/results/iccr_errors.mat');
[error_x, error_y] = cummErrorCurve(iccr_error_66_cat_3);
plot(error_x, error_y, 'DisplayName', 'iCCR', 'LineWidth',line_width);

load('results/300VW_OpenFace.mat');
[error_x, error_y] = cummErrorCurve(clnf_error_66_cat_3);
plot(error_x, error_y, 'DisplayName', 'OpenFace - CLNF', 'LineWidth',line_width);

% Make sure CE-CLM is drawn on top
[error_x, error_y] = cummErrorCurve(ceclm_error_66_cat_3);
plot(error_x, error_y, 'r', 'LineWidth',line_width, 'DisplayName', 'OpenFace - CE-CLM');

% Make it looks nice and print to a pdf
set(gca,'xtick',[0.01:0.01:0.08])
xlim([0.01,0.08]);
xlabel('IOD normalized MAE','FontName','Helvetica');
ylabel('Proportion of images','FontName','Helvetica');
grid on
ax=legend('show', 'Location', 'SouthEast');
ax.FontSize = 30;

print -dpdf results/300VWres_66_cat3.pdf


%% sv3
% Below is a demonstration of the features of the |sv3| function

%%
clear; close all; clc;

%% Syntax
% |[varargout]=sv3(varargin);|

%% Description
% 3D slice viewer

%% Example: Visualizing MRI data

%%
% Example image data
load mri;
M=squeeze(D); %example image data set
v=2./[1,1,.4]; %example voxel size

%%
% Using |sv3|

hf=sv3(M,v);

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