%RangeBearingSensor Range and bearing sensor class
%
% A concrete subclass of the Sensor class that implements a range and bearing
% angle sensor that provides robot-centric measurements of landmark points in
% the world. To enable this it holds a references to a map of the world (LandmarkMap object)
% and a robot (Vehicle subclass object) that moves in SE(2).
%
% The sensor observes landmarks within its angular field of view between
% the minimum and maximum range.
%
% Methods::
%
% reading   range/bearing observation of random landmark
% h         range/bearing observation of specific landmark
% Hx        Jacobian matrix with respect to vehicle pose dh/dx
% Hp        Jacobian matrix with respect to landmark position dh/dp
% Hw        Jacobian matrix with respect to noise dh/dw
%-
% g         feature position given vehicle pose and observation
% Gx        Jacobian matrix with respect to vehicle pose dg/dx
% Gz        Jacobian matrix with respect to observation dg/dz
%
% Properties (read/write)::
% W            measurement covariance matrix (2x2)
% interval     valid measurements returned every interval'th call to reading()
% landmarklog  time history of observed landmarks
%
% Reference::
%
%   Robotics, Vision & Control, Chap 6,
%   Peter Corke,
%   Springer 2011
%
% See also Sensor, Vehicle, LandmarkMap, EKF.

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

classdef RangeBearingSensor < Sensor

    properties
        W           % measurment covariance
        r_range     % range limits
        theta_range % angle limits

        randstream  % random stream just for Sensors

        landmarklog  % time history of observed landmarks
    end

    properties (SetAccess = private)
        count       % number of reading()s
    end

    methods

        function s = RangeBearingSensor(robot, map, varargin)
            %RangeBearingSensor.RangeBearingSensor Range and bearing sensor constructor
            %
            % S = RangeBearingSensor(VEHICLE, MAP, OPTIONS) is an object
            % representing a range and bearing angle sensor mounted on the Vehicle
            % subclass object VEHICLE and observing an environment of known landmarks
            % represented by the LandmarkMap object MAP.  The sensor covariance is W
            % (2x2) representing range and bearing covariance.
            %
            % The sensor has specified angular field of view and minimum and maximum
            % range.
            %
            % Options::
            % 'covar',W               covariance matrix (2x2)
            % 'range',xmax            maximum range of sensor
            % 'range',[xmin xmax]     minimum and maximum range of sensor
            % 'angle',TH              angular field of view, from -TH to +TH
            % 'angle',[THMIN THMAX]   detection for angles betwen THMIN
            %                         and THMAX
            % 'skip',K                return a valid reading on every K'th call
            % 'fail',[TMIN TMAX]      sensor simulates failure between
            %                         timesteps TMIN and TMAX
            % 'animate'               animate sensor readings
            %
            % See also options for Sensor constructor.
            %
            % See also RangeBearingSensor.reading, Sensor.Sensor, Vehicle, LandmarkMap, EKF.

            % call the superclass constructor
            s = s@Sensor(robot, map, varargin{:});

            s.randstream = RandStream.create('mt19937ar');

            opt.range = [];
            opt.angle = [];
            opt.covar = zeros(2,2);

            [opt,args] = tb_optparse(opt, varargin);

            s.W = opt.covar;
            if ~isempty(opt.range)
                if length(opt.range) == 1
                    s.r_range = [0 opt.range];
                elseif length(opt.range) == 2
                    s.r_range = opt.range;
                end
            end
            if ~isempty(opt.angle)
                if length(opt.angle) == 1
                    s.theta_range = [-opt.angle opt.angle];
                elseif length(opt.angle) == 2
                    s.theta_range = opt.angle;
                end
            end

            s.count = 0;
            s.verbose = opt.verbose;
        end

        function init(s)
            s.landmarklog = [];
        end

        function k = selectFeature(s)
            k = s.randstream.randi(s.map.nlandmarks);
        end

        function [z,jf] = reading(s)
            %RangeBearingSensor.reading Choose landmark and return observation
            %
            % [Z,K] = S.reading() is an observation of a random visible landmark where
            % Z=[R,THETA] is the range and bearing with additive Gaussian noise of
            % covariance W (property W). K is the index of the map feature that was
            % observed.
            %
            % The landmark is chosen randomly from the set of all visible landmarks,
            % those within the angular field of view and range limits.  If no valid
            % measurement, ie. no features within range, interval subsampling enabled
            % or simulated failure the return is Z=[] and K=0.
            %
            % Notes::
            % - Noise with covariance W (property W) is added to each row of Z.
            % - If 'animate' option set then show a line from the vehicle to the
            %   landmark
            % - If 'animate' option set and the angular and distance limits are set
            %   then display that region as a shaded polygon.
            % - Implements sensor failure and subsampling if specified to constructor.
            %
            % See also RangeBearingSensor.h.

            % TODO probably should return K=0 to indicated invalid

            % model a sensor that emits readings every interval samples
            s.count = s.count + 1;

            % check conditions for NOT returning a value
            z = [];
            jf = 0;
            % sample interval
            if mod(s.count, s.interval) ~= 0
                return;
            end
            % simulated failure
            if ~isempty(s.fail) && (s.count >= s.fail(1)) && (s.count <= s.fail(2))
                return;
            end

            % create a polygon to indicate the active sensing area based on range+angle limits
            if s.animate && ~isempty(s.theta_range) && ~isempty(s.r_range)
                h = findobj(gca, 'tag', 'sensor-area');
                if isempty(h)

                    th=linspace(s.theta_range(1), s.theta_range(2), 20);
                    x = s.r_range(2) * cos(th);
                    y = s.r_range(2) * sin(th);
                    if s.r_range(1) > 0
                        th = flip(th);
                        x = [x s.r_range(1) * cos(th)];
                        y = [y s.r_range(1) * sin(th)];
                    else
                        x = [x 0];
                        y = [y 0];
                    end
                    % no sensor zone, create one
                    plot_poly([x; y], 'fillcolor', 'r', 'alpha', 0.1, 'edgecolor', 'none', 'animate', 'tag', 'sensor-area');
                else
                    %hg = get(h, 'Parent');
                    plot_poly(h, s.robot.x);

                end
            end

            if ~isempty(s.r_range)  || ~isempty(s.theta_range)
                % if range and bearing angle limits are in place look for
                % any landmarks that match criteria

                % get range/bearing to all landmarks, one per row
                z = s.h(s.robot.x');
                jf = 1:numcols(s.map.map);

                if ~isempty(s.r_range)
                    % find all within range
                    k = find( z(:,1) >= s.r_range(1) & z(:,1) <= s.r_range(2) );
                    z = z(k,:);
                    jf = jf(k);
                end
                if ~isempty(s.theta_range)
                    % find all within angular range as well
                    k = find( z(:,2) >= s.theta_range(1) & z(:,2) <= s.theta_range(2) );
                    z = z(k,:);
                    jf = jf(k);
                end

                % deal with cases for 0 or > 1 features found
                if isempty(z)
                    % no landmarks found
                    jf = 0;
                elseif length(k) >= 1
                    % more than 1 in range, pick a random one
                    i = s.randstream.randi(length(k));
                    z = z(i,:);
                    jf = jf(i);
                end

            else
                % randomly choose the feature
                jf = s.selectFeature();

                % compute the range and bearing from robot to feature
                z = s.h(s.robot.x', jf);
            end

            if s.verbose
                if isempty(z)
                    fprintf('Sensor:: no features\n');
                else
                    fprintf('Sensor:: feature %d: %.1f %.1f\n', jf, z);
                end
            end
            if s.animate
                s.plot(jf);
            end

            z = z';

            % add the reading to the landmark log
            s.landmarklog = [s.landmarklog jf];
        end

        function z = h(s, xv, jf)
            %RangeBearingSensor.h Landmark range and bearing
            %
            % Z = S.h(X, K) is a sensor observation (1x2), range and bearing, from vehicle at
            % pose X (1x3) to the K'th landmark.
            %
            % Z = S.h(X, P) as above but compute range and bearing to a landmark at coordinate P.
            %
            % Z = s.h(X) as above but computes range and bearing to all
            % map features.  Z has one row per landmark.
            %
            % Notes::
            % - Noise with covariance W (propertyW) is added to each row of Z.
            % - Supports vectorized operation where XV (Nx3) and Z (Nx2).
            % - The landmark is assumed visible, field of view and range liits are not
            %   applied.
            %
            % See also RangeBearingSensor.reading, RangeBearingSensor.Hx, RangeBearingSensor.Hw, RangeBearingSensor.Hp.

            % get the landmarks, one per row
            if nargin < 3
                % s.h(XV)
                xlm = s.map.map';
            elseif length(jf) == 1
                % s.h(XV, JF)
                xlm = s.map.map(:,jf)';
            else
                % s.h(XV, XF)
                xlm = jf(:)';
            end

            % Straightforward code:
            %
            % dx = xf(1) - xv(1); dy = xf(2) - xv(2);
            %
            % z = zeros(2,1);
            % z(1) = sqrt(dx^2 + dy^2);       % range measurement
            % z(2) = atan2(dy, dx) - xv(3);   % bearing measurement
            %
            % Vectorized code:

            % compute range and bearing
            dx = xlm(:,1) - xv(:,1); dy = xlm(:,2) - xv(:,2);
            z = [sqrt(dx.^2 + dy.^2) angdiff(atan2(dy, dx), xv(:,3)) ];   % range & bearing measurement

            % add noise with covariance W
            z = z + s.randstream.randn(size(z)) * sqrtm(s.W) ;
        end

        function J = Hx(s, xv, jf)
            %RangeBearingSensor.Hx Jacobian dh/dx
            %
            % J = S.Hx(X, K) returns the Jacobian dh/dx (2x3) at the vehicle
            % state X (3x1) for map landmark K.
            %
            % J = S.Hx(X, P) as above but for a landmark at coordinate P.
            %
            % See also RangeBearingSensor.h.
            if length(jf) == 1
                xf = s.map.map(:,jf);
            else
                xf = jf;
            end
            if isempty(xv)
                xv = s.robot.x;
            end
            Delta = xf - xv(1:2)';
            r = norm(Delta);
            J = [
                -Delta(1)/r,    -Delta(2)/r,        0
                Delta(2)/(r^2),  -Delta(1)/(r^2),   -1
            ];
        end

        function J = Hp(s, xv, jf)
            %RangeBearingSensor.Hp Jacobian dh/dp
            %
            % J = S.Hp(X, K) is the Jacobian dh/dp (2x2) at the vehicle
            % state X (3x1) for map landmark K.
            %
            % J = S.Hp(X, P) as above but for a landmark at coordinate P (1x2).
            %
            % See also RangeBearingSensor.h.
            if length(jf) == 1
                xf = s.map.map(:,jf);
            else
                xf = jf;
            end
            Delta = xf - xv(1:2)';
            r = norm(Delta);
            J = [
                Delta(1)/r,         Delta(2)/r
                -Delta(2)/(r^2),    Delta(1)/(r^2)
            ];
        end

        function J = Hw(s, xv, jf)
            %RangeBearingSensor.Hx Jacobian dh/dw
            %
            % J = S.Hw(X, K) is the Jacobian dh/dw (2x2) at the vehicle
            % state X (3x1) for map landmark K.
            %
            % See also RangeBearingSensor.h.
            J = eye(2,2);
        end

        function xf = g(s, xv, z)
            %RangeBearingSensor.g Compute landmark location
            %
            % P = S.g(X, Z) is the world coordinate (2x1) of a feature given
            % the observation Z (1x2) from a vehicle state with X (3x1).
            %
            % See also RangeBearingSensor.Gx, RangeBearingSensor.Gz.

            range = z(1);
            bearing = z(2) + xv(3); % bearing angle in vehicle frame

            xf = [xv(1)+range*cos(bearing); xv(2)+range*sin(bearing)];
        end

        function J = Gx(s, xv, z)
            %RangeBearingSensor.Gxv Jacobian dg/dx
            %
            % J = S.Gx(X, Z) is the Jacobian dg/dx (2x3) at the vehicle state X (3x1) for
            % sensor observation Z (2x1).
            %
            % See also RangeBearingSensor.g.
            theta = xv(3);
            r = z(1);
            bearing = z(2);
            J = [
                1,   0,   -r*sin(theta + bearing);
                0,   1,    r*cos(theta + bearing)
                ];
        end

        function J = Gz(s, xv, z)
            %RangeBearingSensor.Gz Jacobian dg/dz
            %
            % J = S.Gz(X, Z) is the Jacobian dg/dz (2x2) at the vehicle state X (3x1) for
            % sensor observation Z (2x1).
            %
            % See also RangeBearingSensor.g.
            theta = xv(3);
            r = z(1);
            bearing = z(2);
            J = [
                cos(theta + bearing),   -r*sin(theta + bearing);
                sin(theta + bearing),    r*cos(theta + bearing)
                ];
        end

        function str = char(s)
            str = char@Sensor(s);
            str = char(str, ['W = ', mat2str(s.W, 3)]);

            str = char(str, sprintf('interval %d samples', s.interval) );
            if ~isempty(s.r_range)
                str = char(str, sprintf('range: %g to %g', s.r_range) );
            end
            if ~isempty(s.theta_range)
                str = char(str, sprintf('angle: %g to %g', s.theta_range) );
            end
        end

    end % method
end % classdef


function dataOut=plotMultipleProjectedAmps_PerCondition(view,projectionPhase)
%
% plotMultipleProjectedAmps_PerCondition(view,projectionPhase)
%
% Bar plot of the amplitudes for each scan, averaging across
% all pixels (in all slices) in a selection of ROIs. All y-axes are made the same. The bar heights
% and a coarse SEM can be obtained from get(gca,'UserData').
% Amplitudes are projected against a single phase (supplied).

% gmb  5/25/98
% bw   2/19/99  Added seY field to the UserData field.
%	    seY is an estimate of the variability in the
%      amplitudes.  It is the SEM of the in the complex
%      (amp*exp(-i*ph)) representation.  The values are
%      computed in vectorMean.m
% fwc   11/07/02 plots data relative to current view
%       added plotting of multiple ROIs
%       ROI selection copied from plotMultipleTSeries.m
% arw   071204 Correctly extracts projections phase from current scan
% Compute means across scans, for all pixels in the
% currently selected ROI.  The seZ value is the mean
% distance from the mean.
% projectionPhase (if supplied) is in radians.

mrGlobals;

refScan = getCurScan(view);
if (~exist('projectionPhase','var'))
    computeProjPhase=1;
else
    if (isnan(projectionPhase))
        % User input requested
        projectionPhase=input('Enter projection phase in radians:');
    end

    computeProjPhase=0;
end

% Select ROIs
nROIs=size(view.ROIs,2);
roiList=cell(1,nROIs);
for r=1:nROIs
    roiList{r}=view.ROIs(r).name;
end
selectedROIs = find(buttondlg('ROIs to Plot',roiList));
nROIs=length(selectedROIs);
if (nROIs==0)
    error('No ROIs selected');
end

% Plot it
selectGraphWin
clf
fontSize = 8;
headerStr = ['Mean Amplitudes'];
set(gcf,'Name',headerStr);

minylim=0;
maxylim=0;
nrows=0;
ncols=0;

nscans = numScans(view);
ROIamps=zeros(nscans,nROIs);
ROIseZ=zeros(nscans,nROIs);
ROImeanPhs=zeros(nscans,nROIs);

for r=1:nROIs

    n=selectedROIs(r);
    view = selectROI(view,n); % is there another way?
    [meanAmps,meanPhs,seZ] = vectorMeans(view);

    if (computeProjPhase)
        projectionPhase=meanPhs(refScan);
    end

    % Compute the amplitude projected onto the reference phase
    meanAmps = meanAmps.*cos(meanPhs-projectionPhase);

    ROIamps(:,r)=meanAmps(:);
    ROIseZ(:,r)=seZ(:);

    ROImeanPhs(:,r)=meanPhs(:);
    %xstr{r}=[view.ROIs(selectedROIs(r)).name];
    xstr{r}=int2str(r);
    roiName{r}=view.ROIs(selectedROIs(r)).name;
    fprintf(['\n#%d :',roiName{r}],r);
end

dataOut.ROIamps=ROIamps;
dataOut.ROIseZ=ROIseZ;
dataOut.ROIname=roiName;

% Now do the plotting

if nscans<=3
    nrows=1;
    ncols=nscans;
    fontSize = 9;
elseif nscans<=8
    nrows=2;
    ncols=ceil(nscans/nrows);
    fontSize = 8;
else
    nrows=ceil(sqrt(nscans));
    ncols=ceil(nscans/nrows);
    fontSize = 6;
end

    scanList = [1:numScans(view)];

for r=1:nscans

    subplot(nrows,ncols,r);

    %plot the bar graph
    if(r==refScan)
        h=mybar(ROIamps(r,:)',ROIseZ(r,:)',xstr,[],[1 0 0]);
    else
        h=mybar(ROIamps(r,:)',ROIseZ(r,:)',xstr,[],[0 0 1]);
    end

    xlabel('ROI','FontSize',fontSize);
    ylabel('Mean Amplitude','FontSize',fontSize);
    set(gca,'FontSize',ceil(fontSize*1.2));
    conditionName{r}=dataTYPES(view.curDataType).scanParams(r).annotation;
    fprintf(['\nCondition #%d :',conditionName{r}],r);

    title(['Condition #: ' int2str(r)]);
    yl=ylim;

    if yl(1)< minylim
        minylim=yl(1);
    end
    if yl(2)> maxylim
        maxylim=yl(2);
    end
end

% 	xlabel('Scan','FontSize',fontSize);
% 	ylabel('Mean Amplitude','FontSize',fontSize);
% 	ylim =get(gca,'YLim');
% 	set(gca,'YLim',ylim*1.1);
%     % slightly bigger title
% 	set(gca,'FontSize',ceil(fontSize*1.2));
% 	title(['ROI: ' view.ROIs(selectedROIs(r)).name]);

%  foo=cell2struct(h,'bar');
% 	hbar=foo.bar(refScan);
% 	set(hbar,'FaceColor','r')

%Save the data in gca('UserData')
data.y =ROIamps(r,:);
data.refScan = refScan;
data.seY = ROIseZ(r,:); % this should probably be adapted

set(gca,'UserData',data);

% give all plots same y-axis

for r=1:nscans
    subplot(nrows,ncols,r);
    ylim([minylim maxylim]);
end

return;


function blas1_c_test02 ( )

%*****************************************************************************80
%
%% TEST02 tests CABS2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 April 2006
%
%  Author:
%
%    John Burkardt
%
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST02\n' );
  fprintf ( 1, '  CABS2 returns the L2 norm of a complex number.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      Real      Imaginary\n' );
  fprintf ( 1, '      Part      Part           CABS2(Z)\n' );
  fprintf ( 1, '\n' );

  for i = 1 : 10

    [ c, seed ] = c4_uniform_01 ( seed );
    c = 5.0 * c;
    c_norm = cabs2 ( c );

    fprintf ( 1, '  %10f  %10f     %10f\n', real ( c ), imag ( c ), c_norm );

  end

  return
end


function x = p09_start ( option, nvar )

%*****************************************************************************80
%
%% P09_START returns a starting point for problem 9.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 September 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer OPTION, the option index.
%
%    Input, integer NVAR, the number of variables.
%
%    Output, real X(NVAR), the starting point.
%
  x = zeros ( nvar, 1 );

  [ height, ival, val ] = p09_gx ( option );

  x(ival) = val;

  return
end


function tests = test_ft_preproc_derivative

% MEM 1gb
% WALLTIME 00:10:00
% DEPENDENCY ft_preproc_derivative

if nargout
  % assume that this is called by RUNTESTS
  tests = functiontests(localfunctions);
else
  % assume that this is called from the command line
  fn = localfunctions;
  for i=1:numel(fn)
    feval(fn{i});
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function testOptions(testCase)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

nchan   = 8;
nsample = 1000;
dat     = randn(nchan, nsample) + 1;

result = {};
result{end+1} = ft_preproc_derivative(dat, 1);
result{end+1} = ft_preproc_derivative(dat, 2);
result{end+1} = ft_preproc_derivative(dat, 4);
result{end+1} = ft_preproc_derivative(dat, 8);

% all iterations were done with (slightly) different options, hence the results should not be equal
for i=1:numel(result)
  for j=(i+1):numel(result)
    assert(~isequal(result{i}, result{j}), 'the results %d and %d should not be equal', i, j);
  end
end


function M = moveUDB(F, d)
%------------------------------------------------------------------------------
% Moves gridfunction F in vertical direction.
% Excess area is filled by extension of boundaryvalues.
%
% Design and implementation by:
% Dr. Paul M. de Zeeuw <Paul.de.Zeeuw@cwi.nl>  http://homepages.cwi.nl/~pauldz/
% Last Revision: July 7, 2001.
% (c) 1999-2002 Stichting CWI, Amsterdam
%------------------------------------------------------------------------------
[n, m] = size(F);
if d > 0
  if (d+1) > n
    error(' moveUDB    d >= n ')
  else
%   M = [flipud(F(2:(d+1),:)); F(1:(n-d),:)];
    B = F(1,:);
    M = [B(ones(1,d),:); F(1:(n-d),:)];
  end
elseif d < 0
  if (-d+1) > n
    error(' moveUDB   -d >= n ')
  else
%   M = [F((-d+1):n,:); flipud(F((n+d):(n-1),:))];
    B = F(n,:);
    M = [F((-d+1):n,:); B(ones(1,-d),:)];
  end
else
  M = F;
end
%------------------------------------------------------------------------------


function test_suite = test_sample_unique
% tests for cosmo_sample_unique
%
% #   For CoSMoMVPA's copyright information and license terms,   #
% #   see the COPYING file distributed with CoSMoMVPA.           #
    try % assignment of 'localfunctions' is necessary in Matlab >= 2016
        test_functions=localfunctions();
    catch % no problem; early Matlab versions can use initTestSuite fine
    end
    initTestSuite;

function test_sample_unique_basics
    aet=@(varargin)assertExceptionThrown(@()...
                    cosmo_sample_unique(varargin{:}),'');

    for k=from_range_randomly(10,5)
        for n=from_range_randomly(10,5);
            for count=[0,from_range_randomly(100,10)];
                if count==0
                    args={k,n};
                    ncol=1;
                else
                    args={k,n,count};
                    ncol=count;
                end

                if k>n
                    % n too large; should throw error
                    aet(args{:});
                    continue;
                else
                    i=cosmo_sample_unique(args{:});
                    assertEqual(sort(i,1),i);

                    % check size
                    assert(isequal(size(i),[k,ncol]))

                    % check contents
                    i_vec=i(:);
                    assert(all(i_vec>=1 | i_vec<=n | round(i_vec)==i_vec));

                    % each element must be select about equally often,
                    % and each column must have unique elements
                    i_s=sort(i,1);

                    % each element unique
                    assert(all(all(diff(i_s,1,1)>0)));

                    % check counts
                    counts=zeros(n,1);
                    for col=1:ncol
                        counts(i(:,col))=counts(i(:,col))+1;
                    end

                    % counts differ at most by one
                    assert(min(counts)+1>=max(counts));
                end
            end
        end
    end

function test_sample_unique_full_randperm()
    k=2+from_range_randomly(10);
    n=k;
    c=5+from_range_randomly(10);

    i=cosmo_sample_unique(k,n,c);
    assertEqual(sort(i,1),repmat((1:k)',1,c));

function test_sample_unique_difference_sequences()
    k=2+from_range_randomly(10);
    n=k+from_range_randomly(10);
    c=5+from_range_randomly(10);

    seed=1+from_range_randomly(1e5);
    last_args={{'seed',seed},{'seed',[]'},{}};
    for k=1:numel(last_args)
        all_same=true;

        last_arg=last_args{k};
        use_seed=numel(last_arg)==2 && ~isempty(last_arg{2});

        for tries=1:5
            i=cosmo_sample_unique(k,n,c,last_arg{:});
            if tries==1
                i_first=i;
            elseif ~isequal(i_first,i)
                all_same=false;
                break;
            end
        end

        if xor(all_same,use_seed)
            assertFalse(true,'randomness mismatch with respect to seed');
        end
    end

function test_sample_unique_exceptions
    aet=@(varargin)assertExceptionThrown(@()...
                    cosmo_sample_unique(varargin{:}),'');

    bad_args={-1, 1.5, [1 1], struct(), false};
    for k=1:numel(bad_args)
        bad_arg=bad_args{k};

        for j=1:3
            all_args={1,1,1};
            all_args{j}=bad_arg;
            aet(all_args{:});
        end
    end

function vs=from_range_randomly(n, count)
    if nargin<=2
        count=1;
    end

    assert(n>=count);
    vals=1:n;

    rp=randperm(n);
    vs=vals(rp(1:count));


% function intersect = dtiFiberIntersectMesh(fibers, triangles, vertices)
%
% Given a set of 'fibers' and a surface mesh (defined by
% triangles/vertices), will find every triangle in the surface mesh
% that is intersected by the fibers.
%
% ARGUMENTS:
%
% fibers = an Nx1 cell array of fibers, where each cell contains a
%          3xN real array of points (XYZ order) that specify a fiber
%          path. A fiber must contain more than 1 point (or be empty).
%
% triangles = a 3xN uint32 array of triangles, where each entry is
%             an index into the vertices array.
%
% vertices = a 3xN double array of vertices in YXZ order.
%
% NOTE! We assume that the mesh vertices are X-Y swapped relative to
% the fiber vertices. (Because they are in our data representations.)
%
% RETURNS:
%
% intersect = an Nx1 cell array, where N = the number of fibers
%             (ie. length(fibers)). Each cell contains a 5xN array
%             where N is the number of intersections. It has the
%             following structure: [triangleIndex  fiberIndex X Y Z]
%
% NOTES:
% The hard work is done by the RAPID collision detection library. This
% is only free for non-commercial use, so you'll need to get the library
% from its maintainer if you want to rebuild the mex file. See:
% http://www.cs.unc.edu/~geom/OBB/OBBT.html or search google for
% "Robust and Accurate Polygon Interference Detection".
%
% To compile on Linux:
%    mex -O -I. dtiFiberIntersectMesh.cxx libRAPID.a
%
% on Windows:
%    mex -O -I. dtiFiberIntersectMesh.cxx RAPID.lib
%
% To make Rapid.lib on Windows, there is a directory
%    ..\mrDiffusion\src\RAPID_VStudio_201\MSVC_Compile
% with a Visual Studio Solution file.
%
% HISTORY:
% 2004.07.28 Bob Dougherty: wrote it.


function [ y, j, f ] = now_to_yjf_common ( )

%*****************************************************************************80
%
%% NOW_TO_YMDF_COMMON expresses the current date as a Common YJF date.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 April 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, integer Y, J, real F, the YJF date.
%
  c = clock ( );

  y1 = c(1);
  m1 = c(2);
  d1 = c(3);
  h1 = c(4);
  n1 = c(5);
  s1 = c(6);
  mu1 = 0.0;

  f1 = mu1;
  f1 = s1 + f1 / 1000.0;
  f1 = n1 + f1 / 60.0;
  f1 = h1 + f1 / 60.0;
  f1 = f1 / 24.0;

  [ y, j, f ] = ymdf_to_yjf_common ( y1, m1, d1, f1 );

  return
end


function [vclassifier, hclassifier] = mcmcTrainSegmentRegressor(features, labelprobs, weights)

ntrees = 20;
nnodes = 8;

maxdata = 30000;

for f = 1:numel(labelprobs)
    vprobs{f} = [labelprobs{f}(:, 1) sum(labelprobs{f}(:, 2:6), 2) labelprobs{f}(:, 7)];
    ind = find(vprobs{f}(:, 2)~=0);
    hprobs{f} = zeros(size(vprobs{f}, 1), 5);
    hprobs{f}(ind, :) = labelprobs{f}(ind, 2:6) ./ repmat(vprobs{f}(ind, 2), [1 5]);
end

[vdata, vlab, vw] = formatData(features, vprobs, weights, maxdata);
disp(numel(vw))
[hdata, hlab, hw] = formatData(features, hprobs, weights, maxdata);
disp(numel(hw))

vnames = {'000', '090', 'sky'};
hnames = {'045', '090', '135', 'por', 'sol'};

catids = 75;

vclassifier =  train_boosted_dt_mc(vdata, catids, vnames(vlab)', ntrees, nnodes, vw, vnames);

hclassifier =  train_boosted_dt_mc(hdata, catids, hnames(hlab)', ntrees, nnodes, hw, hnames);

function [data, lab, w] = formatData(features, probs, weights, maxdata)
% concatenate data

nimages = numel(features);
nlabels = size(probs{1}, 2);

[tmp, nvars] = size(features{1});

% count segments
nseg = 0;
for f = 1:nimages
    nseg = nseg + size(features{f}, 1);
end
disp(num2str(nseg))

data = zeros(nseg, nvars);
labprob = zeros(nseg, nlabels);
w = zeros(nseg, 1);

% concatenate data
vc = 0;
for f = 1:nimages
    ind = [1:size(features{f}, 1)];
    data(vc+1:vc+numel(ind), :) = features{f}(ind, :);
    labprob(vc+1:vc+numel(ind), :) = probs{f}(ind, :);
    w(vc+1:vc+numel(ind)) = weights{f}(ind);
    vc = vc + numel(ind);
%     for y = 1:nlabels
%         data(vc+1:vc+numel(ind), :) = features{f}(ind, :);
%         lab(vc+1:vc+numel(ind)) = y;
%         % weight according to (image area) * (label percentage)
%         w(vc+1:vc+numel(ind)) = weights{f}(ind).*probs{f}(ind, y);
%         vc = vc + numel(ind);
%     end
end

if nseg > maxdata
    rind = randperm(nseg);
    data = data(rind(1:maxdata), :);
    labprob = labprob(rind(1:maxdata), :);
    w = w(rind(1:maxdata));
    nseg = maxdata;
end
tmpw = w;

w = zeros(nseg*nlabels, 1);
lab = zeros(nseg*nlabels, 1);

data = repmat(data, [nlabels 1]);
for y = 1:nlabels
    lab((y-1)*nseg+1:y*nseg) = y;
    w((y-1)*nseg+1:y*nseg) = tmpw .* labprob(:, y);
end

% remove zero weight data
ind = find(w==0);
data(ind, :) = [];
lab(ind) = [];
w(ind) = [];

w = w / sum(w);


% CONVERTLOCS - Convert electrode locations between coordinate systems
%                 using the EEG.chanlocs structure.
%
% Usage: >> newchans = convertlocs( chanlocs, 'command');
%
% Input:
%   chanlocs  - An EEGLAB EEG dataset OR a EEG.chanlocs channel locations structure
%   'command' - ['cart2topo'|'sph2topo'|'sphbesa2topo'| 'sph2cart'|'topo2cart'|'sphbesa2cart'|
%               'cart2sph'|'sphbesa2sph'|'topo2sph'| 'cart2sphbesa'|'sph2sphbesa'|'topo2sphbesa'|
%               'cart2all'|'sph2all'|'sphbesa2all'|'topo2all']
%                These command modes convert between four coordinate frames: 3-D Cartesian
%                (cart), Matlab spherical (sph), Besa spherical (sphbesa), and 2-D polar (topo)
%               'auto' -- Here, the function finds the most complex coordinate frame
%                 and constrains all the others to this one. It searches first for Cartesian
%                 coordinates, then for spherical and finally for polar. Default is 'auto'.
%
% Optional input
%   'verbose' - ['on'|'off'] default is 'off'.
%
% Outputs:
%   newchans - new EEGLAB channel locations structure
%
% Ex:  CHANSTRUCT = convertlocs( CHANSTRUCT, 'cart2topo');
%      % Convert Cartesian coordinates to 2-D polar (topographic).
%
% Author: Arnaud Delorme, CNL / Salk Institute, 22 Dec 2002
%
% See also: READLOCS

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

function chans = convertlocs(chans, command, varargin)

if nargin < 1
   help convertlocs;
   return;
end

if ~isfield(chans, 'theta') && ~isfield(chans, 'X') && ~isfield(chans, 'radius') && ~isfield(chans, 'sph_theta_besa')
    return
end

if nargin < 2
   command = 'auto';
end
if nargin == 4 && strcmpi(varargin{2}, 'on')
    verbose = 1;
else
    verbose = 0; % off
end

% test if value exists for default
% --------------------------------
if strcmp(command, 'auto')
    if isfield(chans, 'X') && any(~cellfun(@isempty, { chans.X }))
        command = 'cart2all';
        if verbose
            disp('Make all coordinate frames uniform using Cartesian coords');
        end
    else
        if isfield(chans, 'sph_theta') && ~isempty(chans(1).sph_theta)
            command = 'sph2all';
            if verbose
                disp('Make all coordinate frames uniform using spherical coords');
            end
        else
            if isfield(chans, 'sph_theta_besa') && ~isempty(chans(1).sph_theta_besa)
                command = 'sphbesa2all';
                if verbose
                    disp('Make all coordinate frames uniform using BESA spherical coords');
                end
            else
                command = 'topo2all';
                if verbose
                    disp('Make all coordinate frames uniform using polar coords');
                end
            end
        end
    end
end

% convert
% -------
switch command
 case 'topo2sph'
   theta  = {chans.theta};
   radius = {chans.radius};
   indices = find(~cellfun('isempty', theta));
   [sph_phi, sph_theta] = topo2sph( [ [ theta{indices} ]' [ radius{indices}]' ] );
   if verbose
       disp('Warning: electrodes forced to lie on a sphere for polar to 3-D conversion');
   end
   for index = 1:length(indices)
      chans(indices(index)).sph_theta  = sph_theta(index);
      chans(indices(index)).sph_phi    = sph_phi  (index);
   end
   if isfield(chans, 'sph_radius')
       meanrad = mean([ chans(indices).sph_radius ]);
       if isempty(meanrad)
           [chans(indices).sph_radius] = deal(85);
           meanrad = 85;
       end
   else
        [chans(indices).sph_radius] = deal(85);
        meanrad = 85;
   end
   sph_radius(1:length(indices)) = {meanrad};
case 'topo2sphbesa'
   chans = convertlocs(chans, 'topo2sph', varargin{:}); % search for spherical coords
   chans = convertlocs(chans, 'sph2sphbesa', varargin{:}); % search for spherical coords
case 'topo2cart'
   chans = convertlocs(chans, 'topo2sph', varargin{:}); % search for spherical coords
   if verbose
       disp('Warning: spherical coordinates automatically updated');
   end
   chans = convertlocs(chans, 'sph2cart', varargin{:}); % search for spherical coords
case 'topo2all'
   chans = convertlocs(chans, 'topo2sph', varargin{:}); % search for spherical coords
   chans = convertlocs(chans, 'sph2sphbesa', varargin{:}); % search for spherical coords
   chans = convertlocs(chans, 'sph2cart', varargin{:}); % search for spherical coords
case 'sph2cart'
   sph_theta  = {chans.sph_theta};
   sph_phi    = {chans.sph_phi};
   indices = find(~cellfun('isempty', sph_theta));
   if ~isfield(chans, 'sph_radius')
        [chans(indices).sph_radius] = deal(85);
       sph_radius(1:length(indices)) = {85};
   else
       sph_radius = {chans.sph_radius};
   end
   inde = find(cellfun('isempty', sph_radius));
   if ~isempty(inde)
       meanrad = mean( [ sph_radius{:} ]);
       sph_radius(inde) = { meanrad };
   end
   [x, y, z] = sph2cart([ sph_theta{indices} ]'/180*pi, [ sph_phi{indices} ]'/180*pi, [ sph_radius{indices} ]');
   for index = 1:length(indices)
      chans(indices(index)).X = x(index);
      chans(indices(index)).Y = y(index);
      chans(indices(index)).Z = z(index);
   end
case 'sph2topo'
 if verbose
     % disp('Warning: all radii constrained to one for spherical to topo transformation');
 end
 sph_theta  = {chans.sph_theta};
 sph_phi    = {chans.sph_phi};
 indices = find(~cellfun('isempty', sph_theta));
 [chan_num,angle,radius] = sph2topo([ ones(length(indices),1)  [ sph_phi{indices} ]' [ sph_theta{indices} ]' ], 1, 2); % using method 2
 for index = 1:length(indices)
     chans(indices(index)).theta  = angle(index);
     chans(indices(index)).radius = radius(index);
     if ~isfield(chans, 'sph_radius') || isempty(chans(indices(index)).sph_radius)
         chans(indices(index)).sph_radius = 85;
     end
 end
case 'sph2sphbesa'
   % using polar coordinates
   sph_theta  = {chans.sph_theta};
   sph_phi    = {chans.sph_phi};
   indices = find(~cellfun('isempty', sph_theta));
   [chan_num,angle,radius] = sph2topo([ones(length(indices),1)  [ sph_phi{indices} ]' [ sph_theta{indices} ]' ], 1, 2);
   [sph_theta_besa, sph_phi_besa] = topo2sph([angle radius], 1, 1);
   for index = 1:length(indices)
      chans(indices(index)).sph_theta_besa  = sph_theta_besa(index);
      chans(indices(index)).sph_phi_besa    = sph_phi_besa(index);
   end
case 'sph2all'
   chans = convertlocs(chans, 'sph2topo', varargin{:}); % search for spherical coords
   chans = convertlocs(chans, 'sph2sphbesa', varargin{:}); % search for spherical coords
   chans = convertlocs(chans, 'sph2cart', varargin{:}); % search for spherical coords
case 'sphbesa2sph'
   % using polar coordinates
   sph_theta_besa  = {chans.sph_theta_besa};
   sph_phi_besa    = {chans.sph_phi_besa};
   indices = find(~cellfun('isempty', sph_theta_besa));
   [chan_num,angle,radius] = sph2topo([ones(length(indices),1)  [ sph_theta_besa{indices} ]' [ sph_phi_besa{indices} ]' ], 1, 1);
   %for index = 1:length(chans)
   %   chans(indices(index)).theta  = angle(index);
   %   chans(indices(index)).radius = radius(index);
   %   chans(indices(index)).labels = int2str(index);
   %end;
   %figure; topoplot([],chans, 'style', 'blank', 'electrodes', 'labelpoint');

   [sph_phi, sph_theta] = topo2sph([angle radius], 2);
   for index = 1:length(indices)
      chans(indices(index)).sph_theta  = sph_theta(index);
      chans(indices(index)).sph_phi    = sph_phi  (index);
   end
case 'sphbesa2topo'
   chans = convertlocs(chans, 'sphbesa2sph', varargin{:}); % search for spherical coords
   chans = convertlocs(chans, 'sph2topo', varargin{:}); % search for spherical coords
case 'sphbesa2cart'
   chans = convertlocs(chans, 'sphbesa2sph', varargin{:}); % search for spherical coords
   chans = convertlocs(chans, 'sph2cart', varargin{:}); % search for spherical coords
case 'sphbesa2all'
   chans = convertlocs(chans, 'sphbesa2sph', varargin{:}); % search for spherical coords
   chans = convertlocs(chans, 'sph2all', varargin{:}); % search for spherical coords
case 'cart2topo'
   chans = convertlocs(chans, 'cart2sph', varargin{:}); % search for spherical coords
   chans = convertlocs(chans, 'sph2topo', varargin{:}); % search for spherical coords
case 'cart2sphbesa'
   chans = convertlocs(chans, 'cart2sph', varargin{:}); % search for spherical coords
   chans = convertlocs(chans, 'sph2sphbesa', varargin{:}); % search for spherical coords
case 'cart2sph'
    if verbose
        disp('WARNING: If XYZ center has not been optimized, optimize it using Edit > Channel Locations');
    end
    X  = {chans.X};
    Y  = {chans.Y};
    Z  = {chans.Z};
    indices = find(~cellfun('isempty', X));
    [th, phi, radius] = cart2sph( [ X{indices} ], [ Y{indices} ], [ Z{indices} ]);
	for index = 1:length(indices)
		 chans(indices(index)).sph_theta     = th(index)/pi*180;
		 chans(indices(index)).sph_phi       = phi(index)/pi*180;
		 chans(indices(index)).sph_radius    = radius(index);
	end
case 'cart2all'
   chans = convertlocs(chans, 'cart2sph', varargin{:}); % search for spherical coords
   chans = convertlocs(chans, 'sph2all', varargin{:}); % search for spherical coords
end


function tests = test_ft_preproc_standardize

% MEM 1gb
% WALLTIME 00:10:00
% DEPENDENCY ft_preproc_standardize

if nargout
  % assume that this is called by RUNTESTS
  tests = functiontests(localfunctions);
else
  % assume that this is called from the command line
  fn = localfunctions;
  for i=1:numel(fn)
    feval(fn{i});
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function testOptions(testCase)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

nchan   = 8;
nsample = 1000;
dat     = randn(nchan, nsample) + 1;

state = [];

result = [];
result{end+1} = ft_preproc_standardize(dat, 1, 1000, state);
result{end+1} = ft_preproc_standardize(dat, 1, 900, state);
result{end+1} = ft_preproc_standardize(dat, 101, 1000, state);
result{end+1} = ft_preproc_standardize(dat, 101, 900, state);

% all iterations were done with (slightly) different options, hence the results should not be equal
for i=1:numel(result)
  for j=(i+1):numel(result)
    assert(~isequal(result{i}, result{j}), 'the results %d and %d should not be equal', i, j);
  end
end


function [ut, itn] = project_lin_sys( work, data, n, m, u, v, rho_x, i, use_indirect, cg_rate, extra_verbose, h, g, gTh)
    ut = u+v;
    ut(1:n) = rho_x*ut(1:n);
    ut(1:n+m) = ut(1:n+m) - ut(end)*h;
    ut(1:n+m) = ut(1:n+m) - h*((g'*ut(1:n+m))/(gTh+1));
    warm_start = u(1:n+m);
    ut(n+1:end-1) = -ut(n+1:end-1);
    [ut(1:n+m), itn] = solve_lin_sys(work, data, ut(1:n+m), n, m, warm_start, rho_x, i, use_indirect, cg_rate, extra_verbose);
    ut(end) = (ut(end) + h'*ut(1:n+m));
end


function epoch = mydatestdi (epoch_std, year)
    [year_len, year_start] = mydatestd_aux(year);

    std_year_len = mydatestd_aux();

    epoch = epoch_std ./ std_year_len .* year_len + year_start;

    if any(epoch_std > 2.*std_year_len) ...
    || any(epoch_std <   -std_year_len)
        error('MATLAB:mydatestdi:outRange', ...
        'First argument is out of valid input range.');
    end

    year = repmat(year, length(epoch_std), 1);

    idx = (epoch_std > std_year_len);
    if any(idx)
        epoch(idx) = mydatestdi(epoch_std(idx)-std_year_len, year(idx)+1);
    end

    idx = (epoch_std < 0);
    if any(idx)
        epoch(idx) = mydatestdi(std_year_len-abs(epoch_std(idx)), year(idx)-1);
    end
end

%!test
%! % mydatestdi()
%! test('mydatestd_aux')


%     Steffen Urban email: steffen.urban@kit.edu
%     Copyright (C) 2014  Steffen Urban
%
%     This program is free software; you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation; either version 2 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License along
%     with this program; if not, write to the Free Software Foundation, Inc.,
%     51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.

clear all
close all

% !!! change those if you changed Step1_perform_test_calibrations.m !!!
nr_cams    = 7;
nr_tests   = 5;

%% load scaramuzza data
for i=1:nr_tests
    path = sprintf('CalibData%i.mat', i);
    calibMeth{i} = load(path);
end

for cam=1:nr_cams
    for meth = 1:nr_tests
        rms(cam,meth) = calibMeth{meth}.calib_data{cam}.rms;
    end
end

figure
bar(rms)
title('root mean square error')
xlabel('data set')
ylabel('root mean square error [pixel]')
legend('ocam standard','ocam subpixel','urban', 'urban subpixel' ,'urban subpixel robust')

for cam=1:nr_cams
    for meth = 1:nr_tests
        runtime(cam,meth) = calibMeth{meth}.calib_data{cam}.runtime;
    end
end

figure
bar(runtime)
title('runtime')
xlabel('data set')
ylabel('time [s]')
legend('ocam standard','ocam subpixel','urban', 'urban subpixel' ,'urban subpixel robust')

badIdx = [];
for cam=1:nr_cams
    for meth = 1:2
        stdEOangle = [];
        stdEOpos = [];
        for imgs = calibMeth{meth}.calib_data{cam}.ima_proc
            try % if an image was omitted during corner extraction
            if (~isempty(calibMeth{meth}.calib_data{cam}.statEO{imgs}.stdEO))
                stdEOangle = [stdEOangle
                              1e3*[calibMeth{meth}.calib_data{cam}.statEO{imgs}.stdEO(1)
                                   calibMeth{meth}.calib_data{cam}.statEO{imgs}.stdEO(2)
                                   calibMeth{meth}.calib_data{cam}.statEO{imgs}.stdEO(3)]];
                stdEOpos   = [stdEOpos
                              [calibMeth{meth}.calib_data{cam}.statEO{imgs}.stdEO(4)
                              calibMeth{meth}.calib_data{cam}.statEO{imgs}.stdEO(5)
                              calibMeth{meth}.calib_data{cam}.statEO{imgs}.stdEO(6)]];
               badIdx = [badIdx, imgs];
            end
            catch

            end
        end
        stdEOangleA(cam,meth) = mean(stdEOangle);
        stdEOposA(cam,meth) = mean(stdEOpos);
        badIdx = [];
    end
end

lauf = 0;
for cam=1:nr_cams
    for meth = 3:nr_tests
        stdEOangle = [];
        stdEOpos = [];
        for imgs = 1:length(calibMeth{meth}.calib_data{cam}.ima_proc)
            stdEOangle = [stdEOangle
                          1e3*[calibMeth{meth}.calib_data{cam}.statEO.stdEO(1+lauf)
                               calibMeth{meth}.calib_data{cam}.statEO.stdEO(2+lauf)
                               calibMeth{meth}.calib_data{cam}.statEO.stdEO(3+lauf)]];
            stdEOpos   = [stdEOpos
                          [calibMeth{meth}.calib_data{cam}.statEO.stdEO(4+lauf)
                          calibMeth{meth}.calib_data{cam}.statEO.stdEO(5+lauf)
                          calibMeth{meth}.calib_data{cam}.statEO.stdEO(6+lauf)]];
            lauf = lauf+6;
        end
        lauf = 0;
        stdEOangleA(cam,meth) = median(stdEOangle);
        stdEOposA(cam,meth) = median(stdEOpos);
    end
end

figure
bar(stdEOangleA)
title('mean standard deviation of camera orientations')
xlabel('data set')
ylabel('mean standard deviation[mrad]')
legend('ocam standard','ocam subpixel','urban', 'urban subpixel' ,'urban subpixel robust')

figure
bar(stdEOposA)
title('mean standard deviation of camera positions')
xlabel('data set')
ylabel('standard deviation [mm]')
legend('ocam standard','ocam subpixel','urban', 'urban subpixel' ,'urban subpixel robust')

%% IO, c and a0
lauf = 0;
for cam=1:nr_cams
    for meth = 1:nr_tests
        std_cde(cam,meth) = calibMeth{meth}.calib_data{cam}.statIO.stdIO(3);
        std_a0(cam,meth) = calibMeth{meth}.calib_data{cam}.statIO.stdIO(6);
    end
end


function status = test_wfs_iir_prefilter(modus)
%TEST_WFS_IIR_PREFILTER tests the IIR WFS pre-equalization filter
%
%   Usage: status = test_wfs_iir_prefilter(modus)
%
%   Input parameters:
%       modus   - 0: numerical
%                 1: visual (not available)
%
%   Output parameters:
%       status - true or false
%
%   TEST_WFS_IIR_PREFILTER(modus) test the WFS pre-euqalization IIR filter
%   design. This works only in Matlab as the Signal Processing Toolbox is used.
%   See wfs_iir_prefilter.m for details

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

status = false;

%% ===== Checking of input parameters ====================================
nargmin = 1;
nargmax = 1;
narginchk(nargmin,nargmax);

%% ===== Configuration ===================================================
conf = SFS_config;

%% ===== Calculation =====================================================
% call with default values
hpre1 = wfs_iir_prefilter(conf)
conf.fs = 44100;
conf.hpreflow = 200;
conf.hprefhigh = 1500;
conf.hpreBandwidth_in_Oct = 2;
conf.hpreIIRorder = 4;
hpre2 = wfs_iir_prefilter(conf)

status = true;


%+========================================================================+
%|                                                                        |
%|            This script uses the GYPSILAB toolbox for Matlab            |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : nrtMshClean.m                                 |
%|    #    |   VERSION    : 0.53                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 14.03.2019                                    |
%| ( === ) |   SYNOPSIS   : Clean tetrahedral mesh                        |
%|  `---'  |                                                              |
%+========================================================================+

% Cleaning
clear all
close all
clc

% Gypsilab path
run('../../addpathGypsilab.m')

% Load mesh
load tetmesh

% Build mesh
mesh1 = msh(X,tet);

% Graphical representation
figure
plot(mesh1)
axis equal
view(45,45)

% Add vertices to tetrahedral mesh
tmp = [zeros(10,3) ; 10*rand(10,3)];
tet = tet + size(tmp,1);
X   = [tmp ; X ; 10*rand(20,3)];

% Build and clean mesh
mesh2 = msh(X,tet);

% Test mesh egality
~isequal(mesh1,mesh2)

% Graphical representation
figure
plot(mesh2)
axis equal
view(45,45)

% Colours
mesh = mshSquare(20,[1 1]);
ctr  = mesh.ctr;
mesh.col(ctr(:,1)<0)  = 1;
mesh.col(ctr(:,1)>=0) = 2;

% Graphical representation
figure
plot(mesh)
axis equal
view(45,45)

% Sub-meshing with small translation
delta     = 1e-2;
mesh1     = mesh.sub(mesh.col==1);
mesh2     = mesh.sub(mesh.col==2);
mesh2.vtx = mesh2.vtx + delta;
meshT     = union(mesh1,mesh2);

% Graphical representation
figure
plot(meshT)
axis equal
view(45,45)

% Clean with specified range
stp   = mesh.stp;
meshT = clean(meshT,0.5*stp(1));

disp('~~> Michto gypsilab !')


% ManhNMF: Manhattan NMF (Guan et al. 2013)
% process_video('NMF', 'ManhNMF', 'dataset/demo.avi', 'output/demo_ManhNMF.avi');
rank = 1;
[W,H] = ManhNMF(M,rank);
L = W' * H;
S = M - L;


function [x]=als_solve_rx(mat, rhs, tol, drx, nswp, addswp)
%Computes an approximate low-rank solution for 2D case
%   [x]=ALS_SOLVE_RX(MAT, RHS, [TOL], [RX], [NSWP])
%   Finds a solution to 2D TTM matrix MAT using the ALS to a 2D TT tensor
%   with rank rx, but the RHS and X are represented as full vectors.
%   TOL is the tolerance for ||x_{i+1}-x_i||/||x_i||,
%   DRX is the random kick rank,
%   NSWP - number of ALS sweeps.
%   default values:
%   tol: 1e-12
%   rx: 1
%   nswp: 10
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------

a1 = mat{1}; a2 = mat{2};
n1 = size(a1,1); m1 = size(a1,2);
n2 = size(a2,1); m2 = size(a2,2);
ra = size(a1,3);

% tol2 = 1e-3;

rhs = reshape(rhs, n1, n2);

if (nargin<3)||(isempty(tol))
    tol=1e-12;
end;
if (nargin<4)||(isempty(drx))
    drx=1;
end;
if (nargin<5)||(isempty(nswp))
    nswp=10;
end;
if (nargin<6)||(isempty(addswp))
    addswp=2;
end;

if (drx>m1)||(drx>m2)
    drx = min(m1,m2);
end;

rx=1;

curx = cell(2,1);
curx{1}=rand(m1,rx);
curx{2}=rand(m2,rx);
x = curx{1}*(curx{2}.');
derr = 2;
sp = 0;
resid_old = 1;
for swp=1:nswp

    % QR 2-1
    [q,rv]=qr(curx{2},0); % m2,rx' - rx',rx
    rx = size(q,2);
    curx{2}=q;
%     curx{1}=curx{1}*(rv.');

    % compute phi
    a2 = permute(mat{2}, [1 3 2]);
    a2 = reshape(a2, n2*ra, m2);
    phi = a2*curx{2}; % size n2*ra, rx
    phi = reshape(phi, n2, ra*rx);
    phi = (phi')*phi; % size ra*rx, ra*rx <-- for cplx should also work
    phi = reshape(phi, ra, rx, ra, rx);
    phi = reshape(permute(phi, [1 3 2 4]), ra*ra, rx*rx);
%     phi = reshape(permute(phi, [3 1 2 4]), ra, ra*rx*rx);
%     a2 = reshape(mat{2}, n2, m2*ra);
%     phi = (a2.')*phi; % size m2*ra, ra*rx
%     phi = reshape(phi, m2, ra*ra*rx);
%     phi = (curx{2}.')*phi; % size rx, ra*ra*rx
%     phi = reshape(permute(reshape(phi, rx, ra, ra, rx), [2 3 1 4]), ra*ra, rx*rx);
%     % And the projection of the matrix
    a1 = reshape(permute(mat{1}, [3 2 1]), ra*m1, n1);
    a1 = conj(a1)*reshape(mat{1}, n1, m1*ra); % size ra*m1, m1*ra <-- conjugate!
    a1 = reshape(a1, ra, m1, m1, ra);
    a1 = reshape(permute(a1, [1 4 2 3]), ra*ra, m1*m1);
%     a1 = reshape(permute(mat{1}, [3 2 1]), ra, m1*n1);
    a1 = (phi.')*a1; % size rx*rx, m1*n1
    a1 = reshape(a1, rx, rx, m1, m1);
    a1 = reshape(permute(a1, [2 4 1 3]), rx*m1, rx*m1);
%     a1 = reshape(mat{1}, n1*m1, ra)*phi; % size n1*m1, ra*rx*rx
%     a1 = reshape(a1, n1, m1, ra, rx, rx);
%     a1 = reshape(permute(a1, [1 3 2 4 5]), n1*ra, m1*rx*rx);
%     a1 = conj(reshape(permute(mat{1}, [2 1 3]), m1, n1*ra))*a1; % size m1, m1*rx*rx
%     a1 = reshape(a1, m1, m1, rx, rx);
%     a1 = reshape(permute(a1, [3 1 4 2]), rx*m1, rx*m1);

    %rhs:

    rhs1 = rhs*conj(reshape(mat{2}, n2, m2*ra)); % size n1, m2*ra <-- conjugate
    rhs1 = reshape(rhs1, n1, m2, ra);
    rhs1 = reshape(permute(rhs1, [1 3 2]), n1*ra, m2);

    rhs1 = conj(reshape(permute(mat{1}, [2 1 3]), m1, n1*ra))*rhs1; % size m1, m2
%     rhs1 = rhs;
    rhs1 = rhs1*conj(curx{2}); % size m1,rx
    rhs1 = reshape(rhs1.', rx*m1, 1);

    curx{1}=a1 \ rhs1; % new first block
%     cond_a1 = cond(a1)
    curx{1}=reshape(curx{1}, rx, m1).';

    % Now, let's try the kickass by rank drx:
    if (mod(swp,addswp)==0)
%     if (sp>5)
        curx{1}=[curx{1}, randn(m1,drx)];
%         sp=0;
    end;
%     rx=rx+1;

    % Now, let's compute the second block
    [q,rv]=qr(curx{1},0); % m1,rx' - rx',rx
    rx = size(q,2);
    curx{1}=q;

    % compute phi
    a1 = permute(mat{1}, [1 3 2]);
    a1 = reshape(a1, n1*ra, m1);
    phi = a1*q; % size n1*ra, rx
    phi = reshape(phi, n1, ra*rx);
    phi = (phi')*phi; % size ra*rx, ra*rx
    phi = reshape(phi, ra, rx, ra, rx);
    phi = reshape(permute(phi, [1 3 2 4]), ra*ra, rx*rx);
%     a1 = reshape(mat{1}, n1, m1*ra);
%     phi = (a1.')*phi; % size m1*ra, ra*rx
%     phi = reshape(phi, m1, ra*ra*rx);
%     phi = (curx{1}.')*phi; % size rx, ra*ra*rx
%     phi = reshape(permute(reshape(phi, rx, ra, ra, rx), [2 3 1 4]), ra*ra, rx*rx);
    % And the projection of the matrix
    a2 = reshape(permute(mat{2}, [3 2 1]), ra*m2, n2);
    a2 = conj(a2)*reshape(mat{2}, n2, m2*ra); % size ra*m2, m2*ra
    a2 = reshape(a2, ra, m2, m2, ra);
    a2 = reshape(permute(a2, [1 4 2 3]), ra*ra, m2*m2);
%     a2 = reshape(permute(mat{2}, [3 2 1]), ra, m2*n2);
    a2 = (phi.')*a2; % size rx*rx, m2*n2
    a2 = reshape(a2, rx, rx, m2, m2);
    a2 = reshape(permute(a2, [2 4 1 3]), rx*m2, rx*m2);

    %rhs:
    rhs2 = rhs*conj(reshape(mat{2}, n2, m2*ra)); % size n1, m2*ra
    rhs2 = reshape(rhs2, n1, m2, ra);
    rhs2 = reshape(permute(rhs2, [1 3 2]), n1*ra, m2);

    rhs2 = conj(reshape(permute(mat{1}, [2 1 3]), m1, n1*ra))*rhs2; % size m1, m2
%     rhs2 = rhs;
    rhs2 = (curx{1}')*rhs2; % size rx,m2
    rhs2 = reshape(rhs2, rx*m2, 1);

    curx{2}=a2 \ rhs2; % new first block
    curx{2}=reshape(curx{2}, rx, m2).';

    x_new = curx{1}*(curx{2}.'); % size m1,m2
    derr = norm(x_new(:)-x(:))/norm(x(:));

    x = x_new;

    resid = norm(tt_mat_full_vec(mat, x(:))-rhs(:))/norm(rhs(:));
    conv_fact = resid_old/resid;
    if (conv_fact-1<1e-4)
        sp=sp+1;
    end;

    fprintf('als_solve: swp=%d, derr=%3.3e, rx=%d, resid=%3.3e, conv-1=%3.5e\n', swp, derr, rx, resid, conv_fact-1);
    if (derr<tol)
        break;
    end;
    resid_old = resid;
end;

x = x(:);

end


function [ a, b] = p03_interval ( )

%*****************************************************************************80
%
%% P03_INTERVAL returns a starting interval for optimization for problem 3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 February 2002
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real A, B, two points defining an interval in which
%    the local minimizer should be sought.
%
  a = -2.0;
  b = +2.0;

  return
end


% SVM_TEST Calculate labels for an SVM model
%
% Usage
%    [labels, votes, feature_labels] = SVM_TEST(db, model, test_set)
%
% Input
%    db (struct): The database containing the feature vector.
%    model (struct): The affine space model obtained from svm_train.
%    test_set (int): The object indices of the testing instances.
%
% Output
%    labels (int): The assigned labels.
%    votes (int): The number of votes for each testing instance and class
%       pair.
%    feature_labels (int): The labels assigned to the individual features.
%
% See also
%    SVM_TRAIN, CLASSIF_ERR, CLASSIF_RECOG

function [labels,votes,feature_labels,K,sv_coef,dec] = svm_test(db,model,test_set)
	% Determine indices of feature vectors belonging to the test objects.
	ind_features = [db.indices{test_set}];

	if ~model.full_test_kernel && ...
		(model.svm.Parameters(2) == 4 || ...
		model.svm.Parameters(2) == 5 || ...
		model.svm.Parameters(2) == 6 || ...
		model.svm.Parameters(2) == 7)
		% If we're working with a precalculated kernel and only some of the testing
		% vectors are available, only use these (unless full_test_kernel is set).

		[kernel_mask,kernel_ind] = ismember(1:size(db.features,2),db.kernel.kernel_set);
		ind_features = ind_features(kernel_mask(ind_features));
	end

	% Obtain the feature vector labels for ind_features.
	[feature_labels,temp,K,sv_coef,dec] = svm_feature_test(db,model.svm,ind_features,model.full_test_kernel);

	for l = 1:length(test_set)
		% For each object in the test set, count the votes assigned to each of its feature vectors.

		ind = find(ismember(ind_features,db.indices{test_set(l)}));
		votes(:,l) = histc(feature_labels(ind),1:length(db.src.classes));
		[temp,labels(l)] = max(votes(:,l));
	end
end

function [labels,votes,K,sv_coef,dec] = svm_feature_test(db,model,ind_features0,full_test_kernel)
	% As in SVM_CALC_KERNEL, it's useful to break down kernel computations into
	% blocks.
	block_size = 8192;

	% How many classes are in this model?
	class_ct = model.nr_class;

	% Prepare the matrix of support vector coefficients for each pair of classes,
	% since each corresponds to a different SVM.
	sv_coef = zeros(model.totalSV,class_ct*(class_ct-1)/2,class(db.features));

	pairs = [];

	% Fill the sv_coef matrix. For more details on the model.sv_coef matrix, see
	% the LIBSVM FAQ. The difference with our sv_coefs is that our stores the
	% coefficients for each class pair, with zeros for the vectors belonging to
	% neither class.
	r = 1;
	for n1 = 1:class_ct
		for n2 = n1+1:class_ct
			pairs = [pairs [n1; n2]];

			class1_SVs = 1+sum(model.nSV(1:n1-1)):sum(model.nSV(1:n1));
			class2_SVs = 1+sum(model.nSV(1:n2-1)):sum(model.nSV(1:n2));

			sv_coef(class1_SVs,r) = model.sv_coef(class1_SVs,n2-1);
			sv_coef(class2_SVs,r) = model.sv_coef(class2_SVs,n1);
			r = r+1;
		end
	end

	% Prepare matrix of decision values for each testing vector and class pair.
	dec = zeros(length(ind_features0),size(sv_coef,2),class(db.features));

	if full_test_kernel && ...
		(model.Parameters(2) == 4 || ...
		model.Parameters(2) == 5 || ...
		model.Parameters(2) == 6 || ...
		model.Parameters(2) == 7)
		% If we have to recalculate kernel, prepare square norms. This should only
		% be necessary in Gaussian kernel case, though (6 & 7)...
		norm2 = sum(abs(db.features(:,db.kernel.kernel_set(model.SVs))).^2,1);
	end

	n = 1;
	while n <= length(ind_features0)
		% Only deal with testing vectors block by block.
		mask = n:min(n+block_size-1,length(ind_features0));
		ind_features = ind_features0(mask);

		if model.Parameters(2) == 0			% linear kernel
			% Linear kernel was calculated in LIBSVM; recalculate it here.
			K = db.features(:,ind_features).'*model.SVs.';
		elseif model.Parameters(2) == 2		% Gaussian kernel
			% Gaussian kernel was calculated in LIBSVM; recalculate it here.
			norm1 = sum(abs(db.features(:,ind_features)).^2,1);
			model.SVs=full(model.SVs);
			norm2 = sum(abs(model.SVs.').^2,1);
			norm2=full(norm2);
			K = bsxfun(@plus,norm1.',norm2)-...
				2*db.features(:,ind_features).'*model.SVs.';
			K = exp(-model.Parameters(4)*K);
		elseif full_test_kernel && ...
			(model.Parameters(2) == 4 || ...
			model.Parameters(2) == 5 || ...
			model.Parameters(2) == 6 || ...
			model.Parameters(2) == 7)
			% Kernel was precalculated, but for testing, we have to recalculate it. This
			% can be necessary if the set of kernel vectors is restricted.

			if strcmp(db.kernel.kernel_type,'linear')
				% Calculate the linear kernel of the testing vector with respect to the
				% support vectors.
				K = db.features(:,ind_features).'*db.features(:,db.kernel.kernel_set(model.SVs));
			elseif strcmp(db.kernel.kernel_type,'gaussian')
				% Same for Gaussian kernel, except that we don't multiply by gamma and
				% exponentiate here. This is done later, as in the precalculated case.
				norm1 = sum(abs(db.features(:,ind_features)).^2,1);
				K = bsxfun(@plus,norm1.',norm2)-...
					2*db.features(:,ind_features).'*db.features(:,db.kernel.kernel_set(model.SVs));
			else
				error('unknown kernel type');
			end
		elseif model.Parameters(2) == 4	|| ...
			model.Parameters(2) == 6
			% Precalculated kernel in square format, so just extract the rows corre-
			% sponding to the support vectors and the columns corresponding to the
			% testing vectors.

			% Translate to the kernel indexation.
			[kernel_mask,kernel_ind] = ismember(1:size(db.features,2),db.kernel.kernel_set);
			K = db.kernel.K(1+model.SVs,kernel_ind(ind_features)).';
		elseif model.Parameters(2) == 5 || ...
			model.Parameters(2) == 7
			% Precalculated kernel in triangular format. Need to unpack the kernel for
			% desired support vectors and testing vectors.

			% Prepare the kernel matrix.
			K = zeros(length(ind_features),length(model.SVs),class(db.kernel.K));

			% Obtain kernel indexation.
			[kernel_mask,kernel_ind] = ismember(1:size(db.features,2),db.kernel.kernel_set);

			for k = 1:length(ind_features)
				% For each of the testing vectors, extract the indices in the triangular
				% kernel.

				% Translate to the kernel indexation.
				idx = kernel_ind(ind_features(k));

				% Separate support vectors into those with index below testing vector index
				% and those above. Need to cast to integers since floating-point indexing
				% causes problems. Take 64 bits to be safe...
				lower_SVs = int64(model.SVs(model.SVs<=idx));
				upper_SVs = int64(model.SVs(model.SVs>idx));

				% Lower SVs are obtained from the SV rows in the idx column.
				K(k,model.SVs<=idx) = ...
					db.kernel.K((idx-1)*(idx-1+3)/2+1+lower_SVs);
				% Upper SVs are obtained from the idx row in the SV columns.
				K(k,model.SVs>idx) = ...
					db.kernel.K((upper_SVs-1).*(upper_SVs-1+3)/2+1+idx);
			end
		end

		if (model.Parameters(2) == 4 && strcmp(db.kernel.kernel_type,'gaussian')) || ...
			model.Parameters(2) == 6 || model.Parameters(2) == 7
			% If we have a Gaussian kernel, we need to multiply by gamma and exponentia-
			% te.
			K = exp(-model.Parameters(4)*K);
		end

		% Calculate the decision values for the current block. See LIBSVM FAQ for
		% the detailed formula.
		dec(mask,:) = bsxfun(@minus,K*sv_coef,model.rho.');

		n = n+length(mask);
	end

	% Prepare the votes matrix, which will contain the votes assigned to each
	% class for each testing vector.
	votes = zeros(size(dec,1),class_ct);

	for r = 1:size(pairs,2)
		% For each pair, calculate the "winners" and add one to their tally.
		n1 = pairs(1,r);
		n2 = pairs(2,r);

		votes(dec(:,r)>=0,n1) = votes(dec(:,r)>=0,n1)+1;
		votes(dec(:,r)<0,n2) = votes(dec(:,r)<0,n2)+1;
	end

	% Assign the class to that with the largest number of votes.
	[temp,labels] = max(votes,[],2);

	% Translate labels to those used in the model.
	labels = model.Label(labels);
end


function [irf_record,D_record,gamma_record,struct_irf_record,irf_estimates,D_estimates,gamma_estimates,strshocks_record,strshocks_estimates]=...
    panel5irf(Y,Xdot,theta_gibbs,sigma_gibbs,Xi,It,Bu,IRFperiods,IRFband,N,n,m,p,k,T,IRFt,favar)

% create first the cells storing the results
irf_record={};
irf_estimates={};

% deal with shocks in turn (there are N*n shocks with this model)
for ii=1:N*n

   % start iterating
   for jj=1:It-Bu

   % draw theta from its posterior distribution
   theta=theta_gibbs(:,jj);

   % create a matrix of zeros of dimension p*(N*n)
   Ysim=zeros(p,N*n);
   %  step 2: set the value of the last row, column i, equal to 1
   Ysim(p,ii)=1;

      % step 4: for each iteration kk, repeat the algorithm for periods T+1 to T+h
      for kk=1:IRFperiods-1

      % use the function lagx to obtain a matrix temp, containing the endogenous regressors
      temp=bear.lagx(Ysim,p-1);

      % define the vector X
      Xsim=[temp(end,:) zeros(1,m)];

      % obtain Xbar et Xtilde
      Xbar=kron(speye(N*n),Xsim);
      Xtilde=Xbar*Xi;

      % obtain the predicted value for T+kk
      yp=Xtilde*theta;

      % concatenate yp at the top of Y
      Ysim=[Ysim;yp'];

      % repeat until values are obtained for T+h
      end

      % record the results from current iteration in cell irf_record
      % loop over variables
      for kk=1:N*n
      % consider column kk of matrix Ysim and trim the (p-1) initial periods: what remains is the series of IRFs for period T to period T+h-1, for variable kk
      temp=Ysim(p:end,kk);
      % record these values in the corresponding matrix of irf_record
      irf_record{kk,ii}(jj,:)=temp';
      end

   % then go for next iteration
   end

% conduct the same process with shocks in other variables
end

% then apply the structural decomposition (if applicable)

% if IRFs have been set to an unrestricted VAR (IRFt=1):
if IRFt==1
% run a pseudo Gibbs sampler to obtain records for D and gamma (for the trivial SVAR)
[D_record,gamma_record]=bear.irfunres(N*n,It,Bu,sigma_gibbs);
struct_irf_record=[];
% compute posterior estimates
[irf_estimates,D_estimates,gamma_estimates]=bear.irfestimates(irf_record,N*n,IRFperiods,IRFband,IRFt,[],[],favar);
% if IRFs have been set to an SVAR with Choleski identification (IRFt=2):
elseif IRFt==2
% run the Gibbs sampler to transform unrestricted draws into orthogonalised draws
[struct_irf_record,D_record,gamma_record]=bear.irfchol(sigma_gibbs,irf_record,It,Bu,IRFperiods,N*n,favar);
% compute posterior estimates
[irf_estimates,D_estimates,gamma_estimates]=bear.irfestimates(struct_irf_record,N*n,IRFperiods,IRFband,IRFt,D_record,gamma_record,favar);
elseif IRFt==3
% run the Gibbs sampler to transform unrestricted draws into orthogonalised draws
[struct_irf_record,D_record,gamma_record]=bear.irftrig(sigma_gibbs,irf_record,It,Bu,IRFperiods,N*n,favar);
% compute posterior estimates
[irf_estimates,D_estimates,gamma_estimates]=bear.irfestimates(struct_irf_record,N*n,IRFperiods,IRFband,IRFt,D_record,gamma_record,favar);
end

% also, if a s structural identification was implemented, compute structural shocks
strshocks_record={};
strshocks_estimates={};
if IRFt~=1
% run the Gibbs sampler
[strshocks_record]=bear.strshockspan5(theta_gibbs,Xi,D_record,Y,Xdot,N*n,k,T,It,Bu);
% obtain point estimates and credibility intervals
[strshocks_estimates]=bear.strsestimates(strshocks_record,N*n,T,IRFband);
% reshape
strshocks_estimates=reshape(strshocks_estimates,[n 1 N]);
end