% PROMAX - perform Promax oblique rotation after orthogonal Varimax
%            rotation of the rows of the input data. A method for
%            linear decomposition by "rotating to simple structure."
% Usage:
%        >> [R]   = promax(data,ncomps);
%        >> [R,V] = promax(data,ncomps,maxit);
%
% Inputs:
%     data    - Promax operates on rows of the input data matrix
%     ncomps  - operate on the N largest PCA components (default|0 -> all)
%     maxit   - maximum number of iterations {default|0 -> 5}
%
% Outputs:
%     R       - is the non-orthogonal Promax rotation matrix
%                 i.e.,  >> promax_rotated_data = R*data;
%     V       - is the orthogonal Varimax rotation matrix
%                 i.e.,  >> varimax_rotated_data = V*data;
%
% Author: Colin Humphries, CNL / Salk Institute, 1998
%
% See also: RUNICA

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

% formatted and modified to return V by Scott Makeig, 6/23/98
% reset maxit default to 5, added ncomps -sm 7/8/98
% 01-25-02 reformated help & license, added links -ad
%
% Reference:
%
% Hendrickson AE and White PO (1964) Promax: A quick method for rotation
% to oblique simple structure, Br J of Stat Psych, X:xxx-xxx.

function [R,V] = promax(data,ncomps,maxit)

v = version;
indp = find(v == '.');
v = str2num(v(1:indp(2)-1));
if v >= 8.1
    disp('Note: for some unknown reason, this function does not return')
    disp('      NaN for a singular matrix under Matlab 2013a and later versions.')
    disp('      Promax on other matrices seems to as in previous revisions though.');
end

DEFAULT_POWER     = 4;
DEFAULT_TOLERANCE = 1e-5;
MAX_ITERATIONS    = 5;
NEAR_ZERO         = 1e-8;

powr = DEFAULT_POWER;
tol = DEFAULT_TOLERANCE;

if nargin < 1
  help promax
  return
end
if isempty(data)
  help promax
  return
end

if nargin < 2
   ncomps = 0;
end
chans = size(data,1)
if ncomps == 0
   ncomps = chans
end
if ncomps > chans
    error(sprintf('promax(): components must be <= number of data rows (%d).\n',chans));
end
if nargin < 3
  maxit = 0;
end
if maxit == 0
  maxit = MAX_ITERATIONS;
end

if ncomps < chans
  [eigenvectors,eigenvalues,compressed,datamean] = pcsquash(data,ncomps);
  data = compressed;
  clear compressed;
  eigenvectors = eigenvectors(:,1:ncomps); % make non-square
  eigenwts = pinv(eigenvectors); % find forward (non-square) weight matrix
end

R = varimax(data); % run Varimax on the (projected) data
B = R*data;        % compute rotated data
V = R;             % save Varimax matrix as V
if ncomps < chans
 V = V*eigenwts;   % include PCA reduction matrix
end
B = B';            % transpose
R = R';
cont = 1;
fprintf(...
  'Finding oblique Promax rotation using exponent %g and tolerance %g\n',...
                                                  powr,tol)
it = 1;
Pz = zeros(size(B));
while cont & it <= maxit
  P = Pz;
  ii = find(abs(B) > NEAR_ZERO); % avoid division by 0
  P(ii) = (abs(B(ii).^(powr+1)))./B(ii);
  tmp = inv(B'*B)*B'*P;
  tmp = normalcol(tmp);
  Rn = R*tmp;
  B = B*tmp;
  distnew = dot(Rn(:),R(:));
  if it > 1
    delta = abs(distnew-distold);
    if delta < tol
      cont = 0;
    end
    fprintf('#%d delta %f\n',it,delta)
    if isnan(delta)
      cont = 0;
    end
  end
  R = Rn;
  distold = distnew;
  it = it+1;
end
B = B';
R = R';
if ncomps < chans
   R = R*eigenwts; % include the pcsquash() compression
end

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

function n=normalcol(m)

if isempty(m)
  fprintf('normalcol() has empty input!\n');
  return
end
[mr,mc] = size(m);
n = sqrt(ones./sum(m.*m));
n = ones(mr,1)*n;
n = n.*m;


function SO3F = conv(SO3F1,SO3F2,varargin)
% convolution of an SO3FunHarmonic with a function or a kernel on SO(3)
%
% 1) SO3Fun * SO3Fun
% There are two SO3Funs $f: _{S_f^L\backslash}SO(3)_{/S_f^R} \to \mathbb{C}$
% where $S_f^L$ is the Left symmetry and $S_f^R$ is the Right symmetry and
% $g: _{S_g^L\backslash}SO(3)_{/S_g^R} \to \mathbb{C}$ given.
% Then the convolution $ f *_L g : _{S_f^L\backslash}SO(3)_{/S_g^R} \to
% \mathbb{C}$ is defined by
%
% $$ (f *_L g)(R) = \frac1{8\pi^2} \int_{SO(3)} f(q) \cdot g(q^{-1}\,R) \, dq $$
%
% and the convolution $ f *_R g : _{S_g^L\backslash}SO(3)_{/S_f^R} \to
% \mathbb{C}$ is defined by
%
% $$ (f *_R g)(R) = \frac1{8\pi^2} \int_{SO(3)} f(q) \cdot g(R\,q^{-1}) \, dq $$.
%
% with $vol(SO(3)) = \int_{SO(3)} 1 \, dR = 8\pi^2$.
% The convolution $*_L$ is used as default.
% The convolution of matrices of SO3Functions with matrices of SO3Functions
% works elementwise.
%
% 2) SO3Fun * S2Fun
% The convolution of an SO3Fun  $f: _{S_f^L\backslash}SO(3)_{/S_f^R} \to \mathbb{C}$
% with an S2Fun $h: \mathbb S^2_{/S_h} \to \mathbb{C}$ yields
% $f*h:\mathbb S^2_{/S_f^L} \to \mathbb{C}$ with
%
% $$ (f * h)(\xi) =  \frac1{8\pi^2} \int_{SO(3)} f(q) \cdot h(q^{-1}\,\xi) \, dq $$.
%
% 3) In particular we convolute an SO3Fun with an SO3Kernel similar to the
% first case. Therefore the Right and Left sided convolution are
% equivalent. The convolution of an SO3Fun with an S2Kernel works analogue
% to case 2.
%
%
% Syntax
%   SO3F = conv(SO3F1,SO3F2)
%   SO3F = conv(SO3F1,SO3F2,'Right')
%   SO3F = conv(SO3F1,psi)
%   sF2 = conv(SO3F1,sF1)
%   sF2 = conv(SO3F1,phi)
%
% Input
%  SO3F1, SO3F2 - @SO3Fun
%  psi          - convolution @SO3Kernel
%  sF1          - @S2Fun
%  phi          - convolution @S2Kernel
%
% Output
%  SO3F - @SO3FunHarmonic
%  sF2  - @S2FunHarmonic
%
% See also
% SO3Kernel/conv SO3FunHarmonic/conv SO3FunRBF/calcFourier S2FunHarmonic/conv S2Kernel/conv

% The convolution is defined like above. But in MTEX the convolution of two
% SO3Funs is mostly calculated by
%                    conv(inv(conj(SO3F1)),SO3F2).
%

if isnumeric(SO3F1)
  SO3F = conv(SO3F2,SO3F1,varargin{:});
  return
end
if isnumeric(SO3F2)
  v = 2*(1:length(SO3F2))'-1;
  SO3F2 = SO3Kernel(SO3F2.*v);
end

% ------------------- convolution with a S2Kernel -------------------
if isa(SO3F2,'S2Kernel')
  psi = SO3F2;
  L = min(SO3F1.bandwidth,psi.bandwidth);

  fhat = zeros((L+1)^2,1);
  for l = 0:L
    fhat(l^2+1:(l+1)^2) = 2*sqrt(pi)/(2*l+1) * ...
          SO3F1.fhat(deg2dim(l)+(2*l+1)*l+(1:2*l+1)')*psi.A(l+1);
  end

  warning(['There is no symmetry given for the S2Kernel function. But for convolution the ' ...
      'right symmetry of the SO3Fun has to be compatible with the symmetry of the S2Fun.'])
  SO3F = S2FunHarmonic(fhat);
  return

end

% ------------------- convolution with a S2Fun -------------------
if isa(SO3F2,'S2Fun')

  sF = S2FunHarmonic(SO3F2);
  L = min(SO3F1.bandwidth,sF.bandwidth);

  fhat = zeros((L+1)^2,1);
  for l = 0:L
    fhat(l^2+1:(l+1)^2) = reshape(SO3F1.fhat(deg2dim(l)+1:deg2dim(l+1)),2*l+1,2*l+1) * ...
          sF.fhat(l^2+1:(l+1)^2) ./ sqrt(2*l+1);
  end

  % we need that SO3F1.SRight == SO3F.Sym
  if numProper(SO3F1.SLeft) == 1
    SO3F = S2FunHarmonicSym(fhat,SO3F1.SRight);
  elseif isa(SO3F2,'S2FunHarmonicSym')
    ensureCompatibleSymmetries(SO3F1,SO3F2);
    SO3F = S2FunHarmonicSym(fhat,SO3F1.SRight);
  else
    warning(['There is no symmetry of the S2Fun given. But for convolution the ' ...
      'right symmetry of the SO3Fun has to be compatible with the unknown symmetry.'])
    SO3F = S2FunHarmonic(fhat);
  end

  return

end

% ------------------- convolution with a SO3Kernel -------------------
% ( Here *L is the same like *R )
if isa(SO3F2,'SO3Kernel')

  L = min(SO3F1.bandwidth,SO3F2.bandwidth);
  SO3F1.bandwidth = L;
  s = size(SO3F1); SO3F1 = SO3F1.subSet(':');

  % multiply Wigner-D coefficients of SO3F1
  % with the Chebyshev coefficients A of SO3F2
  A = SO3F2.A;
  for l = 0:L
    SO3F1.fhat(deg2dim(l)+1:deg2dim(l+1),:) = ...
      A(l+1)./(2*l+1) * SO3F1.fhat(deg2dim(l)+1:deg2dim(l+1),:);
  end

  SO3F = reshape(SO3F1,s);
  return

end

% ------------------- convolution of SO3Fun's -------------------
% i) right sided convolution
if check_option(varargin,'Right')
  SO3F = inv(conv(inv(SO3F1),inv(SO3F2)));
  return
end

% ii) left sided convolution (default)
ensureCompatibleSymmetries(SO3F1,SO3F2,'conv_Left');

L = min(SO3F1.bandwidth,SO3F2.bandwidth);

% compute Fourier coefficients
fhat1 = SO3F1.calcFourier('bandwidth',L);
fhat2 = SO3F2.calcFourier('bandwidth',L);

% construct SO3FunHarmonic by multiplying the Fourier coefficients
SO3F = SO3FunHarmonic(convSO3(fhat1,fhat2),SO3F2.SRight,SO3F1.SLeft);

end


%% Bacterial foraging
% Animiation of bacteria movement to get the global minimum solution every chemotactic
%
% Author: Wael Mansour (wael192@yahoo.com)
%
% MSc Student, Electrical Enginering Dept,
% Faculty of Engineering Cairo University, Egypt

%%
%Initialization
clear all
clc
p=2;                         % dimension of search space
s=26;                        % The number of bacteria
Nc=50;                       % Number of chemotactic steps
Ns=4;                        % Limits the length of a swim
Nre=4;                       % The number of reproduction steps
Ned=2;                       % The number of elimination-dispersal events
Sr=s/2;                      % The number of bacteria reproductions (splits) per generation
Ped=0.25;                    % The probabilty that each bacteria will be eliminated/dispersed
c(:,1)=0.05*ones(s,1);       % the run length
for m=1:s                    % the initital posistions
    P(1,:,1,1,1)= 50*rand(s,1)';
    P(2,:,1,1,1)= .2*rand(s,1)';
   %P(3,:,1,1,1)= .2*rand(s,1)';
end

%%
%Main loop

%Elimination and dispersal loop
for ell=1:Ned

%Reprodution loop

    for K=1:Nre

%  swim/tumble(chemotaxis)loop

        for j=1:Nc

            for i=1:s
                J(i,j,K,ell)=Live_fn(P(:,i,j,K,ell));

% Tumble

                Jlast=J(i,j,K,ell);
                Delta(:,i)=(2*round(rand(p,1))-1).*rand(p,1);
                P(:,i,j+1,K,ell)=P(:,i,j,K,ell)+c(i,K)*Delta(:,i)/sqrt(Delta(:,i)'*Delta(:,i)); % This adds a unit vector in the random direction

% Swim (for bacteria that seem to be headed in the right direction)

                J(i,j+1,K,ell)=Live_fn(P(:,i,j+1,K,ell));
                m=0;         % Initialize counter for swim length
                    while m<Ns
                          m=m+1;
                          if J(i,j+1,K,ell)<Jlast
                             Jlast=J(i,j+1,K,ell);
                             P(:,i,j+1,K,ell)=P(:,i,j+1,K,ell)+c(i,K)*Delta(:,i)/sqrt(Delta(:,i)'*Delta(:,i)) ;
                             J(i,j+1,K,ell)=Live_fn(P(:,i,j+1,K,ell));
                          else
                             m=Ns ;
                          end

                    end
                J(i,j,K,ell)=Jlast;
                sprintf('The value of interation i %3.0f ,j = %3.0f  , K= %3.0f, ell= %3.0f' , i, j, K ,ell );

            end % Go to next bacterium

            x = P(1,:,j,K,ell);
            y = P(2,:,j,K,ell);
            clf
            plot(x, y , 'h')
            axis([-5 5 -5 5]);
            pause(.1)
        end  % Go to the next chemotactic

%Reprodution
        Jhealth=sum(J(:,:,K,ell),2);              % Set the health of each of the S bacteria
        [Jhealth,sortind]=sort(Jhealth);          % Sorts the nutrient concentration in order of ascending
        P(:,:,1,K+1,ell)=P(:,sortind,Nc+1,K,ell);
        c(:,K+1)=c(sortind,K);                    % And keeps the chemotaxis parameters with each bacterium at the next generation

%Split the bacteria (reproduction)
            for i=1:Sr
                P(:,i+Sr,1,K+1,ell)=P(:,i,1,K+1,ell); % The least fit do not reproduce, the most fit ones split into two identical copies
                c(i+Sr,K+1)=c(i,K+1);
            end
        end %  Go to next reproduction

%Eliminatoin and dispersal
        for m=1:s
            if  Ped>rand % % Generate random number
                P(1,:,1,1,1)= 50*rand(s,1)';
                P(2,:,1,1,1)= .2*rand(s,1)';
               %P(3,:,1,1,1)= .2*rand(s,1)';
            else
                P(:,m,1,1,ell+1)=P(:,m,1,Nre+1,ell); % Bacteria that are not dispersed
            end
        end
    end % Go to next elimination and disperstal

%Report
           reproduction = J(:,1:Nc,Nre,Ned);
           [jlastreproduction,O] = min(reproduction,[],2);  % min cost function for each bacterial
           [Y,I] = min(jlastreproduction)
           pbest=P(:,I,O(I,:),K,ell)


function [ Acc ] = mi_performance_off2on(CNT,general_initparam)
%CNT{1} = train
%CNT{2} = test
opt = opt_cellToStruct(general_initparam);

% Pre-processing
for onoff=1:length(opt.task)
    CNTch = prep_selectChannels(CNT{onoff}, {'Index', opt.channel_index});
    CNTchfilt =prep_filter(CNTch , {'frequency', opt.band});
    all_SMT{onoff} = prep_segmentation(CNTchfilt, {'interval', opt.time_interval});
    clear CNTch CNTchfilt
end
train = all_SMT{1}; test = all_SMT{2};

% Feature extracion and Classification
[SMT, CSP_W, CSP_D]=func_csp(train,{'nPatterns', opt.CSPFilter});
FT=func_featureExtraction(SMT, {'feature','logvar'});
[CF_PARAM]=func_train(FT,{'classifier','LDA'});

SMT_te=func_projection(test, CSP_W);
FT_te=func_featureExtraction(SMT_te, {'feature','logvar'});
[cf_out]=func_predict(FT_te, CF_PARAM);
[loss out]=eval_calLoss(FT_te.y_dec, cf_out);
Acc=1-loss;
end


clear

load('menpo_68_pts_flip');

pdmLoc = ['../../models/pdm/pdm_68_aligned_wild.mat'];
load(pdmLoc);
% Before plugging into NRSFM, align the points in 2D
num_pts = size(all_pts,1)/2;

num_lmks = numel(M) / 3;
m = reshape(M, num_lmks, 3)';
width_model = max(m(1,:)) - min(m(1,:));
height_model = max(m(2,:)) - min(m(2,:));

for i=1:size(all_pts,1)/2

    shape2D = cat(2, all_pts(i,:)', all_pts(i+num_pts,:)');

    M_n = M;

    if(sum(shape2D(:)==-1) > 0)

        hidden = true;
        % which indices to remove
        inds_to_rem = shape2D(:,1) == -1 | shape2D(:,2) == -1;

        shape2D = shape2D(~inds_to_rem,:);

        inds_to_rem = repmat(inds_to_rem, 3, 1);

        M_n = M(~inds_to_rem);

    end

    % To deal with really extreme cases of roll
    M2D = cat(2, M_n(1:end/3), M_n(end/3+1:2*end/3));
    [ A, t, error, alignedShape, s ] = AlignShapesWithScale(M2D, shape2D);
    R = A/s;

    % Transform the shape
    shape2D(:,1) = shape2D(:,1) - t(1);
    shape2D(:,2) = shape2D(:,2) - t(2);

    shape2D = (R' * shape2D')/s;
    shape2D = shape2D';

    all_pts(i,all_pts(i,:)~=-1) = shape2D(:,1);
    all_pts(i+num_pts,all_pts(i+num_pts,:)~=-1) = shape2D(:,2);
end

%%
% to_rem = randperm(round(0.1*num_pts)); % Remove 10% to break some symmetry
% all_pts([to_rem, to_rem+num_pts],:) = [];
num_pts = size(all_pts,1)/2;

left_ids = all_pts(1:num_pts,10) == -1;
right_ids = all_pts(1:num_pts,8) == -1;
frontal = true(num_pts,1);
frontal(left_ids | right_ids) = false;

%%
xs = all_pts(1:num_pts,:);
ys = all_pts(num_pts+1:end,:);

% Randperm the data, as a test

% xs_f = xs(frontal,:);
% ys_f = ys(frontal,:);
% scatter(xs_f(:), -ys_f(:));

%% Perform NRSFM by Torresani
addpath('../nrsfm-em');
% (T is the number of frames, J is the number of points)

J = size(all_pts,2);
T = size(all_pts,1)/2;

use_lds = 0; % not modeling a linear dynamic system here
max_em_iter = 200;
tol = 0.001;
K = 30; % number of deformation shapes

MD = all_pts(1:end/2,:)==-1;

[P3, S_hat, V, RO, Tr, Z] = em_sfm(all_pts, MD, K, use_lds, tol, max_em_iter);

save('Torr_menpo', 'P3', 'S_hat', 'V', 'RO', 'Tr', 'Z');

%%
% xs = P3(1:num_pts,:);
% ys = P3(num_pts+1:2*num_pts,:);
% zs = P3(2*num_pts+1:end,:);
% %
% xs_f = xs(left_ids,:);
% ys_f = ys(left_ids,:);
% zs_f = zs(left_ids,:);
% scatter3(xs_f(:), ys_f(:), zs_f(:));


function [ a, b ] = p46_lim ( )

%*****************************************************************************80
%
%% P46_LIM returns the integration limits for problem 46.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 November 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real A, B, the limits of integration.
%
  a = 0.0;
  b = 2.0 * pi;

  return
end


function E=fit2(x0,x,y)
E=sum(abs( x0(1)*x+x0(2)-y ));


function z = msign(x)
%% MSIGN modified sign function
%
% z = MSIGN(x) returns zero when abs(x)<sqrt(eps)

z = sign(x);
idx = (abs(x)< sqrt(eps));
z(idx) = 0;


function [L,a,b] = rgb2lab(R,G,B)
% function [L, a, b] = RGB2Lab(R, G, B)
% RGB2Lab takes matrices corresponding to Red, Green, and Blue, and
% transforms them into CIELab.  This transform is based on ITU-R
% Recommendation  BT.709 using the D65 white point reference.
% The error in transforming RGB -> Lab -> RGB is approximately
% 10^-5.  RGB values can be either between 0 and 1 or between 0 and 255.
% By Mark Ruzon from C code by Yossi Rubner, 23 September 1997.
% Updated for MATLAB 5 28 January 1998.

if (nargin == 1)
  B = double(R(:,:,3));
  G = double(R(:,:,2));
  R = double(R(:,:,1));
end

if ((max(max(R)) > 1.0) | (max(max(G)) > 1.0) | (max(max(B)) > 1.0))
  R = R/255;
  G = G/255;
  B = B/255;
end

[M, N] = size(R);
s = M*N;

% Set a threshold
T = 0.008856;

RGB = [reshape(R,1,s); reshape(G,1,s); reshape(B,1,s)];

% RGB to XYZ
MAT = [0.412453 0.357580 0.180423;
       0.212671 0.715160 0.072169;
       0.019334 0.119193 0.950227];
XYZ = MAT * RGB;

X = XYZ(1,:) / 0.950456;
Y = XYZ(2,:);
Z = XYZ(3,:) / 1.088754;

XT = X > T;
YT = Y > T;
ZT = Z > T;

fX = XT .* X.^(1/3) + (~XT) .* (7.787 .* X + 16/116);

% Compute L
Y3 = Y.^(1/3);
fY = YT .* Y3 + (~YT) .* (7.787 .* Y + 16/116);
L  = YT .* (116 * Y3 - 16.0) + (~YT) .* (903.3 * Y);

fZ = ZT .* Z.^(1/3) + (~ZT) .* (7.787 .* Z + 16/116);

% Compute a and b
a = 500 * (fX - fY);
b = 200 * (fY - fZ);

L = reshape(L, M, N);
a = reshape(a, M, N);
b = reshape(b, M, N);

if ((nargout == 1) | (nargout == 0))
  L = cat(3,L,a,b);
end


% ****************************************************************
% This is a function used for robotics project
%
% MAE 505: Robotic Final Project.
% Created By: Gan Tao
% Date: 10 Dec 2003.
% ****************************************************************

function [X_dot] = good_lang2(t, q)

% Xc                ***X coordinate of center mass of the mobile platform
% Yc                ***Y coordinate of center mass of the mobile platform
% fai               ***heading angle of the platform measured from the X axis of the world coordinates
% theta_R           ***angular displacement of the right wheel
% theta_L           ***angular displacement of the left wheel
% theta_dot_R       ***angular velocity of the right wheel??
% theta_dot_L       ***
% Xr                ***X coordinate of world coordinates of the reference point
% Yr                ***Y coordinate of world coordinates of the reference point
% Xr_c              ***
% Yr_c              ***
% c                 ***c=(r/2*b)
% b                 ***the distance between the driving wheels and the axis of symmetry
% d                 ***the distance from Po(the intersection of the axis of symmetry with the driving wheel axis) to Pc(the center
%                   ***of mass of the platform

Xr_c=5;
Yr_c=0;
c   =0.5;
b   =0.75;
d   =1;
Kp  =2;
Kd  =5;

Xc          = q(1); % xc
Yc          = q(2); % yc
fai         = q(3); % fai
theta_R     = q(4); % theta right
theta_L     = q(5); % theta left
theta_dot_R = q(6); % theta dot right
theta_dot_L = q(7); % theta dot left

% ****************** The path to trace:**************************************************************************

y_desire = t;       %line case ii x=y;
x_desire = t;

xy_dot_desire = [1; 1]; %2x1 matrix         line x=y case ii;

xy_2_dot_desire = [0; 0]; %2x1 matrix

xy_desire =[x_desire y_desire]';

%****************** To get the new input V(y2_dot) **************************************************************

Xr = Xc+Xr_c*cos(fai)-Yr_c*sin(fai);
Yr = Yc+Xr_c*sin(fai)+Yr_c*cos(fai);

xy = [Xr;Yr];

S = [c*(b*cos(fai)-d*sin(fai)) c*(b*cos(fai)+d*sin(fai));
     c*(b*sin(fai)+d*cos(fai)) c*(b*sin(fai)-d*cos(fai));
     c                         -c                       ;
     1                         0                        ;
     0                         1                        ]; %5*2;

faifunction = [ c*(b*cos(fai)-d*sin(fai))+(-Xr_c*sin(fai)-Yr_c*cos(fai))*c,c*(b*cos(fai)+d*sin(fai))-(-Xr_c*sin(fai)-Yr_c*cos(fai))*c;
                c*(b*sin(fai)+d*cos(fai))+(Xr_c*cos(fai)-Yr_c*sin(fai))*c,c*(b*sin(fai)-d*cos(fai))-(Xr_c*cos(fai)-Yr_c*sin(fai))*c ]; %2*2;

%*** faifunction_dot = diff(faifunction,fai);*****
niu = [theta_dot_R;theta_dot_L];

faifunction_dot =[ c*(-b*sin(fai)-d*cos(fai))+(-Xr_c*cos(fai)+Yr_c*sin(fai))*c,c*(-b*sin(fai)+d*cos(fai))-(-Xr_c*cos(fai)+Yr_c*sin(fai))*c;
             c*(b*cos(fai)-d*sin(fai))+(-Xr_c*sin(fai)-Yr_c*cos(fai))*c,c*(b*cos(fai)+d*sin(fai))-(-Xr_c*sin(fai)-Yr_c*cos(fai))*c ]; %2*2;

xy_dot  = faifunction*niu;%2*1;

xy2_dot = xy_2_dot_desire+Kd*(xy_dot_desire - xy_dot)+Kp*(xy_desire - xy);%2*1;

%**************** Using V(xy2_dot) to get the U input ************************************************************

U = inv(faifunction)*(xy2_dot-faifunction_dot*niu); %2*1

%**************** Using U to get the X_dot ***********************************************************************

f1 = S*niu;%5*1;

X_dot = [f1(1);f1(2);f1(3);f1(4);f1(5); 0; 0] + [0 0;0 0;0 0;0 0;0 0;1 0;0 1]*[U(1);U(2)];


filename='RVE_Square_Triangle';
ptype = 'MICRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'circleInclusion';
cost={'volume','perimeter'};
weights=[1 0.1];
constraint = {'enforceCh_CCstar_eq'};%inf & equality
optimizer = 'MMA'; incrementFactor = 1;
filterType = 'P1';

nsteps = 1;
Vfrac_final = 1;
Perimeter_target=1;
optimality_final =1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-3;
constr_initial = 1e-3;

TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;

%Micro
epsilon_isotropy_initial=1e-1;
epsilon_isotropy_final = 1e-3;
selectiveC_Cstar = [1,1,1;
    1,1,1;
    1,1,1]; % 1 to select the component


function [c, ceq] = ConstraintFCN_models(u,uold,x,N,LBo,UBo,LBdu,UBdu,p,select_model)
%% Constraint function of nonlinear MPC for F8 system
%
% Inputs:
%   u:      optimization variable, from time k to time k+N-1
%   x:      current state at time k
%   Ts:     controller sample time
%   N:      prediction horizon length
%   uold:   latest applied control input
%   LBo:    Lower bound of output x
%   UBo:    Upper bound of output x
%   LBdu:   Lower bound for input difference uk - uk-1
%   UBdu:   Upper bound for input difference uk - uk-1
%   p:      Parameters for model
%   select_model: Selects model future-state prediction
%
% Output:
%   c:      inequality constraints applied across prediction horizon
%   ceq:    equality constraints  (empty)
%
Nvar = length(x);
%% Nonlinear MPC design parameters
% range of angle of attack
zMin = LBo(1);
zMax = UBo(1);

%% Integrate system
if strcmp(select_model,'DelayDMDc')
     [xk,~] = lsim(p.sys,[p.udelay(1:N);u'],[0:N-1].*p.dt,[p.xdelay(:,1); x]-[p.xmean; p.xmean]);
    xk = xk(:,Nvar+1:2*Nvar);
    xk = xk + repmat(p.xmean',[N 1]); xk = xk';
elseif strcmp(select_model,'DMDc')
    [xk,~] = lsim(p.sys,[u',0],[0:N].*p.dt,x-p.xmean);
    xk = xk(2:end,:) + repmat(p.xmean',[N 1]); xk = xk';
elseif strcmp(select_model,'SINDYc')
    Ns = size(x,1);
    xk = zeros(Ns,N+1); xk(:,1) = x;
    for ct=1:N
        % Obtain plant state at next prediction step.
        xk(:,ct+1) = rk4u(@sparseGalerkinControl_Discrete,xk(:,ct),u(ct),p.dt,1,[],p);
    end
    xk = xk(:,2:N+1);
elseif strcmp(select_model,'NARX')
    Hu = [u',0];
    Hx = zeros(Nvar,length(Hu)); Hx(:,1) = x;
    [Us,Ui,Si] = preparets(p.net,con2seq(Hu),{},con2seq(Hx));
    xk = p.net(Us,Ui,Si);
    xk = cell2mat(xk);
end

%% Inequality constraints calculation
c = zeros(2*N,1);

% Apply N population size constraints across prediction horizon, from time
% k+1 to k+N
duk = u(1)-uold;
for ct=1:N
    c(2*ct-1) = -duk+LBdu;
    c(2*ct) = duk-UBdu;
    if ct<N
        duk = u(ct+1)-u(ct);
    end
end

c1 = zeros(2*N,1);
c2 = zeros(2*N,1);
c3 = zeros(2*N,1);
for ct=1:N
    c1(2*ct-1) = -xk(1,ct)+zMin;
    c2(2*ct-1) = -xk(2,ct)+LBo(2);
    c3(2*ct-1) = -xk(3,ct)+LBo(3);
    c1(2*ct) = xk(1,ct)-zMax;
    c2(2*ct) = xk(2,ct)-UBo(2);
    c3(2*ct) = xk(3,ct)-UBo(3);
end

c = [c;c1;c2;c3];

%% No equality constraints
ceq = [];


function pass = test_isNotMultOrDiff(~)
%TEST_ISNOTMULTORDIFF    Test that the property of operator not being
%diff/integration is dealt with correctly.

% Construct the primitive operatorBlocks
dom = [0, 2];
[Z, I, D, C, M] = linop.primitiveOperators(dom);
M = M(chebfun(@sin, dom));

% Check that they have the expected value of the property
pass(1) = ( Z.isNotDiffOrInt == 1 );
pass(2) = ( I.isNotDiffOrInt == 1 );
pass(3) = ( D.isNotDiffOrInt == 0 );
pass(4) = ( C.isNotDiffOrInt == 0 );
pass(5) = ( M.isNotDiffOrInt == 1 );

% Operations on operatorBlocks
A = Z + I;
pass(6) = ( A.isNotDiffOrInt == 1 );
A = Z + D;
pass(7) = ( A.isNotDiffOrInt == 0 );
A = M*Z;
pass(8) = ( A.isNotDiffOrInt == 1 );
A = C*Z;
pass(9) = ( A.isNotDiffOrInt == 1 );
A = C + M;
pass(10) = ( A.isNotDiffOrInt == 0 );
A = 2*M;
pass(11) = ( A.isNotDiffOrInt == 1 );
A = 2*D;
pass(12) = ( A.isNotDiffOrInt == 0 );
end


% WAVELET_FACTORY_1D Create wavelet cascade
%
% Usage
%    [Wop, filters] = WAVELET_FACTORY_1D(N)
%
%    [Wop, filters] = WAVELET_FACTORY_1D(N, filt_opt)
%
%    [Wop, filters] = WAVELET_FACTORY_1D(N, filt_opt, scat_opt)
%
% Input
%    N (int): The size of the signals to be transformed.
%    filt_opt (struct): The filter options, same as FILTER_BANK.
%    scat_opt (struct): The scattering options, same as WAVELET_LAYER_1D.
%
% Output
%    Wop (cell): A cell array of wavelet layer transforms needed for the
%       scattering transform.
%    filters (cell): A cell array of the filters used in defining the
%       wavelets.
%
% Description
%    In order to calculate the scattering coefficients of a signal, a set of
%    filter banks need to be defined first. Given these filter banks, wavelet
%    transforms can be defined on scattering layers. To obtain the former,
%    WAVELET_FACTORY_1D calls the FILTER_BANK function using the parameters
%    provided in filt_opt, the result of which is returned as filters.
%    Each filter bank is then used to create a layer operator, using the
%    function WAVELET_LAYER_1D. This function takes a layer, a filter bank
%    and a set of parameters as input. WAVELET_FACTORY_1D fixes the filter
%    bank as to the element of filters corresponding to the layer order, while
%    scat_opt is used as the parameters. The result is a cell array Wop,
%    of layer operators which take one layer as input, and return two layers
%    as output, A and V corresponding to the "average" and "variation" of the
%    input layer.
%
% See also
%    WAVELET_LAYER_1D, WAVELET_1D, FILTER_BANK

function [Wop, filters] = wavelet_factory_1dave(N, filt_opt, scat_opt)
	if nargin < 2
		filters = filter_bank(N);
    else
        filters = filter_bank(N, filt_opt);
    end

	if nargin < 3
		scat_opt.M = 2; % M is the scattering order
    else
        scat_opt = fill_struct(scat_opt, 'M', 2);
    end

    Wop = cell(1,scat_opt.M);
	for m = 0:scat_opt.M
		filt_ind = min(numel(filters), m+1);
		Wop{m+1} = @(X)(wavelet_layer_1dave(X, filters{filt_ind}, scat_opt));
	end
end


function X = rocholBackSub(ch, Y);

% ROCHOLBACKSUB Backsubstitute the representation of the rank one Cholesky.

% ROCHOL

%/~
% This would be the long way of doing it.
%L = rocholExtract(ch);
%X1 = L'\Y;
%~/
X = zeros(size(Y));
t = zeros(1, size(Y, 2));
X(ch.n, :) = Y(ch.n, :)/ch.s(ch.n);
for i = ch.n-1:-1:1
  t = t + ch.v(i+1)*X(i+1, :);
  X(i, :) = Y(i, :)/ch.s(i) - ch.u(i)*t;
end

%/~
%disp(max(max(X - X1)));
%~/


function visNNLearning(net,fig,transWeights);
%  visNNLearning(net);

layers = net.layers;

if notDefined('fig')
	fig = 99;
end

figure(fig);
clf

nLayers = numel(layers);

if notDefined('transWeights')
	transWeights = zeros(nLayers,1);
end

for iL = 1:nLayers
	subplot(3,nLayers,iL);
	visWeights(layers{iL}.W,transWeights(iL));
	title(sprintf('Layer %i Weights',iL));
	axis square
end

for iL = nLayers+1:2*nLayers
	subplot(3,nLayers,iL);
	visWeights(layers{iL-nLayers}.dW,transWeights(iL-nLayers));
	title(sprintf('Layer %i Gradients',iL-nLayers));
	axis square
end

for iL = 2*nLayers+1:3*nLayers
	subplot(3,nLayers,iL);
	hist(layers{iL-2*nLayers}.output(:),20);
	title(sprintf('Layer %i -- %s output',iL-2*nLayers,layers{iL-2*nLayers}.actFun));
	axis square
end
drawnow


function [elem2edge,edge,elem2edgeSign] = dof3edge(elem)
%% DOF3EDGE dof structure for the lowest order edge elements.
%
% [elem2edge,edge] = DOF3EDGE(elem) constructs data structure
% for the lowest order edge element. elem is the connectivity matrix for a
% 3-D triangulation. elem2edge is the elementwise pointer from elem to dof
% indices. edge is the edge matrix. The orientation of edge is
% from the node with smaller index to bigger one. The orientation of local
% edges of a tetrahedron formed by [1 2 3 4] is lexicographic order:
%           [1 2], [1 3], [1 4], [2 3], [2 4], [3 4]
%
% [elem2edge,edge,elem2edgeSign] = DOF3EDGE(elem) also output elem2edgeSign
% which records the consistency of the local and global edge orientation.
% If elem is ascend ordered, then elem2edgeSign is 1 and do not needed.
%
% See also dof3edge.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

NT = size(elem,1);
totalEdge = int32([elem(:,[1 2]); elem(:,[1 3]); elem(:,[1 4]); ...
                    elem(:,[2 3]); elem(:,[2 4]); elem(:,[3 4])]);
sortedTotalEdge = sort(totalEdge,2);
matlabversion = version;
if str2double(matlabversion(end-5:end-2)) > 2012
    [edge, tempvar, j] = unique(sortedTotalEdge,'rows','legacy'); %#ok<ASGLU>
else
    [edge, tempvar, j] = unique(sortedTotalEdge,'rows'); %#ok<ASGLU>
end
elem2edge = uint32(reshape(j,NT,6));
direction = ones(6*NT,1);
idx = (totalEdge(:,1)>totalEdge(:,2));
direction(idx) = -1;
elem2edgeSign = reshape(direction,NT,6);


function y = vl_nnchannelshuffle(x, group, varargin)
% VL_NNCHANNELSHUFFLE Channel shuffling
%   Y = VL_NNRESHAPE(X, SHAPE) reshpaes the input data X to have
%   the dimensions specified by SHAPE. X is a SINGLE array of
%   dimension H x W x D x N where (H,W) are the height and width of
%   the map stack, D is the image depth (number of feature channels)
%   and N the number of of images in the stack. SHAPE is a 1 x 3 cell
%   array, the contents of which are passed in order to the MATLAB
%   reshape function. As a consequence, `[]` an be used to specify a
%   dimension which should be computed from the other two. The batch size
%   (the fourth dimension of the input) is left unchanged by this
%   reshaping operation.
%
%   Example:
%       Inputs: X with shape [100 100 3 5] and SHAPE = { 100 3 [] }
%       will produce an output Y with shape [100 3 100 5]
%
%   DZDX = VL_NNRESHAPE(X, SHAPE, DZDY) computes the derivatives of the
%   block projected onto DZDY. DZDX and DZDY have the same dimensions
%   as X and Y respectively.
%
%  This operation was originally described in:
%
%  Zhang, X., Zhou, X., Lin, M., & Sun, J. (2017).
%  ShuffleNet: An Extremely Efficient Convolutional Neural Network for
%  Mobile Devices. arXiv preprint arXiv:1707.01083.
%
% Copyright (C) 2017 Samuel Albanie
% Licensed under The MIT License [see LICENSE.md for details]

  [~, dzdy] = vl_argparsepos(struct(), varargin) ;
  keyboard

  batchSize = size(x, 4) ;

  if isempty(dzdy)
    y = reshape(x, shape{1}, shape{2}, shape{3}, batchSize) ;
  else
    y = reshape(dzdy{1}, size(x)) ;
  end


function pass = test_fevalm( pref )
% Test diskfun/fevalm

if ( nargin == 0)
    pref = chebfunpref;
end

tol = 100*pref.cheb2Prefs.chebfun2eps;
rng(2016);

% Check empty diskfun:
f = diskfun;
t = pi*(2*rand(5,1) - 1);
r = 2*rand(5,1)-1;
B = fevalm(f, t, r);
pass(1) = isempty( B );

% Check rank 1 diskfun:
f = diskfun(@(t,r) r.*sin(t), 'polar');
t = pi*(2*rand(5,1) - 1);
r = rand(5,1);
[tt, rr] = meshgrid( t, r);
A = feval(f, tt, rr, 'polar');
B = fevalm(f,t,r);
pass(2) = norm( A - B ) < tol;

% Check essentially one dimensional function:
f = diskfun(@(t,r) exp(-r.^2), 'polar');
t = pi*(2*rand(5,1) - 1);
r = rand(5,1);
[tt, rr] = meshgrid( t, r);
A = feval(f, tt, rr, 'polar');
B = fevalm(f, t, r);
pass(3) = norm( A - B ) < tol;

end


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

%       F. Auger, Dec. 1995 - O. Lemoine, March 1996.

% We test each property of the corresponding TFR :

N=128;

% Covariance by translation in time
t1=55; t2=70; f=0.3;
sig1=amgauss(N,t1).*fmconst(N,f,t1);
sig2=amgauss(N,t2).*fmconst(N,f,t2);
tfr1=tfrspwv(sig1);
tfr2=tfrspwv(sig2);
[tr,tc]=size(tfr1);
nu=round(f*(tc-1)*2)+1;
tfr=tfr1-tfr2(:,modulo((1:tc)-t1+t2,tc));
if any(any(abs(tfr)>sqrt(eps))),
 error('tfrspwv test 1 failed');
end

% Reality of the TFR
sig=noisecg(N);
tfr=tfrspwv(sig);
if sum(any(abs(imag(tfr))>sqrt(eps)))~=0,
 error('tfrspwv test 2 failed');
end

% Energy conservation
sig=noisecg(N);
tfr=tfrspwv(sig,1:N,N,[1]);
Es=norm(sig)^2;
Etfr=sum(mean(tfr));
if abs(Es-Etfr)>sqrt(eps),
 error('tfrspwv test 3 failed');
end

% time localization
t0=30; sig=((1:N)'==t0);
tfr=tfrspwv(sig,1:N,N,[1]);
[ik,jk]=find(tfr~=0.0);
if any(jk~=t0)|any(ik'-(1:N)),
 error('tfrspwv test 4 failed');
end;

% frequency localization
f0=10;
sig=fmconst(N+6,f0/N);
tfr=tfrspwv(sig,N/2+2,N,tftb_window(11,'rect'),tftb_window(N+1,'rect'));
if (find(tfr>1/N)~=2*f0+1)|(abs(mean(tfr)-1.0)>2.0*eps),
 error('tfrspwv test 5 failed');
end;

% A SPWVD with a Dirac time-smoothing window is a PWVD
sig=noisecg(N);
tfr1=tfrspwv(sig,1:N,N,1);
tfr2=tfrpwv(sig);
if any(any(abs(tfr1-tfr2)>sqrt(eps))),
 error('tfrspwv test 6 failed');
end;

N=127;

% Covariance by translation in time
t1=55; t2=70; f=0.3;
sig1=amgauss(N,t1).*fmconst(N,f,t1);
sig2=amgauss(N,t2).*fmconst(N,f,t2);
tfr1=tfrspwv(sig1);
tfr2=tfrspwv(sig2);
[tr,tc]=size(tfr1);
nu=round(f*(tc-1)*2)+1;
tfr=tfr1-tfr2(:,modulo((1:tc)-t1+t2,tc));
if any(any(abs(tfr)>sqrt(eps))),
 error('tfrspwv test 7 failed');
end

% Reality of the TFR
sig=noisecg(N);
tfr=tfrspwv(sig);
if sum(any(abs(imag(tfr))>sqrt(eps)))~=0,
 error('tfrspwv test 8 failed');
end

% Energy conservation
sig=noisecg(N);
tfr=tfrspwv(sig,1:N,N,[1]);
Es=norm(sig)^2;
Etfr=sum(mean(tfr));
if abs(Es-Etfr)>sqrt(eps),
 error('tfrspwv test 9 failed');
end

% time localization
t0=30; sig=((1:N)'==t0);
tfr=tfrspwv(sig,1:N,N,[1]);
[ik,jk]=find(tfr~=0.0);
if any(jk~=t0)|any(ik'-(1:N)),
 error('tfrspwv test 10 failed');
end;

% frequency localization
f0=10;
sig=fmconst(N+6,f0/N);
tfr=tfrspwv(sig,round(N/2)+2,N,tftb_window(11,'rect'),tftb_window(N,'rect'));
if (find(tfr>1/N)~=2*f0+1)|(abs(mean(tfr)-1.0)>sqrt(eps)),
 error('tfrspwv test 11 failed');
end;

% A SPWVD with a Dirac time-smoothing window is a PWVD
sig=noisecg(N);
tfr1=tfrspwv(sig,1:N,N,1);
tfr2=tfrpwv(sig);
if any(any(abs(tfr1-tfr2)>sqrt(eps))),
 error('tfrspwv test 12 failed');
end;


function [idcs, nSmpls] = class_samples( lbls, nActive )

% CLASS_SAMPLES
% COPYRIGHT: Patrick Sauer, 2012
% VARGPLVM

    if size(lbls,1) ~= 1
        lbls = lbls';
    end

    clids  = unique(lbls);
    nEx    = zeros(1, numel(clids));
    nSmpls = zeros(1, numel(clids));
    idcs   = cell( 1, numel(clids));

    count = 1;
    for c = clids
        tmp = find(lbls==c);
        idcs{count} = tmp;
        nEx(count)  = numel(tmp);
        nSmpTmp =  ceil(nEx(count)/length(lbls)*nActive);
        if nSmpTmp > nEx(count)
            nSmpTmp = nEx(count);
        end
        nSmpls(count) = nSmpTmp;

        count = count + 1;
    end

    if sum(nEx) < nActive
       error('There must be at least as many training samples as inducing variables.');
    end

    nRes = nActive - sum(nSmpls);
    if nRes > 0
        while nRes > 0
            [nSmplsSort, ind] = sort(nSmpls,'ascend');
            for count = ind
                if nSmpls(count) < nEx(count)
                    nSmpls(count) = nSmpls(count)+1;
                    nRes = nRes-1;
                end

                if nRes == 0
                    break;
                end
            end
        end
    else
       while nRes < 0
          [mx, ind] = max(nSmpls);
          nSmpls(ind) = nSmpls(ind) - 1;
          nRes = nRes + 1;
       end
    end
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Q=SELECT_CONFIGURATION(ROBOT, Qinv, CONF)
% Returns the joint coordinates Q that comply with the axes configuration
% vector CONF, given a set of solutions of the inverse kinematic problem Qinv.
% For 6DOF or less manipulators, the variable CONF={CF1, CF4, CF6, CFX}
% specifies univoquely only one of the solutions.
%
% See also:
%   COMPUTE_CONFIGURATION, GET_CONF_DATA
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 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=select_configuration(robot, qinv, conf)

q=qinv(:,1); %zeros(robot.DOF,1);

for i=1:size(qinv,2),
    confi=compute_configuration(robot, qinv(:,i))
    if isequal(conf(1:4), confi(1:4))
       %if the same configuration is found, store the joint values and
       %return
       q = qinv(:,i);
       return;
    end
end

disp('ERROR: RAPID/select_configuration: No solutions complies with the exact specified configuration ');

disp('WARNING: Selecting now the closest configuration');

q=select_closest_configuration(robot, qinv, conf);


  function [xs, precon] = eml_osem(x, Gb, yi, ci, ri, varargin)
%|function xs = eml_osem(x, Gb, yi, ci, ri, [options])
%| E-ML-OSEM algorithm for image reconstruction from Poisson emission data
%| (ordered subsets expectation maximization)
%| model: Y_i ~ Poisson(c_i [G x]_i + r_i)
%|
%| in
%|	x	[np 1]		initial estimate
%|	Gb	[nd np]		Gblock object (see eml_osem_test.m)
%|	yi,ci,ri [nb na]	see em_fbp.m (for model too)
%|
%| option
%|	'niter', int		# iterations (default: 1+1)
%|	'isave', []		which iterations to archive (default: 'all')
%|	'pixmax', float		upper constraint for pixel values
%|	'precon [np {1|nblock}]	preconditioners; recommend: 'classic'
%|	'relax0' [1] or [2]	relax0 or (relax0, relax_rate)
%|	userfun @		user-defined function handle
%| out
%|	xs [np length(isave)]	updated image vectors each iteration
%|
%| Copyright 2002-3-14, Jeff Fessler, University of Michigan

if nargin < 3, ir_usage, end

% backward compatability with old usage
% eml_osem(x, Gb, yi, ci, ri, niter, pixmax, precon, relax0)
if length(varargin) && isnumeric(varargin{1})
	[xs, precon] = eml_osem_orig(x, Gb, yi, ci, ri, varargin{:});
return
end

% defaults
arg.niter = 1;
arg.isave = 'all';
arg.pixmax = inf;
arg.chat = false;
arg.relax0 = 1;
arg.relax_rate = 0;
arg.precon = 'classic';
arg.userfun = [];
arg = vararg_pair(arg, varargin);

arg.isave = iter_saver(arg.isave, arg.niter);

Gb = block_ob(Gb, 'ensure'); % make it a block object (if not already)
nblock = block_ob(Gb, 'n');
starts = subset_start(nblock);

if ~isvar('ci') || isempty(ci)
	ci = ones(size(yi));
end
if ~isvar('ri') || isempty(ri)
	ri = zeros(size(yi));
end

if length(arg.relax0) == 1
	arg.relax_rate = 0;
elseif length(arg.relax0) == 2
	arg.relax_rate = arg.relax0(2);
	arg.relax0 = arg.relax0(1);
else
	error relax
end

eml_check(yi, ci, ri, 'os', nblock);
[nb na] = size(yi);

%
% precompute the preconditioners for classic OSEM
%
if streq(arg.precon, 'classic')
	precon = zeros(size(Gb,2), nblock);
	for iset=1:nblock
		ticker([mfilename ' : precon'], iset, nblock)
		istart = starts(iset);
		ia = istart:nblock:na;
		Asum = Gb{istart}' * col(ci(:,ia));
		Asum(Asum == 0) = Inf; % avoid divide by 0
		precon(:, iset) = 1 ./ Asum;
	end, clear Asum

%
% This 'fast' approach uses the same preconditioner for each subset
% which helps convergence (if diminishing relaxation is used).
% Of course, convergence is not really essential in the unregularized case,
% but this saves a bit of memory by using just one preconditioner, which
% ought to work when the subsets are reasonably balanced.
% But, if diminishing relaxation is not used, then this can cause problems
% because pixels can get stuck at zero. So I no longer recommend this.
%
elseif streq(arg.precon, 'fast')
	warning 'Using fast preconditioner rather than classic OSEM'
	precon = Gb' * ci(:); % complete backprojection
	precon(precon == 0) = Inf; % avoid divide by 0
	precon = nblock ./ precon;

else % user-supplied precon
	precon = arg.precon;
	if ncol(precon) ~= nblock && ncol(precon) ~= 1
		error 'precon columns must be 1 or nblock'
	end
end

%
% loop over iterations
%
xs = zeros(numel(x), length(arg.isave));
if any(x <= 0), error 'need x > 0', end
x = min(x, arg.pixmax);
if any(arg.isave == 0)
	xs(:,find(arg.isave == 0)) = x;
end

for iter = 1:arg.niter
	relax = arg.relax0 / (1 + arg.relax_rate * (iter-2));

	%
	% loop over subsets
	%
	for iset=1:nblock
		ticker(mfilename, [iter iset], [arg.niter nblock])

		istart = starts(iset);
		ia = istart:nblock:na;

		% predicted measurements
		yp = reshape(Gb{istart} * x, nb, length(ia));
		yp = ci(:,ia) .* yp + ri(:, ia);
		yp(yp == 0) = inf;	% avoids /0 error

		pre = precon(:,min(iset,ncol(precon)));	% 1 or iset

		dhi = ci(:,ia) .* (yi(:,ia) ./ yp - 1);
		grad = Gb{istart}' * dhi(:);

		x = x + relax * (x .* pre) .* grad;

		% fix: implement ahn's shift trick? no, just use classic.
		nneg = sum(x < 0);
		if nneg, printm('oh no! %d negatives', nneg), minmax(x), end

		x = max(x,0);	% caution: if x becomes zero, it is always zero!
		x = min(x,arg.pixmax);
	end

	if arg.chat, printm('Range %g %g', min(x), max(x)), end
	if any(arg.isave == iter)
		xs(:,find(arg.isave == iter)) = x;
	end

	if ~isempty(arg.userfun)
		feval(arg.userfun);
	end
end

%
% eml_osem_orig()
% arguments: pixmax, precon, relax0
%
function [xs, precon] = eml_osem_orig(x, Gb, yi, ci, ri, niter, varargin)
arg = {'niter', niter};
opt = {'pixmax', 'precon', 'relax0'};
for ii=1:length(varargin)
	arg = {arg{:}, opt{ii}, varargin{ii}};
end
[xs, precon] = eml_osem(x, Gb, yi, ci, ri, arg{:});


function [y, w] = pts_tps_map( s, t, x, w, FAST, PROGRESS )
% PTS_TPS_MAP  Interpolate/warp/map N-dimensional points using a thin-plate
% spline transformation
%
% [Y, W] = PTS_TPS_MAP(S, T, X, W)
%
%    S is a (P,Ds,N)-volume where each (:,:,i)-matrix has the coordinates
%    of the source points that define the warp.
%
%    T is a (P,Dt,N)-volume where each (:,:,i)-matrix has the coordinates
%    of the target points that define the warp.
%
%      (P is the number of points, Ds and Dt are the dimension and N is the
%      number of configurations).
%
%    X is a (P,Ds,N)-volume where each (:,:,i)-matrix has the coordinates
%    of the points to be interpolated.
%
%    Y is a (P,Dt,N)-volume with the interpolated points.
%
%    X can also be a struct with 3 volumes: X.in (P1,Ds,N), X.endo (P2,Ds,N),
%    X.epi (P3,Ds,N). In this case, 3 warps are computed: With all points
%    for X.in; with the first half of S and T points for X.endo; and with
%    the second half of S and T points for X.epi.
%
%    W is a D2-column matrix where each column has the weight and affine
%    weights vector computed with PTS_TSP_WEIGHTS (if W is empty or
%    missing, then W is computed internally).
%
%    If X is a struct, then W has to be a similar struct with the weights
%    for each of the warps.
%
% ... = PTS_TPS_MAP(S, T, X, W, FAST)
%
%    To speed up computations, all points in the same configuration are
%    warped together using vector and matrix operations.
%
%    This is more memory expensive. If for some reason memory becomes a
%    problem, using FAST=false it is possible to run the algorithm using
%    a "for" loop to warp 1 point at a time. This is roughly 14 times
%    slower. By default the faster method is used, FAST=true.
%
% ... = PTS_TPS_MAP(S, T, X, W, FAST, PROGRESS)
%
%    Making PROGRESS=true, and if you have function STATUSBAR() by
%    Leutenegger Marcel, you will see a progress bar with an estimate of
%    the time remaining to completion if you are using the slow method.
%
%
% See also: pts_tps_weights.

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2006-2011 University of Oxford
% Version: 0.6.4
% $Rev: 434 $
% $Date: 2011-06-03 17:16:33 +0100 (Fri, 03 Jun 2011) $
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

% check arguments
error(nargchk(3, 6, nargin));
error(nargoutchk(0, 2, nargout));

% defaults
if (nargin < 5 || isempty(FAST))
    FAST = true;
end
if (nargin < 6 || isempty(PROGRESS))
    PROGRESS = false;
end

% struct and sizes check
P = size(s, 1); % number of control points
if (P ~= size(t, 1))
    error('S and T must have the same number of points')
end

% compute weights if needed
N = size(s, 3); % number of frames
if (nargin < 4 || isempty(w))
    if isstruct(x) % triple warp

        for I = 1:N
            w(I).in = pts_tps_weights(s(:, :, I), t(:, :, I));
            w(I).endo = pts_tps_weights(s(1:(P/2), :, I), ...
                t(1:(P/2), :, I));
            w(I).epi = pts_tps_weights(s((P/2+1):end, :, I), ...
                t((P/2+1):end, :, I));
        end

    else
        w = pts_tps_weights(s, t);
    end
end

% deal with empty point sets
if isempty(x)
    y = [];
    return;
end

if isstruct(x) % triple warp
    if any(~isfield(x, {'endo', 'epi', 'in'}))
        error('Invalid fields for struct in X')
    end

    % compute each warp separately
    y(N) = struct( ...
        'in', zeros(size(x(1).in)), ...
        'endo', zeros(size(x(1).endo)), ...
        'epi', zeros(size(x(1).epi)));
    for I = 1:N
        y(I).in = pts_tps_map(s(:, :, I), t(:, :, I), ...
            x(I).in, w(I).in, FAST);
        y(I).endo = pts_tps_map(s(1:(P/2), :, I), ...
            t(1:(P/2), :, I), x(I).endo, w(I).endo, FAST);
        y(I).epi = pts_tps_map(s((P/2+1):end, :, I), ...
            t((P/2+1):end, :, I), x(I).epi, w(I).epi, FAST);
    end

    return
end

% dimensionality of points
D = size(s, 2);

if (D ~= size(x, 2))
    error(['Dimensionality of S is ' num2str(D) ', of X is ' num2str(size(x, 2))])
end
if (N ~= size(t, 3) || N ~= size(x, 3))
    error('S, T and X must have the same number of point configurations')
end

% init output
y = zeros(size(x, 1), size(w, 2));

PP = size(x, 1); % PP: number of points to be interpolated

if FAST

    % warp every input point
    for I = 1:N % loop frames

        % init aux matrix
        u = zeros(P * PP, D);

        % interleave points to be warped
        for J = 1:D
            u(:, J) = reshape(repmat(x(:, J, I), 1, P)', P * PP, 1);
        end

        % compute norm(Pi - (x,y)).^2
        u = sum((u - repmat(s(:, :, I), PP, 1)) .^ 2, 2);

        % reshape to have 1 row vector per point to be interpolated
        u = reshape(u, P, PP)';

        % thin-plate spline distance function
        % U(r) = r^2 log10(r)
        % note: it's faster to compute the log10 this way than directly
        warning('off', 'MATLAB:log:logOfZero');
        u = 0.5 * u .* log(u) * (1/log(10));
        u(isnan(u)) = 0;
        warning('on', 'MATLAB:log:logOfZero');

        % factor by weights f(x,y) = a1 + ax*x + ay*y + sum(wi*U(|Pi - (x,y)|))
        y(:, :, I) = [u , ones(PP, 1) , x(:, :, I)] * w(:, :, I);

    end

else % loop throug each point; this is slower but less memory consuming

    % show status bar
    if PROGRESS
        delete(statusbar)
        bar = statusbar('Progress bar...');
    else
        bar = [];
    end

    % warp every input point
    for I = 1:N % loop frames
        for J = 1:PP
            % compute norm(Pi - (x,y)).^2
            % U(r) = r^2 log10(r)
            % note: it's faster to compute the log10 this way than directly
            u = sum(...
                (repmat(x(J, :, I), P, 1) - s(:, :, I)) .^ 2, 2)';
            warning('off', 'MATLAB:log:logOfZero');
            u = 0.5 * u .* log(u) * (1/log(10));
            u(isnan(u)) = 0;
            warning('on', 'MATLAB:log:logOfZero');

            % factor by weights f(x,y) = a1 + ax*x + ay*y + sum(wi*U(|Pi - (x,y)|))
            y(J, :, I) = ...
                [u , ...
                1 , ...
                x(J, :, I)] ... % x, y, z, etc
                * w(:, :, I);

            % update progress bar every 10th of the total number of points
            if mod(J, round(PP/10))
                if (PROGRESS && isempty(statusbar(((I-1)*PP+J)/(N*PP), bar)))
                    break
                end
            end

        end

%         % update progress bar every frame
%         if isempty(statusbar(I/N, bar))
%             break
%         end

    end

    % delete progress bar
    delete(bar);

end


function boundedIndices=boundsCheck3D(minCoords,maxCoords,inputCoords)
% function boundedIndices=boundsCheck(minCoords,maxCoords,inputCoords)
% PURPOSE:  returns the indices of those points in inputCoords
% That lie within the bounding box specified by the min and max coordinates
% Works with 3D coordinates.
% AUTHOR : Wade
% DATE : 020801
% Last: $Date: 2007/07/05 19:51:58 $
% Do some size checks
[nInputCoords,nInpDims]=size(inputCoords);
if (nInpDims~=3)
    error('In boundsCheck3D, inputCoords must be n*3');
end
if ((prod(size(minCoords))~=3) | (prod(size(maxCoords))~=3))
 error ('In boundsCheck3D, min and max coords must be 3*1 vectors');
end
% do bounds check
okPoints=(inputCoords(:,1)>minCoords(1)).*(inputCoords(:,2)>minCoords(2)).*(inputCoords(:,3)>minCoords(3));
okPoints=okPoints.*(inputCoords(:,1)<maxCoords(1)).*(inputCoords(:,2)<maxCoords(2)).*(inputCoords(:,3)<maxCoords(3));
boundedIndices=find(okPoints);


function out = numSources(D)
    % D is the diagonal eigen value matrix of the matrix of empirical
    % covariance of the antenna data
    num = NaN;
    for i = size(D, 1):-1:3

        if (D(i-1, i-1)-D(i-2, i-2))/(D(i, i)-D(i-1, i-1))<=0.1
            num = i-1;
        end

    end

    out = num;
end


function t = trace(T)
% compute the traces of a rank 2 tensor
%
% Synatx
%
%   t = trace(T)
%
% Input
%  T - @tensor
%
% Output
%  t - double
%

if T.rank == 2

  t = EinsteinSum(T,[-1 -1]);

elseif T.rank == 4

  id = [1 11 13 21 25 29 31 41 51 53 57 61 69 71 81];

  M = reshape(T.M,81,[]);
  M = M(id,:).' * [1 0.5 0.5 0.5 0.5 0.5 0.5 1  0.5 0.5 0.5 0.5 0.5 0.5 1].';

  t = reshape(M,size(T));

elseif T.rank > 2

  % first dimension -> tensor, second dimension -> multiples of the tensor
  M = reshape(T.M,3^T.rank,[]);

  % id's of the diagonal
  % id = sub2ind([3,3,...,3],[1 2 3],[1 2 3],...,[1 2 3])
  id = 1+3.^(0:(T.rank-1))*repmat(0:2,T.rank,1);

  % sum up diagonal
  M = sum(M(id,:));

  % reshape back
  t = reshape(M,size(T));

else
  error('Trace is only implemented for tensors with rank at least 2')
end


function [rez, X] = splitAllClusters(rez, flag)
% i call this algorithm "bimodal pursuit"
% split clusters if they have bimodal projections
% the strategy is to maximize a bimodality score and find a single vector projection
% that maximizes it. If the distribution along that maximal projection crosses a
% bimodality threshold, then the cluster is split along that direction
% it only uses the PC features for each spike, stored in rez.cProjPC

ops = rez.ops;

wPCA = gather(ops.wPCA); % use PCA projections to reconstruct templates when we do splits

ccsplit = rez.ops.AUCsplit; % this is the threshold for splits, and is one of the main parameters users can change

NchanNear   = min(ops.Nchan, 32);
Nnearest    = min(ops.Nchan, 32);
sigmaMask   = ops.sigmaMask;

ik = 0;
Nfilt = size(rez.W,2);
nsplits= 0;

[iC, mask, C2C] = getClosestChannels(rez, sigmaMask, NchanNear); % determine what channels each template lives on

ops.nt0min = getOr(ops, 'nt0min', 20); % the waveforms must be aligned to this sample

[~, iW] = max(abs(rez.dWU(ops.nt0min, :, :)), [], 2); % find the peak abs channel for each template
iW = squeeze(int32(iW));

isplit = 1:Nfilt; % keep track of original cluster for each cluster. starts with all clusters being their own origin.
dt = 1/1000;
nccg = 0;

while ik<Nfilt
    if rem(ik, 100)==1
      % periodically write updates
       fprintf('Found %d splits, checked %d/%d clusters, nccg %d \n', nsplits, ik, Nfilt, nccg)
    end
    ik = ik+1;

    %
    isp = find(rez.st3(:,2)==ik); % get all spikes from this cluster
    nSpikes = numel(isp);
    if  nSpikes<300
       continue; % do not split if fewer than 300 spikes (we cannot estimate cross-correlograms accurately)
    end

    ss = rez.st3(isp,1)/ops.fs; % convert to seconds

    clp0 = rez.cProjPC(isp, :, :); % get the PC projections for these spikes
    clp0 = gpuArray(clp0(:,:));
    clp = clp0 - mean(clp0,1); % mean center them

    % now use two different ways to initialize the bimodal direction
    % the main script calls this function twice, and does both initializations
    if flag
        [u s v] = svdecon(clp');
        w = u(:,1); % initialize with the top PC
    else
        w = mean(clp0, 1)'; % initialize with the mean of NOT drift-corrected trace
        w = w/sum(w.^2)^.5; % unit-normalize
    end

    % initial projections of waveform PCs onto 1D vector
    x = gather(clp * w);
    s1 = var(x(x>mean(x))); % initialize estimates of variance for the first
    s2 = var(x(x<mean(x))); % and second gaussian in the mixture of 1D gaussians

    mu1 = mean(x(x>mean(x))); % initialize the means as well
    mu2 = mean(x(x<mean(x)));
    p  = mean(x>mean(x)); % and the probability that a spike is assigned to the first Gaussian

    logp = zeros(numel(isp), 2); % initialize matrix of log probabilities that each spike is assigned to the first or second cluster

    % do 50 pursuit iteration
    for k = 1:50
        % for each spike, estimate its probability to come from either Gaussian cluster
        logp(:,1) = -1/2*log(s1) - (x-mu1).^2/(2*s1) + log(p);
        logp(:,2) = -1/2*log(s2) - (x-mu2).^2/(2*s2) + log(1-p);

        lMax = max(logp,[],2);
        logp = logp - lMax; % subtract the max for floating point accuracy
        rs = exp(logp); % exponentiate the probabilities

        pval = log(sum(rs,2)) + lMax; % get the normalizer and add back the max
        logP(k) = mean(pval); % this is the cost function: we can monitor its increase

        rs = rs./sum(rs,2); % normalize so that probabilities sum to 1

        p = mean(rs(:,1)); % mean probability to be assigned to Gaussian 1
        mu1 = (rs(:,1)' * x )/sum(rs(:,1)); % new estimate of mean of cluster 1 (weighted by "responsibilities")
        mu2 = (rs(:,2)' * x )/sum(rs(:,2)); % new estimate of mean of cluster 2 (weighted by "responsibilities")

        s1 = (rs(:,1)' * (x-mu1).^2 )/sum(rs(:,1)); % new estimates of variances
        s2 = (rs(:,2)' * (x-mu2).^2 )/sum(rs(:,2));

        if (k>10 && rem(k,2)==1)
            % starting at iteration 10, we start re-estimating the pursuit direction
            % that is, given the Gaussian cluster assignments, and the mean and variances,
            % we re-estimate w
            StS  = clp' * (clp .* (rs(:,1)/s1 + rs(:,2)/s2))/nSpikes; % these equations follow from the model
            StMu = clp' * (rs(:,1)*mu1/s1 + rs(:,2)*mu2/s2)/nSpikes;

            w = StMu'/StS; % this is the new estimate of the best pursuit direection
            w = normc(w'); % which we unit normalize
            x = gather(clp * w);  % the new projections of the data onto this direction
        end
    end

    ilow = rs(:,1)>rs(:,2); % these spikes are assigned to cluster 1
    %    ps = mean(rs(:,1));
    plow = mean(rs(ilow,1)); % the mean probability of spikes assigned to cluster 1
    phigh = mean(rs(~ilow,2)); % same for cluster 2
    nremove = min(mean(ilow), mean(~ilow)); % the smallest cluster has this proportion of all spikes

    % did this split fix the autocorrelograms?
    [K, Qi, Q00, Q01, rir] = ccg(ss(ilow), ss(~ilow), 500, dt); % compute the cross-correlogram between spikes in the putative new clusters
    Q12 = min(Qi/max(Q00, Q01)); % refractoriness metric 1
    R = min(rir);                % refractoriness metric 2

    % if the CCG has a dip, don't do the split.
    % These thresholds are consistent with the ones from merges.
    if Q12<.25 && R<.05 % if both metrics are below threshold.
        nccg = nccg+1; % keep track of how many splits were voided by the CCG criterion
        continue;
    end

    % now decide if the split would result in waveforms that are too similar
    c1  = wPCA * reshape(mean(clp0(ilow,:),1), 3, []); %  the reconstructed mean waveforms for putatiev cluster 1
    c2  = wPCA * reshape(mean(clp0(~ilow,:),1), 3, []); %  the reconstructed mean waveforms for putative cluster 2
    cc = corrcoef(c1, c2); % correlation of mean waveforms
    n1 =sqrt(sum(c1(:).^2)); % the amplitude estimate 1
    n2 =sqrt(sum(c2(:).^2)); % the amplitude estimate 2

    r0 = 2*abs(n1 - n2)/(n1 + n2); % similarity of amplitudes

    % if the templates are correlated, and their amplitudes are similar, stop the split!!!
    if cc(1,2)>.9 && r0<.2
        continue;
    end

    % finaly criteria to continue with the split: if the split piece is more than 5% of all spikes,
    % if the split piece is more than 300 spikes, and if the confidences for assigning spikes to
    % both clusters exceeds a preset criterion ccsplit
    if nremove > .05 && min(plow,phigh)>ccsplit && min(sum(ilow), sum(~ilow))>300
       % one cluster stays, one goes
       Nfilt = Nfilt + 1;

       % the templates for the splits have been estimated from PC coefficients
       rez.dWU(:,iC(:, iW(ik)),Nfilt) = c2;
       rez.dWU(:,iC(:, iW(ik)),ik)    = c1;

       % the temporal components are therefore just the PC waveforms
       rez.W(:,Nfilt,:) = permute(wPCA, [1 3 2]);
       iW(Nfilt) = iW(ik); % copy the best channel from the original template
       isplit(Nfilt) = isplit(ik); % copy the provenance index to keep track of splits

       rez.st3(isp(ilow), 2)    = Nfilt; % overwrite spike indices with the new index
       rez.simScore(:, Nfilt)   = rez.simScore(:, ik); % copy similarity scores from the original
       rez.simScore(Nfilt, :)   = rez.simScore(ik, :); % copy similarity scores from the original
       rez.simScore(ik, Nfilt) = 1; % set the similarity with original to 1
       rez.simScore(Nfilt, ik) = 1; % set the similarity with original to 1

       rez.iNeigh(:, Nfilt)     = rez.iNeigh(:, ik); % copy neighbor template list from the original
       rez.iNeighPC(:, Nfilt)     = rez.iNeighPC(:, ik); % copy neighbor channel list from the original

       % try this cluster again
       ik = ik-1; % the cluster piece that stays at this index needs to be tested for splits again before proceeding
       % the piece that became a new cluster will be tested again when we get to the end of the list
       nsplits = nsplits + 1; % keep track of how many splits we did

    end
end

fprintf('Finished splitting. Found %d splits, checked %d/%d clusters, nccg %d \n', nsplits, ik, Nfilt, nccg)

Nfilt = size(rez.W,2); % new number of templates
Nrank = 3;
Nchan = ops.Nchan;
Params     = double([0 Nfilt 0 0 size(rez.W,1) Nnearest ...
    Nrank 0 0 Nchan NchanNear ops.nt0min 0]); % make a new Params to pass on parameters to CUDA

% we need to re-estimate the spatial profiles
[Ka, Kb] = getKernels(ops, 10, 1); % we get the time upsampling kernels again
[rez.W, rez.U, rez.mu] = mexSVDsmall2(Params, rez.dWU, rez.W, iC-1, iW-1, Ka, Kb); % we run SVD

[WtW, iList] = getMeWtW(single(rez.W), single(rez.U), Nnearest); % we re-compute similarity scores between templates
rez.iList = iList; % over-write the list of nearest templates

isplit = rez.simScore==1; % overwrite the similarity scores of clusters with same parent
rez.simScore = gather(max(WtW, [], 3));
rez.simScore(isplit) = 1; % 1 means they come from the same parent

rez.iNeigh   = gather(iList(:, 1:Nfilt)); % get the new neighbor templates
rez.iNeighPC    = gather(iC(:, iW(1:Nfilt))); % get the new neighbor channels

rez.isplit = isplit; % keep track of origins for each cluster

% figure(1)
% subplot(1,4,1)
% plot(logP(1:k))
%
% subplot(1,4,2)
% [~, isort] = sort(x);
% epval = exp(pval);
% epval = epval/sum(epval);
% plot(x(isort), epval(isort))
%
% subplot(1,4,3)
% ts = linspace(min(x), max(x), 200);
% xbin = hist(x, ts);
% xbin = xbin/sum(xbin);
%
% plot(ts, xbin)
%
% figure(2)
% plotmatrix(v(:,1:4), '.')
%
% drawnow
%
% % compute scores for splits
% ilow = rs(:,1)>rs(:,2);
% ps = mean(rs(:,1));
% [mean(rs(ilow,1)) mean(rs(~ilow,2)) max(ps, 1-ps) min(mean(ilow), mean(~ilow))]


function [S] = spm_mci_random (mcmc,R,v,M,U,Y)
% Random effects estimation
% FORMAT [S] = spm_mci_random (mcmc,R,v,M,U,Y)
%
% mcmc  Sampling parameters
% R     Priors on random effects (R.pE, R.pC)
% v     Fixed effects
% M     Model Structure (single subject)
% U     Inputs (single subject)
% Y     Data (single subject)
%
% S     Samples, [maxits x M.n]
%
% Uses Langevin Monte Carlo
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_mci_random.m 6697 2016-01-27 14:57:28Z spm $

% Data precision

try, verbose=mcmc.verbose; catch, verbose=0; end
try, maxits=mcmc.maxits; catch, maxits=64; end
try, plot_int=mcmc.plot_int; catch, plot_int=1; end
try, h=mcmc.h; catch, h=0.5; end

% Assign init/flow/out params as fixed/random effects
try, assign=mcmc.assign; catch, assign=[]; end

% Compute eigen-parameterisation
M = spm_mci_minit (M);

try, init=mcmc.init; catch, init=R.pE; end
% Initial param in eigenspace
xinit = init;

% Read data points and time indices
try, ind=Y.ind; catch, ind=1:M.N; end
Nt=length(ind);
y=Y.y;

% Sample matrix
Nx=size(R.pE,1);
x = zeros(maxits,Nx);
x(1,:) = xinit';

ipC=pinv(R.pC);
logdet_RCp=spm_logdet(R.pC);

if verbose, figure; end

% Tune h by monitoring acceptance rate
tune_h=1;
acc_block=64;
acc_low=0.3;
acc_high=0.7;
total_acc_target=64; % Number of accepted samples to get
acc=zeros(maxits,1);

i=1;
while (i <= maxits),

    if verbose
        if mod(i,plot_int) == 0 && i > 2
            spm_mci_progress (x,E,i);
        end
    end

    if mod(i,acc_block)==0 && tune_h
        % Change step size h ?
        Nacc=sum(acc(i-acc_block+1:i-1));
        prop_acc=Nacc/acc_block;
        if prop_acc < acc_low
            if verbose, disp('Decreasing step size ...'); end
            h=h/2;
        elseif prop_acc > acc_high
            if verbose, disp('Increasing step size ...'); end
            h=h*2;
        end
    end

    % Proposal (first proposal always accepted)
    if i==1
        pos=x(1,:)';
        curr=[];
    else
        pos=spm_normrnd(curr.mu,curr.Cp,1);
    end

    % Quantities re proposal
    prop.pos=pos;
    if isfield(M,'IS')
        % Other model types
        [j,iCpY,st,L] = spm_mci_joint_grad (pos,M,U,Y);
    else
        % Differential equation models
        [dLdp,iCpY,st] = spm_mci_grad_curve (assign,pos,v,M,U,Y,'random');
        % Gradient of log prior
        ep = pos-R.pE;
        dlogprior=-ep'*ipC;
        j=dLdp+dlogprior;

        [p_init,p_flow] = spm_mci_init_flow (assign,pos,v,M);
        log_like = spm_mci_like_ind (p_flow,p_init,M,U,Y);
        log_prior = -0.5*ep'*ipC*ep + logdet_RCp-0.5*M.Np*log(2*pi);
        L=log_like+log_prior;
    end

    if st==-1
        error('Integration problem in spm_mci_random.m');
    end
    prop.L = L;

    % Posterior covariance under local linear approximation
    prop.Cp = h*inv(iCpY+ipC);
    prop.mu = pos+0.5*h*prop.Cp*j(:);

    prop.iCp = inv(prop.Cp);
    prop.logdetCp = spm_logdet(prop.Cp);

    % Accept proposal ?
    [curr,accepted,bayes_fb(i),dL(i)] = spm_mci_mh_update(curr,prop,verbose);
    acc(i)=accepted;
    E(i) = -curr.L;
    if i > 1
        dEdit(i-1)=100*(E(i)-E(i-1))/E(i-1);
    end
    x(i,:)=curr.pos;

    i=i+1;
end

S=x(1:i-1,:)';


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

% used market data
load 'MODEL_Hist';
% load 'MODEL_Hist_new';

% Discretization parameters
NTime = 12;                    % number of time steps
NSim = 100;                  % number of simulations
NBatches = 1;                  % number of batches

NHist = 251;                     % number of historic scenarios

NModels = 10;                  % number of models

% Options
%K = 6800;                              % Strike
%C = 1;                                 % call (1) or put (0)
LF = 0; LC = 0.04; GF = 0; GC = 0.15;   % cliquet params
U = Spot * 0.9;
L = Spot * 1.1;
T= 1;                                   % maturity of option

% Comment in the option you wish to price
option1 = 'Arithmetic Asian Call';
option2 = 'Arithmetic Asian Put';
exotic_price_c = @(x,y,r) Price_ArithmeticAsian(x,y,1,r,T);
exotic_price_p = @(x,y,r) Price_ArithmeticAsian(x,y,0,r,T);

%option1 = 'FixedStrike Lookback Call';
%option2 = 'FixedStrike Lookback Put';
%exotic_price_c = @(x,y,r) Price_LookBackFixedStrike(x,y,1,r,T);
%exotic_price_p = @(x,y,r) Price_LookBackFixedStrike(x,y,0,r,T);

%option1 = 'FloatingStrike Lookback Call';
%option2 = 'FloatingStrike Lookback Put';
%exotic_price_c = @(x,y,r) Price_LookBackFloatingStrike(x,y,1,r,T);
%exotic_price_p = @(x,y,r) Price_LookBackFloatingStrike(x,y,0,r,T);

% option1 = 'Arithmetic Cliquet';
% exotic_price_c = @(x,r) Price_ArithmeticCliquet(x,LF,LC,GF,GC,r,T);

% option1 = 'Knock-Out Call';
% option2 = 'Knock-Out Put';
% exotic_price_c = @(x,y,r) Price_KnockOut(x,x(1,1),y,1,r,T);
% exotic_price_p = @(x,y,r) Price_KnockOut(x,x(1,1),y,0,r,T);

vanilla_price_c = @(x,y,r) Price_CallPut(x,y,1,r,T);
vanilla_price_p = @(x,y,r) Price_CallPut(x,y,0,r,T);

% output
ep1c = zeros(NHist,NModels); %ep2c = ep1c; ep3c = ep1c;
ep1p = ep1c; %ep2p = ep1c; ep3p = ep1c;

vp_c = ep1c; vp_p = ep1c;
qp_c = ep1c; qp_p = ep1c;

vc = zeros(NHist,1); vp = vc;

% Black Scholes
for l = 1: NHist
    pathS = MC_B(Spot(l),r1y(l),0,T,BS_Par_Hist(l),NTime,NSim,NBatches);
    ep1c(l,1) = exotic_price_c(pathS,Spot(l),r1y(l));
    ep1p(l,1) = exotic_price_p(pathS,Spot(l),r1y(l));
    vp_c(l,1) = vanilla_price_c(pathS,Spot(l),r1y(l));
    vp_p(l,1) = vanilla_price_p(pathS,Spot(l),r1y(l));
    [vc(l), vp(l)] = blsprice(Spot(l), Spot(l), r1y(l), T, BS_Par_Hist(l),0);
    qp_c(l,1) = ep1c(l,1)/vp_c(l,1);
    qp_p(l,1) = ep1p(l,1)/vp_p(l,1);
end

% Merton
for l = 1: NHist
    pathS = MC_M(Spot(l),r1y(l),0,T,...
        MJ_Par_Hist(l,1),MJ_Par_Hist(l,2),MJ_Par_Hist(l,3),MJ_Par_Hist(l,4),...
        NTime,NSim,NBatches);
    ep1c(l,2) = exotic_price_c(pathS,Spot(l),r1y(l));
    ep1p(l,2) = exotic_price_p(pathS,Spot(l),r1y(l));
    vp_c(l,2) = vanilla_price_c(pathS,Spot(l),r1y(l));
    vp_p(l,2) = vanilla_price_p(pathS,Spot(l),r1y(l));
    qp_c(l,2) = ep1c(l,2)/vp_c(l,2);
    qp_p(l,2) = ep1p(l,2)/vp_p(l,2);
end

% Heston
for l = 1: NHist
    [pathS, pathV] = MC_QE(Spot(l),r1y(l),0,T,...
        HESTON_Par_Hist(l,1),HESTON_Par_Hist(l,2),HESTON_Par_Hist(l,4),...
        HESTON_Par_Hist(l,5),HESTON_Par_Hist(l,3),...
        NTime,NSim,NBatches);
    ep1c(l,3) = exotic_price_c(pathS,Spot(l),r1y(l));
    ep1p(l,3) = exotic_price_p(pathS,Spot(l),r1y(l));
    vp_c(l,3) = vanilla_price_c(pathS,Spot(l),r1y(l));
    vp_p(l,3) = vanilla_price_p(pathS,Spot(l),r1y(l));
    qp_c(l,3) = ep1c(l,3)/vp_c(l,3);
    qp_p(l,3) = ep1p(l,3)/vp_p(l,3);
end

% Bates
for l = 1: NHist
    [pathS, pathV] = MC_QE_j(Spot(l),r1y(l),0,T,...
        BATES_Par_Hist(l,1),BATES_Par_Hist(l,2),BATES_Par_Hist(l,4),BATES_Par_Hist(l,5),...
        BATES_Par_Hist(l,3),BATES_Par_Hist(l,7),BATES_Par_Hist(l,7),BATES_Par_Hist(l,8),...
        NTime,NSim,NBatches);
    ep1c(l,4) = exotic_price_c(pathS,Spot(l),r1y(l));
    ep1p(l,4) = exotic_price_p(pathS,Spot(l),r1y(l));
    vp_c(l,4) = vanilla_price_c(pathS,Spot(l),r1y(l));
    vp_p(l,4) = vanilla_price_p(pathS,Spot(l),r1y(l));
    qp_c(l,4) = ep1c(l,4)/vp_c(l,4);
    qp_p(l,4) = ep1p(l,4)/vp_p(l,4);
end

% Variance Gamma CGM
for l = 1:NHist
    pathS  = MC_VG_CGM(Spot(l),r1y(l),0,T,...
        VG_Par_Hist(l,1),VG_Par_Hist(l,2),VG_Par_Hist(l,3),...
        NTime,NSim,NBatches);
    ep1c(l,5) = exotic_price_c(pathS,Spot(l),r1y(l));
    ep1p(l,5) = exotic_price_p(pathS,Spot(l),r1y(l));
    vp_c(l,5) = vanilla_price_c(pathS,Spot(l),r1y(l));
    vp_p(l,5) = vanilla_price_p(pathS,Spot(l),r1y(l));
    qp_c(l,5) = ep1c(l,5)/vp_c(l,1);
    qp_p(l,5) = ep1p(l,5)/vp_p(l,1);
end

% Normal Inverse Gaussian
for l = 1:NHist
    pathS  = MC_NIG(Spot(l),r1y(l),0,T,...
        NIG_Par_Hist(l,1),NIG_Par_Hist(l,2),NIG_Par_Hist(l,3),...
        NTime,NSim,NBatches);
    ep1c(l,6) = exotic_price_c(pathS,Spot(l),r1y(l));
    ep1p(l,6) = exotic_price_p(pathS,Spot(l),r1y(l));
    vp_c(l,6) = vanilla_price_c(pathS,Spot(l),r1y(l));
    vp_p(l,6) = vanilla_price_p(pathS,Spot(l),r1y(l));
    qp_c(l,6) = ep1c(l,1)/vp_c(l,1);
    qp_p(l,6) = ep1p(l,6)/vp_p(l,1);
end

% Variance Gamma - GOU
for l = 1:NHist
    pathS  = MC_VGGOU(Spot(l),r1y(l),0,T,...
        VGGOU_Par_Hist(l,1), VGGOU_Par_Hist(l,2), VGGOU_Par_Hist(l,3),...
        VGGOU_Par_Hist(l,6), VGGOU_Par_Hist(l,4), VGGOU_Par_Hist(l,5),...
        NTime,NSim,NBatches);
    ep1c(l,7) = exotic_price_c(pathS,Spot(l),r1y(l));
    ep1p(l,7) = exotic_price_p(pathS,Spot(l),r1y(l));
    vp_c(l,1) = vanilla_price_c(pathS,Spot(l),r1y(l));
    vp_p(l,7) = vanilla_price_p(pathS,Spot(l),r1y(l));
    qp_c(l,7) = ep1c(l,7)/vp_c(l,1);
    qp_p(l,7) = ep1p(l,7)/vp_p(l,1);
end

% % Variance Gamma - CIR
for l = 1: NHist
    pathS =  MC_VGCIR(Spot(l),r1y(l),0,T,...
        VGCIR_Par_Hist(l,1),VGCIR_Par_Hist(l,2),VGCIR_Par_Hist(l,3), ...
        VGCIR_Par_Hist(l,4),VGCIR_Par_Hist(l,5),VGCIR_Par_Hist(l,6),...
        NTime,NSim,NBatches);
    ep1c(l,8) = exotic_price_c(pathS,Spot(l),r1y(l));
    ep1p(l,8) = exotic_price_p(pathS,Spot(l),r1y(l));
    vp_c(l,8) = vanilla_price_c(pathS,Spot(l),r1y(l));
    vp_p(l,8) = vanilla_price_p(pathS,Spot(l),r1y(l));
    qp_c(l,8) = ep1c(l,8)/vp_c(l,1);
    qp_p(l,8) = ep1p(l,8)/vp_p(l,1);
end

% NIG - GOU
for l = 1: NHist
    pathS =  MC_NIGGOU(Spot(l),r1y(l),0,T,...
        NIGGOU_Par_Hist(l,1),NIGGOU_Par_Hist(l,2),NIGGOU_Par_Hist(l,3), ...
        NIGGOU_Par_Hist(l,4),NIGGOU_Par_Hist(l,5),NIGGOU_Par_Hist(l,6),NTime,NSim,NBatches);
    ep1c(l,9) = exotic_price_c(pathS,Spot(l),r1y(l));
    ep1p(l,9) = exotic_price_p(pathS,Spot(l),r1y(l));
    vp_c(l,9) = vanilla_price_c(pathS,Spot(l),r1y(l));
    vp_p(l,9) = vanilla_price_p(pathS,Spot(l),r1y(l));
    qp_c(l,9) = ep1c(l,9)/vp_c(l,1);
    qp_p(l,9) = ep1p(l,9)/vp_p(l,1);
end

% NIG - CIR
for l = 1: NHist
    pathS =  MC_NIGCIR(Spot(l),r1y(l),0,T,...
        NIGCIR_Par_Hist(l,1),NIGCIR_Par_Hist(l,2),NIGCIR_Par_Hist(l,3), ...
        NIGCIR_Par_Hist(l,5),NIGCIR_Par_Hist(l,6),NIGCIR_Par_Hist(l,4),NTime,NSim,NBatches);
    ep1c(l,10) = exotic_price_c(pathS,Spot(l),r1y(l));
    ep1p(l,10) = exotic_price_p(pathS,Spot(l),r1y(l));
    vp_c(l,10) = vanilla_price_c(pathS,Spot(l),r1y(l));
    vp_p(l,10) = vanilla_price_p(pathS,Spot(l),r1y(l));
    qp_c(l,10) = ep1c(l,10)/vp_c(l,1);
    qp_p(l,10) = ep1p(l,10)/vp_p(l,1);
end
m1 = 'BS'; m2 = 'MJ'; m3 = 'Heston'; m4 = 'Bates';
m5 = 'VG'; m6 = 'NIG'; m7 = 'VGGOU'; m8 = 'VGCIR';
m9 = 'NIGGOU'; m10 =  'NIGCIR';

% plot the results
plotit(ep1c,option1,m1, m2, m3, m4, m5, m6 ,m7, m8, m9, m10);
plotit(ep1p,option2,m1, m2, m3, m4, m5, m6 ,m7, m8, m9, m10);
plotit(qp_c,strcat('Normalized ',option1),m1, m2, m3, m4, m5, m6 ,m7, m8, m9, m10);
plotit(qp_p,strcat('Normalized ',option2),m1, m2, m3, m4, m5, m6 ,m7, m8, m9, m10);

modelriskc = repmat(max(ep1c,[],2),1,NModels)-ep1c;
modelriskp = -repmat(max(ep1p,[],2),1,NModels)+ep1p;


function marginal = marginal_nodes(engine, query)
% MARGINAL_NODES Compute the marginal on the specified query nodes (gaussian)
% marginal = marginal_nodes(engine, query)

% Compute sum_{Hsum} Pr(Hkeep, Hsum | o)
H = engine.hnodes;
bnet = bnet_from_engine(engine);
ns = bnet.node_sizes;
Hkeep = myintersect(H, query);
Hsum = mysetdiff(H, Hkeep);

[marginal.mu, marginal.Sigma] = marginalize_gaussian(engine.Hmu, engine.HSigma, Hkeep, Hsum, ns);
marginal.domain = query;
marginal.T = 1;


classdef ShiftingFunctionComputer < handle

    properties (Access = private)
        LHS
        RHS
    end

    properties (Access = private)
        meshDisc
        mesh
        corrector
        interpolator
    end

    methods (Access = public)

        function obj = ShiftingFunctionComputer(cParams)
            obj.init(cParams);
        end

        function sF = compute(obj)
            obj.computeLHS();
            obj.computeRHS();
            uC = obj.solveSystem();
            In = obj.interpolator;
            u  = In*uC;
            u = reshape(u,obj.mesh.nnodeElem,[]);
            s.mesh = obj.mesh;
            s.fValues(1,:,:) = u;
            sF = P1DiscontinuousFunction(s);
        end

    end

    methods (Access = private)

        function init(obj,cParams)
            obj.mesh     = cParams.mesh;
            obj.corrector    = cParams.corrector;
            obj.interpolator = cParams.interpolator;
            obj.meshDisc     = obj.mesh.createDiscontinuousMesh();
        end

        function computeLHS(obj)
            K = obj.computeStiffnessMatrix();
            In = obj.interpolator;
            K = In'*K*In;
            obj.LHS = K;
        end

        function K = computeStiffnessMatrix(obj)
            s.mesh = obj.mesh;
            s.type = 'StiffnessMatrix';
            s.fun  = P1DiscontinuousFunction.create(obj.mesh, 1);
            lhs = LHSintegrator.create(s);
            K = lhs.compute();
        end

        function computeRHS(obj)
            q = Quadrature.set(obj.mesh.type);
            q.computeQuadrature('QUADRATIC');
            cG = obj.corrector.computeGradient(q);

            s.mesh = obj.meshDisc;
            s.type = 'ShapeDerivative';
            s.quadratureOrder = 'QUADRATIC';
            rhs  = RHSintegrator.create(s);
            rhsF = rhs.compute(cG);
            In = obj.interpolator;
            rhsV = In'*rhsF.fValues;
            obj.RHS = rhsV;
        end

        function u = solveSystem(obj)
            a.type = 'DIRECT';
            s = Solver.create(a);
            u = s.solve(obj.LHS,obj.RHS);
            u = u(1:end);
        end

    end

end


function [TrainData, TestData, TrainLabel, TestLabel, numX] = createData()
    ncFeature = 36;  % features of color
    ntFeature = 51;  % features of text

    % load source domain 1
    A = textread(strcat('data/img/11_color36.txt'));
    B = textread(strcat('data/img/11_texture51.txt'));
    n1 = size(A,1)/ncFeature;
    for i = 1:n1
        Cc(:,i) = A((ncFeature*(i-1)+1):ncFeature*i,1);
        D(:,i) = B((ntFeature*(i-1)+1):ntFeature*i,1);
        TrainY1(1,i) = 1;
    end
    A = textread(strcat('data/img/15_color36.txt'));
    B = textread(strcat('data/img/15_texture51.txt'));
    n2 = size(A,1)/ncFeature;
    for i = 1:n2
        Cc(:,i+n1) = A((ncFeature*(i-1)+1):ncFeature*i,1);
        D(:,i+n1) = B((ntFeature*(i-1)+1):ntFeature*i,1);
        TrainY1(1,i+n1) = 2;
    end
    numX = [n1,n2];
    TrainX1 = [Cc;D];
    clear Cc;
    clear D;
    % load target domain
    A = textread(strcat('data/img/12_color36.txt'));
    B = textread(strcat('data/img/12_texture51.txt'));
    n1 = size(A,1)/ncFeature;
    for i = 1:n1
        Cc(:,i) = A((ncFeature*(i-1)+1):ncFeature*i,1);
        D(:,i) = B((ntFeature*(i-1)+1):ntFeature*i,1);
        TestY(1,i) = 1;
    end
    A = textread(strcat('data/img/16_color36.txt'));
    B = textread(strcat('data/img/16_texture51.txt'));
    n2 = size(A,1)/ncFeature;
    for i = 1:n2
        Cc(:,i+n1) = A((ncFeature*(i-1)+1):ncFeature*i,1);
        D(:,i+n1) = B((ntFeature*(i-1)+1):ntFeature*i,1);
        TestY(1,i+n1) = 2;
    end
    TestX = [Cc;D];

    clear A B Cc D;

    %% normalization
    TrainData = TrainX1;
    TrainLabel = TrainY1;
    TestData = TestX;
    TestLabel = TestY;
    column = size(TrainData,2);
    mode_TrainX = sqrt(sum(TrainData.*TrainData,1));
    for i = 1 : column
        TrainData(:,i) = TrainData(:,i)/mode_TrainX(1,i);
    end
    column = size(TestData,2);
    mode_TestX = sqrt(sum(TestData.*TestData,1));
    for i = 1 : column
        TestData(:,i) = TestData(:,i)/mode_TestX(1,i);
    end
    save TrainData.mat TrainData
    save TrainLabel.mat TrainLabel
    save TestData.mat TestData
    save TestLabel.mat TestLabel
    clear TrainX1 TrainY1 TestX TestY
end


function [dat, w] = ft_preproc_denoise(dat, refdat, hilbertflag)

% FT_PREPROC_DENOISE performs a regression of the matrix dat onto
% refdat, and subtracts the projected data. This is for the
% purpose of removing signals generated by coils during continuous
% head motion tracking, for example.
%
% Use as
%   [dat] = ft_preproc_denoise(dat, refdat, hilbertflag)
% where
%   dat         data matrix (Nchan1 X Ntime)
%   refdat      data matrix (Nchan2 X Ntime)
%   hilbertflag specifying to regress out the real and imaginary parts of
%                 the hilbert transformed signal. Only meaningful for narrow
%                 band reference data
%
% See also PREPROC

% Copyright (C) 2009, Jan-Mathijs Schoffelen
%
% 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$

if nargin<3
  hilbertflag = 0;
end

% preprocessing fails on channels that contain NaN
if any(isnan(dat(:))) || any(isnan(refdat(:)))
  ft_warning('FieldTrip:dataContainsNaN', 'data contains NaN values');
end

n1 = size(dat,2);
n2 = size(refdat,2);
m1 = mean(dat,2);
m2 = mean(refdat,2);

%remove mean
refdat  = refdat-m2(:,ones(n2,1));
tmpdat  = dat-m1(:,ones(n1,1));

%do hilbert transformation
if hilbertflag>0
  hrefdat = hilbert(refdat')';
  refdat  = [real(hrefdat);imag(hrefdat)];
end

c12 = tmpdat*refdat'; %covariance between signals and references
c1  = refdat*refdat'; %covariance between references and references
w   = (pinv(c1)*c12')'; %regression weights

%subtract
dat = dat-w*refdat;


function node_h = h_coef ( node_num, node_xy )

%*****************************************************************************80
%
%% H_COEF evaluates the coefficient H(X,Y) of DEL U in the Poisson equation.
%
%  Discussion:
%
%    The equation is
%
%      - Del H(X,Y) DEL U + K(X,Y) * U = RHS(X,Y)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 December 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, real NODE_XY(2,NODE_NUM),
%    the coordinates of the points.
%
%    Output, real NODE_H(NODE_NUM,1),
%    the value of the coefficient of DEL U .
%
  node_h(1:node_num,1) = 1.0;

  return
end


function showgraphmatrix(G,node)
%% SHOWGRAPHMATRIX displays a planar graph
%
%    showgraphmatrix(G,node) displays a planar undirected graph represented
%    by a sparse matrix G.
%
%   See also showgraph, showmesh, findedge
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

[i,j] = find(triu(G,1));
showgraph(node,[i j]);


function f = cart2pol(f, varargin)
%CART2POL    Transforms a DISKFUN F(x,y) in Cartesian coordinates to a
%   CHEBFUN2 F(th, r) in polar coordinates, with -pi <= th <= pi, and 0 <=
%   r <=1. Working with F in this setting does not ensure that F is smooth
%   at the origin of the disk (i.e. r=0), and can reduce the
%   accuracy of some computations. It may be better to use the 'polar' flag
%   in DISKFUN, or to work directly with the CDR decomposition of F.
%
%   F = cart2pol(f, 'cdr') returns F(th, r) with -pi <= th <= pi and
%   -1 <= r <= 1. This is equivalent to the CDR decomposition of F.
%
% See also CDR.

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

% Use the CDR to build a chebfun2 with trigfuns for rows and chebfuns for
% columns

[c, d, r] = cdr(f);

% Restrict columns down to r in [0 , 1]:
if nargin < 2
    c = restrict(c, [0 1]);
end

% Form a chebfun2
f = c*d*r.';

end


function [ EEGbase ] = eeg_baseline(EEGdata,baseline_samples)

% eeg_baseline - remove mean of baseline period from all EEG epochs
%
% [ EEGbase ] = eeg_baseline(EEGdata,baseline_samples)
%
% It is assumed that EEGdata is an MxNxP matrix, with M data samples (ie,
% time), N channels and P epochs (if P=1, EEGdata is MxN).
%
% baseline_samples is an array of data sample indices (of M) that define
% the baseline period.  If baseline_samples is a scalar, the function
% assumes the baseline index array is 1:baseline_samples.
%
% The function calculates the baseline mean for each epoch as follows:
%
% baseline_mean = mean(EEGdata(baseline_samples,:,epoch));
%

% Copyright (C) 2004  Darren L. Weber
%
% This program is free software; you can redistribute it and/or
% modify it under the terms of the GNU General Public License
% as published by the Free Software Foundation; either version 2
% of the License, or (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

% $Revision: 1.1 $ $Date: 2009-04-28 22:13:50 $
% Created:  10/2004, copyright 2004 Darren.Weber_at_radiology.ucsf.edu
% Modified: 05/2005, Darren.Weber_at_radiology.ucsf.edu
%                    baseline_samples can now be an array or a scalar input
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

version = '[$Revision: 1.1 $]';
fprintf('\nEEG_BASELINE [v%s]\n',version(12:16)); tic

if ~exist('baseline_samples','var'),
    error('no baseline_samples specified');
end
if isempty(baseline_samples),
    error('no baseline_samples specified');
end
if length(baseline_samples) == 1,
    baseline_samples = 1:baseline_samples;
end

number_samples = size(EEGdata,1);

for epoch = 1:size(EEGdata,3),

    baseline_mean = mean(EEGdata(baseline_samples,:,epoch));

    baseline_mean = repmat(baseline_mean,number_samples,1);

    EEGbase(:,:,epoch) = EEGdata(:,:,epoch) - baseline_mean;

end

t = toc; fprintf('...done (%6.2f sec).\n\n',t);
return


%--------------------------------------------------------------------------
%----------------------  Validation of the result -------------------------
%--------------------------------------------------------------------------

% With this function you can re-estimated the CCR for the optimal Feature
% Subset found by a Feature Selection method

 function [ResultMat, UpLimitCOD,  LowLimitCOD] = ...
                    CCRForOptSet(ResultMat, DatasetToUse, ErrorEstMethod)
 format short g
 axis([0 length(ResultMat(:,1)) 0 1])
 axis manual
%----------- Feed from Feature Selection method----------------------------
%Cross-val
%PoolSelectFeat = [18;19;45;14;28;40;6;90;11;68;84;22;69;2;44;70;25;7;65;1;5;4;8;57;9;21;13;10;29;3;53;20;27;15;47;23;16;33;26;30;35;89;24;82;31;59;62;79;32;50;34;36;51;55;60;37;72;43;12;38;17;58;39;41;86;67;42;46;48;49;52;54;56;64;88;61;66;63;75;71;85;73;74;76;80;77;78;81;87;83;];
%Resub
PoolSelectFeat =ResultMat(:,3);
%--------------------------------------------------------------------------

 if strcmp(DatasetToUse, 'finalvecDESsubset')
     [Patterns, Targets] = Preprocessing('finalvecDES');
     Patterns = Patterns(1:360,:);
     Targets  = Targets(1:360);
 elseif strcmp(DatasetToUse, 'finalvecSUSASsubset')
     [Patterns, Targets] = Preprocessing('finalvecSUSAS');
     rand('twister',260); % To have always the same set
     IndexSubset = randperm(length(Targets));
     IndexSubset = IndexSubset(1:end);
     Patterns = Patterns(IndexSubset,:);
     Targets  = Targets(IndexSubset);
     size(Patterns)
 else
     [Patterns, Targets] = Preprocessing(DatasetToUse);
 end

[NPatterns, KFeatures] = size(Patterns);
CClasses = max(Targets);
%  ------------------------- sffs settings -------------
PercTest             = 10;             % Percentage of data to use for testing. Options 5,...,50.
GammaParam = 0;                        % confidence interval to control number of repetitions.
                                       % Options:0<GammaParam <1. The lowest, the better.

if strcmp(ErrorEstMethod,'ProposedAB')
    NRepThres = [];                   % Only for programming. Do not change.
elseif strcmp(ErrorEstMethod,'ProposedA') || strcmp(ErrorEstMethod,'Standard')
   NRepThres = 1;                  % Number of repetitions for the standard SFFS. Options: >10,
                                               % The greater, the better.
   GammaParam = [];                          % Only for programming. Do not change.
elseif strcmp(ErrorEstMethod,'Resubstitution')
    NRepThres  =1;
end

ResultMat           = [];          		        	     % Result matrix with selection history.

for FeatIndx = 1:length(PoolSelectFeat)
    ResultMat(end+1,1)  = FeatIndx;
    ResultMat(end,2) = PoolSelectFeat(FeatIndx);

    [Critval(FeatIndx)] = EnsembCriterEval(...
                               Patterns(:,PoolSelectFeat(1:FeatIndx)), ...
                                        Targets, PercTest, ErrorEstMethod);

    ResultMat(end,4) = 1;
    ResultMat(end,3) = Critval(FeatIndx);

    NDc = NPatterns/CClasses;

    [InfoLossResub]   = CalcInfoLoss(FeatIndx, floor(NDc), 1);
    [InfoLossCross]   = CalcInfoLoss(FeatIndx, floor(NDc), 0);
    AverInfoLoss      = (InfoLossCross + InfoLossResub)/2;

    CCR                   = ResultMat(end,3);
    UpLimitCOD(FeatIndx)  = CCR  + AverInfoLoss*(1-CCR);
    LowLimitCOD(FeatIndx) = CCR  - AverInfoLoss*(CCR-1/CClasses);

    hold on
    plot(ResultMat(1:end,3))
    plot(   UpLimitCOD, 'g.-')
    plot(   LowLimitCOD, 'r.-')

    drawnow
end

return

%--------------------------------------------------------------------------------------------------------------
%--------------------------------------------------------------------------------------------------------------
%--------------------------------------------------------------------------------------------------------------
% -------------------------------  Criterion evaluation method ---------------------------------------
%--------------------------------------------------------------------------------------------------------------
%--------------------------------------------------------------------------------------------------------------

function [AverCorr] = EnsembCriterEval(Patterns, Targets, PercTest,...
               ErrorEstMethod)

NPatterns = size(Patterns,1);
Kfeatures = size(Patterns,2);
CClasses  = max(Targets);

GaussConst = (2*pi)^(-Kfeatures/2);

CCRTable = zeros(1, 2000);
Prior         = zeros(1,CClasses);
NRep = 500;

rep=0;
while rep  <  NRep,
        rep=rep+1;
        if strcmp(ErrorEstMethod,'ProposedAB') || strcmp(ErrorEstMethod,'ProposedA') ...
                || strcmp(ErrorEstMethod,'Standard'),
           IndexAll  = randperm(NPatterns);
           IndexTrain = IndexAll((floor(NPatterns*PercTest/100)+1):end);
           TrainSetPatterns = Patterns(IndexTrain,:);
           TrainSetTargets = Targets(IndexTrain);
           IndexTest = IndexAll(1:floor(NPatterns*PercTest/100));
           TestSetPatterns   = Patterns(IndexTest,:);
           TestSetTargets = Targets(IndexTest);
        elseif strcmp(ErrorEstMethod,'Resubstitution')
            TrainSetPatterns = Patterns;
            TrainSetTargets = Targets;
            TestSetPatterns = Patterns;
            TestSetTargets = Targets;
        end

           NTestPatterns = size(TestSetPatterns,1);
           NTrainPatterns = size(TrainSetPatterns,1);
           TargetsProbs = zeros(NTestPatterns, CClasses);

            for IndexClass = 1:CClasses
                    TrainSetIndexPerClass = find(TrainSetTargets == IndexClass);
                    TrainSetPatternPerClass = TrainSetPatterns(TrainSetIndexPerClass,: );
                    ResCl.mean(IndexClass,:)         = mean(TrainSetPatternPerClass);
                    ResCl.cov(:,:,IndexClass)         = cov(TrainSetPatternPerClass);
                    ResCl.covDet(IndexClass)        = det(ResCl.cov(:,:,IndexClass));
                    if ResCl.covDet(IndexClass) ~= 0
                       ResCl.covInv(:,:,IndexClass)     = inv(ResCl.cov(:,:,IndexClass));
%                    [ResCl.covInv(:,:,IndexClass), ResCl.covDet(IndexClass)] =...
   %                                                                               InvDet(ResCl.cov(:,:,IndexClass));
%                        NTestPatternsPerClass  =  sum(TestSetTargets == IndexClass);
%                       Prior(IndexClass) = NTestPatternsPerClass /length(TestSetTargets);
                          Prior(IndexClass)  = length(TrainSetIndexPerClass) / NTrainPatterns;
%                              Prior(IndexClass)  = 1/CClasses; % all equiprobable
                          ResCl.prior(IndexClass) = Prior(IndexClass);
                          ClassConst = GaussConst * ResCl.prior(IndexClass)*ResCl.covDet(IndexClass)^(-0.5);
                          DistMeanVectorTest = TestSetPatterns - ...
                                                                   repmat( ResCl.mean(IndexClass,:), NTestPatterns,1);
                          A = DistMeanVectorTest * ResCl.covInv(:,:,IndexClass);
                          G = sum(A.*DistMeanVectorTest,2);
                          TargetsProbs(1:NTestPatterns, IndexClass) = ClassConst * exp(-0.5*G);
                    else
                       TargetsProbs(1:NTestPatterns, IndexClass) = eps*rand(NTestPatterns,1);
                    end
            end

           [TargetMaxProb, TargetPrediction] = max(TargetsProbs');
           TargetPrediction = TargetPrediction';
           CorrClassified = length(find(TargetPrediction - TestSetTargets == 0));
           CCRTable(rep) = CorrClassified/ NTestPatterns;

 end                 % repetetion
AverCorr = sum(CCRTable(1:NRep))/NRep;
return


function convnet=getConvNetPara(net)
convLayers=[];
for l=1:numel(net.layers)
    if(strcmp(net.layers{l}.type,'conv')||strcmp(net.layers{l}.type,'conv_aNet')||strcmp(net.layers{l}.type,'conv_mask'))
        convLayers(end+1)=l;
    end
end
len=length(convLayers);
convnet.targetLayers=convLayers+1;
convnet.targetScale=zeros(1,len);
convnet.targetStride=zeros(1,len);
convnet.targetCenter=zeros(1,len);
for i=1:len
    tarLay=convLayers(i);
    layer=net.layers{tarLay};
    pad=layer.pad(1);
    scale=size(layer.weights{1},1);
    stride=layer.stride(1);
    if(i==1)
        convnet.targetStride(i)=stride;
        convnet.targetScale(i)=scale;
        convnet.targetCenter(i)=(1+scale-pad*2)/2;
    else
        IsPool=false;
        poolStride=0;
        poolSize=0;
        poolPad=0;
        for j=convLayers(i-1)+1:tarLay-1
            if(strcmp(net.layers{j}.type,'pool'))
                IsPool=true;
                poolSize=net.layers{j}.pool(1);
                poolStride=net.layers{j}.stride(1);
                poolPad=net.layers{j}.pad(1);
            end
        end
        convnet.targetStride(i)=(1+IsPool*(poolStride-1))*stride*convnet.targetStride(i-1);
        convnet.targetScale(i)=convnet.targetScale(i-1)+IsPool*(poolSize-1)*convnet.targetStride(i-1)+convnet.targetStride(i)*(scale-1);
        if(IsPool)
            convnet.targetCenter(i)=(scale-pad*2-1)*poolStride*convnet.targetStride(i-1)/2+(convnet.targetCenter(i-1)+convnet.targetStride(i-1)*(poolSize-2*poolPad-1)/2);
        else
            convnet.targetCenter(i)=(scale-pad*2-1)*convnet.targetStride(i-1)/2+convnet.targetCenter(i-1);
        end
    end
end
end