% The COBRAToolbox: testEFlux.m
% Aerobic and Anaerobic.txt are obtained from GEO (GSM1126445 and GSM1010240, respectively)
%
% Purpose:
%     - Tests the eFlux function
% Author:
%     - Original file: Thomas Pfau

currentDir = pwd;
% initialize the test
fileDir = fileparts(which('testEFlux.m'));
cd(fileDir);

% requires access to GEO to download data.
% Matlabs tolerances and precision seem incompatible for this function.
solverPkgs = prepareTest('needsLP',true,'toolboxes',{'bioinformatics_toolbox'},'needsWebAddress','https://www.ncbi.nlm.nih.gov/geo/query/','excludeSolvers',{'matlab','pdco'});

model = getDistributedModel('ecoli_core_model.mat');
model = removeGenesFromModel(model,'s0001','keepReactions',true);
geoaddress = 'https://www.ncbi.nlm.nih.gov/geo/query/acc.cgi?form=text&acc=%s&view=full';
anaerobic = geosoftread(urlread(sprintf(geoaddress,'GSM1010240')));
aerobic = geosoftread(urlread(sprintf(geoaddress,'GSM1126445')));
expressionControl.preprocessed = false;
expressionControl.value = cell2mat(aerobic.Data(:,2));
expressionControl.target = cellfun(@(x) x(1:5),aerobic.Data(:,1),'Uniform',false);
expressionCondition.preprocessed = false;
expressionCondition.value = cell2mat(anaerobic.Data(:,2));
expressionCondition.target = cellfun(@(x) x(1:5),anaerobic.Data(:,1),'Uniform',false);
% There is a potential cycle between reaction 44 and 89 in the model.
% an additional cycle exists for reactions 42,43,68,92, which we will also
% ignore.
% we will ignore those reactions for the assertions.
toAssert = true(numel(model.rxns),1);
toAssert([42,43,68,92,44,89]) = false;

% return to original directory
for k = 1:numel(solverPkgs.LP)
    changeCobraSolver(solverPkgs.LP{k},'LP');
    fprintf('Testing EFlux with %s ...\n',solverPkgs.LP{k})
    [fChangeOrig,~,solContOrig,solCondOrig] = eFlux(model,expressionControl,expressionCondition);
    %anaerobic growth less than aerobic.
    assert(fChangeOrig < 1);
    oxpos = ismember(model.rxns,'EX_o2(e)');
    % oxygen uptake in aerobic > than anaerobic (i.e. the value is smaller, exchange reaction).
    assert(solContOrig.x(oxpos) < solCondOrig.x(oxpos))
    % also test the soft bounds
    [fChangeSoft,~,solContSoft,solCondSoft] = eFlux(model,expressionControl,expressionCondition,'softBounds',true);
    %since this is punished with a value of 1, this is exactly the same as
    %the original solution.
    assert(abs(fChangeSoft-fChangeOrig) < 1e-4);

    assert(all(abs(solContOrig.x(toAssert)-solContSoft.x(toAssert)) < 1e-4));
    assert(all(abs(solCondOrig.x(toAssert)-solCondSoft.x(toAssert)) < 1e-4));
    % Different use of gpr rules
    [fChange,~,solCont,solCond] = eFlux(model,expressionControl,expressionCondition,'minSum',false);
    % results are still similar.
    assert(fChange < 1);
    oxpos = ismember(model.rxns,'EX_o2(e)');
    % oxygen uptake in aerobic > than anaerobic (i.e. the value is smaller, exchange reaction).
    assert(solCont.x(oxpos) < solCond.x(oxpos))

    %test this with a 1% noise.
    [fChangeNoise,stderr,solContNoise,solCondNoise] = eFlux(model,expressionControl,expressionCondition,'testNoise',true,'noiseStd',expressionControl.value*0.01);
    assert(abs(fChangeNoise-fChangeOrig) < 1e-4);
    assert(stderr > 0) % this should could be wrong at some point, but it is so unlikely....
end
cd(currentDir)


function varargout = process_diff_mean( varargin )
% PROCESS_DIFF_MEAN: Difference of means of sets A and B.

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

eval(macro_method);
end

%% ===== GET DESCRIPTION =====
function sProcess = GetDescription() %#ok<DEFNU>
    % Description the process
    sProcess.Comment     = 'Difference of means';
    sProcess.Category    = 'Custom';
    sProcess.SubGroup    = 'Test';
    sProcess.Index       = 100;
    sProcess.Description = 'https://neuroimage.usc.edu/brainstorm/Tutorials/Difference';
    % 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;
    sProcess.isSeparator = 1;

    % === Absolue values: legend
    sProcess.options.labelavg.Type = 'label';
    sProcess.options.labelavg.Comment = 'Function to estimate the average across the files:';
    sProcess.options.labelavg.InputTypes = {'data', 'results', 'matrix'};
    % === Absolue values: type
    sProcess.options.avg_func.Comment = {'Arithmetic average
mean(A) - mean(B)', ...
                                         'Absolute value of average
abs(mean(A)) - abs(mean(B))', ...
                                         'Average of absolute values
mean(abs(A)) - mean(abs(B))', ...
                                         'Power
mean(A^2) - mean(B^2)'};
    sProcess.options.avg_func.Type    = 'radio';
    sProcess.options.avg_func.Value   = 2;
    sProcess.options.avg_func.InputTypes = {'data', 'results', 'matrix'};
    % === WEIGHTED AVERAGE
    sProcess.options.weighted.Comment    = 'Weighted average:  mean(x) = sum(Leff_i * x(i)) / sum(Leff_i)';
    sProcess.options.weighted.Type       = 'checkbox';
    sProcess.options.weighted.Value      = 0;
    sProcess.options.weightedlabel.Comment    = '      Leff_i = Effective number of averages for file #i';
    sProcess.options.weightedlabel.Type       = 'label';
    % === MATCH ROWS WITH NAMES
    sProcess.options.matchrows.Comment    = 'Match signals between files using their names';
    sProcess.options.matchrows.Type       = 'checkbox';
    sProcess.options.matchrows.Value      = 1;
    sProcess.options.matchrows.InputTypes = {'timefreq', 'matrix'};
    % === EXCLUDE ZEROS FROM THE AVERAGE
    sProcess.options.iszerobad.Comment    = 'Exclude the flat signals from the average (zero at all times)';
    sProcess.options.iszerobad.Type       = 'checkbox';
    sProcess.options.iszerobad.Value      = 1;
    sProcess.options.iszerobad.InputTypes = {'timefreq', 'matrix'};

    % === UNCONSTRAINED SOURCES
    sProcess.options.label_norm.Comment    = ['Note: For unconstrained sources, "absolute value" refers to the norm
' ...
                                              'of the three orientations: abs(F) = sqrt(Fx^2 + Fy^2 + Fz^2)'];
    sProcess.options.label_norm.Type       = 'label';
    sProcess.options.label_norm.InputTypes = {'results'};
end

%% ===== FORMAT COMMENT =====
function [Comment, strAbs] = FormatComment(sProcess)
    % Weighted
    if isfield(sProcess.options, 'weighted') && isfield(sProcess.options.weighted, 'Value') && ~isempty(sProcess.options.weighted.Value) && sProcess.options.weighted.Value
        Comment = 'Difference of weighted means';
    else
        Comment = 'Difference of means';
    end
    % If sources: averaging option
    if isfield(sProcess.options, 'avg_func')
        switch(sProcess.options.avg_func.Value)
            case 1,  strAbs = '[mean]';
            case 2,  strAbs = '[abs(mean)]';
            case 3,  strAbs = '[mean(abs)]';
            case 4,  Comment = 'Difference of power';    strAbs = '';
        end
    else
        strAbs = '[mean]';
    end
    if ~isempty(strAbs)
        Comment = [Comment, ' ', strAbs];
    end
end

%% ===== RUN =====
function OutputFiles = Run(sProcess, sInputsA, sInputsB) %#ok<DEFNU>
    OutputFiles = {};
    % Make sure that file type is indentical for both sets
    if ~isempty(sInputsA) && ~isempty(sInputsB) && ~strcmpi(sInputsA(1).FileType, sInputsB(1).FileType)
        bst_report('Error', sProcess, sInputsA, 'Cannot process inputs from different types.');
        return;
    end
    % === GET OPTIONS ===
    isResults = strcmpi(sInputsA(1).FileType, 'results');
    if isfield(sProcess.options, 'avg_func')
        switch (sProcess.options.avg_func.Value)
            case 1,  Function = 'mean'; isAbsDiff = 0; strComment = '';
            case 2,  Function = 'mean'; isAbsDiff = 1; strComment = ' [abs(avg)]';
            case 3,  Function = 'norm'; isAbsDiff = 0; strComment = ' [avg(abs)]';
            case 4,  Function = 'rms';  isAbsDiff = 0; strComment = ' [power]';
        end
    else
        strComment = '';
        Function = 'mean';
        isAbsDiff = 0;
    end
    % Weighted
    if isfield(sProcess.options, 'weighted') && isfield(sProcess.options.weighted, 'Value') && ~isempty(sProcess.options.weighted.Value)
        isWeighted = sProcess.options.weighted.Value;
    else
        isWeighted = 0;
    end
    % Match signals between files using their names
    if isfield(sProcess.options, 'matchrows') && isfield(sProcess.options.matchrows, 'Value') && ~isempty(sProcess.options.matchrows.Value)
        isMatchRows = sProcess.options.matchrows.Value;
    else
        isMatchRows = 1;
    end
    % Exclude zero values
    if isfield(sProcess.options, 'iszerobad') && isfield(sProcess.options.iszerobad, 'Value') && ~isempty(sProcess.options.iszerobad.Value)
        isZeroBad = sProcess.options.iszerobad.Value;
    else
        isZeroBad = 1;
    end
    % Read first file of the list
    [sMat, matName] = in_bst(sInputsA(1).FileName);

    % === COMPUTE DIFFERENCE OF AVG ===
    % Compute average of the two sets of files
    isVariance = 0;
    [StatA, MessagesA, iAvgFileA] = bst_avg_files({sInputsA.FileName}, [], Function, isVariance, isWeighted, isMatchRows, isZeroBad);
    [StatB, MessagesB, iAvgFileB] = bst_avg_files({sInputsB.FileName}, [], Function, isVariance, isWeighted, isMatchRows, isZeroBad);
    % Add messages to report
    if ~isempty(MessagesA)
        if isempty(StatA)
            bst_report('Error', sProcess, sInputsA, MessagesA);
            return;
        else
            bst_report('Warning', sProcess, sInputsA, MessagesA);
        end
    end
    if ~isempty(MessagesB)
        if isempty(StatB)
            bst_report('Error', sProcess, sInputsB, MessagesB);
            return;
        else
            bst_report('Warning', sProcess, sInputsB, MessagesB);
        end
    end
    % Absolute values before difference
    if isAbsDiff
        % Unconstrained sources: Convert to flat map (norm of the three orientations)
        if isResults && isfield(sMat, 'nComponents') && (sMat.nComponents ~= 1)
            % Average A
            sMatTmp = sMat;
            sMat.(matName) = StatA.mean;
            sMat = process_source_flat('Compute', sMat, 'rms');
            StatA.mean = sMat.(matName);
            % Average B
            sMat = sMatTmp;
            sMat.(matName) = StatB.mean;
            sMat = process_source_flat('Compute', sMat, 'rms');
            StatB.mean = sMat.(matName);
            % Edit the comment field to indicate we used a norm
            strComment = strrep(strComment, 'abs', 'norm');
        end
        % Enforce absolute values
        StatA.mean = abs(StatA.mean);
        StatB.mean = abs(StatB.mean);
    % Unconstrained or mixed sources: source maps have been flattened by bst_avg_files
    elseif isResults && ismember(Function, {'norm', 'rms'}) && (sMat.nComponents ~= 1)
        sMat = process_source_flat('Compute', sMat, 'rms');
    end
    % Difference of power: square the rms that is returned by the averaging function
    if strcmpi(Function, 'rms')
        StatA.mean = StatA.mean .^ 2;
        StatB.mean = StatB.mean .^ 2;
    end
    % Check timefreq measures
    if ~isempty(StatA.Measure) && ~strcmpi(StatA.Measure, StatB.Measure)
        bst_report('Error', sProcess, [sInputsA(:)',sInputsB(:)'], 'The two sets of files have different measures applied to the time-frequency coefficients.');
        return
    end
    % Compute difference of averages
    StatA.mean = StatA.mean - StatB.mean;

    % === CREATE OUTPUT STRUCTURE ===
    bst_progress('start', 'Difference of means', 'Saving result...');
    [processComment, strAbs] = FormatComment(sProcess);
    % Get output study
    [sStudy, iStudy] = bst_process('GetOutputStudy', sProcess, [sInputsA, sInputsB]);
    % Comment: forced in the options
    if isfield(sProcess.options, 'Comment') && isfield(sProcess.options.Comment, 'Value') && ~isempty(sProcess.options.Comment.Value)
        Comment = sProcess.options.Comment.Value;
    % Comment: process default
    else
        % Get comment for files A and B
        [tmp__, tmp__, CommentA] = bst_process('GetOutputStudy', sProcess, sInputsA, [], 0);
        [tmp__, tmp__, CommentB] = bst_process('GetOutputStudy', sProcess, sInputsB, [], 0);
        if strcmpi(CommentA, CommentB) && ~strcmpi(sInputsA(1).Condition, sInputsB(1).Condition)
            CommentA = sInputsA(1).Condition;
            CommentB = sInputsB(1).Condition;
            % Add file number
            if (length(sInputsA) > 1) || (length(sInputsB) > 1)
                CommentA = [CommentA ' (' num2str(length(sInputsA)) ')'];
                CommentB = [CommentB ' (' num2str(length(sInputsB))  ')'];
            end
        end
        % Comment: difference A-B
        Comment = [CommentA ' - ' CommentB];
        if ~isempty(strAbs)
           Comment = [Comment ' ' strAbs];
        end
    end
    % Copy fields from StatA structure
    sMat.(matName)   = StatA.mean;
    sMat.Comment     = Comment;
    sMat.ChannelFlag = StatA.ChannelFlag;
    sMat.Time        = StatA.Time;
    sMat.nAvg        = StatA.nAvg + StatB.nAvg;
    % Effective number of averages
    % Leff = 1 / sum_i(w_i^2 / Leff_i),  with w1=1 and w2=-1
    %      = 1 / (1/Leff_A + 1/Leff_B))
    sMat.Leff = 1 / (1/StatA.Leff + 1/StatB.Leff);

    % Colormap for recordings: keep the original
    % Colormap for sources, timefreq... : difference (stat2)
    if ~strcmpi(sInputsA(1).FileType, 'data')
        sMat.ColormapType = 'stat2';
    end
    % Time-frequency: Change the measure type
    % if strcmpi(sInputsA(1).FileType, 'timefreq')
    %     sMat.Measure = 'other';
    % end

    % History: Average
    if isfield(sMat, 'History')
        prevHistory = sMat.History;
        sMat = bst_history('reset', sMat);
        sMat = bst_history('add', sMat, 'diff_mean', processComment);
        sMat = bst_history('add', sMat, 'diff_mean', 'History of the first file:');
        sMat = bst_history('add', sMat, prevHistory, ' - ');
    else
        sMat = bst_history('add', sMat, 'diff_mean', Comment);
    end
    % History: Number of files averaged for each channel
    if any(StatA.nGoodSamples(1) ~= StatA.nGoodSamples) && isfield(StatA, 'RowNames') && ~isempty(StatA.RowNames) && iscell(StatA.RowNames) && (length(StatA.RefRowNames) <= 1)
        strInfo = 'Number of files that were averaged, for each signal (FilesA):';
        sMat = bst_history('add', sMat, 'average', strInfo);
        uniqueGood = unique(StatA.nGoodSamples);
        for i = length(uniqueGood):-1:1
            strHistoryGood = [sprintf(' - %d files: ', uniqueGood(i)), sprintf('%s ', StatA.RowNames{StatA.nGoodSamples == uniqueGood(i)})];
            sMat = bst_history('add', sMat, 'average', strHistoryGood);
            strInfo = [strInfo, 10, strHistoryGood];
        end
        % Generate process warning
        bst_report('Warning', sProcess, sInputsA, strInfo);
    end
    % History: List files A
    sMat = bst_history('add', sMat, 'diff_mean', 'List of files in group A:');
    for i = 1:length(iAvgFileA)
        sMat = bst_history('add', sMat, 'diff_mean', [' - ' sInputsA(iAvgFileA(i)).FileName]);
    end
    % History: List files B
    sMat = bst_history('add', sMat, 'diff_mean', 'List of files in group B:');
    for i = 1:length(iAvgFileB)
        sMat = bst_history('add', sMat, 'diff_mean', [' - ' sInputsB(iAvgFileB(i)).FileName]);
    end

    % Averaging results from the different data file: reset the "DataFile" field
    if isfield(sMat, 'DataFile')
        sMat.DataFile = [];
    end
    % Do not keep the events
    if isfield(sMat, 'Events') && ~isempty(sMat.Events)
        sMat.Events = [];
    end
    % Fix surface link for warped brains
    if isfield(sMat, 'SurfaceFile') && ~isempty(sMat.SurfaceFile) && ~isempty(strfind(sMat.SurfaceFile, '_warped'))
        sMat = process_average('FixWarpedSurfaceFile', sMat, sInputsA(1), sStudy);
    end
    % Rownames
    if isfield(StatA, 'RowNames') && ~isempty(StatA.RowNames)
        if strcmpi(matName, 'TF')
            sMat.RowNames = StatA.RowNames;
        elseif strcmpi(matName, 'Value')
            sMat.Description = StatA.RowNames;
        end
    end

    % === SAVE FILE ===
    % Output filename
    fileTag = bst_process('GetFileTag', sInputsA(1).FileName);
    OutputFiles{1} = bst_process('GetNewFilename', bst_fileparts(sStudy.FileName), [fileTag, '_diff_mean']);
    % Save on disk
    bst_save(OutputFiles{1}, sMat, 'v6');
    % Register in database
    db_add_data(iStudy, OutputFiles{1}, sMat);
end


function report(dir1, suffix1, showcls, do_auc_ap)
% Print scores for all classes.

% AUTORIGHTS
% -------------------------------------------------------
% Copyright (C) 2009-2012 Ross Girshick
%
% This file is part of the voc-releaseX code
% (http://people.cs.uchicago.edu/~rbg/latent/)
% and is available under the terms of an MIT-like license
% provided in COPYING. Please retain this notice and
% COPYING if you use this file (or a portion of it) in
% your project.
% -------------------------------------------------------

conf = voc_config();

if nargin < 3
  showcls = true;
end

if nargin < 4
  do_auc_ap = false;
end

count1 = 0;
for i=1:length(conf.pascal.VOCopts.classes)
  cls = conf.pascal.VOCopts.classes{i};
  try
    load([dir1 cls suffix1]);
    ap1 = ap;
    if do_auc_ap
      ap2 = xVOCap(recall, prec);
      score2(i) = ap2;
    end
    if showcls
      if do_auc_ap
        fprintf('%12s %.3f %.3f\n', cls, ap1, ap2);
      else
        fprintf('%12s %.3f\n', cls, ap1);
      end
    else
      if do_auc_ap
        fprintf('%.3f %.3f\n', ap1);
      else
        fprintf('%.3f\n', ap1);
      end
    end
    score1(i) = ap1;
  catch
    score1(i) = nan;
    score2(i) = nan;
    if showcls
      fprintf('%12s -\n', cls);
    else
      fprintf('-\n');
    end
  end
end

a1 = nanmean(score1);
if do_auc_ap
  a2 = nanmean(score2);
end
if showcls
  fprintf('%s\n', repmat('-', [1 12]));
  if do_auc_ap
    fprintf('%12s %.3f %.3f\n', 'mAP', a1, a2);
  else
    fprintf('%12s %.3f\n', 'mAP', a1);
  end
else
  if do_auc_ap
    fprintf('%.3f %.3f\n', a1, a2);
  else
    fprintf('%.3f\n', a1);
  end
end


function C = cs_droptol (A, tol)                                            %#ok
%CS_DROPTOL remove small entries from a sparse matrix.
%   C = cs_droptol(A,tol) removes entries from A of magnitude less than or
%   equal to tol.  Same as A = A .* (abs (A) >= tol).
%
%   Example:
%       Prob = UFget ('HB/arc130') ; A = Prob.A ;
%       cspy (abs (A) >= 1e-10) ;
%       C = cs_droptol (A, 1e-10) ;
%       cspy (C) ;
%
%   See also: RELOP, ABS

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

error ('cs_droptol mexFunction not found') ;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% To run:  Edit user options in mtfcalc.m and execute the code.
%
% This code is intended to accompany the paper:
%
% S. N. Friedman, G. S. K. Fung, J. H. Siewerdsen, and B. M. W. Tsui.  "A
% simple approach to measure computed tomography (CT) modulation transfer
% function (MTF) and noise-power spectrum (NPS) using the American College
% of Radiology (ACR) accreditation phantom," Med. Phys. 40, 051907-1 -
% 051907-9 (2013).
% http://dx.doi.org/10.1118/1.4800795
%
% This code is free to distribute (see below).
%
% This program requires the following 2 files:
%   mtfcalc.m
%   license.txt
%
% Purpose:  To calculate the 1D radial (axial) MTF using CT data of the
%           American College of Radiology (ACR) accreditation phantom.
%
% Input:    Two consecutive scans of the phantom are needed.  The program
%           requires a data directory to be selected in which there are
%           only two subdirectories containing only CT slices corresponding
%           to the third module of the phantom.  Be careful of partial
%           volume effects with surrounding modules.
%
%           i.e.,  datadir
%                      |-> scan 1 dir
%                      |           | -> only module 3 slices
%                      |
%                      |-> scan 2 dir
%                                  |-> only module 3 slices
%
% Copyright 2012 Saul N. Friedman
%   Distributed under the terms of the "New BSD License."  Please see
%   license.txt.
%
% N.B.:  This code assumed the data are already organized such that only
%        relevant axial slices are contained within the data directory.
%        ESF = edge spread function
%        LSF = line spread function
%        MTF = modulation transfer function
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

close all;
clear all;
clc;

fprintf('****************************************************************************\n');
fprintf('CT MTF Calculation\n\n')
fprintf('This code is intended to accompany the paper:\n');
fprintf('S. N. Friedman, G. S. K. Fung, J. H. Siewerdsen, and B. M. W. Tsui.  "A\n')
fprintf('simple approach to measure computed tomography (CT) modulation transfer \n')
fprintf('function (MTF) and noise-power spectrum (NPS) using the American College \n')
fprintf('of Radiology (ACR) accreditation phantom," Med. Phys. 40, 051907-1 - \n')
fprintf('051907-9 (2013).\n')
fprintf('http://dx.doi.org/10.1118/1.4800795\n\n')
fprintf('This code can be freely distributed.  Please see license.txt for details.\n');
fprintf('****************************************************************************\n\n');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% User selected options
%

selectdir = 1;   % 1 = prompt for directly locations via dialogue box, 0 = hardcode locations below
    datadir = fullfile('data');
    resultsdir = fullfile('results',datadir);

saveresults = 1;  % Choose whether to save the calculated MTF to disk in text files

showfig = 3;  % decide how important a figure should be in order to be plotted, 1 = no figures, 4 = all figures

rstep = 0.1; %  Choose bin size in mm (i.e., effective pixel size) for oversampled edge spread function (ESF)

window = 1;  % Decide whether to apply a Hann window to the LSF before calculating the MTF (typically applied)

% Background and foreground values:  1 = prompted to draw rectangle to select ROI, 0 = use values below
manualROI = 1;
   % box border for background ROI in pixels
   BGxROI1 = 1;
   BGxROI2 = 100;
   BGyROI1 = 1;
   BGyROI2 = 100;

   % box border for foreground ROI in pixels
   FGxROI1 = 205;
   FGxROI2 = 305;
   FGyROI1 = 208;
   FGyROI2 = 308;

% 2D map of good pixels (1) and bad pixels (0) to use in calculation
% 1 = prompted to draw rectangle(s) to select ROI(s), 0 = use values below
manualmask = 1;
selectmask = 1; %prompt for image to use when creating data mask (only applicable if manualmask = 1)
    mask = ones(512,512);
    mask(450:512,1:125) = 0;
    mask(450:512,400:512) = 0;
    mask(400:425,84:105) = 0;
    mask(410:430,400:420) = 0;
    mask(220:230,279:288) = 0;
    mask(371:381,138:147) = 0;

% Determine which angles of data to use in radians (-pi to pi to use all)
thlim1 = -pi;
thlim2 = pi;

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Start of analysis code
%

homedr = cd;

if selectdir ==1
    fprintf('Select the data directory.\n\n');
    datadir = uigetdir(homedr,'Pick data directory');
    if saveresults == 1
        fprintf('Select the results directory.\n\n');
        resultsdir = uigetdir(homedr,'Pick results directory');
    end
else
    datadir = fullfile(homedr,datadir);
    resultsdir = fullfile(homedr,resultsdir);
end

if saveresults == 1
    mkdir(resultsdir)
end

cd(datadir)

scandir = dir;

cd(homedr)

while or(strcmp(scandir(1).name,'.'),strcmp(scandir(1).name,'..'))
    scandir = scandir(2:end);
end

Nscan = size(scandir,1);
Nscan = 1;

nfilescheck = 0;

for i = 1:Nscan
    cd(fullfile(datadir,scandir(i).name))

    % get list of filenames

    ffiles = dir;

    cd(homedr)

    while or(strcmp(ffiles(1).name,'.'),strcmp(ffiles(1).name,'..'))
        ffiles = ffiles(2:end);
    end

    Nz = size(ffiles,1);

     if nfilescheck ~= Nz && nfilescheck ~=0
        fprintf('Error.  Scan directories do not contain the same number of slices.\n')
        return
    end

    nfilescheck = Nz;

    if i == 1

        % Read DICOM header info and determine correct mapping function to get HU
        % values as well as voxel sizes.

        im = single(dicomread(fullfile(datadir,scandir(i).name,ffiles(1).name)));

        imheader = dicominfo(fullfile(datadir,scandir(i).name,ffiles(1).name));

        b = imheader.RescaleIntercept;
        m = imheader.RescaleSlope;

        im = im.* m + b;

        pixelx = imheader.PixelSpacing(1);
        pixely = imheader.PixelSpacing(2);
        pixelz = imheader.SliceThickness;

        if showfig > 2
            figure
            imagesc(im)
            colormap gray
            hold on
            axis image;
            impixelinfo;
        end

        Nx = size(im,2);
        Ny = size(im,1);

        imaxisx = [1:Nx] .* pixelx;
        imaxisy = [1:Ny] .* pixely;

        imavg = zeros(Ny,Nx);
        im3D = zeros(Ny,Nx,Nz);
    end

    % Load data into memory and calculate an average 2D image to later
    % determine phantom location

    for z = 1:Nz
        im = single(dicomread(fullfile(datadir,scandir(i).name,ffiles(z).name)));
        im = im.* m + b;
        im3D(:,:,z,i) = im;
        imavg = imavg + im;
    end

end

imavg = imavg ./(Nz*Nscan);

if selectmask == 1 && manualmask == 1
    fprintf('Select the DICOM image to use for mask creation.\n\n');
    [imfile impath] = uigetfile('*','Pick a DICOM image to use for mask creation',datadir);
    if isequal(imfile,0) || isequal(impath,0)
        im = imavg;
    else
        im = single(dicomread(fullfile(impath,imfile)));
        im = im.* m + b;
    end
else
    im = imavg;
end

% Plot sample slice.  Use to create mask image if set to manual selection.

imaxisx = [1:size(im,2)] .* pixelx;
imaxisy = [1:size(im,1)] .* pixely;

if showfig > 3 || manualmask == 1
    figure
    imagesc(im)
    xlabel('pixel')
    ylabel('pixel')
    colormap gray
    hold on
    axis image;
    impixelinfo;
end

if manualmask ==1

    mask = ones(size(im));

    selectROI = 1;

    fprintf('Select the ROIs corresponding to bad data.\n\n');

    while selectROI == 1
        choice = menu('Select additional ROI of bad data?','Yes','No');

        if choice == 2
            selectROI = 0;
            hold off
        else
            h = imrect;
            ptmask = wait(h);

            maskx1 = round(ptmask(1));
            maskx2 = maskx1 + round(ptmask(3));
            masky1 = round(ptmask(2));
            masky2 = masky1 + round(ptmask(4));

            % force selected ROI to be within the bounds of the image
            maskx1 = min(maskx1,size(im,2));
            maskx2 = min(maskx2,size(im,2));
            masky1 = min(masky1,size(im,1));
            masky2 = min(masky2,size(im,1));
            maskx1 = max(maskx1,1);
            maskx2 = max(maskx2,1);
            masky1 = max(masky1,1);
            masky2 = max(masky2,1);

            mask(masky1:masky2,maskx1:maskx2) = 0;

            plot([maskx1,maskx2,maskx2,maskx1,maskx1],[masky1,masky1,masky2,masky2,masky1],'r-')
        end
    end
end

% Plot mask image overlying sample image to verify mask

if showfig > 3
    figure
    imagesc([imaxisy(1) imaxisy(end)],[imaxisx(1) imaxisx(end)],(im+100).*mask)
    xlabel('mm')
    ylabel('mm')
    colormap gray
    hold on
    axis image;
    impixelinfo;
end

% Show averaged image if desired or needed

if showfig > 2 || manualROI == 1
    figure
    imagesc(imavg)
    xlabel('pixel')
    ylabel('pixel')
    colormap gray
    hold on
    axis image;
    impixelinfo;
end

% Determine background and foreground values to properly normalized the
% data to range [0,1]

if manualROI == 1
    fprintf('Click and drag to create a rectangular ROI representing the background.  Double click the ROI when finished.\n\n');

    h = imrect;
    ptROI = wait(h);

    BGxROI1 = round(ptROI(1));
    BGxROI2 = BGxROI1 + round(ptROI(3));
    BGyROI1 = round(ptROI(2));
    BGyROI2 = BGyROI1 + round(ptROI(4));

    % force selected ROI to be within the bounds of the image
    BGxROI1 = min(BGxROI1,size(im,2));
    BGxROI2 = min(BGxROI2,size(im,2));
    BGyROI1 = min(BGyROI1,size(im,1));
    BGyROI2 = min(BGyROI2,size(im,1));
    BGxROI1 = max(BGxROI1,1);
    BGxROI2 = max(BGxROI2,1);
    BGyROI1 = max(BGyROI1,1);
    BGyROI2 = max(BGyROI2,1);
end

BGvalue = mean(mean(imavg(BGyROI1:BGyROI2,BGxROI1:BGxROI2)));

if manualROI == 1
    fprintf('Click and drag to create a rectangular ROI representing the foreground.  Double click the ROI when finished.\n\n');

    h = imrect;
    ptROI = wait(h);

    FGxROI1 = round(ptROI(1));
    FGxROI2 = FGxROI1 + round(ptROI(3));
    FGyROI1 = round(ptROI(2));
    FGyROI2 = FGyROI1 + round(ptROI(4));

    % force selected ROI to be within the bounds of the image
    FGxROI1 = min(FGxROI1,size(im,2));
    FGxROI2 = min(FGxROI2,size(im,2));
    FGyROI1 = min(FGyROI1,size(im,1));
    FGyROI2 = min(FGyROI2,size(im,1));
    FGxROI1 = max(FGxROI1,1);
    FGxROI2 = max(FGxROI2,1);
    FGyROI1 = max(FGyROI1,1);
    FGyROI2 = max(FGyROI2,1);
end

FGvalue = mean(mean(imavg(FGyROI1:FGyROI2,FGxROI1:FGxROI2)));

if or(showfig > 2,manualROI ==1)
    plot([BGxROI1,BGxROI2,BGxROI2,BGxROI1,BGxROI1],[BGyROI1,BGyROI1,BGyROI2,BGyROI2,BGyROI1],'r-')
    plot([FGxROI1,FGxROI2,FGxROI2,FGxROI1,FGxROI1],[FGyROI1,FGyROI1,FGyROI2,FGyROI2,FGyROI1],'r-')
    hold off
end

% Normalize averaged image such that values are [0,1]

imavg = (imavg - BGvalue) ./ (FGvalue - BGvalue);

if showfig > 2
    figure
    imagesc([imaxisy(1) imaxisy(end)],[imaxisx(1) imaxisx(end)],imavg)
    xlabel('mm')
    ylabel('mm')
    title('averaged and normalized image')
    colormap gray
    hold on
    axis image;
    impixelinfo;
end

% Determine which pixels correspond to the phantom cylinder

imlog = zeros(size(imavg));

level = graythresh(imavg);
imlog = im2bw(imavg,level);

% Calculate the center of the cylinder

fprintf('Calculating the center of the phantom and determining phantom-pixel locations.\n\n');

if exist('bwconncomp')==2
    CC = bwconncomp(imlog,26);
    for i = 1:CC.NumObjects;
        Lobj(i) =  length(CC.PixelIdxList{1,i}) ;
    end
else
    [CC CCn] = bwlabel(imlog,8);
    for i = 1:CCn
        Lobj(i) = length(find(CC == i));
    end
end

L = find(Lobj == max(Lobj));

S = regionprops(CC,'Centroid');

c.y = S(L).Centroid(2);
c.x = S(L).Centroid(1);

if showfig>2
    figure
    imagesc([imaxisy(1) imaxisy(end)],[imaxisx(1) imaxisx(end)],imlog)
    xlabel('mm')
    ylabel('mm')
    title('Cylinder with center marked')
    colormap gray
    hold on
    axis image;
    impixelinfo;
    if exist('bwconncomp')==2
        [I J] = ind2sub(size(imavg),CC.PixelIdxList{1,L});
    else
        [I,J] = find(CC == L);
    end
    plot(J*pixely,I.*pixelx,'.r')
    plot(c.x*pixelx,c.y*pixely,'*y')
    hold off
end

xaxis = ([1:size(imavg,2)] - c.x) .* pixelx;
yaxis = ([1:size(imavg,1)] - c.y) .* pixely;

% Convert all image coords to polar coords with origin at center of the
% cylinder

r = zeros(size(imavg));
th = zeros(size(imavg));

for i = 1 :size(imavg,2)
    for j = 1 : size(imavg,1)
        %         r(j,i) = sqrt(xaxis(i).^2 + yaxis(j).^2);
        %         th(j,i) = atan2(yaxis(j),xaxis(i));
        [th(j,i) r(j,i)] = cart2pol(xaxis(i),yaxis(j));
    end
end

% Create oversampled ESF excluding data specified in mask

rmax = max(max(r));

esf = zeros(ceil(rmax/rstep)+1,1);
nsamp = zeros(length(esf),1);

rbin = 0:rstep:rmax+rstep;

if showfig > 3
    figure
    imagesc((imavg+1).*mask)
    colormap gray
    hold all
    axis image;
    impixelinfo;
end

% data with radius falling within a given bin are averaged together for a
% low noise approximation of the ESF at the given radius

fprintf('Calculating the MTF.\n');
fprintf('The may take several minutes. \n');
fprintf('Please be patient.\n\n');

thlim1 = min(thlim1,thlim2);
thlim2 = max(thlim1,thlim2);

for i = 1:length(rbin)
    R1 = find(r >= rbin(i));
    R2 = find(r < rbin(i) + rstep);
    R3 = intersect(R1,R2);
    R4 = find(th >= thlim1);
    R5 = find(th < thlim2);
    R6 = intersect(R5,R4);
    R7 = intersect(R3,R6);
    R8 = R7.*mask(R7);
    R=R8(R8~=0);
    if showfig > 3
        [X Y] = ind2sub(size(imavg),R);
        plot(Y,X,'.')
        hold all
    end
    esf(i) = sum(imavg(R));
    nsamp(i) = length(R);
end

i1 = find(nsamp,1,'first');
i2 = find(nsamp,1,'last');
nsamp = nsamp(i1:i2);
esf = esf(i1:i2);
rbin = rbin(i1:i2);

I = find(nsamp > 0);
esf(I) = esf(I)./nsamp(I);

% Plot oversampled ESF
if showfig > 2
    figure
    plot(rbin,esf)
    xlabel('radius (mm)')
    ylabel('normalized + oversampled ESF')
    grid on
    axis([rbin(1) rbin(end) -1.2*min(esf) 1.1*max(esf)])
end

% Calculated the LSF from the ESF
lsf = diff(esf);

% artifacts caused by empty bins likely have a value of -/+ 1, so try to
% remove them.
I = find(abs(lsf)> 0.9);
lsf(I) = 0;

if max(lsf) < max(abs(lsf))
    lsf = -lsf;
end

maxlsf = max(lsf);

rcent = find(lsf == maxlsf);
lsfaxis = rbin(1:end-1) - rbin(rcent);

% Center edge location in LSF and keep surrounding data range specified by
% pad.  N.B. data from regions greater than the FOV is removed in
% this manner.
pad = round(length(lsfaxis)/5);
lsfaxis = lsfaxis(rcent-pad:rcent+pad);
lsf = lsf(rcent-pad:rcent+pad);

nlsf = length(lsf);

%calculate Hann window and apply to data if specified.
if window == 1
    w = 0.5 .* (1 - cos(2*pi*[1:length(lsfaxis)]./length(lsfaxis)));
    w = w';
else
    w = ones(length(lsfaxis),1);
end
lsfw = lsf .* w;

% Plot windowed LSF and scaled window together
if showfig > 1
    figure
    plot(lsfaxis,lsfw)
    hold on
    plot(lsfaxis,w.*max(lsfw),'r--')
    hold off
    legend('oversampled + normalized LSF','scaled Hann window')
    xlabel('radius (mm)')
    ylabel('signal')
    axis([lsfaxis(1) lsfaxis(end) 1.1*min(lsfw) 1.1*max(lsfw)])
    grid on
end

% Calculate the MTF from the LSF

T = fftshift(fft(lsfw));
faxis = ([1:nlsf]./nlsf - 0.5) ./ rstep;

MTF = abs(T);

%Plot the MTF
if showfig > 1
    figure
    plot(faxis,MTF,'.-')
    xlabel('spatial frequency (mm^{-1})');
    ylabel('MTF_r')
    axis([0 max(faxis) 0 1.1*max(MTF)])
    grid on
end

fprintf('MTF calculated.\n\n');

% Save MTF results if indicated
if saveresults == 1
    save(fullfile(resultsdir,'axisvalues.txt'),'faxis','-ASCII')
    save(fullfile(resultsdir,'MTFvalues.txt'),'MTF','-ASCII')
    fprintf('Results saved.\n\n');
end

return;
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function V = spm_write_vol(V,Y)
% Write an image volume to disk, setting scales and offsets as appropriate
% FORMAT V = spm_write_vol(V,Y)
% V (input)  - a structure containing image volume information (see spm_vol)
% Y          - a one, two or three dimensional matrix containing the image voxels
% V (output) - data structure after modification for writing.
%
% Note that if there is no 'pinfo' field, then SPM will figure out the
% max and min values from the data and use these to automatically determine
% scalefactors.  If 'pinfo' exists, then the scalefactor in this is used.
%_______________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% John Ashburner
% $Id$

use_offset = false;

if ndims(Y)>3, error('Can only handle a maximum of 3 dimensions.'), end
dim = [size(Y) 1 1 1];
if ~all(dim(1:3) == V.dim(1:3)),
    error('Incompatible dimensions.');
end

if ~isfield(V,'pinfo'),
    V.pinfo = [1;0;0];
    rescal  = true;
elseif ~all(isfinite(V.pinfo(1:2))) || V.pinfo(1) == 0,
    V.pinfo(1:2) = [1;0];
    rescal  = true;
else
    rescal = false;
end

if rescal,
    % Set scalefactors and offsets
    %-----------------------------------------------------------------------
    dt           = V.dt(1);
    s            = find(dt == [2 4 8 256 512 768]);
    if isempty(s),
        V.pinfo(1:2) = [1;0];
    else
        dmnmx        = [0 -2^15 -2^31 -2^7 0 0 ; 2^8-1 2^15-1 2^31-1 2^7-1 2^16-1 2^32-1];
        dmnmx        = dmnmx(:,s);
        mxs          = zeros(dim(3),1)+NaN;
        mns          = zeros(dim(3),1)+NaN;

        for p=1:dim(3),
            tmp    = double(Y(:,:,p));
            tmp    = tmp(isfinite(tmp));
            if ~isempty(tmp),
                mxs(p) = max(tmp);
                mns(p) = min(tmp);
            end
        end

        mx = max(mxs(isfinite(mxs)));
        mn = min(mns(isfinite(mns)));
        if isempty(mx), mx = 0; end;
        if isempty(mn), mn = 0; end;
        if mx~=mn,
            if use_offset,
                V.pinfo(1,1) = (mx-mn)/(dmnmx(2)-dmnmx(1));
                V.pinfo(2,1) = (dmnmx(2)*mn-dmnmx(1)*mx)/(dmnmx(2)-dmnmx(1));
            else
                if dmnmx(1)<0,
                    V.pinfo(1) = max(mx/dmnmx(2),mn/dmnmx(1));
                else
                    V.pinfo(1) = mx/dmnmx(2);
                end
                V.pinfo(2) = 0;
            end
        else
            V.pinfo(1,1) = mx/dmnmx(2);
            V.pinfo(2,1) = 0;
        end
    end
end

%-Create and write image
%-----------------------------------------------------------------------
V = spm_create_vol(V);
V = spm_write_plane(V,Y,':');


function  [n, adchans] = plx_ad_chanmap(filename)
% plx_ad_chanmap(filename): return map of raw continuous channel numbers for each channel
%            for the specified .plx or .pl2 file
%
% [n, adchans] = plx_ad_chanmap(filename)
%
% INPUT:
%   filename - if empty string, will use File Open dialog
%
% OUTPUT:
%   n - number of continuous channels
%   adchans - 1 x n array of continuous channel numbers
%
% Normally, there is one channel entry in the .plx for for each raw continuous channel,
% so the mapping is trivial adchans[i] = i-1 (because continuous channels start at 0).
% However, for certain .plx files saved in some ways from OFS (notably after
% loading data files from other vendors), the mapping can be more complex.
% E.g. there may be only 2 non-empty channels in a .plx file, but those channels
% correspond to raw channel numbers 7 and 34. So in this case NChans = 2,
% and adchans[1] = 7, adchans[2] = 34.
% The plx_ routines that return arrays always return arrays of size NChans. However,
% routines that take channels numbers as arguments always expect the raw
% channel number.  So in the above example, to get the data from
% the second channel, use
%   [adfreq, n, ts, fn, ad] = plx_ad(filename, adchans[2])

n = 0;
adchans = -1;

if nargin ~= 1
    error 'Expected 1 input argument';
end

[ filename, isPl2 ] = internalPL2ResolveFilenamePlx( filename );
if isPl2 == 1
    pl2 = PL2GetFileIndex(filename);
    n = numel(pl2.AnalogChannels);
    adchans = 0:(n-1);
    return;
end

[n, adchans] = mexPlex(27,filename);

end


% POP_RMBASE - remove channel baseline means from an epoched or
%                continuous EEG dataset. Calls RMBASE.
% Usage:
%   >> OUTEEG = pop_rmbase( EEG ); % pop up an interactive arg entry window
%   >> OUTEEG = pop_rmbase( EEG, timerange, pointrange, chanlist); % call RMBASE
%
% Graphic interface:
%    "Baseline latency range" - [edit box] Latency range for the baseline in ms.
%                               Collects the 'timerange' command line input.
%                               Empty or [] input -> Use whole epoch as baseline
%    "Baseline points vector" - [edit box] Collects the 'pointrange' command line
%                               option (below). (Overwritten by 'timerange' above).
%                               Empty or [] input -> Use whole epoch as baseline
% Inputs:
%   EEG        - Input dataset
%   timerange  - [min_ms max_ms] Baseline latency range in milliseconds.
%                                Empty or [] input -> Use whole epoch as baseline
%   pointrange - [min:max]       Baseline points vector (overwritten by timerange).
%                                Empty or [] input -> Use whole epoch as baseline
%   chanlist   - [cell]          List of channels. Default is all.
%
% Outputs:
%   OUTEEG     - Output dataset
%
% Note: If dataset is continuous, channel means are removed separately
%       for each continuous data region, respecting 'boundary' events
%       marking boundaries of excised or concatenated data portions.
%
% Author: Arnaud Delorme, CNL / Salk Institute, 2001
%
% See also: RMBASE, EEGLAB

% Copyright (C) 2001 Arnaud Delorme, Salk Institute, arno@salk.edu
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
% THE POSSIBILITY OF SUCH DAMAGE.

% 01-25-02 reformated help & license -ad

function [EEG, com] = pop_rmbase( EEG, timerange, pointrange, chanlist)

com ='';
if nargin < 1
    help pop_rmbase;
    return;
end
if isempty(EEG(1).data)
    disp('pop_rmbase(): cannot remove baseline of an empty dataset'); return;
end
if nargin < 1
	help pop_rmbase;
	return;
end
if nargin < 4 || isempty(chanlist)
    chanlist = 1:EEG(1).nbchan;
end
if nargin < 2, timerange = [];  end
if nargin < 3, pointrange = []; end
if nargin < 2
    % popup window parameters
    % -----------------------
    defaultbase = [num2str(EEG(1).times(1)) ' 0'];
    if EEG(1).times(1) >= 0
        defaultbase = '[ ]';
    end
    if EEG(1).trials > 1
        uilist = { { 'style' 'text' 'string' 'Baseline latency range ([min max] in ms) ([ ] = whole epoch):' } ...
            { 'style' 'edit' 'string'  defaultbase } ...
            { 'style' 'text' 'string' 'Or remove baseline points vector (ex:1:56):' } ...
            { 'style' 'edit' 'string' '' } ...
            { 'style' 'text' 'string' 'Note: press Cancel if you do not want to remove the baseline' } };
        uigeom = { [3 1] [3 1] [1] };
    else
        uilist = { { 'style' 'text' 'string' 'Removing the mean of each data channel (press cancel to skip)' } };
        uigeom = { [1] };
    end

    % add channel selection
    % --------------
    if ~isempty(EEG(1).chanlocs)
        tmpchanlocs = EEG(1).chanlocs;
    else
        tmpchanlocs = [];
        for index = 1:EEG(1).nbchan
            tmpchanlocs(index).labels = int2str(index);
            tmpchanlocs(index).type = '';
        end
    end
    cb_type = 'pop_chansel(get(gcbf, ''userdata''), ''field'', ''type'',   ''handle'', findobj(''parent'', gcbf, ''tag'', ''chantypes''));';
    cb_chan = 'pop_chansel(get(gcbf, ''userdata''), ''field'', ''labels'', ''handle'', findobj(''parent'', gcbf, ''tag'', ''channels''));';
    enableChan = fastif(length(EEG) > 1, 'off', 'on') ;
    uilist = { uilist{:} ...
        { 'style' 'text'       'string' 'Channel type(s)' 'enable' enableChan} ...
        { 'style' 'edit'       'string' '' 'tag' 'chantypes' 'enable' enableChan}  ...
        { 'style' 'pushbutton' 'string' '...'  'callback' cb_type  'enable' enableChan} ...
        { 'style' 'text'       'string' 'OR channel(s) (default all)'  'enable' enableChan} ...
        { 'style' 'edit'       'string' '' 'tag' 'channels'  'enable' enableChan}  ...
        { 'style' 'pushbutton' 'string' '...' 'callback' cb_chan  'enable' enableChan}
        };
    uigeom = { uigeom{:} [2 1.5 0.5] [2 1.5 0.5] };
    [result, usrdat, sres2, sres] = inputgui( 'uilist', uilist, 'geometry', uigeom, 'title', 'Baseline removal - pop_rmbase()', 'helpcom', 'pophelp(''pop_rmbase'');', 'userdata', tmpchanlocs);
    if isempty(result), return; end

    % decode parameters
    % -----------------
    if EEG(1).trials > 1
        if numel(result) < 2 || ((isempty(result{1}) || strcmp(result{1},'[]') ) ...
                && (isempty(result{2}) || strcmp(result{2},'[]')))
            timerange = [ EEG(1).times(1) EEG(1).times(end) ]; % whole epoch latency range
            pointrange = [];
            fprintf('pop_rmbase(): using whole epoch as baseline.\n');
        else
            timerange  = eval( [ '[' result{1} ']' ] );
            pointrange = eval( [ '[' result{2} ']' ] );
        end
    end
    if ~isempty(sres.chantypes)
        chanlist = eeg_decodechan(EEG.chanlocs, parsetxt(sres.chantype), 'type');
    elseif ~isempty(sres.channels)
        chanlist = eeg_decodechan(EEG(1).chanlocs, sres.channels);
    end

end

% process multiple datasets
% -------------------------
if length(EEG) > 1
    if nargin < 2
        [ EEG, com ] = eeg_eval( 'pop_rmbase', EEG, 'warning', 'on', 'params', { timerange pointrange } );
    else
        [ EEG, com ] = eeg_eval( 'pop_rmbase', EEG, 'params', { timerange pointrange } );
    end
    return;
end

flag_timerange = 1; % provide timerange as input (so use in history)
if ~isempty(timerange)
    if timerange(1) < EEG.times(1) || timerange(end) > EEG.times(end)
        error('pop_rmbase(): Bad time range');
    end
    pointrange = find( EEG.times >= timerange(1) & EEG.times <= timerange(2));
elseif ~isempty(pointrange)
    flag_timerange = 0;
    if pointrange(1) < 1, pointrange(1) = 1; end
    if pointrange(end) > EEG.pnts, pointrange(end) = EEG.pnts; end
else
    pointrange = [1:EEG.pnts];
end

%
% Respect excised data boundaries if continuous data
% ---------------------------------------------------
fprintf('pop_rmbase(): Removing baseline...\n');
if EEG.trials == 1 && ~isempty(EEG.event) ...
                     && isfield(EEG.event, 'type') ...
                        && ischar(EEG.event(1).type)
    tmpevent = EEG.event;
    boundaries = eeg_findboundaries(tmpevent);
	if ~isempty(boundaries) % this is crashing
        fprintf('Pop_rmbase(): finding continuous data discontinuities\n');
        boundaries = round([ tmpevent(boundaries).latency ] -0.5-pointrange(1)+1);
        boundaries(boundaries>=pointrange(end)-pointrange(1)) = [];
        boundaries(boundaries<1) = [];
        boundaries = [0 boundaries pointrange(end)-pointrange(1)+1];
        for index=1:length(boundaries)-1
            tmprange = [boundaries(index)+1:boundaries(index+1)];
            if length(tmprange) > 1
                EEG.data(chanlist,tmprange) = rmbase( EEG.data(:,tmprange), length(tmprange), ...
                                                   [1:length(tmprange)]);
            elseif length(tmprange) == 1
                EEG.data(chanlist,tmprange) = 0;
            end
        end
    else
        EEG.data(chanlist,:) = rmbase( EEG.data(chanlist,:), EEG.pnts, pointrange );
    end
else
    for indc = chanlist
        tmpmean  = mean(double(EEG.data(indc,pointrange,:)),2);
        EEG.data(indc,:,:) = EEG.data(indc,:,:) - repmat(tmpmean, [1 EEG.pnts 1]);
    end
%    EEG.data = rmbase( reshape(EEG.data, EEG.nbchan, EEG.pnts*EEG.trials), EEG.pnts, pointrange );
end

EEG.data = reshape( EEG.data, EEG.nbchan, EEG.pnts, EEG.trials);
EEG.icaact = [];

if isequal(chanlist, [1:EEG.nbchan]), chanlist = []; end
if flag_timerange, pointrange = []; else, timerange = []; end
if isempty(chanlist)
    com = sprintf('EEG = pop_rmbase( EEG, %s);',vararg2str({timerange pointrange}));
else
    com = sprintf('EEG = pop_rmbase( EEG, %s);',vararg2str({timerange pointrange chanlist}));
end
return;


function V=computeV(geo,angles,alphablocks,orig_index,varargin)
if nargin>=6
    [gpuids] = parse_inputs(varargin{1:length(varargin)});
elseif nargin==4
    gpuids = GpuIds();
else
    error('Wrong amount of inputs, 4 or 6 expected');
end

V=zeros(geo.nVoxel(1),geo.nVoxel(2) ,length(alphablocks),'single');
%geo.offDetector(1) = geo.offDetector(1) + (geo.DSD(1) / geo.DSO(1)) * geo.COR(1);
geo=checkGeo(geo,angles);

if ~isfield(geo,'mode')||~strcmp(geo.mode,'parallel')
    for ii=1:length(alphablocks)
        auxang=alphablocks{ii};
        auxindex=orig_index{ii};
        auxgeo = geo;
        % expand the detector to avoiding zeros in backprojection
        maxsize=max(auxgeo.sVoxel+geo.offOrigin(:,auxindex),[],2);
        auxgeo.sDetector=max(auxgeo.sDetector , [maxsize(1); maxsize(3)] *geo.DSD/geo.DSO);
        auxgeo.dDetector = auxgeo.sDetector ./ auxgeo.nDetector;
        % subset of projection angles
        auxgeo.DSD = geo.DSD(auxindex);
        auxgeo.DSO = geo.DSO(auxindex);
        auxgeo.offOrigin = geo.offOrigin(:,auxindex);
        auxgeo.offDetector = geo.offDetector(:,auxindex);
        auxgeo.rotDetector = geo.rotDetector(:,auxindex);
        auxgeo.COR = geo.COR(auxindex);
        %auxgeo=geo;
        V(:,:,ii) = mean(Atb(ones(geo.nDetector(2),geo.nDetector(1),size(auxang,2),'single'),auxgeo,auxang,'gpuids',gpuids),3)+0.000001;
    end
    V(V==0.0)=Inf;
else
    for ii=1:length(alphablocks)
        V(:,:,ii)=ones(geo.nVoxel(1:2).','single')*length(alphablocks{ii});
    end
end
end

function [gpuids]=parse_inputs(varargin)
    %fprintf('parse_inputs0(varargin (%d))\n', length(varargin));
    if isempty(varargin)
        gpuids = GpuIds();
    else
        % create input parser
        p=inputParser;
        % add optional parameters
        addParameter(p,'gpuids', GpuIds());
        %execute
        parse(p,varargin{:});
        %extract
        gpuids=p.Results.gpuids;
    end
end


function [Q,c] = compileQuadratic(c,p,onlysquares)
Q = spalloc(length(c),length(c),0);
%c = p.c;
for i = 1:size(p.bilinears,1)
    quadratic = (p.bilinears(i,2)==p.bilinears(i,3));
    if onlysquares == 3 && quadratic
        if c(p.bilinears(i,1))>0
            Q(p.bilinears(i,2),p.bilinears(i,3)) = c(p.bilinears(i,1));
            c(p.bilinears(i,1)) = 0;
        end
    elseif c(p.bilinears(i,1)) && (~onlysquares || quadratic) && ~(onlysquares == 3)
        %if ~quadratic || (quadratic && c(p.bilinears(i,1))>0)
            Q(p.bilinears(i,2),p.bilinears(i,3)) = c(p.bilinears(i,1))/2;
            Q(p.bilinears(i,3),p.bilinears(i,2)) = Q(p.bilinears(i,3),p.bilinears(i,2))+c(p.bilinears(i,1))/2;
            c(p.bilinears(i,1)) = 0;
       % end
    end
end


function imagenet_subset(dataset, subset)
% IMAGENET_SUBSET Modifies a ImageNet dataset to use a subset of classes

  rng(0);
  if isnumeric(subset)
    if isscalar(subset)
      % pick random subset with given size
      idx = randperm(1000, subset);
    else  % passed it in directly
      idx = subset;
    end
  else
    % named subsets
    switch subset
    case 'fruits'
      idx = [323, 320, 324, 229, 322, 326, 321, 318, 331, 319];
    case 'animals'
      idx = [1347, 1123, 1161, 1276, 1215];
    otherwise
      error('Unknown subset.');
    end
  end

  % index to synset mapping
  synsets = [];
  load([dataset.dataDir '/ILSVRC2014_devkit/data/meta_clsloc.mat'], 'synsets');

  if ischar(idx)
    % find classes by name
    subset_names = strtrim(strsplit(lower(idx), ','));
    synset_names = {synsets.words};

    valid = false(numel(synset_names), 1);
    for i = 1:numel(synset_names)
      % each synset name contains multiple synonims, separated by commas
      synonyms = strtrim(strsplit(lower(synset_names{i}), ','));
      valid(i) = any(ismember(subset_names, synonyms));
    end

    idx = find(valid);
    assert(numel(idx) == numel(subset_names), 'At least one class was not found.');
  end

  % synset IDs (e.g. 'n01542...', a string) of the selected classes
  synset_ids = {synsets(idx).WNID};

  % binary vector of which synsets/classes to keep
  valid_classes = ismember(dataset.classes.name, synset_ids);

  assert(nnz(valid_classes) == numel(synset_ids), 'At least one class was not found.');

  % remove all other classes, and their samples
  invalid_samples = ~ismember(dataset.labels, find(valid_classes));
  dataset.labels(invalid_samples) = [];
  dataset.sets(invalid_samples) = [];
  dataset.filenames(invalid_samples) = [];
  dataset.augmentImage(invalid_samples) = [];

  dataset.trainSet = find(dataset.sets == 1);
  dataset.valSet = find(dataset.sets == 2);

  % remap labels to 1:N
  map = cumsum(double(valid_classes));
  dataset.labels = map(dataset.labels);
end


function [y,outside,leads] = spm_eeg_wrap_momfit_vbecd(P,M,U)
% A cost function/wrapper to sit between non-linear optimisation spm_nlsi_gn.m
% and dipole fit routine spm_cfg_eeg_momentfit.m
% FORMAT [y,outside,leads] = spm_eeg_wrap_momfit_vbecd(P,M,U)
% sens and vol structures should be passed in M, where
%   sens = M.Setup.forward.sens;
%   vol  = M.Setup.forward.vol;
% P contains a list of the free parameters (assuming all position
%   parameters come first (in triplets) followed by all moment paameters
%   (also in triplets)
% U is unused
% At the momnent reduces the rank of the MEG leadfield 2 dimensions.
% leads are the lead fields of the dipoles fit
%__________________________________________________________________________
% Copyright (C) 2020 Wellcome Trust Centre for Neuroimaging

% Gareth Barnes
% $Id: spm_eeg_wrap_momfit_vbecd.m 7764 2020-01-02 15:34:03Z spm $

x = U.u; % input , unused

sens = M.Setup.forward.sens;
vol  = M.Setup.forward.vol;

siunits = M.Setup.forward.siunits;
if ~siunits
    warning('Data not in SI units, scaling (and therefore priors) maybe wrong');
end
chanunits = M.Setup.forward.chanunits;

mompars = P; % one moment parameter per source
Ndips   = length(mompars);
allpos  = M.pos; % source positions
allori  = M.ori; % source orientations

if (size(allpos,1)~=size(allori,1))|| (size(allpos,1)~=length(mompars))
    error('There must be equal number of moments, positions and orientations');
end

Ndips = length(mompars);

allmom = allori.*repmat(spm_vec(mompars),1,3);

if ft_senstype(sens, 'meg')
    RANK = 2; % restricting rank of MEG data, could change this in future
else
    RANK = 3;
end

y = 0;
outside = 0;
leads = zeros(Ndips,3,numel(sens.label));
for i=1:Ndips

    pos = allpos(i,:);

    mom = allmom(i,:); %% in nAm

    if siunits
        [tmp] = ft_compute_leadfield(1e-3*pos, sens, vol, 'reducerank',RANK, 'dipoleunit', 'nA*m', 'chanunit', chanunits);

    else
        [tmp] = ft_compute_leadfield(pos, sens, vol, 'reducerank',RANK);

    end

    gmn = tmp;
    leads(i,:,:) = gmn';

    y = y + gmn * mom';

end % for i


classdef VehicleArticulatedNonlinear < VehicleDynamicsLateral.VehicleArticulated
    % VehicleArticulatedNonlinear Nonlinear articulated vehicle model.
    %
    % It inherits properties from VehicleArticulated.

    methods
        % Constructor
        function self = VehicleArticulatedNonlinear()
            self.mF0    = 5200;    % Mass at front axle of the tractor without semi-trailer [kg]
            self.mR0    = 2400;    % Mass at rear axle of the tractor without semi-trailer [kg]
            self.mF     = 6000;    % Mass at front axle of the tractor with semi-trailer [kg]
            self.mR     = 10000;   % Mass at rear axle of the tractor with semi-trailer [kg]
            self.mM     = 17000;   % Mass at the axle of the semi-trailer [kg]
            self.IT     = 46000;   % Moment of inertia of the tractor [kg.m2]
            self.IS     = 450000;  % Moment of inertia of the semi-trailer [kg.m2]
            self.lT     = 3.5;     % Wheelbase of the tractor [m]
            self.lS     = 7.7;     % Distance from articulation (fifth wheel) to semi-trailer axle [m]
            self.c      = -0.3;    % Position of articulation (fifth wheel) to semi-trailer axle [m]
            self.nF     = 2;       % Number of wheels at front axle
            self.nR     = 4;       % Number of wheels at rear axle
            self.nM     = 8;       % Number of wheels at semi-trailer axle
            self.wT     = 2.6;     % Tractor width [m]
            self.wS     = 2.4;     % Semi-trailer width [m]
            self.muy    = 0.3;     % Friction
            self.deltaf = 0;       % Steering angle front [rad]
            self.deltar = 0;       % Steering angle rear [rad]
            self.deltam = 0;       % Steering angle semi-trailer [rad]
            self.Fxf    = 0;       % Longitudinal force at front axle [N]
            self.Fxr    = 0;       % Longitudinal force at rear axle [N]
            self.Fxm    = 0;       % Longitudinal force at semi-trailer axle [N]
        end

        function dx = Model(self,t, states,tspan)
            % dx = VehicleModel.Model(t,states,tspan)
            % dx = VehicleModel.MassMatrix(t,states,tspan)
            %
            % Sintax
            % |dx = _VehicleModel_.Model(t,states,tspan)|
            %
            % |dx = _VehicleModel_.MassMatrix(t,states,tspan)|
            %
            % Arguments
            % The following table describes the input arguments:
            %
            % t - Time
            % states - Model state variables: [XT YT PSI PHI VT ALPHAT dPSI dPHI]
            % tspan - Time span

            % Vehicle parameters
            mT  = self.mT;
            mS  = self.mS;
            % IT = self.IT;
            % IS = self.IS;
            a   = self.a;
            b   = self.b;
            c   = self.c;
            d   = self.d;
            e   = self.e;
            nF  = self.nF;
            nR  = self.nR;
            nM  = self.nM;

            g = 9.81;

            FzF = self.mF * g;
            FzR = self.mR * g;
            FzM = self.mM * g;
            muy = self.muy;

            % States
            X       = states(1,1);
            Y       = states(2,1);
            PSI     = states(3,1);
            PHI     = states(4,1);
            VT      = states(5,1);
            ALPHAT  = states(6,1);
            dPSI    = states(7,1);
            dPHI    = states(8,1);

            if isa(self.deltaf,'function_handle')
                % Closed loop steering
                deltaf = self.deltaf([X;Y;PSI;PHI;VT;ALPHAT;dPSI;dPHI],t);
            elseif length(self.deltaf)>1
                % Open loop variable steering
                deltaf = interp1(tspan,self.deltaf,t);
            else
                % Open loop single value steering
                deltaf = self.deltaf;
            end

            if isa(self.deltar,'function_handle')
                % Closed loop steering
                deltar = self.deltar([X;Y;PSI;PHI;VT;ALPHAT;dPSI;dPHI],t);
            elseif length(self.deltar)>1
                % Open loop variable steering
                deltar = interp1(tspan,self.deltar,t);
            else
                % Open loop single value steering
                deltar = self.deltar;
            end

            if isa(self.deltam,'function_handle')
                % Closed loop steering
                deltam = self.deltam([X;Y;PSI;PHI;VT;ALPHAT;dPSI;dPHI],t);
            elseif length(self.deltam)>1
                % Open loop variable steering
                deltam = interp1(tspan,self.deltam,t);
            else
                % Open loop single value steering
                deltam = self.deltam;
            end

            % Slip angles
            ALPHAF = - deltaf + atan2(a*dPSI + VT*sin(ALPHAT), VT*cos(ALPHAT));
            ALPHAR = - deltar + atan2(VT*sin(ALPHAT) - b*dPSI, VT*cos(ALPHAT));
%           ALPHAM = atan2(((d + e)*(dPHI - dPSI) + VT * sin(ALPHAT + PHI) - b * dPSI * cos(PHI) - c * dPSI * cos(PHI)),(VT * cos(ALPHAT + PHI) + b * dPSI * sin(PHI) + c * dPSI * sin(PHI)));
            ALPHAM = - deltam + atan2((d + e)*(dPHI - dPSI) + VT*sin(ALPHAT + PHI) - dPSI*cos(PHI)*(b + c), VT*cos(ALPHAT + PHI) + dPSI*sin(PHI)*(b + c));
            % Longitudinal forces
            % Front
            if isa(self.Fxf,'function_handle')
                FxF = self.Fxf([X;Y;PSI;PHI;VT;ALPHAT;dPSI;dPHI],t);
            elseif length(self.Fxf)>1
                FxF = interp1(tspan,self.Fxf,t);
            else
                FxF = self.Fxf;
            end
            % Rear
            if isa(self.Fxr,'function_handle')
                FxR = self.Fxr([X;Y;PSI;PHI;VT;ALPHAT;dPSI;dPHI],t);
            elseif length(self.Fxr)>1
                FxR = interp1(tspan,self.Fxr,t);
            else
                FxR = self.Fxr;
            end
            % Semitrailer
            if isa(self.Fxm,'function_handle')
                FxM = self.Fxm([X;Y;PSI;PHI;VT;ALPHAT;dPSI;dPHI],t);
            elseif length(self.Fxm)>1
                FxM = interp1(tspan,self.Fxm,t);
            else
                FxM = self.Fxm;
            end

            % Lateral forces
            FyF = nF * self.tire.Characteristic(ALPHAF, FzF/nF, muy);
            FyR = nR * self.tire.Characteristic(ALPHAR, FzR/nR, muy);
            FyM = nM * self.tire.Characteristic(ALPHAM, FzM/nM, muy);

            t2 = cos(ALPHAT);
            t3 = cos(PSI);
            t4 = sin(PHI);
            t5 = sin(PSI);
            t6 = cos(deltam);
            t7 = sin(deltam);
            t8 = PSI+deltaf;
            t9 = PSI+deltar;
            t10 = dPHI.^2;
            t11 = dPSI.^2;
            t12 = ALPHAT+PHI;
            t13 = ALPHAT+PSI;
            t21 = -PHI;
            t22 = -PSI;
            t23 = -deltam;
            t14 = cos(t12);
            t15 = cos(t13);
            t16 = sin(t13);
            t17 = cos(t8);
            t18 = cos(t9);
            t19 = sin(t8);
            t20 = sin(t9);
            t24 = FyM.*d.*t6;
            t25 = FyM.*e.*t6;
            t26 = FxM.*d.*t7;
            t27 = FxM.*e.*t7;
            t28 = PHI+t22;
            t29 = PHI+t23;
            t31 = PSI+deltam+t21;
            t30 = cos(t28);
            t32 = sin(t28);
            t33 = cos(t29);
            t34 = sin(t29);
            t35 = cos(t31);
            t36 = sin(t31);
            t37 = VT.*d.*dPSI.*mS.*t14;
            f = [VT.*t15;VT.*t16;dPSI;dPHI;FxF.*t17+FxM.*t35+FxR.*t18-FyF.*t19-FyM.*t36-FyR.*t20-b.*mS.*t3.*t11-c.*mS.*t3.*t11-d.*mS.*t10.*t30-d.*mS.*t11.*t30+VT.*dPSI.*mS.*t16+VT.*dPSI.*mT.*t16+d.*dPHI.*dPSI.*mS.*t30.*2.0;FxF.*t19+FxM.*t36+FxR.*t20+FyF.*t17+FyM.*t35+FyR.*t18-b.*mS.*t5.*t11-c.*mS.*t5.*t11+d.*mS.*t10.*t32+d.*mS.*t11.*t32-VT.*dPSI.*mS.*t15-VT.*dPSI.*mT.*t15-d.*dPHI.*dPSI.*mS.*t32.*2.0;-t24-t25-t26-t27+t37+FxM.*b.*t34-FyM.*b.*t33+FxM.*c.*t34-FyM.*c.*t33+FyF.*a.*cos(deltaf)-FyR.*b.*cos(deltar)+FxF.*a.*sin(deltaf)-FxR.*b.*sin(deltar)+VT.*b.*dPSI.*mS.*t2+VT.*c.*dPSI.*mS.*t2-b.*d.*mS.*t4.*t10-c.*d.*mS.*t4.*t10+b.*d.*dPHI.*dPSI.*mS.*t4.*2.0+c.*d.*dPHI.*dPSI.*mS.*t4.*2.0;t24+t25+t26+t27-t37-b.*d.*mS.*t4.*t11-c.*d.*mS.*t4.*t11];

            dx = f;
        end

        function [value,isterminal,direction] = velocity(~,~,states)
            % If the velocity is less than 0.1m/s the integrator stops.
            % The MassMatrix is singular when the velocity is 0 m/s.
            value = states(5,1) - 0.1;
            isterminal = 1;
            direction = -1;
        end

        function M = MassMatrix(self,~,states)
            % Vehicle Parameters
            mT = self.mT;
            mS = self.mS;
            IT = self.IT;
            IS = self.IS;
            % a = self.a;
            b = self.b;
            c = self.c;
            d = self.d;
            % e = self.e;
            % deltaf = self.deltaf;
            % nF = self.nF;
            % nR = self.nR;
            % nM = self.nM;

            % g = 9.81;

            % FzF = self.mF * g;
            % FzR = self.mR * g;
            % FzM = self.mM * g;
            % muy = self.muy;

            % States
            PSI = states(3,1);
            PHI = states(4,1);
            VT = states(5,1);
            ALPHAT = states(6,1);
            % dPSI = states(7,1);
            % dPHI = states(8,1);

            t2 = cos(ALPHAT);
            t3 = cos(PHI);
            t4 = sin(ALPHAT);
            t5 = b+c;
            t6 = mS+mT;
            t7 = d.^2;
            t8 = ALPHAT+PHI;
            t9 = ALPHAT+PSI;
            t14 = -IS;
            t15 = -PSI;
            t10 = cos(t8);
            t11 = cos(t9);
            t12 = sin(t8);
            t13 = sin(t9);
            t16 = mS.*t7;
            t17 = b.*d.*mS.*t3;
            t18 = c.*d.*mS.*t3;
            t19 = PHI+t15;
            t20 = -t16;
            t21 = cos(t19);
            t22 = sin(t19);
            t23 = -t17;
            t24 = -t18;
            t25 = t14+t20+t23+t24;
            M = reshape([1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t6.*t11,t6.*t13,-mS.*(b.*t4+c.*t4+d.*t12),d.*mS.*t12,0.0,0.0,0.0,0.0,-VT.*t6.*t13,VT.*t6.*t11,-VT.*mS.*(b.*t2+c.*t2+d.*t10),VT.*d.*mS.*t10,0.0,0.0,0.0,0.0,mS.*(d.*t22.*2.0-t5.*sin(PSI).*2.0).*(-1.0./2.0),mS.*(d.*t21.*2.0+t5.*cos(PSI).*2.0).*(-1.0./2.0),IS+IT+t16+t17.*2.0+t18.*2.0+b.^2.*mS+c.^2.*mS+b.*c.*mS.*2.0,t25,0.0,0.0,0.0,0.0,d.*mS.*t22,d.*mS.*t21,t25,IS+t16],[8,8]);
        end
    end

end


classdef (Abstract) mme_buslink_pf_ac < mp.mme_buslink

%   MATPOWER
%   Copyright (c) 2021-2022, 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.

%     properties
%         name = 'buslink';
%     end

    methods
        function obj = add_vars(obj, mm, nm, dm, mpopt)
            nme = obj.network_model_element(nm);

            mm.init_indexed_name('var', 'Plink', {3});
            mm.init_indexed_name('var', 'Qlink', {3});
            mmx_i1 = mm.var.N + 1;
            for p = 1:3
                mm.add_var('Plink', {p}, nme.nk, 0, -Inf, Inf);
            end
            mmx_iN = mm.var.N;
            mm.aux_data.var_map{end+1} = ...
                {'zr', nm.zr.idx.i1.Plink(1), nm.zr.idx.iN.Plink(end), [], mmx_i1, mmx_iN, []};

            mmx_i1 = mm.var.N + 1;
            for p = 1:3
                mm.add_var('Qlink', {p}, nme.nk, 0, -Inf, Inf);
            end
            mmx_iN = mm.var.N;
            mm.aux_data.var_map{end+1} = ...
                {'zi', nm.zi.idx.i1.Qlink(1), nm.zi.idx.iN.Qlink(end), [], mmx_i1, mmx_iN, []};
        end

        function obj = add_constraints(obj, mm, nm, dm, mpopt)
            nme = obj.network_model_element(nm);

            %% add Qlink constraints for PV node on 3-phase side

            %% indices of buslinks connected to PV nodes via port 2 (3&4)
            ad = mm.aux_data;
            [~, k, ~] = find(nme.C(ad.pv, nme.nk+1:2*nme.nk));
            n = length(k);
            if n
                I = sparse(1:n, k, 1, n, nme.nk);
                zz = sparse(n, nme.nk);
                A = [I -I zz; I zz -I];
                b = zeros(2*n, 1);
                vs = struct('name', {'Qlink', 'Qlink', 'Qlink'}, ...
                            'idx', {{1}, {2}, {3}} );
                mm.add_lin_constraint('buslink_qlink', A, b, b, vs);
            end
        end

        function [A_va_pq, A_va_pv, b_va, A_vm, b_vm] = voltage_constraints(obj, nme, ad)
            %% form constraint matrices for matching
            %%  voltage angles for pv and pq nodes
            %%  voltage magnitudes for pq nodes
            %% columns of A_va_pv correspond to ...
            %%  'Va_pv', 'Va3_pv', 'Va3_pv', 'Va3_pv',
            %% columns of A_va_pq correspond to ...
            %%  'Va_pq', 'Va3_pq', 'Va3_pq', 'Va3_pq'
            %% columns of A_vm correspond to ...
            %%  'Vm_pq', 'Vm3_pq', 'Vm3_pq', 'Vm3_pq'
            nk = nme.nk;

            %% basic constraint matrix for voltage equality (all nodes)
            [A, b_va, b_vm, Istack] = nme.voltage_constraints();

            %% sub-matrices by node type
            A_ref = A(:, ad.ref);
            A_pv = A(:, ad.pv);
            A_pq = A(:, ad.pq);

            %% voltage angle constraints (all buslinks)
            A_va_pq = A_pq;
            A_va_pv = A_pv;

            %% voltage magnitude constraints (all buslinks)
            A_vm = A_pq;

            %% indices of buslinks connected to REF nodes (fixed va)
            [~, k_va1, ~] = find(nme.C(ad.ref, 1:nk));      %% via port 1
            [~, k_va2, ~] = find(nme.C(ad.ref, nk+1:2*nk)); %% via port 2 (3&4)

            %% adjust RHS of va constraints involving fixed voltage angles
            if ~isempty(k_va1) || ~isempty(k_va2)
                [va, vm] = nme.aux_data_va_vm(ad);
                va1 = nme.C(:, 1:nk)' * va;         %% port 1 voltage angles
                va2 = nme.C(:, nk+1:2*nk)' * va;    %% port 2 voltage angles
                va_adj = zeros(nk, 1);
                va_adj(k_va1) = -va1(k_va1);
                va_adj(k_va2) =  va2(k_va2);
                b_va = b_va + Istack * va_adj;

                %% delete redundate (and currently incorrect) constraints
                %% corresponding to k_va2 and ports 3 and 4
                if ~isempty(k_va2)
                    d_va = [nk+k_va2; 2*nk+k_va2];
                    A_va_pq(d_va, :) = [];
                    A_va_pv(d_va, :) = [];
                    b_va(d_va) = [];
                end
            end

            %% indices of buslinks connected to REF/PV nodes (fixed vm)
            rpv = [ad.ref;ad.pv];   %% indices of REF/PV nodes
            [~, k_vm1, ~] = find(nme.C(rpv, 1:nk));         %% via port 1
            [~, k_vm2, ~] = find(nme.C(rpv, nk+1:2*nk));    %% via port 2 (3&4)

            %% adjust RHS of vm constraints involving fixed voltage magnitudes
            if ~isempty(k_vm1) || ~isempty(k_vm2)
                [va, vm] = nme.aux_data_va_vm(ad);
                vm1 = nme.C(:, 1:nk)' * vm;         %% port 1 voltage magnitudes
                vm2 = nme.C(:, nk+1:2*nk)' * vm;    %% port 2 voltage magnitudes
                vm_adj = zeros(nk, 1);
                vm_adj(k_vm1) = -vm1(k_vm1);
                vm_adj(k_vm2) =  vm2(k_vm2);
                b_vm = b_vm + Istack * vm_adj;

                %% delete redundate (and currently incorrect) constraints
                %% corresponding to k_vm2 and ports 3 and 4
                if ~isempty(k_vm2)
                    d_vm = [nk+k_vm2; 2*nk+k_vm2];
                    A_vm(d_vm, :) = [];
                    b_vm(d_vm) = [];
                end
            end
        end

        function obj = data_model_update(obj, mm, nm, dm, mpopt)
        end
    end     %% methods
end         %% classdef


function output = callipopt(model)

options = [];
try
    options.ipopt = optiRemoveDefaults(model.options.ipopt,ipoptset());
catch
    options.ipopt = model.options.ipopt;
end
options.ipopt.print_level = 2+model.options.verbose;

model = yalmip2nonlinearsolver(model);

if ~model.derivative_available
    disp('Derivate-free call to ipopt not yet implemented')
    error('Derivate-free call to ipopt not yet implemented')
end
if model.options.savedebug
    save ipoptdebug model
end
showprogress('Calling IPOPT',model.options.showprogress);

% Figure out which variables are artificially introduced to normalize
% arguments in callback operators to simplify chain rules etc. We can do
% our function evaluations and gradient computations in our lifted world,
% but only expose the model in the original variables to the nonlinear
% solver.
% model = compressLifted(model);

Fupp = [ repmat(0,length(model.bnonlinineq)+length(model.K.q)*(model.K.q(1)>0),1);
    repmat(0,length(model.bnonlineq),1);
    repmat(0,length(model.b),1);
    repmat(0,length(model.beq),1)];

Flow = [ repmat(-inf,length(model.bnonlinineq)+length(model.K.q)*(model.K.q(1)>0),1);
    repmat(0,length(model.bnonlineq),1);
    repmat(-inf,length(model.b),1);
    repmat(0,length(model.beq),1)];

if isempty(Flow)
    Flow = [];
    Fupp = [];
end

% Since ipopt react strangely on lb>ub, we should bail if that is detected
% (ipopt creates an exception)
if ~isempty(model.lb)
    if any(model.lb>model.ub)
        problem = 1;
        solverinput = [];
        solveroutput = [];
        output = createoutput(model.c*0,[],[],problem,'IPOPT',solverinput,solveroutput,0);
        return
    end
end

% These are needed to avoid recomputation due to ipopts double call to get
% f and df, and g and dg
global latest_x_f
global latest_x_g
global latest_df
global latest_f
global latest_G
global latest_g
global latest_xevaled
global latest_x_xevaled
latest_G = [];
latest_g = [];
latest_x_f = [];
latest_x_g = [];
latest_xevaled = [];
latest_x_xevaled = [];

funcs.objective = @(x)ipopt_callback_f(x,model);
funcs.gradient = @(x)ipopt_callback_df(x,model);
if ~isempty(Fupp)
    funcs.constraints = @(x)ipopt_callback_g(x,model);
    funcs.jacobian  = @(x)ipopt_callback_dg(x,model);
end

options.lb = model.lb(:)';
options.ub = model.ub(:)';
if ~isempty(Fupp)
    options.cl = Flow;
    options.cu = Fupp;
end

if ~isempty(Fupp)
    Z = jacobiansparsityfromnonlinear(model);
    funcs.jacobianstructure = @() Z;
end

if ~model.options.usex0
    model.x0 = (options.lb+options.ub)/2;
    model.x0(isinf(options.ub)) = options.lb(isinf(options.ub))+1;
    model.x0(isinf(options.lb)) = options.ub(isinf(options.lb))-1;
    model.x0(isinf(model.x0)) = 0;
    if any(model.variabletype == 4)
        problematic = find(any(model.monomtable(:,model.linearindicies) < 0 ,1));
        if ~isempty(problematic)
            problematic = problematic(find(model.x0(problematic)==0));
            Oneisfeas = problematic(find(model.ub(problematic) > 1));
            model.x0(Oneisfeas) = 1;
        end
    end
    model.x0(find(model.lb==model.ub)) = model.lb(find(model.lb==model.ub));
end

% If quadratic objective and no nonlinear constraints, we can supply an
% Hessian of the Lagrangian
usedinObjective = find(model.c | any(model.Q,2));
if ~any(model.variabletype(usedinObjective)) & any(model.Q)
    if  length(model.bnonlinineq)==0 & length(model.bnonlineq)==0
        H = model.Q(:,model.linearindicies);
        H = H(model.linearindicies,:);
        funcs.hessian = @(x,s,l) tril(2*H);
        funcs.hessianstructure = @()tril(sparse(double(H | H)));
        options.ipopt.hessian_approximation = 'exact';
    end
end

solvertime = tic;
[xout,info] = ipopt(model.x0,funcs,options);
solvertime = toc(solvertime);

% Duals currently not supported
lambda = [];

if ~isempty(xout) && ~isempty(model.lift);
    x = zeros(length(model.linearindicies),1);
    x(model.lift.linearIndex) = xout(:);
    x(model.lift.liftedIndex) = model.lift.T*xout(:) + model.lift.d;
    x = RecoverNonlinearSolverSolution(model,x);
else
    x = RecoverNonlinearSolverSolution(model,xout);
end

switch info.status
    case {0,1}
        problem = 0;
    case {2}
        problem = 1;
    case {-1}
        problem = 3;
    case {3,4,-2,-3}
        problem = 4;
    case {-11,-12,-13}
        problem = 7;
    case {-10,-100,-101,-102,-199}
        problem = 11;
    otherwise
        problem = -1;
end

% Internal format for duals
D_struc = [];

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

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

% Standard interface
output = createoutput(x,D_struc,[],problem,'IPOPT',solverinput,solveroutput,solvertime);


function f = plotkf(kf, plotDir);
%-------------------------------------------------------------------------------
% f = plotkf(kf, isPrintToFile);
% generate plots for Kalman filter estimates
% input:
% 1) kf is the data loaded into memory
% => use kf = readdata(solFileName, 1+9+9) to load binary file format
% => use kf = load(solFileName) to load text file format
% 2) plotDir => to control where to print a tiff file for each plot or not,
% use empty [] to turn off print
% output:
% f => figure handles
% by Dr. Yudan Yi
% modified date on: 04/08/2013
%-------------------------------------------------------------------------------
if (nargin<1) return; end;
[nn,mm] = size(kf);
if (nn<1) return; end;
isPrintToFile = false;
if nargin<2 plotDir = []; end;
if ~isempty(plotDir)
    isPrintToFile = true;
    n = length(plotDir);
    if (plotDir(n)~='\')
        plotDir = [plotDir '\'];
    end
    if ~isdir(plotDir)
        mkdir(plotDir);
    end
end
%-------------------------------------------------------------------------------
max_x = max(abs(kf(:,3)));
max_y = max(abs(kf(:,2)));
isNED = false;
if (max_y>pi||max_x>(2.0*pi))
    isNED = true;
end
f(1) = figure;
plot(kf(:,3),kf(:,2),'marker','.')
grid
axis equal
if (isNED)
    xlabel('East [m]')
    ylabel('North[m]')
else
    xlabel('Lontitude [radian]')
    ylabel('Latitude [radian]')
end
title('Ground Track');
if isPrintToFile
    print(f(1), '-dtiff', [plotDir 'ground_track']);
end
%close all
f(2) = figure;
plot(kf(:,1)-kf(1,1),kf(:,4),'marker','.')
grid
xlabel('Time [s]')
ylabel('m')
title('Height Profile');
if isPrintToFile
    print(f(2), '-dtiff', [plotDir 'ht']);
end
%close all
f(3) = figure;
plot(kf(:,1)-kf(1,1),kf(:,5:7),'marker','.')
grid
xlabel('Time [s]')
ylabel('m/s')
legend('Vx','Vy','Vz')
title('Velocity');
if isPrintToFile
    print(f(3), '-dtiff', [plotDir 'vel']);
end
%close all
% % note: roll is around pi, make to around 0
% kf(:,8) = kf(:,8);
% loc = find(kf(:,8)>pi);
% kf(loc,8) = kf(loc,8);
f(4) = figure;
plot(kf(:,1)-kf(1,1),kf(:,8:10),'marker','.')
grid
xlabel('Time [s]')
ylabel('degree')
legend('Roll','Pitch','Yaw/Heading')
title('Orientation');
if isPrintToFile
    print(f(4), '-dtiff', [plotDir 'att']);
end
%close all
if mm==19
f(5) = figure;
plot(kf(:,1)-kf(1,1),kf(:,11:13),'marker','.')
grid
xlabel('Time [s]')
ylabel('m')
legend('X','Y','Z')
title('Position RMS in ECEF XYZ');
if isPrintToFile
    print(f(5), '-dtiff', [plotDir 'rms_pos']);
end
%close all
f(6) = figure;
plot(kf(:,1)-kf(1,1),kf(:,14:16),'marker','.')
grid
xlabel('Time [s]')
ylabel('m/s')
legend('Vn','Ve','Vd')
title('Velocity RMS in NED');
if isPrintToFile
    print(f(6), '-dtiff', [plotDir 'rms_vel']);
end
%close all
f(7) = figure;
plot(kf(:,1)-kf(1,1),kf(:,17:19)*180/pi,'marker','.')
grid
xlabel('Time [s]')
ylabel('radian')
legend('Roll','Pitch','Yaw/Heading')
title('Orientation RMS in RPY');
if isPrintToFile
    print(f(7), '-dtiff', [plotDir 'rms_att']);
end
end
%close all


function stk = bin_stack(w,bin,ovr)

   %BIN_STACK: Stack input waveforms with N waveforms per stack. The number N
   %    is specified by input 'bin'. The input 'ovr' specifies the amount of
   %    overlap between bins.
   %
   %USAGE: stk = bin_stack(w,bin,ovr)
   %    If input w contains 100 waveforms then:
   %    stk = bin_stack(w,20,0) % stk has 5 stacks of 20 waveforms
   %    stk = bin_stack(w,5,0)  % stk has 20 stacks of 5 waveforms
   %    stk = bin_stack(w,20,10)  % stk has 9 stacks of 20 waveforms
   %
   %INPUTS:  w   - Input waveform
   %         bin - Bin size of stacked waveforms
   %         ovr - Number of overlapping events between bins
   %
   %OUTPUTS: stk - Array of stacked waveforms

   % Author: Dane Ketner, Alaska Volcano Observatory
   % $Date$
   % $Revision$

   nw = numel(w);
   inc = bin-ovr;
   n=1;
   while 1
      if (n-1)*inc+bin > nw
         return
      else
         stk(n) = stack(w((n-1)*inc+1:(n-1)*inc+bin));
         n=n+1;
      end
   end
end


function gx = g_ip(x,P,u,in)

gx = P(1) + real((u+P(3)).^(1-P(2)));

if isnan(gx)
    gx
end


function Z = plus(X,Y)
% PLUS (overloaded)

dim = [];
if isa(X,'ndsdpvar')
    if isa(Y,'double') && numel(Y)==1
        % Quick return for scalar  + ndsdpvar
        Z = X;Z.basis(:,1) = Z.basis(:,1) + Y;
        Z.conicinfo = [0 0];
        Z.extra.opname='';
        return
    else
        dim = X.dim;
        X = sdpvar(X);
    end
elseif isa(X,'double')
    X = X(:);
end

if isa(Y,'ndsdpvar')
    if isa(X,'double') && numel(X)==1
        % Quick return for scalar  + ndsdpvar
        Z = Y;Z.basis(:,1) = Z.basis(:,1) + X;
        Z.conicinfo = [0 0];
        Z.extra.opname='';
        return
    else
        if isempty(dim)
            dim = Y.dim;
        else
            if isequal(dim,Y.dim)
            else
                error('Dimension mismatch in nD addition')
            end
        end
        Y = sdpvar(Y);
    end
elseif isa(Y,'double')
    Y = Y(:);
end

Z = X + Y;
Z = reshape(Z,dim);


function SpeedTuning=SpeedTuningCalcultation(SpeedTrain,SpikeTrain,FrameRate,SpeedRange,MinSpanTime,extend)
% FrameRate=FullNeuronBehaviorDataSet.FrameRate./double(FullNeuronBehaviorDataSet.ImagingPlane);
% Frameselect=find(~isnan(FullNeuronBehaviorDataSet.FullNeuronBehaviorMatrix{1,2}(:,12+4*i)));
% SpeedTrain=FullNeuronBehaviorDataSet.FullNeuronBehaviorMatrix{1,2}(Frameselect,5);
% SpikeTrain=FullNeuronBehaviorDataSet.FullNeuronBehaviorMatrix{1,2}(Frameselect,4*i+12);
% SpeedRange=[0 4 8 12];
% MinSpanTime=10;

SpeedTuning=struct;
SpeedTuning.Rate=zeros(size(SpeedRange));
SpeedTuning.Occ=zeros(size(SpeedRange));
SpeedTuning.SpikeCount=zeros(size(SpeedRange));
SpeedTuning.SpeedSelectivity=0;
SpeedTuning.SpeedScore=0;
SpeedTuning.PeakRate=0;
SpeedTuning.PeakSpeed=1;
SpeedTuning.SpeedRange=SpeedRange;
SpeedRangeExpend=[SpeedRange,extend];
for j=1:1:length(SpeedTrain)
    for k=1:1:size(SpeedRangeExpend,2)-1
        if SpeedTrain(j)>=SpeedRangeExpend(k)&&SpeedTrain(j)<SpeedRangeExpend(k+1)
            SpeedTuning.Occ(k)=SpeedTuning.Occ(k)+1;
            SpeedTuning.SpikeCount(k)=SpeedTuning.SpikeCount(k)+SpikeTrain(j);
        else
        end
    end
end
SpeedTuning.Occ=SpeedTuning.Occ./FrameRate;% frame to second;
SpeedTuning.Occ(SpeedTuning.Occ==0)=0.0000001;
SpeedTuning.Rate=SpeedTuning.SpikeCount./SpeedTuning.Occ;
% SpeedTuning.Rate=smooth(SpeedTuning.Rate,speedsmooth);
SpeedTuning.Rate(SpeedTuning.Occ<MinSpanTime)=nan;

[SpeedTuning.PeakRate,SpeedTuning.PeakSpeed]=max(SpeedTuning.Rate(~isnan(SpeedTuning.Rate)));
% SpeedTuning.PeakSpeed=SpeedRange(SpeedTuning.PeakSpeed);
MinRate=min(SpeedTuning.Rate(~isnan(SpeedTuning.Rate)));
SpeedTuning.SpeedSelectivity=(SpeedTuning.PeakRate-MinRate)./(SpeedTuning.PeakRate+MinRate);

% Con1=find((SpeedRange<=ConditionSeperation).*(~isnan(SpeedTuning.Rate))==1);
% Con2=find((SpeedRange>ConditionSeperation).*(~isnan(SpeedTuning.Rate))==1);
% Condtion1=mean(SpeedTuning.Rate(Con1));
% Condtion2=mean(SpeedTuning.Rate(Con2));
% SpeedTuning.SpeedScore=(Condtion2-Condtion1)./(Condtion1+Condtion2);

end

% end


function ff = ifft(data)
% ff = ifft(data) does the inverse fourier transform of the data (power of 2 only)

% This m-file implementation of the fft is meant solely for use with adiff
% objects; it's far too inefficient for general use.

ff = fft(conj(data))/length(data);


% MP_SP_TO_VTK: Export multipatch results to VTK format for plotting.
%
%  mp_sp_to_vtk (u, space, geometry, gnum, npts, filename, fieldnames, [option], [lambda_lame, mu_lame])
%
% INPUT:
%
%     u:           vector of dof weights
%     space:       object representing the space of discrete functions (see sp_bspline)
%     geometry:    geometry structure (see geo_load)
%     gnum:        array that relates the local numbering on each patch with the global one
%     npts:        number of points along each parametric direction where to evaluate
%     filename:    name of the output file.
%     fieldnames:  how to name the saved variables in the vtk file
%     options:     cell array with the fields to plot
%                   accepted options are 'value' (default), 'gradient',
%                   and for vectors also 'curl', 'divergence', 'stress'
%     lambda_lame: function handle to the first Lame coefficient (only needed to compute 'stress')
%     mu_lame:     function handle for the second Lame coefficient (only needed to compute 'stress')
%
% OUTPUT:
%
%    none
%
% Copyright (C) 2010 Carlo de Falco, Rafael Vazquez
% Copyright (C) 2011, 2012, 2015 Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.

function mp_sp_to_vtk (u, space, geometry, gnum, npts, filename, fieldname, varargin)

  if (isa (space, 'sp_multipatch'))
    warning ('For a space of the class SP_MULTIPATCH, we use the method SP_TO_VTK in the class, instead of the (deprecated) MP_SP_TO_VTK')
    sp_to_vtk (u, space, geometry, npts, filename, fieldname, varargin{:});
    return
  end

  str1 = cat (2,'<?xml version="1.0"?> \n', ...
'<VTKFile type="Collection" version="0.1"> \n', ...
'<Collection> \n');

  str2 = cat (2, '<DataSet part="%d" file="%s.vts"/> \n');

  str3 = cat (2, ...
'</Collection>\n', ...
'</VTKFile> \n');

  if (length (filename) < 4 || ~strcmp (filename(end-3:end), '.pvd'))
    pvd_filename = cat (2, filename, '.pvd');
  else
    pvd_filename = filename;
    filename = filename (1:end-4);
  end

  fid = fopen (pvd_filename, 'w');
  if (fid < 0)
    error ('mp_sp_to_vtk: could not open file %s', pvd_filename);
  end

  fprintf (fid, str1);
  ind = union (find (filename == '/', 1, 'last'), find (filename == '\', 1, 'last')) + 1;
  if (isempty (ind)); ind = 1; end
  for iptc = 1:numel(geometry)
    filename_patch_without_path = cat (2, filename(ind:end), '_', num2str (iptc));
    filename_patch = cat (2, filename, '_', num2str (iptc));
    fprintf (fid, str2, iptc, filename_patch_without_path);
    sp_to_vtk (u(abs(gnum{iptc})).*sign(gnum{iptc})', space{iptc}, geometry(iptc), npts, ...
                           filename_patch, fieldname, varargin{:})
  end
  fprintf (fid, str3);

  fclose (fid);

end


function Y = sec(X)
    % Symbolic secant.

    % Convert inputs to SymExpression
    % X = SymExpression(X);

    % construct the operation string
    sstr = ['Sec[' X.s ']'];

    % create a new object with the evaluated string
    Y = SymExpression(sstr);
end


function Draw_MPC_point_stabilization_v1 (t,xx,xx1,u_cl,xs,N,rob_diam)

set(0,'DefaultAxesFontName', 'Times New Roman')
set(0,'DefaultAxesFontSize', 12)

line_width = 1.5;
fontsize_labels = 14;

%--------------------------------------------------------------------------
%-----------------------Simulate robots -----------------------------------
%--------------------------------------------------------------------------
x_r_1 = [];
y_r_1 = [];

r = rob_diam/2;  % obstacle radius
ang=0:0.005:2*pi;
xp=r*cos(ang);
yp=r*sin(ang);

figure(500)
% Animate the robot motion
%figure;%('Position',[200 200 1280 720]);
set(gcf,'PaperPositionMode','auto')
set(gcf, 'Color', 'w');
set(gcf,'Units','normalized','OuterPosition',[0 0 0.55 1]);

tic
for k = 1:size(xx,2)
    h_t = 0.14; w_t=0.09; % triangle parameters

    x1 = xs(1); y1 = xs(2); th1 = xs(3);
    x1_tri = [ x1+h_t*cos(th1), x1+(w_t/2)*cos((pi/2)-th1), x1-(w_t/2)*cos((pi/2)-th1)];%,x1+(h_t/3)*cos(th1)];
    y1_tri = [ y1+h_t*sin(th1), y1-(w_t/2)*sin((pi/2)-th1), y1+(w_t/2)*sin((pi/2)-th1)];%,y1+(h_t/3)*sin(th1)];
    fill(x1_tri, y1_tri, 'g'); % plot reference state
    hold on;
    x1 = xx(1,k,1); y1 = xx(2,k,1); th1 = xx(3,k,1);
    x_r_1 = [x_r_1 x1];
    y_r_1 = [y_r_1 y1];
    x1_tri = [ x1+h_t*cos(th1), x1+(w_t/2)*cos((pi/2)-th1), x1-(w_t/2)*cos((pi/2)-th1)];%,x1+(h_t/3)*cos(th1)];
    y1_tri = [ y1+h_t*sin(th1), y1-(w_t/2)*sin((pi/2)-th1), y1+(w_t/2)*sin((pi/2)-th1)];%,y1+(h_t/3)*sin(th1)];

    plot(x_r_1,y_r_1,'-r','linewidth',line_width);hold on % plot exhibited trajectory
    if k < size(xx,2) % plot prediction
        plot(xx1(1:N,1,k),xx1(1:N,2,k),'r--*')
    end

    fill(x1_tri, y1_tri, 'r'); % plot robot position
    plot(x1+xp,y1+yp,'--r'); % plot robot circle

    hold off
    %figure(500)
    ylabel('$y$-position (m)','interpreter','latex','FontSize',fontsize_labels)
    xlabel('$x$-position (m)','interpreter','latex','FontSize',fontsize_labels)
    axis([-0.2 1.8 -0.2 1.8])
    pause(0.0)
    box on;
    grid on
    %aviobj = addframe(aviobj,gcf);
    drawnow
    % for video generation
    F(k) = getframe(gcf); % to get the current frame
end
toc
close(gcf)
%viobj = close(aviobj)
%video = VideoWriter('exp.avi','Uncompressed AVI');

% video = VideoWriter('exp.avi','Motion JPEG AVI');
% video.FrameRate = 5;  % (frames per second) this number depends on the sampling time and the number of frames you have
% open(video)
% writeVideo(video,F)
% close (video)

figure
subplot(211)
stairs(t,u_cl(:,1),'k','linewidth',1.5); axis([0 t(end) -0.35 0.75])
ylabel('v (rad/s)')
grid on
subplot(212)
stairs(t,u_cl(:,2),'r','linewidth',1.5); axis([0 t(end) -0.85 0.85])
xlabel('time (seconds)')
ylabel('\omega (rad/s)')
grid on


function model = rmSliceSet(model,tmp,slice)
% rmSliceSet - put slice info into model struct
% we convert back to double precision here.
%
% model = rmSliceSet(model,tmp,slice);
%
% 2008/01 SOD: extracted from rmGridFit.

% loop over models
for n=1:numel(model),
    % variables may have slightly different names
    ftmp   = {'x0','y0','s','x02','y02','s2','s_major','s_minor','s_theta','rss','rss2','rsspos','rssneg','rawrss','rawrss2', 'exponent'};

    % now get values from model and put in new slice values
    for fn = 1:numel(ftmp),
        % check whether data exists and has data in tmp structure
        if isfield(tmp{n},ftmp{fn}) && ~isempty(tmp{n}.(ftmp{fn}))
            % get data from model structure
            val = rmGet(model{n},ftmp{fn});
            % if no data in model structure make it (zeros)
            if isempty(val)
                val = zeros(size(rmGet(model{n},'x0')));
            end
            % now put data in model structure format
            switch length(size(val))
                case 3 % presumably means INPLANE view, where slice is the third dimension
                    data = reshape(double(tmp{n}.(ftmp{fn})), size(val,1), size(val,2));
                    val(:,:,slice) = data;
                otherwise
                    if size(val,2) == size(tmp{n}.(ftmp{fn}),2)
                        val(slice,:)  = double(tmp{n}.(ftmp{fn}));
                    elseif slice == 1 && size(val,2) ~= size(tmp{n}.(ftmp{fn}),2)
                        val = double(tmp{n}.(ftmp{fn}));
                    end
            end
            % save data in model
            model{n}      = rmSet(model{n},ftmp{fn},val);
        end
    end

    % other params
    if isfield(tmp{n},'df')
        model{n}       = rmSet(model{n},'dfglm',double(tmp{n}.df));
    end
    if isfield(tmp{n},'desc')
        model{n}       = rmSet(model{n},'desc',tmp{n}.desc);
    end

    % distribute beta values
    val = rmGet(model{n},'b');
    switch length(size(val))
        % switch on the number of dimensions, since inplane data has diff
        % dimesnsionality than other views
        case 4 % 4 dimensions means Inplane model:
            %  3 dimensions of coords, and one dimension of beta values.
            %  Hence the dims are x, y, slice, betas.
            nbetas = size(val,4);
            %tmp{n}.b = zeros(nbetas,nvoxelsPerSlice,'single');
            for fn = 1:nbetas,
                data = reshape(double(tmp{n}.b(fn,:)), size(val,1), size(val,2));
                val(:,:,slice,fn) = data;
            end;

        otherwise
            val = val(:,:,1:size(tmp{n}.b,1));
            for fn = 1:size(val,3),
                val(slice,:,fn) = double(tmp{n}.b(fn,:));
            end;
    end
    model{n} = rmSet(model{n},'b',val);
end;
return;
%-----------------------------------