% Locate place fields in a firing map.
%
% Identifies the place fields in 2D firing map. First map is converted to binary image
% by applying a global threshold. During the second step connected regions are labeled
% by function bwlabel. Final step includes filtering of identified regions and verification
% that they are valid place fields.
%
%  USAGE
%   [fieldsMap, fields] = analyses.palcefield(map, <options>)
%   map           Firing rate map either structure obtained using analyses.map
%                 or a matrix that represents firing map.
%   <options>     optional list of property-value pairs (see table below)
%
%   =========================================================================
%    Properties     Values
%   -------------------------------------------------------------------------
%    'threshold'    value above threshold*peak belong to a field (default = 0.2).
%    'minBins'      Minimum number of bins in a place field, i.e. total number
%                   of bins of place field or an area of place field. Fields with
%                   fewer bins are not considered as place fields. Remember to
%                   adjust this value when you change the bin width.
%                   (default = 9).
%    'minPeak'      peaks smaller than this value are considered spurious
%                   and ignored (default = 1). Peak is normally a rate, however
%                   it's units not necessary are Hz.
%    'binWidth'     Width of the bins in cm. It is used to calculate field size.
%                   (default = 1).
%    'pos'          Position samples. Used to calculate posInd. If not provided,
%                   then posInd will be an empty matrix.
%    'minMeanRate'  Fields with mean rate smaller that this value are ignored (default = 0).
%   =========================================================================
%
%  OUTPUT
%
%   fieldsMap       Matrix of the same size as firing map. The elements of fieldsMap
%                   are integer values greater or equal to 0. The elements labeled 0
%                   are the background (not a place field). The pixels labeled 1 make up
%                   first field; the pixels labeled 2 make up a second object; and so on.
%
%   fields          Structure with information about each field. Structure fields are:
%       row         vector of rows that constitute this field;
%       col         vector of columns that constitute this field;
%       area        area of field measured by regionprops function, i.e. number of bins in the field;
%       bbox        bounding box of the field;
%       peak        field peak value;
%       size        field size in cm; Calculated with help of binWidth argument.
%       x, y        field centre of mass point;
%       meanRate    mean firing rate;
%       PixelIdxList    linear list of indices that constitute the field.
%       peakX       X-coordinate of field's peak
%       peakY       Y-coordinate of field's peak
%       map         Binary map of the field. It has the same size as firing map. The elements of 'map'
%                   equal to one if they belong to the field and are zeros otherwise.
%       posInd      Indices of position samples that correspond to this field. In case position sample
%                   matrix is of size Nx5 ([t x y x1 y1]), posInd corresponds to the left most position
%                   columns ([x y]). If pos argument is not provided, then posInd will be an empty matrix.
%
%  EXAMPLES
%
%   1. To obtain positions of the first field run:
%       [~, fields] = analyses.placefield(map, 'threshold', 0.3, 'minBins', 5, 'minPeak', 0.1, 'pos', pos);
%       fieldPos = pos(fields(1).posInd, :);
%

% Note to developers:
% In order to improve speed and capture all fields the map is processed in two passes. First
% we extract fields based solely on global peak value. This should be enough for most of the cases.
% Secondly we check if extracted fields maxima is the same as results of imregionalmax. If they are
% different, then we perform another extraction, which is slower.
%
function [fieldsMap, fields] = placefield(map, varargin)
    inp = inputParser;
    defaultMinPeak = 1.;
    defaultThreshold = 0.2;
    defaultMinBins = 9;
    defaultBinWidth = 1;
    defaultPos = [];
    defaultMinMeanRate = 0;

    % input argument check functions
    checkThreshold = @(x) helpers.isdscalar(x, '>=0', '<=1');
    checkScalarZero = @(x) helpers.isiscalar(x, '>=0');
    checkDScalar = @(x) helpers.isdscalar(x, '>0');
    checkDScalarZero = @(x) helpers.isdscalar(x, '>=0');

    % fill input parser object
    addRequired(inp, 'map');
    addParameter(inp, 'threshold', defaultThreshold, checkThreshold);
    addParameter(inp, 'minBins', defaultMinBins, checkScalarZero);
    addParameter(inp, 'minPeak', defaultMinPeak, checkDScalarZero);
    addParameter(inp, 'binWidth', defaultBinWidth, checkDScalar);
    addParameter(inp, 'minMeanRate', defaultMinMeanRate, checkScalarZero);
    addParameter(inp, 'pos', defaultPos, @(x) ismatrix(x) && size(x, 2) >= 3);

    parse(inp, map, varargin{:});

    % get parsed arguments
    minPeak = inp.Results.minPeak;
    threshold = inp.Results.threshold;
    minBins = inp.Results.minBins;
    binWidth = inp.Results.binWidth;
    pos = inp.Results.pos;
    minMeanRate = inp.Results.minMeanRate;

    originalMap = [];
    if isstruct(map)
        originalMap = map;
        map = map.z;
    end

    fieldsMap = zeros(size(map));
    fields = struct('row', {}, 'col', {}, ...
        'size', {}, 'peak', {}, 'peakX', {}, 'peakY', {}, ...
        'area', {}, 'bbox', {}, ...
        'x', {}, 'y', {}, ...
        'meanRate', {}, 'PixelIdxList', {}, ...
        'map', {}, 'posInd', {} ...
        );

    if isempty(map)
        return
    end

    globalPeak = nanmax(nanmax(map));
    if isnan(globalPeak) || globalPeak == 0
        return;
    end

    mapNans = isnan(map);
    map(mapNans) = 0;
    regionalMaxMap = imregionalmax(map, 4); % obtain all local maxima
    testRegionalMx = zeros(size(map)); % map that will contain located fields maxima

    [ir, ic] = find(regionalMaxMap > 0);            % get peak locations
    foundPeaks = map(sub2ind(size(map), ir ,ic));   % obtain peaks value

    % remove peaks that have smaller rate than minPeak
    selected = foundPeaks < minPeak;
    regionalMaxMap(ir(selected), ic(selected)) = 0;

    % Counter for the number of fields
    nFields = 0;

    binMap = ones(size(map));

    binMap(map < globalPeak*threshold) = 0;
    binMap(mapNans) = 0;

    cc = bwconncomp(binMap, 4);
    stats = regionprops(cc, 'Area', 'BoundingBox', 'Centroid');

    for i = 1:cc.NumObjects
        linInd = cc.PixelIdxList{i};
        [fieldPeak, peakLinearInd] = max(map(linInd));
        [r, c] = ind2sub(size(map), linInd);
        meanRate = nanmean(map(linInd));

        [pr, pc] = ind2sub(size(map), linInd(peakLinearInd));
        % mark this field as visited even if we reject if further down
        testRegionalMx(pr, pc) = 1;

        if fieldPeak < minPeak
            continue;
        end
        if meanRate < minMeanRate
            continue;
        end

        if length(r) >= minBins
            nFields = nFields + 1;

            fields(nFields).row = r;
            fields(nFields).col = c;
            fields(nFields).size = length(r) * binWidth^2;
            fields(nFields).peak = fieldPeak;
            fields(nFields).peakX = pc;
            fields(nFields).peakY = pr;
            fields(nFields).area = stats(i).Area;
            fields(nFields).bbox = stats(i).BoundingBox;
            fields(nFields).PixelIdxList = linInd;

            fields(nFields).x = stats(i).Centroid(1);
            fields(nFields).y = stats(i).Centroid(2);

            fields(nFields).meanRate = meanRate;
            fields(nFields).map = zeros(size(map));
            fields(nFields).map(linInd) = 1;
            fields(nFields).map(mapNans) = nan;

            % fields(nFields).Extent = stats(i).Extent;
            % fields(nFields).Orientation = stats(i).Orientation;
            % fields(nFields).Eccentricity = stats(i).Eccentricity;
            % fields(nFields).MinorAxisLength = stats(i).MinorAxisLength;
            % fields(nFields).MajorAxisLength = stats(i).MajorAxisLength;
            % fields(nFields).Perimeter = stats(i).Perimeter;

            if ~isempty(pos)
                intRect = ceil(fields(nFields).bbox); % bounding box with integers
                if isempty(originalMap)
                    % we do not have position distribution from the map, so assume that it's based
                    % on data min/max values
                    nBins = size(map);
                    limitsX = [nanmin(pos(:, bntConstants.PosX)) nanmax(pos(:, bntConstants.PosX))];
                    limitsY = [nanmin(pos(:, bntConstants.PosY)) nanmax(pos(:, bntConstants.PosY))];
                    xSpace = linspace(limitsX(1), limitsX(2), nBins(2));
                    ySpace = linspace(limitsY(1), limitsY(2), nBins(1));
                else
                    xSpace = originalMap.x;
                    ySpace = originalMap.y;
                end

                xMin = xSpace(intRect(1));
                xMax = xSpace(intRect(1) + intRect(3) - 1);

                yMin = ySpace(intRect(2));
                yMax = ySpace(intRect(2) + intRect(4) - 1);

                posIndX = pos(:, bntConstants.PosX) >= xMin & pos(:, bntConstants.PosX) <= xMax;
                posIndY = pos(:, bntConstants.PosY) >= yMin & pos(:, bntConstants.PosY) <= yMax;
                fields(nFields).posInd = find(posIndX & posIndY);
            else
                fields(nFields).posInd = [];
            end

            fieldsMap(linInd) = nFields;
        end
    end

    if ~isequaln(testRegionalMx, regionalMaxMap)
        map(fieldsMap > 0) = 0; % turn off map values for known fields, prevent field duplicates

        % we have some uncounted fields
        leftPeaksMap = regionalMaxMap - testRegionalMx;
        [ir, ic] = find(leftPeaksMap > 0);                    % get peak locations
        foundPeaks = map(sub2ind(size(leftPeaksMap), ir ,ic));   % obtain peaks value
        mapThresholds = foundPeaks * threshold;
        if sum(foundPeaks) == 0
            return;
        end

        finalMap = zeros(size(map));
        for i = 1:length(foundPeaks)
            binMap = zeros(size(map));
            binMap( (map > mapThresholds(i)) & (map <= foundPeaks(i)) ) = 1;
            binMap(mapNans) = 0;

            [binMap, linInd] = bwselect(binMap, ic(i), ir(i), 4); % leave only region that relates to current field
            if isempty(linInd)
                continue;
            end

            % check for minimum number of bins
            [r, ~] = ind2sub(size(map), linInd);
            if length(r) < minBins
                continue;
            end

            stats = regionprops(binMap, 'Centroid', 'EulerNumber'); % find statistics on field candidates
            distToPeak = sqrt((stats.Centroid(1) - ic(i))^2 + (stats.Centroid(2) - ir(i))^2);
    %         distToPeak = pdist2(stats.Centroid, [ic(i) ir(i)]);
            if distToPeak > 4 || stats.EulerNumber < 1 % we want object without holes (euler number)
                continue;
            end
            finalMap(linInd) = 1;
        end

        cc = bwconncomp(finalMap, 4); % somehow regionprops is not working if finalMap is passed directly
        stats = regionprops(cc, 'Area', 'BoundingBox', 'Centroid');

        for fieldInd = 1:length(stats)
            linInd = cc.PixelIdxList{fieldInd};
            [r, c] = ind2sub(size(map), linInd);
            meanRate = nanmean(map(linInd));
            [peakRate, peakInd] = nanmax(map(linInd));
            [pr, pc] = ind2sub(size(map), linInd(peakInd));

            if peakRate < minPeak
                continue;
            end
            if meanRate < minMeanRate
                continue;
            end

            nFields = nFields + 1;

            fields(nFields).row = r;
            fields(nFields).col = c;
            fields(nFields).size = length(r) * binWidth^2;
            fields(nFields).peak = peakRate;
            fields(nFields).peakX = pc;
            fields(nFields).peakY = pr;
            fields(nFields).area = stats(fieldInd).Area;
            fields(nFields).bbox = stats(fieldInd).BoundingBox;
            fields(nFields).PixelIdxList = linInd;

            fields(nFields).x = stats(fieldInd).Centroid(1);
            fields(nFields).y = stats(fieldInd).Centroid(2);

            fields(nFields).meanRate = meanRate;
            fields(nFields).map = zeros(size(map));
            fields(nFields).map(linInd) = 1;
            fields(nFields).map(mapNans) = nan;

            if ~isempty(pos)
                intRect = ceil(fields(nFields).bbox); % bounding box with integers
                if isempty(originalMap)
                    % we do not have position distribution from the map, so assume that it's based
                    % on data min/max values
                    nBins = size(map);
                    limitsX = [nanmin(pos(:, bntConstants.PosX)) nanmax(pos(:, bntConstants.PosX))];
                    limitsY = [nanmin(pos(:, bntConstants.PosY)) nanmax(pos(:, bntConstants.PosY))];
                    xSpace = linspace(limitsX(1), limitsX(2), nBins(2));
                    ySpace = linspace(limitsY(1), limitsY(2), nBins(1));
                else
                    xSpace = originalMap.x;
                    ySpace = originalMap.y;
                end

                xMin = xSpace(intRect(1));
                xMax = xSpace(intRect(1) + intRect(3) - 1);

                yMin = ySpace(intRect(2));
                yMax = ySpace(intRect(2) + intRect(4) - 1);

                posIndX = pos(:, bntConstants.PosX) >= xMin & pos(:, bntConstants.PosX) <= xMax;
                posIndY = pos(:, bntConstants.PosY) >= yMin & pos(:, bntConstants.PosY) <= yMax;
                fields(nFields).posInd = find(posIndX & posIndY);
            else
                fields(nFields).posInd = [];
            end

            fieldsMap(linInd) = nFields;
        end
    end
end


function F = stream1d(A,vector)
%% Stream function for any D1Qn Lattice
% There are only 2 basic movements and any other is a combination of the
% basic ones:

% Matlab equivalent solution:
% F = circshift(A,[y,x])

%% Evaluate input data

v = length(vector);

    x = vector(2);
    y = vector(1);

n = length(A);

    b = zeros(1,n);

%% Stream in XY plane

if x == 1       % x+ stream
    b = A(:,n);
    for i = n:-1:2;
        A(:,i) = A(:,i-1);
    end
    A(:,1) = b;
elseif x == -1  % x- stream
    b = A(:,1);
    for i = 1:n-1;
        A(:,i) = A(:,i+1);
    end
    A(:,n) = b;
else            % x=0 stream
    %do nothing
end

F = A;


clear all
close all
path(path,'..\..\..\FUZZCLUST')
%data set
data.X = nDexample(5,400,2,1)

%parameters
param.c=3;
param.m=2;
param.e=1e-6;
param.ro=ones(1,param.c);
param.val=1;
%normalization
data=clust_normalize(data,'range');

result = GKclust(data,param);
result = validity(result,data,param);

plot(data.X(:,1),data.X(:,2),'b.',result.cluster.v(:,1),result.cluster.v(:,2),'ro');
hold on
%draw contour-map
new.X=data.X;
eval=clusteval(new,result,param);
result.validity


function Offspring = Operator(Problem,Parent1,Parent2)
% Crossover and mutation used in MOEA/D-M2M
% proM --- 1 --- The expectation of number of bits doing mutation

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    %% Parameter setting
    Parent1 = Parent1.decs;
    Parent2 = Parent2.decs;
    [N,D]   = size(Parent1);

    %% Crossover
    rc = (2*rand(N,1)-1).*(1-rand(N,1).^(-(1-Problem.FE/Problem.maxFE).^0.7));
    OffDec = Parent1 + repmat(rc,1,D).*(Parent1-Parent2);

    %% Mutation
    rm    = 0.25*(2*rand(N,D)-1).*(1-rand(N,D).^(-(1-Problem.FE/Problem.maxFE).^0.7));
    Site  = rand(N,D) < 1/D;
    Lower = repmat(Problem.lower,N,1);
    Upper = repmat(Problem.upper,N,1);
    OffDec(Site) = OffDec(Site) + rm(Site).*(Upper(Site)-Lower(Site));

	%% Set the infeasible decision variables to feasible values
    temp1 = OffDec < Lower;
    temp2 = OffDec > Upper;
    rnd   = rand(N,D);
    OffDec(temp1) = Lower(temp1) + 0.5*rnd(temp1).*(Parent1(temp1)-Lower(temp1));
    OffDec(temp2) = Upper(temp2) - 0.5*rnd(temp2).*(Upper(temp2)-Parent1(temp2));
    Offspring     = Problem.Evaluation(OffDec);
end


% DESCRIPTION:
%       subscript to create the absorption variables
%
% ABOUT:
%       author      - Bradley Treeby
%       date        - 26th November 2010
%       last update - 11th February 2014
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox

% This file is part of k-Wave. k-Wave is free software: you can
% redistribute it and/or modify it under the terms of the GNU Lesser
% General Public License as published by the Free Software Foundation,
% either version 3 of the License, or (at your option) any later version.
%
% k-Wave is distributed in the hope that it will be useful, but WITHOUT ANY
% WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for
% more details.
%
% You should have received a copy of the GNU Lesser General Public License
% along with k-Wave. If not, see <http://www.gnu.org/licenses/>.

% define the lossy derivative operators and proportionality coefficients
if strcmp(equation_of_state, 'absorbing')

    % make sure the operators are positive and real
    medium.alpha_coeff = abs(real(medium.alpha_coeff));
    medium.alpha_power = abs(real(medium.alpha_power));

    % convert the absorption coefficient to nepers.(rad/s)^-y.m^-1
    medium.alpha_coeff = db2neper(medium.alpha_coeff, medium.alpha_power);

    % compute the absorbing fractional Laplacian operator and coefficient
    if ~(isfield(medium, 'alpha_mode') && strcmp(medium.alpha_mode, 'no_absorption'))
        absorb_nabla1 = (kgrid.k).^(medium.alpha_power-2);
        absorb_nabla1(isinf(absorb_nabla1)) = 0;
        absorb_nabla1 = ifftshift(absorb_nabla1);
        absorb_tau = -2*medium.alpha_coeff.*medium.sound_speed.^(medium.alpha_power - 1);
    else
        absorb_nabla1 = 0;
        absorb_tau = 0;
    end

    % compute the dispersive fractional Laplacian operator and coefficient
    if ~(isfield(medium, 'alpha_mode') && strcmp(medium.alpha_mode, 'no_dispersion'))
        absorb_nabla2 = (kgrid.k).^(medium.alpha_power-1);
        absorb_nabla2(isinf(absorb_nabla2)) = 0;
        absorb_nabla2 = ifftshift(absorb_nabla2);
        absorb_eta = 2*medium.alpha_coeff.*medium.sound_speed.^(medium.alpha_power)*tan(pi*medium.alpha_power/2);
    else
        absorb_nabla2 = 0;
        absorb_eta = 0;
    end

    % pre-filter the absorption parameters if alpha_filter is defined (this
    % is used for time-reversal photoacoustic image reconstruction
    % with absorption compensation)
    if isfield(medium, 'alpha_filter');

        % update command line status
        disp('  filtering absorption variables...');

        % frequency shift the absorption parameters
        absorb_nabla1 = fftshift(absorb_nabla1);
        absorb_nabla2 = fftshift(absorb_nabla2);

        % apply the filter
        absorb_nabla1 = absorb_nabla1.*medium.alpha_filter;
        absorb_nabla2 = absorb_nabla2.*medium.alpha_filter;

        % shift the parameters back
        absorb_nabla1 = ifftshift(absorb_nabla1);
        absorb_nabla2 = ifftshift(absorb_nabla2);

    end

    % modify the sign of the absorption operators if alpha_sign is defined
    % (this is used for time-reversal photoacoustic image reconstruction
    % with absorption compensation)
    if isfield(medium, 'alpha_sign')
       if numel(medium.alpha_sign) == 2
           % if two parameters are given, apply the first to the absorption
           % parameter and the second to the disperion parameters
           absorb_tau = sign(medium.alpha_sign(1))*absorb_tau;
           absorb_eta = sign(medium.alpha_sign(2))*absorb_eta;
       else
           error('medium.alpha_sign must be given as a 2 element array controlling absorption and dispersion, respectively.');
       end
    end
end


function mmldemo
% MMLDEMO is a guided tour about using MML.
disp(' ')
disp('  MMLDEMO')
disp(' ')
disp('  MMLDEMO demonstrates the capabilities of the MML function and how to use it.')
disp('  This demo guides through a number of examples showing the syntactical')
disp('  variations MML can be used with.')
disp(' ')
disp('  To run properly this demo requires:')
disp('  1.) Symbolic Math Toolbox 3.0.1 for MATLAB with Maple 8 Kernel or later.')
disp('  2.) To display the expression in MS Internet Explorer install MATHPLAYER.')
disp('      Download MATHPLAYER (freeware) at')
disp('      http://www.dessci.com/webmath/mathplayer')
disp(' ')
disp('[Press any key to continue with Example 1]')
pause
disp('  ')
disp('  Example 1: MML(TERM)')
disp('  ')
disp('  The following line in the MATLAB Command Window displays a mathematical')
disp('  expression in the system''s standard webbrowser. In this example the area')
disp('  function of a circle is entered for TERM. When the line below is executed')
disp('  the webbrowser opens and displays the term.')
disp('  ')
disp('  >>mml(pi/4*d^2)')
disp('  ')
disp('[Press any key to demonstrate]')
pause
disp('  ')
disp('  Now the webbrowser opens and displays the term.')
mml('pi/4*d^2')
disp('  ')
disp('  ')
disp('[Press any key to continue]')
pause
disp('  ')
disp('  In the example above the term was entered as a symbolic MATLAB expression.')
disp('  Using symbolic expressions is subject to certain restrictions of MATLAB syntax.')
disp('  For example it is not valid to enter equations, like')
disp('  >>mml(a=pi/4*d^2)')
disp('  To avoid some restrictions it is possible to enter the term as a character ')
disp('  string using apostrophes, like in the following example. The character string is')
disp('  passed to the Symbolic Toolbox''s Maple Kernel for interpretation. The term has ')
disp('  to be entered in valid Maple or MATLAB syntax.')
disp('  ')
disp('  >>mml(''a=pi/4*d^2'')')
disp('  ')
disp('[Press any key to demonstrate]')
pause
disp('  ')
disp('  Now the webbrowser opens and displays the term.')
mml('a=pi/4*d^2')
disp('  ')
disp('  ')
disp('[Press any key to continue with Example 2]')
pause
disp('  ')
disp('  Example 2: MML')
disp('  ')
disp('  A useful feature of MML is the possibility of displaying the current content ')
disp('  of the global variable ANS by simply typing MML.')
disp('  To demonstrate this we assume that ANS contains i.e. a symbolic matrix.')
disp('  ')
disp('  >> [x1 x2 x3; y1 y2 y3]')
disp('  ')
disp('  ans =')
disp('  ')
disp('  [ x1, x2, x3]')
disp('  [ y1, y2, y3]')
disp('  ')
disp('  >>mml')
disp('  ')
disp('[Press any key to demonstrate]')
pause
disp('  ')
disp('  Now the webbrowser opens and displays the content of ANS, here: the matrix.')
mml('[x1 x2 x3; y1 y2 y3]')
disp('  ')
disp('  ')
disp('[Press any key to continue]')
pause
disp('  ')
disp('  Example 3: Options')
disp('  ')
disp('  This example demonstrates the use of the display options ''type'' and ''nwb''.')
disp('  Both options can be used in each syntax variation of the MML function.')
disp('  If MathML code typed in the Command Window is needed in addition to displaying')
disp('  the term in the webbrowser, the option ''type'' can be used.')
disp('  ')
disp('  >>mml(sqrt(a),''type'')')
disp('  ')
disp('[Press any key to demonstrate]')
pause
disp('  ')
disp('  Now the webbrowser is opened displaying the term and the corresponding ')
disp('  MathML code is typed in the Command Window.')
disp('  ')
disp('  ')
mml('sqrt(a)','type')
disp('  ')
disp('  ')
disp('[Press any key to continue]')
pause
disp('  ')
disp('  The following example shows the use of the ''type'' option with the syntax ')
disp('  of EXAMPLE 2: MML.')
disp('  ')
disp('  >> sqrt(a)')
disp('  ')
disp('  ans =')
disp('  ')
disp('  sqrt(a)')
disp('  ')
disp('  >>mml(''type'')')
disp('  ')
disp('[Press any key to demonstrate]')
pause
disp('  ')
disp('  Now the webbrowser is opened displaying the content of ANS ')
disp('  and the corresponding MathML code is typed in the Command Window.')
disp('  ')
disp('  ')
mml('sqrt(a)','type')
disp('  ')
disp('  ')
disp('[Press any key to continue]')
pause
disp('  ')
disp('  If only MathML code typed in the Command Window is needed instead of displaying')
disp('  the term in the webbrowser, the option ''nwb''("No Web Browser") can be used.')
disp('  ')
disp('  >>mml(sqrt(a),''nwb'')')
disp('  ')
disp('[Press any key to demonstrate]')
disp('  ')
pause
disp('  The MathML code of the term is now typed in the Command Window.')
disp('  The webbrowser is not opened.')
disp('  ')
disp('  ')
mml('sqrt(a)','nwb')
disp('  ')
disp('  ')
disp('[Press any key to continue]')
pause
disp('  ')
disp('  The following example shows the use of the ''nwb'' option with the syntax ')
disp('  of EXAMPLE 2: MML.')
disp('  ')
disp('  >> sqrt(a)')
disp('  ')
disp('  ans =')
disp('  ')
disp('  sqrt(a)')
disp('  ')
disp('  >>mml(''nwb'')')
disp('  ')
disp('[Press any key to demonstrate]')
disp('  ')
pause
disp('  Now the MathML code of the content of ANS is typed in the Command Window.')
disp('  The webbrowser is not opened.')
disp('  ')
disp('  ')
mml('sqrt(a)','nwb')
disp('  ')
disp('  ')
disp('[Press any key to continue]')
pause
disp('  ')
disp('  Example 4: "Torture Test"')
disp('  ')
disp('  The following example demonstrates a term with higher complexity. ')
disp('  The displayed term makes no mathematical sense. It has been choosen to contain')
disp('  a number of different mathematical aspects.')
disp('  ')
disp('  >>mml(''g(x+y)<=asinh(exp(15^(1/3)*pi^factorial(x)))/(a*x^2-b*x+c)+2/3-2.5*y'')')
disp('  ')
disp('[Press any key to demonstrate]')
disp('  ')
pause
mml('g(x+y)<=asinh(exp(15^(1/3)*pi^factorial(x)))/(a*x^2-b*x+c)+2/3-2.5*y')
disp('  ')
disp('  End of MMLDEMO.')
disp('  ')
disp('  See also: help MML ')


function check_roi_extraction()

% test ROI extraction
mask_image = fmri_data(which('atlas_labels_combined.img'));
dat = mask_image;
wh_region = dat.dat;

% create timeseries data -- add data "after" 1st image
nimgs = 50;

n = unique(wh_region);
ts = linspace(-10, 10, nimgs);
v = size(dat.dat, 1);
dat.dat = zeros(v, nimgs);

for i = 1:length(n)
    my_indx = wh_region == n(i);

    dat.dat(my_indx, :) = repmat(ts .* sqrt(i), sum(my_indx), 1);
end

fprintf('\nDATA GENERATED\n\n');

cl1 = extract_roi_averages(dat, mask_image, 'unique_mask_values');

% extracted average values for all the regions in mask_image.
all_reg1 = cat(2, cl1(:).dat);

dat.fullpath = fullfile(pwd, 'test_image.img');
write(dat);
fprintf('\nSAVED DATA TO %s\n\n', dat.fullpath);

% reload
fprintf('\nLOADING DATA FROM %s\n\n', dat.fullpath);
dat = fmri_data(dat.fullpath, which('gray_matter_mask.img'));

% region obj -> extract_data
% test extract_roi_averages
cl2 = extract_roi_averages(dat, mask_image, 'unique_mask_values');

% extracted average values for all the regions in mask_image.
all_reg2 = cat(2, cl2(:).dat);

% uniq_reg = sort(unique(mask_image.dat));

% compare roi averages of initially data generated and data loaded from file
if is_equal(all_reg1, all_reg2, 0.001)
    fprintf('\nExtracted ROI averages are equal with an error threshold of 0.001\nPASS\n');
else
    fprintf('\nExtracted ROI averages are not equal with an error threshold of 0.001\nFAIL\n');
end

% ------------------------------------------------------
% INLINE FUNCTION
% ------------------------------------------------------

    function res = is_equal(A, B, error_threshold)
        err = abs((A-B)./B) <= error_threshold;

        res = all(err(:));
    end % is_equal

end  % main function


function tf=in_range(value, R)
    % in_range returns true where  value in inverval [A B)  , where  R = [A B];
    %
    % example:
    %  >> in_range( 1:5 , [2 4])  % returns logical aray [0 1 1 0 0]
    %
    % see also in_range_inclusive, in_range_exclusive

    if numel(R)==2
        tf = R(1) <= value & value < R(2);
    else
        error('range should be a 1x2 vector of [minval maxval]');
    end
end


function jed = yjf_to_jed_gregorian ( y, j, f )

%*****************************************************************************80
%
%% YJF_TO_JED_GREGORIAN converts a Gregorian YJF date to a JED.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, J, real F, the YJF date.
%
%    Output, real JED, the Julian Ephemeris Date.
%

%
%  Copy the input.
%
  y1 = y;
  j1 = j;
  f1 = f;
%
%  Check the input.
%
  [ y1, j1, ierror ] = yj_check_gregorian ( y1, j1 );

  if ( ierror ~= 0 )
    jed = -1.0;
    return
  end
%
%  Convert the input.
%
  [ y2, m2, d2, f2 ] = yjf_to_ymdf_gregorian ( y1, j1, f1 );

  jed = ymdf_to_jed_gregorian ( y2, m2, d2, f2 );

  return
end


function[z]=celladd(x,y)
%CELLADD  Addition acting on each element in a cell array.
%
%   Z=CELLADD(X,Y) where X and Y are both cell arrays of N arrays, with
%   corresponding elements in X and in Y having the same size, returns the
%   cell array Z containing their sums,
%
%       Z{1}=X{1}+Y{1}, Z{2}=X{2}+Y{2},..., Z{N}=X{N}+Y{N}.
%
%   One of X or Y may also be a scalar or a numeric array of the same
%   length as the other input argument.
%
%   Usage: z=celladd(x,y);
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2008--2019 J.M. Lilly --- type 'help jlab_license' for details

if ~iscell(x)&&iscell(y)
    if length(x)==1
        x=x.*ones(length(y),1);
    else
        x=x(:);
    end
elseif iscell(x)&&~iscell(y)
    if length(y)==1
        y=y.*ones(length(x),1);
    else
        y=y(:);
    end
end

if iscell(x)
    z=x;
else
    z=y;
end

for i=1:length(y)
     if iscell(x)&&iscell(y)
         z{i}=x{i}+y{i};
     elseif ~iscell(x)&&iscell(y)
         z{i}=x(i)+y{i};
     elseif iscell(x)&&~iscell(y)
         z{i}=x{i}+y(i);
     end
end


function otherImage = rot90(this, K)
% (Multiple of) 90 deg image rotation; mimicks rot90 in matlab functionality
%
%   Y = MrDataNd()
%   Y.rot90(K)
%
% This is a method of class MrDataNd.
%
% IN
%   K   multiple of 90 degrees used for counterclockwise rotation
%       i.e.    K = 0,1,2 (counterclockwise rotation)
%           or  K = -1,-2,-3,... (clockwise rotation);
%       default : 1
%
% OUT
%   otherImage      MrDataNd where data matrix is rotated and header is updated to
%                   reflect that change
%
% EXAMPLE
%   Y = MrDataNd();
%   Y.rot90(1); % rotate all slices counterclockwise by 90 degrees
%   Y.rot90(-2) % rotate all slices clockwise by 2*90 = 180 degrees
%
%   See also MrDataNd categorical/rot90

% Author:   Saskia Klein & Lars Kasper
% Created:  2014-08-04
% Copyright (C) 2014 Institute for Biomedical Engineering
%                    University of Zurich and ETH Zurich
%
% This file is part of the TAPAS UniQC Toolbox, which is released
% under the terms of the GNU General Public Licence (GPL), version 3.
% You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version).
% For further details, see the file COPYING or
%  <http://www.gnu.org/licenses/>.

if nargin < 2
    K = 1;
end

otherImage = this.perform_unary_operation(@(x) rot90(x, K));

% First and second dimension change through rotation around 1, 3 etc.
% multiples of 90 degree...
% doSwapDimensions = mod(K,2) == 1;
% TODO: or shall this be reflected in affineTransformation?
% No, not in affine transformation! We rather take the stance that this is really
% a change of the data and if one wants to adapt the dimInfo, she has to do
% it actively.
doSwapDimensions = false;

if doSwapDimensions
    otherImage.dimInfo.permute([2 1]);
end


function [ n, indx ] = r8vec_indexed_heap_d_insert ( n, a, indx, indx_insert )

%*****************************************************************************80
%
%% R8VEC_INDEXED_HEAP_D_INSERT: insert value into heap descending indexed R8VEC.
%
%  Discussion:
%
%    An R8VEC is a vector of R8's.
%
%    An indexed R8VEC is an R8VEC of data values, and an R8VEC of N indices,
%    each referencing an entry of the data vector.
%
%    Note that the argument N must be a variable, and will be incremented before
%    return, and that INDX must be able to hold one more entry on output than
%    it held on input.
%
%    This is one of three functions needed to model a priority queue.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 August 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Thomas Cormen, Charles Leiserson, Ronald Rivest,
%    Introduction to Algorithms,
%    MIT Press, 2001,
%    ISBN: 0262032937,
%    LC: QA76.C662.
%
%  Parameters:
%
%    Input, integer N, the number of items in the
%    index vector.
%
%    Input, real A(*), the data vector.
%
%    Input, integer INDX(N), the index vector.
%
%    Input, integer INDX_INSERT, the index in A of the value
%    to be inserted into the heap.
%
%    Output, integer N, the number of items in the revised index vector.
%
%    Output, integer INDX(N), the revised index vector.
%
  n = n + 1;
  i = n;

  while ( 1 < i )

    parent = floor ( i / 2 );

    if ( a(indx_insert) <= a(indx(parent)) )
      break
    end

    indx(i) = indx(parent);
    i = parent;

  end

  indx(i) = indx_insert;

  return
end


% @Author: amishkin
% @Date:   2018-07-10T14:56:18-07:00
% @Email:  amishkin@cs.ubc.ca
% @Last modified by:   aaronmishkin
% @Last modified time: 2018-07-25T13:29:13-07:00

% ###############################################
% ############# Implements mf-exact #############
% ###############################################

function [nlz, log_loss, Sigma, mu] = mf_exact(method_name, y, X, gamma, y_te, X_te, options, mu_start, sigma_start)

    fprintf('%s\n',method_name);
    [N,D] = size(X);

    % set default options
    [max_iters, lowerBoundTol, display, num_samples, beta_start, alpha_start, decay_rate, mini_batch_size] = myProcessOptions(options, ...
    'max_iters', 2000, 'lowerBoundTol',1e-4, 'display', 1, 'num_samples', 1, 'beta', 0.1, 'alpha', 0.8, 'decay_rate', 0.55, 'mini_batch_size', N);

    % minfunc options
    optMinFunc = struct('display', display,...
        'Method', 'lbfgs',...
        'DerivativeCheck', 'off',...
        'LS', 2,...
        'recordPath', 1, ...
        'recordPathIters', 1, ...
        'MaxIter', max_iters+1,...
        'MaxFunEvals', max_iters+1,...
        'TolFun', lowerBoundTol,......
        'TolX', lowerBoundTol);

    V = sigma_start;
    m = mu_start;

    v0 = [m; sqrt(diag(V))];
    funObj = @funObj_mfvi_exact;

    % compute loss at iter =0
    post_dist.mean = m;
    post_dist.covMat =V;
    iter = 0;
    [pred, log_lik]=get_loss(iter, post_dist, X, y, gamma, X_te, y_te);
    nlz(iter+1)=-log_lik;%nlz0
    log_loss(iter+1)=pred.log_loss;%log_loss0

    % optimize using minfunc
    [v, f, exitflag, inform] = minFunc(funObj, v0, optMinFunc, y, X, gamma);
    v_all = inform.trace.x;

    % compute loss for iter>0
    for ii=1:size(v_all,2)
        vi = v_all(:,ii);
        post_dist.mean = vi(1:D);
        U = diag(vi(D+1:end));
        post_dist.covMat = U'*U;
        if sum(eig(post_dist.covMat) <= 1e-8) > 0
            ALERT = 'NOT PD'
            nlz(ii) = NaN;
            log_loss(ii) = NaN;
            continue
        end
    end
    [pred, log_lik] = get_loss(ii-1, post_dist, X, y, gamma, X_te, y_te);
    nlz =-log_lik;
    log_loss = pred.log_loss;

    Sigma = post_dist.covMat;
    mu = post_dist.mean;
end


%%
close all; clc;
addpath('./scripts');
col = {[0 114 178],[0 158 115], [213 94 0],[230 159 0],...
    [86 180 233], [204 121 167], [64 224 208], [240 228 66]};
figure('papersize', [14, 7]);
init_fig;


function test_suite = test_regression1

%   Run specific demo and save values for comparison.
%
%   See also
%     TEST_ALL, DEMO_REGRESSION1

initTestSuite;

  function testDemo
    % Set random number stream so that test failing isn't because randomness.
    % Run demo & save test values.
    prevstream=setrandstream(0);

    disp('Running: demo_regression1')
    demo_regression1
    Eft_map = Eft_map(1:50);
    Varft_map = Varft_map(1:50);
    Eft_ia = Eft_ia(1:50);
    Varft_ia = Varft_ia(1:50);
    Eft_mc = Eft_mc(1:50);
    Varft_mc = Varft_mc(1:50);
    path = which('test_regression1.m');
    path = strrep(path,'test_regression1.m', 'testValues');
    if ~(exist(path, 'dir') == 7)
      mkdir(path)
    end
    path = strcat(path, '/testRegression1');
    save(path, 'K', 'C', 'w', 'Eft_map', 'Varft_map', ...
      'Eft_ia', 'Varft_ia', 'Eft_mc', 'Varft_mc');

    % Set back initial random stream
    setrandstream(prevstream);
    drawnow;clear;close all

% Test saved values with multiple tests. Covariance matrices and
% optimized parameters are tested with zero tolerance, while mean and
% variances from various approximations are tested with relative tolerance
% (5% for MCMC and 1% for grid and IA)

    function testCovarianceMatrices
        values.real = load('realValuesRegression1.mat','K','C');
        values.test = load(strrep(which('test_regression1.m'), 'test_regression1.m', 'testValues/testRegression1.mat'),'K','C');
        assertElementsAlmostEqual(values.real.K, values.test.K);
        assertElementsAlmostEqual(values.real.C, values.test.C);

    function testOptimizedParameter
        values.real = load('realValuesRegression1.mat','w');
        values.test = load(strrep(which('test_regression1.m'), 'test_regression1.m', 'testValues/testRegression1.mat'),'w');
        assertElementsAlmostEqual(values.real.w, values.test.w, 'relative', 0.1);

    function testPredictedMeanVarianceGrid
        values.real = load('realValuesRegression1.mat','Eft_map','Varft_map');
        values.test = load(strrep(which('test_regression1.m'), 'test_regression1.m', 'testValues/testRegression1.mat'),'Eft_map','Varft_map');
        if length(values.test.Eft_map) > 50
            assertElementsAlmostEqual(values.real.Eft_map(1:50), values.test.Eft_map(1:50), 'relative', 0.1);
            assertElementsAlmostEqual(values.test.Varft_map(1:50), values.real.Varft_map(1:50), 'relative', 0.1);
        else
            assertElementsAlmostEqual(values.test.Eft_map, values.real.Eft_map, 'relative', 0.1);
            assertElementsAlmostEqual(values.test.Varft_map, values.real.Varft_map, 'relative', 0.1);
        end

    function testPredictedMeanVarianceMC
        values.real = load('realValuesRegression1.mat','Eft_mc','Varft_mc');
        values.test = load(strrep(which('test_regression1.m'), 'test_regression1.m', 'testValues/testRegression1.mat'),'Eft_mc','Varft_mc');
        if length(values.test.Eft_mc) > 50
            assertElementsAlmostEqual(mean(mean(values.test.Eft_mc(1:50))), mean(mean(values.real.Eft_mc(1:50))), 'relative', 0.1);
            assertElementsAlmostEqual(mean(values.test.Varft_mc(1:50)), mean(values.real.Varft_mc(1:50)), 'absolute', 0.3);
       else
            assertElementsAlmostEqual(mean(mean(values.test.Eft_mc)), mean(mean(values.real.Eft_mc)), 'relative', 0.1);
            assertElementsAlmostEqual(mean(mean(values.test.Varft_mc)), mean(mean(values.real.Varft_mc)), 'absolute', 0.3);
        end

    function testPredictedMeanVarianceIA
        values.real = load('realValuesRegression1.mat','Eft_ia','Varft_ia');
        values.test = load(strrep(which('test_regression1.m'), 'test_regression1.m', 'testValues/testRegression1.mat'),'Eft_ia','Varft_ia');
        if length(values.test.Eft_ia) > 50
            assertElementsAlmostEqual(mean(values.test.Eft_ia(1:50)), mean(values.real.Eft_ia(1:50)), 'relative', 0.1);
            assertElementsAlmostEqual(mean(values.test.Varft_ia(1:50)), mean(values.real.Varft_ia(1:50)), 'relative', 0.1);
        else
            assertElementsAlmostEqual(mean(values.test.Eft_ia), mean(values.real.Eft_ia), 'relative', 0.1);
            assertElementsAlmostEqual(mean(values.test.Varft_ia), mean(values.real.Varft_ia), 'relative', 0.1);
        end


function [s, d, S_rf, S_sf, S_k, S_l] = projHmgLinIntoPinHoleOnRob(Rf, Sf, Sk, l)

% PROJHMGLININTOPINHOLEONROB Project Hmg line into pinhole on robot.
%   [s, d] = PROJHMGLININTOPINHOLEONROB(Rf, Sf, Sk, l) projects the
%   inverse-depth line l into a pin/hole camera with intrinsic parameters
%   Sk mounted on a robot. The robot frame is Rf and the sensor frame in
%   the robot is Sf.
%
%   The results are a 2D segment S and a depths vector D.
%
%   [s, d, S_rf, S_sf, S_k, S_l] = (...) returns the Jacobians wrt all
%   input parameters.

%   Copyright 2009 Teresa Vidal.

if nargout <= 2

    sw = hmgLin2seg(l);
    s   = projSegLinIntoPinHoleOnRob(Rf, Sf, Sk, sw);
    d   = 1./l([6 9]);

else

    [sw, SW_l] = hmgLin2seg(l);
    [s, d, S_rf, S_sf, S_k, S_sw] = projSegLinIntoPinHoleOnRob(Rf, Sf, Sk, sw);
    S_l  = S_sw*SW_l;

end

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


report_this_filefun(mfilename('fullpath'));

dy = 0.15;
dx = 0.15 ;
dz = 0.30 ;
ni = 150;

%a.Longitude = (a.Longitude - mean(a.Longitude))*111;
%a.Latitude = (a.Latitude - mean(a.Latitude))*111;
xc = 0; yc = 0; zc =0 ;
x0 = min(a.Longitude); x1 = max(a.Longitude);
y0 = min(a.Latitude); y1 = max(a.Latitude);
z0 = min(a.Depth); z1 = max(a.Depth);
xv = x0:dx:x1;yv= y0:dy:y1;zv = z0:dz:z1;
bvg = ones(length(x0:dx:x1),length(y0:dy:y1),length(z0:dz:z1));
ra = ones(length(x0:dx:x1),length(y0:dy:y1),length(z0:dz:z1));
itotal = length(x0:dx:x1)*length(y0:dy:y1)*length(z0:dz:z1)

wai = waitbar(0,' Please Wait ...  ');
set(wai,'NumberTitle','off','Name','b-value grid - percent done');;
drawnow

allcount = 0;
i2 = 0;
for x = x0:dx:x1
    xc = xc+1;
    for y = y0:dy:y1
        yc = yc+1;
        for z = z0:dz:z1
            zc = zc+1;
            allcount = allcount + 1.;
            i2 = i2+1;

            % calculate distance from center point and sort wrt distance
            l = sqrt(((a.Longitude - x)).^2 + ((a.Latitude - y).^2 + ((a.Depth - z)).^2)) ;
            [s,is] = sort(l);
            b = a(is(:,1),:) ;       % re-orders matrix to agree row-wise

            % take first ni points
            b = b(1:ni,:);      % new data per grid point (b) is sorted in distance

            % call the b-value function
            [bv magco stan av me mer me2,  pr] =  bvalca3(b,inb1,inb2);
            l2 = sort(l);
            b2 = b;
            %if inb2 ==  1
            %l = b(:,6) >= magco;
            %b2 = b(l,:);
            %end
            %[av2 bv2 stan2 ] =  bmemag(b2);
            bvg(xc,yc,zc) = bv;
            ra(xc,yc,zc) = l2(ni);
            waitbar(allcount/itotal)
        end   % for z
        zc = 0;
    end   % for y
    yc = 0;
end   % for x
xc = 0;

close(wai)

bv2 = bvg;
bvg = bv2;
l = ra > 2.500;
bvg(l)=nan;
figure
[X,Y,Z] = meshgrid(yv,xv,zv);
zs = [z0:4*dz:z1]
sl = slice(X,Y,Z,bvg,[x0+3*dx],[y0+4*dy],zs)
clf
sl1 = slice(X,Y,Z,bvg,x1-6*dx,y1-6*dy,[ z0+4*dz 9 14])
hold on
%plot3(a.Latitude,a.Longitude,a.Depth,'k.','MarkerSize',2)
rotate3d on
caxis([1.8 2.8])
set(gca,'XLim',[-3 3 ],'xgrid','off')
set(gca,'YLim',[-3 3 ],'ygrid','off')
set(gca,'ZLim',[  -1 20 ],'zgrid','off')
colormap(h)
shading interp
cob = colorbar('vert')
set(cob,'TickDir','out','pos',[0.8 0.3 0.07 0.3])
set(gca,'Box','on','vis','on')
tmp = ra*nan;
tmp(1,1,1) = 0;
tmp(1,1,2) = 1;
hold on
sl = slice(X,Y,Z,tmp,x1-6*dx,y1-6*dy,[ z0+4*dz 9 14 ])
set(sl(:),'EdgeColor','w')
caxis([1.8 2.8])
view([-36 10])


%#eml
function [min,med,max] = get_median_2d(inbuf)

[nrows ncols] = size(inbuf);

tbuf = inbuf;

for ii=eml.unroll(1:nrows)
    tbuf(ii, :) = get_median_1d(inbuf(ii, :));
end

for jj=eml.unroll(1:ncols)
    tbuf(:, jj) = get_median_1d(tbuf(:, jj)')';
end

min = tbuf(1, 1);
med = tbuf(ceil(nrows/2), ceil(ncols/2));
max = tbuf(nrows, ncols);

end


function cnn = cnnupdateweights(cnn)
%CNNUPDATEWEIGHTS Updates the weights and biases of a cnn.
%   CNN = CNNUPDATEWEIGHTS(CNN) Updates the kernel weights and biases of
%   convolutional neural net CNN.
%
%   Copyright 2002-2020 Gatesmark
%
%   This function, and other functions in the DIPUM Toolbox, are based
%   on the theoretical and practical foundations established in the
%   book Digital Image Processing Using MATLAB, 3rd ed., Gatesmark
%   Press, 2020.
%
%   Book website: http://www.imageprocessingplace.com
%   License: https://github.com/dipum/dipum-toolbox/blob/master/LICENSE.txt

% Number of layers.
Lc = numel([cnn.NumLayerKernels]);

% All the alphas are the same.
alpha = cnn(1).Alpha;

% Update cnn weights and biases.
for k = 1:Lc
   for j = 1:cnn(k).NumOutputMaps
      for i = 1:cnn(k).NumInputMaps
         % The following implement Eqs. (14-65) and (14-66) in DIPUM3E.
         cnn(k).Kernel{i}{j} = cnn(k).Kernel{i}{j} - alpha*cnn(k).WeightGradient{i}{j};
      end
         cnn(k).Bias{j} = cnn(k).Bias{j} - alpha*cnn(k).BiasGradient{j};
   end
end


function digest = md5(message)
    % digest = md5(message)
    %  Compute the MD5 digest of the message, as a hexadecimal digest.
    %  based on the source code from https://rosettacode.org/wiki/MD5

    % Follow the MD5 algorithm from RFC 1321 [1] and Wikipedia [2].
    %  [1] http://tools.ietf.org/html/rfc1321
    %  [2] http://en.wikipedia.org/wiki/MD5

    % m is the modulus for 32-bit unsigned arithmetic.
    m = 2 ^ 32;

    % s is the shift table for circshift(). Each shift is negative
    % because it is a left shift.
    s = [-7, -12, -17, -22
         -5,  -9, -14, -20
         -4, -11, -16, -23
         -6, -10, -15, -21];

    % t is the sine table. Each sine is a 32-bit integer, unsigned.
    t = floor(abs(sin(1:64)) .* m);

    % Initialize the hash, as a row vector of 32-bit integers.
    digest = [hex2dec('67452301') ...
              hex2dec('EFCDAB89') ...
              hex2dec('98BADCFE') ...
              hex2dec('10325476')];

    % If message contains characters, convert them to ASCII values.
    message = double(message);
    bytelen = numel(message);

    % Pad the message by appending a 1, then appending enough 0s to make
    % the bit length congruent to 448 mod 512. Because we have bytes, we
    % append 128 '10000000', then append enough 0s '00000000's to make
    % the byte length congruent to 56 mod 64.
    message = [message, 128, zeros(1, mod(55 - bytelen, 64))];

    % Convert the message to 32-bit integers, little endian.
    % For little endian, first byte is least significant byte.
    message = reshape(message, 4, numel(message) / 4);
    message = message(1,:) + ...            % least significant byte
              message(2,:) * 256 + ...
              message(3,:) * 65536 + ...
              message(4,:) * 16777216;      % most significant byte

    % Append the bit length as a 64-bit integer, little endian.
    bitlen = bytelen * 8;
    message = [message, mod(bitlen, m), mod(bitlen / m, m)];

    % Process each 512-bit block. Because we have 32-bit integers, each
    % block has 16 elements, message(k + (0:15)).
    for k = 1:16:numel(message)
        % Copy hash.
        a = digest(1); b = digest(2); c = digest(3); d = digest(4);

        % Do 64 operations.
        for i = (1:64)
            % Convert b, c, d to row vectors of bits (0s and 1s).
            bv = dec2bin(b, 32) - '0';
            cv = dec2bin(c, 32) - '0';
            dv = dec2bin(d, 32) - '0';

            % Find f  = mix of b, c, d.
            %      ki = index in 0:15, to message(k + ki).
            %      sr = row in 1:4, to s(sr, :).
            if i <= 16          % Round 1
                f = (bv & cv) | (~bv & dv);
                ki = i - 1;
                sr = 1;
            elseif i <= 32      % Round 2
                f = (bv & dv) | (cv & ~dv);
                ki = mod(5 * i - 4, 16);
                sr = 2;
            elseif i <= 48      % Round 3
                f = xor(bv, xor(cv, dv));
                ki = mod(3 * i + 2, 16);
                sr = 3;
            else                % Round 4
                f = xor(cv, bv | ~dv);
                ki = mod(7 * i - 7, 16);
                sr = 4;
            end

            % Convert f, from row vector of bits, to 32-bit integer.
            f = bin2dec(char(f + '0'));

            % Do circular shift of sum.
            sc = mod(i - 1, 4) + 1;
            sum = mod(a + f + message(k + ki) + t(i), m);
            sum = dec2bin(sum, 32);
            sum = circshift(sum, [0, s(sr, sc)]);
            sum = bin2dec(sum);

            % Update a, b, c, d.
            temp = d;
            d = c;
            c = b;
            b = mod(b + sum, m);
            a = temp;
        end %for i

        % Add hash of this block to hash of previous blocks.
        digest = mod(digest + [a, b, c, d], m);
    end %for k

    % Convert hash from 32-bit integers, little endian, to bytes.
    digest = [digest                % least significant byte
              digest / 256
              digest / 65536
              digest / 16777216];   % most significant byte
    digest = reshape(mod(floor(digest), 256), 1, numel(digest));

    % Convert hash to hexadecimal.
    digest = dec2hex(digest);
    digest = lower(reshape(transpose(digest), 1, numel(digest)));
end %md5


function CPDpot = convert_dbn_CPDs_to_pots(bnet, evidence, pot_type, softCPDpot)
% CONVERT_DBN_CPDS_TO_POTS Convert CPDs of (possibly instantiated) DBN nodes to potentials
% CPDpot = convert_dbn_CPDs_to_pots(bnet, evidence, pot_type, softCPDpot)
%
% CPDpot{n,t} is a potential containing P(n,t|pa(n,t), ev)
% softCPDpot{n,t} is a potential containing P(n,t|pa(n,t), ev) insted of using n's CPD

[ss T] = size(evidence);

if nargin < 4, softCPDpot = cell(ss,T); end
CPDpot = softCPDpot;

% Convert CPDs of instantiated nodes to potential form
t = 1;
for n=1:ss
  fam = family(bnet.dag, n);
  e = bnet.equiv_class(n, 1);
  if isempty(softCPDpot{n,t})
    CPDpot{n,t} = convert_to_pot(bnet.CPD{e}, pot_type, fam(:), evidence(:,1));
  end
end
for n=1:ss
  fam = family(bnet.dag, n, 2);
  e = bnet.equiv_class(n, 2);
  for t=2:T
    if isempty(softCPDpot{n,t})
      CPDpot{n,t} = convert_to_pot(bnet.CPD{e}, pot_type, fam(:), evidence(:,t-1:t));
    end
  end
end


function rmsData=RmsFin(rmsWindow,rmsStride,foldedData)
%%
rmsData=foldedData;
for i =1:length(foldedData)
    %data is trial*channel*time
    data=permute(foldedData(i).x,[3 2 1]);
    [nTrials,nChannel,nTime]=size(data);
    for ii=1:rmsStride:nTime-rmsWindow
        window=zeros(nTrials,nChannel,rmsWindow);
        window=data(:,:,ii:ii+rmsWindow);
        %% rms
        windowSquare=window.*window;
        windowRms=sqrt(sum(windowSquare,3)/rmsWindow);
        if ii==1
            windowFin=windowRms;
            time=1;
        else
            time=cat(2,time,ii);
            windowFin=cat(3,windowFin,windowRms);
        end
    end
    windowFin=permute(windowFin,[3 2 1]);
    rmsData(i).x=windowFin;
    rmsData(i).t=time;
end


function [hIm, ww] = L1SR(lIm, zooming, patch_size, overlap, Dh, Dl, lambda, regres)
% Use sparse representation as the prior for image super-resolution
% Usage
%       [hIm] = L1SR(lIm, zooming, patch_size, overlap, Dh, Dl, lambda)
%
% Inputs
%   -lIm:           low resolution input image, single channel, e.g.
%   illuminance
%   -zooming:       zooming factor, e.g. 3
%   -patch_size:    patch size for the low resolution image
%   -overlap:       overlap among patches, e.g. 1
%   -Dh:            dictionary for the high resolution patches
%   -Dl:            dictionary for the low resolution patches
%   -regres:       'L1' use the sparse representation directly to high
%                   resolution dictionary;
%                   'L2' use the supports found by sparse representation
%                   and apply least square regression coefficients to high
%                   resolution dictionary.
% Ouputs
%   -hIm:           the recovered image, single channel
%
% Written by Jianchao Yang @ IFP UIUC
% April, 2009
% Webpage: http://www.ifp.illinois.edu/~jyang29/
% For any questions, please email me by jyang29@uiuc.edu
%
% Reference
% Jianchao Yang, John Wright, Thomas Huang and Yi Ma. Image superresolution
% as sparse representation of raw image patches. IEEE Computer Society
% Conference on Computer Vision and Pattern Recognition (CVPR), 2008.
%

[lhg, lwd] = size(lIm);
hhg = lhg*zooming;
hwd = lwd*zooming;

mIm = imresize(lIm, 2,'bicubic');
[mhg, mwd] = size(mIm);
hpatch_size = patch_size*zooming;
mpatch_size = patch_size*2;

% extract gradient feature from lIm
hf1 = [-1,0,1];
vf1 = [-1,0,1]';
hf2 = [1,0,-2,0,1];
vf2 = [1,0,-2,0,1]';

lImG11 = conv2(mIm,hf1,'same');
lImG12 = conv2(mIm,vf1,'same');
lImG21 = conv2(mIm,hf2,'same');
lImG22 = conv2(mIm,vf2,'same');

lImfea(:,:,1) = lImG11;
lImfea(:,:,2) = lImG12;
lImfea(:,:,3) = lImG21;
lImfea(:,:,4) = lImG22;

lgridx = 2:patch_size-overlap:lwd-patch_size;
lgridx = [lgridx, lwd-patch_size];
lgridy = 2:patch_size-overlap:lhg-patch_size;
lgridy = [lgridy, lhg-patch_size];

mgridx = (lgridx - 1)*2 + 1;
mgridy = (lgridy - 1)*2 + 1;

% using linear programming to find sparse solution
bhIm = imresize(lIm, zooming, 'bicubic');
hIm = zeros([hhg, hwd]);
nrml_mat = zeros([hhg, hwd]);

hgridx = (lgridx-1)*zooming + 1;
hgridy = (lgridy-1)*zooming + 1;

disp('Processing the patches sequentially...');
count = 0;

%ProjM = inv(Dl'*Dl+0.001*eye(size(Dl,2)))*Dl';

% loop to recover each patch
for xx = 1:length(mgridx),
    for yy = 1:length(mgridy),

        mcolx = mgridx(xx);
        mrowy = mgridy(yy);

        count = count + 1;
%         if ~mod(count, 10000),
%             fprintf('.\n');
%         else
%             fprintf('.');
%         end;
        if ~mod(count, 1000),
            fprintf('%g/%g\n',count,length(mgridx)*length(mgridy));
        end
        mpatch = mIm(mrowy:mrowy+mpatch_size-1, mcolx:mcolx+mpatch_size-1);
        mmean = mean(mpatch(:));

        mpatchfea = lImfea(mrowy:mrowy+mpatch_size-1, mcolx:mcolx+mpatch_size-1, :);
        mpatchfea = mpatchfea(:);

        mnorm = sqrt(sum(mpatchfea.^2));

        if mnorm > 1,
            y = mpatchfea./mnorm;
        else
            y = mpatchfea;
        end;
        %w = ProjM*y;
        w = SolveLasso(Dl, y, size(Dl, 2), 'nnlasso', [], lambda);
        %w = feature_sign(Dl, y, lambda*2);

        if isempty(w),
            w = zeros(size(Dl, 2), 1);
        end;
        switch regres,
            case 'L1'
                if mnorm > 1,
                    hpatch = Dh*w*mnorm;
                else
                    hpatch = Dh*w;
                end;
            case 'L2'
                idx = find(w);
                lsups = Dl(:, idx);
                hsups = Dh(:, idx);
                w = inv(lsups'*lsups)*lsups'*mpatchfea;
                hpatch = hsups*w;
            otherwise
                error('Unknown fitting!');
        end;

        hpatch = reshape(hpatch, [hpatch_size, hpatch_size]);
        hpatch = hpatch + mmean;

        hcolx = hgridx(xx);
        hrowy = hgridy(yy);

        hIm(hrowy:hrowy+hpatch_size-1, hcolx:hcolx+hpatch_size-1)...
            = hIm(hrowy:hrowy+hpatch_size-1, hcolx:hcolx+hpatch_size-1) + hpatch;
        nrml_mat(hrowy:hrowy+hpatch_size-1, hcolx:hcolx+hpatch_size-1)...
            = nrml_mat(hrowy:hrowy+hpatch_size-1, hcolx:hcolx+hpatch_size-1) + 1;
    end;
end;

fprintf('done!\n');

% fill the empty
hIm(1:3, :) = bhIm(1:3, :);
hIm(:, 1:3) = bhIm(:, 1:3);

hIm(end-2:end, :) = bhIm(end-2:end, :);
hIm(:, end-2:end) = bhIm(:, end-2:end);

nrml_mat(nrml_mat < 1) = 1;
hIm = hIm./nrml_mat;
% hIm = uint8(hIm);


function f_x = ParFor4(in1)
%PARFOR4
%    F_X = PARFOR4(IN1)

%    This function was generated by the Symbolic Math Toolbox version 8.2.
%    20-Sep-2019 09:35:35

u = in1(:,1);
ux = in1(:,4);
uxx = in1(:,5);
uxxx = in1(:,6);
f_x = ((u.*1.807785219463578e16+ux.*2.87844038948516e19+uxx.*1.111806867891814e17-uxxx.*7.848202243764388e17-u.*ux.*2.277629423136014e19-u.*uxxx.*3.623747593487516e18+4.461317998952448e15).*-1.0)./(u.*6.942494202789888e15-2.602608832707297e17);


classdef mme_reserve_gen < mp.mm_element

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

%     properties
%     end

    methods
        function name = name(obj)
            name = 'reserve_gen';
        end

        function obj = add_vars(obj, mm, nm, dm, mpopt)
            dme = obj.data_model_element(dm);
            mm.add_var('R', dme.n, 0, 0, dme.r_ub);
        end

        function obj = add_costs(obj, mm, nm, dm, mpopt)
            dme = obj.data_model_element(dm);
            c = dme.tab.cost(dme.on) * dm.base_mva;    %% p.u. cost coeffs
            mm.add_quad_cost('Rcost', [], c, 0, {'R'});
        end

        function obj = add_constraints(obj, mm, nm, dm, mpopt)
            dme = obj.data_model_element(dm);
            gen_dme = dm.elements.gen;
            ng = gen_dme.n; %% number of online generators
            ngr = dme.n;    %% number of online reserve gens
            igr = dme.gen;  %% index of online gen for each online reserve gen

            Ar = [sparse(1:ngr, igr, 1, ngr, ng) speye(ngr)];
            ur = gen_dme.pg_ub(igr);

            mm.add_lin_constraint('Pg_plus_R', Ar, [], ur, {'Pg', 'R'});
        end

        function obj = data_model_update(obj, mm, nm, dm, mpopt)
            dme = obj.data_model_element(dm);
            rz_dme = dm.elements.reserve_zone;

            %% get reserve allocations, bounds, shadow prices, cost
            [vv, ll] = mm.get_idx('var', 'lin');
            r = mm.soln.x(vv.i1.R:vv.iN.R) * dm.base_mva;
            r_lb = 0;
            r_ub = dme.r_ub * dm.base_mva;
            zprc = zeros(rz_dme.nr, 1);
            zprc(rz_dme.on) = mm.soln.lambda.mu_l(ll.i1.Rreq:ll.iN.Rreq) / dm.base_mva;
            prc = rz_dme.tab.zones(:, dme.tab.gen)' * zprc;
            mu_lb = mm.soln.lambda.lower(vv.i1.R:vv.iN.R) / dm.base_mva;
            mu_ub = mm.soln.lambda.upper(vv.i1.R:vv.iN.R) / dm.base_mva;
            mu_pg_ub = mm.soln.lambda.mu_u(ll.i1.Pg_plus_R:ll.iN.Pg_plus_R) / dm.base_mva;
            total_cost = mm.eval_quad_cost(mm.soln.x, 'Rcost');

            %% update in the data model
            dme.tab.r(dme.on) = r;
            dme.tab.r_lb(dme.on) = r_lb;
            dme.tab.r_ub(dme.on) = r_ub;
            dme.tab.prc = prc;
            dme.tab.mu_lb(dme.on) = mu_lb;
            dme.tab.mu_ub(dme.on) = mu_ub;
            dme.tab.mu_pg_ub(dme.on) = mu_pg_ub;
            dme.tab.total_cost(dme.on) = total_cost;
        end
    end     %% methods
end         %% classdef


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Superposed Hawkes Processes: various source_num, D = 5
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear
run setup
D = 5;
K = [2, 5, 10];
N = 5;
nTest = 10;
rate = 0;

Err = zeros(4, length(K), N, nTest);

para = cell(length(K), nTest);
Seqs = cell(K(end), nTest);
models = cell(4, length(K), N, nTest);

alg.LowRank = 0;
alg.Sparse = 0;
alg.GroupSparse = 0;
alg.outer = 10;
alg.rho = 0.1;
alg.inner = 10;
alg.thres = 1e-2;
alg.Tmax = [];
alg.w = 1;

algmu = alg;
algmu.Sparse = 1;
algmu.alphaS = 0.1;
algmu.w = 1;

options.N = 20; % the number of sequences
options.Nmax = 200; % the maximum number of events per sequence
options.Tmax = 100; % the maximum size of time window
options.tstep = 0.1;
options.dt = 0.1;
options.M = 250;
options.GenerationNum = 200;

for n = 1:nTest
    A = rand(D, D);
    A(A<0.4) = 0;
    A = A + eye(D);
    A = 0.5*A./max(abs(eig(A)));
    L = zeros(K(end),1);
    for k = 1:K(end)
        para{k, n}.kernel = 'exp';
        para{k, n}.landmark = 0;
        para{k, n}.w = 1;
        para{k, n}.mu = zeros(D,1);
        id = randperm(D);
        para{k, n}.mu(id(1)) = 0.1+rand;
        para{k, n}.A = reshape(A, [D, 1, D]);
        Seqs{k, n} = SimulationFast_Thinning_ExpHP(para{k, n}, options);
        L(k) = length(Seqs{k, n});
    end

    for k = 1:length(K)%:-1:1
        model.A = rand(D,1,D)./(D^2);
        model.mu = rand(D,1)./D;
        model.kernel = 'exp';
        model.w = 1;
        model.landmark = 0;

        for nn = 1:N

            num = min([nn*(options.N/N); L]);
            seq_sum = Seqs{1, n}(1:num);
            seq_super = Seqs{1,n}(1:num);
            for kk = 2:K(k)
                seq_sum = [seq_sum, Seqs{kk, n}(1:num)];
                seq_super = SuperPosition(seq_super, ...
                                             Seqs{kk, n}(1:num), rate);
            end

%             seq_super = [];
%             for m = 1:K(k)
%                 seq_super_tmp = Seqs{1, n}(1:num);
%                 for kk = 2:K(k)
%                     tmp = Seqs{kk, n}(1:num);
%                     index = randperm(num);
%                     seq_super_tmp = SuperPosition(seq_super_tmp, ...
%                                             Seqs{kk, n}(index), rate);
%                 end
%                 seq_super = [seq_super, seq_super_tmp];
%             end

            models{1, k, nn, n} = model;
            models{1, k, nn, n} = Learning_WLS_HP( ...
                Seqs{1, n}(1:num), models{1, k, nn, n}, alg, 'singlesource' );

            models{2, k, nn, n} = model;
            models{2, k, nn, n} = Learning_WLS_HP( ...
                seq_sum, models{2, k, nn, n}, alg, 'singlesource' );

            model2 = model;
            model2.mu = rand(D, num*K(k))./D;
            models{3, k, nn, n} = model2;
            models{3, k, nn, n} = Learning_WLS_HP( ...
                seq_sum, models{3, k, nn, n}, algmu, 'multisource' );

            models{4, k, nn, n} = model;
            alg2 = alg;
            alg2.w = 1/sqrt(K(k));
            models{4, k, nn, n} = Learning_WLS_HP( ...
                seq_super, models{4, k, nn, n}, alg2, 'singlesource' );

            Err(1, k, nn, n) = norm(models{1, k, nn, n}.A(:) - ...
                A(:))/norm(A(:));

            Err(2, k, nn, n) = norm(models{2, k, nn, n}.A(:) - ...
                A(:))/norm(A(:));

            Err(3, k, nn, n) = norm(models{3, k, nn, n}.A(:) - ...
                A(:))/norm(A(:));

            Err(4, k, nn, n) = norm(models{4, k, nn, n}.A(:) - ...
                A(:))/norm(A(:));

%             A1 = models{1,k,nn,n}.A;
%             A2 = models{2,k,nn,n}.A;
%             imagesc([A,reshape(A1,[D,D]),reshape(A2,[D,D])]);
%             axis tight;axis equal;colorbar

        end
    end
end

save('Result_superpose_error_syn_WLS_D5.mat', 'Err', 'models', ...
'para', 'options', 'K');


function h = polarerrorpatch(theta,r,r_err,color,varargin)

[isclosed,leftovers] = myparse_nocheck(varargin,'isclosed',true);

if isclosed,
  r = [r,r(1)];
  r_err = [r_err,r_err(1)];
  theta = [theta,theta(1)];
end

% bounds
plus_x = max(0,(r+r_err)).*cos(theta);
plus_y = max(0,(r+r_err)).*sin(theta);
minus_x = max(0,(r-r_err)).*cos(theta);
minus_y = max(0,(r-r_err)).*sin(theta);

% edge of patch
err_x = [plus_x,fliplr(minus_x)];
err_y = [plus_y,fliplr(minus_y)];

h = patch(err_x,err_y,color,leftovers{:});


 Dataflow {
     // This is an Eyeriss-like row-stationary dataflow”
    SpatialMap(1,1) Y';
    TemporalMap(1,1) X';
    TemporalMap(1,1) C;
    TemporalMap(16,16) K;
    TemporalMap(Sz(R),Sz(R)) R;
    TemporalMap(Sz(S),Sz(S)) S;
    Cluster(Sz(R),P);
    SpatialMap(1,1) Y;
    SpatialMap(1,1) R;
    TemporalMap(Sz(S),Sz(S)) S;
}


function Ypred = continuous_prediction_2class(Y,Ypred)
% Which one is the most likely class, out of a continuous estimation?
[N,q] = size(Y); % N is time points
L = length(unique(Y(:)));
if q == 1 && L == 2 % class is -1 or +1
    Ypred(Ypred<=0) = -1;
    Ypred(Ypred>0) = +1;
elseif q > 1 && L == 2 % dummy variable representation for classification
    [~,m] = max(Ypred,[],2);
    Ypred = zeros(size(Ypred));
    ind = sub2ind(size(Ypred),1:N, m');
    Ypred(ind) = 1;
end
end


function ar=lpcrand(p,n,bw)
% generate n random stable polynomials of order p with a minimum pole
% bandwidth of bw*fs where fs is the sampling fequency.
% To limit the pole radius to r set bw=-log(r)/pi
% bw may be a vector specifying a different max bandwidth for each row

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: lpcrand.m,v 1.4 2007/05/04 07:01:39 dmb Exp $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin < 3
   bw=0;
   if nargin < 2
      n=1;
   end
end
if p
   if ~bw
      ar=lpcrf2ar(2*rand(n,p+1)-1);
   else
      k=exp(-pi*bw(:)*(0:p));
      if size(k,1)==1
         ar=lpcrf2ar(2*rand(n,p+1)-1).*k(ones(n,1),:);
      else
         ar=lpcrf2ar(2*rand(n,p+1)-1).*k;
      end
   end
else
   ar=ones(n,1);
end


function c = tapas_ehgf_ar1_binary_config
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Contains the configuration for the enhanced Hierarchical Gaussian Filter (eHGF)
% for binary inputs in the absence of perceptual uncertainty.
%
% The HGF is the model introduced in
%
% Mathys C, Daunizeau J, Friston, KJ, and Stephan KE. (2011). A Bayesian foundation
% for individual learning under uncertainty. Frontiers in Human Neuroscience, 5:39.
%
% The binary HGF model has since been augmented with a positive factor kappa1 which
% scales the second level with respect to the first, i.e., the relation between the
% first and second level is
%
% p(x1=1|x2) = s(kappa1*x2), where s(.) is the logistic sigmoid.
%
% By default, kappa1 is fixed to 1, leading exactly to the model introduced in
% Mathys et al. (2011).
%
% This file refers to BINARY inputs (Eqs 1-3 in Mathys et al., (2011));
% for continuous inputs, refer to tapas_ehgf_config.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% The HGF configuration consists of the priors of parameters and initial values. All priors are
% Gaussian in the space where the quantity they refer to is estimated. They are specified by their
% sufficient statistics: mean and variance (NOT standard deviation).
%
% Quantities are estimated in their native space if they are unbounded (e.g., the omegas). They are
% estimated in log-space if they have a natural lower bound at zero (e.g., the sigmas).
%
% Parameters can be fixed (i.e., set to a fixed value) by setting the variance of their prior to
% zero. Aside from being useful for model comparison, the need for this arises whenever the scale
% and origin at the j-th level are arbitrary. This is the case if the observation model does not
% contain the representations mu_j and sigma_j. A choice of scale and origin is then implied by
% fixing the initial value mu_j_0 of mu_j and either kappa_j-1 or omega_j-1.
%
% Fitted trajectories can be plotted by using the command
%
% >> tapas_ehgf_binary_plotTraj(est)
%
% where est is the stucture returned by tapas_fitModel. This structure contains the estimated
% perceptual parameters in est.p_prc and the estimated trajectories of the agent's
% representations (cf. Mathys et al., 2011). Their meanings are:
%
%         est.p_prc.mu_0       row vector of initial values of mu (in ascending order of levels)
%         est.p_prc.sa_0       row vector of initial values of sigma (in ascending order of levels)
%         est.p_prc.rho        row vector of rhos (representing drift; in ascending order of levels)
%         est.p_prc.ka         row vector of kappas (in ascending order of levels)
%         est.p_prc.om         row vector of omegas (in ascending order of levels)
%
% Note that the first entry in all of the row vectors will be NaN because, at the first level,
% these parameters are either determined by the second level (mu_0 and sa_0) or undefined (rho,
% kappa, and omega).
%
%         est.traj.mu          mu (rows: trials, columns: levels)
%         est.traj.sa          sigma (rows: trials, columns: levels)
%         est.traj.muhat       prediction of mu (rows: trials, columns: levels)
%         est.traj.sahat       precisions of predictions (rows: trials, columns: levels)
%         est.traj.v           inferred variance of random walk (rows: trials, columns: levels)
%         est.traj.w           weighting factors (rows: trials, columns: levels)
%         est.traj.da          volatility prediction errors  (rows: trials, columns: levels)
%         est.traj.ud          updates with respect to prediction  (rows: trials, columns: levels)
%         est.traj.psi         precision weights on prediction errors  (rows: trials, columns: levels)
%         est.traj.epsi        precision-weighted prediction errors  (rows: trials, columns: levels)
%         est.traj.wt          full weights on prediction errors (at the first level,
%                                  this is the learning rate) (rows: trials, columns: levels)
%
% Note that in the absence of sensory uncertainty (which is the assumption here), the first
% column of mu, corresponding to the first level, will be equal to the inputs. Likewise, the
% first column of sa will be 0 always.
%
% Tips:
% - When analyzing a new dataset, take your inputs u and use
%
%   >> est = tapas_fitModel([], u, 'tapas_ehgf_binary_config', 'tapas_bayes_optimal_binary_config');
%
%   to determine the Bayes optimal perceptual parameters (given your current priors as defined in
%   this file here, so choose them wide and loose to let the inputs influence the result). You can
%   then use the optimal parameters as your new prior means for the perceptual parameters.
%
% - If you get an error saying that the prior means are in a region where model assumptions are
%   violated, lower the prior means of the omegas, starting with the highest level and proceeding
%   downwards.
%
% - Alternatives are lowering the prior means of the kappas, if they are not fixed, or adjusting
%   the values of the kappas or omegas, if any of them are fixed.
%
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2020 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Config structure
c = struct;

% Model name
c.model = 'ehgf_ar1_binary';

% Number of levels (minimum: 3)
c.n_levels = 3;

% Input intervals
% If input intervals are irregular, the last column of the input
% matrix u has to contain the interval between inputs k-1 and k
% in the k-th row, and this flag has to be set to true
c.irregular_intervals = false;

% Sufficient statistics of Gaussian parameter priors

% Initial mus and sigmas
% Format: row vectors of length n_levels
% For all but the first two levels, this is usually best
% kept fixed to 1 (determines origin on x_i-scale). The
% first level is NaN because it is determined by the second,
% and the second implies neutrality between outcomes when it
% is centered at 0.
c.mu_0mu = [NaN, 0, 1];
c.mu_0sa = [NaN, 0, 0];

c.logsa_0mu = [NaN,   log(0.1), log(1)];
c.logsa_0sa = [NaN,          0,      0];

% Phis
% Format: row vector of length n_levels.
% Undefined (therefore NaN) at the first level.
% Fix this to zero (-Inf in logit space) to set to zero.
c.logitphimu = [NaN, -Inf, tapas_logit(0.5,1)]; %tapas_logit(0.1,1)
c.logitphisa = [NaN,    0,                  2];

% ms
% Format: row vector of length n_levels.
% This should be fixed for all levels where the omega of
% the next lowest level is not fixed because that offers
% an alternative parametrization of the same model.
c.mmu = [NaN, c.mu_0mu(2), c.mu_0mu(3)];
c.msa = [NaN,           0,           1];

% Rhos
% Format: row vector of length n_levels.
% Undefined (therefore NaN) at the first level.
% Fix this to zero to turn off drift.
c.rhomu = [NaN, 0, 0];
c.rhosa = [NaN, 0, 0];

% Kappas
% Format: row vector of length n_levels-1.
% Fixing log(kappa1) to log(1) leads to the original HGF model.
% Higher log(kappas) should be fixed (preferably to log(1)) if the
% observation model does not use mu_i+1 (kappa then determines the
% scaling of x_i+1).
c.logkamu = [log(1), log(1)];
c.logkasa = [     0,      0];

% Omegas
% Format: row vector of length n_levels.
% Undefined (therefore NaN) at the first level.
c.ommu = [NaN,  -3,   2];
c.omsa = [NaN,   4,   4];

% Gather prior settings in vectors
c.priormus = [
    c.mu_0mu,...
    c.logsa_0mu,...
    c.logitphimu,...
    c.mmu,...
    c.rhomu,...
    c.logkamu,...
    c.ommu,...
         ];

c.priorsas = [
    c.mu_0sa,...
    c.logsa_0sa,...
    c.logitphisa,...
    c.msa,...
    c.rhosa,...
    c.logkasa,...
    c.omsa,...
         ];

% Check whether we have the right number of priors
expectedLength = 5*c.n_levels+2*(c.n_levels-1)+1;
if length([c.priormus, c.priorsas]) ~= 2*expectedLength
    error('tapas:hgf:PriorDefNotMatchingLevels', 'Prior definition does not match number of levels.')
end

% Model function handle
c.prc_fun = @tapas_ehgf_ar1_binary;

% Handle to function that transforms perceptual parameters to their native space
% from the space they are estimated in
c.transp_prc_fun = @tapas_ehgf_ar1_binary_transp;

end


%go_calib_optim
%
%Main calibration function. Computes the intrinsic andextrinsic parameters.
%Runs as a script.
%
%INPUT: x_1,x_2,x_3,...: Feature locations on the images
%       X_1,X_2,X_3,...: Corresponding grid coordinates
%
%OUTPUT: fc: Camera focal length
%        cc: Principal point coordinates
%        alpha_c: Skew coefficient
%        kc: Distortion coefficients
%        KK: The camera matrix (containing fc and cc)
%        omc_1,omc_2,omc_3,...: 3D rotation vectors attached to the grid positions in space
%        Tc_1,Tc_2,Tc_3,...: 3D translation vectors attached to the grid positions in space
%        Rc_1,Rc_2,Rc_3,...: 3D rotation matrices corresponding to the omc vectors
%
%Method: Minimizes the pixel reprojection error in the least squares sense over the intrinsic
%        camera parameters, and the extrinsic parameters (3D locations of the grids in space)
%
%Note: If the intrinsic camera parameters (fc, cc, kc) do not exist before, they are initialized through
%      the function init_intrinsic_param.m. Otherwise, the variables in memory are used as initial guesses.
%
%Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the
%      corresponding entry in the active_images vector to zero.
%
%VERY IMPORTANT: This function works for 2D and 3D calibration rigs, except for init_intrinsic_param.m
%that is so far implemented to work only with 2D rigs.
%In the future, a more general function will be there.
%For now, if using a 3D calibration rig, set quick_init to 1 for an easy initialization of the focal length

if ~exist('n_ima'),
   data_calib_no_read; % Load the images
   click_calib_no_read; % Extract the corners
end;

check_active_images;

check_extracted_images;

check_active_images;

desactivated_images = [];

recompute_extrinsic = (length(ind_active) < 100); % if there are too many images, do not spend time recomputing the extrinsic parameters twice..

%%% MAIN OPTIMIZATION CALL!!!!! (look into this function for the details of implementation)
go_calib_optim_iter;

if ~isempty(desactivated_images),

   param_list_save = param_list;

   fprintf(1,'\nNew optimization including the images that have been deactivated during the previous optimization.\n');
   active_images(desactivated_images) = ones(1,length(desactivated_images));
   desactivated_images = [];

   go_calib_optim_iter;

   if ~isempty(desactivated_images),
      fprintf(1,['List of images left desactivated: ' num2str(desactivated_images) '\n' ] );
   end;

   param_list = [param_list_save(:,1:end-1) param_list];

end;

%%%%%%%%%%%%%%%%%%%% GRAPHICAL OUTPUT %%%%%%%%%%%%%%%%%%%%%%%%

%graphout_calib;


function bnet = mk_car_bnet()
% MK_CAR_BNET Make the car trouble-shooter bayes net.
%
% This network is from p13 of "Troubleshooting under uncertainty", Heckerman, Breese and
% Rommelse, Microsoft Research Tech Report 1994.

BatteryAge = 1;
Battery = 2;
Starter = 3;
Lights = 4;
TurnsOver = 5;
FuelPump = 6;
FuelLine = 7;
FuelSubsys =8;
Fuel = 9;
Spark = 10;
Starts = 11;
Gauge = 12;

n = 12;
dag = zeros(n);
dag(1,2) = 1;
dag(2,[4 5])=1;
dag(3,5) = 1;
dag(6,8) = 1;
dag(7,8) = 1;
dag(8,11) = 1;
dag(9,12) = 1;
dag(10,11) = 1;

arity = 2;
ns = arity*ones(1,n);
bnet = mk_bnet(dag, ns);
for i=1:n
    bnet.CPD{i} = tabular_CPD(bnet, i);
end


function []=plot_uw(ifg_list);
%PLOT_UW plot some figures of intermediate unwrapping steps
%   PLOT_UW(IFG_LIST) - default is to plot the first
%
% Andy Hooper, March 2012
% modifications:
% DB    11/2014         Show the interferogram dates when plotted

if nargin<1
    ifg_list=1;
end

load uw_interp
load uw_grid
load ps2

drop_ifg = getparm('drop_ifg');
ifg_keep = [1:n_ifg]';
ifg_keep(drop_ifg)=[];
ifg_keep = ifg_keep(ifg_list);
if strcmpi(getparm('small_baseline_flag'),'y')
    ifg_str = [repmat('ifg ',length(ifg_keep),1)  num2str(ifg_keep) repmat(': ',length(ifg_keep),1) datestr(ifgday(ifg_keep,1)) repmat(' till ',length(ifg_keep),1)  datestr(ifgday(ifg_keep,2))];
else
    ifg_str = '';
end
ut=load('uw_space_time')

ph=ph(:,ifg_list);

n_ifg=size(ph,2);
ni=floor(sqrt(n_ifg+1));
nj=ceil(sqrt(n_ifg+1));
if ni*nj<n_ifg+1
    ni=ni+1;
end
[nrow,ncol]=size(Z);

figure
for i=1:n_ifg
     subplot(ni,nj,i)
     ifgw=ones(nrow,ncol)*(pi+pi/31.9);
     ifgw(nzix)=angle(ph(:,i));
     imagesc(ifgw,[-pi,pi+pi/31.9])
     axis off
     axis equal
     axis xy
     if ~isempty(ifg_str)
        title(ifg_str(i,:),'fontsize',15)
     end
end
colormap([jet(64);0,0,0]);
subplot(ni,nj,i+1)
imagesc(1,[-pi,pi])
colorbar('westoutside')
cla
axis off
set(gcf,'name','Resampled (Filtered) Phase')

figure
for i=1:n_ifg
    subplot(ni,nj,i)
    ifgw=angle(reshape(ph(Z,i),nrow,ncol));
    imagesc(ifgw,[-pi,pi])
    axis off
    axis equal
    axis xy
    if ~isempty(ifg_str)
        title(ifg_str(i,:),'fontsize',15)
     end
end
set(gcf,'name','Interpolated phase')
subplot(ni,nj,i+1)
imagesc(1,[-pi,pi])
colorbar('westoutside')
cla
axis off

load uw_space_time ifreq_ij jfreq_ij

if ~isempty(ifreq_ij)
    ifreq_ij=ifreq_ij(:,n_ifg);
    jfreq_ij=jfreq_ij(:,n_ifg);
    figure
    for i=1:n_ifg
        subplot(ni,nj,i)
        ifgw=zeros(nrow,ncol);
        clim=[min(ifreq_ij(:)),max(ifreq_ij(:))];
        clim(2)=clim(2)+diff(clim)/63.9;
        ifgw(nzix)=(ifreq_ij(:,i));
        ifgw(~nzix)=clim(2);
        imagesc(ifgw,clim);
        axis off
    %    axis equal
        axis xy
    end
    colormap([jet(64);0,0,0]);
    set(gcf,'name','North Phase Gradient')
    subplot(ni,nj,i+1)
    imagesc(1,clim)
    colorbar('westoutside')
    cla
    axis off
end

if ~isempty(jfreq_ij)
figure
for i=1:n_ifg
    subplot(ni,nj,i)
    ifgw=zeros(nrow,ncol);
    clim=[min(jfreq_ij(:)),max(jfreq_ij(:))];
    clim(2)=clim(2)+diff(clim)/63.9;
    ifgw(nzix)=(jfreq_ij(:,i));
    ifgw(~nzix)=clim(2);
    imagesc(ifgw,clim);
    axis off
%    axis equal
    axis xy
end
colormap([jet(64);0,0,0]);
set(gcf,'name','East Phase Gradient')
subplot(ni,nj,i+1)
imagesc(1,clim)
colorbar('westoutside')
cla
axis off
end

figure
ifgw=ones(nrow,ncol)*(pi+pi/31.9);
ifgw(nzix)=angle(ph(:,1));
imagesc([min(xy(:,2)),max(xy(:,2))],[min(xy(:,3)),max(xy(:,3))],ifgw,[-pi,pi+pi/31.9])
colormap([jet(64);0,0,0]);

plot_edges(edges,xy(:,2),xy(:,3),'w');
if isfield(ut,'shaky_ix')
    plot_edges(edges(ut.shaky_ix,:),xy(:,2),xy(:,3),'r');
end

axis off
axis xy

set(gcf,'name','Arcs')

end


function [node,elem,face]=s2m(v,f,keepratio,maxvol,method)
%
% [node,elem,face]=s2m(v,f,keepratio,maxvol)
%
% volumetric mesh generation from a closed surface, shortcut for surf2mesh
%
% author: Qianqian Fang (fangq <at> nmr.mgh.harvard.edu)
%
% inputs and outputs are similar to those defined in surf2mesh
%
% if method='cgalpoly', s2m will call cgals2m and keepratio should be a
% structure (as the 'opt' input in cgals2m)
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

p0=min(v(:,1:3));
p1=max(v(:,1:3));
if(nargin>=5)
  if(strcmp(method,'cgalpoly'))
    [node,elem,face]=cgals2m(v,f,keepratio,maxvol);
    return;
  end
end
[node,elem,face]=surf2mesh(v,f,p0,p1,keepratio,maxvol,[],[]);


function [x,fval,exitflag,info] = opti_lmder(fun,grad,x0,ydata,opts)
%OPTI_LMDER Solve a NLS using LM_DER (Levenberg-Marquardt MINPACK Routine)
%
%   min sum[ (F(x) - ydata)^2 ]
%    x
%
%   x = opti_lmder(fun,grad,x0,ydata) solves a NLS where fun is the fitting
%   function. grad is an optional gradient of the fitting function and x0
%   is a starting guess. ydata is the data to fit the function to. Use
%   ydata = zeros() for solving a SNLE.
%
%   x = opti_lmder(fun,grad,x0,ydata,opts) uses opts to pass optiset
%   options to the solver.
%
%   [x,fval,exitflag,info] = opti_lmder(...) returns the objective value at
%   the solution, together with the solver exitflag, and an information
%   structure.
%
%   THIS IS A WRAPPER FOR LM_DER + LM_DIF
%   See supplied License

%   Copyright (C) 2012 Jonathan Currie (I2C2)

if(nargin < 5), opts = optiset; end
if(nargin < 4), error('LM_DER requires at least 4 arguments'); end

%Setup display level
opts.display = dispLevel(opts.display);

%Check we have a valid x0
if(isempty(x0) || any(isnan(x0)))
    error('LMDER requires an initial guess, x0!');
end

t = tic;
% Run LM_DER
[x, fval, exitflag, feval] = lmder(fun,grad,x0,ydata,opts);

%Collect Results
info.FuncEvals = feval;
info.Time = toc(t);
info.Algorithm = 'LM_DER: MINPACK Levenberg-Marquardt';

switch(exitflag)
    case 1
        info.Status = 'Optimal';
    case 0
        info.Status = 'Exceeded Function Evaluations';
    case -1
        info.Status = 'Infeasible / Could not Converge';
    case -2
        info.Status = 'LM_DER Error';
    case -5
        info.Status = 'User Exited';
    otherwise
        info.Status = 'LM_DER Error';
end


%% ------------------------------------------------------------------------------
% Configuration File for Visual Odometry Algorithm
%% -------------------------------------------------------------------------------

% Path to the directories containing images
data_params.path1 = '../data/kitti/00/image_0/';
data_params.path2 = '../data/kitti/00/image_1/';

% Path to calibration text file
data_params.calib_file = '../data/kitti/00/calib.txt';

% Path to groundtruth poses. Set flag to 1 to plot groundtruth as well
data_params.gt_file = '../data/kitti/poses/00.txt';
data_params.show_gt_flag = 1;

% Use parallel threads (requires Parallel Processing Toolbox)
% !! TO-DO: fix parfor and for loops for this functionality!
data_params.use_multithreads = 1;                % 0: disabled, 1: enabled

%% Read the calibration file to find parameters of the cameras
% !! TO-DO: Read from the calib_file instead

% calibration parameters for sequence 2010_03_09_drive_0000
cam_params.fx = 7.188560000000e+02;               % focal length (u-coordinate) in pixels
cam_params.cx = 6.071928000000e+02;               % principal point (u-coordinate) in pixels
cam_params.fy = 7.188560000000e+02;               % focal length (v-coordinate) in pixels
cam_params.cy = 1.852157000000e+02;               % principal point (v-coordinate) in pixels
cam_params.base = 3.861448000000e+02;             % baseline in meters (absolute value)

%% Parameters for Feature Extraction
vo_params.feature.nms_n = 8;                      % non-max-suppression: min. distance between maxima (in pixels)
vo_params.feature.nms_tau = 50;                   % non-max-suppression: interest point peakiness threshold
vo_params.feature.margin = 21;                    % leaving margin for safety while computing features ( >= 25)

%% Parameters for Feature Matching
vo_params.matcher.match_binsize = 50;             % matching bin width/height (affects efficiency only)
vo_params.matcher.match_radius = 200;             % matching radius (du/dv in pixels)
vo_params.matcher.match_disp_tolerance = 1;       % dx tolerance for stereo matches (in pixels)
vo_params.matcher.match_ncc_window = 21;          % window size of the patch for normalized cross-correlation
vo_params.matcher.match_ncc_tolerance = 0.3;      % threshold for normalized cross-correlation
% !! TO-DO: add subpixel-refinement using parabolic fitting
vo_params.matcher.refinement = 2;                 % refinement (0=none,1=pixel,2=subpixel)

%% Paramters for Feature Selection using bucketing
vo_params.bucketing.max_features = 1;             % maximal number of features per bucket
vo_params.bucketing.bucket_width = 50;            % width of bucket
vo_params.bucketing.bucket_height = 50;           % height of bucket
% !! TO-DO: add feature selection based on feature tracking
vo_params.bucketing.age_threshold = 10;           % age threshold while feature selection

%% Paramters for motion estimation
% !! TO-DO: use Nister's algorithm for Rotation estimation (along with SLERP) and
% estimate translation using weighted optimization equation
vo_params.estim.ransac_iters = 200;              % number of RANSAC iterations
vo_params.estim.inlier_threshold = 2.0;          % fundamental matrix inlier threshold
vo_params.estim.reweighing = 1;                  % lower border weights (more robust to calibration errors)


function [charAcc, wordAcc] = ScoreModel (words, imageModel, pairwiseModel, tripletList)
% This function runs the Markov network model end-to-end and computes the
% per-character and per-word accuracy on provided data.
%
% Input:
%   words: A cell array where words{i} is the struct array for the ith
%     word (this is the structure of the provided 'allWords' data).
%   imageModel: The provided image model struct.
%   pairwiseModel: A K-by-K matrix (K is the alphabet size) where pairwiseModel(i,j)
%     is the factor value for the pairwise factor of character i followed by
%     character j.
%   tripletModel: The array of character triplets we will consider (along
%     with their corresponding factor values).
%
% Output:
%   charAcc: The percentage of all characters (across all words) correctly
%     identified. (Between 0 and 1)
%   wordAcc: The percentage of the words in which every character is
%     correctly identified. (Between 0 and 1)
%
% Copyright (C) Daphne Koller, Stanford University, 2012

predictions = ComputeWordPredictions(words, imageModel, pairwiseModel, tripletList);
[charAcc, wordAcc] = ScorePredictions(words, predictions, true);

end


function niederreiter2_test04 ( )

%*****************************************************************************80
%
%% NIEDERREITER2_TEST04 tests NIEDERREITER2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 November 2008
%
%  Author:
%
%    John Burkardt
%
  dim_max = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'NIEDERREITER2_TEST04\n' );
  fprintf ( 1, '  NIEDERREITER2 computes the next element of\n' );
  fprintf ( 1, '  a Niederreiter quasirandom sequence using base 2.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  In this test, we call NIEDERREITER2 repeatedly.\n' );

  for dim_num = 2 : dim_max

    seed = 0;

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Using dimension DIM_NUM =   %d\n', dim_num );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Seed  Seed   Niederreiter2\n' );
    fprintf ( 1, '  In    Out\n' );
    fprintf ( 1, '\n' );

    for i = 0 : 110
      seed_in = seed;
      [ r, seed ] = niederreiter2 ( dim_num, seed );
      seed_out = seed;
      if ( i <= 11 | 95 <= i )
        fprintf ( 1, '%3d  %3d  ', seed_in, seed_out );
        for j = 1 : dim_num
          fprintf ( 1, '%10f', r(j) );
        end
        fprintf ( 1, '\n' );
      elseif ( i == 12 )
        fprintf ( 1, '......................\n' );
      end
    end

  end

  return
end


function [ bgU, status, OPTS ] = bgtraining( videopath, OPTIONS, max_cycles, subsampling, training_size, FPS_ONLY , TRAIN_FRAME)
%BGTRAINING Summary of this function goes here
%   Detailed explanation goes here
thresh = 0.1;

FILE_EXT        = '.bmp';
OPTS            = struct(); % initial a empty struct for OPTS
status.init     = 0;   % status of grasta at each iteration
U_hat           = zeros(1);

%
Dir_lists = dir(videopath);
Video_Length = min(training_size,length(Dir_lists));

if TRAIN_FRAME == 1, % Random selection
    p = randperm(length(Dir_lists)); % length(Dir_lists)-1500 for hall
    frames_idx = p(1:Video_Length);
else                % Select all frames
    frames_idx = 1:Video_Length;
end

if ~FPS_ONLY,
    h_fg = subplot(2,2,1);set(gca,'nextplot','replacechildren');title('GRASTA-Foreground');
    h_training_bg = subplot(2,2,2);set(gca,'nextplot','replacechildren');title('Training Background');
    h_img = subplot(2,2,3);set(gca,'nextplot','replacechildren');title('Training video');
end

for outiter = 1:max_cycles,
    video_order = randperm(Video_Length);  %  1:Video_Length; %
    t_start = tic;
    frame_count  = 0;
    for i=1:Video_Length,
        if Dir_lists(frames_idx(video_order(i))).isdir,
            continue;
        end
        if isempty(strfind(Dir_lists(frames_idx(video_order(i))).name,FILE_EXT)) ,
            continue;
        end

        frame_count = frame_count+1;
        fname = [videopath Dir_lists(frames_idx(video_order(i))).name];

        % prepare the image
        I = imread(fname);
        I = double(rgb2gray(I));

        if frame_count==1,
            [rows,cols]     = size(I);
            VW_ROWS         = rows;
            VW_COLS         = cols; %ceil(cols * VW_RATIO);
            DIM             = VW_ROWS * VW_COLS;
            OPTIONS.DIM_M   = DIM; % video ambient dimension
        end

        I = I/max(max(I));

        % random subsampling the frame I
        M = round(subsampling * DIM);
        p = randperm(DIM);
        idx = p(1:M)';

        I_Omega = I(idx);

        [U_hat, status, OPTS] = grasta_stream(I_Omega, idx, U_hat, status, OPTIONS, OPTS);

        if mod(i,1) == 0 && ~FPS_ONLY,
            bg_img = reshape(U_hat * status.w * status.SCALE, VW_ROWS,VW_COLS);
            axes(h_training_bg); imagesc(bg_img);colormap gray;axis off;axis ij ;

            o_img = reshape( I ,VW_ROWS,VW_COLS );
            axes(h_img); imagesc(o_img);colormap gray;axis off;axis ij ;

            s_hat = I(:) - U_hat * status.w * status.SCALE;
            s_hat = fg_thresholding(s_hat,thresh);

            s_img = reshape(s_hat,VW_ROWS,VW_COLS);
            axes(h_fg); imagesc(s_img);colormap gray;axis off;axis ij ;

        end
    end
    t_end = toc(t_start);
    fprintf('Training %d/%d: %.2f seconds, %.2f fps, grasta_t %.2e \n',...
        outiter, max_cycles,t_end, frame_count/t_end,status.grasta_t);
end

bgU = U_hat;
end


classdef prtPreProcLogDiscPostPlsda < prtPreProcClass
    % prtPreProcLogDisc   Histogram equalization processing
    %
    %   LOGDISC = prtPreProcLogDisc creates a logistic discriminant pre
    %   processing object. A prtPreProcLogDisc object processes the input data
    %   so that each feature dimension is scaled between 0 and 1 to best
    %   match the data set class labels.
    %
    %   prtPreProcLogDisc has no user settable properties.
    %
    %   A prtPreProcLogDisc object also inherits all properties and
    %   functions from the prtAction class
    %
    %   Example:
    %
    %   dataSet = prtDataGenUnimodal;     % Load a data set
    %   logDisc = prtPreProcLogDisc;      % Create a pre processing object
    %
    %   logDisc = logDisc.train(dataSet);  % Train
    %   dataSetNew = logDisc.run(dataSet); % Run
    %
    %   % Plot
    %   subplot(2,1,1); plot(dataSet);
    %   title('Original Data');
    %   subplot(2,1,2); plot(dataSetNew);
    %   title('LogDisc Data');
    %
    %   See Also: prtPreProc, prtPreProcPca, prtPreProcPls,
    %   prtPreProcHistEq, prtPreProcZeroMeanColumns, prtPreProcLda,
    %   prtPreProcZeroMeanRows, prtPreProcLogDisc, prtPreProcZmuv,
    %   prtPreProcMinMaxRows

    properties (SetAccess=private)
        name = 'Logistic Discriminant' % 'Logistic Discriminant'
        nameAbbreviation = 'LogDisc' % LogDisc
    end

    properties (SetAccess=private, Hidden = true)
        % General Classifier Properties
        logDiscWeights = [];
        logDiscMeans = [];
    end

    methods
        function Obj = prtPreProcLogDiscPostPlsda(varargin)
            % Allow for string, value pairs
            % There are no user settable options though.
            Obj = prtUtilAssignStringValuePairs(Obj,varargin{:});
        end
    end

    methods (Access = protected, Hidden = true)

        function Obj = trainAction(Obj,DataSet)

			y = DataSet.getTargetsAsBinaryMatrix;
			LogDisc = prtClassLogisticDiscriminant;
			for iFeature = 1:DataSet.nFeatures
				cLogDisc = LogDisc.train(setTargets(DataSet.retainFeatures(iFeature),y(:,iFeature)));
				Obj.logDiscMeans(iFeature) = cLogDisc.w(1);
				Obj.logDiscWeights(iFeature) = cLogDisc.w(2);
			end

        end

        function DataSet = runAction(Obj,DataSet)
			sigmaFn = @(x) 1./(1 + exp(-x));
			for iFeature = 1:length(Obj.logDiscWeights)
                DataSet.X(:,iFeature) = sigmaFn(DataSet.X(:,iFeature)*Obj.logDiscWeights(iFeature) + Obj.logDiscMeans(iFeature));
			end

			if length(Obj.logDiscWeights) > 1
				DataSet.X = bsxfun(@rdivide, DataSet.X, sum(DataSet.X,2));
			else
				DataSet.X = 1-DataSet.X;
			end

        end

		function X = runActionFast(Obj,X)
			for iFeature = 1:length(Obj.logDiscWeights)
				X(:,iFeature) = 1./(1 + exp(- (X(:,iFeature)*Obj.logDiscWeights(iFeature) + Obj.logDiscMeans(iFeature))));
			end
			if size(X,2) > 1
				X = bsxfun(@rdivide, X, sum(X,2));
			else
				X = 1-X;
			end

        end
    end

end


classdef (InferiorClasses = {?rotation,?quaternion}) DelaunaySO3 < orientation
%
% Syntax
%
%   % define a Delaunay trinangulation from a list of orientations
%   DSO3 = DelaunaySO3(ori)
%
  properties

    tetra          % list of vertices of the tetrahegons
    tetraNeighbour % neigbouring tetraeders orderd as faces
    lookup         % lookup table orientation -> tetrahedrons

  end

  methods

    function DSO3 = DelaunaySO3(varargin)

      DSO3 = DSO3@orientation(varargin{:});

      if nargin == 0, return; end

      % pertube data a bit
      % it would be better if this would be needed only for the
      % triangulation, but for some reason we cant skip it
      [DSO3.a,DSO3.b,DSO3.c,DSO3.d] = double(perturbe(DSO3,0.05*degree));

      % compute tetrahegons
      DSO3.tetra = calcDelaunay2(DSO3);

      % compute neighbouring list
      DSO3.tetraNeighbour = calcNeighbour(DSO3.tetra);

      % compute lookup table
      res = 40*degree;
      for i = 1:4
        res = res / 2;
        DSO3.lookup = calcLookUp(DSO3,res);
      end
    end

end

end


function new_la = find_good_la(rows,cols,obs,goal,start,old_la,curveset)
%
%find a good look-ahead value to use
%
limits = 0.05;%set threshold
w = extend_world(rows,cols,obs,goal,old_la);
p = calc_pot_value(w);
stpot = p(start.r,start.c,1);
ti = 1;
for i = 1:16
    if curveset.cy(i) <= stpot
        ti = i;
    else
        break;
    end
end
la1 = curveset.cx(ti);
la0 = old_la;
la2 = 500;
for i = ti+1:16
    sig = std(curveset.pty((1:25)+25*(i-1)));
    mu = curveset.cy(i);
    z = (stpot - mu)/sig;
    zVal = normcdf(z);% z table value
    if (zVal <= limits)
        la2 = curveset.cx(i);
    else
        break;
    end
end
a = la2 - la0;
b = la2 - la1;
new_la = la0 + max([a b]);
end


function [g]= spm_gx_adem_cue(x,v,a,P)
% returns the prediction for cued responses (proprioception and vision)
% FORMAT [g]= spm_gx_adem_cue(x,v,a,P)
%
% x    - hidden states:
%   x.o  - intrinsic motor state (proprioceptive)
%
% v    - hidden causes
%
% P    - target locations (visual) - extrinsic coordinates (Cartesian)
%
% g    - sensations:
%   g.o  - motor angle (proprioception)
%   g.p  - finger location (visual)
%   g.c  - target contrast (visual)
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_gx_adem_cue.m 4230 2011-03-07 20:58:38Z karl $

% evaluate positions in intrinsic (polar) coordinates
%--------------------------------------------------------------------------
g.o = x.o;
g.p = tan(x.o);
g.c = x.a*4;