function output = callqpas(interfacedata)

% Retrieve needed data
options = interfacedata.options;
model = yalmip2quadprog(interfacedata);

if size(model.Aeq,1)==0
    model.Aeq = [];
    model.beq = [];
end

if options.savedebug
    save debugfile model
end

if options.showprogress;showprogress(['Calling ' interfacedata.solver.tag],options.showprogress);end
solvertime = tic;
[x,flag,lm] = qpas(model.Q, model.c, model.A, model.b, model.Aeq, model.beq, model.lb, model.ub, options.verbose);
solvertime = toc(solvertime);

% Internal format for duals
if ~isempty(lm)
    D_struc = [lm.equality;lm.inequality];
else
    D_struc = [];
end

if isempty(flag)
    problem = 9;
else
    % Check, currently not exhaustive...
    switch flag
        case 0
            problem = 0;
        case 2
            problem = 1;
        otherwise
            problem = 1;
    end
end

% Save all data sent to solver?
if options.savesolverinput
    solverinput = model;
else
    solverinput = [];
end

% Save all data from the solver?
if options.savesolveroutput
    solveroutput.x = x;
    solveroutput.flag = flag;
    solveroutput.lm = lm;
else
    solveroutput = [];
end

% Standard interface
output = createOutputStructure(x(:),D_struc,[],problem,interfacedata.solver.tag,solverinput,solveroutput,solvertime);


function [Y, Phi] = modelOut(model, X, varargin)

% MODELOUT Give the output of a model for given X.
% FORMAT
% DESC gives the output of the model for a given input X. For
% latent variable models it gives a position in data space given a
% position in latent space.
% ARG model : structure specifying the model.
% ARG X : input location(s) for which output is to be computed.
% RETURN Y : output location(s) corresponding to given input
% locations.
%
% FORMAT
% DESC gives the output of the model for a given input X. For
% latent variable models it gives a position in data space given a
% position in latent space.
% ARG model : structure specifying the model.
% ARG X : input location(s) for which output is to be computed.
% RETURN Phi : output basis function(s) corresponding to given input
% RETURN Y : output location(s) corresponding to given input
% locations.
%
% SEEALSO : modelCreate
%
% COPYRIGHT : Neil D. Lawrence, 2005, 2006
%
% MODIFICATIONS : Cark Henrik Ek, 2008

% MLTOOLS

fhandle = str2func([model.type 'Out']);
if nargout > 1
  [Y, Phi] = fhandle(model, X, varargin{:});
else
  Y = fhandle(model, X, varargin{:});
end
if(isfield(model,'indexOut')&&~isempty(model.indexOut))
  Y(:,setdiff(1:1:size(Y,2),model.indexOut)) = NaN;
end


function connection = skelConnectionMatrix(skel);

% SKELCONNECTIONMATRIX Compute the connection matrix for the structure.
% FORMAT
% DESC computes the connection matrix for the structure. Returns a matrix
% which has zeros at all entries except those that are connected in the
% skeleton.
% ARG skel : the skeleton for which the connectivity is required.
% RETURN connection : connectivity matrix.
%
% COPYRIGHT : Neil D. Lawrence, 2006
%
% SEEALSO : skelVisualise, skelModify

% MOCAP

connection = zeros(length(skel.tree));
for i = 1:length(skel.tree);
  for j = 1:length(skel.tree(i).children)
    connection(i, skel.tree(i).children(j)) = 1;
  end
end


% OPTIMI toolbox
% Version 0.13		Monday 21 Nov 2005 at 08:49
% Copyright (c) 2005 Neil D. Lawrence
%
% CMPNDTIEPARAMETERS Tie parameters together.
% EXPTRANSFORM Constrains a parameter to be positive through exponentiation.
% NEGLOGLOGIT Constrains a parameter to be positive.
% OPTIMISEPARAMS Optimise parameters.
% OPTOPTIONS Give optimisation options for NETLAB.
% SIGMOIDTRANSFORM Constrains a parameter to be between 0 and 1.


function varargout = dsxy2figxy2(varargin)
% dsxy2figxy -- Transform point or position from axis to figure coords
% Transforms [axx axy] or [xypos] from axes hAx (data) coords into coords
% wrt GCF for placing annotation objects that use figure coords into data
% space. The annotation objects this can be used for are
%    arrow, doublearrow, textarrow
%    ellipses (coordinates must be transformed to [x, y, width, height])
% Note that line, text, and rectangle anno objects already are placed
% on a plot using axes coordinates and must be located within an axes.
% Usage: Compute a position and apply to an annotation, e.g.,
%   [axx axy] = ginput(2);
%   [figx figy] = getaxannopos(gca, axx, axy);
%   har = annotation('textarrow',figx,figy);
%   set(har,'String',['(' num2str(axx(2)) ',' num2str(axy(2)) ')'])
%% Obtain arguments (only limited argument checking is performed).
% Determine if axes handle is specified
if length(varargin{1})== 1 && ishandle(varargin{1}) && ...
  strcmp(get(varargin{1},'type'),'axes')
 hAx = varargin{1};
 varargin = varargin(2:end);
else
 hAx = gca;
end;
% Parse either a position vector or two 2-D point tuples
if length(varargin)==1 % Must be a 4-element POS vector
 pos = varargin{1};
else
 [x,y] = deal(varargin{:});  % Two tuples (start & end points)
end
%% Get limits
axun = get(hAx,'Units');
set(hAx,'Units','normalized');  % Need normaized units to do the xform
axpos = get(hAx,'Position');
axlim = axis(hAx);              % Get the axis limits [xlim ylim (zlim)]
axwidth = diff(axlim(1:2));
axheight = diff(axlim(3:4));
yisreversed = strcmpi(get(hAx,'ydir'),'reverse');
xisreversed = strcmpi(get(hAx,'xdir'),'reverse');
%% Transform data from figure space to data space
if exist('x','var')     % Transform a and return pair of points
  xout = (x-axlim(1))/axwidth;
  yout = (y-axlim(3))/axheight;
  if xisreversed,
    xout = 1 - xout;
  end
  if yisreversed
    yout = 1 - yout;
  end

  varargout{1} = xout*axpos(3) + axpos(1);
  varargout{2} = yout*axpos(4) + axpos(2);
else                    % Transform and return a position rectangle
  xout = (pos(1)-axlim(1))/axwidth;
  yout = (pos(2)-axlim(3))/axheight;
  if xisreversed,
    xout = 1 - xout;
  end
  if yisreversed,
    yout = 1 - yout;
  end
  pos(1) = xout*axpos(3) + axpos(1);
  pos(2) = yout*axpos(4) + axpos(2);
  pos(3) = pos(3)*axpos(3)/axwidth;
  pos(4) = pos(4)*axpos(4)/axheight;
  varargout{1} = pos;
end
%% Restore axes units
set(hAx,'Units',axun)


function varargout = process_baseline_norm2( varargin )
% PROCESS_BASELINE_NORM2: Normalization with respect to a baseline (A=baseline).

% @=============================================================================
% 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, 2016

eval(macro_method);
end

%% ===== GET DESCRIPTION =====
function sProcess = GetDescription() %#ok<DEFNU>
    % Description the process
    sProcess.Comment     = 'Baseline normalization (A=baseline)';
    sProcess.FileTag     = @GetFileTag;
    sProcess.Category    = 'Filter2';
    sProcess.SubGroup    = 'Standardize';
    sProcess.Index       = 205;
    sProcess.Description = 'https://neuroimage.usc.edu/brainstorm/Tutorials/SourceEstimation#Z-score';
    % Definition of the input accepted by this process
    sProcess.InputTypes  = {'data', 'results', 'timefreq', 'matrix'};
    sProcess.OutputTypes = {'data', 'results', 'timefreq', 'matrix'};
    sProcess.nInputs     = 2;
    sProcess.nMinFiles   = 1;
    % Default values for some options
    sProcess.isSourceAbsolute = 0;
    sProcess.processDim       = 1;    % Process channel by channel
    sProcess.isPaired         = 1;

    % === Process description
    sProcess.options.label1.Comment = ['This process normalizes each signal and frequency bin (FilesB)
' ...
                                       'with respect to a baseline (FilesA). In the formulas below:
'...
                                       '  x = data to normalize (FilesB)
' ...
                                       '  μ = mean over the baseline (FilesA) [mean(x(iBaseline))]
' ...
                                       '  σ = standard deviation over the baseline (FilesA) [std(x(iBaseline))]

' ...
                                       'Data selection:'];
    sProcess.options.label1.Type = 'label';
    % Common options
    sProcess = process_baseline_norm('DefineOptions', sProcess);
end

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

%% ===== GET FILE TAG =====
function fileTag = GetFileTag(sProcess)
    fileTag = sProcess.options.method.Value;
end

%% ===== RUN =====
function sInputB = Run(sProcess, sInputA, sInputB) %#ok<DEFNU>
    % Call the corresponding process1
    sInputB = process_baseline_norm('Run', sProcess, sInputA, sInputB);
end


function [imuData, frameRange] = loadImuData(imuDataFolder, imageTimestamps)
% loadImageData Read captured image data into memory. Tries to load
% individual images or a saved mat file if one exists.

    %Read IMU data
    oxtsData = loadOxtsliteData(imuDataFolder);
    % load IMU Data
    v_index = 9:11; % 12:14 body frame, 15:17 FLU frame
    a_index = 15:17; % 15:17 FLU frame
    omega_index = 21:23; % 18:20 body frame, 21:23 FLU frame

    dateStrings = loadTimestamps([imuDataFolder '/oxts']);
    timestamps = zeros(1, length(dateStrings));
    for i = 1:length(dateStrings)
        timestamps(i) =  datenum_to_unixtime(datenum(dateStrings(i))) + 0.00001;
    end

    frameRange = 1:length(timestamps);
    frameNum = length(frameRange);
    imuData.timestamps = zeros(1, frameNum);
    imuData.measVel = zeros(3, frameNum);
    imuData.measOrient = zeros(4, frameNum);
    imuData.measOmega = zeros(3,frameNum);
    imuData.measAccel = zeros(3, frameNum);
    imuData.initialVelocity = zeros(3,1);

    % for all oxts packets do
    for meas_i=1:length(frameRange)

      i = frameRange(meas_i);
      % if there is no data
      if isempty(oxtsData{i})
        continue;
      end

     imuData.timestamps(1,meas_i) = timestamps(i);
     imuData.measOmega(:,meas_i) = oxtsData{i}(omega_index);

      rx = oxtsData{i}(4); % roll
      ry = oxtsData{i}(5); % pitch
      rz = oxtsData{i}(6); % heading
      Rx = [1 0 0; 0 cos(rx) -sin(rx); 0 sin(rx) cos(rx)]; % base => nav  (level oxts => rotated oxts)
      Ry = [cos(ry) 0 sin(ry); 0 1 0; -sin(ry) 0 cos(ry)]; % base => nav  (level oxts => rotated oxts)
      Rz = [cos(rz) -sin(rz) 0; sin(rz) cos(rz) 0; 0 0 1]; % base => nav  (level oxts => rotated oxts)
      R  = Rz*Ry*Rx;

      imuData.measOrient(:,meas_i) = rotMatToQuat(R);
      imuData.measVel(:,meas_i) =  oxtsData{i}(v_index);
      imuData.measAccel(:,meas_i) =  oxtsData{i}(a_index)' - R'*[0;0;9.81];

      if meas_i == 1
          imuData.initialVelocity = getRnb(oxtsData{i})'*[ oxtsData{i}(8); oxtsData{i}(7); 0; ];
      end
    end

    function dn = datenum_to_unixtime( date_num )
      dn =  (date_num - 719529)*86400;         %# 719529 == datenum(1970,1,1)
    end

end


function t_opf_tspopf_pdipm(quiet)
%T_OPF_TSPOPF_PDIPM  Tests for PDIPM-based optimal power flow.

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

if nargin < 1
    quiet = 0;
end

num_tests = 159;

t_begin(num_tests, quiet);

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

casefile = 't_case9_opf';
if quiet
    verbose = 0;
else
    verbose = 0;
end

t0 = 'PDIPMOPF : ';
mpopt = mpoption('opf.violation', 1e-6, 'pdipm.gradtol', 1e-8, ...
        'pdipm.comptol', 1e-8, 'pdipm.costtol', 1e-9);
mpopt = mpoption(mpopt, 'out.all', 0, 'verbose', verbose, 'opf.ac.solver', 'PDIPM');

%% turn off some warnings
if have_feature('matlab')
    sing_matrix_warn_id = 'MATLAB:nearlySingularMatrix';
    s2 = warning('query', sing_matrix_warn_id);
    warning('off', sing_matrix_warn_id);
end

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

    %% get solved AC OPF case from MAT-file
    load soln9_opf;     %% defines bus_soln, gen_soln, branch_soln, f_soln

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

    %% run with automatic conversion of single-block pwl to linear costs
    t = [t0 '(single-block PWL) : '];
    mpc = loadcase(casefile);
    mpc.gencost(2, NCOST) = 2;
    [r, success] = runopf(mpc, mpopt);
    [f, bus, gen, branch] = deal(r.f, r.bus, r.gen, r.branch);
    t_ok(success, [t 'success']);
    t_is(f, f_soln, 3, [t 'f']);
    t_is(   bus(:,ib_data   ),    bus_soln(:,ib_data   ), 10, [t 'bus data']);
    t_is(   bus(:,ib_voltage),    bus_soln(:,ib_voltage),  3, [t 'bus voltage']);
    t_is(   bus(:,ib_lam    ),    bus_soln(:,ib_lam    ),  3, [t 'bus lambda']);
    t_is(   bus(:,ib_mu     ),    bus_soln(:,ib_mu     ),  2, [t 'bus mu']);
    t_is(   gen(:,ig_data   ),    gen_soln(:,ig_data   ), 10, [t 'gen data']);
    t_is(   gen(:,ig_disp   ),    gen_soln(:,ig_disp   ),  3, [t 'gen dispatch']);
    t_is(   gen(:,ig_mu     ),    gen_soln(:,ig_mu     ),  3, [t 'gen mu']);
    t_is(branch(:,ibr_data  ), branch_soln(:,ibr_data  ), 10, [t 'branch data']);
    t_is(branch(:,ibr_flow  ), branch_soln(:,ibr_flow  ),  3, [t 'branch flow']);
    t_is(branch(:,ibr_mu    ), branch_soln(:,ibr_mu    ),  2, [t 'branch mu']);
    xr = [r.var.val.Va;r.var.val.Vm;r.var.val.Pg;r.var.val.Qg;0;r.var.val.y];
    t_is(r.x, xr, 8, [t 'raw x returned from OPF']);

%     %% get solved AC OPF case from MAT-file
%     load soln9_opf_Plim;       %% defines bus_soln, gen_soln, branch_soln, f_soln
%
%     %% run OPF with active power line limits
%     t = [t0 '(P line lim) : '];
%     mpopt1 = mpoption(mpopt, 'opf.flow_lim', 'P');
%     [baseMVA, bus, gen, gencost, branch, f, success, et] = runopf(casefile, mpopt1);
%     t_ok(success, [t 'success']);
%     t_is(f, f_soln, 3, [t 'f']);
%     t_is(   bus(:,ib_data   ),    bus_soln(:,ib_data   ), 10, [t 'bus data']);
%     t_is(   bus(:,ib_voltage),    bus_soln(:,ib_voltage),  3, [t 'bus voltage']);
%     t_is(   bus(:,ib_lam    ),    bus_soln(:,ib_lam    ),  3, [t 'bus lambda']);
%     t_is(   bus(:,ib_mu     ),    bus_soln(:,ib_mu     ),  2, [t 'bus mu']);
%     t_is(   gen(:,ig_data   ),    gen_soln(:,ig_data   ), 10, [t 'gen data']);
%     t_is(   gen(:,ig_disp   ),    gen_soln(:,ig_disp   ),  3, [t 'gen dispatch']);
%     t_is(   gen(:,ig_mu     ),    gen_soln(:,ig_mu     ),  3, [t 'gen mu']);
%     t_is(branch(:,ibr_data  ), branch_soln(:,ibr_data  ), 10, [t 'branch data']);
%     t_is(branch(:,ibr_flow  ), branch_soln(:,ibr_flow  ),  3, [t 'branch flow']);
%     t_is(branch(:,ibr_mu    ), branch_soln(:,ibr_mu    ),  2, [t 'branch mu']);
%
%     %% run OPF with active power line limits
%     t = [t0 '(P^2 line lim) : '];
%     mpopt1 = mpoption(mpopt, 'opf.flow_lim', '2');
%     [baseMVA, bus, gen, gencost, branch, f, success, et] = runopf(casefile, mpopt1);
%     t_ok(success, [t 'success']);
%     t_is(f, f_soln, 3, [t 'f']);
%     t_is(   bus(:,ib_data   ),    bus_soln(:,ib_data   ), 10, [t 'bus data']);
%     t_is(   bus(:,ib_voltage),    bus_soln(:,ib_voltage),  3, [t 'bus voltage']);
%     t_is(   bus(:,ib_lam    ),    bus_soln(:,ib_lam    ),  3, [t 'bus lambda']);
%     t_is(   bus(:,ib_mu     ),    bus_soln(:,ib_mu     ),  2, [t 'bus mu']);
%     t_is(   gen(:,ig_data   ),    gen_soln(:,ig_data   ), 10, [t 'gen data']);
%     t_is(   gen(:,ig_disp   ),    gen_soln(:,ig_disp   ),  3, [t 'gen dispatch']);
%     t_is(   gen(:,ig_mu     ),    gen_soln(:,ig_mu     ),  3, [t 'gen mu']);
%     t_is(branch(:,ibr_data  ), branch_soln(:,ibr_data  ), 10, [t 'branch data']);
%     t_is(branch(:,ibr_flow  ), branch_soln(:,ibr_flow  ),  3, [t 'branch flow']);
%     t_is(branch(:,ibr_mu    ), branch_soln(:,ibr_mu    ),  2, [t 'branch mu']);

    %%-----  test OPF with quadratic gen costs moved to generalized costs  -----
    mpc = loadcase(casefile);
    mpc.gencost = [
        2   1500    0   3   0.11    5   0;
        2   2000    0   3   0.085   1.2 0;
        2   3000    0   3   0.1225  1   0;
    ];
    [baseMVA, bus_soln, gen_soln, gencost, branch_soln, f_soln, success, et] = runopf(mpc, mpopt);
    branch_soln = branch_soln(:,1:MU_ST);

    A = sparse(0,0);
    l = [];
    u = [];
    nb = size(mpc.bus, 1);      % number of buses
    ng = size(mpc.gen, 1);      % number of gens
    thbas = 1;                thend    = thbas+nb-1;
    vbas     = thend+1;       vend     = vbas+nb-1;
    pgbas    = vend+1;        pgend    = pgbas+ng-1;
    qgbas    = pgend+1;       qgend    = qgbas+ng-1;
    nxyz = 2*nb + 2*ng;
    N = sparse((1:ng)', (pgbas:pgend)', mpc.baseMVA * ones(ng,1), ng, nxyz);
    fparm = [ 1    0   0 1;
              1 -100 100 1;
              1  -10  10 1 ];
    H = 2 * spdiags(mpc.gencost(:, 5), 0, ng, ng);
    Cw = mpc.gencost(:, 6);
    mpc.gencost(:, 5:7) = 0;

    %% run OPF with quadratic gen costs moved to generalized costs
    t = [t0 'w/quadratic generalized gen cost : '];
    [r, success] = opf(mpc, A, l, u, mpopt, N, fparm, H, Cw);
    [f, bus, gen, branch] = deal(r.f, r.bus, r.gen, r.branch);
    t_ok(success, [t 'success']);
    t_is(f, f_soln, 3, [t 'f']);
    t_is(   bus(:,ib_data   ),    bus_soln(:,ib_data   ), 10, [t 'bus data']);
    t_is(   bus(:,ib_voltage),    bus_soln(:,ib_voltage),  3, [t 'bus voltage']);
    t_is(   bus(:,ib_lam    ),    bus_soln(:,ib_lam    ),  3, [t 'bus lambda']);
    t_is(   bus(:,ib_mu     ),    bus_soln(:,ib_mu     ),  2, [t 'bus mu']);
    t_is(   gen(:,ig_data   ),    gen_soln(:,ig_data   ), 10, [t 'gen data']);
    t_is(   gen(:,ig_disp   ),    gen_soln(:,ig_disp   ),  3, [t 'gen dispatch']);
    t_is(   gen(:,ig_mu     ),    gen_soln(:,ig_mu     ),  3, [t 'gen mu']);
    t_is(branch(:,ibr_data  ), branch_soln(:,ibr_data  ), 10, [t 'branch data']);
    t_is(branch(:,ibr_flow  ), branch_soln(:,ibr_flow  ),  3, [t 'branch flow']);
    t_is(branch(:,ibr_mu    ), branch_soln(:,ibr_mu    ),  2, [t 'branch mu']);
    t_is(r.cost.usr, f, 12, [t 'user cost']);

    %%-----  run OPF with legacy costs and deadzone  -----
    load soln9_opf;
    mpc = loadcase(casefile);
    mpc.N = sparse((1:nb)', (vbas:vend)', ones(nb,1), nb, nxyz);
    mpc.fparm = ones(nb,1) * [ 2 1.08 0.02 1e8 ];
    mpc.Cw = ones(nb, 1);
    t = [t0 'w/legacy cost, in deadzone : '];
    r = runopf(mpc, mpopt);
    [f, bus, gen, branch] = deal(r.f, r.bus, r.gen, r.branch);
    t_ok(r.success, [t 'success']);
    t_is(f, f_soln, 3, [t 'f']);
    t_is(   bus(:,ib_data   ),    bus_soln(:,ib_data   ), 10, [t 'bus data']);
    t_is(   bus(:,ib_voltage),    bus_soln(:,ib_voltage),  3, [t 'bus voltage']);
    t_is(   bus(:,ib_lam    ),    bus_soln(:,ib_lam    ),  3, [t 'bus lambda']);
    t_is(   bus(:,ib_mu     ),    bus_soln(:,ib_mu     ),  2, [t 'bus mu']);
    t_is(   gen(:,ig_data   ),    gen_soln(:,ig_data   ), 10, [t 'gen data']);
    t_is(   gen(:,ig_disp   ),    gen_soln(:,ig_disp   ),  3, [t 'gen dispatch']);
    t_is(   gen(:,ig_mu     ),    gen_soln(:,ig_mu     ),  3, [t 'gen mu']);
    t_is(branch(:,ibr_data  ), branch_soln(:,ibr_data  ), 10, [t 'branch data']);
    t_is(branch(:,ibr_flow  ), branch_soln(:,ibr_flow  ),  3, [t 'branch flow']);
    t_is(branch(:,ibr_mu    ), branch_soln(:,ibr_mu    ),  2, [t 'branch mu']);
    t_is(r.cost.usr, 0, 12, [t 'user cost']);

    t = [t0 'w/legacy cost, not in deadzone : '];
    mpc.fparm = ones(nb,1) * [ 2 1.08 0.01 1e8 ];
    r = runopf(mpc, mpopt);
    [f, bus, gen, branch] = deal(r.f, r.bus, r.gen, r.branch);
    t_ok(r.success, [t 'success']);
    t_is(f, 9009.0890, 3, [t 'f']);
    t_is([min(bus(:, VM)) mean(bus(:, VM)) max(bus(:, VM))], ...
        [1.066624, 1.083980, 1.091698], 5, [t 'bus voltage']);
    t_is(r.cost.usr, 1673.065465, 5, [t 'user cost']);

    %%-----  run OPF with extra linear user constraints & costs  -----
    %% single new z variable constrained to be greater than or equal to
    %% deviation from 1 pu voltage at bus 1, linear cost on this z
    %% get solved AC OPF case from MAT-file
    load soln9_opf_extras1;   %% defines bus_soln, gen_soln, branch_soln, f_soln
    A = sparse([1;1;2;2],[10;25;10;25],[-1;1;1;1],2,25);
    u = [Inf; Inf];
    l = [-1; 1];

    N = sparse(1, 25, 1, 1, 25);    %% new z variable only
    fparm = [1 0 0 1];              %% w = r = z
    H = sparse(1,1);                %% no quadratic term
    Cw = 100;

    t = [t0 'w/extra constraints & costs 1 : '];
    [r, success] = opf(casefile, A, l, u, mpopt, N, fparm, H, Cw);
    [f, bus, gen, branch] = deal(r.f, r.bus, r.gen, r.branch);
    t_ok(success, [t 'success']);
    t_is(f, f_soln, 3, [t 'f']);
    t_is(   bus(:,ib_data   ),    bus_soln(:,ib_data   ), 10, [t 'bus data']);
    t_is(   bus(:,ib_voltage),    bus_soln(:,ib_voltage),  3, [t 'bus voltage']);
    t_is(   bus(:,ib_lam    ),    bus_soln(:,ib_lam    ),  3, [t 'bus lambda']);
    t_is(   bus(:,ib_mu     ),    bus_soln(:,ib_mu     ),  2, [t 'bus mu']);
    t_is(   gen(:,ig_data   ),    gen_soln(:,ig_data   ), 10, [t 'gen data']);
    t_is(   gen(:,ig_disp   ),    gen_soln(:,ig_disp   ),  3, [t 'gen dispatch']);
    t_is(   gen(:,ig_mu     ),    gen_soln(:,ig_mu     ),  3, [t 'gen mu']);
    t_is(branch(:,ibr_data  ), branch_soln(:,ibr_data  ), 10, [t 'branch data']);
    t_is(branch(:,ibr_flow  ), branch_soln(:,ibr_flow  ),  3, [t 'branch flow']);
    t_is(branch(:,ibr_mu    ), branch_soln(:,ibr_mu    ),  2, [t 'branch mu']);
    t_is(r.var.val.z, 0.025419, 6, [t 'user variable']);
    t_is(r.cost.usr, 2.5419, 4, [t 'user cost']);

    %%-----  test OPF with capability curves  -----
    mpc = loadcase('t_case9_opfv2');
    %% remove angle diff limits
    mpc.branch(1, ANGMAX) = 360;
    mpc.branch(9, ANGMIN) = -360;

    %% get solved AC OPF case from MAT-file
    load soln9_opf_PQcap;   %% defines bus_soln, gen_soln, branch_soln, f_soln

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

    %%-----  test OPF with angle difference limits  -----
    mpc = loadcase('t_case9_opfv2');
    %% remove capability curves
    mpc.gen(2:3, [PC1, PC2, QC1MIN, QC1MAX, QC2MIN, QC2MAX]) = zeros(2,6);

    %% get solved AC OPF case from MAT-file
    load soln9_opf_ang;   %% defines bus_soln, gen_soln, branch_soln, f_soln

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

    %%-----  test OPF with ignored angle difference limits  -----
    %% get solved AC OPF case from MAT-file
    load soln9_opf;   %% defines bus_soln, gen_soln, branch_soln, f_soln

    %% run OPF with ignored angle difference limits
    t = [t0 'w/ignored angle difference limits : '];
    mpopt1 = mpoption(mpopt, 'opf.ignore_angle_lim', 1);
    [baseMVA, bus, gen, gencost, branch, f, success, et] = runopf(mpc, mpopt1);
    %% ang limits are not in this solution data, so let's remove them
    branch(1, ANGMAX) = 360;
    branch(9, ANGMIN) = -360;
    t_ok(success, [t 'success']);
    t_is(f, f_soln, 3, [t 'f']);
    t_is(   bus(:,ib_data   ),    bus_soln(:,ib_data   ), 10, [t 'bus data']);
    t_is(   bus(:,ib_voltage),    bus_soln(:,ib_voltage),  3, [t 'bus voltage']);
    t_is(   bus(:,ib_lam    ),    bus_soln(:,ib_lam    ),  3, [t 'bus lambda']);
    t_is(   bus(:,ib_mu     ),    bus_soln(:,ib_mu     ),  2, [t 'bus mu']);
    t_is(   gen(:,ig_data   ),    gen_soln(:,ig_data   ), 10, [t 'gen data']);
    t_is(   gen(:,ig_disp   ),    gen_soln(:,ig_disp   ),  3, [t 'gen dispatch']);
    t_is(   gen(:,ig_mu     ),    gen_soln(:,ig_mu     ),  3, [t 'gen mu']);
    t_is(branch(:,ibr_data  ), branch_soln(:,ibr_data  ), 10, [t 'branch data']);
    t_is(branch(:,ibr_flow  ), branch_soln(:,ibr_flow  ),  3, [t 'branch flow']);
    t_is(branch(:,ibr_mu    ), branch_soln(:,ibr_mu    ),  2, [t 'branch mu']);

    %% angle bounded above by 0, unbounded below
    %% for issue/18
    t = [t0 'w/angle difference limit = 0 : '];
    mpc = loadcase(casefile);
    b = 5;
    mpc.branch(b, ANGMAX) = 0;
    r = runopf(mpc, mpopt);
    t_ok(success, [t 'success']);
    diff = r.bus(r.branch(b, F_BUS), VA) - r.bus(r.branch(b, T_BUS), VA);
    t_is(diff, 0, 5, [t 'angle diff']);

    %%-----  OPF with ref bus not = bus 1, ref angle not = 0  -----
    t = [t0 'ref bus ~= 1, ref ang ~= 0 : '];
    mpc = loadcase(casefile);
    mpc.bus([1;3], BUS_TYPE) = [PV; REF];   %% swap bus types
    bus_soln([1;3], BUS_TYPE) = bus_soln([3;1], BUS_TYPE);   %% swap bus types
    mpc.bus(3, VA) = 3.3014277;
    r = runopf(mpc, mpopt);
    [success, f, bus, gen, branch] = deal(r.success, r.f, r.bus, r.gen, r.branch);
    t_ok(success, [t 'success']);
    t_is(f, f_soln, 3, [t 'f']);
    t_is(   bus(:,ib_data   ),    bus_soln(:,ib_data   ), 10, [t 'bus data']);
    t_is(   bus(:,ib_voltage),    bus_soln(:,ib_voltage),  3, [t 'bus voltage']);
    t_is(   bus(:,ib_lam    ),    bus_soln(:,ib_lam    ),  3, [t 'bus lambda']);
    t_is(   bus(:,ib_mu     ),    bus_soln(:,ib_mu     ),  2, [t 'bus mu']);
    t_is(   gen(:,ig_data   ),    gen_soln(:,ig_data   ), 10, [t 'gen data']);
    t_is(   gen(:,ig_disp   ),    gen_soln(:,ig_disp   ),  3, [t 'gen dispatch']);
    t_is(   gen(:,ig_mu     ),    gen_soln(:,ig_mu     ),  3, [t 'gen mu']);
    t_is(branch(:,ibr_data  ), branch_soln(:,ibr_data  ), 10, [t 'branch data']);
    t_is(branch(:,ibr_flow  ), branch_soln(:,ibr_flow  ),  3, [t 'branch flow']);
    t_is(branch(:,ibr_mu    ), branch_soln(:,ibr_mu    ),  2, [t 'branch mu']);

    %%-----  test OPF with opf.use_vg  -----
    %% get solved AC OPF case from MAT-file
    load soln9_opf_vg;  %% defines bus_soln, gen_soln, branch_soln, f_soln
                        %%    and bus_soln1, gen_soln1, branch_soln1, f_soln1

    %% run with opf.use_vg = 1
    t = [t0 'w/opf.use_vg = 1 : '];
    mpc = loadcase(casefile);
    mpc.gen = mpc.gen([1 2 1 3], :);
    mpc.gencost = mpc.gencost([1 2 1 3], :);
    mpc.gen([1 3], [PMAX PMIN]) = mpc.gen([1 3], [PMAX PMIN]) / 2;
    mpc.gen(3, [QMIN, QMAX]) = 0;   %% no reactive capability for gen 3
    mpc.gencost([1 3], COST:end) = mpc.gencost([1 3], COST:end) / 2;
    mpc.gen(1, VG) = 1.05;
    mpc.gen(3, VG) = 1.06;
    mpopt1 = mpoption(mpopt, 'opf.use_vg', 1);
    r = runopf(mpc, mpopt1);
    t_ok(r.success, [t 'success']);
    t_is(r.f, f_soln, 3, [t 'f']);
    t_is(   r.bus(:,ib_data   ),    bus_soln(:,ib_data   ), 10, [t 'bus data']);
    t_is(   r.bus(:,ib_voltage),    bus_soln(:,ib_voltage),  3, [t 'bus voltage']);
    t_is(   r.bus(:,ib_lam    ),    bus_soln(:,ib_lam    ),  3, [t 'bus lambda']);
    t_is(   r.bus(:,ib_mu     ),    bus_soln(:,ib_mu     ),  2, [t 'bus mu']);
    t_is(   r.gen(:,ig_data   ),    gen_soln(:,ig_data   ), 10, [t 'gen data']);
    t_is(   r.gen(:,ig_disp   ),    gen_soln(:,ig_disp   ),  3, [t 'gen dispatch']);
    t_is(   r.gen(:,ig_mu     ),    gen_soln(:,ig_mu     ),  3, [t 'gen mu']);
    t_is(r.branch(:,ibr_data  ), branch_soln(:,ibr_data  ), 10, [t 'branch data']);
    t_is(r.branch(:,ibr_flow  ), branch_soln(:,ibr_flow  ),  3, [t 'branch flow']);
    t_is(r.branch(:,ibr_mu    ), branch_soln(:,ibr_mu    ),  2, [t 'branch mu']);

    %% run with opf.use_vg = 0.9
    t = [t0 'w/opf.use_vg = 0.9 : '];
    mpopt1 = mpoption(mpopt, 'opf.use_vg', 0.9);
    r = runopf(mpc, mpopt1);
    t_ok(r.success, [t 'success']);
    t_is(r.f, f_soln1, 3, [t 'f']);
    t_is(   r.bus(:,ib_data   ),    bus_soln1(:,ib_data   ), 10, [t 'bus data']);
    t_is(   r.bus(:,ib_voltage),    bus_soln1(:,ib_voltage),  3, [t 'bus voltage']);
    t_is(   r.bus(:,ib_lam    ),    bus_soln1(:,ib_lam    ),  3, [t 'bus lambda']);
    t_is(   r.bus(:,ib_mu     ),    bus_soln1(:,ib_mu     ),  2, [t 'bus mu']);
    t_is(   r.gen(:,ig_data   ),    gen_soln1(:,ig_data   ), 10, [t 'gen data']);
    t_is(   r.gen(:,ig_disp   ),    gen_soln1(:,ig_disp   ),  3, [t 'gen dispatch']);
    t_is(   r.gen(:,ig_mu     ),    gen_soln1(:,ig_mu     ),  3, [t 'gen mu']);
    t_is(r.branch(:,ibr_data  ), branch_soln1(:,ibr_data  ), 10, [t 'branch data']);
    t_is(r.branch(:,ibr_flow  ), branch_soln1(:,ibr_flow  ),  3, [t 'branch flow']);
    t_is(r.branch(:,ibr_mu    ), branch_soln1(:,ibr_mu    ),  2, [t 'branch mu']);

    t = [t0 'hi-deg polynomial Pg costs : '];
    mpc = loadcase(casefile);
    mpc.gencost = [
        2   1500    0   6   1e-6/5  0   0   0   0   0;
        2   3000    0   5   1e-4/4  0   0   0   0   0;
        2   2000    0   3   1/2     0   0   0   0   0;
    ];
    r = runopf(mpc, mpopt);
    [f, bus, gen, branch] = deal(r.f, r.bus, r.gen, r.branch);
    t_ok(r.success, [t 'success']);
    t_is(f, 11899.4652, 4, [t 'f']);
    t_is(gen(:, PG), [100.703628; 88.719864; 128.679485], 5, [t 'Pg']);
    t_is(gen(:, QG), [8.151114; -27.105856; 12.471647], 5, [t 'Qg']);
    t_is([min(bus(:, VM)) mean(bus(:, VM)) max(bus(:, VM))], ...
        [1.059191 1.079404 1.1], 5, [t 'bus voltage']);

    t = [t0 'hi-deg polynomial Qg costs : '];
    mpc = loadcase(casefile);
    mpc.gencost = [
        2   1500    0   6   1e-6/5  0   0   0   0   0;
        2   3000    0   5   1e-4/4  0   0   0   0   0;
        2   2000    0   3   1/2     0   0   0   0   0;
        2   0       0   6   1e-6/5  0   0   0   0   0;
        2   0       0   3   1/2     0   0   0   0   0;
        2   0       0   5   1e-4/4  0   0   0   0   0;
    ];
    r = runopf(mpc, mpopt);
    [f, bus, gen, branch] = deal(r.f, r.bus, r.gen, r.branch);
    t_ok(r.success, [t 'success']);
    t_is(f, 11998.8345, 4, [t 'f']);
    t_is(gen(:, PG), [100.853092; 89.258007; 128.518423], 5, [t 'Pg']);
    t_is(gen(:, QG), [-20.834352; -7.966093; 34.591995], 5, [t 'Qg']);
    t_is([min(bus(:, VM)) mean(bus(:, VM)) max(bus(:, VM))], ...
        [1.007729 1.050776 1.1], 5, [t 'bus voltage']);

%     %% OPF with user-defined nonlinear constraints
%     t = [t0 'w/nonlin eq constraint : '];
%     mpc = loadcase('case30');
%     mpc.user_constraints.nle = {
%         {'Pg_usr', 1, 'opf_nle_fcn1', 'opf_nle_hess1', {'Pg'}, {}}
%     };
%     r = runopf(mpc, mpopt);
%     t_ok(r.success, [t 'success']);
%     t_is(r.gen(1, PG) * r.gen(2, PG) / 100, r.gen(6, PG), 8, t);
%     t_is(r.gen(6, PG), 20.751163, 5, t);
%     %% check for indexing error, issue #77
%     t_is(r.raw.pimul(61), 0, 6, [t 'issue #77']);
%     t_is(r.mu.nln.u(61), 0, 6, [t 'issue #77']);

    %% OPF with all buses isolated
    t = [t0 'all buses isolated : '];
    mpc.bus(:, BUS_TYPE) = NONE;
    try
        r = runopf(mpc, mpopt);
        t_is(r.success, 0, 12, [t 'success = 0']);
    catch
        t_ok(0, [t 'unexpected fatal error']);
    end

    %% OPF with no branch limits
    t = [t0 'w/no branch limits : '];
    mpc = loadcase(casefile);
    mpc.branch(:, RATE_A) = 0;
    r = runopf(mpc, mpopt);
    t_ok(r.success, [t 'success']);
    t_is(r.f, 5496.128635, 4, [t 'f']);
    t_is(r.gen(:, PG), [90; 220.463932; 10], 5, [t 'Pg']);
    t_is([min(r.bus(:, VM)) mean(r.bus(:, VM)) max(r.bus(:, VM))], ...
        [1.070692 1.090449 1.1], 5, [t 'bus voltage']);
else
    t_skip(num_tests, [t0 'not available']);
end

if have_feature('matlab')
    warning(s2.state, sing_matrix_warn_id);
end

t_end;


function [ resized_patch ] = get_pixels(im, pos, sz, resize_target)

if isscalar(sz)  %square sub-window
    sz = [sz, sz];
end

%make sure the size is not to small
if sz(1) < 1
    sz(1) = 2;
end
if sz(2) < 1
    sz(2) = 2;
end

xs = floor(pos(2)) + (1:sz(2)) - floor(sz(2)/2);
ys = floor(pos(1)) + (1:sz(1)) - floor(sz(1)/2);

%check for out-of-bounds coordinates, and set them to the values at
%the borders
xs(xs < 1) = 1;
ys(ys < 1) = 1;
xs(xs > size(im,2)) = size(im,2);
ys(ys > size(im,1)) = size(im,1);

%extract image
im_patch = im(ys, xs, :);

if isempty(resize_target)
    resized_patch = im_patch;
else
    resized_patch = mexResize(im_patch,resize_target,'auto');
end
end


function change_making_test01 ( )

%*****************************************************************************80
%
%% CHANGE_MAKING_TEST01 lists the problem data.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 August 2014
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CHANGE_MAKING_TEST01:\n' );
  fprintf ( 1, '  List the problem data.\n' );

  test_num = 7;

  coin_num_list = [ ...
    3, ...
    5, ...
    6, ...
    7, ...
    3, ...
    6, ...
    3 ];
  coin_value_list = { ...
    [  5,  9, 13 ], ...
    [  1,  4,  5,  8, 11 ], ...
    [  1,  5, 10, 25, 50, 100 ], ...
    [  1,  2,  6, 12, 24,  48,  60 ], ...
    [  1,  3,  4 ], ...
    [ 16, 17, 23, 24, 39,  40 ], ...
    [  6,  9, 20 ] };
  target_list = [ ...
    19, ...
    29, ...
    96, ...
    96, ...
     6, ...
   100, ...
    43 ];

  for test = 1 : test_num

    coin_num = coin_num_list(test);
    coin_value = coin_value_list{test};
    target = target_list(test);

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Test %d:\n', test );
    fprintf ( 1, '  Number of coins = %d\n', coin_num );
    fprintf ( 1, '  Values = ' );
    for i = 1 : coin_num
      fprintf ( 1, '  %d', coin_value(i) );
    end
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Target = %d\n', target );

  end

  return
end


function [fx,dfdx,dfdp] = f_HRF2(Xt,P,ut,in)
% Balloon (HRF) model evolution function in log-space
% function [fx,dfdx,dfdp] = f_HRF2(Xt,P,ut,in)
% This function evaluates the evolution function derived from the balloon
% model for the hemodynamic response function. It can be called in two
% ways: (i) as a "stand-alone" evolution function, whereby the system's
% states are hemodynamic states of the balloon model, or (ii) as a
% generalized observation function, where the real system's states are the
% neuronal states of a DCM for fMRI model. Note that the hemodynamic states
% are in log-space, for positivity constrints.

deltat = in.deltat;
n = size(Xt,1);

% Get parameters
[E0,V0,tau0,kaf,kas,epsilon,alpha] = BOLD_parameters;
if isfield(in,'fullDCM') && in.fullDCM
    nreg = n./5;
    ind1 = in.ind1;
    ind2 = in.ind2;
    ind3 = in.ind3;
    ind4 = in.ind4;
    n1 = in.n1;
    n2 = in.n2;
    n3 = in.n3;
    n4 = in.n4;
    try, n5=in.n5; catch, n5=[];end
    epsilon = 1;
    alpha = alpha.*exp(P(in.ind5));
else
    nreg = n./4;
    ind1 = 1;
    ind2 = 2;
    ind3 = 3;
    ind4 = 4;
    n1 = 1;
    n2 = 2;
    n3 = 3;
    n4 = 4;
    epsilon = epsilon.*exp(P(5));
    alpha = alpha.*exp(P(6));
end
% [E0,dsdp] = sigm(P(ind1)-0.6633,struct('beta',1,'G0',1,'INV',0));
% E0 = E0(:);
E0 = 1./(1+exp(-(P(ind1)-0.6633)));
dsdp = E0.*(1-E0);
tau0 = tau0.*exp(P(ind2));
kaf = kaf.*exp(P(ind3));
kas = kas.*exp(P(ind4));

if isfield(in,'xshift') % for numerical stability
    xshift = in.xshift;
else
    xshift = 0;
end

% hemodynamic states ...
if isfield(in,'linearized') && in.linearized
    x1 = zeros(nreg,1);
    x2 = ones(nreg,1);
    x3 = ones(nreg,1);
    x4 = ones(nreg,1);
else
    % vasodilatory signal s(t)
    x1 = Xt(n1,:);
    % blood inflow f(t)
    if isfield(in,'logx2') && ~in.logx2
        x2 = Xt(n2,:) + 1; % deviation to steady-state!
    else
        x2 = exp(Xt(n2,:)) + xshift;
    end
    % blood volume v(t)
    x3 = exp(Xt(n3,:)) + 0.*xshift;
    % dHb content q(t)
    x4 = exp(Xt(n4,:)) + 0.*xshift;
end
% blood outflow
fv = x3.^(1./alpha);
% d[blood flow]/dXt(3)
dfvdx = (1./alpha).*fv;
% oxygen extraction
ff = (1-(1-E0).^(1./x2))./E0;
% d[O2 extraction]/dXt(2)
if isfield(in,'logx2') && ~in.logx2
    dffdx = log(1-E0).*(1-E0).^(1./x2)./(E0.*x2.^2);
else
    dffdx = log(1-E0).*(1-E0).^(1./x2)./(E0.*x2);
end
% ... and flow field, derivatives, etc...
f = zeros(n,1);
J = zeros(n,n);
dfdp = zeros(size(P,1),n);

% Evaluate flow field
f(n1) = (epsilon.*ut - kas.*x1 - kaf.*(x2 - 1));
if isfield(in,'logx2') && ~in.logx2
    f(n2) = x1;
else
    f(n2) = x1./x2;
end
f(n3) = (x2 - fv)./(tau0.*x3);
f(n4) = (x2.*ff./x4 - fv./x3)./tau0;

% Evaluate jacobian and gradients wrt parameters
for i=1:nreg

    if isfield(in,'logx2') && ~in.logx2
        J(n1(i),n1(i):n1(i)+3) = [ -kas(i) , 1, 0, 0 ];
        J(n2(i),n1(i):n1(i)+3) = [ -kaf(i), 0, ...
            1./(tau0(i).*x3(i)), ...
            (ff(i)+x2(i).*dffdx(i))./(x4(i).*tau0(i))];
    else
        J(n1(i),n1(i):n1(i)+3) = [ -kas(i) , 1./x2(i), 0, 0 ];
        J(n2(i),n1(i):n1(i)+3) = [ -kaf(i).*x2(i), -x1(i)./x2(i), ...
            x2(i)./(tau0(i).*x3(i)), ...
            x2(i).*(ff(i)+dffdx(i))./(tau0(i).*x4(i))];
    end
    J(n3(i),n1(i):n1(i)+3) = [ 0, 0,...
        -f(n3(i)) - dfvdx(i)./(tau0(i).*x3(i)) , ...
        (fv(i)-dfvdx(i))./(tau0(i).*x3(i))];
    J(n4(i),n1(i):n1(i)+3) = [ 0, 0, 0, ...
        -(x2(i).*ff(i))./(tau0(i).*x4(i)) ];

    if isfield(in,'linearized') && in.linearized

        tmp = log(1-E0(i))./E0(i);

        dfdp(ind1(i),n1(i):n1(i)+3) = [0,0,0,...
            -dsdp(i).*(1+tmp).*Xt(n2(i))./(tau0(i).*E0(i))];
        dfdp(ind2(i),n1(i):n1(i)+3) = [0,0,...
            (-Xt(n2(i))+Xt(n3(i))./alpha(i))./tau0(i),...
            -((ff(i)+dffdx(i)).*Xt(n2(i)) + (fv(i)-dfvdx(i)).*Xt(n3(i)) ...
            - ff(i).*Xt(n4(i)))./tau0(i)];
        dfdp(ind3(i),n1(i):n1(i)+3) = kaf(i).*[-Xt(n2(i)),0,0,0];
        dfdp(ind4(i),n1(i):n1(i)+3) = kas(i).*[-Xt(n1(i)),0,0,0];

        if isfield(in,'fullDCM') && in.fullDCM
            if  ~isempty(n5)
                J(n5(i),n1(i)) = epsilon;
            end
            dfdp(in.ind5(i),n1(i):n1(i)+3) = [0,0,...
                Xt(n3(i))./(tau0(i).*alpha(i)),...
                Xt(n3(i))./(tau0(i).*alpha(i))];
        else
            dfdp(5,:) = epsilon.*[ut,0,0,0];
            dfdp(6,:) = [0,0,...
                Xt(n3(i))./(tau0(i).*alpha(i)),...
                Xt(n3(i))./(tau0(i).*alpha(i))];
        end

    else

        % gradient wrt parameters
        dfdp(ind1(i),n1(i):n1(i)+3) = [0,0,0,...
            (((1-E0(i)).^(-1+1./x2(i)))-x2(i).*ff(i))...
            .*dsdp(i)./(tau0(i).*x4(i).*E0(i))];
        dfdp(ind2(i),n1(i):n1(i)+3) = [0,0,...
            -(x2(i) - fv(i))./(tau0(i).*x3(i)),...
            -(x2(i).*ff(i)./x4(i) - fv(i)./x3(i))./tau0(i)];
        dfdp(ind3(i),n1(i):n1(i)+3) = kaf(i).*[-x2(i)+1,0,0,0];
        dfdp(ind4(i),n1(i):n1(i)+3) = kas(i).*[-x1(i),0,0,0];

        % complement gradients if used for full DCM model
        if isfield(in,'fullDCM') && in.fullDCM
            if  ~isempty(n5)
                J(n5(i),n1(i)) = epsilon;
            end
            dfdp(in.ind5(i),n1(i):n1(i)+3) = [0,0,...
                log(x3(i)).*fv(i)./(tau0(i).*x3(i).*alpha(i)),...
                log(x3(i)).*fv(i)./(tau0(i).*x3(i).*alpha(i))];
        else
            dfdp(5,:) = epsilon.*[ut,0,0,0];
            dfdp(6,:) = [0,0,...
                log(x3).*fv./(tau0.*x3.*alpha),...
                log(x3).*fv./(tau0.*x3.*alpha)];
        end

    end

end

% Apply Euler discretization
if isfield(in,'linearized') && in.linearized
     if isfield(in,'fullDCM') && in.fullDCM && ~isempty(n5)
         Cu = 0;
     else
         Cu = zeros(n,1);
         Cu(n1) = epsilon.*ut;
     end
    fx = Xt + deltat.*(J'*Xt + Cu);
else
    fx = Xt + deltat.*f;
end
dfdp = deltat.*dfdp;
dfdx = eye(n) + deltat.*J;


function [img, v2smap]=surf2vol(node,face,xi,yi,zi,varargin)
%
% [img, v2smap]=surf2vol(node,face,xi,yi,zi,'options',values,...)
%
% convert a triangular surface to a shell of voxels in a 3D image
%
% author: Qianqian Fang (fangq <at> nmr.mgh.harvard.edu)
%
% input:
%	 node: node list of the triangular surface, 3 columns for x/y/z
%	 face: triangle node indices, each row is a triangle
%              if face contains the 4th column, it indicates the label of
%              the face triangles (each face componment must be closed); if
%              face contains 5 columns, it stores a tetrahedral mesh with
%              labels, where the first 4 columns are the element list and
%              the last column is the element label;
%	 xi,yi,zi: x/y/z grid for the resulting volume
%        options: 'fill', if set to 1, the enclosed voxels are labeled by 1
%                 'label', if set to 1, the enclosed voxels are labeled by
%                          the corresponding label of the face or element;
%                          setting 'label' to 1 also implies 'fill'.
%
% output:
%	 img: a volumetric binary image at position of ndgrid(xi,yi,zi)
%        v2smap (optional): a 4x4 matrix denoting the Affine transformation to map
%             the voxel coordinates back to the mesh space.
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

fprintf(1,'converting a closed surface to a volumetric binary image ...\n');

opt=varargin2struct(varargin{:});
label=jsonopt('label',0,opt);

elabel=[1];
if(size(face,2)>=4)
        elabel=unique(face(:,end));
        if(size(face,2)==5)
            label=1;
            el=face;
            face=[];
            for i=1:length(elabel)
                    fc=volface(el(el(:,5)==elabel(i),1:4));
                    fc(:,4)=elabel(i);
                    face=[face ; fc];
            end
        end
else
        fc=face;
end

img=zeros(length(xi),length(yi),length(zi));

for i=1:length(elabel)
        if(size(face,2)==4)
            fc=face(face(:,4)==elabel(i),1:3);
        end
        im=surf2volz(node(:,1:3),fc(:,1:3),xi,yi,zi);
        im=im | shiftdim(surf2volz(node(:,[3 1 2]),fc(:,1:3),zi,xi,yi),1);
        im=im | shiftdim(surf2volz(node(:,[2 3 1]),fc(:,1:3),yi,zi,xi),2);

        v2smap=[];

        % here we assume the grid is uniform; surf2vol can handle non-uniform grid,
        % but the affine output is not correct in this case

        if(jsonopt('fill',0,opt) || label)
                im=imfill(im,'holes');
                if(label)
                    im=im*elabel(i);
                end
        end
        img=max(im,img);
end

if(nargout>1)
        dlen=abs([xi(2)-xi(1) yi(2)-yi(1) zi(2)-zi(1)]);
        p0=min(node);
        offset=p0;
        v2smap=diag(abs(dlen));
        v2smap(4,4)=1;
        v2smap(1:3,4)=offset';
end


function expmDimAdjust = getExpmDimAdjust(disc)
%GETEXPMDIMADJUST    Adjust dimension of discretization for LINOP/EXPM.

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

expmDimAdjust = max(getDiffOrder(disc.source), 0);
expmDimAdjust = max(expmDimAdjust, [], 1);

end


function plotOcclusionBoundaries(bndinfo, blabels)
% im = drawOcclusionBoundaries(im, bndinfo, blabels)
% blabels(i) = (0, 1, or 2) for off, left, or right
%

if numel(blabels)==bndinfo.ne*2
    blabels = blabels(1:end/2) + 2*blabels(end/2+1:end);
end

hold on
imsize = bndinfo.imsize;

color = [0.01 0.01 0.01];

indices = bndinfo.edges.indices;

arrowdist = ceil(sqrt(imsize(1).^2 + imsize(2).^2)/10);

for k = 1:numel(indices)

    if blabels(k)>0

        ind = double(indices{k});
        [ey, ex] = ind2sub(bndinfo.imsize(1:2), ind);
        npix = numel(ind);

        narrows = ceil(npix/arrowdist);

        epos = ceil((1:narrows) / (narrows+1) * npix);

        for j = 1:numel(epos)

            [ay, ax] = ind2sub(imsize(1:2), ind(epos(j)));

            if blabels(k)==1
                [y1, x1] = ind2sub(imsize(1:2), ind(max(epos(j)-10,1)));
                [y2, x2] = ind2sub(imsize(1:2), ind(min(epos(j),npix)));
            else % blabels(k)==2;
                [y1, x1] = ind2sub(imsize(1:2), ind(min(epos(j)+10,npix)));
                [y2, x2] = ind2sub(imsize(1:2), ind(min(epos(j),npix)));
            end
            theta = atan2(y2-y1, x2-x1);
%             if blabels(k)==2
%                 theta = mod(theta+pi, 2*pi);
%             end

            asx = ax - cos(theta);
            asy = ay - sin(theta);

            %[ax,ay] = dsxy2figxy(gca, ax, ay);
            %[asx, asy] = dsxy2figxy(gca, asx, asy);
            ax = (ax-1)/(imsize(2)-1);   ay = 1-(ay-1)/(imsize(1)-1);
            asx = (asx-1)/(imsize(2)-1);   asy = 1-(asy-1)/(imsize(1)-1);

            plot(ex, ey, 'Color', 1-color, 'LineWidth', 3);
            plot(ex, ey, 'Color', color, 'LineWidth', 1);

            annotation('arrow', [asx ax], [asy ay], 'LineStyle', 'none', ...
                'HeadWidth', 17, 'HeadLength', 10);
        end
    end
end

% arrhsv = ones(5, 3);
% arrhsv(:, 1) = [100 0 135 169 42]/255;
%
% colors = max(hsv2rgb(arrhsv), 1/255); % avoid pixel intensity of 0
%
% [imh, imw] = size(bndinfo.wseg);
% edgeim = zeros([imh imw 3]);
%
% hasarrow = false(bndinfo.ne*2, 1);
% arrowpos = zeros(1000, 2);
% narrow = 0;
%
% for k = 1:numel(blabels)
%     if blabels(k) > 0
%
%         ku = mod(k-1, bndinfo.ne)+1;
%
%         cols = colors(blabels(k), :);
%         pix = bndinfo.edges.indices{ku};
%         npix = size(edgeim, 1)*size(edgeim, 2);
%         for b = 1:3
%             edgeim(pix + (b-1)*npix) = cols(b);
%         end
%
%         jcts = bndinfo.edges.junctions(ku, :);
%         if k~=ku
%             jcts = jcts([2 1]);
%         end
%
%         x = bndinfo.junctions.position(jcts, 1);
%         y = bndinfo.junctions.position(jcts, 2);
%
%         if ~any(sum((arrowpos(1:narrow, :) - repmat([x(2) y(2)], narrow, 1)).^2, 2) < 20^2) ...
%                 && (numel(pix)>10)
%
%             narrow = narrow + 1;
%             arrowpos(narrow, :) = [x(2) y(2)];
%
%             arrow = struct('x', x(2), 'y', y(2), ...
%                 'angle', atan2(-(y(2)-y(1)), x(2)-x(1)), ...
%                 'radius', 0, 'head_length', round(sqrt(npix)/50), ...
%                 'head_base_angle', 30);
%             arrow.angle = mod(arrow.angle/pi*180, 360);
%             edgeim = draw_arrow_image(edgeim, arrow, cols);
%         end
%
%     end
% end
%
% sz = ceil(sqrt(size(im, 1).^2 + size(im,2).^2) / 500);
%
% for b = 1:3
%     edgeim(:, :, b) = ordfilt2(edgeim(:, :, b), sz*sz, ones(sz, sz));
% end
%
% if strcmp(class(im), 'uint8')
%     edgeim = im2uint8(edgeim);
% end
%
% im(edgeim>0) = edgeim(edgeim>0);
%
% % hold off, clf, imagesc(im), axis image, hold on
% % drawnow;
%
% % dy = -1;
% % dx = -1;
% %
% % for k = 1:numel(blabels)
% %     if blabels(k) > 0
% %         ku = mod(k-1, bndinfo.ne)+1;
% %         jcts = bndinfo.edges.junctions(ku, :);
% %         if k~=ku
% %             jcts = jcts([2 1]);
% %         end
% %
% %         x = min(max((bndinfo.junctions.position(jcts, 1)+dx),1), imw);
% %         y = min(max((bndinfo.junctions.position(jcts, 2)+dy), 1), imh);
% %         [arrowx,arrowy] = dsxy2figxy(gca, x, y);
% %
% %         if mod(k, 5)==0
% %             annotation('arrow', arrowx, 1-arrowy, ...
% %                 'Color', colors(blabels(k), :), 'HeadStyle', 'vback2', 'LineStyle', 'none');
% %         end
% %
% %     end
% % end
%
%


function [ eigenvalues, kpoints, nelect ] = import_eigenval( filename )
%IMPORT_EIGENVAL Import a VASP EIGENVAL file.
%   [eigenvalues,kpoints,nelect] = IMPORT_EIGENVAL(filename) imports a VASP
%   EIGENVAL file. Optional parameter filename specifies the name of the
%   file. eigenvalues is a NKPOINTS x NBANDS x ISPIN array of eigenvalues,
%   kpoints is a NKPOINTS x 4 array of k-point coordinates and weights, and
%   nelect is the number of electrons.

% todo:
% check compatibility with non-spin-polarized files

  if nargin == 0
      filename='EIGENVAL';
  end

  fid = fopen(filename);
  if fid==-1
    error(['File ' filename ' not found']);
  end

  buffer = fscanf(fid, '%d', 4); % various data
  ispin = buffer(4); % 1 = non-polarized, 2 = polarized
  fgetl(fid); % empty string
  fgetl(fid); % various data
  fgetl(fid); % various data
  fgetl(fid); % various data
  fgetl(fid); % comment
  nelect = fscanf(fid, '%d', 1); % number of electrons
  nkpoints = fscanf(fid, '%d', 1); % number of k-points
  nbands = fscanf(fid, '%d', 1); % number of bands

  fgetl(fid); % empty string

  kpoints = zeros(nkpoints,4);
  eigenvalues = zeros(nkpoints, nbands, ispin);

  for kpoint = 1:nkpoints
    fgetl(fid); % blank line
    kpoints(kpoint,:) = fscanf(fid, '%f', 4)';
    fgetl(fid); % empty string
    for band = 1:nbands
        buffer = fscanf(fid,'%f',ispin+1);
        eigenvalues(kpoint,band,1:ispin) = buffer(2:(ispin+1));
        fgetl(fid); % empty string
    end
  end

  eigenvalues = sort(eigenvalues,2); % sort the bands by energy

end


function  noisyOut = findNoisyChannels(signal, noisyIn)
% Identify bad channels in EEG using a two-stage approach
%
% reference = findNoisyChannels(signal)
% reference = findNoisyChannels(signal, reference)
%
% First remove bad channels by amplitude, noise level, and correlation
% Apply ransac after these channels have been removed.
%
% Input parameters:
%     signal - structure with srate, chanlocs, chaninfo, and data fields
%     noisyIn - structure with input parameters
%
%  Notes: the signal is assumed to be high-passed. Removing line noise
%  is a good idea too.
%
%  noisyIn: (fields are filled in on input if not present and propagated to output)
%     name - name of the input file
%     srate - sample rate in HZ
%     samples - number of samples in the data
%     evaluationChannels - a vector of channels to use
%     channelLocations - a structure of EEG channel locations
%     chaninfo - standard EEGLAB chaninfo (nose direction is relevant)
%     chanlocs - standard EEGLAB chanlocs structure
%     robustDeviationThreshold - z score cutoff for robust channel deviation
%     highFrequencyNoiseThreshold -  z score cutoff for SNR (signal above 50 Hz)
%     correlationWindowSeconds - correlation window size in seconds (default = 1 sec)
%     correlationThreshold - correlation below which window is bad (default = 0.4)
%     badTimeThreshold - cutoff fraction of bad corr windows (default = 0.01)
%     ransacSampleSize - samples for computing ransac (default = 50)
%     ransacChannelFraction - fraction of channels for robust reconstruction (default = 0.25)
%     ransacCorrelationThreshold - cutoff correlation for abnormal wrt neighbors(default = 0.75)
%     ransacUnbrokenTime - cutoff fraction of time channel can have poor ransac predictability (default = 0.4)
%     ransacWindowSeconds - correlation window for ransac (default = 5 sec)
%
% Output parameters (c channels, w windows):
%    ransacPerformed - true if there were enough good channels to do ransac
%    noisyChannels - list of identified bad channel numbers
%    badChannelsFromCorrelation  - list of bad channels identified by correlation
%    badChannelsFromDeviation   - list of bad channels identified by amplitude
%    badChannelsFromHFNoise - list of bad channels identified by SNR
%    badChannelsFromRansac - list of channels identified by ransac
%    fractionBadCorrelationWindows - c x 1 vector with fraction of bad correlation windows
%    robustChannelDeviation - c x 1 vector with robust measure of average channel deviation
%    zscoreHFNoise - c x 1 vector with measure of channel noise level
%    maximumCorrelations - w x c array with max window correlation
%    ransacCorrelations = c x wr array with ransac correlations
%
% This function uses 4 methods for detecting bad channels after removing
% from consideration channels that have NaN data or channels that are
% identically constant.
%
% Method 1: too low or high amplitude. If the z score of robust
%           channel deviation falls below robustDeviationThreshold, the channel is
%           considered to be bad.
% Method 2: too low an SNR. If the z score of estimate of signal above
%           50 Hz to that below 50 Hz above highFrequencyNoiseThreshold, the channel
%           is considered to be bad.
%
% Method 3: low correlation with other channels. Here correlationWindowSize is the window
%           size over which the correlation is computed. If the maximum
%           correlation of the channel to the other channels falls below
%           correlationThreshold, the channel is considered bad in that window.
%           If the fraction of bad correlation windows for a channel
%           exceeds badTimeThreshold, the channel is marked as bad.
%
% After the channels from methods 2 and 3 are removed, method 4 is
% computed on the remaining signals
%
% Method 4: each channel is predicted using ransac interpolation based
%           on a ransac fraction of the channels. If the correlation of
%           the prediction to the actual behavior is too low for too
%           long, the channel is marked as bad.
%
% Assumptions:
%  - The signal is a structure of continuous data with data, srate, chanlocs,
%    and chaninfo fields.
%  - The signal.data has been high pass filtered.
%  - No segments of the EEG data have been removed

% Methods 1 and 4 are adapted from code by Christian Kothe and Methods 2
% and 3 are adapted from code by Nima Bigdely-Shamlo
%
%% Check the incoming parameters
if nargin < 1
    error('findNoisyChannels:NotEnoughArguments', 'requires at least 1 argument');
elseif isstruct(signal) && ~isfield(signal, 'data')
    error('findNoisyChannels:NoDataField', 'requires a structure data field');
elseif size(signal.data, 3) ~= 1
    error('findNoisyChannels:DataNotContinuous', 'data must be a 2D array');
elseif nargin < 2 || ~exist('noisyIn', 'var') || isempty(noisyIn)
    noisyIn = struct();
end

%% Set the defaults and initialize as needed
noisyOut = getNoisyStructure();
defaults = getPrepDefaults(signal, 'reference');
[noisyOut, errors] = checkPrepDefaults(noisyIn, noisyOut, defaults);
if ~isempty(errors)
    error('findNoisyChannels:BadParameters', ['|' sprintf('%s|', errors{:})]);
end
%% Fix the channel locations
channelLocations = noisyOut.channelLocations;
evaluationChannels = sort(noisyOut.evaluationChannels); % Make sure channels are sorted
evaluationChannels = evaluationChannels(:)';            % Make sure row vector
noisyOut.evaluationChannels = evaluationChannels;
originalChannels = 1:size(signal.data, 1);

%% Extract the data required
data = signal.data;
originalNumberChannels = size(data, 1);          % Save the original channels
data = double(data(evaluationChannels, :))';      % Remove the unneeded channels
signalSize = size(data, 1);
correlationFrames = noisyOut.correlationWindowSeconds * signal.srate;
correlationWindow = 0:(correlationFrames - 1);
correlationOffsets = 1:correlationFrames:(signalSize-correlationFrames);
WCorrelation = length(correlationOffsets);
ransacFrames = noisyOut.ransacWindowSeconds*noisyOut.srate;
ransacWindow = 0:(ransacFrames - 1);
ransacOffsets = 1:ransacFrames:(signalSize-ransacFrames);
WRansac = length(ransacOffsets);
noisyOut.zscoreHFNoise = zeros(originalNumberChannels, 1);
noisyOut.noiseLevels = zeros(originalNumberChannels, WCorrelation);
noisyOut.maximumCorrelations = ones(originalNumberChannels, WCorrelation);
noisyOut.dropOuts = zeros(originalNumberChannels, WCorrelation);
noisyOut.correlationOffsets = correlationOffsets;
noisyOut.channelDeviations = zeros(originalNumberChannels, WCorrelation);
noisyOut.robustChannelDeviation = zeros(originalNumberChannels, 1);
noisyOut.ransacCorrelations = ones(originalNumberChannels, WRansac);
noisyOut.ransacOffsets = ransacOffsets;

%% Detect constant or NaN channels and remove from consideration
nanChannelMask = sum(isnan(data), 1) > 0;
noSignalChannelMask = mad(data, 1, 1) < 10e-10 | std(data, 1, 1) < 10e-10;
noisyOut.noisyChannels.badChannelsFromNaNs = evaluationChannels(nanChannelMask);
noisyOut.noisyChannels.badChannelsFromNoData = evaluationChannels(noSignalChannelMask);
evaluationChannels = setdiff(evaluationChannels, ...
    union(noisyOut.noisyChannels.badChannelsFromNaNs, ...
          noisyOut.noisyChannels.badChannelsFromNoData));
data = signal.data;
data = double(data(evaluationChannels, :))';
[signalSize, numberChannels] = size(data);

%% Method 1: Unusually high or low amplitude (using robust std)
channelDeviation = 0.7413 *iqr(data); % Robust estimate of SD
channelDeviationSD =  0.7413 * iqr(channelDeviation);
channelDeviationMedian = nanmedian(channelDeviation);
noisyOut.robustChannelDeviation(evaluationChannels) = ...
    (channelDeviation - channelDeviationMedian) / channelDeviationSD;

% Find channels with unusually high deviation
badChannelsFromDeviation = ...
    abs(noisyOut.robustChannelDeviation) > ...
             noisyOut.robustDeviationThreshold | ...
             isnan(noisyOut.robustChannelDeviation);
badChannelsFromDeviation = originalChannels(badChannelsFromDeviation);
noisyOut.noisyChannels.badChannelsFromDeviation = badChannelsFromDeviation(:)';
noisyOut.channelDeviationMedian = channelDeviationMedian;
noisyOut.channelDeviationSD = channelDeviationSD;

%% Method 2: Compute the SNR (based on Christian Kothe's clean_channels)
% Note: RANSAC uses the filtered values X of the data
if noisyOut.srate > 100
    % Remove signal content above 50Hz and below 1 Hz
    B = design_fir(100,[2*[0 45 50]/noisyOut.srate 1],[1 1 0 0]);
    X = zeros(signalSize, numberChannels);
    parfor k = 1:numberChannels  % Could be changed to parfor
        X(:,k) = filtfilt_fast(B, 1, data(:, k)); end
    % Determine z-scored level of EM noise-to-signal ratio for each channel
    noisiness = mad(data- X, 1)./mad(X, 1);
    noisinessMedian = nanmedian(noisiness);
    noisinessSD = mad(noisiness, 1)*1.4826;
    zscoreHFNoiseTemp = (noisiness - noisinessMedian) ./ noisinessSD;
    noiseMask = (zscoreHFNoiseTemp > noisyOut.highFrequencyNoiseThreshold) | ...
                isnan(zscoreHFNoiseTemp);
    % Remap channels to original numbering
    badChannelsFromHFNoise  = evaluationChannels(noiseMask);
    noisyOut.noisyChannels.badChannelsFromHFNoise = badChannelsFromHFNoise(:)';
else
    X = data;
    noisinessMedian = 0;
    noisinessSD = 1;
    zscoreHFNoiseTemp = zeros(numberChannels, 1);
    noisyOut.noisyChannels.badChannelsFromHFNoise = [];
end

% Remap the channels to original numbering for the zscoreHFNoise
noisyOut.zscoreHFNoise(evaluationChannels) = zscoreHFNoiseTemp;
noisyOut.noisinessMedian = noisinessMedian;
noisyOut.noisinessSD = noisinessSD;

%% Method 3: Global correlation criteria (from Nima Bigdely-Shamlo)
channelCorrelations = ones(WCorrelation, numberChannels);
noiseLevels = zeros(WCorrelation, numberChannels);
channelDeviations = zeros(WCorrelation, numberChannels);
n = length(correlationWindow);
xWin = reshape(X(1:n*WCorrelation, :)', numberChannels, n, WCorrelation);
dataWin = reshape(data(1:n*WCorrelation, :)', numberChannels, n, WCorrelation);
parfor k = 1:WCorrelation
    eegPortion = squeeze(xWin(:, :, k))';
    dataPortion = squeeze(dataWin(:, :, k))';
    windowCorrelation = corrcoef(eegPortion);
    abs_corr = abs(windowCorrelation - diag(diag(windowCorrelation)));
    channelCorrelations(k, :)  = quantile(abs_corr, 0.98);
    noiseLevels(k, :) = mad(dataPortion - eegPortion, 1)./mad(eegPortion, 1);
    channelDeviations(k, :) =  0.7413 *iqr(dataPortion);
end
dropOuts = isnan(channelCorrelations) | isnan(noiseLevels);
channelCorrelations(dropOuts) = 0.0;
noiseLevels(dropOuts) = 0.0;
clear xWin;
clear dataWin;
noisyOut.maximumCorrelations(evaluationChannels, :) = channelCorrelations';
noisyOut.noiseLevels(evaluationChannels, :) = noiseLevels';
noisyOut.channelDeviations(evaluationChannels, :) = channelDeviations';
noisyOut.dropOuts(evaluationChannels, :) = dropOuts';
thresholdedCorrelations = ...
    noisyOut.maximumCorrelations < noisyOut.correlationThreshold;
fractionBadCorrelationWindows = mean(thresholdedCorrelations, 2);
fractionBadDropOutWindows = mean(noisyOut.dropOuts, 2);

% Remap channels to their original numbers
badChannelsFromCorrelation = find(fractionBadCorrelationWindows > noisyOut.badTimeThreshold);
noisyOut.noisyChannels.badChannelsFromCorrelation = badChannelsFromCorrelation(:)';
badChannelsFromDropOuts = find(fractionBadDropOutWindows > noisyOut.badTimeThreshold);
noisyOut.noisyChannels.badChannelsFromDropOuts = badChannelsFromDropOuts(:)';
noisyOut.medianMaxCorrelation =  median(noisyOut.maximumCorrelations, 2);

%% Bad so far by amplitude and correlation (take these out before doing ransac)
noisyChannels = union(noisyOut.noisyChannels.badChannelsFromDeviation, ...
    union(noisyOut.noisyChannels.badChannelsFromCorrelation, ...
          noisyOut.noisyChannels.badChannelsFromDropOuts));

%% Method 4: Ransac corelation (may not be performed)
% Setup for ransac (if a 2-stage algorithm, remove other bad channels first)
if noisyOut.ransacOff
    noisyOut.ransacBadWindowFraction = 0;
    noisyOut.ransacPerformed = false;
elseif isempty(channelLocations)
    warning('findNoisyChannels:noChannelLocation', ...
        'ransac could not be computed because there were no channel locations');
    noisyOut.ransacBadWindowFraction = 0;
    noisyOut.ransacPerformed = false;
else % Set up parameters and make sure enough good channels to proceed
    [ransacChannels, idiff] = setdiff(evaluationChannels, noisyChannels);
    X = X(:, idiff);

    % Calculate the parameters for ransac
    ransacSubset = round(noisyOut.ransacChannelFraction*size(data, 2));
    if noisyOut.ransacUnbrokenTime < 0
        error('find_noisyChannels:BadUnbrokenParameter', ...
            'ransacUnbrokenTime must be greater than 0');
    elseif noisyOut.ransacUnbrokenTime < 1
        ransacUnbrokenFrames = signalSize*noisyOut.ransacUnbrokenTime;
    else
        ransacUnbrokenFrames = srate*noisyOut.ransacUnbrokenTime;
    end

    nchanlocs = channelLocations(ransacChannels);
    if length(nchanlocs) ~= size(nchanlocs, 2)
        nchanlocs = nchanlocs';
    end
    if length(nchanlocs) < ransacSubset + 1 || length(nchanlocs) < 3 || ...
            ransacSubset < 2
        warning('find_noisyChannels:NotEnoughGoodChannels', ...
            'Too many channels have failed quality tests to perform ransac');
        noisyOut.ransacBadWindowFraction = 0;
        noisyOut.ransacPerformed = false;
    end
end

if noisyOut.ransacPerformed
    try
    % Calculate all-channel reconstruction matrices from random channel subsets
       locs = [cell2mat({nchanlocs.X}); cell2mat({nchanlocs.Y});cell2mat({nchanlocs.Z})];
    catch err
       error('findNoisyChannels:NoXYZChannelLocations', ...
             'Must provide valid channel locations');
    end
    if isempty(locs) || size(locs, 2) ~= length(ransacChannels) ...
            || any(isnan(locs(:)))
          error('find_noisyChannels:EmptyChannelLocations', ...
            'The signal chanlocs must have valid X, Y, and Z components');
    end
    P = hlp_microcache('cleanchans', @calc_projector, locs, ...
        noisyOut.ransacSampleSize, ransacSubset);
    ransacCorrelationsT = zeros(length(locs), WRansac);

    % Calculate each channel's correlation to its RANSAC reconstruction for each window
    n = length(ransacWindow);
    m = length(ransacChannels);
    p = noisyOut.ransacSampleSize;
    Xwin = reshape(X(1:n*WRansac, :)', m, n, WRansac);
    parfor k = 1:WRansac
        ransacCorrelationsT(:, k) = ...
            calculateRansacWindow(squeeze(Xwin(:, :, k))', P, n, m, p);
    end
    clear Xwin;
    noisyOut.ransacCorrelations(ransacChannels, :) = ransacCorrelationsT;
    flagged = noisyOut.ransacCorrelations < noisyOut.ransacCorrelationThreshold;
    badChannelsFromRansac = find(sum(flagged, 2)*ransacFrames > ransacUnbrokenFrames)';
    noisyOut.noisyChannels.badChannelsFromRansac = badChannelsFromRansac(:)';
    noisyOut.ransacBadWindowFraction = sum(flagged, 2)/size(flagged, 2);
end

% Combine bad channels detected from all methods
noisy = noisyOut.noisyChannels;
noisyOut.noisyChannels.badChannelsFromLowSNR = ...
    intersect(noisy.badChannelsFromHFNoise, noisy.badChannelsFromCorrelation);
noisyChannels = union(noisyChannels, ...
    union(union(noisy.badChannelsFromRansac, ...
          noisy.badChannelsFromHFNoise), ...
    union(noisy.badChannelsFromNaNs, ...
          noisy.badChannelsFromNoData)));
noisyOut.noisyChannels.all = noisyChannels(:)';
noisyOut.medianMaxCorrelation =  median(noisyOut.maximumCorrelations, 2);

%% Helper functions for findNoisyChannels
function P = calc_projector(locs, numberSamples, subsetSize)
% Calculate a bag of reconstruction matrices from random channel subsets

[permutedLocations, subsets] = getRandomSubsets(locs, subsetSize, numberSamples);
randomSamples = cell(1, numberSamples);
parfor k = 1:numberSamples
    tmp = zeros(size(locs, 2));
    slice = subsets(k, :);
    tmp(slice, :) = real(spherical_interpolate(permutedLocations(:, :, k), locs))';
    randomSamples{k} = tmp;
end
P = horzcat(randomSamples{:});

function [permutedLocations, subsets] = getRandomSubsets(locs, subsetSize, numberSamples)
 stream = RandStream('mt19937ar', 'Seed', 435656);
 numberChannels = size(locs, 2);
 permutedLocations = zeros(3, subsetSize, numberSamples);
 subsets = zeros(numberSamples, subsetSize);
 for k = 1:numberSamples
     subset = randsample(1:numberChannels, subsetSize, stream);
     subsets(k, :) = subset;
     permutedLocations(:, :,  k) = locs(:, subset);
 end

function Y = randsample(X, num, stream)
Y = zeros(1, num);
for k = 1:num
    pick = round(1 + (length(X)-1).*rand(stream));
    Y(k) = X(pick);
    X(pick) = [];
end

function rX = calculateRansacWindow(XX, P, n, m, p)
    YY = sort(reshape(XX*P, n, m, p),3);
    YY = YY(:, :, round(end/2));
    rX = sum(XX.*YY)./(sqrt(sum(XX.^2)).*sqrt(sum(YY.^2)));

function noisyOut = getNoisyStructure()
    noisyOut = struct('srate', [], ...
        'samples', [], ...
        'evaluationChannels', [], ...
        'channelLocations', [], ...
        'robustDeviationThreshold', [], ...
        'highFrequencyNoiseThreshold', [], ...
        'correlationWindowSeconds', [], ...
        'correlationThreshold', [], ...
        'badTimeThreshold', [], ...
        'ransacSampleSize', [], ...
        'ransacChannelFraction', [], ...
        'ransacCorrelationThreshold', [], ...
        'ransacUnbrokenTime', [], ...
        'ransacWindowSeconds', [], ...
        'noisyChannels', getBadChannelStructure(), ...
        'ransacPerformed', true, ...
        'channelDeviationMedian', [], ...
        'channelDeviationSD', [], ...
        'channelDeviations', [], ...
        'robustChannelDeviation', [], ...
        'noisinessMedian', [], ...
        'noisinessSD', [], ...
        'zscoreHFNoise', [], ...
        'noiseLevels', [], ...
        'maximumCorrelations', [], ...
        'dropOuts', [], ...
        'medianMaxCorrelation', [], ...
        'correlationOffsets', [], ...
        'ransacCorrelations', [], ...
        'ransacOffsets', [], ...
        'ransacBadWindowFraction', []);


function [DMIN,DMIN_aerr] = dicomrt_MINcal(inputdose,inputerror,dose_xmesh,dose_ymesh,dose_zmesh,VOI,voi2use)
% dicomrt_MINcal(inputdose,inputerror,dose_xmesh,dose_ymesh,dose_zmesh,VOI,voi2use)
%
% Calculate the MIN Dose for a given 3D dose distribution within a given VOI
%
% inputdose is the input 3D dose (e.g. RTPLAN or MC generated)
% inputerror is the relative error asociated with the calculated dose. If inputerror=0 error
% calculation is not performed (e.g. for TPS dose matrices), otherwise
% inputerror dimensions must match those of inputdose (e.g. for MC dose matrices).
% dose_xmesh,dose_ymesh,dose_zmesh are x-y-z coordinates of the center of the dose-pixel
%
% VOI is a cell array which contain the patients VOIs as read by dicomrt_loadvoi
% voi2use is a vector pointing to the number of VOIs to be used ot the analysis and for the display.
%
% Example:
%
% [A]=dicomrt_MINcal(B,0,dose_xmesh,dose_ymesh,dose_zmesh,demo_voi,9);
%
% returns in A the min dose for the dose matrix B and the voi # 9 within demo_voi.
%
% NOTE:
% due to the algorithm that MATLAB uses to filter matrices (roipoly), it can happen that
% some of the voxels which are partially outside the patient are included in the VOI.
% This can happen when the border of the VOI are too close to the patient outline with
% respect to the size of the calculation matrix used.
% This is not a problem for MC which calculate dose also outside the patient outline.
% It may represent a problem when dealing with TPS generated data, which do not calculate
% dose outside the patient outline, because voxels with zero dose can be counted in the target
% volume. This event is rare (1/10000) and should not influence dose analysis.
% A simple way to overcome this problem is to define VOIs as separate as possible.
% In this function the minimum dose ~=0 is reported.
%
% See also dicomrt_MDNcal, dicomrt_MDcal, dicomrt_MAXcal
%
% Copyright (C) 2002 Emiliano Spezi (emiliano.spezi@physics.org)

% Check case and set-up some parameters and variables
[dose_temp,derror_temp,type_dose,doselabel]=dicomrt_checkinputanderr(inputdose,inputerror,1);
dose=dicomrt_varfilter(dose_temp);
derror=dicomrt_varfilter(derror_temp);
[VOI_temp,type,label]=dicomrt_checkinput(VOI);
VOI=dicomrt_varfilter(VOI_temp);

% Initialise parameters
count=0;

[mask_dose,volume_VOI,mask4VOI]=dicomrt_mask(VOI_temp,dose_temp,dose_xmesh,dose_ymesh,dose_zmesh,voi2use,'nan','n');
mask=dicomrt_varfilter(mask_dose);

if size(derror)~=1
    [mask_error,volume_VOI,mask4VOI]=dicomrt_mask(VOI_temp,derror_temp,dose_xmesh,dose_ymesh,dose_zmesh,voi2use,'nan','n');
    msk_error=dicomrt_varfilter(mask_error);
end

for k=1:size(mask,3)
    [ilocation,jlocation]=find(isnan(mask(:,:,k))~=1);
    if isnumeric(ilocation)==1 | isnumeric(jlocation)==1
        for i=1:length(ilocation)
            count=count+1;
            dose_VOI(count)=mask(ilocation(i),jlocation(i),k);
            if size(derror)~=1
                % this is absolute error now
                error_VOI(count)=msk_error(ilocation(i),jlocation(i),k).*mask(ilocation(i),jlocation(i),k);
            else
                error_VOI=0;
            end
        end
    end
end

if size(derror)~=1
    % Mean is calculated as weighted average of data points
    [DMIN,I]=min(dose_VOI);
    % Dose min cannot be 0.
    if DMIN==0
        dose_VOI(I)=[];
        [DMIN,I]=min(dose_VOI);
    end
    % error is calulated as standard error of the mean
    DMIN_aerr=error_VOI(I);
else
    % Mean is calculated as average of data points
    [DMIN,I]=min(dose_VOI);
    % Dose min cannot be 0.
    if DMIN==0
        dose_VOI(I)=[];
        [DMIN,I]=min(dose_VOI);
    end
    % error is calulated as standard error of the mean
    DMIN_aerr=0;
end


function truefalse = ch_is_format_code ( c )

%*****************************************************************************80
%
%% CH_IS_FORMAT_CODE returns TRUE if a character is a FORTRAN format code.
%
%  Discussion:
%
%    The format codes accepted here are not the only legal format
%    codes in FORTRAN90.  However, they are more than sufficient
%    for my needs!
%
%  Table:
%
%    A  Character
%    B  Binary digits
%    D  Real number, exponential representation
%    E  Real number, exponential representation
%    F  Real number, fixed point
%    G  General format
%    I  Integer
%    L  Logical variable
%    O  Octal digits
%    Z  Hexadecimal digits
%    *  Free format
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 November 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, character C, the character to be analyzed.
%
%    Output, logical TRUEFALSE, is TRUE if C is a FORTRAN format code
%     and FALSE otherwise.
%
      if ( ch_eqi ( c, 'A' ) )
    truefalse = 1;
  elseif ( ch_eqi ( c, 'B' ) )
    truefalse = 1;
  elseif ( ch_eqi ( c, 'D' ) )
    truefalse = 1;
  elseif ( ch_eqi ( c, 'E' ) )
    truefalse = 1;
  elseif ( ch_eqi ( c, 'F' ) )
    truefalse = 1;
  elseif ( ch_eqi ( c, 'G' ) )
    truefalse = 1;
  elseif ( ch_eqi ( c, 'I' ) )
    truefalse = 1;
  elseif ( ch_eqi ( c, 'L' ) )
    truefalse = 1;
  elseif ( ch_eqi ( c, 'O' ) )
    truefalse = 1;
  elseif ( ch_eqi ( c, 'Z' ) )
    truefalse = 1;
  elseif ( c == '*' )
    truefalse = 1;
  else
    truefalse = 0;
  end

  return
end


function [X,S] = calculator2(D)
%UNTITLED Summary of this function goes here
%   Detailed explanation goes here

global K_S S_0 mu_max Y_XS

X = Y_XS ./ 3 .* (S_0 - D .* K_S ./ (mu_max - D));

S = D .* K_S ./ (mu_max - D);

end


function M_hat = run_mc(params)
  M = params.M;
  A = inexact_alm_mc(M,1e-4,100);
  M_hat = A.U*A.V';
end


function [mpe, ll] = calc_mpe_bucket(bnet, new_evidence, max_over)
%
% PURPOSE:
%       CALC_MPE Computes the most probable explanation to the network nodes
%       given the evidence.
%
%       [mpe, ll] = calc_mpe(engine, new_evidence, max_over)
%
% INPUT:
%       bnet  - the bayesian network
%       new_evidence - optional, if specified - evidence to be incorporated [cell(1,n)]
%       max_over - optional, if specified determines the variable elimination order [1:n]
%
% OUTPUT:
%       mpe - the MPE assignmet for the net variables (or [] if no satisfying assignment)
%       ll - log assignment probability.
%
% Notes:
% 1. Adapted from '@var_elim_inf_engine\marginal_nodes' for MPE by Ron Zohar, 8/7/01
% 2. Only discrete potentials are supported at this time.
% 3. Complexity: O(nw*) where n is the number of nodes and w* is the induced tree width.
% 4. Implementation based on:
%  - R. Dechter, "Bucket Elimination: A Unifying Framework for Probabilistic Inference",
%                 UA1 96, pp. 211-219.

ns = bnet.node_sizes;
n = length(bnet.dag);
evidence = cell(1,n);
if (nargin<2)
    new_evidence = evidence;
end

onodes = find(~isemptycell(new_evidence));  % observed nodes
hnodes = find(isemptycell(new_evidence));  % hidden nodes
pot_type = determine_pot_type(bnet, onodes);

if pot_type ~= 'd'
  error('only disrete potentials supported at this time')
end

for i=1:n
  fam = family(bnet.dag, i);
  CPT{i} = convert_to_pot(bnet.CPD{bnet.equiv_class(i)}, pot_type, fam(:), evidence);
end

% handle observed nodes: set impossible cases' probability to zero
% rather than prun matrix (this makes backtracking easier)

for ii=onodes
  lIdx = 1:ns(ii);
  lIdx = setdiff(lIdx, new_evidence{ii});

  sCPT=struct(CPT{ii});  % violate object privacy

  sargs = '';
  for jj=1:(length(sCPT.domain)-1)
    sargs = [sargs, ':,'];
  end
  for jj=lIdx
    eval(['sCPT.T(', sargs, num2str(jj), ')=0;']);
  end
  CPT{ii}=dpot(sCPT.domain, sCPT.sizes, sCPT.T);
end

B = cell(1,n);
for b=1:n
  B{b} = mk_initial_pot(pot_type, [], [], [], []);
end

if (nargin<3)
  max_over = (1:n);
end
order = max_over; % no attempt to optimize this

% Initialize the buckets with the CPDs assigned to them
for i=1:n
  b = bucket_num(domain_pot(CPT{i}), order);
  B{b} = multiply_pots(B{b}, CPT{i});
end

% Do backward phase
max_over = max_over(length(max_over):-1:1); % reverse
for i=max_over(1:end-1)
  % max-ing over variable i which occurs in bucket j
  j = bucket_num(i, order);
  rest = mysetdiff(domain_pot(B{j}), i);
  %temp = marginalize_pot_max(B{j}, rest);
  temp = marginalize_pot(B{j}, rest, 1);
  b = bucket_num(domain_pot(temp), order);
  %        fprintf('maxing over bucket %d (var %d), putting result into bucket %d\n', j, i, b);
  sB=struct(B{b});  % violate object privacy
  if ~isempty(sB.domain)
    B{b} = multiply_pots(B{b}, temp);
  else
    B{b} = temp;
  end
end
result = B{1};
marginal = pot_to_marginal(result);
[prob, mpe] = max(marginal.T);

% handle impossible cases
if ~(prob>0)
  mpe = [];
  ll = -inf;
  %warning('evidence has zero probability')
  return
end

ll = log(prob);

% Do forward phase
for ii=2:n
  marginal = pot_to_marginal(B{ii});
  mpeidx = [];
  for jj=order(1:length(mpe))
    assert(ismember(jj, marginal.domain)) %%% bug
    temp = find_equiv_posns(jj, marginal.domain);
    mpeidx = [mpeidx, temp] ;
    if isempty(temp)
      mpeidx = [mpeidx, Inf] ;
    end
  end
  [mpeidxsorted sortedtompe] = sort(mpeidx) ;

  % maximize the matrix obtained from assigning values from previous buckets.
  % this is done by building a string and using eval.

  kk=1;
  sargs = '(';
  for jj=1:length(marginal.domain)
    if (jj~=1)
      sargs = [sargs, ','];
    end
    if (mpeidxsorted(kk)==jj)
      sargs = [sargs, num2str(mpe(sortedtompe(kk)))];
      if (kk<length(mpe))
	kk = kk+1 ;
      end
    else
      sargs = [sargs, ':'];
    end
  end
  sargs = [sargs, ')'] ;
  eval(['[val, loc] = max(marginal.T', sargs, ');'])
  mpe = [mpe loc];
end
[I,J] = sort(order);
mpe = mpe(J);

%%%%%%%%%

function b = bucket_num(domain, order)

b = max(find_equiv_posns(domain, order));


function polygon_struct = Read_shapefile( finputname, polygon, bbox, ...
                                          h0, boubox, plot_on, shapefile_3d)
% Read_shapefile: Reads a shapefile or a NaN-delimited vector
% containing polygons and/or segments in the the desired region
% of interest. Classifies the vector data as either a
% mainland, island, or outer boundary. The program will automatically
% trim islands that are have an area smaller than 4*h0^2 and will in gaps
% in the vector that are larger than h0/2. This is necessary for the
% use of boundary rejection method in dpoly.

% INPUTS:
% finputname : file name(s) of the shapefile listed in a cell array
% polygon    : a NaN-delimited vector of polygons and/or segments.
% bbox       : the bounding box that we want to extract out
% h0         : minimum edge length in region of interest.
% plot_on    : plot the final polygon or not (=1 or =0)
%
% OUTPUTS:
% polygon_struct    : a structure containing the vector features identified as
%              either islands, mainland, or outer.
% Written by William Pringle and Keith Roberts, CHL,UND, 2017
% Edits by Keith Roberts, July 2018.
%% Loop over all the filenames and get the shapefile within bbox
SG = [];
if bbox(1,2) > 180 && bbox(1,1) < 180
    % bbox straddles 180/-180 meridian
    loop = 2; minus = 0;
elseif all(bbox(1,:) > 180)
    % beyond 180 in 0 to 360 format
    loop = 1; minus = 1;
else
    loop = 1; minus = 0;
end
if (size(finputname,1)~=0)
    for fname = finputname
        for nn = 1:loop
            bboxt = bbox';
            if loop == 2
                if nn == 1
                    bboxt(2,1) = 180;
                else
                    bboxt(1,1) = -180; bboxt(2,1) = bboxt(2,1) - 360;
                end
            end
            if minus
               bboxt(:,1) = bboxt(:,1) - 360;
            end
            % Read the structure
            try
                % The shaperead is faster if it is available
                S = shaperead(fname{1},'BoundingBox',bboxt);
                % Get rid of unwanted components;
                D = struct2cell(S);
                S = cell2struct(D(3:4,:)',{'X','Y'},2);
                disp('Read shapefile with shaperead')
                sr = 1;
            catch
                % If only m_shaperead is available or if some error occured
                % with shaperead (e.g., 3D shapefile, m_shaperead may work)
                disp('Reading shapefile with m_shaperead')
                % This uses m_map (slower but free)
                S = m_shaperead(fname{1},reshape(bboxt',4,1));
                % Let's just keep the x-y data
                D = S.ncst;
                if isfield(S,'dbf')  || isfield(S,'dbfdata')
                    code = S.dbfdata(:,1);
                    S = cell2struct([D code]',{'points' 'type'},1);
                else
                    S = cell2struct(D','points',1);
                end
                sr = 0;
            end
            if ~isempty(S)
                % Keep the following polygons
                SG = [SG; S];
            end
        end
    end
else
    sr = 1 ;
    % convert NaN-delimited vector to struct
    count = 1; j=1;
    for i = 1 : length(polygon)
        % the end of the segment

        if(isnan(polygon(i,1))==1)
            % put NaN at end
            SG(count,1).X(:,j) =NaN;
            SG(count,1).Y(:,j) =NaN;
            % reset
            j = 1 ; count = count + 1;

            continue
        else
            % keep going
            SG(count,1).X(:,j) = polygon(i,1);
            SG(count,1).Y(:,j) = polygon(i,2);
            j=j+1;
        end
    end
end
% If we don't have an outer polygon already then make it by bbox
polygon_struct.outer = boubox;
% Densify the outer polygon (fills gaps larger than half min edgelength).
[latout,lonout] = my_interpm(polygon_struct.outer(:,2),...
                             polygon_struct.outer(:,1),h0/2);
polygon_struct.outer = [];
polygon_struct.outer(:,1) = lonout;
polygon_struct.outer(:,2) = latout;

%% Find whether the polygons are wholey inside the bbox..
%% Set as islands or mainland
disp('Partitioning the boundary into islands, mainland, ocean')
polygon_struct.inner = [];
polygon_struct.mainland = [];
polygon_struct.innerb = [];
polygon_struct.mainlandb = [];
polygon_struct.innerb_type = [];
polygon_struct.mainlandb_type = [];
edges = Get_poly_edges( polygon_struct.outer );

if isempty(SG); return; end

if sr
    tmpM = [[SG.X]',[SG.Y]'] ; % MAT
    if bbox(1,2) > 180
        tmpM(tmpM(:,1) < 0,1) =  tmpM(tmpM(:,1) < 0,1) + 360;
    end
    for i = 1 : length(SG)
       dims(i) = length(SG(i).X) ;
    end
    tmpC = mat2cell(tmpM,dims); % TO CELL
else
    tmpC =  struct2cell(SG)';
    tmpCC = []; nn = 0;
    for ii = 1:size(tmpC,1)
       % may have NaNs inside
       isnan1 = find(isnan(tmpC{ii,1}(:,1)));
       if isempty(isnan1)
           isnan1 = length(tmpC{ii,1})+1;
       elseif isnan1(end) ~= length(tmpC{ii,1})
           isnan1(end+1) = length(tmpC{ii,1})+1;
       end
       is = 1;
       for jj = 1:length(isnan1)
           nn = nn + 1;
           ie = isnan1(jj)-1;
           tmpCC{nn,1} = tmpC{ii,1}(is:ie,:);
           tmpCC{nn,2} = tmpC{ii,2};
           is = isnan1(jj)+1;
       end
    end
    if ~isempty(tmpCC)
        tmpC = tmpCC;
    end
    tmpM =  cell2mat(tmpC(:,1));
    if size(tmpM,2) == 3
       tmpM = tmpM(:,1:2);
    end
end
% Get current polygon
% Check proportion of polygon that is within bbox
tmpIn = inpoly(tmpM,polygon_struct.outer, edges);
tmpInC = mat2cell(tmpIn,cellfun(@length,tmpC(:,1)));

j = 0 ; k = 0 ; height = []; new_islandb = []; new_mainb = [];
new_islandb_type = []; new_mainb_type = [];
for i = 1 : size(tmpC,1)
    if sr
        % using shaperead
        points = tmpC{i,1}(1:end-1,:) ;
        In     = tmpInC{i,1}(1:end-1) ;
    else
        % using m_shaperead
        points = tmpC{i,1}(1:end,:) ;
        if size(points,2) == 3
            points = points(:,1:2);
        end
        if shapefile_3d
            % if 3-D shapefile
            height = points(:,3);
            points = points(:,1:2);
            type   = tmpC{i,2};
            if strcmp(type,'BA040')
                type = 'ocean';
            elseif strcmp(type,'BH080')
                type = 'lake';
            elseif strcmp(type,'BH140')
                type = 'river';
            end
        else
            height = [];
        end
        In     = tmpInC{i,1}(1:end) ;
    end
    if bbox(1,2) > 180
       lond = abs(diff(points(:,1)));
       if any(lond > 350)
           points(points(:,1) > 180,1) = 0;
       end
    end
    % lets calculate the area of the
    % feature using the shoelace algorithm and decided whether to keep or
    % not based on area.
    if all(points(1,:) == points(end,:))
        area = shoelace(points(:,1),points(:,2));
    else
        area = 999; % not a polygon
    end
    if length(find(In == 1)) == length(points)
        % Wholey inside box
        if area < 4*h0^2 % too small, then don't consider it.
            continue;
        end
        % Set as island (with NaN delimiter)
        k = k + 1 ;
        new_island{k} = [points; NaN NaN];
        if ~isempty(height)
            new_islandb{k} = [points height; NaN NaN NaN];
            new_islandb_type{k} = type;
        end
    else
        %Partially inside box
        if area < 100*h0^2 % too small, then don't consider it.
            continue;
        end
        % Set as mainland
        j = j + 1 ;
        new_main{j} = [points; NaN NaN];
        if ~isempty(height)
            new_mainb{j} = [points height; NaN NaN NaN];
            new_mainb_type{j} = type;
        end
    end
end
if k > 0
    polygon_struct.inner = [polygon_struct.inner; cell2mat(new_island')];
    polygon_struct.innerb = cell2mat(new_islandb');
    polygon_struct.innerb_type = new_islandb_type;
end
if j > 0
    polygon_struct.mainland = [polygon_struct.mainland; cell2mat(new_main')];
    polygon_struct.mainlandb = cell2mat(new_mainb');
    polygon_struct.mainlandb_type = new_mainb_type;
end
% Merge overlapping mainland and inner
if exist('polybool','file') || exist('polyshape','file')
    if ~isempty(new_mainb) && k > 0
        polym = polygon_struct.mainland;
        mergei = false(k,1);
        for kk = 1:k
            polyi = new_island{kk};
            IA = find(ismembertol(polym,polyi,1e-5,'ByRows',true));
            if length(IA) > 2
               if exist('polyshape','file')
                  polyout = union(polyshape(polym),polyshape(polyi));
                  polym = polyout.Vertices;
               else
                  [x,y] = polybool('union',polym(:,1),polym(:,2),...
                                   polyi(:,1),polyi(:,2));
                  polym = [x,y];
               end
               mergei(kk) = 1;
            end
        end
        if ~isnan(polym(end,1)); polym(end+1,:) = NaN; end
        polygon_struct.mainland = polym;
        new_island(mergei) = [];
        polygon_struct.inner = cell2mat(new_island');
    end
else
    warning(['no polyshape or polybool available to merge possible ' ...
             'overlapping of mainland and inner'])
end
% Remove parts of inner and mainland overlapping with outer
polygon_struct.outer = [polygon_struct.outer; polygon_struct.mainland];
%% Plot the map
if plot_on >= 1 && ~isempty(polygon_struct)
    figure(1);
    hold on
    plot(polygon_struct.outer(:,1),polygon_struct.outer(:,2))
    if ~isempty(polygon_struct.inner)
        plot(polygon_struct.inner(:,1),polygon_struct.inner(:,2))
    end
    if ~isempty(polygon_struct.mainland)
        plot(polygon_struct.mainland(:,1),polygon_struct.mainland(:,2))
    end
end
%EOF
end


function mask2conv()
    global config mem;
    curr_layer_idx = config.misc.current_layer;
    in = mem.mask_li{curr_layer_idx};
    for m = 1:config.batch_size
        for n = 1:config.chs
            mem.mask_inputs{curr_layer_idx}((n-1)*config.kernel_size(1, 1)*config.kernel_size(1, 2)+1:n*config.kernel_size(1, 1)*config.kernel_size(1, 2), (m-1)*(size(mem.mask_inputs{curr_layer_idx}, 2)/config.batch_size)+1:m*size(mem.mask_inputs{curr_layer_idx}, 2)/config.batch_size) = ...
                    config.IM2COL(in(:,:,n,m), [config.kernel_size(1, 1), config.kernel_size(1, 2)]);
        end
    end
    % only for SR now
    gen_mask_patch_cat_idx_for_super_res(mem.mask_inputs{1});
end


function sphere_llt_grid_test ( )

%*****************************************************************************80
%
%% SPHERE_LLT_GRID_TEST tests the SPHERE_LLT_GRID library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 April 2015
%
%  Author:
%
%    John Burkardt
%
  timestamp ( )
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPHERE_LLT_GRID_TEST\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Test the SPHERE_LLT_GRID library.\n' );

  sphere_llt_grid_point_count_test ( );
  sphere_llt_grid_points_test ( );
  sphere_llt_grid_line_count_test ( );
  sphere_llt_grid_lines_test ( );
  sphere_llt_grid_display_test ( );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPHERE_LLT_GRID_TEST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


function imout=padimage(im, sizeimout)
% imout=padimage(im, sizeimout)

sizeim = size(im);

sizedifhalf = floor((sizeimout-sizeim)/2);

imout = padarray(im, sizedifhalf);

sizedif = sizeimout-size(imout);

if sizedif(1)>0
    imout = [imout; zeros(1,size(imout,2))];
end
if sizedif(2)>0
    imout = [imout, zeros(size(imout,1),1)];
end


% pdfbcreate_equiv_models
% written by:   Duncan Po
% Date:         December 5/2002
% Usage:        models = pdfbcreate_equiv_models(model)
% Input:    model   -   original model
% Output:   models  -   an array of equivalent models
%
% create equivalent models by flipping the states

function models = pdfbcreate_equiv_models(model)

l = 1;
temp_model{l}{1} = model;
for j = 1:length(model.stdv)
    for k= 1:length(model.stdv{j})
        l = l+1;
        for mm = 1:2.^(l-1)
            if mod(mm, 2) == 1
                temp_model{l}{mm} = pdfbflip_model(temp_model{l-1}{ceil(mm/2)}, j, k);
            else
                temp_model{l}{mm} = temp_model{l-1}{ceil(mm/2)};
            end;
        end;
    end;
end;

num = length(temp_model{end});
for ddd = 1:num
    models{ddd} = temp_model{end}{ddd};
end;


function y = squish(x)
% y = squish(x);    SQUISH "x" to remove all singleton dimensions.
%   Since singleton dimensions can confuse many operations, this function removes
%   absolutely ALL singleton dimensions in "x". SQUEEZE is similar, however
%   it will not operate on 2D arrays, of which row vectors are included. Thus the
%   expected result may not always occur with SQUEEZE.
%   NOTE! This function will convert all row vectors to column vectors!
%   Example:        [1;2;3;4;5] = squish(shiftdim([1:5]',-5))

%   To see the differences between SQUISH and SQUEEZE compare the results
%   of the following for any positive or negative n:
%       size(squeeze(shiftdim([1:3]',n)))
%       size(squish(shiftdim([1:3]',n)))
%   created 08/16/2006 by Mirko Hrovat with Matlab ver.7.2
%   modified 02/25/2010 by MIH as per Jan Simon's suggestion (slight speed improvement)
%   contact: mhrovat@email.com

dims = size(x);
y = reshape(x,[dims(dims~=1),1,1]);    % the extra 1's help in case x is a scalar or vector.
%**************** END ***************


%% Compare the two implementations
% Copyright 2013 The MathWorks, Inc.

%% Run the data through the two implementations
adjustedImageCPU = whitebalance(imageData);
adjustedImageGPU = whitebalance_gpu(imageData);

%% Visualize the results and their difference
subplot(1,2,1); imshow(adjustedImageCPU); title('CPU Output');
subplot(1,2,2); imshow(adjustedImageGPU); title('GPU Output');

difference = double(adjustedImageCPU(:) - adjustedImageGPU(:));
msgbox(['The norm of the difference is: ' num2str(norm(difference))]);


function [xs,nxs] = get_position_and_orientation_ls(xs,conf);
%GET_POSITION_AND_ORIENTATION_LS position and orientation for a line source
%
%   Usage: [xs,nxs] = get_position_and_orientation_ls(xs,conf);
%
%   Input parameters:
%       xs          - combined position and orientation / m [nx3] or [nx6]
%       args        - list of args
%
%   Output parameters:
%       xs          - position of line source / m [nx3]
%       nxs         - orientation of line source / m [nx3]
%
%   GET_POSITION_AND_ORIENTATION_LS(xs,conf) returns the position and
%   orientation of a point source. The orientation is a vector perpendicular to
%   the traveling direction of the line source. For 2D or 2.5D the orientation
%   will always be returned as [0 0 1]. For 3D [0 0 1] will be returned if no
%   explicit orientation is given.
%
%   See also: secondary_source_selection, driving_function_mono_wfs

%*****************************************************************************
% The MIT License (MIT)                                                      *
%                                                                            *
% Copyright (c) 2010-2019 SFS Toolbox Developers                             *
%                                                                            *
% Permission is hereby granted,  free of charge,  to any person  obtaining a *
% copy of this software and associated documentation files (the "Software"), *
% to deal in the Software without  restriction, including without limitation *
% the rights  to use, copy, modify, merge,  publish, distribute, sublicense, *
% and/or  sell copies of  the Software,  and to permit  persons to whom  the *
% Software is furnished to do so, subject to the following conditions:       *
%                                                                            *
% The above copyright notice and this permission notice shall be included in *
% all copies or substantial portions of the Software.                        *
%                                                                            *
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
% IMPLIED, INCLUDING BUT  NOT LIMITED TO THE  WARRANTIES OF MERCHANTABILITY, *
% FITNESS  FOR A PARTICULAR  PURPOSE AND  NONINFRINGEMENT. IN NO EVENT SHALL *
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
% LIABILITY, WHETHER  IN AN  ACTION OF CONTRACT, TORT  OR OTHERWISE, ARISING *
% FROM,  OUT OF  OR IN  CONNECTION  WITH THE  SOFTWARE OR  THE USE  OR OTHER *
% DEALINGS IN THE SOFTWARE.                                                  *
%                                                                            *
% The SFS Toolbox  allows to simulate and  investigate sound field synthesis *
% methods like wave field synthesis or higher order ambisonics.              *
%                                                                            *
% https://sfs.readthedocs.io                            sfstoolbox@gmail.com *
%*****************************************************************************

%% ===== Checking of input  parameters ==================================
nargmin = 2;
nargmax = 2;
narginchk(nargmin,nargmax);
isargnumeric(xs);
isargstruct(conf);

%% ===== Configuration ===================================================
dimension = conf.dimension;

%% ===== Checking for vector =============================================
% Handling of line source orientation
if (strcmp('2D',dimension) ||  strcmp('2.5D',dimension))
    % Ignore orientation for 2D and 2.5D
    if size(xs,2)>3
        warning('%s: %s-WFS ignores virtual line source orientation.', ...
            upper(mfilename),dimension);
    end
    xs(:,3) = 0;
    nxs = repmat([0 0 1],[size(xs,1) 1]);
else
    if size(xs,2)~=6
        warning('%s: set ls orientation to [0 0 1]',upper(mfilename));
        nxs = repmat([0 0 1],[size(xs,1) 1]);
    else
        nxs = xs(:,4:6) / norm(xs(1,4:6),2);
    end
end
xs = xs(:,1:3);


% DC to Dc Buck Converter Parameters
% Dick Benson
% Copyright 2013 The MathWorks, Inc.

FET_Ron=.004;    % FET
FET_Snub= 1000;

L1=250e-9;       % Inductor
L1_R = 0.01;

Cout=1000e-6;    % Filter Cap
Cout_R = 0.0012;

Rload=1.0;       % Load Resistance

Vinput=12;       % Vin to converter

Fswitch=500e3;   % PWM switching frequency
Comp_Tau=100e-9; % PWM Comparitor time constant

Fs_ADC=2.5e6;    % Sampling rate of discrete time controller