%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Q = INVERSEKINEMATIC_KUKA_KR5_ARC(robot, T)
%   Solves the inverse kinematic problem for the KUKA KR5 ARC robot
%   where:
%   robot stores the robot parameters.
%   T is an homogeneous transform that specifies the position/orientation
%   of the end effector.
%
%   A call to Q=INVERSEKINEMATIC__KUKA_KR5_ARC returns 8 possible solutions, thus,
%   Q is a 6x8 matrix where each column stores 6 feasible joint values.
%
%
%   Example code:
%
%   robot=load_robot('KUKA', 'KR160_R1570_nanoC');
%   q = [0 0 0 0 0 0];
%   T = directkinematic(robot, q);
%   %Call the inversekinematic for this robot
%   qinv = inversekinematic(robot, T);
%   check that all of them are feasible solutions!
%   and every Ti equals T
%   for i=1:8,
%        Ti = directkinematic(robot, qinv(:,i))
%   end
%	See also DIRECTKINEMATIC.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE 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.
%
% ARTE 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 ARTE.  If not, see <http://www.gnu.org/licenses/>.
function q = inversekinematic_kuka_kr160_r1570_nanoC(robot, T)

%initialize q,
%eight possible solutions are generally feasible
q=zeros(6,8);

% %Evaluate the parameters
% theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
L6=abs(d(6));

%T= [ nx ox ax Px;
%     ny oy ay Py;
%     nz oz az Pz];
Px=T(1,4);
Py=T(2,4);
Pz=T(3,4);

%Compute the position of the wrist, being W the Z component of the end effector's system
W = T(1:3,3);

% Pm: wrist position
Pm = [Px Py Pz]' + L6*W;

%first joint, two possible solutions admited:
% if q(1) is a solution, then q(1) + pi is also a solution
q1=atan2(Pm(2), Pm(1));

%solve for q2
q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm);
%the other possible solution is q1 + pi
q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm);

%solve for q3
q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm);
%solver for q3 for both cases
q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm);

%the next matrix doubles each column. For each two columns, two different
%configurations for theta4, theta5 and theta6 will be computed. These
%configurations are generally referred as wrist up and wrist down solution
q = [q1         q1         q1        q1       q1+pi   q1+pi   q1+pi   q1+pi;
     q2_1(1)    q2_1(1)    q2_1(2)   q2_1(2)  q2_2(1) q2_2(1) q2_2(2) q2_2(2);
     q3_1(1)    q3_1(1)    q3_1(2)   q3_1(2)  q3_2(1) q3_2(1) q3_2(2) q3_2(2);
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0];
%leave only the real part of the solutions
q=real(q);

%Note that in this robot, the joint q3 has a non-simmetrical range. In this
%case, the joint ranges from 60 deg to -219 deg, thus, the typical normalizing
%step is avoided in this angle (the next line is commented). When solving
%for the orientation, the solutions are normalized to the [-pi, pi] range
%only for the theta4, theta5 and theta6 joints.

%normalize q to [-pi, pi]
q(1,:) = normalize(q(1,:));
q(2,:) = normalize(q(2,:));

% solve for the last three joints
% for any of the possible combinations (theta1, theta2, theta3)
for i=1:2:size(q,2),
    qtemp = solucion_muneca_esferica(robot, q(:,i), T, 1,'geometric'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i)=qtemp;

    qtemp = solucion_muneca_esferica(robot, q(:,i), T, -1, 'geometric'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i+1)=qtemp;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for second joint theta2, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q2 = solve_for_theta2(robot, q, Pm)

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%See geometry
L2=abs(a(2));
L3=abs(d(4));
A2 = abs(a(3));

%See geometry of the robot
%compute L4
L4 = sqrt(A2^2 + L3^2);

%The inverse kinematic problem can be solved as in the IRB 140 (for example)

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

beta = atan2(-p1(2), p1(1));
gamma = real(acos((L2^2+r^2-L4^2)/(2*r*L2)));

%return two possible solutions
%elbow up and elbow down
%the order here is important and is coordinated with the function
%solve_for_theta3
q2(1) = pi/2 - beta - gamma; %elbow up
q2(2) = pi/2 - beta + gamma; %elbow down

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for third joint theta3, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q3 = solve_for_theta3(robot, q, Pm)

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%See geometry
L2=abs(a(2));
L3=abs(d(4));

A2 = abs(a(3));

%See geometry of the robot
%compute L4
L4 = sqrt(A2^2 + L3^2);

%the angle phi is fixed
phi=acos((A2^2+L4^2-L3^2)/(2*A2*L4));

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

eta = real(acos((L2^2 + L4^2 - r^2)/(2*L2*L4)));

%return two possible solutions
%elbow up and elbow down solutions
%the order here is important
q3(1) = phi - eta;
q3(2) = -2*pi +( phi + eta); %%%?????????????????????????????????????????

function q = solucion_muneca_esferica(robot, q, T, wrist, method)

switch method

     %algebraic solution
    case 'algebraic'
        T01=dh(robot, q, 1);
        T12=dh(robot, q, 2);
        T23=dh(robot, q, 3);

        Q=inv(T23)*inv(T12)*inv(T01)*T;

        %detect the degenerate case when q(5)=0, this leads to zeros
        % in Q13, Q23, Q31 and Q32 and Q33=1
        thresh=1e-12;
        %detect if q(5)==0
        % this happens when cos(q5) in the matrix Q is close to 1
        if abs(Q(3,3)-1)>thresh
            %normal solution
            if wrist==1 %wrist up
                q(4)=atan2(-Q(2,3),-Q(1,3));
                q(6)=atan2(-Q(3,2),Q(3,1));
                %q(5)=atan2(-Q(3,2)/sin(q(6)),Q(3,3));
            else %wrist down
                q(4)=atan2(-Q(2,3),-Q(1,3))-pi;
                q(6)=atan2(-Q(3,2),Q(3,1))+pi;
                %q(5)=atan2(-Q(3,2)/sin(q(6)),Q(3,3));
            end
            if abs(cos(q(6)+q(4)))>thresh
                cq5=(Q(1,1)+Q(2,2))/cos(q(4)+q(6))-1;
            end
            if abs(sin(q(6)+q(4)))>thresh
                cq5=(-Q(1,2)+Q(2,1))/sin(q(4)+q(6))-1;
            end
            if abs(sin(q(6)))>thresh
                sq5=-Q(3,2)/sin(q(6));
            end
            if abs(cos(q(6)))>thresh
                sq5=Q(3,1)/cos(q(6));
            end
            q(5)=atan2(sq5,cq5);

        else %degenerate solution, in this case, q4 cannot be determined,
             % so q(4)=0 is assigned
            if wrist==1 %wrist up
                q(4)=0;
                q(5)=0;
                q(6)=atan2(-Q(1,2)+Q(2,1),Q(1,1)+Q(2,2));
            else %wrist down
                q(4)=-pi;
                q(5)=0;
                q(6)=atan2(-Q(1,2)+Q(2,1),Q(1,1)+Q(2,2))+pi;
            end

        end

       %geometric solution
    case 'geometric'
        % T is the noa matrix defining the position/orientation of the end
        % effector's reference system
        vx6=T(1:3,1);
        vz5=T(1:3,3); % The vector a z6=T(1:3,3) is coincident with z5

        % Obtain the position and orientation of the system 3
        % using the already computed joints q1, q2 and q3
        T01=dh(robot, q, 1);
        T12=dh(robot, q, 2);
        T23=dh(robot, q, 3);
        T03=T01*T12*T23;

        vx3=T03(1:3,1);
        vy3=T03(1:3,2);
        vz3=T03(1:3,3);

        % find z4 normal to the plane formed by z3 and a
        vz4=cross(vz3, vz5);	% end effector's vector a: T(1:3,3)

        % in case of degenerate solution,
        % when vz3 and vz6 are parallel--> then z4=0 0 0, choose q(4)=0 as solution
        if norm(vz4) <= 0.000001
            if wrist == 1 %wrist up
                q(4)=0;
            else
                q(4)=-pi; %wrist down
            end
        else
            %this is the normal and most frequent solution
            cosq4=wrist*dot(vy3,vz4);
            sinq4=wrist*dot(-vx3,vz4);
            q(4)=atan2(sinq4, cosq4);
        end
        %propagate the value of q(4) to compute the system 4
        T34=dh(robot, q, 4);
        T04=T03*T34;
        vx4=T04(1:3,1);
        vy4=T04(1:3,2);

        % solve for q5
        cosq5=dot(-vy4,vz5);
        sinq5=dot(vx4,vz5);
        q(5)=atan2(sinq5, cosq5);

        %propagate now q(5) to compute T05
        T45=dh(robot, q, 5);
        T05=T04*T45;
        vx5=T05(1:3,1);
        vy5=T05(1:3,2);

        % solve for q6
        cosq6=dot(vx6,vx5);
        sinq6=dot(vx6,vy5);
        q(6)=atan2(sinq6, cosq6);

    otherwise
        disp('no method specified in solve_spherical_wrist');
end


function [R, nt] = buildr(S, varargin)

%BUILDR Builds response matrix for input to functions ENTROPY and
% INFORMATION
%
%   ------
%   SYNTAX
%   ------
%
%       [R, nt] = build_R(S, R1, R2, ..., RL)
%
%   -----------
%   DESCRIPTION
%   -----------
%   Let's consider an experiment in which, during each trial, a stimulus is
%   presented out of NS available stimuli and L distinct neural responses
%   are recorded simultaneously. Within this framework the input to BUILDR
%   is as follow:
%   - S is the stimulus-array, i.e., S(i) stores the value of the stimulus
%     presented during the i-th trial.
%   - Rj, j=1,...,L, is the j-th response array, i.e., Rj(i) stores the
%     value of the j-th response recorded during the i-th experiment.
%
%   The function will return the response matrix R which can be input to
%   the routines ENTROPY and INFORMATION. The routine also outputs the
%   trials per stimulus array, NT, which can be used as one of the
%   parameters of the option structure for ENTROPY and INFORMATION.
%
%   -------
%   REMARKS
%   -------
%
%   - Although the one described above is the framework which was kept in
%     mind while creating the toolbox functions, it has to be noted that
%     this function (and also the other in the toolbox) can be easily
%     applied to several other situations.
%
%   - The goal of this function is to help the user get familiar with the
%     structure of the response-matrix R which is input to the ENTROPY and
%     INFORMATION functions and, in particular, with the fact that the
%     stimulus values are not provided to these functions. This is because,
%     for the sake of information computation the only important parameter
%     concerning the stimulus is the number of times each stimulus was
%     presented: the value of each stimulus can thus be mapped to an
%     integer value and made implicit as the index to a page of the
%     response matrix R. It needs to be noted, however, that BUILDR, having
%     to be as generic as possible, will also be relatively slow. Often,
%     the way responses are recorded or computed allows building a response
%     matrix much more quickly than the way done by BUILR. For
%     computationally intensive tasks it is thus suggested that custom
%     routines are used instead of this built-in tool.
%
%   See also ENTROPY, INFORMATION

%   Copyright (C) 2009 Cesare Magri
%   Version: 1.0.4

% -------
% LICENSE
% -------
% This software is distributed free under the condition that:
%
% 1. it shall not be incorporated in software that is subsequently sold;
%
% 2. the authorship of the software shall be acknowledged and the following
%    article shall be properly cited in any publication that uses results
%    generated by the software:
%
%      Magri C, Whittingstall K, Singh V, Logothetis NK, Panzeri S: A
%      toolbox for the fast information analysis of multiple-site LFP, EEG
%      and spike train recordings. BMC Neuroscience 2009 10(1):81;
%
% 3.  this notice shall remain in place in each source file.

if ~isvector(S)
    error('Stimulus array must be a 1-D array');
end

totNt = length(S);

L = length(varargin);

R1toL = zeros(L, totNt);
for k=1:L
    if ~isvector(varargin{k})
        msg = 'Response arrays must be 1-D.';
        error('buildr:respNot1D', msg);
    end

    if length(varargin{k}) ~= totNt
        msg = 'Each response-array must have the same length as the stimulus array';
        error('buildr:differentTotNt', msg);
    end

    R1toL(k,:) = varargin{k};
end

uniqueS = unique(S);
Ns = length(uniqueS);

% Dispalying informations:
disp('Building R and nt:');
disp(['- number of stimuli = ' num2str(Ns)]);
disp(['- number of responses = ' num2str(L)]);

nt = zeros(Ns,1);
tFlag = false(totNt, Ns);
for s=1:Ns
    tFlag(:,s) = S==uniqueS(s);
	nt(s) = sum(tFlag(:,s));
end

maxNt = max(nt);
disp(['- maximum numer of trials = ' num2str(maxNt)]);
disp(['- minimum numer of trials = ' num2str(min(nt))]);
R = zeros(L, maxNt, Ns);
for s=1:Ns
    if nt(s)>0
        R(:,1:nt(s), s) = R1toL(:, tFlag(:,s));
    else
        msg = 'One or more stimuli with no corresponding response.';
        error('buildr:noResponseStimulus', msg);
    end
end


% Copyright (C) 1994-2015 John W. Eaton
%
% This file is part of Octave.
%
% Octave 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.
%
% Octave 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 Octave; see the file COPYING.  If not, see
% <http://www.gnu.org/licenses/>.

% -*- texinfo -*-
% @deftypefn  {Function File} {[@var{h}, @var{w}] =} freqz (@var{b}, @var{a}, @var{n}, "whole")
% @deftypefnx {Function File} {[@var{h}, @var{w}] =} freqz (@var{b})
% @deftypefnx {Function File} {[@var{h}, @var{w}] =} freqz (@var{b}, @var{a})
% @deftypefnx {Function File} {[@var{h}, @var{w}] =} freqz (@var{b}, @var{a}, @var{n})
% @deftypefnx {Function File} {@var{h} =} freqz (@var{b}, @var{a}, @var{w})
% @deftypefnx {Function File} {[@var{h}, @var{w}] =} freqz (@dots{}, @var{Fs})
% @deftypefnx {Function File} {} freqz (@dots{})
%
% Return the complex frequency response @var{h} of the rational IIR filter
% whose numerator and denominator coefficients are @var{b} and @var{a},
% respectively.
%
% The response is evaluated at @var{n} angular frequencies between 0 and
% @ifnottex
% 2*pi.
% @end ifnottex
% @tex
% $2\pi$.
% @end tex
%
% @noindent
% The output value @var{w} is a vector of the frequencies.
%
% If @var{a} is omitted, the denominator is assumed to be 1 (this
% corresponds to a simple FIR filter).
%
% If @var{n} is omitted, a value of 512 is assumed.  For fastest computation,
% @var{n} should factor into a small number of small primes.
%
% If the fourth argument, @qcode{'whole'}, is omitted the response is
% evaluated at frequencies between 0 and
% @ifnottex
% pi.
% @end ifnottex
% @tex
% $\pi$.
% @end tex
%
% @code{freqz (@var{b}, @var{a}, @var{w})}
%
% Evaluate the response at the specific frequencies in the vector @var{w}.
% The values for @var{w} are measured in radians.
%
% @code{[@dots{}] = freqz (@dots{}, @var{Fs})}
%
% Return frequencies in Hz instead of radians assuming a sampling rate
% @var{Fs}.  If you are evaluating the response at specific frequencies
% @var{w}, those frequencies should be requested in Hz rather than radians.
%
% @code{freqz (@dots{})}
%
% Plot the magnitude and phase response of @var{h} rather than returning them.
%
% @seealso{freqz_plot}
% @end deftypefn

% Author: jwe ???

function [h_r, f_r] = freqz (b, a, n, region, Fs)

  if (nargin < 1 || nargin > 5)
    print_usage ();
  elseif (nargin == 1)
    % Response of an FIR filter.
    a = [];
    n = [];
    region = [];
    Fs = [];
  elseif (nargin == 2)
    % Response of an IIR filter
    n = [];
    region = [];
    Fs = [];
  elseif (nargin == 3)
    region = [];
    Fs = [];
  elseif (nargin == 4)
    Fs = [];
    if (~ischar (region) && ~isempty (region))
      Fs = region;
      region = [];
    end
  end

  if (isempty (b))
    b = 1;
  elseif (~isvector (b))
    error ('freqz: B must be a vector');
  end
  if (isempty (a))
    a = 1;
  elseif (~isvector (a))
    error ('freqz: A must be a vector');
  end
  if (isempty (n))
    n = 512;
  elseif (isscalar (n) && n < 1)
    error ('freqz: N must be a positive integer');
  end
  if (isempty (region))
    if (isreal (b) && isreal (a))
      region = 'half';
    else
      region = 'whole';
    end
  end
  if (isempty (Fs))
    freq_norm = true;
    if (nargout == 0)
      Fs = 2;
    else
      Fs = 2*pi;
    end
  else
    freq_norm = false;
  end
  plot_output = (nargout == 0);
  whole_region = strcmp (region, 'whole');

  a = a(:);
  b = b(:);

  if (~isscalar (n))
    % Explicit frequency vector given
    w = n;
    f = n;
    if (nargin == 4)
      % Sampling rate Fs was specified
      w = 2*pi*f/Fs;
    end
    k = max (length (b), length (a));
    hb = polyval (oc_postpad (b, k), exp (j*w));
    ha = polyval (oc_postpad (a, k), exp (j*w));
  else
    % polyval(fliplr(P),exp(jw)) is O(p n) and fft(x) is O(n log(n)),
    % where p is the order of the polynomial P.  For small p it
    % would be faster to use polyval but in practice the overhead for
    % polyval is much higher and the little bit of time saved isn't
    % worth the extra code.
    k = max (length (b), length (a));
    if (k > n/2 && nargout == 0)
      % Ensure a causal phase response.
      n = n * 2 .^ ceil (log2 (2*k/n));
    end

    if (whole_region)
      N = n;
      if (plot_output)
        f = Fs * (0:n).' / N;    % do 1 more for the plot
      else
        f = Fs * (0:n-1).' / N;
      end
    else
      N = 2*n;
      if (plot_output)
        n = n+1;
      end
      f = Fs * (0:n-1).' / N;
    end

    pad_sz = N*ceil (k/N);
    b = oc_postpad (b, pad_sz);
    a = oc_postpad (a, pad_sz);

    hb = zeros (n, 1);
    ha = zeros (n, 1);

    for i = 1:N:pad_sz
      fftb = fft(oc_postpad (b(i:i+N-1), N));
      ffta = fft(oc_postpad (a(i:i+N-1), N));
      hb = hb + fftb(1:n);
      ha = ha + ffta(1:n);
    end

  end

  h = hb ./ ha;

  if (plot_output)
    % Plot and don't return values.
    if (whole_region)
      h(end+1) = h(1); % Solution is periodic.  Copy first value to end.
    end
    freqz_plot (f, h, freq_norm);
  else
    % Return values and don't plot.
    h_r = h;
    f_r = f;
  end

end


%% Stereo Image Matching
%
% Example of stereo image matching to produce a disparity map and point cloud
% generation.
%
% Resulting .ply file can also be viewed using
% <http://meshlab.sourceforge.net/ MeshLab>.
%
% Sources:
%
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/python/stereo_match.py>
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/cpp/stereo_match.cpp>
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/gpu/stereo_match.cpp>
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/cpp/tutorial_code/calib3d/stereoBM/SBM_Sample.cpp>
% * <https://docs.opencv.org/3.2.0/dd/d53/tutorial_py_depthmap.html>
%

%% Theory
%
% Previously, we saw basic concepts like epipolar constraints and other
% related terms. We also saw that if we have two images of same scene, we can
% get depth information from that in an intuitive way. Below is an image and
% some simple mathematical formulas which prove that intuition:
%
% <<https://docs.opencv.org/3.2.0/stereo_depth.jpg>>
%
% The above diagram contains equivalent triangles. Writing their equivalent
% equations will yield us following result:
%
% $$disparity = x - x' = \frac{Bf}{Z}$$
%
% $x$ and $x'$ are the distance between points in image plane corresponding to
% the scene point 3D and their camera center. $B$ is the distance between two
% cameras (which we know) and $f$ is the focal length of camera (already
% known). So in short, the above equation says that the depth of a point in a
% scene is inversely proportional to the difference in distance of
% corresponding image points and their camera centers. So with this
% information, we can derive the depth of all pixels in an image.
%
% So it finds corresponding matches between two images. We have already seen
% how epiline constraint make this operation faster and accurate. Once it
% finds matches, it finds the disparity.
%

%% Code

function stereo_match_demo()
    %% Images
    % load pair of images
    % (SGBM works with either grayscale or color images, BM only grayscale)
    imgL = cv.imread(fullfile(mexopencv.root(),'test','aloeL.jpg'), 'Color',true);
    imgR = cv.imread(fullfile(mexopencv.root(),'test','aloeR.jpg'), 'Color',true);
    subplot(121), imshow(imgL), title('Left')
    subplot(122), imshow(imgR), title('Right')

    %%
    % downscale for faster processing
    scale = 0.5;
    [imgL, imgR] = scale_images(imgL, imgR, scale);
    whos imgL imgR
    [h,w,cn] = size(imgL);

    %%
    % disparity-to-depth mapping 4x4 matrix, used to compute point cloud
    if true
        % manually enter Q matrix
        % (turns points 180 deg around x-axis, so that y-axis looks up)
        f = 0.8*w;  % guess for local length
        Q = [1 0 0 -0.5*w; 0 -1 0 0.5*h; 0 0 0 -f; 0 0 1 0];
    elseif false
        % images must be rectified, if not we rectify and get Q matrix
        % (requires calibrated stereo camera, see stereo_calibration_demo.m)
        intrinsicFile = fullfile(tempdir(), 'stereo_intrinsic.yml');
        extrinsicFile = fullfile(tempdir(), 'stereo_extrinsic.yml');
        [imgL, imgR, Q] = rectify_images(imgL, imgR, scale, ...
            intrinsicFile, extrinsicFile);
    else
        % when empty, point cloud generation is skipped
        Q = [];
    end
    display(Q)

    %% Stereo Matching
    % create stereo matcher, with params tuned for 'aloe' image pair
    win_size = 3;
    min_disp = 16;             % 0
    num_disp = 112 - min_disp; % fix(w/8) + 15
    num_disp = double(bitand(int32(num_disp), int32(-16))); % divisible by 16
    stereo = cv.StereoSGBM('MinDisparity',min_disp, 'NumDisparities',num_disp, ...
        'BlockSize',16, 'P1',8*cn*win_size^2, 'P2',32*cn*win_size^2, ...
        'Disp12MaxDiff',1, 'UniquenessRatio',10, ...
        'SpeckleWindowSize',100, 'SpeckleRange',32, 'Mode','SGBM');
    display(stereo)

    %%
    % compute disparity map (from a pair of rectified stereo images)
    tic, D = stereo.compute(imgL, imgR); toc
    fprintf('16-bit disparity map: min=%d, max=%d\n', min(D(:)), max(D(:)));
    D = single(D) / 16; % fixed-point numbers with 4 fractional bits -> float
    DD = (D - stereo.MinDisparity) / stereo.NumDisparities; % normalized [0,1]
    figure, imshow(DD), colorbar, title('Disparity')

    %% Point Cloud
    if ~isempty(Q)
        % generate 3d point cloud
        ptc = create_point_cloud(D, Q, imgL, false);
        fprintf('%d point cloud\n', ptc.Count);

        % write point cloud to PLY file
        plyfile = fullfile(tempdir(), 'aloe.ply');
        write_point_cloud(ptc, plyfile);
        fprintf('Point cloud saved to: %s\n', plyfile);

        % visualize point cloud
        if ~mexopencv.isOctave()
            %HACK: Octave hangs if we plot too many scatter points
            figure, vis_point_cloud(ptc);
            title('Point Cloud'); xlabel('X'); ylabel('Y'); zlabel('Z');
            %axis([-10 10 -10 10 -20 0])
        end
    end
end

%% Helper functions

function [imgL, imgR] = scale_images(imgL, imgR, scale)
    if scale ~= 1
        if scale < 1
            interpo = 'Area';
        else
            interpo = 'Cubic';
        end
        imgL = cv.resize(imgL, scale, scale, 'Interpolation',interpo);
        imgR = cv.resize(imgR, scale, scale, 'Interpolation',interpo);
    end
end

function [imgL, imgR, Q] = rectify_images(imgL, imgR, scale, intrinsicFile, extrinsicFile)
    % load params from previously calibrated stereo camera
    assert(exist(intrinsicFile, 'file') == 2, 'missing intrinsic params');
    assert(exist(extrinsicFile, 'file') == 2, 'missing extrinsic params');
    I = cv.FileStorage(intrinsicFile);  % intrinsic: M1, D1, M2, D2
    E = cv.FileStorage(extrinsicFile);  % extrinsic: R, T

    % account for new image size by scaling camera matrix: fx, fy, cx, cy
    I.M1 = I.M1 * scale;
    I.M2 = I.M2 * scale;

    % Note: we assume that cameras were calibrated using images of same size
    % as the original image size here, i.e: [I.width, I.height] == sz/scale)
    sz = [size(imgL,2) size(imgL,1)];

    % re-rectify using scaled image size
    RCT = cv.stereoRectify(I.M1, I.D1, I.M2, I.D2, sz, E.R, E.T);
    Q = RCT.Q;

    % apply rectification
    [map11, map12] = cv.initUndistortRectifyMap(I.M1, I.D1, sz, ...
        'P',RCT.P1, 'R',RCT.R1);
    [map21, map22] = cv.initUndistortRectifyMap(I.M2, I.D2, sz, ...
        'P',RCT.P2, 'R',RCT.R2);
    imgL = cv.remap(imgL, map11, map12, 'Interpolation','Linear');
    imgR = cv.remap(imgR, map21, map22, 'Interpolation','Linear');
end

function ptc = create_point_cloud(D, Q, imgL, cvst)
    xyz = cv.reprojectImageTo3D(D, Q);
    mask = repmat(D > min(D(:)), [1 1 3]);  % where disparity was not computed
    xyz = reshape(xyz(mask), [], 3);
    if size(imgL,3) == 3
        clr = reshape(imgL(mask), [], 3);
    else
        clr = repmat(imgL(mask(:,:,1)), 1, 3);
    end

    if nargin < 4, cvst = true; end
    if cvst && ~mexopencv.isOctave() && mexopencv.require('vision')
        ptc = pointCloud(xyz, 'Color',clr);
    else
        %HACK: pointCloud and related functions are not implemented in Octave
        ptc = struct('Location',xyz, 'Color',clr, 'Count',size(xyz,1));
    end
end

function vis_point_cloud(ptc)
    if isobject(ptc)
        pcshow(ptc);
    else
        scatter3(ptc.Location(:,1), ptc.Location(:,2), ptc.Location(:,3), ...
            6, single(ptc.Color)/255, '.')
        axis tight vis3d
        rotate3d on
    end
end

function write_point_cloud(ptc, fname)
    if isobject(ptc)
        pcwrite(ptc, fname, 'Encoding','ascii');
    else
        fid = fopen(fname, 'wt');
        fprintf(fid, 'ply\n');
        fprintf(fid, 'format ascii 1.0\n');
        fprintf(fid, 'element vertex %d\n', ptc.Count);
        fprintf(fid, 'property float x\n');
        fprintf(fid, 'property float y\n');
        fprintf(fid, 'property float z\n');
        fprintf(fid, 'property uchar red\n');
        fprintf(fid, 'property uchar green\n');
        fprintf(fid, 'property uchar blue\n');
        fprintf(fid, 'end_header\n');
        fprintf(fid, '%f %f %f %d %d %d\n', [ptc.Location single(ptc.Color)].');
        fclose(fid);
    end
end


%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled non-linear
% 	fluid-structure eraction models. This version of the code is based off of
%	Peskin's Immersed Boundary Method Paper in Acta Numerica, 2002.
%
% Author: Nicholas A. Battista
% Email:  nick.battista@unc.edu
% Date Created: May 27th, 2015
% Institution: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs)
% 	3. Target Pos
%	4. Muscle-Model (combined Force-Length-Velocity model, "Hill+(Length-Tension)")
%
% One is able to update those Lagrangian Structure parameters, e.g., spring ants, resting lengths, etc
%
% There are a number of built in Examples, mostly used for teaching purposes.
%
% If you would like us to add a specific muscle model, please let Nick (nick.battista@unc.edu) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: updates the beam attributes!
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function beams_info = update_nonInv_Beams(dt,current_time,beams_info)

% beams_info:   col 1: 1ST PT.
%               col 2: MIDDLE PT. (where force is exerted)
%               col 3: 3RD PT.
%               col 4: beam stiffness
%               col 5: x-curvature
%               col 6: y-curvature

% GEOMETRIC PARAMETERS
pi = 4*atan(1);
L1 = 8;                              % length of computational domain (m)
N1 = 256;                            % number of Cartesian grid meshwidths at the finest level of the AMR grid
bell_length = 2;                     % bell length (m)
npts_bell = ceil(2.0*(bell_length/L1)*N1);  % number of pos along the length of the bell
npts_circ = 1;                       %number of pos along the circumference (if in 3D)
npts = npts_bell*npts_circ;	         % total number pos
ds1 = bell_length/(npts_bell-1);     % mesh spacing(m) along length of bell

% Values from Alben, Peng, and Miller
betao = 0.5;
betam = 0.3;
to = 0.4;
Zs = L1/8;
xRef = L1/4;

 %These are used to keep track of cycle number and time into the cycle
pulse_time = current_time-floor(current_time); % determine time since beginning of first pulse

    % GIVE BELL STATES
    if (pulse_time<to)  %contract bell
        beta = betao+(betam-betao)*(pulse_time/to);
    else                % expand bell
        beta = betam+(betao-betam)*((pulse_time-to)/(1-to));
    end

    %---------------------
    %Xb and Yb are calculated here and will be used to determine new curvatures
    s1=0;
    zl = Zs;
    ro = xRef;

    % Pre-allocate memory for speed
    Xb_lam = zeros(npts,1);
    Yb_lam = zeros(npts,1);

    %top of bell
    Xb_lam(1)=ro;
    Yb_lam(1)=zl;

    %right side of bell
    for s1 = 2:ceil(npts_bell/2)
        thetaj = -1.55*(1-exp(-(s1)*ds1/beta));
        zl = zl + ds1*sin(thetaj);
        ro = ro + ds1*cos(thetaj);
        Xb_lam(s1)=ro;
        Yb_lam(s1)=zl;
    end

    zl = Zs;
    ro = xRef;

    %left side of bell
    for s1 = (ceil(npts_bell/2))+1:npts_bell
        s2=s1-(ceil(npts_bell/2));
        thetaj = -1.55*(1-exp(-(s2)*ds1/beta));
        zl = zl + ds1*sin(thetaj);
        ro = ro - ds1*cos(thetaj);
        Xb_lam(s1)=ro;
        Yb_lam(s1)=zl;
    end

% CHANGE CURVATURE BTWN SIDES OF JELLYFISH BELL
for s = 1:length( beams_info(:,1) )

    s1 = beams_info(s,2); % gives lag_idx of the middle node

    %top of bell
    if (s1==1)
         beams_info(s,5) = Xb_lam(ceil(npts/2)+1)+Xb_lam(2)-2*Xb_lam(s1);
         beams_info(s,6) = Yb_lam(ceil(npts/2)+1)+Yb_lam(2)-2*Yb_lam(s1);
    elseif ( s1<ceil(npts/2) )
    % right side of bell
        beams_info(s,5) = Xb_lam(s1-1)+Xb_lam(s1+1)-2*Xb_lam(s1);
        beams_info(s,6) = Yb_lam(s1-1)+Yb_lam(s1+1)-2*Yb_lam(s1);
    elseif ( s1==ceil(npts/2)+1 )
    % top-left side of bell
         beams_info(s,5) = Xb_lam(s1+1)+Xb_lam(1)-2*Xb_lam(s1);
         beams_info(s,6) = Yb_lam(s1+1)+Yb_lam(1)-2*Yb_lam(s1);
    elseif (s1 > ceil(npts/2)+1)
    % left side of bell
         beams_info(s,5) = Xb_lam(s1+1)+Xb_lam(s1-1)-2*Xb_lam(s1);
         beams_info(s,6) = Yb_lam(s1+1)+Yb_lam(s1-1)-2*Yb_lam(s1);
    end

end


function [DCM] = VBA_spm_dcm_fmri_check(P)
% post-hoc diagnostics for DCM (bilinear or nonlinear) of fMRI data
% FORMAT [DCM] = spm_dcm_fmri_check(DM)
% DCM - DCM structure or its filename
%
% This routine provides some diagnostics to ensure model inversion has
% converged. It plots the predicted and observed responses over all regions
% and provides the coefficient of determination ? or percent variance
% explained. This should normally be above 10%. An abnormally low
% coefficient of determination is highlighted in red. Quantitatively, one
% would normally expect to see one or more extrinsic (between source)
% connections with the strength of 1/8 Hz or greater. If all the extrinsic
% posterior expectations are below this value, then this suggests a failure
% of convergence or that the data are very noisy (possibly due to using
% very small regions of interest to summarise regional responses). Finally,
% the posterior correlations among all parameters are shown in terms of a
% correlation matrix. The number of effective parameters estimated is
% reported in terms of the (KL) divergence between the posterior and
% prior densities over parameters. This is divided by the log of the
% number of observations, by appealing to the Bayesian information
% criterion. The divergence corresponds to complexity or Bayesian
% surprise. Normally, one would expect the posterior and prior to diverge
% in a non-trivial fashion.
%
% Posterior densities are shown as bars with 90% confidence intervals in
% pink. An informed model inversion would normally provide posterior
% densities with confidence intervals that are, for some connections,
% displaced from prior expectations (at or around zero).
%
% This routine is compatible with DCM8, DCM10 and DCM12 files.
%__________________________________________________________________________
% Copyright (C) 20012 Wellcome Trust Centre for Neuroimaging
% Karl Friston

% $Id: spm_dcm_fmri_check.m karl $

%-Load DCM structure
%--------------------------------------------------------------------------
if ~nargin
  uiopen('load');
elseif isstruct(P)
  DCM = P;
else
  load(P)
end

% Assemble diagnostics
%==========================================================================
% coefficient of determination (percent variance explained)
%--------------------------------------------------------------------------
PSS  = sum(sum(DCM.y.^2));
RSS  = sum(sum(DCM.R.^2));
D(1) = 100*PSS/(PSS + RSS);

% largest absolute posterior expectation (extrinsic connections)
%--------------------------------------------------------------------------
try
   A = DCM.Ep.A;
catch
   A = DCM.A;
end
D(2) = max(max(abs(A - diag(diag(A)))));

% complexity and effective number of parameters estimated
%--------------------------------------------------------------------------
qE = VBA_spm_vec(DCM.Ep);
pE = VBA_spm_vec(DCM.M.pE);
qC = DCM.Cp;
pC = DCM.M.pC;
k  = rank(full(pC));
pC = VBA_spm_inv(pC);

D(3) = (trace(pC*qC) + (pE - qE)'*pC*(pE - qE) - VBA_spm_logdet(qC*pC) - k)/2;
D(3) = D(3)/log(DCM.v);

% Plot summary of inversion
%==========================================================================
VBA_spm_figure('GetWin','DCM diagnostics'); clf

% plot predicted and observed regional responses
%--------------------------------------------------------------------------
subplot(2,1,1);
t = (1:DCM.v)*DCM.Y.dt;

plot(t,DCM.y,t,DCM.y + DCM.R,':');
str = sprintf('variance explained %0.0f%%', D(1));
str = {'responses and predictions',str};
if D(1) > 10
  title(str,'FontSize',16);
else
  title(str,'FontSize',16,'Color','r');
end
xlabel('time {seconds}');

% posterior densities over A parameters
%--------------------------------------------------------------------------
try
  i = VBA_spm_fieldindices(DCM.Ep,'A');
catch
  i = 1 + (1:DCM.n^2);
end
qE = VBA_spm_vec(DCM.Ep);
qC = DCM.Cp;
subplot(2,2,3)
VBA_spm_plot_ci(qE(i),qC(i,i)), hold on
str = sprintf('largest connection strength %0.2f', D(2));
str = {'intrinsic and extrinsic connections',str};
if D(2) > 1/8
  title(str,'FontSize',16);
else
  title(str,'FontSize',16,'Color','r');
end
xlabel('parameter}');
axis square

% posterior correlations among all parameters
%--------------------------------------------------------------------------
subplot(2,2,4)
imagesc(VBA_cov2corr(DCM.Cp))
title('posterior correlations','FontSize',16)
str = sprintf('estimable parameters %0.0f', D(3));
str = {'posterior correlations',str};
if D(3) > 1
  title(str,'FontSize',16);
else
  title(str,'FontSize',16,'Color','r');
end
axis square


% GP_COV - This is a general documentation file for using covariance
%          functions.
%
% The covariance functions are used similarly. The covariance function is
% initialized by giving some function specific inputs, for instance,
%
%   COVFUNC = GP_COV_EXAMPLE(P1, P2, ...)
%
% where the number of inputs and their meaning depends on the covariance
% function. The input arguments can be, for instance, a (squared) distance
% matrix between the inputs, the dimensionality of the input space or
% another covariance function. Thus, be careful especially when changing
% covariance functions that you also change the inputs as needed! For
% instance, some covariance functions may use a distance matrix while
% some others a squared distance matrix.
%
% The returned covariance function COVFUNC can be used to obtain the
% covariance matrix by giving a parameter vector THETA:
%
%   K = COVFUNC(THETA)
%   [K, DK] = COVFUNC(THETA)
%
% where K is the covariance matrix and DK is a cell array of derivatives of
% K with respect to the parameters THETA. The length of THETA and the
% meaning of its elements depends on the covariance function. The parameters
% can be, for instance, characteristic length scale, magnitude or
% period. Again, be sure that you know which element of THETA corresponds to
% which parameter of the covariance function. Especially, if you modify your
% covariance function construction, also modify THETA appropriately!
%
% You can form complex covariance functions by using some functions to chain
% simple covariance functions. For instance, GP_COV_SCALE can be used to add
% a scaling parameter, GP_COV_SUM to sum two covariance functions and
% GP_COV_PRODUCT to multiply two covariance functions. Make sure how the
% additional parameters are added to the vector THETA and how the parameter
% vectors of multiple covariance functions are combined.
%
% For most covariance functions, you can fix some parameters:
%
%   COVFUNC = GP_COV_EXAMPLE(..., 'PARAMETER', VALUE)
%
% Then the parameter is not read from the vector THETA anymore. For
% instance, if you construct a covariance function as
%
%   COVFUNC = GP_COV_PERIODIC(SQRT(SQ_DIST(X1,X2)),'WAVELENGTH',10)
%
% then THETA should contain only one element, the smoothness parameter.
%
% You can obtain some dimensionality information by calling COVFUNC
% without any inputs:
%
%   [N_THETA, N1, N2] = COVFUNC()
%
%   N_THETA : Number of non-fixed parameters
%   N1      : Number of rows in the covariance matrix
%   N2      : Number of columns in the covariance matrix
%
% In order to obtain the covariance matrix of a covariance function with no
% parameters, you should call COVFUNC([]), not COVFUNC(). Note the
% difference!
%
% At least the following covariance functions are implemented:
%
%   GP_COV_SE       : Isotropic squared exponential
%   GP_COV_RQ       : Isotropic rational quadratic
%   GP_COV_PP       : Piecewise polynomial
%   GP_COV_DELTA    : Delta function, isotropic noise
%   GP_COV_SCALE    : Scales a covariance function
%   GP_COV_SUM      : Sums two covariance functions
%   GP_COV_PRODUCT  : Multiplies two covariance functions
%   GP_COV_JITTER   : Adds a small constant to the diagonal for numerical
%                     stability
%   GP_COV_TOEPLITZ : Creates a covariance matrix with Toeplitz structure
%   GP_COV_WRAP     : Makes a covariance function from a covariance matrix
%
% You can write your own covariance functions as long as they fulfill the
% specification described here. In some cases, it might be a good idea to
% write some wrapper functions for the covariance functions to make the
% usage more straightforward. For instance,
%
%   COVFUNC_SE = @(THETA,X1,X2) FEVAL(GP_COV_SE(SQ_DIST(X1,X2)),THETA)
%
% would be used as
%
%   [K, DK] = COVFUNC_SE(THETA, X1, X2)
%
% which some users may prefer. However, this kind of interface may reduce
% efficiency, as one may need to compute same things several times (squared
% distance matrix in the above example).
%
% Note: Despite the similar naming convention, GP_COV_PSEUDO is
% fundamentally different kind of covariance function and can not be used
% similarly to other covariance functions. See GP_COV_PSEUDO for more
% information.
%
% See also GP_LEARN, GP_LEARN_PSEUDO, GP_PREDICT, GP_PREDICT_PSEUDO.

% Last modified 2010-01-27
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@tkk.fi)

function gp_cov()

help gp_cov


classdef KTA2 < ALGORITHM
% <multi/many> <real/integer> <expensive>
% Kriging-assisted Two_Arch2
% tau  --- 0.75 --- Proportion of one type noninfluential points in training data
% phi  ---  0.1 --- Number of randomly selected individuals
% wmax ---   10 --- Number of generations before updating CA and DA
% mu   ---    5 --- Number of re-evaluated solutions at each generation

%------------------------------- Reference --------------------------------
% Z. Song, H. Wang, C. He and Y. Jin, A Kriging-assisted two-archive
% evolutionary algorithm for expensive many-objective optimization, IEEE
% Transactions on Evolutionary Computation, 2021, 25(6): 1013-1027.
%------------------------------- 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".
%--------------------------------------------------------------------------

% This function is written by Zhenshou Song
% Email:zssong@stu.xidian.edu.cn

    methods
        function main(Algorithm,Problem)
            %% Parameter setting
            [tau,phi,wmax,mu] = Algorithm.ParameterSet(0.75,0.1,10,5);

            %% Initialization
            p = 1/Problem.M;
            CAsize = Problem.N;
            N = Problem.N;
            P = UniformPoint(N, Problem.D, 'Latin');
            Population = Problem.Evaluation(repmat(Problem.upper-Problem.lower,N,1).*P+repmat(Problem.lower,N,1));
            All_Population = Population;
            Ho_Population = All_Population;
            CA = UpdateCA([],Population,CAsize);
            DA = Population;
            THETA_S = 5.*ones(Problem.M,Problem.D);
            THETA_IS =  5.*ones(2,Problem.M,Problem.D);
            Model_sensitive = cell(1,Problem.M);
            Model_insensitive = cell(2,Problem.M);
            %% Optimization
            while Algorithm.NotTerminated(All_Population)
                %***** Building influential point-insensitive model********
                % build sensitive model
                Dec = All_Population.decs;
                Obj = All_Population.objs;
                for i = 1:Problem.M
                    dmodel     = dacefit(Dec,Obj(:,i),'regpoly0','corrgauss',THETA_S(i,:),1e-5.*ones(1,Problem.D),100.*ones(1,Problem.D));
                    Model_sensitive{i}   = dmodel;
                    THETA_S(i,:) = dmodel.theta;
                end
                % build insensitive models
                Centers = zeros(Problem.M,2);
                for i = 1 : Problem.M
                    [~,N1] = sort(Obj(:,i));
                    num = ceil(length(All_Population).*tau);
                    mean_index{1} = N1(1:num);
                    mean_index{2} = N1(end-num:end);
                    for j = 1:2
                        Centers(i,j) = mean(Obj(mean_index{j},i));  % lambda and miu
                    end
                    for j = 1 : 2
                        train_X = Dec(mean_index{j},:);
                        train_Y = Obj(mean_index{j},i);
                        dmodel  = dacefit(train_X,train_Y,'regpoly0','corrgauss',THETA_IS(j,i,:),1e-5.*ones(1,Problem.D),100.*ones(1,Problem.D));
                        Model_insensitive{j,i} = dmodel;
                        THETA_IS(j,i,:)        = dmodel.theta;
                    end
                end
                % Set the CCA and CDA as the current CA and DA
                CAobj = CA.objs; CAdec = CA.decs;
                DAobj = DA.objs; DAdec = DA.decs;
                w = 1;
                while w <= wmax   % this part is same as Two_Arch2
                    [~,ParentCdec,~,ParentMdec] = MatingSelection_KTA2(CAobj,CAdec,DAobj,DAdec,Problem.N);
                    OffspringDec = [OperatorGA(Problem,ParentCdec,{1,20,0,0});OperatorGA(Problem,ParentMdec,{0,0,1,20})];
                    PopDec = [DAdec;CAdec;OffspringDec];
                    N      = size(PopDec,1);
                    PopObj = zeros(N,Problem.M);
                    MSE    = zeros(N,Problem.M);
                    %****** Using influential point-insensitive model *****
                    for i = 1:N
                        for j = 1:Problem.M
                            [PopObj(i,j),~,~] = predictor(PopDec(i,:),Model_sensitive{j});
                            if abs(PopObj(i,j)- Centers(j,1)) <= abs(PopObj(i,j)- Centers(j,2))
                                model = Model_insensitive{1,j};
                            else
                                model = Model_insensitive{2,j};
                            end
                            [PopObj(i,j),~,MSE(i,j)] = predictor(PopDec(i,:),model);
                        end
                    end
                    [CAobj,CAdec,~] = K_UpdateCA(PopObj,PopDec,MSE,CAsize);
                    [DAobj,DAdec,DAvar] = K_UpdateDA(PopObj,PopDec,MSE,Problem.N,p);
                    w = w + 1;
                end

                % Adaptive sampling
                Offspring01 = Adaptive_sampling(CAobj,DAobj,CAdec,DAdec,DAvar,DA,mu,p,phi);

                [~,index] = unique(Offspring01 ,'rows');
                PopNew = Offspring01(index,:);
                Offspring02 = [];
                for i = 1:size(PopNew,1)
                    dist2 = pdist2(real( PopNew(i,:)),real(All_Population.decs));
                    if min(dist2) > 1e-5
                        Offspring02 = [Offspring02;PopNew(i,:)];
                    end
                end
                if ~isempty(Offspring02)
                    Offspring = Problem.Evaluation(Offspring02);

                    temp =  All_Population.decs;
                    for i = 1:size(Offspring,2)
                        dist2 = pdist2(real(Offspring(i).decs),real(temp));
                        if min(dist2) > 1e-5
                            All_Population = [All_Population,Offspring(i)];
                        end
                        temp = All_Population.decs;
                    end
                    CA = UpdateCA(CA,Offspring,CAsize);
                    DA = UpdateDA(DA,Offspring,Problem.N,p);
                    Ho_Population = [Ho_Population,Offspring];
                end
            end
        end
    end
end


 function [T, reuse] = Gdsft_gram(ob, W, reuse)
%function [T, reuse] = Gdsft_gram(A, W, reuse)
%|
%| Construct Toeplitz gram matrix object T = A'WA for a Gdsft object.
%| This object performs T*x rapidly using an over-sampled FFT.
%|
%| in
%|	A	[M np]		Gdsft object (fatrix2)
%|	W	[M M]		W = Gdiag() for fatrix2 (often simply "1" or [])
%|				assume W=diag(wi) with real wi so T is Hermitian
%|	reuse	struct		stuff from the last call that can be reused
%|
%| out
%|	T	[np np]		fatrix2 object
%|	reuse	struct		stuff that can be reused on future calls
%|
%| Copyright 2012-06-05, Jeff Fessler, University of Michigan

if nargin == 1 && streq(ob, 'test'), Gdsft_gram_test, return, end
if nargin == 1 && streq(ob, 'test1'), Gdsft_gram_test1, return, end
if nargin == 1 && streq(ob, 'test2'), Gdsft_gram_test2, return, end
if nargin == 1 && streq(ob, 'test3'), Gdsft_gram_test3, return, end
if nargin < 2 || nargin > 3, ir_usage, end

if ~isvar('reuse'), reuse = []; end

if ~isa(ob, 'fatrix2')
	fail('A wrong class: %s', class(ob))
end

[nd np] = size(ob);
forw = @(arg, x) Gdsft_gram_mult(x, arg.fftkern, arg.mask);

% extract wi
if isnumeric(W) % includes case where W is empty
	wi = W;
elseif isa(W, 'fatrix2') && streq(W.caller, 'Gdiag')
	wi = W.arg.diag;
else
	error 'W must be diag_sp or Gdiag or wi array'
end
clear W

if isempty(wi)
	wi = ones(nd, 1); % the usual unweighted case
end

if isscalar(wi)
	wi = wi * ones(nd, 1);
end

if isreal(wi)
	back = forw; % trick: because Hermitian!
else
	fail('not implemented for complex wi')
end

arg = ob.arg;
arg.mask = ob.imask_array;
[arg.fftkern arg.reuse] = Gdsft_gram_init(arg, wi, reuse, ob.idim);

if any(~arg.mask(:))
	omask = arg.mask;
else
	omask = []; % trick: omask not needed in full case
end
T = fatrix2('arg', arg', ...
	'idim', ob.idim, 'imask', arg.mask, ...
	'odim', ob.idim, 'omask', omask, ...
	'forw', forw, 'back', back);

% Gdsft_gram_init()
% construct kernel of circulant matrix into which T is embedded
% and take its DFT to prepare for multiplication.
% out
%	fftkern [[2Nd]]		FFT of kernel of circulant matrix
%
function [fftkern, reuse] = Gdsft_gram_init(arg, wi, reuse, Nd)

switch numel(Nd)
case 1
	[fftkern reuse] = Gdsft_gram_init1(arg, wi, reuse, Nd, arg.show);
case 2
	[fftkern reuse] = Gdsft_gram_init2(arg, wi, reuse, Nd, arg.show);
case 3
	[fftkern reuse] = Gdsft_gram_init3(arg, wi, reuse, Nd, arg.show);
otherwise
	fail('dim %d not done', numel(Nd))
end

% Gdsft_gram_init1()
% 1d filter for circulant matrix
% note: only toeplitz kernel values from -(N-1) to (N-1) are relevant
% so the value at +/- N does not matter so we set it to zero.
function [fftkern, reuse] = Gdsft_gram_init1(arg, wi, reuse, N1, show)

if isempty(reuse)
	reuse.G1 = Gdsft(arg.om_t', arg.Nd); % with n_shift = 0
end

block1 = reuse.G1' * real(wi); % kludge

% kernel of Toeplitz matrix from -N to N-1 but with fftshift
% This is inherently Hermitian symmetric except for the middle [0] value.
% Use 0 for the irrelevant value at -N.
err1 = abs(imag(block1(1))) / abs(block1(1));
tol = 0;
if err1 > tol
	printm('removing imaginary h[0] part of relative size %g', err1)
	block1(1) = real(block1(1));
end
kern = [block1; 0; flipud(conj(block1(2:N1)))]; % [2*N1]
if show
	kern = dsft_gram_hermitify(kern, show); % need not due to block1(1) fix
end
fftkern = fft(kern); % [2*N1]

% Gdsft_gram_init2()
function [fftkern, reuse] = Gdsft_gram_init2(arg, wi, reuse, Nd, show)

if isempty(reuse)
	[reuse.G1 reuse.G2] = Gdsft_gram_setup2(arg);
end

N1 = Nd(1);
N2 = Nd(2);

block1 = reshape(reuse.G1' * real(wi), Nd); % kludge
block2 = reshape(reuse.G2' * real(wi), Nd);

z1 = zeros(N1,1);
z2 = zeros(N1-1,1);
kern = [
	[block1 z1 conj(fliplr([block1(1,2:N2); block2(2:N1,2:N2)]))];
	zeros(1,2*N2);
	[flipud(block2(2:N1,:)) z2 fliplr(flipud(conj(block1(2:N1, 2:N2))))]
]; % [(2Nd)]
kern = dsft_gram_hermitify(kern, show); % force Hermitian symmetry
fftkern = fftn_fast(kern);

% Gdsft_gram_init3()
function [fftkern reuse] = Gdsft_gram_init3(arg, wi, reuse, Nd, show)
if isempty(reuse)
	[reuse.G1 reuse.G2 reuse.G3 reuse.G4 ] = build_G1_G2_G3_G4(arg);
end
N1 = Nd(1);
N2 = Nd(2);
N3 = Nd(3);

filtblk1 = reshape(reuse.G1' * real(wi), Nd);
filtblk2 = reshape(reuse.G2' * real(wi), Nd);
filtblk3 = reshape(reuse.G3' * real(wi), Nd);
filtblk4 = reshape(reuse.G4' * real(wi), Nd);

tblk1 = filtblk1;
tblk2 = filtblk2(2:N1,:,:);	% remove the duplicated part with filtblk1
tblk3 = filtblk3(2:N1,2:N2,:);	% remove the duplicated part with block 1, 2, 4
tblk4 = filtblk4(:,2:N2,:);	% remove the duplicated part with block 1

z1 = zeros(N1,1,N3);
z2 = zeros(N1-1,1,N3);

% top half of the 3D filter
kern_top = [
	tblk1 z1 flipdim(tblk4,2);	% upper block
	zeros(1,2*N2,N3);		% zero padding in the middle
	flipdim(tblk2,1) z2 flipdim(flipdim(tblk3,1),2) % lower block
];

% construct the bottom half now
bblk1 = flipdim(conj(filtblk3(:,:,2:N3)),3);
bblk2 = flipdim(conj(filtblk4(:,:,2:N3)),3);
bblk2 = bblk2(2:N1,:,:);
bblk3 = flipdim(conj(filtblk1(:,:,2:N3)),3);
bblk3 = bblk3(2:N1,2:N2,:);
bblk4 = flipdim(conj(filtblk2(:,:,2:N3)),3);
bblk4 = bblk4(:,2:N2,:);

z4 = zeros(N1,1,N3-1);
z5 = zeros(N1-1,1,N3-1);
kern_bottom = [
	bblk1 z4 flipdim(bblk4,2);
	zeros(1,2*N2,N3-1);
	flipdim(bblk2,1) z5 flipdim(flipdim(bblk3,1),2)
];

kern = cat(3, kern_top, zeros(2*N1,2*N2,1));
kern = cat(3, kern, kern_bottom);

kern = dsft_gram_hermitify(kern, show); % force Hermitian symmetry
fftkern = fftn_fast(kern);

% Gdsft_gram_setup2()
% modified versions of A for 2D
% this routine is quite tricky.  nothing new is really rebuilt here,
% but some structure values are changed.
function [G1, G2] = Gdsft_gram_setup2(arg)
om = arg.om_t';
Nd = arg.Nd;
G1 = Gdsft(om, Nd);
om(:,1) = -om(:,1);
G2 = Gdsft(om, Nd);

% build_G1_G2_G3_G4()
% Created by Daehyun Yoon for 3D case
% build more Gnufft objects, with negative om.
function [G1, G2, G3, G4] = build_G1_G2_G3_G4(arg)
om = arg.om_t';
Nd = arg.Nd;
G1 = Gdsft(om, Nd);
om(:,1) = -om(:,1);
G2 = Gdsft(om, Nd);
om(:,2) = -om(:,2);
G3 = Gdsft(om, Nd);
om(:,1) = -om(:,1);
G4 = Gdsft(om, Nd);

% Gdsft_gram_mult()
% multiply an image x by a toeplitz matrix
% by embedding it into a circulant matrix of twice the size and using FFT.
% in
%	x	[(Nd)]
%	fftkern	[[2Nd]]
%	mask	[(Nd)]
% out
%	y	[(Nd)]
function y = Gdsft_gram_mult(x, fftkern, mask)

N2 = size(fftkern);
if numel(N2) == 2 && N2(2) == 1
	N2 = N2(1);
end
Nd = N2 / 2;

LL = size(x,1+numel(N2));
y = zeros([Nd LL]);

switch numel(N2)
case 1
	tmp = fft(x, N2); % [N2 L]
	tmp = tmp .* repmat(fftkern, [1 ncol(tmp)]);
	y = ifft(tmp);
	y = y(1:Nd,:); % [Nd L]
	y = y .* repmat(mask, [1 ncol(y)]); % fatrix2 requires mask

case 2
	for ll=1:LL
		tmp = ifftn_fast(fftkern .* fftn_fast(x(:,:,ll), N2));
		tmp = tmp(1:Nd(1), 1:Nd(2));
		tmp = tmp .* mask; % fatrix2 requires mask
		y(:,:,ll) = tmp;
	end

case 3
	for ll=1:LL
		tmp = ifftn_fast(fftkern .* fftn_fast(x(:,:,:,ll), N2));
		tmp = tmp(1:Nd(1), 1:Nd(2), 1:Nd(3));
		tmp = tmp .* mask; % fatrix2 requires mask
		y(:,:,:,ll) = tmp;
	end

otherwise
	fail('dim %d not done', numel(N2))
end

% Gdsft_gram_test1()
% test 1d version
function Gdsft_gram_test1
N = 16;
M = 21;
omega = 2*pi*rand(M,1);
wi = [1:size(omega,1)]';
mask = true(N,1); mask(end-3) = false;
A = Gdsft(omega, N, 'mask', mask, 'n_shift', N/2, 'show', 1);
T = build_gram(A, wi);
test_adjoint(T, 'complex', 1, 'tolre', 1e-7);
fatrix2_tests(A, 'complex', 1, 'tol_gram', 2e-7);

% Gdsft_gram_test2()
function Gdsft_gram_test2
N = [16 14];
fov = N;
ktype = 'spiral0';
[kspace omega wi] = mri_trajectory(ktype, {}, N, fov, {'voronoi'});
ig = image_geom('nx', N(1), 'ny', N(2), 'dx', 1);
mask = ellipse_im(ig, [0 0 14 15 0 1], 'oversample', 3) > 0;
A = Gdsft(omega, N, 'n_shift', N/2, 'show', 1);
fatrix2_tests(A, 'complex', 1, 'tol_gram', 2e-7);

T = build_gram(A, wi);
fatrix2_tests(T, 'complex', 1, 'tol_gram', 1e-7);
test_adjoint(T, 'complex', 1, 'tolre', 1e-7); % matches!

if 1
	x = ellipse_im(ig, 'shepplogan-emis');
	x1 = A' * (wi .* (A * x));
	x1 = embed(x1, mask);
	x2 = T * x;
	equivs(x1, x2, 'thresh', 2e-7)
end

tic
for ii=1:50, b1 = embed(A' * (wi .* (A * x(mask))), mask); end
t1 = toc;
tic
for ii=1:50, b2 = embed(T * x(mask), mask); end
t2 = toc;
equivs(b1, b2)
printm('time: A''Ax = %.3f, Tx=%.3f', t1, t2)

% Gdsft_gram_test3()
function Gdsft_gram_test3
N = [8 4 2];
rng(0)
omega = rand(21,3) * 2 * pi;
A = Gdsft(omega, N, 'n_shift', N/2, 'show', 1);
fatrix2_tests(A, 'complex', 1, 'tol_gram', 2e-7);
test_adjoint(A, 'complex', 1, 'big', 1);

wi = [1:size(omega,1)]';
T = build_gram(A, wi);
fatrix2_tests(T, 'complex', 1, 'tol_gram', 1e-7);
test_adjoint(T, 'complex', 1, 'tolre', 1e-7);

if 1
	x = cumsum(ones(N), 2);
	x1 = A' * (wi .* (A * x));
	x1 = iembed(A, x1);
	x2 = T * x;
	equivs(x1, x2)
end

% Gdsft_gram_test()
function Gdsft_gram_test
Gdsft_gram_test1
Gdsft_gram_test2
Gdsft_gram_test3


function [sourcemodel] = patchsvd(cfg, sourcemodel)

% This function does something like Limpiti et al.
% IEEE trans biomed eng 2006;53(9);1740-54

% Copyright (c) 2006, Jan-Mathijs Schoffelen & Robert Oostenveld, F.C. Donders Centre
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

% set the defaults
if ~isfield(cfg, 'patchsvd'),    cfg.patchsvd    = 3;         end
if ~isfield(cfg, 'patchsvdnum'), cfg.patchsvdnum = 5;         end
if ~isfield(cfg, 'feedback'),    cfg.feedback    = 'textbar'; end

if isnumeric(cfg.patchsvd),
  Ndipoles = size(sourcemodel.pos,1);
else
  Ndipoles = 1;
end
Ninside  = length(sourcemodel.inside);
Nchans   = size(sourcemodel.leadfield{sourcemodel.inside(1)}, 1);
lfall    = cell(1,Ndipoles);
coeff    = cell(1,Ndipoles);
nghbr    = cell(1,Ndipoles);

if isnumeric(cfg.patchsvd) && ~isfield(sourcemodel, 'patchindx'),
  fprintf('computing patches in 3D, not taking topology into account\n');
  progress('init', cfg.feedback, 'computing patchsvd');
  for dipindx=1:Ninside
    % renumber the loop-index variable to make it easier to print the progress bar
    i = sourcemodel.inside(dipindx);

    % compute the distance from this dipole to each other dipole
    dist = sqrt(sum((sourcemodel.pos-repmat(sourcemodel.pos(i,:), [Ndipoles 1])).^2, 2));

    % define the region of interest around this dipole
    sel  = find(dist<=cfg.patchsvd);
    sel  = intersect(sel, sourcemodel.inside);
    Nsel = length(sel);

    progress(dipindx/Ninside, 'computing patchsvd %d/%d, Nsel=%d\n', dipindx, Ninside, Nsel);
    % concatenate the leadfield of all dipoles that are inside the ROI into one matrix
    lfr     = cell2mat(sourcemodel.leadfield(sel(:)'));
    % svd of leadfields of dipoles inside the ROI
    [U,S,V] = svd(lfr);

    if cfg.patchsvdnum < 1,
      % Limpiti et al 2006 formula 12
      s          = diag(S).^2;
      s          = cumsum(s)./sum(s);
      n(dipindx) = find(s - cfg.patchsvdnum > 0, 1);
    else
      n(dipindx) = cfg.patchsvdnum;
    end
    lfall{i} = U(:,1:n(dipindx)); %*S(1:n(dipindx),1:n(dipindx)); %klopt dit?
    nghbr{i} = sel;
    coeff{i} = V(1:n(dipindx), :);
  end
  progress('close');
elseif isnumeric(cfg.patchsvd) && isfield(sourcemodel, 'patchindx'),
  fprintf('computing patches in 2D, taking surface topology into account\n');
  progress('init', cfg.feedback, 'computing patchsvd');
  for dipindx=1:Ninside
    % renumber the loop-index variable to make it easier to print the progress bar
    i = sourcemodel.inside(dipindx);

    % define the region of interest around this dipole
    sel  = nearest(sourcemodel.patchsize(i,:), cfg.patchsvd);
    sel  = sourcemodel.patchindx(i,1:sel);
    Nsel = length(sel);

    progress(dipindx/Ninside, 'computing patchsvd %d/%d, Nsel=%d\n', dipindx, Ninside, Nsel);
    % concatenate the leadfield of all dipoles that are inside the ROI into one matrix
    lfr     = cell2mat(sourcemodel.leadfield(sel(:)'));
    % svd of leadfields of dipoles inside the ROI
    [U,S,V] = svd(lfr);

    if cfg.patchsvdnum < 1,
      % Limpiti et al 2006 formula 12
      s          = diag(S).^2;
      s          = cumsum(s)./sum(s);
      n(dipindx) = find(s - cfg.patchsvdnum > 0, 1);
    else
      n(dipindx) = min(cfg.patchsvdnum, size(lfr,2));
    end
    lfall{i} = U(:,1:n(dipindx)); %*S(1:n(dipindx),1:n(dipindx)); %klopt dit?
    nghbr{i} = sel;
    coeff{i} = V(1:n(dipindx), :);
    sv{i}    = s;
  end
  progress('close');
elseif strcmp(cfg.patchsvd, 'all'),
  lfr     = cell2mat(sourcemodel.leadfield(sourcemodel.inside(:)'));
  [U,S,V] = svd(lfr);

  if cfg.patchsvdnum < 1,
    % Limpiti et al 2006 formula 12
    s = diag(S).^2;
    s = cumsum(s)./sum(s);
    n = find(s - cfg.patchsvdnum > 0, 1);
  else
    n = cfg.patchsvdnum;
  end
  lfall{1} = U(:,1:n); %*S(1:n(dipindx),1:n(dipindx)); %klopt dit?
  nghbr{1} = sourcemodel.inside;
  coeff{1} = V(1:n, :);

  %---change output
  sourcemodel.pos    = mean(sourcemodel.pos(sourcemodel.inside,:),1);
  sourcemodel.inside = 1;
  sourcemodel.dim    = [1 1 1];
  sourcemodel.xsourcemodel  = sourcemodel.pos(1);
  sourcemodel.ysourcemodel  = sourcemodel.pos(2);
  sourcemodel.zsourcemodel  = sourcemodel.pos(3);
else
  %do nothing
end

if ~all(n==n(1)),
  nmax = max(n);
  for dipindx = 1:Ninside
    i = sourcemodel.inside(dipindx);
    if n(dipindx)<nmax,
      lfall{i} = [lfall{i} zeros(Nchans, nmax-n(dipindx))];
    end
  end
end

% update the leadfields
sourcemodel.leadfield = lfall;
%sourcemodel.expcoeff  = coeff;
sourcemodel.neighbours= nghbr;


% use EKF to estimate gyro and accel biases, for
% MEMS IMU grade close to microstrain 3dm gx3-35
% s0 frame of the sensor coincides a earth-fixed n-frame(NED) at epoch t0, n0.

% The state
% \f$ p_S^N \f$ position of the IMU in n0-frame
% \f$ v_N \f$ velocity of the IMU in n0-frame
% \f$ q_N^S \f$ quaternion representing the rotation from n0-frame to IMU frame
% \f$ b_a \f$ accelerometer bias
% \f$ b_g \f$ gyroscope bias

% The error state
% \f$ \delta p_S^N = \hat{p}_S^N - p_S^N \f$
% \f$ \delta v^N = \hat{v}^N - v^N \f$
% \f$ \psi^E \f$ Error in the rotation from n0-frame to IMU frame $C_N^S$
% defined as \f$ C_N^S = (I + \psi \times) \hat{C}_N^S \f$
% \f$ \delta b_a = b_a - \hat{b}_a \f$ accelerometer bias
% \f$ \delta b_g = b_g - \hat{b}_g \f$ gyro bias

classdef EKF_filter_s0frame_bias < handle
    properties (Hidden)
        type = 'ekf';
        tag  = 'EKF_GPS_IMU_GRAVITY';  % ID tag
    end
    % The following properties can be set only by class methods
    properties (SetAccess = private)
        stateTime; % state timestamp
        invalidateIMUerrors; % reset IMU errors when they goes beyond expectation
        imuType; % the type of IMU, e.g., MEMS 3DX GM 3-35, Steval inems, etc.
        imuErrorModel=3; % the IMU error model corresponding to biases
        dt; % IMU sampling interval, unit sec
        rqs02e; % position of s0 in the e frame, qs02e, constant
        rvqs0; % position of current s(k) frame in s0 frame, velocity in s0 frame, q s0 to s(k)
        imuErrors; %ba, bg
        imuOrientSIP=7; % imu orientation covariance start index in s-frame
        imuBiasDriftSIP=10; %imu bias error covariance start index
        covDim=15; % the dimension of P matrix
        velNoiseStd; % noise density driving horizontal velocity
        p_k_k; % the covariance of the entire state vector
    end
    methods
        function filter = EKF_filter_s0frame_bias(options)
            filter.stateTime = options.startTime;
            filter.invalidateIMUerrors=options.InvalidateIMUerrors;
            filter.imuType=options.imutype;
            filter.imuErrorModel=options.imuErrorModel;
            filter.dt=options.dt;

            inillh=options.inillh_ant;
            Ce2n0=llh2dcm_v000(inillh(1:2),[0;1]);
            filter.rqs02e=[ecef2geo_v000(options.inillh_ant,1); rotro2qr(Ce2n0')];
            % assume the IMU is very close to antenna
            filter.rvqs0=zeros(10,1);

            qs2n=quatmult_v001(options.qb2n,rotro2qr(options.Cb2imu),2);
            filter.rvqs0(7:10)=[qs2n(1);-qs2n(2:4)];
            filter.rvqs0(4:6)=options.Vn;
            filter.rvqs0(1:3) = quatrot_v000(qs2n, -options.imu_p_ant, 0);

            filter.imuErrors=options.imuErrors;
            filter.velNoiseStd=options.velNoiseStd;
            filter.p_k_k=zeros(filter.covDim);    % in s0 frame, position, velocity, attitude are of very small variance

            filter.p_k_k(1:3,1:3)=diag([0.2, 0.2, 0.5].^2); %position errors in meter
            filter.p_k_k(4:6,4:6)=diag([0.1, 0.1, 0.2].^2); %vel error in m/s
            filter.p_k_k(filter.imuOrientSIP+(0:2),filter.imuOrientSIP+(0:2))=...
                diag(([options.initAttVar,options.initAttVar,options.initAttVar*3]).^2);
            IMU_ERRDEF=imu_err_defs_v000(options.imutype);
            filter.p_k_k(filter.imuBiasDriftSIP+(0:2),filter.imuBiasDriftSIP+(0:2))=4*eye(3)*IMU_ERRDEF.acc_bias_var;% enlarge initial std by 2
            filter.p_k_k(filter.imuBiasDriftSIP+(3:5),filter.imuBiasDriftSIP+(3:5))=4*eye(3)*IMU_ERRDEF.gyro_bias_var;

            filter.p_k_k=sparse (filter.p_k_k);
        end
        %===============================================================================================
        %-- State transition function
        % propogate state with accelerometer and gyro input at time k-1 to predict state at k,
        % i.e., X(k|k-1), from state at k-1. U1 contains IMU measurement
        % acc, and gyro angular rate, or delta v, and delta theta
        % and 7th row is the previous epoch(k-1) and 8th is the current
        % epoch(k). Optionally, 9-11th rows are preset gravity in s0 frame,
        % 12-14 th row is $\omega_{ie}_{s_0}$, angular rate of local s0-frame w.r.t i-frame
        % represented in s0-frame. We also call s0 frame as w-frame
        % imuaccum records the accumulated delta v and delta angle for two speed
        % covariance update
        function imuaccum= ffun_state(filter, imuaccum, U1, isDelta, isConstantVel)
            if(isDelta)
                dt1=U1(8,end)-U1(7,1);
                gyroinc=sum(U1(4:6,:),2);
                accinc=sum(U1(1:3,:),2);

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

                if(filter.invalidateIMUerrors)
                    spuracc=find(abs(filter.imuErrors(1:3))>2*IMU_ERRDEF.initacc_bias_err,1);
                    spurgyro=find(abs(filter.imuErrors(4:6))>2*IMU_ERRDEF.initgyro_bias_err,1);
                    if(~isempty(spuracc)||~isempty(spurgyro))
                        disp(['IMU errors diverge at ' num2str(U1(8,end)) '!']);
                        filter.imuErrors=zeros(6,1);
                    end
                end
                angleinc=gyroinc-filter.imuErrors(4:6)*dt1;
                velinc=accinc-filter.imuErrors(1:3)*dt1;
                %imu data accumulator for the covariance update
                imuaccum=imuaccum+[velinc;angleinc];
                gyroinc=angleinc/dt1;
                accinc=velinc/dt1;
            else
                dt1=U1(8,end)-U1(7,1);
                gyroinc=mean(U1(4:6,:),2);
                accinc=mean(U1(1:3,:),2);
                %reset the imu errors if estimates diverges
                IMU_ERRDEF=imu_err_defs_v000(filter.imuType);
                if(filter.invalidateIMUerrors)
                    spuracc=find(abs(filter.imuErrors(1:3))>2*IMU_ERRDEF.initacc_bias_err,1);
                    spurgyro=find(abs(filter.imuErrors(4:6))>2*IMU_ERRDEF.initgyro_bias_err,1);
                    if(~isempty(spuracc)||~isempty(spurgyro))
                        disp(['IMU errors diverge at ' num2str(U1(8,end)) '!']);
                        filter.imuErrors=zeros(6,1);
                    end
                end
                gyroinc=gyroinc-filter.imuErrors(4:6);
                accinc=accinc-filter.imuErrors(1:3);
                %imu data accumulator for the covariance update
                imuaccum=imuaccum+[accinc;gyroinc]*dt1;
            end
            if(size(U1,1)>8)
                gwomegaw=U1(9:end,:);
            else
                %Gravity (most time consuming part of ecef implementations)
                xyz_imu=filter.rqs02e(1:3)+quatrot_v000(filter.rqs02e(4:7),filter.rvqs0(1:3),0);
                Llh=ecef2geo_v000(xyz_imu,0);
                Cn2e=pos2Cne_v000(Llh(1), Llh(2));
                [Rn, Re, gn, sL, cL, WIE_E]=geoparam_v000(Llh);
                gwomegaw=[quatrot_v000(filter.rqs02e(4:7), Cn2e*[0;0;gn], 1);quatrot_v000(filter.rqs02e(4:7),[0;0;WIE_E],1)];
            end
            if(~isConstantVel)
                filter.rvqs0=strapdown_local_quat_bias(filter.rvqs0, filter.rqs02e, accinc, gyroinc, dt1, gwomegaw);
            else
                filter.rvqs0=strapdown_local_quat_constvel(filter.rvqs0, filter.rqs02e, accinc, gyroinc, dt1, gwomegaw);
            end
            filter.stateTime = U1(8);
        end
        function ffun_covariance(filter, imuaccum, covupt_time, curimutime, isConstantVel )
            % propagate the covariance in the local s0 frame
            % the covariance corresponds to states,
            % rs in s0, v s in s0, q s0 2s, ba, bg
            covdt=curimutime-covupt_time;
            if(~isConstantVel)
            [STM, Qd]=sys_local_dcm_bias(filter.rqs02e, filter.rvqs0,...
                imuaccum(1:3)/covdt,imuaccum(4:6)/covdt, covdt,filter.imuType, filter.imuErrorModel);
            else
                [STM, Qd]=sys_local_dcm_constvel(filter.rqs02e, filter.rvqs0,...
                imuaccum(1:3)/covdt,imuaccum(4:6)/covdt, covdt,filter.imuType, filter.velNoiseStd);
            end

            Pvf = STM*filter.p_k_k*STM'+Qd;  % the covariance of the navigation states and imu error terms
            filter.p_k_k=sparse(Pvf);
        end
        %==============================================================================================
        function applied = correctstates(filter, predict, measure, H, R, measurementTime, gatingtest)
            % Suppose the prediction function is h, and the measurement is
            % z, then z = h(p, v, q, ba, bg). Formally,
            % \f$ H_p = - \frac{\partial h(p \oplus \delta)}{\partial \delta} \f$
            % \f$ H_\psi = - \frac{\partial h(C(q) \oplus \psi)}{\partial
            % \psi} \f$. Note in this update function,
            % \f$ p \oplus \delta = p - \delta \f$, and
            % \f$ C(q) \oplus \psi = \exp(\psi) C(q) \f$.

            if nargin < 7
                gatingtest = true;
            end
            if nargin < 6
                measurementTime = filter.stateTime;
            else
                if (measurementTime > filter.stateTime)
                    fprintf('Warn: Measurement time %.6f  >= state time %.6f\n', measurementTime, filter.stateTime);
                end
            end
            p_km1_k=filter.p_k_k;
            inno=predict-measure;
            S=H*p_km1_k*H'+R;
            if gatingtest
                gamma=inno'/S*inno;
                tol = chi2inv(0.99, 3);
                if gamma > tol
                    fprintf(['Discard measurement at %.6f with large ', ...
                             'gamma %.4f > tol %.4f.\n'], measurementTime, gamma, tol);
                    applied = false;
                    return;
                end
            end
            K=p_km1_k*H'/S;
            deltaX=K*inno;

            filter.p_k_k=(eye(size(p_km1_k,1))-K*H)*p_km1_k*(eye(size(p_km1_k,1))-K*H)'+K*R*K';
            % compute updated states
            filter.rvqs0(1:6)=filter.rvqs0(1:6)-deltaX(1:6); % position and velocity
            qst=rvec2quat_v000(deltaX(7:9));
            filter.rvqs0(7:10)=quatmult_v000(qst, filter.rvqs0(7:10));
            filter.imuErrors = filter.imuErrors + deltaX(filter.imuBiasDriftSIP:end);
            applied = true;
        end

        function SaveToFile(filter, ~, preimutime, ffilres)
            formatString = ['%.8f,%.5f,%.5f,%.5f,%.5f,%.5f,%.5f,%.8f,%.8f,%.8f,%.8f,', ...
                '%.6f,%.6f,%.6f,%.6f,%.6f,%.6f,%.8f,%.8f,%.8f\n'];
            fprintf(ffilres, formatString, [preimutime;filter.rvqs0(1:6); ...
                [-filter.rvqs0(8:10); filter.rvqs0(7)];...
                full(sqrt(diag(filter.p_k_k(1:9,1:9))))]);
        end
        function mag=GetVelocityMag(filter)
            mag=norm(filter.rvqs0(4:6),2);
        end
    end
end


classdef ParadigmSpecRCSP < ParadigmDataflowSimplified
    % Advanced paradigm for oscillatory processes via the Spectrally weighted regularized CSP algorithm.
    %
    % This method is a combination of the features of the Spec-CSP [1] and the RCSP [7] methods.
    %
    % The Spec-CSP paradigm [1] is a more advanced variant of CSP, developed for the Berlin
    % Brain-Computer Interface (BBCI); the primary focus was motor imagery BCI, but the algorithm was
    % designed from the outset to be applicable for a wider range of applications. The implementation
    % closely follows the TR [2].
    %
    % The RCSP (Regularized Common Spatial Patterns) method [7] is a recently-developed regularized
    % extension of the CSP method, which combines multiple regularization terms (of which we here
    % implemented covariance shrinkage and Tikhonov regularization); these terms are themselves
    % derived from earlier regularized CSP methods, such as TRCSP (Tikhonov Regularized CSP).
    %
    % In addition to the aforementioned papers this method also use multi-taper spectral estimation
    % instead of the Welch method to regularize the spectral estimates, unlike the Spec-CSP method.
    %
    % The paradigm is applicable to the majority of oscillatory processes, and is the most advanced
    % spatio-spectrally adaptive method that is currently provided in the toolbox. Whenever the exact
    % frequency and location of some (conjectured) oscillatory process is not known exactly, Spec-RCSP
    % can be used, and typically gives better results than CSP with an appropriately unrestricted (e.g.,
    % broad-band) spectral filter. Several other methods exist to adapt the spectrum to a process of
    % interest, among others Common Spatio-Spectral Patterns [3], Common Sparse Spectral Spatial Pattern
    % [4], r^2-based heuristics [5], automated parameter search, and manual selection based on visual
    % inspection. Several of these methods have been shown to give approx. comparable results [2]. An
    % alternative and competitive method, especially when there complex interactions between frequency
    % bands and time periods are to be modeled is the Dual-Augmented Lagrange paradigm
    % (para_dal/para_dal_hf).
    %
    % The method iteratively optimizes spatial and spectral filters in alternation and extracts
    % log-variance features from the resulting (filtered) signal. These features are subsequently
    % processed by a (typically simple) machine learning component, by default LDA. Learning is
    % therefore significantly slower than CSP. An option is to impose custom prior knowledge on the
    % relevant data spectrum, for example by placing a focus on the alpha rhythm, without ruling out
    % other frequencies. Note that there are parameters which constrain the spectrum: one is the
    % frequency prior and the other is the spectral filter that is applied before running the alorithm;
    % both need to be adapted when the considered spectrum shall be extended (e.g. to high-gamma
    % oscillations). Other parameters which are frequently adapted are the time window of interest and
    % the learner component (e.g., logistic regression is a good alternative choice).
    %
    % Some application areas include detection of major brain rhythm modulations (e.g. theta, alpha,
    % beta, gamma), for example related to relaxation/stress, aspects of workload, emotion,
    % sensori-motor imagery, and in general cortical idle oscillations in various modalities.
    %
    % Example: Consider the goal of predicting the emotion felt by a person at a given time. A possible
    % calibration data set for this task would contain a sequence of blocks in each of which the subject
    % is one out of several possible emotions, indicated by events 'e1','e2','e3','e4' covering these
    % blocks at regular rate. The data might for example be induced via guided imagery [6].
    %
    %   calib = io_loadset('data sets/bruce/emotions.eeg')
    %   myapproach = {'SpecRCSP' 'SignalProcessing',{'EpochExtraction',[-2.5 2.5]}};
    %   [loss,model,stats] = bci_train('Data',calib,'Approach',myapproach, 'TargetMarkers',{'e1','e2','e3','e4'});
    %
    %
    % References:
    %  [1] Tomioka, R., Dornhege, G., Aihara, K., and Mueller, K.-R. "An iterative algorithm for spatio-temporal filter optimization."
    %      In Proceedings of the 3rd International Brain-Computer Interface Workshop and Training Course 2006.
    %  [2] Ryota Tomioka, Guido Dornhege, Guido Nolte, Benjamin Blankertz, Kazuyuki Aihara, and Klaus-Robert Mueller
    %      "Spectrally Weighted Common Spatial Pattern Algorithm for Single Trial EEG Classification",
    %      Mathematical Engineering Technical Reports (METR-2006-40), July 2006.
    %  [3] Steven Lemm, Benjamin Blankertz, Gabriel Curio, and Klaus-Robert M?ller.
    %      "Spatio-spectral filters for improving classification of single trial EEG."
    %      IEEE Trans Biomed Eng, 52(9):1541-1548, 2005.
    %  [4] G. Dornhege, B. Blankertz, M. Krauledat, F. Losch, G. Curio, and K.-R. M?ller,
    %      "Combined optimization of spatial and temporal filters for improving brain-computer interfacing,"
    %      IEEE Transactions on Biomedical Engineering, vol. 53, no. 11, pp. 2274?2281, 2006.
    %  [5] Benjamin Blankertz, Ryota Tomioka, Steven Lemm, Motoaki Kawanabe, and Klaus-Robert Mueller.
    %      "Optimizing spatial filters for robust EEG single-trial analysis."
    %      IEEE Signal Process Mag, 25(1):41-56, January 2008
    %  [6] Onton J & Makeig S. "Broadband high-frequency EEG dynamics during emotion imagination."
    %      Frontiers in Human Neuroscience, 2009.
    %  [7] Lotte, F., Guan, G., "Regularizing Common Spatial Patterns to Improve BCI Designs: Unified Theory and New Algorithms."
    %      IEEE Trans Biomed Eng 58, 2 (2011), 355-362.
    %
    % Name:
    %   Spectrally Weighted RCSP
    %
    %                           Christian Kothe, Swartz Center for Computational Neuroscience, UCSD
    %                           2015-11-05

    methods

        function defaults = preprocessing_defaults(self) %#ok<MANU>
            defaults = {'IIRFilter',{'Frequencies',[0.5 5],'Mode','highpass'}, 'EpochExtraction',[0.5 3.5], 'Resampling',100};
        end

        function model = feature_adapt(self,varargin) %#ok<INUSL>
            args = arg_define(varargin, ...
                arg_norep('signal'), ...
                ... % standard CSP parameters
                arg({'patterns','PatternPairs'},3,uint32([1 1 64 1000]),'Number of CSP patterns (times two).','cat','Feature Extraction'), ...
                ... % multi-taper spectral estimation
                arg({'freqwnd','FrequencyRange','SpectralPrior'},[7 30],[0 Inf],'Frequency range of interest. This is the overall frequency range within which to compute the cross spectrum.'), ...
                arg({'bandwidth','TimeBandwidth'},5,[],'Spectral smoothing. Controls the bias vs. variance of the spectral estimation. Reasonable values are 1 to 3 (1 being fairly noisy, and 3 being fairly smooth but 5x slower)'), ...
                arg({'tapers','Tapers'},[],[],'Number of tapers. Should be an integer smaller than 2*TimeBandwith; default 2*TimeBandwidth-1'), ...
                arg({'subsample_spectrum','SubsampleSpectrum'},1,[1 Inf],'Sub-sample the spectrum. This is the sub-sampling factor.'), ...
                arg({'robust_spectrum','RobustSpectrum'},false,[],'Robust spectral statistics. Can help when some trials are noisy.'), ...
                arg({'normalize_spectrum','NormalizeSpectrum'},false,[],'Normalize spectrum. This corrects for the 1/f characteristic of the spectrum, which is useful when employing the power-based spec-csp prior to low frequencies.'), ...
                ... % RCSP regularization parameters
                arg({'cov_shrinkage','CovarianceShrinkage','covariance_shrinkage'},0,[0 1],'Covariance shrinkage. This is the shrinkage parameter gamma that blends between the empirical covariance (if 0) and the identity matrix (if 1). A good search range is: search(0:0.1:0.9)'), ...
                arg({'tikhonov_reg','TikhonovRegularization','tikhonov_regularization'},0,[0 Inf],'Tikhonov regularization. This parameter regularizes the CSP objective function. A good search range is: search(10.^(-10:-1))'), ...
                arg({'robust_cov','RobustCovariance','robust_covariance'},false,[],'Robust covariance estimation. Can help when some trials are noisy.'), ...
                ... % Spec-CSP specific parameters (rarely used)
                arg({'pp','ParameterP'},0,[-1 1],'Regularization parameter p''. Can be searched over -1:0.5:1.','cat','Feature Extraction','guru',true), ...
                arg({'qp','ParameterQ'},1,[0 4],'Regularization parameter q''. Can be searched over 0:0.5:4.','cat','Feature Extraction','guru',true), ...
                arg({'steps','MaxIterations'},3,uint32([1 3 10 50]),'Number of iterations. A step is spatial optimization, followed by spectral optimization.','cat','Feature Extraction'));

            [signal,n_of,pp,qp,steps] = deal(args.signal,args.patterns,args.pp,args.qp,args.steps);
            if signal.nbchan == 1
                error('Spec-RCSP does intrinsically not support single-channel data (it is a spatial filter).'); end
            if signal.nbchan < args.patterns
                error('Spec-RCSP prefers to work on at least as many channels as you request output patterns. Please reduce the number of pattern pairs.'); end
            p = pp+qp; q = qp;  % re-parameterize the hyper-parameters p' and q' into p and q
            % number of nC=Channels, nS=Samples, nT=trials, nF=frequency bins
            [nC,nS,nT] = size(signal.data); %#ok<ASGLU>
            for c=2:-1:1
                % Extension: compute the per-class multi-taper cross-spectrum across trials
                X{c} = exp_eval_optimized(set_picktrials(signal,'rank',c));
                F{c} = 2*utl_calc_crossspec('Signal',X{c},'FrequencyRange',args.freqwnd,'TimeBandwidth',args.bandwidth, ...
                    'Tapers',args.tapers,'SubsampleSpectrum',args.subsample_spectrum, 'Measure','real',...
                    'FilteredSubsampling',true,'FeatureFilters',false);
                F{c} = permute(F{c},[2 3 1 4]);
                % perform 1/f normalization
                if args.normalize_spectrum
                    F{c} = bsxfun(@times,F{c},reshape((1:size(F{c},3))/size(F{c},3),1,1,[])); end
                % compute the cross-spectrum V as an average over trials
                if args.robust_cov && X{c}.trials > 1
                    V{c} = reshape(geometric_median(reshape(F{c},[],X{c}.trials)')',nC,nC,[]);
                else
                    V{c} = mean(F{c},4);
                end
            end
            nF = size(F{1},3);
            % 1. initialize the filter set alpha and the number of filters J
            J = 1; alpha{J}(1:nF) = 1;
            % 2. for each step
            for step=1:steps
                % 3. for each set of spectral coefficients alpha{j} (j=1,...,J)
                for j=1:J
                    % 4. calculate sensor covariance matrices for each class from alpha{j}
                    for c=2:-1:1
                        Sigma{c} = zeros(nC); %#ok<*AGROW>
                        for b=1:nF
                            Sigma{c} = Sigma{c} + alpha{j}(b)*V{c}(:,:,b); end
                        % Extension: optionally implement shrinkage towards identity
                        if args.cov_shrinkage > 0
                            Sigma{c} = (1-args.cov_shrinkage)*Sigma{c} + args.cov_shrinkage*eye(nC); end
                    end

                    if args.tikhonov_reg == 0
                        % 5. solve the generalized eigenvalue problem Eq. (2) in Spec-CSP
                        [VV,DD] = eig(Sigma{1},Sigma{1}+Sigma{2});
                        iVV = inv(VV)';
                        % and retain n_of top eigenvectors at both ends of the eigenvalue spectrum...
                        W{j} = {VV(:,1:n_of), VV(:,end-n_of+1:end)};
                        P{j} = {iVV(:,1:n_of), iVV(:,end-n_of+1:end)};
                        % as well as the top eigenvalue for each class
                        lambda(j,:) = [DD(1), DD(end)];
                    else
                        [VV,DD,iVV] = deal({});
                        % Extension: use Tikhonov regularization as in TRCSP
                        for c=2:-1:1
                            M{c} = Sigma{c}/(Sigma{3-c} + args.tikhonov_reg*eye(nC));
                            try
                                [VV{c}, DD{c}] = eig(M{c});
                                [DD{c},order] = sort(diag(DD{c}),'descend');
                                VV{c} = VV{c}(:,order);
                                iVV{c} = inv(VV{c});
                                if ~all(isfinite(iVV{c}(:)))
                                    error('Divergence.'); end
                            catch
                                fprintf('ParadigmSpecRCSP: encountered a divergence.\n');
                                % keep going, results like this will be weeded out by a parameter
                                % search
                                VV{c} = randn(size(M{c}));
                                DD{c} = randn(size(M{c}));
                                iVV{c} = randn(size(M{c}));
                            end
                        end
                        % wrap up (note that the roles of patterns and filters are confused in this
                        % formulation, and that we are using the largest EVs from each matrix)
                        W{j} = {iVV{2}(1:n_of,:)',iVV{1}(1:n_of,:)'};
                        P{j} = {VV{2}(:,1:n_of), VV{1}(:,1:n_of)};
                        % ... however, the outer loop finds the minimal first lambda, so we invert it
                        lambda(j,:) = [1/DD{2}(1), DD{1}(1)];
                    end
                end

                % 7. set W{c} from all W{j}{c} such that lambda(j,c) is minimal/maximal over j
                W = {W{argmin(lambda(:,1))}{1}, W{argmax(lambda(:,2))}{2}};
                P = {P{argmin(lambda(:,1))}{1}, P{argmax(lambda(:,2))}{2}};
                % 8. for each projection w in the concatenated [W{1},W{2}]...
                Wcat = [W{1} W{2}]; J = 2*n_of;
                Pcat = [P{1} P{2}];
                for j=1:J
                    w = Wcat(:,j);
                    % 9. calcualate (across trials within each class) mean and variance of the w-projected cross-spectrum components
                    for c=2:-1:1
                        % part of Eq. (3)
                        s{c} = zeros(size(F{c},4),nF);
                        for k=1:nF
                            for t = 1:size(s{c},1)
                                s{c}(t,k) = w'*F{c}(:,:,k,t)*w; end
                        end
                        if args.robust_spectrum
                            mu_s{c} = median(s{c});
                            var_s{c} = median(abs(bsxfun(@minus,s{c},mu_s{c})))*1.4826;
                        else
                            mu_s{c} = mean(s{c});
                            var_s{c} = var(s{c});
                        end
                    end
                    % 10. update alpha{j} according to Eqs. (4) and (5)
                    for c=2:-1:1
                        for k=1:nF
                            % Eq. (4)
                            alpha_opt{c}(k) = max(0, (mu_s{c}(k)-mu_s{3-c}(k)) / (var_s{1}(k) + var_s{2}(k)) );
                            % Eq. (5), with prior from Eq. (6)
                            alpha_tmp{c}(k) = alpha_opt{c}(k).^q * ((mu_s{1}(k) + mu_s{2}(k))/2).^p;
                        end
                    end
                    % ... as the maximum for both classes
                    alpha{j} = max(alpha_tmp{1},alpha_tmp{2});
                    % and normalize alpha{j} so that it sums to unity
                    alpha{j} = alpha{j} / sum(alpha{j});
                end
            end
            alpha = vertcat(alpha{:})';
            model = struct('W',{Wcat},'P',{Pcat},'nF',{nF},'nC',{nC},'alpha',{alpha},'chanlocs',{signal.chanlocs}, ...
                'freqwnd',{args.freqwnd},'bandwidth',{args.bandwidth},'tapers',{args.tapers}, ...
                'subsample_spectrum',{args.subsample_spectrum},'nTrials',[X{1}.trials,X{2}.trials]);
        end

        function features = feature_extract(self,signal,mdl) %#ok<*INUSL>
            spec = 2*utl_calc_crossspec('Signal',signal,'FrequencyRange',mdl.freqwnd,'TimeBandwidth',mdl.bandwidth, ...
                'Tapers',mdl.tapers,'SubsampleSpectrum',mdl.subsample_spectrum, 'Measure','real',...
                'FilteredSubsampling',true,'FeatureFilters',false);
            features = zeros(size(signal.data,3),size(mdl.W,2));
            [W, alpha] = deal(mdl.W, mdl.alpha);
            for f=1:size(W,2)
                C = permute(sum(bsxfun(@times,spec,alpha(:,f)),1),[2,3,4,1]);
                for t=1:size(signal.data,3)
                        features(t,f) = log(W(:,f)'*C(:,:,t)*W(:,f)); end
            end
        end

        function visualize_model(self,varargin) %#ok<*INUSD>
            args = arg_define([0 3],varargin, ...
                arg_norep({'myparent','Parent'},[],[],'Parent figure.'), ...
                arg_norep({'featuremodel','FeatureModel'},[],[],'Feature model. This is the part of the model that describes the feature extraction.'), ...
                arg_norep({'predictivemodel','PredictiveModel'},[],[],'Predictive model. This is the part of the model that describes the predictive mapping.'), ...
                arg({'patterns','PlotPatterns'},true,[],'Plot patterns instead of filters. Whether to plot spatial patterns (forward projections) rather than spatial filters.'), ...
                arg({'paper','PaperFigure'},false,[],'Use paper-style font sizes. Whether to generate a plot with font sizes etc. adjusted for paper.'), ...
                arg_nogui({'nosedir_override','NoseDirectionOverride'},'',{'','+X','+Y','-X','-Y'},'Override nose direction.'));
            arg_toworkspace(args);

            % no parent: create new figure
            if isempty(myparent)
                myparent = figure('Name','Common Spatial Patterns'); end
            % determine nose direction for EEGLAB graphics
            try
                nosedir = args.fmodel.signal.info.chaninfo.nosedir;
            catch
                disp_once('Nose direction for plotting not store in model; assuming +X');
                nosedir = '+X';
            end
            if ~isempty(nosedir_override)
                nosedir = nosedir_override; end
            % number of pairs, and index of pattern per subplot
            np = size(featuremodel.W,2)/2; idxp = [1:np np+(2*np:-1:np+1)]; idxf = [np+(1:np) 2*np+(2*np:-1:np+1)];
            % for each CSP pattern...
            for p=1:np*2
                subplot(4,np,idxp(p),'Parent',myparent);
                if args.patterns
                    plotdata = featuremodel.P(:,p);
                else
                    plotdata = featuremodel.W(:,p);
                end
                topoplot(plotdata,featuremodel.chanlocs,'nosedir',nosedir);
                subplot(4,np,idxf(p),'Parent',myparent);
                alpha = featuremodel.alpha(:,p);
                range = 1:max(find(alpha)); %#ok<MXFND>
                if ~isfield(featuremodel,'freqs')
                    featuremodel.freqs = linspace(featuremodel.freqwnd(1),featuremodel.freqwnd(2),length(range)); end
                pl=plot(featuremodel.freqs(range),featuremodel.alpha(range,p));
                xlim([min(featuremodel.freqs(range)) max(featuremodel.freqs(range))]);
                l1 = xlabel('Frequency in Hz');
                l2 = ylabel('Weight');
                t=title(['Spec-CSP Pattern ' num2str(p)]);
                if args.paper
                    set([gca,t,l1,l2],'FontUnits','normalized');
                    set([gca,t,l1,l2],'FontSize',0.2);
                    set(pl,'LineWidth',2);
                end
            end
            try set(gcf,'Color',[1 1 1]); end
        end

        function layout = dialog_layout_defaults(self)
            layout = {'SignalProcessing.Resampling.SamplingRate', 'SignalProcessing.FIRFilter.Frequencies', ...
                'SignalProcessing.FIRFilter.Type', 'SignalProcessing.EpochExtraction', '', ...
                'Prediction.FeatureExtraction', '', ...
                'Prediction.MachineLearning.Learner'};
        end

        function tf = needs_voting(self)
            tf = true;
        end

    end
end


function days = month_length_persian ( y, m )

%*****************************************************************************80
%
%% MONTH_LENGTH_PERSIAN returns the number of days in a Persian month.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 July 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, the year in which the month occurred.
%
%    Input, integer M, the number of the month.
%
%    Output, integer DAYS, the number of days
%    in the month.
%
  mdays = [ 31, 31, 31, 31, 31, 31, 30, 30, 30, 30, 30, 29 ];
%
%  Copy the input.
%
  m2 = m;
  y2 = y;
%
%  Get the number of days in the month.
%
  days = mdays(m2);
%
%  If necessary, add 1 day for a leap year.
%
  if ( m2 == 12 && year_is_leap_persian ( y2 ) )
    days = days + 1;
  end

  return
end