classdef chebtech2 < chebtech
%CHEBTECH2   Approximate smooth functions on [-1,1] with Chebyshev interpolants.
%
%   Class for approximating smooth functions on the interval [-1,1] using
%   function values at 2nd-kind Chebyshev points and coefficients of the
%   corresponding 1st-kind Chebyshev series expansion.
%
% Constructor inputs:
%   CHEBTECH2(OP) constructs a CHEBTECH2 object from the function handle OP. OP
%   should be vectorised (i.e., accept a vector input) and ouput a vector of
%   the same length. CHEBTECH2 objects allow for array-valued construction
%   (i.e., of an array-valued function), in which case OP should accept a column
%   vector of length N and return a matrix of size NxM.
%
%   CHEBTECH2(OP, DATA) constructs a CHEBTECH2 using the additional data
%   supplied in the DATA structure.  Fields currently recognized are:
%     DATA.VSCALE    (Default:  0)
%     DATA.HSCALE    (Default:  1)
%         The constructor builds a CHEBTECH2 with 'happiness' (see
%         HAPPINESSCHECK.m) relative to the maximum of the given vertical scale
%         DATA.VSCALE and the (column-wise) infinity norm of the sampled
%         function values of OP, and the fixed horizontal scale DATA.HSCALE. If
%         not given (or given as empty), the VSCALE defaults to 0 initially,
%         and HSCALE defaults to 1.
%   If any fields in DATA are empty or not supplied, or if DATA itself is empty
%   or not supplied, appropriate default values are set.
%
%   CHEBTECH2(OP, DATA, PREF) overrides the default behavior with that given by
%   the preference structure PREF. See CHEBTECH.TECHPREF for details.
%
%   CHEBTECH2(VALUES, ...) returns a CHEBTECH2 object which interpolates the
%   values in the columns of VALUES at 2nd-kind Chebyshev points and
%   CHEBTECH2({VALUES, COEFFS}, ... ) uses the Chebyshev coefficients passed in
%   COEFFS rather than computing them from VALUES. If VALUES, is empty then the
%   CHEBTECH2 is constructed directly from the COEFFS. If COEFFS are passed,
%   the resulting CHEBTECH2 is always deemed 'happy'.
%
% Examples: % Basic construction: f = chebtech2(@(x) sin(x))
%
%   % Construction with preferences:
%   p.sampleTest = 0; % See CHEBTECH.TECHPREF for details
%   f = chebtech2(@(x) sin(x), [], [], p)
%
%   % Array-valued construction:
%   f = chebtech2(@(x) [sin(x), cos(x), exp(x)])
%
% See also CHEBTECH, CHEBTECH.TECHPREF, CHEBPTS, HAPPINESSCHECK, REFINE.

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% CHEBTECH2 Class Description:
%
% The CHEBTECH2 class represents smooth functions on the interval [-1,1] using
% function values at 2nd-kind Chebyshev points and coefficients of the
% corresponding 1st-kind Chebyshev series expansion.
%
% The constructor is supplied with a handle that evaluates a given function on
% an increasingly fine Chebyshev 2nd-kind grid (see REFINE.m) until the
% representation is deemed 'happy' (see HAPPINESSCHECK.m). The resulting object
% can be used to evaluate and operate on the input function.
%
% More information can be found in the CHEBTECH class definition.
%
% Class diagram: [<<CHEBTECH>>] <-- [CHEBTECH2]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% CLASS CONSTRUCTOR:
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods ( Access = public, Static = false )

        function obj = chebtech2(op, data, pref)
            % Parse inputs.
            if ( (nargin == 0) || isempty(op) )
                % Return an empty CHEBTECH2 on null input:
                return
            end

            if ( (nargin < 2) || isempty(data) )
                    data = struct();
            end

            if ( (nargin < 3) || isempty(pref) )
                pref = chebtech.techPref();
            else
                pref = chebtech.techPref(pref);
            end

            data = chebtech.parseDataInputs(data, pref);

            % Force nonadaptive construction if PREF.FIXEDLENGTH is numeric and
            % we're not using contour integrals.
            if ( ~(isnumeric(op) || iscell(op)) && ...
                    ~isnan(pref.fixedLength) && ~pref.useTurbo )
                % Evaluate op on the Chebyshev grid of given size:
                op = feval(op, chebtech2.chebpts(pref.fixedLength));
            end

            % Actual construction takes place here:
            [obj, values] = populate(obj, op, data, pref);

            if ( isnumeric(op) || iscell(op) )
                % Set length of obj to PREF.FIXEDLENGTH (if it is non-trivial).
                if ( ~isnan(pref.fixedLength ) )
                    obj = prolong(obj, pref.fixedLength);
                end

                % No need to error check when constructing from discrete data.
                return
            elseif ( obj.ishappy )
                % Use contour integrals if requested.
                if ( pref.useTurbo )
                    obj = constructorTurbo(obj, op, pref);
                end

                % No need to error check if we are happy:
                return
            end

            % Check for NaNs (if not happy):
            if ( pref.extrapolate )
                % Check for NaNs in interior only (because extrapolate was on):
                if ( any(any(isnan(obj.coeffs(2:end-1,:)))) )
                    error('CHEBFUN:CHEBTECH2:chebtech2:nanEval', ...
                        'Function returned NaN when evaluated.')
                end
                % We make sure not to return NaNs at +1 and -1.
                valuesTemp = extrapolate(obj, values);
                valuesTemp([1 end], :) = valuesTemp([1,end],:);
                obj.coeffs = obj.vals2coeffs(valuesTemp);
            elseif ( any(isnan(obj.coeffs(:))) )
                % Here we throw an error if NaNs were encountered anywhere.
                error('CHEBFUN:CHEBTECH2:chebtech2:nanEval2', ...
                    'Function returned NaN when evaluated.')
            end
        end
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% STATIC METHODS:
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods ( Access = public, Static = true )

        % Aliasing:
        coeffs = alias(coeffs, m)

        % Angles of Chebyshev points. (i.e., acos(chebpts(n))
        t = angles(n)

        % Evaluate a Chebyshev interpolant using the barycentric formula:
        out = bary(x, values)

        % Compute Chebyshev barycentric weights:
        w = barywts(n)

        % Compute Chebyshev points (x) and optionally quadrature (w)
        % and barycentric (v) weights:
        [x, w, v, t] = chebpts(n);

        % Tensor product grid of Chebyshev points in 1D, 2D or 3D:
        out = tensorGrid(N, dom)

        % Convert coefficients to values:
        values = coeffs2vals(coeffs);

        % Make a CHEBTECH2 (constructor shortcut):
        f = make(varargin);

        % Compute Chebyshev quadrature weights:
        w = quadwts(n)

        % Refinement function for CHEBTECH2 construction (evaluates OP on grid):
        [values, points, giveUp] = refine(op, values, pref)

        % Return the value-based discretization class which uses CHEBTECH2:
        function disc = returnValsDisc()
            disc = @chebcolloc2;
        end

        % Convert values to coefficients:
        coeffs = vals2coeffs(values)

    end

end


% CORRIMAGE - compute correlation image between an event and amplitude
%               and phase in the time-frequency domain.
%
% Usage:
%     corrimage(data, sortvar, times);
%     [times, freqs, alpha, sigout, limits ] = corrimage(data, sortvar, ...
%                                    times, 'key1', val1, 'key2', val2, ...)
%
% Inputs:
%   data    - [float array] data of size (times,trials)
%   sortvar - [float array] sorting variable of size (trials)
%   times   - [float array] time indices for every data point. Size of
%             (times). Note: same input as the times vector for erpimage.
%
% Optional input:
%   'mode'     - ['phase'|'amp'] compute correlation of event values with
%                phase ('phase') or amplitude ('amp') of signal at the
%                given time-frequency points. Default is 'amp'.
%   'freqs'    - [min nfreqs max] build a frequency vector of size nfreqs
%                with frequency spaced in a log scale. Then compute
%                correlation at these frequency value.
%                Default is 50 points in a log scale between 2.5Hz to 50Hz.
%   'times'    - [float vector] compute correlation at these time value (ms).
%                (uses closest times points in 'times' input and return
%                them in the times output).
%                Default is 100 steps between min time+5% of time range and
%                max time-5%. Enter only 2 values [N X] to generate N
%                time points and trim by X %. If N is negative, uses it as
%                a subsampling factor [-3 5] trims times by 5% and subsample
%                by 3 (by subsampling one obtains a regularly spaced times).
%   'trim'     - [low high] low and high percentile of sorted sortvar values
%                to retain. i.e. [5 95] remove the 5 lowest and highest
%                percentile of sortvar values (and associated data) before
%                computing statistics. Default is [0 100].
%   'align'    - [float] same as 'align' parameter of ERPIMAGE. This
%                parameter is used to constrain the 'times' parameter so
%                correlation with data trials containing 0-values (as a
%                result of data alignment) are avoided: computing these
%                correlations would produce spurious significant results.
%                Default is no alignment.
%   'method'   - ['erpimage'|timefreq'] use either the ERPIMAGE function
%                of the TIMEFREQ function to compute spectral decomposition.
%                Default is 'timefreq' for speed reasons (note that both
%                methods should return the same results).
%   'erpout'   - [min max] regress out ERP using the selected time-window [min
%                max] in ms (the ERP is subtracted from the whole time period
%                but only regressed out in the selected time window).
%   'triallimit' - [integer array] specify trial boundaries for subjects. For
%                instance [1 200 400] indicates 2 subjects, trials 1 to 199
%                for subject 1 and trials 200 to 399 for subject 2. This is
%                currently only used for regressing erp out.
%
% Processing options:
%   'erpimopt' - [cell array] erpimage additional options (number of cycle ...).
%   'tfopt'    - [cell array] timefreq additional options (number of cycle ...).
%
% Plotting options:
%   'plotvals' - [cell array of output values] enter output values here
%                { times freqs alpha sigout} to replot them.
%   'nofig'    - ['on'|'off'] do not create figure.
%   'cbar'     - ['on'|'off'] plot color bar. Default: 'on'.
%   'smooth'   - ['on'|'off'] smooth significance array. Default: 'on'.
%   'plot'     - ['no'|'alpha'|'sigout'|'sigoutm'|'sigoutm2'] plot -10*log(alpha)
%                values ('alpha'); output signal (slope or ITC) ('sigout'),
%                output signal masked by significance ('sigoutm') or the last
%                2 option combined ('sigoutm2'). 'no' prevent the function
%                from plotting. In addition, see pmask. Default is 'sigoutmasked'.
%   'pmask'    - [real] maximum p value to show in plot. Default is 0.00001
%                (0.001 taking into account multiple comparisons (100)). Enter
%                0.9XXX to get the higher tail or a negative value (e.e., -0.001
%                to get both tails).
%   'vert'     - [real array] time vector for vertivcal lines.
%   'limits'   - [min max] plotting limits.
%
% Outputs:
%   times    - [float vector] vector of times (ms)
%   freqs    - [float vector] vector of frequencies (Hz)
%   alpha    - [float array] array (freqs,times) of p-values.
%   sigout   - [float array] array (freqs,times) of signal out (coherence
%              for phase and slope for amplitude).
%
% Important note: the 'timefreq' method may truncate the time window to
%                 compute the spectral decomposition at the lowest freq.
%
% Author: Arnaud Delorme & Scott Makeig, SCCN UCSD,
%         and CNL Salk Institute, 18 April 2003
%
% See also: ERPIMAGE

%123456789012345678901234567890123456789012345678901234567890123456789012

% Copyright (C) 2002 Arnaud Delorme
%
% 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.

function [time, freq, alpha, sigout, limits, tf, sortvar] = corrimage(data, sortvar, timevect, varargin)

if nargin < 3
    help corrimage;
    return;
end

if isempty(timevect), timevect = [1 2]; end

% check inputs
% ------------
g = finputcheck(varargin, { 'freqs'    'real'   [0 Inf]    [2.5 50 50];
                            'times'    'real'   []         [100 5]; % see function at the end
                            'mode'     'string' { 'phase','amp' }  'amp';
                            'vert'     'real'   []         [];
                            'align'    { 'real','cell' }   []         [];
                            'plotvals' 'cell'   []         {};
                            'pmask'    'real'   []         0.00001;
                            'triallimit' 'integer'   []       [];
                            'trim'     'real'   [0 100]    [0 100];
                            'limits'   'real'   []         [];
                            'method'   'string' { 'erpimage','timefreq' }        'timefreq';
                            'plot'     'string' { 'no','alpha','sigout','sigoutm','sigoutp','sigoutm2' }  'sigoutm';
                            'nofig'    'string' { 'on','off' } 'off';
                            'cbar'     'string' { 'on','off' } 'on';
                            'smooth'   'string' { 'on','off' } 'off';
                            'erpout'   'real'   []             [];
                            'tfopt'    'cell'   []             {};
                            'erpimopt' 'cell'   []             {} });
if ischar(g), error(g); end

fprintf('Generating %d frequencies in log scale (ignore message on linear scale)\n', g.freqs(2));
g.freqs = logscale(g.freqs(1), g.freqs(3), g.freqs(2));

frames = length(timevect);
if size(data,1) == 1
    data = reshape(data, frames, size(data,2)*size(data,3)/frames);
end

% trim sortvar values
% -------------------
[sortvar sortorder] = sort(sortvar);
data = data(:, sortorder);
len = length(sortvar);
lowindex  = round(len*g.trim(1)/100)+1;
highindex = round(len*g.trim(2)/100);
sortvar = sortvar(lowindex:highindex);
data    = data(:, lowindex:highindex);
if lowindex ~=1 || highindex ~= length(sortorder)
    fprintf('Actual percentiles %1.2f-%1.2f (indices 1-%d -> %d-%d): event vals min %3.2f; max %3.2f\n', ...
             100*(lowindex-1)/len, 100*highindex/len, len, lowindex, highindex, min(sortvar), max(sortvar));
end

% assign subject number for each trial
% ------------------------------------
if ~isempty(g.triallimit)
    alltrials = zeros(1,len);
    for index = 1:length(g.triallimit)-1
        alltrials([g.triallimit(index):g.triallimit(index+1)-1]) = index;
    end
    alltrials = alltrials(sortorder);
    alltrials = alltrials(lowindex:highindex);
end

%figure; hist(sortvar)

% constraining time values depending on data alignment
% ----------------------------------------------------
srate = 1000*(length(timevect)-1)/(timevect(end)-timevect(1));
if ~isempty(g.align)

    if iscell(g.align)
        fprintf('Realigned sortvar plotted at %g ms.\n',g.align{1});
        shifts = round((g.align{1}-g.align{2})*srate/1000); % shifts can be positive or negative
        g.align = g.align{1};
    else
        if isinf(g.align)
            g.align = median(sortvar);
        end
        fprintf('Realigned sortvar plotted at %g ms.\n',g.align);

        % compute shifts
        % --------------
        shifts = round((g.align-sortvar)*srate/1000); % shifts can be positive or negative
    end

    %figure; hist(shifts)
    minshift = min(shifts); % negative
    maxshift = max(shifts); % positive
    if minshift > 0, error('minshift has to be negative'); end
    if maxshift < 0, error('maxshift has to be positive'); end

    % realign data for all trials
    % ---------------------------
    aligndata=zeros(size(data))+NaN; % begin with matrix of zeros()
    for t=1:size(data,2), %%%%%%%%% foreach trial %%%%%%%%%
        shft = shifts(t);
        if shft>0, % shift right
            aligndata(shft+1:frames,t)=data(1:frames-shft,t);
        elseif shft < 0 % shift left
            aligndata(1:frames+shft,t)=data(1-shft:frames,t);
        else % shft == 0
            aligndata(:,t) = data(:,t);
        end
    end % end trial
    data = aligndata(maxshift+1:frames+minshift,:);
    if any(any(isnan(data))), error('NaNs remaining after data alignment'); end
    timevect = timevect(maxshift+1:frames+minshift);

    % take the time vector subset
    % ---------------------------
    if isempty(timevect), error('Shift too big, empty time vector');
    else fprintf('Time vector truncated for data alignment between %1.0f and %1.0f\n', ...
                 min(timevect), max(timevect));
    end;
end

% regress out the ERP from the data (4 times to have residual ERP close to 0)
% ---------------------------------------------------------------------------
if ~isempty(g.erpout)
    %data = erpregout(data);
    %data = erpregout(data);
    %data = erpregout(data);
    disp('Regressing out ERP');
    erpbeg = mean(data,2);
    if ~isempty(g.triallimit)
        for index = 1:length(g.triallimit)-1
            trials = find(alltrials == index);
            %[data(:,trials) erp factors]= erpregout(data(:,trials), [timevect(1) timevect(end)], [300 400]);
            erpstart = mean(data(:,trials),2);
            data(:,trials) = erpregout(data(:,trials));
            data(:,trials) = erpregout(data(:,trials));
            data(:,trials) = erpregout(data(:,trials));
            data(:,trials) = erpregout(data(:,trials));
            erpend = mean(data(:,trials),2);
            fprintf([ '***********************************************\n' ...
                      'Ratio in ERP after regression (compare to before) is %3.2f\n' ...
                      '***********************************************\n' ], mean(erpend./erpstart));
        end
    end
    erpend = mean(data,2);
    fprintf([ '***********************************************\n' ...
              'Ratio in grand ERP after regression (compare to before) is %3.2f\n' ...
              '***********************************************\n' ], mean(erpend./erpbeg));

    if 0
    %trials = find(alltrials == 1);
    data2 = data;
    dasf

    [data(:,trials) erp factors]= erpregout(data(:,trials), [timevect(1) timevect(end)], [300 400]);

    figure; subplot(1,2,1); erpimage(data, sortvar, timevect, '', 300, 500, 'erp');

    figure;
    subplot(1,2,1); erpimage(data2, sortvar, timevect, '', 300, 500, 'erp');
    subplot(1,2,2); erpimage(data , sortvar, timevect, '', 300, 500, 'erp');

    figure;
    subplot(1,2,1); erpimage(data2(:,trials), sortvar(trials), timevect, '', 0, 0, 'erp');
    subplot(1,2,2); erpimage(data (:,trials), sortvar(trials), timevect, '', 0, 0, 'erp');

    figure;
    for index = 1:length(g.triallimit)-1
        subplot(3,5,index);
        trials = find(alltrials == index);
        erpimage(data(:,trials), sortvar(trials), timevect, '', 50, 100, 'erp');
    end

    disp('Regressing out ERP');
    if ~isempty(g.triallimit)
        for index = 1:length(g.triallimit)-1
            trials = find(alltrials == index);
            [data(:,trials) erp factors]= erpregout(data(:,trials), [timevect(1) timevect(end)], [300 400]);
            data(:,trials) = erpregout(data(:,trials));
            data(:,trials) = erpregout(data(:,trials));
            data(:,trials) = erpregout(data(:,trials));
            data(:,trials) = erpregout(data(:,trials));
            data(:,trials) = erpregout(data(:,trials));
            data(:,trials) = erpregout(data(:,trials));
            data(:,trials) = erpregout(data(:,trials));
            %data(:,trials) = erpregout(data(:,trials), [timevect(1) timevect(end)], g.erpout);
            %data(:,trials) = erpregout(data(:,trials), [timevect(1) timevect(end)], g.erpout);
            %data(:,trials) = erpregout(data(:,trials), [timevect(1) timevect(end)], g.erpout);
            %data(:,trials) = erpregout(data(:,trials), [timevect(1) timevect(end)], g.erpout);
            %data(:,trials) = erpregout(data(:,trials), [timevect(1) timevect(end)], g.erpout);
            %data(:,trials) = erpregout(data(:,trials), [timevect(1) timevect(end)], g.erpout);
            %data(:,trials) = erpregout(data(:,trials), [timevect(1) timevect(end)], g.erpout);
            %data(:,trials) = erpregout(data(:,trials), [timevect(1) timevect(end)], g.erpout);
        end
    else
        %data = erpregout(data, [timevect(1) timevect(end)], g.erpout);
    end
    end
end

% generate time points
% --------------------
g.times = gettimes(timevect, g.times);
data = reshape(data, 1, size(data,2)*size(data,1));

% time frequency decomposition
% ----------------------------
if strcmpi(g.method, 'timefreq') && isempty(g.plotvals)
    data = reshape(data, length(data)/length(sortvar), length(sortvar));
    [tf, g.freqs, g.times] = timefreq(data, srate, 'freqs', g.freqs, 'timesout', g.times, ...
                                      'tlimits', [min(timevect) max(timevect)], 'wavelet', 3);
    outvar   = sortvar;
end

% compute correlation
% -------------------
if strcmpi(g.mode, 'phase') && isempty(g.plotvals)
    for freq = 1:length(g.freqs)
        fprintf('Computing correlation with phase %3.2f Hz ----------------------\n', g.freqs(freq));
        for time = 1:length(g.times)
            if strcmpi(g.method, 'erpimage')
                [outdata,outvar,outtrials,limits,axhndls,erp, ...
                 amps,cohers,cohsig,ampsig,outamps,phsangls,phsamp,sortidx] =erpimage(data,sortvar,timevect, ...
                        '', 0,0,g.erpimopt{:}, 'phasesort', [g.times(time) 0 g.freqs(freq)], 'noshow', 'yes');
            else
                phsangls = angle( squeeze(tf(freq, time, :))' );
            end

            % computing ITCs
            [ ITC(freq, time) alpha(freq, time) ] = ...
                    bootcircle(outvar/mean(outvar), exp(j*phsangls), 'naccu', 250);

            % accumulate 200 values, fitted with a normal distribution
            % --------------------------------------------------------
            %cmplx = outvar .* exp(j*phsangls)/mean(outvar);
            %ITC(freq, time) = mean(cmplx);
            %alpha(freq,time) = bootstat(outvar/mean(outvar), exp(j*phsangls), 'res = mean(arg1 .* arg2)', ...
            %                         'naccu', 100, 'vals', abs(ITC(freq, time)), 'distfit', 'norm');
        end
    end
    sigout = ITC;
elseif isempty(g.plotvals)
    for freq = 1:length(g.freqs)
        fprintf('Computing correlation with amplitude %3.2f Hz ----------------------\n', g.freqs(freq));
        for time = 1:length(g.times)
            if strcmpi(g.method, 'erpimage')
                [outdata,outvar,outtrials,limits,axhndls,erp, ...
                 amps,cohers,cohsig,ampsig,outamps,phsangls,phsamp,sortidx] =erpimage(data,sortvar,timevect, ...
                             '', 0,0,g.erpimopt{:}, 'ampsort', [g.times(time) 0 g.freqs(freq)], 'noshow', 'yes');
            else
                phsamp = abs(squeeze(tf(freq, time, :)))';
            end

            % computing ITCs
            [ypred alpha(freq, time) Rsq slope(freq, time)] = myregress(outvar, 20*log10(phsamp));
        end
    end;
    sigout = slope;
else
    g.times = g.plotvals{1};
    g.freqs = g.plotvals{2};
    alpha   = g.plotvals{3};
    sigout  = g.plotvals{4};
end

% plot correlation
% ----------------
if ~strcmp('plot', 'no')
    if ~isreal(sigout),
        if strcmpi(g.plot, 'sigoutp')
            sigoutplot = angle(sigout);
        else
            sigoutplot = abs(sigout);
        end
    else sigoutplot = sigout;
    end
    sigouttmp = sigoutplot;
    if strcmpi(g.smooth, 'on')
        tmpfilt = gauss2d(3,3,.3,.3);
        tmpfilt = tmpfilt/sum(sum(tmpfilt));
        alpha = conv2(alpha, tmpfilt, 'same');
    end

    % mask signal out
    if g.pmask > 0.5
        indices = find( alpha(:) < g.pmask);
        sigouttmp = sigoutplot;
        sigouttmp(indices) = 0;
    elseif g.pmask < 0 % both sides, i.e. 0.01
        sigouttmp = sigoutplot;
        indices = intersect_bc(find( alpha(:) > -g.pmask), find( alpha(:) < 1+g.pmask));
        sigouttmp(indices) = 0;
    else
        sigouttmp = sigoutplot;
        indices = find( alpha(:) > g.pmask);
        sigouttmp(indices) = 0;
    end

    if strcmpi(g.nofig, 'off'), figure; end
    switch g.plot
     case 'alpha',   limits = plotfig(g.times, g.freqs, -log10(alpha), g);
     case 'sigout',  limits = plotfigsim(g.times, g.freqs, sigoutplot, g);
     case { 'sigoutm' 'sigoutp' }, limits = plotfigsim(g.times, g.freqs, sigouttmp, g);
     case 'sigoutm2', limits = subplot(1,2,1); plotfigsim(g.times, g.freqs, sigoutplot, g);
                      limits = subplot(1,2,2); plotfigsim(g.times, g.freqs, sigouttmp, g);
    end
end

time = g.times;
freq = g.freqs;

return;

% formula for the normal distribution
% -----------------------------------
function y = norm(mu, sigma, x);

y = 1/sqrt(2) * exp( -(x-mu).^2/(sigma*sigma*2) ) / (sqrt(pi)*sigma);

% get time points
% ---------------
function val = gettimes(timevect, timevar);
    if length(timevar) == 2

        if timevar(1) > 0
            % generate linearly space vector
            % ------------------------------
            npoints = timevar(1);
            trim    = timevar(2);
            if length(timevect)-2*round(trim/100*length(timevect)) < npoints
                npoints = length(timevect)-round(trim/100*length(timevect));
            end
            fprintf('Generating %d times points trimmed by %1.1f percent\n', npoints, trim);
            timer = max(timevect) - min(timevect);
            maxt = max(timevect)-timer*trim/100;
            mint = min(timevect)+timer*trim/100;
            val = linspace(mint,maxt, npoints);
        else
            % subsample data
            % --------------
            nsub    = -timevar(1);
            trim    = timevar(2);
            len     = length(timevect);
            trimtimevect = timevect(round(trim/100*len)+1:len-round(trim/100*len));
            fprintf('Subsampling by %d and trimming data by %1.1f percent (%d points)\n', nsub, trim, round(trim/100*len));
            val = trimtimevect(1:nsub:end);
        end
    else
        val = timevar;
    end;

    % find closet points in data
    oldval = val;
    for index = 1:length(val)
        [dum ind] = min(abs(timevect-val(index)));
        val(index) = timevect(ind);
    end
    if length(val) < length(unique(val))
        disp('Warning: duplicate times, reduce the number of output times');
    end
    if all(oldval == val)
        disp('Debug msg: Time value unchanged by finding closest in data');
    end;

% get log scale (for frequency)
% -------------
function val = logscale(a,b,n);
    %val = [5 7 9]; return;
    val = linspace(log(a), log(b), n);
    val = exp(val);

% plot figure
% -----------
function limits = plotfig(times, freqs, vals, g)
    icadefs;
    imagesc(times, [1:size(vals,1)], vals);

    colormap(DEFAULT_COLORMAP);
    ticks = linspace(1, size(vals,1), length(freqs));
    ticks = ticks(1:4:end);
    set(gca, 'ytick', ticks);
    set(gca, 'yticklabel', num2str(freqs(1:4:end)', 3))

    xlabel('Time (ms)'); ylabel('Freq (Hz)');
    if ~isempty(g.limits), caxis(g.limits); end
    limits = caxis;
    if ~isempty(g.vert)
        for vert = g.vert(:)'
            hold on; plot([vert vert], [0.001 500], 'k', 'linewidth', 2);
        end
    end
    if strcmpi(g.cbar,'on')
        cbar;
    end

% plot figure with symmetrical phase
% ---------------------------------
function limits = plotfigsim(times, freqs, vals, g)
    icadefs;
    imagesc(times, [1:size(vals,1)], vals);

    colormap(DEFAULT_COLORMAP);
    ticks = linspace(1, size(vals,1), length(freqs));
    ticks = ticks(1:4:end);
    set(gca, 'ytick', ticks);
    set(gca, 'yticklabel', num2str(freqs(1:4:end)', 3))

    if ~isempty(g.limits)
        caxis(g.limits);
        limits = g.limits;
    else
        clim = max(abs(caxis));
        limits = [-clim clim];
        caxis(limits);
    end
    xlabel('Time (ms)'); ylabel('Freq (Hz)');
    if ~isempty(g.vert)
        for vert = g.vert(:)'
            hold on; plot([vert vert], [0.01 500], 'k', 'linewidth', 2);
        end
    end
    if strcmpi(g.cbar,'on')
        cbar;
    end
    return;

% -----------------------------------------
% plot time-freq point (when clicking on it
% -----------------------------------------
function plotpoint(data, sortvar, timevect, freq, timepnts);

figure;
subplot(2,2,1);
erpimage(act_all(:,:),sortvar_all,timepnts, ...
   '', 300,10,'erp', 'erpstd', 'caxis',[-1.0 1.0], 'srate',256,'align',352, 'yerplabel', '', erpimopt{:}, ...
                                                   'phasesort', [500 0 5]); % aligned to median rt

% plot in polar coordinates phase versus RT
% -----------------------------------------
phaseang2 = [phsangls phsangls-2*pi]; phaseang2 = movav(phaseang2,[], 100);
outvar2   = [outvar   outvar];   outvar2   = movav(outvar2,[], 100);
phaseang2 = phaseang2(length(phsangls)/2-50:length(phsangls)+length(phsangls)/2-50);
outvar2   = outvar2  (length(phsangls)/2-50:length(phsangls)+length(phsangls)/2-50);
subplot(2,2,3);
polar(phsangls, outvar, '.');
hold on; polar(phaseang2, outvar2, 'r');

% computing ITC
% -------------
cmplx = outvar .* exp(j*phsangls);
ITCval = mean(cmplx);
angle(ITCval)/pi*180;
abs(ITCval)/mean(outvar);
text(-1300,-1400,sprintf('ITC value: amplitude %3.4f, angle %3.1f\n', abs(ITCval)/mean(outvar), angle(ITCval)/pi*180));

% accumulate 200 values
% ---------------------
alpha = 0.05;
if exist('accarray') ~= 1, accarray = NaN; end
[sigval accarray] = bootstat(outvar/mean(outvar), phsangls, 'res = mean(arg1 .* exp(arg2*complex(0,1)))', ...
                      'accarray', accarray, 'bootside', 'upper', 'alpha', alpha);
text(-1300,-1600,sprintf('Threshold for significance at %d percent(naccu=200): %3.4f\n', alpha*100, sigval));
title(sprintf('Clust %d corr. theta phase at 500 ms and RT', clust));

% amplitude sorting
% -----------------
figure;
[outdata,outvar,outtrials,limits,axhndls,erp, ...
          amps,cohers,cohsig,ampsig,outamps,phsangls,phsamp,sortidx]=erpimage(act_all(:,:),sortvar_all,timepnts, ...
   '', 0,0,'erp', 'erpstd', 'caxis', 0.5, 'cbar', ...
   'srate',256,'align',352, 'yerplabel', '', erpimopt{:}, 'ampsort', [500 0 5]); % aligned to median rt
close;
subplot(2,2,2);
erpimage(act_all(:,:),sortvar_all,timevect, ...
   '', 300,10,'erp', 'erpstd', 'caxis', 0.5, 'cbar', ...
   'srate',256,'align',352, 'yerplabel', '', erpimopt{:}, 'ampsort', [500 0 5]); % aligned to median rt

% compute correlation
% -------------------
[ypred alpha Rsq] = myregress(outvar, phsamp);
subplot(2,2,4);
plot(outvar, phsamp, '.');
hold on;
plot(outvar, ypred, 'r');
xlabel('Reaction time');
ylabel('Amplitude');
title(sprintf('Theta amp. at 500 ms vs RT (p=%1.8f, R^2=%3.4f)', alpha, Rsq));
set(gcf, 'position', [336 485 730 540], 'paperpositionmode', 'auto');

setfont(gcf, 'fontsize', 10);
eval(['print -djpeg clust' int2str(clust) 'corrthetart.jpg']);

% set up for command line call
% copy text from plotcorrthetaart
%timevect = times;
sortvar = sortvar_all;
data = act_all;
time = 1
freq = 1
g.times = 0;
g.freqs = 5;
g.erpimopt = {};


function [Eft, Varft, lpyt, Eyt, Varyt] = gpia_loopred(gp_array, x, y, varargin)
%GPIA_LOOPRED  Leave-one-out predictions with Gaussian Process IA approximation
%
%  Description
%    [EFT, VARFT, LPYT, EYT, VARYT] = GPIA_LOOPRED(RECGP, X, Y)
%    takes a cell array of GP structures together with matrix X of
%    input vectors, matrix X of training inputs and vector Y of
%    training targets, and evaluates the leave-one-out predictive
%    distribution at inputs X and returns means EFT and variances
%    VARFT of latent variables, the logarithm of the predictive
%    densities PYT, and the predictive means EYT and variances
%    VARYT of observations at input locations X.
%
%    OPTIONS is optional parameter-value pair
%      z      - optional observed quantity in triplet (x_i,y_i,z_i)
%               Some likelihoods may use this. For example, in case of
%               Poisson likelihood we have z_i=E_i, that is, expected value
%               for ith case.
%      is     - defines if importance sampling weighting is used 'on'
%               (default). If set to 'off', integration points and
%               weights for the full posterior are used.
%
%    Given Gaussian likelihood or non-Gaussian likelihood and
%    latent method Laplace or EP, LOO-posterior given
%    hyperparameters is computed analytically or with analytic
%    approximation and LOO-posterior of the hyperparameters is
%    approximated using importance sampling reweighted integration
%    points from gp_ia. Optionally integration weights for full data
%    posterior of hyperparameters can be used (by setting option
%    'is' to 'off').
%
%  References:
%    Aki Vehtari and Jouko Lampinen (2002). Bayesian model
%    assessment and comparison using cross-validation predictive
%    densities. Neural Computation, 14(10):2439-2468.
%
%  See also
%   GP_LOOPRED, GP_IA, GP_PRED
%
% Copyright (c) 2009 Ville Pietil�inen
% Copyright (c) 2009-2010 Jarno Vanhatalo
% Copyright (c) 2010,2012 Aki Vehtari

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

  ip=inputParser;
  ip.FunctionName = 'GPIA_LOOPRED';
  ip.addRequired('gp_array',@iscell);
  ip.addRequired('x', @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
  ip.addRequired('y', @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
  ip.addParamValue('z', [], @(x) isreal(x) && all(isfinite(x(:))))
  ip.addParamValue('is', 'on', @(x) ismember(x,{'on' 'off'}))
  ip.parse(gp_array, x, y, varargin{:});
  z=ip.Results.z;
  is=ip.Results.is;

  nGP = numel(gp_array);
  n=size(x,1);

  P_TH = zeros(1,nGP);
  Efts = zeros(n,nGP);
  Varfts = zeros(n,nGP);
  lpyts = zeros(n,nGP);
  if nargout > 3
    Eyts = zeros(n,nGP);
    Varyts = zeros(n,nGP);
  end

  for i1=1:nGP
    Gp=gp_array{i1};
    P_TH(1,i1) = Gp.ia_weight;
    % compute leave-one-out predictions for each hyperparameter sample
    % if latent method is MCMC, then these samples are for latent values, too
    if nargout <= 3
      [Efts(:,i1), Varfts(:,i1), lpyts(:,i1)] = gp_loopred(Gp, x, y, 'z', z);
    else
      [Efts(:,i1), Varfts(:,i1), lpyts(:,i1), Eyts_temp, Varyts_temp] = gp_loopred(Gp, x, y, 'z', z);
      if ~isempty(Eyts_temp)
        Eyts(:,i1) = Eyts_temp;
      end
      if ~isempty(Varyts_temp)
        Varyts(:,i1) = Varyts_temp;
      end
    end
  end

  if isequal(is,'off')
    P_TH=repmat(P_TH,n,1);
  else
    % log importance sampling weights
    lw=-lpyts;
    % normalize weights
    for i2=1:n
      % this works even when lw have large magnitudes
      lw(i2,:)=lw(i2,:)-sumlogs(lw(i2,:));
    end
    % importance sampling weights
    w=exp(lw);
    % reweight ia weights
    P_TH=bsxfun(@times,P_TH,w);
    P_TH=bsxfun(@rdivide,P_TH,sum(P_TH,2));
    % check the effective sample size
    m_eff=1./sum(P_TH.^2,2);
    if min(m_eff)<nGP/5
      warning(sprintf('For %d folds the effective sample size in IS is less than m/5',sum(m_eff<(nGP/5))))
    end
    %fprintf('nGP=%d, min(neff)=%.0f, min(neff)/nGP=%.2f\n',nGP, min(1./sum(P_TH.^2,2)), min(1./sum(P_TH.^2,2))./nGP)
    % PSIS
    if nGP>=200
        % (new) default sample size for is_normal and is_t is 200
        % CCD with nParam>=12 has at least 281 points
        [lw,pk] = psislw(log(P_TH'),10);
        P_TH=exp(lw');
        % check whether the variance and mean of the raw importance ratios is finite
        % PSIS weights have always finite variance and mean, but if raw importance
        % ratios have infinite variance the convergence to true value is
        % slower and if raw importance ratios have non-existing mean the the
        % estimate can't converge to true value
        vkn1=sum(pk>=0.5&pk<0.7);
        vkn2=sum(pk>=0.7&pk<1);
        vkn3=sum(pk>=1);
        n=numel(pk);
        if vkn1>0
            warning('%d (%.0f%%) PSIS Pareto k estimates between 0.5 and .7',vkn1,vkn1/n*100)
        end
        if vkn2>0
            warning('%d (%.0f%%) PSIS Pareto k estimates between 0.7 and 1',vkn2,vkn2/n*100)
        end
        if vkn3>0
            warning('%d (%.0f%%) PSIS Pareto k estimates greater than 1',vkn3,vkn3/n*100)
        end
    end
  end

  % compute combined predictions
  Eft = sum(Efts.*P_TH, 2);
  Varft = sum(Varfts.*P_TH,2) + sum(bsxfun(@minus,Efts,Eft).^2.*P_TH,2);
  if nargout > 2
    lpyt = log(sum(exp(lpyts+log(P_TH)),2));
    if nargout > 3
      Eyt = sum(Eyts.*P_TH,2);
      Varyt = sum(Varyts.*P_TH,2) + sum(bsxfun(@minus,Eyts,Eyt).^2.*P_TH, 2);
    end
  end


function [y]=rake_fft(x, tol)
% Discrete Fourier Transform in the QTT-Tucker format
%   function [y]=rake_fft(x, tol)
%   Computes the multidimensional DFT of the vector x in the QTT-Tucker format with
%   the approximation tolerance tol.
%   Returns the quantized dimensions in the correct order.
%
%   !!WARNING!! employs a (probably) costly rank reverse procedure.

D = size(x.tuck);
tuck = x.tuck;
crx = core2cell(x.core);
for i=1:D
    [tuck{i}, rv]=qr(tuck{i}, 'lr');
    r1 = size(crx{i}, 1); r2 = size(crx{i}, 3); rt = size(crx{i},2);
    crx{i} = permute(crx{i}, [2, 1, 3]);
    crx{i} = reshape(crx{i}, rt, r1*r2);
    crx{i} = rv*crx{i};
    rt = size(rv,1);
    crx{i} = reshape(crx{i}, rt, r1, r2);
    crx{i} = permute(crx{i}, [2, 1, 3]);
end;
% [crx,rv]=qr(crx, 'rl');
% crx = rv.'*crx;
for i=D:-1:2
    r1 = size(crx{i}, 1); rt = size(crx{i}, 2); r2 = size(crx{i}, 3);
    crx{i} = reshape(crx{i}, r1, rt*r2);
    [crx{i}, rv]=qr(crx{i}.', 0);
    r0 = size(crx{i-1}, 1); rt0 = size(crx{i-1}, 2);
    crx{i-1} = reshape(crx{i-1}, r0*rt0, r1);
    crx{i-1}=crx{i-1}*rv.';
    r1 = size(rv,1);
    crx{i-1} = reshape(crx{i-1}, r0, rt0, r1);
    crx{i} = reshape(crx{i}.', r1, rt, r2);
end;
for i=1:D
    r1 = size(crx{i}, 1); r2 = size(crx{i}, 3); rt = size(crx{i},2);
    crx{i} = permute(crx{i}, [1, 3, 2]);
    crx{i} = reshape(crx{i}, r1*r2, rt);
    [crx{i}, rv]=qr(crx{i}, 0);
    tuck{i} = tuck{i}*rv.';
    tuck{i} = qtt_fft1(tuck{i}, tol);
    % rank is reversed
    r = tuck{i}.r;
    rq1 = r(1);
    for k=1:rq1
        ek = [zeros(1,k-1), 1, zeros(1,rq1-k)];
        yk = ek*tuck{i}; % first rank is 1
        if (k==1)
            ynew = yk;
        else
            ynew = [ynew, yk];
            ynew = round(ynew, tol);
        end;
    end;
    [tuck{i}, rv]=qr(ynew, 'lr');
%     % reversing 2
%     % nonorth block is the last, but we need to SVD from the first
%     tuck{i} = core2cell(tuck{i});
%     for j=numel(tuck{i}):-1:2
%         tuck{i}{j} = reshape(tuck{i}{j}, r(j), 2*r(j+1));
%         [tuck{i}{j}, rv]=qr(tuck{i}{j}.', 0);
%         tuck{i}{j-1} = reshape(tuck{i}{j-1}, r(j-1)*2, r(j));
%         tuck{i}{j-1} = tuck{i}{j-1}*rv.';
%         r(j) = size(rv, 1);
%         tuck{i}{j} = reshape(tuck{i}{j}.', r(j), 2, r(j+1));
%         tuck{i}{j-1} = reshape(tuck{i}{j-1}, r(j-1), 2, r(j));
%     end;
%     tuck{i}{1} = permute(tuck{i}{1}, [2, 1, 3]);
%     tuck{i}{1} = reshape(tuck{i}{1}, 2, rq1*r(2));
%     [u,s,v]=svd(tuck{i}{1}, 'econ');
%     s = diag(s);
%     rnew = my_chop2(s, norm(s)*tol/sqrt(numel(tuck{i})-1));
%     u=u(:,1:rnew);
%     v=v(:,1:rnew)*diag(s(1:rnew));
%     tuck{i}{1} = reshape(u, 1, 2, rnew);
%     r(1) = 1;
%     rv = reshape(v', rnew*rq1, r(2));
%     for j=2:numel(tuck{i})
%         tuck{i}{j} = reshape(tuck{i}{j}, r(j), 2*r(j+1));
%         tuck{i}{j} = rv*tuck{i}{j};
%         r(j) = rnew;
%         tuck{i}{j} = reshape(tuck{i}{j}, r(j), rq1, 2, r(j+1));
%         tuck{i}{j} = permute(tuck{i}{j}, [1, 3, 2, 4]);
%         tuck{i}{j} = reshape(tuck{i}{j}, r(j)*2, rq1*r(j+1));
%         [u,s,v]=svd(tuck{i}{j}, 'econ');
%         s = diag(s);
%         rnew = my_chop2(s, norm(s)*tol/sqrt(numel(tuck{i})-1));
%         u=u(:,1:rnew);
%         v=v(:,1:rnew)*diag(s(1:rnew));
%         tuck{i}{j} = reshape(u, r(j), 2, rnew);
%         rv = reshape(v', rnew*rq1, r(j+1));
%     end;
%     rv = reshape(rv, rnew, rq1);
%     tuck{i} = cell2core(tt_tensor, tuck{i});

    crx{i} = crx{i}*rv.';
    rt = size(rv,1);
    crx{i} = reshape(crx{i}, r1, r2, rt);
    crx{i} = permute(crx{i}, [1, 3, 2]);
    if (i<D)
        crx{i} = reshape(crx{i}, r1*rt, r2);
        [crx{i}, rv]=qr(crx{i}, 0);
        r3 = size(crx{i+1}, 3); rt2 = size(crx{i+1},2);
        crx{i+1} = reshape(crx{i+1}, r2, rt2*r3);
        crx{i+1} = rv*crx{i+1};
        r2 = size(rv,1);
        crx{i} = reshape(crx{i}, r1, rt, r2);
        crx{i+1} = reshape(crx{i+1}, r2, rt2, r3);
    end;
end;

y = x;
y.core = cell2core(tt_tensor, crx);
y.tuck = tuck;
end


%This Matlab script can be used to reproduce Figure 6.7 in the monograph:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.0 (Last edited: 2017-11-04)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Empty workspace and close figures
close all;
clear;

%Number of BSs
L = 16;

%Number of UEs per BS
K = 10;

%Number of BS antennas
M = 100;

%Define the range of pilot reuse factors
fRange = [1 2];

%Select the number of setups with random UE locations
nbrOfSetups = 20;

%Select the number of channel realizations per setup
nbrOfRealizations = 200;

%Select the range of hardware qualities. The values at the same position in
%the different vectors are considered simultaneously.
kappatUE = [0.99 0.99 0.99 0.99 0.99 0.95 0.97 0.98 0.99 0.995 1];
kapparUE = [0.95 0.97 0.98 0.995 1 0.99 0.99 0.99 0.99 0.99 0.99];
kappatBS = kapparUE;
kapparBS = kappatUE;

%% Propagation parameters

%Communication bandwidth
B = 20e6;

%Total uplink transmit power per UE (mW)
p = 100;

%Total downlink transmit power per UE (mW)
rho = 100;

%Noise figure at the BS (in dB)
noiseFigure = 7;

%Compute noise power
noiseVariancedBm = -174 + 10*log10(B) + noiseFigure;

%Select length of coherence block
tau_c = 200;

%Use the approximation of the Gaussian local scattering model
accuracy = 2;

%Angular standard deviation in the local scattering model (in degrees)
ASDdeg = 10;

%Prepare to save simulation results
sumSE_MR = zeros(length(kappatUE),length(fRange),nbrOfSetups);
sumSE_RZF = zeros(length(kappatUE),length(fRange),nbrOfSetups);
sumSE_MMMSE = zeros(length(kappatUE),length(fRange),nbrOfSetups);

%% Go through all setups
for n = 1:nbrOfSetups

    %Output simulation progress
    disp([num2str(n) ' setups out of ' num2str(nbrOfSetups)]);

    %Compute channel statistics for one setup
    [R,channelGaindB] = functionExampleSetup(L,K,M,accuracy,ASDdeg);

    %Compute the normalized average channel gain, where the normalization
    %is based on the noise power
    channelGainOverNoise = channelGaindB - noiseVariancedBm;

    %Go through all pilot reuse factors
    for s = 1:length(fRange)

        %Extract pilot reuse factor
        f = fRange(s);

        %Output simulation progress
        disp([num2str(f) ' reuse factor out of ' num2str(length(fRange))]);

        %Go through all hardware qualities
        for r = 1:length(kappatUE)

            %Generate channel realizations with estimates and estimation
            %error correlation matrices, using LMMSE estimation
            [Hhat,C,tau_p,~,H] = functionChannelEstimates_impairments(R,channelGainOverNoise,nbrOfRealizations,M,K,L,p,f,kappatUE(r),kapparBS(r));

            %Compute SE using Monte-Carlo realizations
            [SE_MR,SE_RZF,SE_MMMSE] = functionComputeSE_DL_impairments(H,Hhat,C,tau_c,tau_p,nbrOfRealizations,M,K,L,p,rho,kappatBS(r),kapparUE(r));

            %Save results
            sumSE_MR(r,s,n) = mean(sum(SE_MR,1));
            sumSE_RZF(r,s,n) = mean(sum(SE_RZF,1));
            sumSE_MMMSE(r,s,n) = mean(sum(SE_MMMSE,1));

            %Delete large matrices
            clear Hhat C H;

        end

    end

    %Delete large matrices
    clear R;

end

%Plot the simulation results
figure;
hold on; box on;

plot(kapparUE([1 2 3 9 4 5]),max(mean(sumSE_MMMSE([1 2 3 9 4 5],:,:),3),[],2),'rd-','LineWidth',1);
plot(kapparUE([1 2 3 9 4 5]),max(mean(sumSE_RZF([1 2 3 9 4 5],:,:),3),[],2),'r-.','LineWidth',1);
plot(kapparUE([1 2 3 9 4 5]),max(mean(sumSE_MR([1 2 3 9 4 5],:,:),3),[],2),'bs-','LineWidth',1);

xlabel('DL hardware quality');
ylabel('Average sum SE [bit/s/Hz/cell]');
legend('M-MMSE','RZF','MR','Location','SouthEast');
xlim([0.95 1]);
ylim([0 40]);

%Plot the simulation results
figure;
hold on; box on;

plot(kappatUE(6:11),max(mean(sumSE_MMMSE(6:11,:,:),3),[],2),'rd-','LineWidth',1);
plot(kappatUE(6:11),max(mean(sumSE_RZF(6:11,:,:),3),[],2),'r-.','LineWidth',1);
plot(kappatUE(6:11),max(mean(sumSE_MR(6:11,:,:),3),[],2),'bs-','LineWidth',1);

xlabel('UL hardware quality');
ylabel('Average sum SE [bit/s/Hz/cell]');
legend('M-MMSE','RZF','MR','Location','SouthEast');
xlim([0.95 1]);
ylim([0 40]);


function out=mtimes(x,y)

precisionx=0;   precisiony=0;
xmp=isa(x,'mp');
if xmp
 precisionx=x(1).precision;
 ymp=isa(y,'mp');
else
 ymp=true;
end
if ymp
 precisiony=y(1).precision;
end
precision=max(precisionx,precisiony);
ex=numel(x);
ey=numel(y);

if ex==1 | ey==1
 if ~xmp,  x=mp(x);  end
 if ~ymp,  y=mp(y);  end
 if ex==1
  out_rval=cell(size(y));
  out_ival=out_rval;

  [xrval,xival]=getVals(x,1);
  %xrval=x(1).rval;  xival=x(1).ival;  if isempty(xival), xival='0'; end

  xReal=hasimag(xival);
  for ii=1:ey
   [yrval,yival]=getVals(y,ii);
   %yrval=y(ii).rval;  yival=y(ii).ival;  if isempty(yival), yival='0'; end
   if xReal | hasimag(yival)
    [out_rval{ii},out_ival{ii}]=mpfr_mulc(precision,xrval,xival,yrval,yival);
   else
    %if isempty(xrval),  'xxxxxxxxxxxx',kb, end
    out_rval{ii}=mpfr_mul(precision,xrval,yrval);
   end
  end
 else
  out_rval=cell(size(x));
  out_ival=out_rval;
  [yrval,yival]=getVals(y,1);
  yReal=hasimag(yival);
  for ii=1:ex
   [xrval,xival]=getVals(x,ii);
   if hasimag(xival) | yReal
    [out_rval{ii},out_ival{ii}]=mpfr_mulc(precision,xrval,xival,yrval,yival);
   else
    out_rval{ii}=mpfr_mul(precision,xrval,yrval);
   end
  end
 end
 out=class(struct('rval',out_rval,...
                  'ival',out_ival,...
                  'precision',precision),'mp');
else %%% out=x*y;
 sx=size(x);
 sy=size(y);
 if sx(2)~=sy(1)
  disp(['size of matrix 1 => ',num2str(sx(1)),'x',num2str(sx(2))]);
  disp(['size of matrix 2 => ',num2str(sy(1)),'x',num2str(sy(2))]);
  error('array size mismatch for mtimes')
 else
  out=mp(zeros(sx(1),sy(2)),precision);
  for ii=1:sx(1)
   for jj=1:sy(2)
    out(ii,jj)=sum(x(ii,1:end).*y(1:end,jj).');
   end
  end
 end
end


function P=compute_constraint_distance_3param_6eq_6unk(m1,m2,m3)

% Copyright (C) <2007>  <Francesc Moreno-Noguer, Vincent Lepetit, Pascal Fua>
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the version 3 of the GNU General Public License
% as published by the Free Software Foundation.
%
% 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/>.
%
% Francesc Moreno-Noguer, CVLab-EPFL, September 2007.
% fmorenoguer@gmail.com, http://cvlab.epfl.ch/~fmoreno/

%redefine variables name, for compatibility with maple
m1_1=m1(1);
m1_2=m1(2);
m1_3=m1(3);
m1_4=m1(4);
m1_5=m1(5);
m1_6=m1(6);
m1_7=m1(7);
m1_8=m1(8);
m1_9=m1(9);
m1_10=m1(10);
m1_11=m1(11);
m1_12=m1(12);

m2_1=m2(1);
m2_2=m2(2);
m2_3=m2(3);
m2_4=m2(4);
m2_5=m2(5);
m2_6=m2(6);
m2_7=m2(7);
m2_8=m2(8);
m2_9=m2(9);
m2_10=m2(10);
m2_11=m2(11);
m2_12=m2(12);

m3_1=m3(1);
m3_2=m3(2);
m3_3=m3(3);
m3_4=m3(4);
m3_5=m3(5);
m3_6=m3(6);
m3_7=m3(7);
m3_8=m3(8);
m3_9=m3(9);
m3_10=m3(10);
m3_11=m3(11);
m3_12=m3(12);

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

t1 = (m1_2 ^ 2);
t4 = (m1_6 ^ 2);
t5 = (m1_3 ^ 2);
t6 = (m1_5 ^ 2);
t11 = (m1_4 ^ 2);
t12 = (m1_1 ^ 2);
t20 = m1_4 * m2_4;
t21 = m1_3 * m2_3;
t22 = m1_5 * m2_5;
t23 = m1_2 * m2_2;
t24 = m1_6 * m2_6;
t25 = m1_1 * m2_1;
t26 = (-m2_4 * m1_1 - m2_5 * m1_2 - m2_6 * m1_3 - m1_6 * m2_3 - m1_4 * m2_1 - m1_5 * m2_2 + t20 + t21 + t22 + t23 + t24 + t25);
t27 = m1_6 * m3_6;
t29 = m1_5 * m3_5;
t30 = m1_4 * m3_4;
t33 = m1_3 * m3_3;
t35 = m1_1 * m3_1;
t38 = m1_2 * m3_2;
t39 = (t27 - m1_6 * m3_3 + t29 + t30 - m1_4 * m3_1 - m3_6 * m1_3 + t33 - m3_5 * m1_2 + t35 - m3_4 * m1_1 - m1_5 * m3_2 + t38);
t40 = (m2_4 ^ 2);
t41 = (m2_2 ^ 2);
t42 = (m2_5 ^ 2);
t43 = (m2_1 ^ 2);
t44 = (m2_6 ^ 2);
t45 = (m2_3 ^ 2);
t53 = m2_4 * m3_4;
t56 = m2_5 * m3_5;
t57 = m2_2 * m3_2;
t60 = m2_1 * m3_1;
t62 = m2_6 * m3_6;
t63 = m2_3 * m3_3;
t65 = (t53 - m2_4 * m3_1 - m3_4 * m2_1 + t56 + t57 - m2_5 * m3_2 - m2_6 * m3_3 + t60 - m3_6 * m2_3 + t62 + t63 - m3_5 * m2_2);
t66 = (m3_5 ^ 2);
t69 = (m3_4 ^ 2);
t70 = (m3_3 ^ 2);
t71 = (m3_2 ^ 2);
t72 = (m3_6 ^ 2);
t75 = (m3_1 ^ 2);
t81 = (m1_9 ^ 2);
t82 = (m1_8 ^ 2);
t83 = (m1_7 ^ 2);
t90 = m1_8 * m2_8;
t91 = m1_7 * m2_7;
t93 = m1_9 * m2_9;
t98 = (-m1_9 * m2_3 + t21 + t90 + t23 + t91 + t25 - m2_7 * m1_1 + t93 - m2_9 * m1_3 - m1_8 * m2_2 - m1_7 * m2_1 - m2_8 * m1_2);
t101 = m1_9 * m3_9;
t102 = m1_7 * m3_7;
t106 = m1_8 * m3_8;
t108 = (-m3_8 * m1_2 + t38 + t33 - m3_9 * m1_3 + t101 + t102 - m1_7 * m3_1 - m1_9 * m3_3 + t35 - m3_7 * m1_1 + t106 - m1_8 * m3_2);
t113 = (m2_8 ^ 2);
t116 = (m2_9 ^ 2);
t117 = (m2_7 ^ 2);
t119 = m2_8 * m3_8;
t122 = m2_9 * m3_9;
t125 = m2_7 * m3_7;
t128 = (t119 - m2_7 * m3_1 - m3_8 * m2_2 + t57 + t63 + t60 + t122 - m2_8 * m3_2 - m3_7 * m2_1 + t125 - m3_9 * m2_3 - m2_9 * m3_3);
t129 = (m3_7 ^ 2);
t130 = (m3_8 ^ 2);
t133 = (m3_9 ^ 2);
t141 = (m1_10 ^ 2);
t142 = (m1_11 ^ 2);
t143 = (m1_12 ^ 2);
t151 = m1_10 * m2_10;
t152 = m1_12 * m2_12;
t154 = m1_11 * m2_11;
t158 = (-m2_10 * m1_1 - m2_12 * m1_3 + t151 + t23 + t25 + t152 + t21 - m1_10 * m2_1 + t154 - m1_11 * m2_2 - m2_11 * m1_2 - m1_12 * m2_3);
t160 = m1_12 * m3_12;
t164 = m1_10 * m3_10;
t165 = m1_11 * m3_11;
t168 = (-m3_10 * m1_1 + t160 - m3_11 * m1_2 + t38 + t33 - m1_12 * m3_3 - m3_12 * m1_3 + t164 + t35 + t165 - m1_11 * m3_2 - m1_10 * m3_1);
t169 = (m2_12 ^ 2);
t170 = (m2_10 ^ 2);
t171 = (m2_11 ^ 2);
t179 = m2_10 * m3_10;
t181 = m2_12 * m3_12;
t185 = m2_11 * m3_11;
t188 = (t57 + t60 + t179 - m2_10 * m3_1 + t181 - m2_12 * m3_3 - m3_12 * m2_3 - m3_10 * m2_1 + t63 + t185 - m2_11 * m3_2 - m3_11 * m2_2);
t191 = (m3_12 ^ 2);
t192 = (m3_10 ^ 2);
t193 = (m3_11 ^ 2);
t212 = (t22 + t91 + t93 - m1_7 * m2_4 + t20 - m2_7 * m1_4 + t90 - m2_8 * m1_5 - m1_8 * m2_5 - m1_9 * m2_6 + t24 - m2_9 * m1_6);
t219 = (t101 + t30 - m3_9 * m1_6 - m3_8 * m1_5 - m1_8 * m3_5 - m3_7 * m1_4 + t27 - m1_9 * m3_6 - m1_7 * m3_4 + t102 + t106 + t29);
t233 = (t53 - m2_8 * m3_5 - m2_9 * m3_6 - m2_7 * m3_4 + t125 + t122 - m3_8 * m2_5 + t62 + t119 + t56 - m3_7 * m2_4 - m3_9 * m2_6);
t254 = (-m2_11 * m1_5 + t154 + t151 - m1_12 * m2_6 + t20 - m1_10 * m2_4 - m2_12 * m1_6 - m2_10 * m1_4 - m1_11 * m2_5 + t152 + t24 + t22);
t261 = (t30 - m3_12 * m1_6 - m1_10 * m3_4 - m1_11 * m3_5 + t160 + t27 + t164 + t165 - m3_11 * m1_5 - m3_10 * m1_4 - m1_12 * m3_6 + t29);
t275 = (-m3_10 * m2_4 + t56 - m2_10 * m3_4 + t62 - m3_12 * m2_6 - m2_11 * m3_5 + t53 - m3_11 * m2_5 - m2_12 * m3_6 + t179 + t181 + t185);
t296 = (-m2_12 * m1_9 + t152 + t91 + t90 - m1_11 * m2_8 + t151 - m2_11 * m1_8 + t93 - m1_10 * m2_7 - m1_12 * m2_9 - m2_10 * m1_7 + t154);
t303 = (t164 + t102 - m3_10 * m1_7 + t160 - m3_12 * m1_9 - m1_10 * m3_7 - m3_11 * m1_8 + t101 - m1_12 * m3_9 + t106 + t165 - m1_11 * m3_8);
t317 = (t125 + t119 - m3_12 * m2_9 - m3_10 * m2_7 - m2_11 * m3_8 - m3_11 * m2_8 + t122 + t179 - m2_12 * m3_9 + t181 - m2_10 * m3_7 + t185);
P(1,1) = t1 - 2 * m1_4 * m1_1 + t4 + t5 + t6 - 2 * m1_5 * m1_2 - 2 * m1_6 * m1_3 + t11 + t12;
P(1,2) = 2 * t26;
P(1,3) = 2 * t39;
P(1,4) = t40 + t41 + t42 + t43 + t44 + t45 - 2 * m2_5 * m2_2 - 2 * m2_6 * m2_3 - 2 * m2_4 * m2_1;
P(1,5) = 2 * t65;
P(1,6) = t66 - 2 * m3_4 * m3_1 + t69 + t70 + t71 + t72 - 2 * m3_6 * m3_3 + t75 - 2 * m3_5 * m3_2;
P(2,1) = -2 * m1_8 * m1_2 + t12 + t81 + t5 + t82 + t83 + t1 - 2 * m1_9 * m1_3 - 2 * m1_7 * m1_1;
P(2,2) = 2 * t98;
P(2,3) = 2 * t108;
P(2,4) = -2 * m2_8 * m2_2 - 2 * m2_9 * m2_3 + t113 - 2 * m2_7 * m2_1 + t116 + t117 + t41 + t43 + t45;
P(2,5) = 2 * t128;
P(2,6) = t75 + t70 + t129 + t71 + t130 - 2 * m3_9 * m3_3 + t133 - 2 * m3_8 * m3_2 - 2 * m3_7 * m3_1;
P(3,1) = -2 * m1_11 * m1_2 + t141 + t142 + t12 + t1 + t5 + t143 - 2 * m1_10 * m1_1 - 2 * m1_12 * m1_3;
P(3,2) = 2 * t158;
P(3,3) = 2 * t168;
P(3,4) = t169 + t41 + t43 + t45 + t170 + t171 - 2 * m2_12 * m2_3 - 2 * m2_10 * m2_1 - 2 * m2_11 * m2_2;
P(3,5) = 2 * t188;
P(3,6) = t71 - 2 * m3_12 * m3_3 + t75 + t191 + t70 + t192 + t193 - 2 * m3_10 * m3_1 - 2 * m3_11 * m3_2;
P(4,1) = -2 * m1_9 * m1_6 + t11 + t4 + t81 + t82 + t6 + t83 - 2 * m1_7 * m1_4 - 2 * m1_8 * m1_5;
P(4,2) = 2 * t212;
P(4,3) = 2 * t219;
P(4,4) = t117 + t113 - 2 * m2_8 * m2_5 - 2 * m2_7 * m2_4 - 2 * m2_9 * m2_6 + t116 + t40 + t42 + t44;
P(4,5) = 2 * t233;
P(4,6) = t129 - 2 * m3_9 * m3_6 + t69 + t66 + t72 + t133 - 2 * m3_8 * m3_5 - 2 * m3_7 * m3_4 + t130;
P(5,1) = t4 + t143 + t11 - 2 * m1_12 * m1_6 - 2 * m1_11 * m1_5 - 2 * m1_10 * m1_4 + t6 + t141 + t142;
P(5,2) = 2 * t254;
P(5,3) = 2 * t261;
P(5,4) = t170 + t171 + t169 - 2 * m2_10 * m2_4 - 2 * m2_11 * m2_5 - 2 * m2_12 * m2_6 + t40 + t42 + t44;
P(5,5) = 2 * t275;
P(5,6) = t69 + t66 + t72 - 2 * m3_12 * m3_6 - 2 * m3_10 * m3_4 + t193 - 2 * m3_11 * m3_5 + t192 + t191;
P(6,1) = t142 - 2 * m1_10 * m1_7 + t141 + t83 + t143 + t82 - 2 * m1_12 * m1_9 + t81 - 2 * m1_11 * m1_8;
P(6,2) = 2 * t296;
P(6,3) = 2 * t303;
P(6,4) = t171 + t170 - 2 * m2_12 * m2_9 - 2 * m2_10 * m2_7 - 2 * m2_11 * m2_8 + t113 + t169 + t116 + t117;
P(6,5) = 2 * t317;
P(6,6) = -2 * m3_11 * m3_8 + t193 + t133 + t129 + t192 + t130 - 2 * m3_12 * m3_9 + t191 - 2 * m3_10 * m3_7;


function doseStat = dispDoseStats(doseBinsV, volHistV, name, nameVol, planC, indexS, opt)
%Command line display of basic dose statistics
%doseBinsV is a vector of the midpoint doseBin values.
%volHistV is either a histogram of volumes or surface areas.
%LM: 14 Oct 02, JOD.
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR 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.
%
% CERR 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 CERR.  If not, see <http://www.gnu.org/licenses/>.

switch lower(opt)

  case 'standarddose'
    disp('-----------------------')
    disp('')
    disp(['Structure is:  ' name])

    totalVol = sum(volHistV);
    disp(['Total volume is:  ' num2str(totalVol) ' cubic cm.'])

    disp(['Dose map name is:  ' nameVol])

    meanD = sum(doseBinsV(:).*volHistV(:))/sum(volHistV);
    disp(['Mean dose is:  ' num2str(meanD)])

    ind = max(find([volHistV~=0]));
    maxD = doseBinsV(ind);
    disp(['Max dose is:  ' num2str(maxD)])

    ind = min(find([volHistV~=0]));
    minD = doseBinsV(ind);

    disp(['Min dose is:  ' num2str(minD)])
    disp('')
    disp('-----------------------')

    doseStat.volume = totalVol;

  case 'dshdose'

    disp('-----------------------')
    disp('')
    disp(['Structure is:  ' name])

    areaV = volHistV;  %actually areas, not volumes.
    dosesV = doseBinsV;

    totalArea = sum(areaV);
    disp(['Total surface area is:  ' num2str(totalArea) ' square cm.'])

    disp(['Dose map name is:  ' nameVol])

    meanD = sum(dosesV(:).*areaV(:))/sum(areaV);
    disp(['Mean surface dose is:  ' num2str(meanD)])

    maxD = max(dosesV);
    disp(['Max dose is:  ' num2str(maxD)])

    minD = min(dosesV);

    disp(['Min dose is:  ' num2str(minD)])
    disp('')
    disp('-----------------------')

    doseStat.volume = totalArea;

end

doseStat.min    = minD;
doseStat.mean   = meanD;
doseStat.max    = maxD;


function [vs, output, v0] = computeRatioConfidenceInterval(v0, expdata, model, max_score, ratio)

% v0 - set of flux vectors to be used as initial guesses.  They may be
% valid or not.
% expdata - experimental data.
% model - The standard model.  Additional field .N (= null(S)) should also
% be provided.  This is a basis of the flux space.
% max score - maximum allowable data fit error.
% ratio - which reactions to take the ratio of.  positive values indicate
% reactions in the numerator and negative values, reactions in the
% denominator.

majorIterationLimit = 2000;  %max number of iterations
minorIterationLimit = 1e7;   % essentially infinity
diffInterval = 1e-5;         %gradient step size.
feasibilityTolerance = max_score/20; % how close you need to be to the max score.

x0 = model.N\v0; % back substitute
numpoints = size(v0,2);
numratios = size(ratio,2);

scores = zeros(numpoints,1);

tProb.user.expdata = expdata;
tProb.user.model = model;
for i = 1:numpoints
    scores(i) = errorComputation2(x0(:,i),tProb);
end

v0(:,scores > max_score) = fitC13Data(v0(:,scores > max_score),expdata,model);

if ~isfield(model, 'N')
   model.N = null(model.S);
end

x0 = model.N\v0; % back substitute

% safety check:
if (max(abs(model.S*v0))> 1e-6)
    display('v0 not quite in null space');
    pause;
end
if(max(abs(model.N*x0 - v0)) > 1e-6)
    display('null basis is weird');
    pause;
end

nalpha = size(model.N, 2);
x_L = -1000*ones(nalpha,1);
x_U = 1000*ones(nalpha,1);
Name = 't2';
[A, b_L, b_U] = defineLinearConstraints(model);

scores = zeros(numpoints,1);
% compute scores for all points.
for i = 1:numpoints
    scores(i) = errorComputation2(x0(:,i),tProb);
end
valid_index = scores < max_score + feasibilityTolerance;
fprintf('found %d valid points\n', sum(valid_index));

c = 'errorComputation2';
c_L = 0; c_U = max_score;

dc = 'errorComputation2_grad';
d2c = []; ConsPattern = [];
%pSepFunc = [];
x_min = []; x_max = []; f_opt = []; x_opt = [];
H = [];  HessPattern = []; pSepFunc = [];  fLowBnd = [];
Solver = 'snopt';

outputminv = 222*ones(numratios, numpoints);
outputminexitflag = -222*ones(numratios, numpoints);
outputminfinalscore = -222*ones(numratios, numpoints);
outputmaxv = -222*ones(numratios, numpoints);
outputmaxexitflag = -222*ones(numratios, numpoints);
outputmaxfinalscore = -222*ones(numratios, numpoints);
outputminstruct = cell(numratios, numpoints);
outputmaxstruct = cell(numratios, numpoints);

%iterate through directions
for i = 1:numratios
%for i = 10:200
    for j = -1:2:1 % max and min
        ration = zeros(size(objective_coefficient(1,model)));
        ratiod = zeros(size(objective_coefficient(1,model)));
        for m = 1:size(ratio,1);
            if(ratio(m,i) > 0)
                ration = ration + objective_coefficient(m,model)*j;
            elseif(ratio(m,i) < 0)
                ratiod = ratiod + objective_coefficient(m,model);
            end
        end

        % initialize temp variables to make parfor work.
        v = j*222*ones(1,numpoints);
        exitflag = -222*ones(1,numpoints);
        finalscore = 222*ones(1,numpoints);
        ostruct = cell(1,numpoints);

        for k = 1:numpoints
            if exist('ttt.txt', 'file')
                fprintf('quitting due to file found\n');
                continue;
            end
            xinitial = x0(:,k);
%                 Prob  = lpconAssign(d, x_L, x_U, Name, xinitial,...
%                                       A, b_L, b_U,...
%                                       c, dc, d2c, ConsPattern, c_L, c_U,...
%                                       fLowBnd, x_min, x_max, f_opt,
%                                       x_opt);

            Prob = conAssign('ratioScore', 'ratioScore_grad', H, HessPattern, x_L, x_U, Name, xinitial, ...
                            pSepFunc, fLowBnd, ...
                            A, b_L, b_U, c, dc, d2c, ConsPattern, c_L, c_U, ...
                            x_min, x_max, f_opt, x_opt);
            %Prob.NumDiff = 2; % central diff
            %Prob.optParam.CentralDiff = 1e-5;
            %pause;
            Prob.user.expdata = expdata;
            Prob.user.model = model;
            Prob.user.ration = ration;
            Prob.user.ratiod = ratiod;

            Prob.user.max_error = max_score;
            Prob.user.diff_interval = diffInterval;
            Prob.user.useparfor = true;

            Prob.optParam.IterPrint = 0;
            %Prob.optParam.cTol = .1*feasibilityTolerance;

            Prob.PriLevOpt = 1;
            Prob.SOL.PrintFile = strcat('temp/snoptp', num2str(i), 'x', num2str(j), 'x', num2str(k),'.txt');
            Prob.SOL.SummFile = strcat('temp/snopts', num2str(i), 'x', num2str(j), 'x', num2str(k),'.txt');
            Prob.SOL.optPar(35) = majorIterationLimit; %This is major iteration count.
            Prob.SOL.optPar(30) = minorIterationLimit; %total iteration limit;
            %Prob.SOL.optPar(11) = feasibilityTolerance; % feasibility tolerance

            Result = tomRun(Solver, Prob, 5);
            tscore = errorComputation2(Result.x_k, tProb);
            tbest = Result.f_k;

            fprintf('reaction %d (%d), x %d; x=%f (%f); score=%f (%f)\n', i,length(model.lb),j, tbest,fLowBnd, tscore, max_score)

            v(k) = j*tbest;
            exitflag(k) = Result.Inform;
            finalscore(k) = tscore;
            ostruct{k} = Result;
        end
        if (j > 0) %minimizing
            outputminv(i,:) = v; % multiply by j to correct sign.
            outputminexitflag(i,:) = exitflag;
            outputminfinalscore(i,:) = finalscore;
            outputminstruct(i,:) = ostruct;
        else
            outputmaxv(i,:) = v; % multiply by j to correct sign.
            outputmaxexitflag(i,:) = exitflag;
            outputmaxfinalscore(i,:) = finalscore;
            outputmaxstruct(i,:) = ostruct;
        end
    end
end

output.minv = outputminv;
output.maxv = outputmaxv;
output.minexitflag = outputminexitflag;
output.maxexitflag = outputmaxexitflag;
output.minfinalscore = outputminfinalscore;
output.maxfinalscore = outputmaxfinalscore;
output.minstruct = outputminstruct;
output.maxstruct = outputmaxstruct;

vs = zeros(numratios, 2);
for i = 1:numratios
    validindex = output.minfinalscore(i,:) < max_score + feasibilityTolerance;
    if any(validindex)
        vs(i,1) = min(output.minv(i,validindex));
    else
        vs(i,1) = 222;
    end
    validindex = output.maxfinalscore(i,:) < max_score + feasibilityTolerance;
    if any(validindex)
        vs(i,2) = max(output.maxv(i,validindex));
    else
        vs(i,2) = -222;
    end
end

return;

% function that returns the proper objective coefficient for each reaction
% takes into account the reversibility of reactinos etc.
function [d] =  objective_coefficient(i, model)
d = zeros(length(model.lb),1);
d(i) = 1;
if (model.match(i))
    d(model.match(i)) = -1;
end
d = (d'*model.N)'; % transform to null space;
return


function [ymu,ys2,fmu,fs2,dymu,dys2,dfmu,dfs2] = gpgrad(x0,gpstruct,method,dx)
%GPGRAD Gradient of Gaussian process mean prediction

if nargin < 3 || isempty(method); method = 'central'; end
if nargin < 4 || isempty(dx); dx = 1e-6; end

D = size(x0,2);
Nhyp = length(gpstruct.hyp);

% Initialize variables
ymu = zeros(Nhyp,1);
fmu = zeros(Nhyp,1);
ys2 = zeros(Nhyp,1);
fs2 = zeros(Nhyp,1);

if nargout > 4
    dymu = zeros(Nhyp,D);
    dfmu = zeros(Nhyp,D);
    dys2 = zeros(Nhyp,D);
    dfs2 = zeros(Nhyp,D);
end

% Compute predictions and derivatives for each hyperparameter sample
for i = 1:Nhyp

    try
        if nargout > 4
            [dy,y0] = fgrad(@(xi_) gpfunc(xi_,gpstruct,gpstruct.hyp(i)),x0,method,'Vectorized','Step',dx);
        else
            y0 = gpfunc(x0,gpstruct,gpstruct.hyp(i));
        end
    catch
        y0 = NaN(1,4);
        if nargout > 4; dy = NaN(4,D); end
        warning('bads:gpGradFail', 'Error in computing the GP derivative.');
    end

    ymu(i) = y0(1);
    ys2(i) = y0(2);
    fmu(i) = y0(3);
    fs2(i) = y0(4);

    if nargout > 4
        dymu(i,:) = dy(1,:);
        dfmu(i,:) = dy(2,:);
        dys2(i,:) = dy(3,:);
        dfs2(i,:) = dy(4,:);
    end

end

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

function yy = gpfunc(xi,gpstruct,hyp)
    if gpstruct.gpmlext
        % Pre-computed Laplace approximation at MAP
        if isfield(gpstruct,'hypSigma') && ~isempty(gpstruct.hypSigma)
            hyp.Sigma = gpstruct.hypSigma;
        end

        if isfield(gpstruct,'post') && ~isempty(gpstruct.post)
        %    [ymuib,ys2ib,fmuib,fs2ib] = mygp(hyp,gpstruct.inf,gpstruct.mean,gpstruct.cov, ...
        %gpstruct.lik,gpstruct.x,gpstruct.post,xi);

            [ymui,ys2i,fmui,fs2i] = mgp(hyp,gpstruct.inf,gpstruct.mean,gpstruct.cov, ...
        gpstruct.lik,gpstruct.x,gpstruct.post,xi);

            %[fmuib(:) - fmui(:), sqrt(fs2ib(:)) - sqrt(fs2i(:))]

        else
            [ymui,ys2i,fmui,fs2i] = mgp(hyp,gpstruct.inf,gpstruct.mean,gpstruct.cov, ...
        gpstruct.lik,gpstruct.x,gpstruct.y,xi);
        end
        yy = [ymui,ys2i,fmui,fs2i];
    else

        if isfield(gpstruct,'post') && ~isempty(gpstruct.post)
            [ymui,ys2i,fmui,fs2i] = mygp(hyp,gpstruct.inf,gpstruct.mean,gpstruct.cov, ...
        gpstruct.lik,gpstruct.x,gpstruct.post,gpstruct.s,xi);
        else
            [ymui,ys2i,fmui,fs2i] = mygp(hyp,gpstruct.inf,gpstruct.mean,gpstruct.cov, ...
        gpstruct.lik,gpstruct.x,gpstruct.y,gpstruct.s,xi);
        end

        if 1
            % [~, ymui, ys2i] = feval(gpstruct.lik{:}, hyp.lik, [], fmui, fs2i);

            lik = hyp.lik; lik(end) = -Inf;
            % fmuiold = fmui; fs2iold = fs2i;
            [~, fmui, fs2i] = feval(gpstruct.lik{:}, lik, [], [], fmui, fs2i);
        end

        yy = [ymui,ys2i,fmui,fs2i];
        % [ymui,sqrt(ys2i),fmui,sqrt(fs2i),fmuiold,sqrt(fs2iold)]
    end


function aistats2012_slides_gp

randn('state', 1);

plot_gp(true, 0.2, 0.4, 100, false, ...
        '/home/jluttine/papers/aistats2012/slides/fig_short', ...
        8, 8);
plot_gp(true, 1.5, 0.4, 100, false, ...
        '/home/jluttine/papers/aistats2012/slides/fig_long', ...
        8, 8);
plot_gp(false, 1, 0.2, 6, 5, ...
        '/home/jluttine/papers/aistats2012/slides/fig_gp', ...
        6, 4);

function plot_gp(noisy, lengthscale, noise, N_all, samples, filename, width, height);
xmin = 0;
xmax = 10;

Nh = 1000;
xh = linspace(xmin, xmax, Nh);

x_all = linspace(xmin+0.5, xmax-0.5, N_all);
%x_all = unifrnd(xmin, xmax, 1, N_all);

% Generate data
covfunc_all = gp_cov_sum(gp_cov_se(sq_dist(x_all), ...
                                 'lengthscale', lengthscale), ...
                       gp_cov_scale(gp_cov_delta(N_all), ...
                                    'scale', noise));

K_y = covfunc_all([]);
L_y = chol(K_y + 1e-6*eye(N_all), 'lower');
y_all = L_y * randn(N_all,1);

for N = [N_all]
  x = x_all(:,1:N);
  y = y_all(1:N);

  covfunc_y = gp_cov_sum(gp_cov_se(sq_dist(x), ...
                                   'lengthscale', lengthscale), ...
                         gp_cov_scale(gp_cov_delta(N), ...
                                      'scale', noise));
  covfunc_yf = gp_cov_se(sq_dist(x,xh), ...
                         'lengthscale', lengthscale);
  if noisy
    covfunc_f = gp_cov_sum(gp_cov_se(zeros(Nh,1), ...
                                     'lengthscale', lengthscale), ...
                           gp_cov_scale(gp_cov_wrap(ones(Nh,1)), ...
                                        'scale', 0.3));
  else
    covfunc_f = gp_cov_se(zeros(Nh,1), ...
                          'lengthscale', lengthscale);
  end

  K_y = covfunc_y([]);
  K_yf = covfunc_yf([]);
  k_f = covfunc_f([]);

  [fh_mean, fh_var] = gp_predict(y, K_y, K_yf, k_f);
  figure
  errorplot(xh, fh_mean, sqrt(fh_var))
  hold on
  plot(x, y, 'r+')
  set(gca, 'xtick', [], 'ytick', []);
  set(gca, 'box', 'off');
  set_figure_size(width,height);
  print('-dpdf', [filename, '.pdf'])
  yl = get(gca, 'ylim');

  if samples
    covfunc_f = gp_cov_se(sq_dist(xh), ...
                          'lengthscale', lengthscale);
    K_f = covfunc_f([]);
    K_post = K_f - K_yf'*(K_y\K_yf) + 1e-6*eye(Nh);
    L_post = chol(K_post, 'lower');
    f = bsxfun(@plus, ...
               K_yf'*(K_y\y), ...
               L_post*randn(Nh,samples));
    figure
    plot(xh, f);
    hold on
    plot(x, y, 'k+');
    set(gca, 'xtick', [], 'ytick', []);
    set(gca, 'box', 'off');
    set(gca, 'ylim', yl);
    set_figure_size(width,height);
    print('-dpdf', [filename, '_samples.pdf'])
  end

end


function [dP_dV, dP_dlam] = cpf_p_jac(parameterization, z, V, lam, Vprv, lamprv, pv, pq)
%CPF_P_JAC Computes partial derivatives of CPF parameterization function.
%   [DP_DV, DP_DLAM ] = CPF_P_JAC(PARAMETERIZATION, Z, V, LAM, ...
%                                                   VPRV, LAMPRV, PV, PQ)
%
%   Computes the partial derivatives of the continuation power flow
%   parameterization function w.r.t. bus voltages and the continuation
%   parameter lambda.
%
%   Inputs:
%       PARAMETERIZATION : Value of cpf.parameterization option.
%       Z : normalized tangent prediction vector from previous step
%       V : complex bus voltage vector at current solution
%       LAM : scalar lambda value at current solution
%       VPRV : complex bus voltage vector at previous solution
%       LAMPRV : scalar lambda value at previous solution
%       PV : vector of indices of PV buses
%       PQ : vector of indices of PQ buses
%
%   Outputs:
%       DP_DV : partial of parameterization function w.r.t. voltages
%       DP_DLAM : partial of parameterization function w.r.t. lambda
%
%   See also CPF_PREDICTOR, CPF_CORRECTOR.

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

if parameterization == 1        %% natural
    npv = length(pv);
    npq = length(pq);
    dP_dV = zeros(1, npv+2*npq);
    if lam >= lamprv
        dP_dlam = 1.0;
    else
        dP_dlam = -1.0;
    end
elseif parameterization == 2    %% arc length
    Va = angle(V);
    Vm = abs(V);
    Vaprv = angle(Vprv);
    Vmprv = abs(Vprv);
    dP_dV = 2*([Va([pv; pq]); Vm(pq)] - [Vaprv([pv; pq]); Vmprv(pq)])';
    if lam == lamprv    %% first step
        dP_dlam = 1.0;  %% avoid singular Jacobian that would result
                        %% from [dP_dV, dP_dlam] = 0
    else
        dP_dlam = 2*(lam-lamprv);
    end
elseif parameterization == 3    %% pseudo arc length
    nb = length(V);
    dP_dV = z([pv; pq; nb+pq])';
    dP_dlam = z(2*nb+1);
end


% General state space model for gps imu integration formulated in n-frame
% either in psi model or phi model. Psi model only appears in some
% papers, Phi model is well documented in books like Jekeli 2000, Titterton
% and Weston, 2000.

% states: IMU position in e frame, IMU velocity in n frame,
% attitude w.r.t n frame, accelerometer and gyro biases, accelerometer and gyro scale factors

% the error states are delta(r x,y,z in earth centered
% "abused" n frame), delta(vx, vy, vz in n frame),
% delta(roll, pitch, yaw) of the imu, accelerometer and gyro bias drift, acc
% scale and gyro scale factor error
% accelerometer and gyro bias are usually modeled as random walk, it can
% also be random constant, This does not make much difference
% acc scale and gyro scale factor are usually modeled as random walk, it can
% also be random constant, This does not make much difference

% The state dynamics are driven by a 18 (accel and gyro white noise,
% bias white noise and scale factor white noise) or 6 (only accel and
% gyro white noise) dimensional white Gaussian
% noise and the observations are corrupted by white Gaussian noise
% test cases: (1) random walk bias and scale facotr error, random constant
% bias and scale factor error,
% (2) covariance update step > sample interval
% (3) different imy types, 3DM GX3-35 AND h764g
% (4) psi and phi model
% There is a very subtle point in implementing a class in matlab. Always
% call a class the same name, do not call it a filter in a function and a
% model at another. It causes strange behaviors.
classdef EKF_filter_nframe < handle
    properties (Hidden)
        type = 'ekf';
        tag  = 'EKF_IMU_GPS_NFRM';  % ID tag
        covDim=9+12; % the dimension of covariance matrix
    end
    % The following properties can be set only by class methods
    properties (SetAccess = private)
        invalidateIMUerrors; % reset IMU errors when they goes beyond expectation
        imuType; % the type of IMU, e.g., MEMS 3DX GM 3-35, HG1700, etc.
        imuErrorModel=3;
        % the IMU error model corresponding to random walk bias and
        % random walk scale factor error,
        % 1 without turn on bias estimates,first order GM bias and random walk scale factor errors
        % 2 with random constant turn on bias estimates, first order GM bias and random walk scale factor errors
        % 3 assumes bias and scale factors are random walks
        % 4 assumes random constant bias and random constant scale factor errors

        % transform from vehicle body frame to IMU sensor frame
        Cb2imu;
        dt; % sampling interval of IMU, unit sec
        % the translation from antenna to IMU sensor frame, i.e., the antenna's
        % position in the s-frame.
        Tant2imu;
        Cen; % rotation from e frame to n frame
        height; % h in geodetic coordinates
        qs2n; % q s(sensor) 2 navigation frame
        Vn; % velocity of the IMU sensor in n frame
        imuErrors; %ba, bg, sa, sg

        imuOrientSIP=7; % imu orientation covariance start index
        imuBiasDriftSIP=10; %imu bias error covariance start index
        imuScaleFactorSIP=16;
        p_k_k; % the covariance of the entire state vector
        mode=1; % 1 for phi model, 2 for psi model
        mechanization=2; % 1 for wander azimuth, 2 for local geodetic
        rvqs2e; % added only for compatibility
        camPose; % added only for compatibility
    end
    methods
        function filter = EKF_filter_nframe(options)
            filter.invalidateIMUerrors=options.InvalidateIMUerrors;
            filter.imuType=options.imutype;
            filter.imuErrorModel=options.imuErrorModel;
            filter.dt=options.dt;
            filter.Cb2imu=options.Cb2imu;
            filter.Tant2imu=filter.Cb2imu*(options.Tant2body-options.Timu2body);
            filter.Vn=options.Vn;
            % initialize states and covariance for imu gps integration,
            % For imu, rs in n, vs in n, q s2n, ba, bg, sa, sg. (s denotes imu)
            % the position of GPS antenna
            inillh=options.inillh_ant;
            % Cb2n initial value given by external reference
            initqbn=options.qb2n;   % H764G INS data, this b refers to the vehicle body frame
            filter.qs2n=quatmult_v001(initqbn,rotro2qr(filter.Cb2imu),2);

            Ce2n0=llh2dcm_v000(inillh(1:2),[0;1]);
            qs2e=quatmult_v001(rotro2qr(Ce2n0), filter.qs2n,1);
            xyz_imu=ecef2geo_v000(inillh,1)-quatrot_v000(qs2e,filter.Tant2imu,0);
            inillh_imu=ecef2geo_v000(xyz_imu, 0);
            filter.height=inillh_imu(3); %elipsoidal height
            filter.Cen=llh2dcm_v000(inillh_imu(1:2),[0;1]);
            filter.imuErrors=options.imuErrors;
            filter.p_k_k=zeros(filter.covDim);
            filter.p_k_k(1:3,1:3)=eye(3)*1^2; %position errors in meter
            filter.p_k_k(4:6,4:6)=eye(3)*0.1^2; %vel error in m/s
            filter.p_k_k(filter.imuOrientSIP+(0:2),filter.imuOrientSIP+(0:2))=...
                diag([options.initAttVar,options.initAttVar,options.initAttVar*2].^2);

            IMU_ERRDEF=imu_err_defs_v000(options.imutype);
            filter.p_k_k(filter.imuBiasDriftSIP+(0:2),filter.imuBiasDriftSIP+(0:2))=4*eye(3)*IMU_ERRDEF.acc_bias_var;% enlarge initial std by 2
            filter.p_k_k(filter.imuBiasDriftSIP+(3:5),filter.imuBiasDriftSIP+(3:5))=4*eye(3)*IMU_ERRDEF.gyro_bias_var;

            filter.p_k_k(filter.imuScaleFactorSIP+(0:2),filter.imuScaleFactorSIP+(0:2))=4*eye(3)*IMU_ERRDEF.acc_scale_var;
            filter.p_k_k(filter.imuScaleFactorSIP+(3:5),filter.imuScaleFactorSIP+(3:5))=4*eye(3)*IMU_ERRDEF.gyro_scale_var;
            filter.mode= options.mode;
        end
        %===============================================================================================
        %-- State transition function
        % propogate state with accelerometer and gyro input at time k-1 to state at k,
        % i.e., X(k|k-1), from state at k-1. U1 contains IMU measurement
        % delta v, and delta angle of gyro or acc and angular rate
        % and 7th row is the previous epoch(k-1) and 8th row is the current epoch(k)
        % imuaccum records the accumulated delta v and delta angle for two speed
        % covariance update
        function imuaccum= ffun_state(filter, imuaccum, U1)
            if(sum(abs(U1(1:3,1)))<1)
                dt1=U1(8,end)-U1(7,1);
                gyroinc=sum(U1(4:6,:),2);
                accinc=sum(U1(1:3,:),2);

                %reset the imu errors if estimates diverges
                IMU_ERRDEF=imu_err_defs_v000(filter.imuType);

                if(filter.invalidateIMUerrors)
                    spuracc=find(abs(filter.imuErrors(1:3))>2*IMU_ERRDEF.initacc_bias_err,1);
                    spurgyro=find(abs(filter.imuErrors(4:6))>2*IMU_ERRDEF.initgyro_bias_err,1);
                    if(~isempty(spuracc)||~isempty(spurgyro))
                        disp(['IMU errors diverge at ' num2str(U1(8,end)) '!']);
                        filter.imuErrors=zeros(12,1);
                    end
                end
                angleinc=gyroinc-filter.imuErrors(4:6)*dt1-diag(gyroinc)*filter.imuErrors(6+(4:6))/1000;
                velinc=accinc-filter.imuErrors(1:3)*dt1-diag(accinc)*filter.imuErrors(6+(1:3))/1000;
                %imu data accumulator for the covariance update
                imuaccum=imuaccum+[velinc;angleinc];
            else
                dt1=U1(8,end)-U1(7,1);
                gyroinc=mean(U1(4:6,:),2);
                accinc=mean(U1(1:3,:),2);

                %reset the imu errors if estimates diverges
                IMU_ERRDEF=imu_err_defs_v000(filter.imuType);

                if(filter.invalidateIMUerrors)
                    spuracc=find(abs(filter.imuErrors(1:3))>2*IMU_ERRDEF.initacc_bias_err,1);
                    spurgyro=find(abs(filter.imuErrors(4:6))>2*IMU_ERRDEF.initgyro_bias_err,1);
                    if(~isempty(spuracc)||~isempty(spurgyro))
                        disp(['IMU errors diverge at ' num2str(U1(8,end)) '!']);
                        filter.imuErrors=zeros(12,1);
                    end
                end
                gyroinc=gyroinc-filter.imuErrors(4:6)-diag(gyroinc)*filter.imuErrors(6+(4:6))/1000;
                accinc=accinc-filter.imuErrors(1:3)-diag(accinc)*filter.imuErrors(6+(1:3))/1000;
                angleinc=gyroinc*dt1;
                velinc=accinc*dt1;
                %imu data accumulator for the covariance update
                imuaccum=imuaccum+[accinc;gyroinc]*dt1;
            end
            %run strapdown with angle and velocity increments
            [filter.qs2n, filter.Vn, filter.Cen, filter.height]=strapdown_Cen_quat_v000(...
                filter.qs2n, filter.Vn, filter.Cen, filter.height, velinc, angleinc, dt1);
        end
        function ffun_covariance(filter, imuaccum, covupt_time, curimutime)
            %propagate the covariance corresponds to states, rs in n, vs in n, \psi or \phi,
            % ba, bg, sa, sg
            covdt=curimutime-covupt_time;
            [STM, Qd]=sys_metric_phipsi_v000(filter.Cen, filter.height, ...
                filter.Vn, filter.qs2n,  imuaccum(1:3)/covdt,imuaccum(4:6)/covdt, ...
                covdt,filter.mode,filter.imuType, filter.imuErrorModel); %use psi implementation with 2
            %              if(filter.imuErrorModel==4)
            %                     [STM, Qd]=sys_metric_phipsi_v002(filter.Cen, filter.height, ...
            %                         filter.Vn, filter.qs2n,  imuaccum(1:3)/covdt,imuaccum(4:6)/covdt, ...
            %                         covdt,filter.mode,filter.imuType); %use psi implementation with 2
            %                     assert(norm(STM1-STM)+norm(Qd1-Qd)<1e-5);
            %              end
            filter.p_k_k=STM*filter.p_k_k*STM'+Qd;  % the covariance of the navigation states and imu error terms
        end
        %==============================================================================================
        %update both the covariance and state
        function correctstates(filter, predict,measure, H,R, ~)
            p_km1_k=filter.p_k_k;
            inno= predict-measure;
            %Kalman
            K=p_km1_k*H'/(H*p_km1_k*H'+R);
            deltaX=K*inno;
            % update covariance
            filter.p_k_k=(eye(filter.covDim)-K*H)*p_km1_k*(eye(filter.covDim)-K*H)'+K*R*K';
            % compute updated state
            [filter.qs2n, filter.Vn, filter.Cen, filter.height]=correctnav_Cen_v000(...
                filter.qs2n, filter.Vn, filter.Cen, filter.height,...
                deltaX(1:filter.imuOrientSIP+2), filter.mode, filter.mechanization);
            filter.imuErrors = filter.imuErrors + deltaX(filter.imuBiasDriftSIP:end);
        end
        % output: position of the IMU in a NED frame anchored at some point or in geodetic coordinates,
        % velocity of the IMU in the body frame, and euler angles of
        % rotation from body frame to the moving NED frame, and covariances
        % of 9 navigation ERROR states in the filter
        function SaveToFile(filter, inillh_ant, preimutime, ffilres)
            vr_c=rotqr2eu('xyz',quatmult_v001(filter.qs2n, rotro2qr(filter.Cb2imu), 0))*180/pi; % body frame w.r.t the moving N frame
            xyz_imu= ecef2geo_v000([asin(-filter.Cen(3,3));asin(-filter.Cen(2,1));filter.height],1);
         %   xyz_ant= xyz_imu + quatrot_v000(quatmult_v001(rotro2qr(filter.Cen),filter.qs2n,1),filter.Tant2imu,0);
            if(~isempty(inillh_ant))
                vr_a=posdiff_v001(xyz_imu, inillh_ant); % position of IMU in the initial N frame
                fwrite(ffilres,[preimutime;vr_a;filter.Vn;vr_c;sqrt(diag(filter.p_k_k(1:9,...
                    1:9)))],'double');
            else
                fwrite(ffilres,[preimutime;ecef2geo_v000(xyz_imu,0);filter.Vn;vr_c;sqrt(diag(filter.p_k_k(1:9,...
                    1:9)))],'double');
            end
        end
        function mag=GetVelocityMag(filter)
            mag=norm(filter.Vn,2);
        end
        function  SetCamAndRvqs2e(filter, options)
            filter.rvqs2e=zeros(10,1);
            Ce2n=filter.Cen;
            heights2n=filter.height;
            Re=6378137/(sqrt(1.0-0.00669437999014*Ce2n(3,3)^2));
            filter.rvqs2e(1:3,1)=-[(Re+heights2n)*Ce2n(3,1);(Re+heights2n)*Ce2n(3,2);(Re*(1-0.00669437999014)+heights2n)*Ce2n(3,3)];
            filter.rvqs2e(4:6,1)=Ce2n'*filter.Vn;
            filter.rvqs2e(7:10,1)=quatmult_v001(rotro2qr(Ce2n),filter.qs2n,1);
            filter.camPose=zeros(7,1);
            filter.camPose(5:7)=options.Cimu2cam*options.Cb2imu*(options.Timu2body-options.Tcam2body);
            filter.camPose(1:4)=rotro2qr(options.Cimu2cam);
        end
    end
end


function [f0v,vrv,dfv,nf,aav]=fixpF0VexMltpBG4(x,fs,f0floor,nvc,nvo,mu,imgi,shiftm,smp,minm,pc,nc)

%	Fixed point analysis to extract F0
%	[f0v,vrv,dfv,nf]=fixpF0VexMltpBG4(x,fs,f0floor,nvc,nvo,mu,imgi,shiftm,smp,minm,pc,nc)
%	x	: input signal
%	fs	: sampling frequency (Hz)
%	f0floor	: lowest frequency for F0 search
%	nvc	: total number of filter channels
%	nvo	: number of channels per octave
%	mu	: temporal stretching factor
%	imgi	: image display indicator (1: display image, default)
%	shiftm	: frame shift in ms
%	smp	: smoothing length relative to fc (ratio)
%	minm	: minimum smoothing length (ms)
%	pc	: exponent to represent nonlinear summation
%	nc	: number of harmonic component to use (1,2,3)

%	Designed and coded by Hideki Kawahara
%	28/March/1999
%	04/April/1999 revised to multi component version
%	07/April/1999 bi-reciprocal smoothing for multi component compensation
%	01/May/1999 first derivative of Amplitude is taken into account
%	17/Dec./2000 display bug fix
%	19/Sep./2002 bug fix (mu information was discarded.)
%   07/Dec./2002 waitbar was added
%	30/April/2005 modification for Matlab v7.0 compatibility
%	10/Aug./2005 modified by Takahashi on waitbar
%	10/Sept./2005 mofidied by Kawahara on waitbar
%   11/Sept./2005 fixed waitbar problem

%f0floor=40;
%nvo=12;
%nvc=52;
%mu=1.1;

x=cleaninglownoise(x,fs,f0floor);

fxx=f0floor*2.0.^((0:nvc-1)/nvo)';
fxh=max(fxx);

dn=max(1,floor(fs/(fxh*6.3)));

if nc>2
	pm3=multanalytFineCSPB(decimate(x,dn),fs/dn,f0floor,nvc,nvo,mu,3,imgi); % error crrect 2002.9.19 (mu was fixed 1.1)
	pif3=zwvlt2ifq(pm3,fs/dn);
	[~,mm]=size(pif3);
	pif3=pif3(:,1:3:mm);
	pm3=pm3(:,1:3:mm);
end;

if nc>1
	pm2=multanalytFineCSPB(decimate(x,dn),fs/dn,f0floor,nvc,nvo,mu,2,imgi);% error crrect 2002.9.19(mu was fixed 1.1)
	pif2=zwvlt2ifq(pm2,fs/dn);
	[~,mm]=size(pif2);
	pif2=pif2(:,1:3:mm);
	pm2=pm2(:,1:3:mm);
end;

pm1=multanalytFineCSPB(decimate(x,dn*3),fs/(dn*3),f0floor,nvc,nvo,mu,1,imgi);% error crrect 2002.9.19(mu was fixed 1.1)
%%%% safe guard added on 15/Jan./2003
mxpm1=max(max(abs(pm1)));
eeps=mxpm1/10000000;
pm1(pm1==0)=pm1(pm1==0)+eeps;
%%%% safe guard end
pif1=zwvlt2ifq(pm1,fs/(dn*3));
%keyboard;

[~,mm1]=size(pif1);
mm=mm1;
if nc>1
	[~,mm2]=size(pif2);
	mm=min(mm1,mm2);
end;

if nc>2
	[~,mm3]=size(pif3);
	mm=min([mm1 mm2 mm3]);
end;

if nc == 2
	for ii=1:mm
		pif2(:,ii)=(pif1(:,ii).*(abs(pm1(:,ii))).^pc ...
			+pif2(:,ii)/2.*(abs(pm2(:,ii))).^pc )...
			./((abs(pm1(:,ii))).^pc+(abs(pm2(:,ii))).^pc);
	end;
end;
if nc == 3
	for ii=1:mm
		pif2(:,ii)=(pif1(:,ii).*(abs(pm1(:,ii))).^pc ...
			+pif2(:,ii)/2.*(abs(pm2(:,ii))).^pc ...
			+pif3(:,ii)/3.*(abs(pm3(:,ii))).^pc )...
			./((abs(pm1(:,ii))).^pc+(abs(pm2(:,ii))).^pc+(abs(pm3(:,ii))).^pc);
	end;
end;
if nc == 1
	pif2=pif1;
end;

%pif2=zwvlt2ifq(pm,fs/dn)*2*pi;
pif2=pif2*2*pi;
dn=dn*3;

[slp,~]=zifq2gpm2(pif2,f0floor,nvo);
[nn,mm]=size(pif2);
dpif=(pif2(:,2:mm)-pif2(:,1:mm-1))*fs/dn;
dpif(:,mm)=dpif(:,mm-1);
[dslp,~]=zifq2gpm2(dpif,f0floor,nvo);

damp=(abs(pm1(:,2:mm))-abs(pm1(:,1:mm-1)))*fs/dn;
damp(:,mm)=damp(:,mm-1);
damp=damp./abs(pm1);

%[c1,c2]=znormwght(1000);
fxx=f0floor*2.0.^((0:nn-1)/nvo)'*2*pi;
mmp=0*dslp;
[c1,c2b]=znrmlcf2(1);
if imgi==1; hpg=waitbar(0,'P/N map calculation'); end; % 07/Dec./2002 by H.K.%10/Aug./2005
for ii=1:nn
%	[c1,c2]=znrmlcf2(fxx(ii)/2/pi); % This is OK, but the next Eq is much faster.
	c2=c2b*(fxx(ii)/2/pi)^2;
	cff=damp(ii,:)/fxx(ii)*2*pi*0;
	mmp(ii,:)=(dslp(ii,:)./(1+cff.^2)/sqrt(c2)).^2+(slp(ii,:)./sqrt(1+cff.^2)/sqrt(c1)).^2;
    if imgi==1; waitbar(ii/nn); end; %,hpg); % 07/Dec./2002 by H.K.%10/Aug./2005
end;
if imgi==1; close(hpg); end;%10/Aug./2005

if smp~=0
	smap=zsmoothmapB(mmp,fs/dn,f0floor,nvo,smp,minm,0.4);
else
	smap=mmp;
end;

fixpp=zeros(round(nn/3),mm);
fixvv=fixpp+100000000;
fixdf=fixpp+100000000;
fixav=fixpp+1000000000;
nf=zeros(1,mm);
if imgi==1; hpg=waitbar(0,'Fixed pints calculation'); end; % 07/Dec./2002 by H.K.%10/Aug./2005
for ii=1:mm
	[ff,vv,df,aa]=zfixpfreq3(fxx,pif2(:,ii),smap(:,ii),dpif(:,ii)/2/pi,pm1(:,ii));
	kk=length(ff);
	fixpp(1:kk,ii)=ff;
	fixvv(1:kk,ii)=vv;
	fixdf(1:kk,ii)=df;
	fixav(1:kk,ii)=aa;
	nf(ii)=kk;
    if imgi==1 && rem(ii,10)==0; waitbar(ii/mm); end;% 07/Dec./2002 by H.K.%10/Aug./2005
end;
if imgi==1; close(hpg); end; % 07/Dec./2002 by H.K.%10/Aug./2005
fixpp(fixpp==0)=fixpp(fixpp==0)+1000000;

%keyboard
%[vvm,ivv]=min(fixvv);
%
%for ii=1:mm
%	ff00(ii)=fixpp(ivv(ii),ii);
%	esgm(ii)=fixvv(ivv(ii),ii);
%end;
np=max(nf);
f0v=fixpp(1:np,round(1:shiftm/dn*fs/1000:mm))/2/pi;
vrv=fixvv(1:np,round(1:shiftm/dn*fs/1000:mm));
dfv=fixdf(1:np,round(1:shiftm/dn*fs/1000:mm));
aav=fixav(1:np,round(1:shiftm/dn*fs/1000:mm));
nf=nf(round(1:shiftm/dn*fs/1000:mm));

if imgi==1
	cnmap(fixpp,smap,fs,dn,nvo,f0floor,shiftm);
end;
%ff00=ff00(round(1:shiftm/dn*fs/1000:mm));
%esgm=sqrt(esgm(round(1:shiftm/dn*fs/1000:mm)));
%keyboard;

return;
%------------------------------------------------------------------
function okid=cnmap(fixpp,smap,fs,dn,nvo,f0floor,shiftm)

% 	This function had a bug in map axis.
%	17/Dec./2000 bug fix by Hideki Kawahara.

dt=dn/fs;
[nn,mm]=size(smap);
aa=figure;
set(aa,'PaperPosition',[0.3 0.25 8 10.9]);
set(aa,'Position',[30 130 520 680]);
subplot(211);
imagesc([0 (mm-1)*dt*1000],[1 nn],20*log10(smap(:,round(1:shiftm/dn*fs/1000:mm))));axis('xy')
hold on;
tx=((1:shiftm/dn*fs/1000:mm)-1)*dt*1000;
plot(tx,(nvo*log(fixpp(:,round(1:shiftm/dn*fs/1000:mm))/f0floor/2/pi)/log(2)+0.5)','ko');
plot(tx,(nvo*log(fixpp(:,round(1:shiftm/dn*fs/1000:mm))/f0floor/2/pi)/log(2)+0.5)','w.');
hold off
xlabel('time (ms)');
ylabel('channel #');
colormap(jet);

okid=1;
return;

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

%%function pm=zmultanalytFineCSPm(x,fs,f0floor,nvc,nvo,mu,mlt);

%       Dual waveleta analysis using cardinal spline manipulation
%               pm=multanalytFineCSP(x,fs,f0floor,nvc,nvo);
%       Input parameters
%
%               x       : input signal (2kHz sampling rate is sufficient.)
%               fs      : sampling frequency (Hz)
%               f0floor : lower bound for pitch search (60Hz suggested)
%               nvc     : number of total voices for wavelet analysis
%               nvo     : number of voices in an octave
%				mu		: temporal stretch factor
%       Outpur parameters
%               pm      : wavelet transform using iso-metric Gabor function
%
%       If you have any questions,  mailto:kawahara@hip.atr.co.jp
%
%       Copyright (c) ATR Human Information Processing Research Labs. 1996
%       Invented and coded by Hideki Kawahara
%       30/Oct./1996

%t0=1/f0floor;
%lmx=round(6*t0*fs*mu);
%wl=2^ceil(log(lmx)/log(2));
%x=x(:)';
%nx=length(x);
%tx=[x,zeros(1,wl)];
%gent=((1:wl)-wl/2)/fs;

%nvc=18;

%wd=zeros(nvc,wl);
%wd2=zeros(nvc,wl);
%ym=zeros(nvc,nx);
%pm=zeros(nvc,nx);
%mpv=1;
%mu=1.0;
%for ii=1:nvc
%  t=gent*mpv;
%  t=t(abs(t)<3.5*mu*t0);
%  wbias=round((length(t)-1)/2);
%  wd1=exp(-pi*(t/t0/mu).^2);%.*exp(i*2*pi*t/t0);
%  wd2=max(0,1-abs(t/t0/mu));
%  wd2=wd2(wd2>0);
%  wwd=conv(wd2,wd1);
%  wwd=wwd(abs(wwd)>0.0001);
%  wbias=round((length(wwd)-1)/2);
%  wwd=wwd.*exp(i*2*pi*mlt*t(round((1:length(wwd))-wbias+length(t)/2))/t0);
%  pmtmp1=fftfilt(wwd,tx);
%  pm(ii,:)=pmtmp1(wbias+1:wbias+nx)*sqrt(mpv);
%  mpv=mpv*(2.0^(1/nvo));
%  keyboard;
%end;
%[nn,mm]=size(pm);
%pm=pm(:,1:mlt:mm);

%----------------------------------------------------------------
function pif=zwvlt2ifq(pm,fs)
%	Wavelet to instantaneous frequency map
%	fqv=wvlt2ifq(pm,fs)

%	Coded by Hideki Kawahara
%	02/March/1999

[~,mm]=size(pm);
pm=pm./(abs(pm));
pif=abs(pm(:,:)-[pm(:,1),pm(:,1:mm-1)]);
pif=fs/pi*asin(pif/2);
pif(:,1)=pif(:,2);

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

function [slp,pbl]=zifq2gpm2(pif,f0floor,nvo)
%	Instantaneous frequency 2 geometric parameters
%	[slp,pbl]=ifq2gpm(pif,f0floor,nvo)
%	slp		: first order coefficient
%	pbl		: second order coefficient

%	Coded by Hideki Kawahara
%	02/March/1999

[nn,~]=size(pif);
fx=f0floor*2.0.^((0:nn-1)/nvo)*2*pi;

c=2.0^(1/nvo);
g=[1/c/c 1/c 1;1 1 1;c*c c 1];
h=inv(g);

%slp=pif(1:nn-2,:)*h(1,1)+pif(2:nn-1,:)*h(1,2)+pif(3:nn,:)*h(1,3);
slp=((pif(2:nn-1,:)-pif(1:nn-2,:))/(1-1/c) ...
    +(pif(3:nn,:)-pif(2:nn-1,:))/(c-1))/2;
slp=[slp(1,:);slp;slp(nn-2,:)];

pbl=pif(1:nn-2,:)*h(2,1)+pif(2:nn-1,:)*h(2,2)+pif(3:nn,:)*h(2,3);
pbl=[pbl(1,:);pbl;pbl(nn-2,:)];

for ii=1:nn
	slp(ii,:)=slp(ii,:)/fx(ii);
	pbl(ii,:)=pbl(ii,:)/fx(ii);
end;

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

%function [c1,c2]=znormwght(n)

%zz=0:1/n:3;
%hh=[diff(zGcBs(zz,0)) 0]*n;
%c1=sum((zz.*hh).^2)/n;
%c2=sum((2*pi*zz.^2.*hh).^2)/n;

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

function p=zGcBs(x,k)

tt=x+0.0000001;
p=tt.^k.*exp(-pi*tt.^2).*(sin(pi*tt+0.0001)./(pi*tt+0.0001)).^2;

%--------------------------------------------
function smap=zsmoothmapB(map,fs,f0floor,nvo,mu,mlim,pex)

[nvc,mm]=size(map);
%mu=0.4;
t0=1/f0floor;
lmx=round(6*t0*fs*mu);
wl=2^ceil(log(lmx)/log(2));
gent=((1:wl)-wl/2)/fs;

smap=map;
mpv=1;
zt=0*gent;
iiv=1:mm;
for ii=1:nvc
	t=gent*mpv; %t0*mu/mpv*1000
	t=t(abs(t)<3.5*mu*t0);
	wbias=round((length(t)-1)/2);
	wd1=exp(-pi*(t/(t0*(1-pex))/mu).^2);
	wd2=exp(-pi*(t/(t0*(1+pex))/mu).^2);
	wd1=wd1/sum(wd1);
	wd2=wd2/sum(wd2);
	tm=fftfilt(wd1,[map(ii,:) zt]);
	tm=fftfilt(wd2,[1.0./tm(iiv+wbias) zt]);
	smap(ii,:)=1.0./tm(iiv+wbias);
	if t0*mu/mpv*1000 > mlim
		mpv=mpv*(2.0^(1/nvo));
	end;
end;

%--------------------------------------------
%function [ff,vv,df]=zfixpfreq2(fxx,pif2,mmp,dfv)
%
%nn=length(fxx);
%iix=(1:nn)';
%cd1=pif2-fxx;
%cd2=[diff(cd1);cd1(nn)-cd1(nn-1)];
%cdd1=[cd1(2:nn);cd1(nn)];
%fp=(cd1.*cdd1<0).*(cd2<0);
%ixx=iix(fp>0);
%ff=pif2(ixx)+(pif2(ixx+1)-pif2(ixx)).*cd1(ixx)./(cd1(ixx)-cdd1(ixx));
%vv=mmp(ixx);
%vv=mmp(ixx)+(mmp(ixx+1)-mmp(ixx)).*(ff-fxx(ixx))./(fxx(ixx+1)-fxx(ixx));
%df=dfv(ixx)+(dfv(ixx+1)-dfv(ixx)).*(ff-fxx(ixx))./(fxx(ixx+1)-fxx(ixx));

%--------------------------------------------
function [ff,vv,df,aa]=zfixpfreq3(fxx,pif2,mmp,dfv,pm)

aav=abs(pm);
nn=length(fxx);
iix=(1:nn)';
cd1=pif2-fxx;
cd2=[diff(cd1);cd1(nn)-cd1(nn-1)];
cdd1=[cd1(2:nn);cd1(nn)];
fp=(cd1.*cdd1<0).*(cd2<0);
ixx=iix(fp>0);
ff=pif2(ixx)+(pif2(ixx+1)-pif2(ixx)).*cd1(ixx)./(cd1(ixx)-cdd1(ixx));
%vv=mmp(ixx);
vv=mmp(ixx)+(mmp(ixx+1)-mmp(ixx)).*(ff-fxx(ixx))./(fxx(ixx+1)-fxx(ixx));
df=dfv(ixx)+(dfv(ixx+1)-dfv(ixx)).*(ff-fxx(ixx))./(fxx(ixx+1)-fxx(ixx));
aa=aav(ixx)+(aav(ixx+1)-aav(ixx)).*(ff-fxx(ixx))./(fxx(ixx+1)-fxx(ixx));

%--------------------------------------------
function [c1,c2]=znrmlcf2(f)

n=100;
x=0:1/n:3;
g=zGcBs(x,0);
dg=[diff(g) 0]*n;
dgs=dg/2/pi/f;
xx=2*pi*f*x;
c1=sum((xx.*dgs).^2)/n*2;
c2=sum((xx.^2.*dgs).^2)/n*2;

%--------------------------------------------
function x=cleaninglownoise(x,fs,f0floor)

flm=50;
flp=round(fs*flm/1000);
nn=length(x);
wlp=fir1(flp*2,f0floor/(fs/2));
wlp(flp+1)=wlp(flp+1)-1;
wlp=-wlp;

tx=[x(:)' zeros(1,2*length(wlp))];
ttx=fftfilt(wlp,tx);
x=ttx((1:nn)+flp);

return;