function [varargout] = DES(input64,mode,key)
%DES: Data Encryption Standard
% Encrypt/Decrypt a 64-bit message using a 64-bit key using the Feistel Network
% -------------------------------------------------------------------------
% Inputs:
%        input64 = a 64-bit message
%           mode = either 'ENC' encryption or 'DEC' decryption (default 'ENC')
%            key = a 56/64-bit key (optional under 'ENC', but mandatory under 'DEC')
% Outputs:
%   varargout{1} = output64, a 64-bit message after encryption/decryption
%   varargout{2} = a 64-bit key, if a 64-bit key is not provided as an input
% -------------------------------------------------------------------------
% Demos:
%   plaintext = round(rand(1,64));
%   [ciphertext,key] = DES(plaintext);       % Encryption syntex 1
%   [ciphertext1,key] = DES(plaintext,'ENC'); % Encryption syntex 2
%   deciphertext1 = DES(ciphertext1,'DEC',key);% Decryption syntex
%
%   key56 = round(rand(1,56));
%   [ciphertext2,key64] = DES(plaintext,'ENC',key56);% Encryption syntex 3 (56-bit key)
%   deciphertext2 = DES(ciphertext2,'DEC',key64);     % Decryption syntex   (64-bit key)
%   ciphertext3 = DES(plaintext,'ENC',key64);       % Encryption syntex 3 (64-bit key)
%   deciphertext3 = DES(ciphertext3,'DEC',key56);     % Decryption syntex   (56-bit key)
%
%   % plot results
%   subplot(4,2,1),plot(plaintext),ylim([-.5,1.5]),xlim([1,64]),title('plaintext')
%   subplot(4,2,2),plot(ciphertext),ylim([-.5,1.5]),xlim([1,64]),title('ciphertext')
%   subplot(4,2,3),plot(deciphertext1),ylim([-.5,1.5]),xlim([1,64]),title('deciphertext1')
%   subplot(4,2,4),plot(ciphertext1),ylim([-.5,1.5]),xlim([1,64]),title('ciphertext1')
%   subplot(4,2,5),plot(deciphertext2),ylim([-.5,1.5]),xlim([1,64]),title('deciphertext2')
%   subplot(4,2,6),plot(ciphertext2),ylim([-.5,1.5]),xlim([1,64]),title('ciphertext2')
%   subplot(4,2,7),plot(deciphertext3),ylim([-.5,1.5]),xlim([1,64]),title('deciphertext3')
%   subplot(4,2,8),plot(ciphertext3),ylim([-.5,1.5]),xlim([1,64]),title('ciphertext3')
% -------------------------------------------------------------------------
% NOTE:
% 1. If a 64-bit key is provided, then its bit parities will be checked. If
%    a 56-bit key is provided, then it is automatically added 8 partity
%    checking bits. However, the 8 parity bits are never used in
%    DES encryption/decryption process. They are included just for the
%    completeness of a DES implementation.
% 2. Cipher modes are not provided in this simple script. If you are
%    interested or do not know what does cipher modes mean, please go to page
%    http://en.wikipedia.org/wiki/Block_cipher_modes_of_operation
%    for details. Please keep in mind that selecting an inappropriate working
%    mode may extremely weaken the security of your messages.
% 3. A general description of DES can be found at its wiki page:
%    http://en.wikipedia.org/wiki/Data_Encryption_Standard
%    The detailed cryptographical primitives can be found under the page:
%    http://en.wikipedia.org/wiki/DES_supplementary_material
%    If you want to speed-up the DES code here, you can simply store these
%    primitives in memory and call them when you need.
% -------------------------------------------------------------------------
% By Yue (Rex) Wu
% ECE Dept @ Tufts Univ.
% 08/18/2012
% If you find bugs, please email me via ywu03@ece.tufts.edu
% -------------------------------------------------------------------------

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%                           0. Initialization                           %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 0.1 check input
error(nargchk(1,3,nargin));
switch nargin
    case 1
        mode = 'ENC';
        K = round(rand(8,7));
        K(:,8) = mod(sum(K,2),2); % note these eight bits of key are never used in encryption
        K = reshape(K',1,64);
        varargout{2} = K;
    case 2
        switch mode
            case 'ENC'
                K = round(rand(8,7));
                K(:,8) = mod(sum(K,2),2); % note these eight bits of key are never used in encryption
                K = reshape(K',1,64);
                varargout{2} = K;
            case 'DEC'
                error('Key has to be provided in decryption mode (DEC)')
            otherwise
                error('WRONG working mode!!! Select either encrtyption mode: ENC or decryption mode: DEC !!!')
        end
    case 3
        if isempty(setdiff(unique(key),[0,1])) % check provided key type
            if numel(key) == 64  % check provided key parity
                keyParityCheck = @(k) (sum(mod(sum(reshape(k,8,8)),2))==0);
                if keyParityCheck(key) == 1
                    K = key(:)';
                else
                    error('Key parity check FAILED!!!')
                end
            elseif numel(key) == 56 % add parity bits
                K = reshape(key,7,8)';
                K(:,8) = mod(sum(K,2),2); % note these eight bits of key are never used in encryption
                K = reshape(K',1,64);
                varargout{2} = K;
                display('Key parity bits added')
            else
                error('Key has to be either 56 or 64-bit long!!!')
            end
        else
            error('Key has to be binary!!!')
        end
end

% 0.2 check message length and type
if numel(input64) == 64 && isempty(setdiff(unique(input64),[0,1]))
    P = input64;
else
    error('Message has to be a 64-bit message!!!')
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%                   1. Cryptographical primitives                       %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 1.1 define splitting function
HALF_L = @(message) message(1:32);
HALF_R = @(message) message(33:64);
% 1.2 define expansion function
EF = @(halfMessage) [halfMessage([32,4:4:28])',(reshape(halfMessage,4,8))',halfMessage([5:4:29,1])'];
% 1.3 define key mixing (KM)
KM = @(expandedHalfMessage,rK) xor(expandedHalfMessage,reshape(rK,6,8)');
% 1.4 define eight substitution tables
% input: 0	1   2   3   4   5   6   7   8   9   10  11  12  13  14  15
st{1} = [14	4	13	1	2	15	11	8	3	10	6	12	5	9	0	7;...
         0  15	7	4	14	2	13	1	10	6	12	11	9	5	3	8;...
         4	1	14	8	13	6	2	11	15	12	9	7	3	10	5	0;...
         15	12	8	2	4	9	1	7	5	11	3	14	10	0	6	13];
st{2} = [15	1	8	14	6	11	3	4	9	7	2	13	12	0	5	10;...
    	3	13	4	7	15	2	8	14	12	0	1	10	6	9	11	5;...
		0	14	7	11	10	4	13	1	5	8	12	6	9	3	2	15;...
		13	8	10	1	3	15	4	2	11	6	7	12	0	5	14	9];
st{3} = [10	0	9	14	6	3	15	5	1	13	12	7	11	4	2	8;...
		13	7	0	9	3	4	6	10	2	8	5	14	12	11	15	1;...
		13	6	4	9	8	15	3	0	11	1	2	12	5	10	14	7;...
		1	10	13	0	6	9	8	7	4	15	14	3	11	5	2	12];
st{4} = [7	13	14	3	0	6	9	10	1	2	8	5	11	12	4	15;...
		13	8	11	5	6	15	0	3	4	7	2	12	1	10	14	9;...
		10	6	9	0	12	11	7	13	15	1	3	14	5	2	8	4;...
		3	15	0	6	10	1	13	8	9	4	5	11	12	7	2	14];
st{5} = [2	12	4	1	7	10	11	6	8	5	3	15	13	0	14	9;...
		14	11	2	12	4	7	13	1	5	0	15	10	3	9	8	6;...
		4	2	1	11	10	13	7	8	15	9	12	5	6	3	0	14;...
		11	8	12	7	1	14	2	13	6	15	0	9	10	4	5	3];
st{6} = [12	1	10	15	9	2	6	8	0	13	3	4	14	7	5	11;...
		10	15	4	2	7	12	9	5	6	1	13	14	0	11	3	8;...
		9	14	15	5	2	8	12	3	7	0	4	10	1	13	11	6;...
		4	3	2	12	9	5	15	10	11	14	1	7	6	0	8	13];
st{7} = [4	11	2	14	15	0	8	13	3	12	9	7	5	10	6	1;...
		13	0	11	7	4	9	1	10	14	3	5	12	2	15	8	6;...
		1	4	11	13	12	3	7	14	10	15	6	8	0	5	9	2;...
		6	11	13	8	1	4	10	7	9	5	0	15	14	2	3	12];
st{8} = [13	2	8	4	6	15	11	1	10	9	3	14	5	0	12	7;...
		1	15	13	8	10	3	7	4	12	5	6	11	0	14	9	2;...
		7	11	4	1	9	12	14	2	0	6	10	13	15	3	5	8;...
		2	1	14	7	4	10	8	13	15	12	9	0	3	5	6	11];
% the eight binary s-boxes
for i = 1:8
    ST{i} = mat2cell(blkproc(st{i},[1,1],@(x) de2bi(x,4,'left-msb')),ones(1,4),ones(1,16)*4);
end
% 1.5 define subsitution function (SBOX)
SUBS = @(expandedHalfMessage,blkNo) ST{blkNo}{bi2de(expandedHalfMessage(blkNo,[1,6]),'left-msb')+1,bi2de(expandedHalfMessage(blkNo,[2:5]),'left-msb')+1};
SBOX = @(expandedHalfMessage) [SUBS(expandedHalfMessage,1);SUBS(expandedHalfMessage,2);...
                               SUBS(expandedHalfMessage,3);SUBS(expandedHalfMessage,4);...
                               SUBS(expandedHalfMessage,5);SUBS(expandedHalfMessage,6);...
                               SUBS(expandedHalfMessage,7);SUBS(expandedHalfMessage,8)];
% 1.6 define permutation function (PBOX)
PBOX = @(halfMessage) halfMessage([16  7 20 21  29 12 28 17 ...
                                    1 15 23 26   5 18 31 10 ...
                                    2  8 24 14  32 27  3  9 ...
                                   19 13 30  6  22 11  4  25]);
% 1.7 define initial permutation (IP)
IP = @(message) message([58	50	42	34	26	18	10	2 ...
                        60	52	44	36	28	20	12	4 ...
                        62	54	46	38	30	22	14	6 ...
                        64	56	48	40	32	24	16	8 ...
                        57	49	41	33	25	17	9	1 ...
                        59	51	43	35	27	19	11	3 ...
                        61	53	45	37	29	21	13	5 ...
                        63	55	47	39	31	23	15	7]);
% 1.8 define final permutation (FP)
FP = @(message) message([40	8	48	16	56	24	64	32 ...
                        39	7	47	15	55	23	63	31 ...
                        38	6	46	14	54	22	62	30 ...
                        37	5	45	13	53	21	61	29 ...
                        36	4	44	12	52	20	60	28 ...
                        35	3	43	11	51	19	59	27 ...
                        34	2	42	10	50	18	58	26 ...
                        33	1	41	9	49	17	57	25]);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%                           2. key schedule                             %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 2.1 define permuted choice 1 (PC1)
PC1L = @(key64) key64([57	49	41	33	25	17	9 ...
                    1	58	50	42	34	26	18 ...
                    10	2	59	51	43	35	27 ...
                    19	11	3	60	52	44	36]);
PC1R = @(key64) key64([63	55	47	39	31	23	15 ...
                    7	62	54	46	38	30	22 ...
                    14	6	61	53	45	37	29 ...
                    21	13	5	28	20	12	4]);
% 2.2 define permuted choice 2 (PC2)
PC2 = @(key56) key56([14 17	11	24	1	5	3	28 ...
                     15	6	21	10	23	19	12	4 ...
                     26	8	16	7	27	20	13	2 ...
                     41	52	31	37	47	55	30	40 ...
                     51	45	33	48	44	49	39	56 ...
                     34	53	46	42	50	36	29	32]);
% 2.3 define rotations in key-schedule (RK)
% round# 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6
   RK = [1 1 2 2 2 2 2 2 1 2 2 2 2 2 2 1];
% 2.4 define key shift function (KS)
KS = @(key28,s) [key28(s+1:end),key28(1:s)];
% 2.5 define sub-keys for each round
leftHKey = PC1L(K); % 28-bit half key
rightHKey = PC1R(K);% 28-bit half key
for i = 1:16
    leftHKey = KS(leftHKey,RK(i));
    rightHKey = KS(rightHKey,RK(i));
    key56 = [leftHKey ,rightHKey];
    subKeys(i,:) = PC2(key56(:));
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%                           3. DES main loop                            %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 3.1 initial permutation
C = IP(P);
switch mode
    case 'ENC' % if encryption, split 64 message to two halves
        L{1} = HALF_L(C); % left-half 32-bit
        R{1} = HALF_R(C); % right-half 32-bit
    case 'DEC' % if decryption, swapping two halves
        L{1} = HALF_R(C);
        R{1} = HALF_L(C);
end
% 3.2 cipher round 1 to 16
for i = 1:16
     L{i+1} = R{i}; % half key: 32-bit
     expended_R = EF(R{i}); % expended half key: 32-bit to 48-bit
     switch mode
        case 'ENC' % if encryption, apply sub-keys in the original order
            mixed_R = KM(expended_R,subKeys(i,:)); % mixed with sub-key: 48-bit
        case 'DEC' % if decryption, apply sub-keys in the reverse order
            mixed_R = KM(expended_R,subKeys(16-i+1,:)); % mixed with sub-key: 48-bit
     end
     substituted_R = SBOX(mixed_R); % substitution: 48-bit to 32-bit
     permuted_R = PBOX(reshape(substituted_R',1,32)); % permutation: 32-bit
     R{i+1} = xor(L{i},permuted_R); % Feistel function: 32-bit
end
% 3.3 final permutation
switch mode
    case 'ENC'
        C = [L{end},R{end}];
    case 'DEC'
        C = [R{end},L{end}];
end
output64 = FP(C);
varargout{1} = output64;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%                                   END                                 %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function [ gc_sum, gc_avg, gc_std  ] = gcFeature( gc, indicator, coor2D_indi )
%GCFEATURE Summary of this function goes here
%   Detailed explanation goes here
if size(indicator,1) ~= size(coor2D_indi,1)
    fprintf('Warning: indicator and coor mismatch!\n');
end
% vector_num = size(indicator,1);
data_num = size(indicator, 2);

gc_sum = zeros( data_num, 11);
gc_avg = zeros( data_num, 11);
gc_std = zeros( data_num, 11);

[sH, sW, ~] = size(gc);
% feature = zeros(data_num, 100);

gc = cat(3, gc, sum(gc(:,:,1:4),3), max(gc(:,:,1:4),[],3), sum(gc(:,:,1:6),3), max(gc(:,:,1:6),[],3));
for did = 1:data_num
    vector_valid = indicator(:,did);
    if ~any(vector_valid)
        continue;
    end
    cood_ind = coor2D_indi(vector_valid);
    loc_orit = zeros(length(cood_ind),11);
    for k = 1:11
        loc_orit(:,k) = gc(cood_ind+(k-1)*sW*sH);
    end

    gc_sum(did,:) = sum(loc_orit, 1);
    gc_avg(did,:) = gc_sum(did,:)./length(cood_ind);
    gc_std(did,:) = std(loc_orit, 1);
end

end


% tutorial2_spikehistcoupledGLM.m
%
% This is an interactive tutorial designed to walk you through the steps of
% fitting an autoregressive Poisson GLM (i.e., a spiking GLM with
% spike-history) and a multivariate autoregressive Poisson GLM (i.e., a
% GLM with spike-history AND coupling between neurons).
%
% Data: from Uzzell & Chichilnisky 2004; see README file for details.
%
% Last updated: Mar 10, 2020 (JW Pillow)

% Instructions: Execute each section below separately using cmd-enter.
% For detailed suggestions on how to interact with this tutorial, see
% header material in tutorial1_PoissonGLM.m

%% ====  1. Load the raw data ============

% ------------------------------------------------------------------------
% Be sure to unzip the data file data_RGCs.zip
% (http://pillowlab.princeton.edu/data/data_RGCs.zip) and place it in
% this directory before running the tutorial.
% ------------------------------------------------------------------------
% (Data from Uzzell & Chichilnisky 2004):
datdir = 'data_RGCs/';  % directory where stimulus lives
load([datdir, 'Stim']);    % stimulus (temporal binary white noise)
load([datdir,'stimtimes']); % stim frame times in seconds (if desired)
load([datdir, 'SpTimes']); % load spike times (in units of stim frames)
ncells = length(SpTimes);  % number of neurons (4 for this dataset).
% Neurons #1-2 are OFF, #3-4 are ON.
% -------------------------------------------------------------------------

% Compute some basic statistics on the stimulus
dtStim = (stimtimes(2)-stimtimes(1)); % time bin size for stimulus (s)
nT = size(Stim,1); % number of time bins in stimulus

% See tutorial 1 for some code to visualize the raw data!

%% ==== 2. Bin the spike trains =========================
%
% For now we will assume we want to use the same time bin size as the time
% bins used for the stimulus. Later, though, we'll wish to vary this.
tbins = (.5:nT)*dtStim; % time bin centers for spike train binnning
sps = zeros(nT,ncells);
for jj = 1:ncells
    sps(:,jj) = hist(SpTimes{jj},tbins)';  % binned spike train
end

% Let's just visualize the spike-train auto and cross-correlations
% (Comment out this part if desired!)
clf;
nlags = 30; % number of time-lags to use
for ii = 1:ncells
    for jj = ii:ncells
        % Compute cross-correlation of neuron i with neuron j
        xc = xcorr(sps(:,ii),sps(:,jj),nlags,'unbiased');

        % remove center-bin correlation for auto-correlations (for ease of viz)
        if ii==jj, xc(nlags+1) = 0;
        end

        % Make plot
        subplot(ncells,ncells,(ii-1)*ncells+jj);
        plot((-nlags:nlags)*dtStim,xc,'.-','markersize',20);
        axis tight; drawnow;
        title(sprintf('cells (%d,%d)',ii,jj)); axis tight;
    end
end
xlabel('time shift (s)');

%% ==== 3. Build design matrix: single-neuron GLM with spike-history =========

% Pick the cell to focus on (for now).
cellnum = 3;  % 1-2: OFF, 3-4: ON

% Set the number of time bins of stimulus to use for predicting spikes
ntfilt = 25;  % Try varying this, to see how performance changes!
% Set number of time bins of auto-regressive spike-history to use
nthist = 20;

% Build stimulus design matrix (using 'hankel');
paddedStim = [zeros(ntfilt-1,1); Stim]; % pad early bins of stimulus with zero
Xstim = hankel(paddedStim(1:end-ntfilt+1), Stim(end-ntfilt+1:end));

% Build spike-history design matrix
paddedSps = [zeros(nthist,1); sps(1:end-1,cellnum)];
% SUPER important: note that this doesn't include the spike count for the
% bin we're predicting? The spike train is shifted by one bin (back in
% time) relative to the stimulus design matrix
Xsp = hankel(paddedSps(1:end-nthist+1), paddedSps(end-nthist+1:end));

% Combine these into a single design matrix
Xdsgn = [Xstim,Xsp];

% Let's visualize the design matrix just to see what it looks like
subplot(1,10,1:9);
imagesc(1:(ntfilt+nthist), 1:50, Xdsgn(1:50,:));
xlabel('regressor');
ylabel('time bin of response');
title('design matrix (including stim and spike history)');
subplot(1,10,10);
imagesc(sps(1:50,cellnum));
set(gca,'yticklabel', []);
title('spike count');

% The left part of the design matrix has the stimulus values, the right
% part has the spike-history values.  The image on the right is the spike
% count to be predicted.  Note that the spike-history portion of the design
% matrix had better be shifted so that we aren't allowed to use the spike
% count on this time bin to predict itself!

%% === 4. fit single-neuron GLM with spike-history ==================

% First fit GLM with no spike-history
fprintf('Now fitting basic Poisson GLM...\n');
pGLMwts0 = glmfit(Xstim,sps(:,cellnum),'poisson'); % assumes 'log' link and 'constant'='on'.
pGLMconst0 = pGLMwts0(1);
pGLMfilt0 = pGLMwts0(2:end);

% Then fit GLM with spike history (now use Xdsgn design matrix instead of Xstim)
fprintf('Now fitting Poisson GLM with spike-history...\n');
pGLMwts1 = glmfit(Xdsgn,sps(:,cellnum),'poisson');
pGLMconst1 = pGLMwts1(1);
pGLMfilt1 = pGLMwts1(2:1+ntfilt);
pGLMhistfilt1 = pGLMwts1(ntfilt+2:end);

%%  Make plots comparing filters
ttk = (-ntfilt+1:0)*dtStim; % time bins for stim filter
tth = (-nthist:-1)*dtStim; % time bins for spike-history filter

clf; subplot(221); % Plot stim filters
h = plot(ttk,ttk*0,'k--',ttk,pGLMfilt0, 'o-',ttk,pGLMfilt1,'o-','linewidth',2);
legend(h(2:3), 'GLM', 'sphist-GLM','location','northwest');axis tight;
title('stimulus filters'); ylabel('weight');
xlabel('time before spike (s)');

subplot(222); % Plot spike history filter
colr = get(h(3),'color');
h = plot(tth,tth*0,'k--',tth,pGLMhistfilt1, 'o-');
set(h(2), 'color', colr, 'linewidth', 2);
title('spike history filter');
xlabel('time before spike (s)');
ylabel('weight'); axis tight;

%% Plot predicted rate out of the two models

% Compute predicted spike rate on training data
ratepred0 = exp(pGLMconst0 + Xstim*pGLMfilt0);
ratepred1 = exp(pGLMconst1 + Xdsgn*pGLMwts1(2:end));

% Make plot
iiplot = 1:60; ttplot = iiplot*dtStim;
subplot(212);
stem(ttplot,sps(iiplot,cellnum), 'k'); hold on;
plot(ttplot,ratepred0(iiplot),ttplot,ratepred1(iiplot), 'linewidth', 2);
hold off;  axis tight;
legend('spikes', 'GLM', 'hist-GLM');
xlabel('time (s)');
title('spikes and rate predictions');
ylabel('spike count / bin');

%% === 5. fit coupled GLM for multiple-neuron responses ==================

% First step: build design matrix containing spike history for all neurons

Xspall = zeros(nT,nthist,ncells); % allocate space
% Loop over neurons to build design matrix, exactly as above
for jj = 1:ncells
    paddedSps = [zeros(nthist,1); sps(1:end-1,jj)];
    Xspall(:,:,jj) = hankel(paddedSps(1:end-nthist+1),paddedSps(end-nthist+1:end));
end

% Reshape it to be a single matrix
Xspall = reshape(Xspall,nT,[]);
Xdsgn2 = [Xstim, Xspall]; % full design matrix (with all 4 neuron spike hist)

clf; % Let's visualize 50 time bins of full design matrix
imagesc(1:1:(ntfilt+nthist*ncells), 1:50, Xdsgn2(1:50,:));
title('design matrix (stim and 4 neurons spike history)');
xlabel('regressor');
ylabel('time bin of response');

%% Fit the model (stim filter, sphist filter, coupling filters) for one neuron

fprintf('Now fitting Poisson GLM with spike-history and coupling...\n');

pGLMwts2 = glmfit(Xdsgn2,sps(:,cellnum),'poisson');
pGLMconst2 = pGLMwts2(1);
pGLMfilt2 = pGLMwts2(2:1+ntfilt);
pGLMhistfilts2 = pGLMwts2(ntfilt+2:end);
pGLMhistfilts2 = reshape(pGLMhistfilts2,nthist,ncells);

% So far all we've done is fit incoming stimulus and coupling filters for
% one neuron.  To fit a full population model, redo the above for each cell
% (i.e., to get incoming filters for 'cellnum' = 1, 2, 3, and 4 in turn).

%% Plot the fitted filters and rate prediction

clf; subplot(221); % Plot stim filters
h = plot(ttk,ttk*0,'k--',ttk,pGLMfilt0, 'o-',ttk,pGLMfilt1,...
    ttk,pGLMfilt2,'o-','linewidth',2); axis tight;
legend(h(2:4), 'GLM', 'sphist-GLM','coupled-GLM', 'location','northwest');
title(['stimulus filter: cell ' num2str(cellnum)]); ylabel('weight');
xlabel('time before spike (s)');

subplot(222); % Plot spike history filter
colr = get(h(3),'color');
h = plot(tth,tth*0,'k--',tth,pGLMhistfilts2,'linewidth',2);
legend(h(2:end),'from 1', 'from 2', 'from 3', 'from 4', 'location', 'northwest');
title(['coupling filters: into cell ' num2str(cellnum)]); axis tight;
xlabel('time before spike (s)');
ylabel('weight');

% Compute predicted spike rate on training data
ratepred2 = exp(pGLMconst2 + Xdsgn2*pGLMwts2(2:end));

% Make plot
iiplot = 1:60; ttplot = iiplot*dtStim;
subplot(212);
stem(ttplot,sps(iiplot,cellnum), 'k'); hold on;
plot(ttplot,ratepred0(iiplot),ttplot,ratepred1(iiplot),...
    ttplot,ratepred2(iiplot), 'linewidth', 2);
hold off;  axis tight;
legend('spikes', 'GLM', 'sphist-GLM', 'coupled-GLM', 'location', 'northwest');
xlabel('time (s)');
title('spikes and rate predictions');
ylabel('spike count / bin');

%% 6. Model comparison: log-likelihoood and AIC

% Let's compute loglikelihood (single-spike information) and AIC to see how
% much we gain by adding each of these filter types in turn:

LL_stimGLM = sps(:,cellnum)'*log(ratepred0) - sum(ratepred0);
LL_histGLM = sps(:,cellnum)'*log(ratepred1) - sum(ratepred1);
LL_coupledGLM = sps(:,cellnum)'*log(ratepred2) - sum(ratepred2);

% log-likelihood for homogeneous Poisson model
nsp = sum(sps(:,cellnum));
ratepred_const = nsp/nT;  % mean number of spikes / bin
LL0 = nsp*log(ratepred_const) - nT*sum(ratepred_const);

% Report single-spike information (bits / sp)
SSinfo_stimGLM = (LL_stimGLM - LL0)/nsp/log(2);
SSinfo_histGLM = (LL_histGLM - LL0)/nsp/log(2);
SSinfo_coupledGLM = (LL_coupledGLM - LL0)/nsp/log(2);

fprintf('\n empirical single-spike information:\n ---------------------- \n');
fprintf('stim-GLM: %.2f bits/sp\n',SSinfo_stimGLM);
fprintf('hist-GLM: %.2f bits/sp\n',SSinfo_histGLM);
fprintf('coupled-GLM: %.2f bits/sp\n',SSinfo_coupledGLM);

% Compute AIC
AIC0 = -2*LL_stimGLM + 2*(1+ntfilt);
AIC1 = -2*LL_histGLM + 2*(1+ntfilt+nthist);
AIC2 = -2*LL_coupledGLM + 2*(1+ntfilt+ncells*nthist);
AICmin = min([AIC0,AIC1,AIC2]); % the minimum of these

fprintf('\n AIC comparison (smaller is better):\n ---------------------- \n');
fprintf('stim-GLM: %.1f\n',AIC0-AICmin);
fprintf('hist-GLM: %.1f\n',AIC1-AICmin);
fprintf('coupled-GLM: %.1f\n',AIC2-AICmin);

% These are whopping differencess! Clearly coupling has a big impact in
% terms of log-likelihood, though the jump from stimulus-only to
% own-spike-history is greater than the jump from spike-history to
% full coupling.

%% Advanced exercises:
% --------------------
% 1. Write code to simulate spike trains from the fitted spike-history GLM.
% Simulate a raster of repeated responses from the stim-only GLM and
% compare to raster from the spike-history GLM

% 2. Write code to simulate the 4-neuron population-coupled GLM. There are
% now 16 spike-coupling filters (including self-coupling), since each
% neuron has 4 incoming coupling filters (its own spike history coupling
% filter plus coupling from three other neurons.  How does a raster of
% responses from this model compare to the two single-neuron models?

% 3. Compute a non-parametric estimate of the spiking nonlinearity for each
% neuron. How close does it look to exponential now that we have added
% spike history? Rerun your simulations using different non-parametric
% nonlinearity for each neuron. How much improvement do you see in terms of
% log-likelihood, AIC, or PSTH % variance accounted for (R^2) when you
% simulate repeated responses?


function [ x, istop, itn, r1norm, r2norm, anorm, acond, arnorm, xnorm, var ]...
  = lsqr( m, n, A, b, damp, atol, btol, conlim, itnlim, show )
%
%        [ x, istop, itn, r1norm, r2norm, anorm, acond, arnorm, xnorm, var ]...
% = lsqr( m, n, A, b, damp, atol, btol, conlim, itnlim, show );
%
% LSQR solves  Ax = b  or  min ||b - Ax||_2  if damp = 0,
% or   min || (b)  -  (  A   )x ||   otherwise.
%          || (0)     (damp I)  ||2
% A  is an m by n matrix defined or a function handle of aprod( mode,x ),
% that performs the matrix-vector operations.
% If mode = 1,   aprod  must return  y = Ax   without altering x.
% If mode = 2,   aprod  must return  y = A'x  without altering x.

%-----------------------------------------------------------------------
% LSQR uses an iterative (conjugate-gradient-like) method.
% For further information, see
% 1. C. C. Paige and M. A. Saunders (1982a).
%    LSQR: An algorithm for sparse linear equations and sparse least squares,
%    ACM TOMS 8(1), 43-71.
% 2. C. C. Paige and M. A. Saunders (1982b).
%    Algorithm 583.  LSQR: Sparse linear equations and least squares problems,
%    ACM TOMS 8(2), 195-209.
% 3. M. A. Saunders (1995).  Solution of sparse rectangular systems using
%    LSQR and CRAIG, BIT 35, 588-604.
%
% Input parameters:
% atol, btol  are stopping tolerances.  If both are 1.0e-9 (say),
%             the final residual norm should be accurate to about 9 digits.
%             (The final x will usually have fewer correct digits,
%             depending on cond(A) and the size of damp.)
% conlim      is also a stopping tolerance.  lsqr terminates if an estimate
%             of cond(A) exceeds conlim.  For compatible systems Ax = b,
%             conlim could be as large as 1.0e+12 (say).  For least-squares
%             problems, conlim should be less than 1.0e+8.
%             Maximum precision can be obtained by setting
%             atol = btol = conlim = zero, but the number of iterations
%             may then be excessive.
% itnlim      is an explicit limit on iterations (for safety).
% show = 1    gives an iteration log,
% show = 0    suppresses output.
%
% Output parameters:
% x           is the final solution.
% istop       gives the reason for termination.
% istop       = 1 means x is an approximate solution to Ax = b.
%             = 2 means x approximately solves the least-squares problem.
% r1norm      = norm(r), where r = b - Ax.
% r2norm      = sqrt( norm(r)^2  +  damp^2 * norm(x)^2 )
%             = r1norm if damp = 0.
% anorm       = estimate of Frobenius norm of Abar = [  A   ].
%                                                    [damp*I]
% acond       = estimate of cond(Abar).
% arnorm      = estimate of norm(A'*r - damp^2*x).
% xnorm       = norm(x).
% var         (if present) estimates all diagonals of (A'A)^{-1} (if damp=0)
%             or more generally (A'A + damp^2*I)^{-1}.
%             This is well defined if A has full column rank or damp > 0.
%             (Not sure what var means if rank(A) < n and damp = 0.)
%
%
%        1990: Derived from Fortran 77 version of LSQR.
% 22 May 1992: bbnorm was used incorrectly.  Replaced by anorm.
% 26 Oct 1992: More input and output parameters added.
% 01 Sep 1994: Matrix-vector routine is now a parameter 'aprodname'.
%              Print log reformatted.
% 14 Jun 1997: show  added to allow printing or not.
% 30 Jun 1997: var   added as an optional output parameter.
% 07 Aug 2002: Output parameter rnorm replaced by r1norm and r2norm.
%              Michael Saunders, Systems Optimization Laboratory,
%              Dept of MS&E, Stanford University.
% 03 Jul 2007: Modified 'aprodname' to A, which can either be an m by n
%              matrix, or a function handle.
%              Ewout van den Berg, University of British Columbia
% 03 Jul 2007: Modified 'test2' condition, omitted 'test1'.
%              Ewout van den Berg, University of British Columbia
%-----------------------------------------------------------------------

%     Initialize.

msg=['The exact solution is  x = 0                              '
     'Ax - b is small enough, given atol, btol                  '
     'The least-squares solution is good enough, given atol     '
     'The estimate of cond(Abar) has exceeded conlim            '
     'Ax - b is small enough for this machine                   '
     'The least-squares solution is good enough for this machine'
     'Cond(Abar) seems to be too large for this machine         '
     'The iteration limit has been reached                      '];

wantvar= nargout >= 6;
if wantvar, var = zeros(n,1); end

if show
   disp(' ')
   disp('LSQR            Least-squares solution of  Ax = b')
   str1 = sprintf('The matrix A has %8g rows  and %8g cols', m, n);
   str2 = sprintf('damp = %20.14e    wantvar = %8g', damp,wantvar);
   str3 = sprintf('atol = %8.2e                 conlim = %8.2e', atol, conlim);
   str4 = sprintf('btol = %8.2e                 itnlim = %8g'  , btol, itnlim);
   disp(str1);   disp(str2);   disp(str3);   disp(str4);
end

itn    = 0;		 istop  = 0;        nstop  = 0;
ctol   = 0;		 if conlim > 0, ctol = 1/conlim; end;
anorm  = 0;		 acond  = 0;
dampsq = damp^2; ddnorm = 0;        res2   = 0;
xnorm  = 0;	     xxnorm = 0;        z      = 0;
cs2    = -1;     sn2    = 0;

% Set up the first vectors u and v for the bidiagonalization.

% These satisfy  beta*u = b,  alfa*v = A'u.

u      = b(1:m);	x    = zeros(n,1);
alfa   = 0;		beta = norm( u );
if beta > 0
   u = (1/beta) * u;	v = Aprod(u,2);
   alfa = norm( v );
end
if alfa > 0
   v = (1/alfa) * v;    w = v;
end

arnorm = alfa * beta;
if arnorm == 0
   if show, disp(msg(1,:)); end
   return
end
arnorm0= arnorm;

rhobar = alfa;		phibar = beta;		bnorm  = beta;
rnorm  = beta;
r1norm = rnorm;
r2norm = rnorm;
head1  = '   Itn      x(1)       r1norm     r2norm ';
head2  = ' Compatible   LS      Norm A   Cond A';

if show
   disp(' ')
   disp([head1 head2])
   test1  = 1;		test2  = alfa / beta;
   str1   = sprintf( '%6g %12.5e',        itn,   x(1) );
   str2   = sprintf( ' %10.3e %10.3e', r1norm, r2norm );
   str3   = sprintf( '  %8.1e %8.1e',   test1,  test2 );
   disp([str1 str2 str3])
end

%------------------------------------------------------------------
%     Main iteration loop.
%------------------------------------------------------------------
while itn < itnlim
      itn = itn + 1;
%     Perform the next step of the bidiagonalization to obtain the
%     next  beta, u, alfa, v.  These satisfy the relations
%                beta*u  =  a*v   -  alfa*u,
%                alfa*v  =  A'*u  -  beta*v.

      u    = Aprod(v,1)  -  alfa*u;
      beta = norm( u );
      if beta > 0
         u     = (1/beta) * u;
         anorm = norm([anorm alfa beta damp]);
         v     = Aprod(u, 2)  -  beta*v;
         alfa  = norm( v );
         if alfa > 0,  v = (1/alfa) * v; end
      end

%     Use a plane rotation to eliminate the damping parameter.
%     This alters the diagonal (rhobar) of the lower-bidiagonal matrix.

      rhobar1 = norm([rhobar damp]);
      cs1     = rhobar / rhobar1;
      sn1     = damp   / rhobar1;
      psi     = sn1 * phibar;
      phibar  = cs1 * phibar;

%     Use a plane rotation to eliminate the subdiagonal element (beta)
%     of the lower-bidiagonal matrix, giving an upper-bidiagonal matrix.

      rho     = norm([rhobar1 beta]);
      cs      =   rhobar1/ rho;
      sn      =   beta   / rho;
      theta   =   sn * alfa;
      rhobar  = - cs * alfa;
      phi     =   cs * phibar;
      phibar  =   sn * phibar;
      tau     =   sn * phi;

%     Update x and w.

      t1      =   phi  /rho;
      t2      = - theta/rho;
      dk      =   (1/rho)*w;

      x       = x      +  t1*w;
      w       = v      +  t2*w;
      ddnorm  = ddnorm +  norm(dk)^2;
      if wantvar, var = var  +  dk.*dk; end

%     Use a plane rotation on the right to eliminate the
%     super-diagonal element (theta) of the upper-bidiagonal matrix.
%     Then use the result to estimate  norm(x).

      delta   =   sn2 * rho;
      gambar  = - cs2 * rho;
      rhs     =   phi  -  delta * z;
      zbar    =   rhs / gambar;
      xnorm   =   sqrt(xxnorm + zbar^2);
      gamma   =   norm([gambar theta]);
      cs2     =   gambar / gamma;
      sn2     =   theta  / gamma;
      z       =   rhs    / gamma;
      xxnorm  =   xxnorm  +  z^2;

%     Test for convergence.
%     First, estimate the condition of the matrix  Abar,
%     and the norms of  rbar  and  Abar'rbar.

      acond   =   anorm * sqrt( ddnorm );
      res1    =   phibar^2;
      res2    =   res2  +  psi^2;
      rnorm   =   sqrt( res1 + res2 );
      arnorm  =   alfa * abs( tau );

%     07 Aug 2002:
%     Distinguish between
%        r1norm = ||b - Ax|| and
%        r2norm = rnorm in current code
%               = sqrt(r1norm^2 + damp^2*||x||^2).
%        Estimate r1norm from
%        r1norm = sqrt(r2norm^2 - damp^2*||x||^2).
%     Although there is cancellation, it might be accurate enough.

      r1sq    =   rnorm^2  -  dampsq * xxnorm;
      r1norm  =   sqrt( abs(r1sq) );   if r1sq < 0, r1norm = - r1norm; end
      r2norm  =   rnorm;

%     Now use these norms to estimate certain other quantities,
%     some of which will be small near a solution.

      test1   =   rnorm / bnorm;
      test2   =   arnorm / arnorm0;
%     test2   =   arnorm/( anorm * rnorm );
      test3   =       1 / acond;
      t1      =   test1 / (1    +  anorm * xnorm / bnorm);
      rtol    =   btol  +  atol *  anorm * xnorm / bnorm;

%     The following tests guard against extremely small values of
%     atol, btol  or  ctol.  (The user may have set any or all of
%     the parameters  atol, btol, conlim  to 0.)
%     The effect is equivalent to the normal tests using
%     atol = eps,  btol = eps,  conlim = 1/eps.

      if itn >= itnlim,   istop = 7; end
      if 1 + test3  <= 1, istop = 6; end
      if 1 + test2  <= 1, istop = 5; end
      if 1 + t1     <= 1, istop = 4; end

%     Allow for tolerances set by the user.

      if  test3 <= ctol,  istop = 3; end
      if  test2 <= atol,  istop = 2; end
%     if  test1 <= rtol,  istop = 1; end

%     See if it is time to print something.

      prnt = 0;
      if n     <= 40       , prnt = 1; end
      if itn   <= 10       , prnt = 1; end
      if itn   >= itnlim-10, prnt = 1; end
      if rem(itn,10) == 0  , prnt = 1; end
      if test3 <=  2*ctol  , prnt = 1; end
      if test2 <= 10*atol  , prnt = 1; end
%     if test1 <= 10*rtol  , prnt = 1; end
      if istop ~=  0       , prnt = 1; end

      if prnt == 1
         if show
            str1 = sprintf( '%6g %12.5e',        itn,   x(1) );
            str2 = sprintf( ' %10.3e %10.3e', r1norm, r2norm );
            str3 = sprintf( '  %8.1e %8.1e',   test1,  test2 );
            str4 = sprintf( ' %8.1e %8.1e',    anorm,  acond );
            disp([str1 str2 str3 str4])
         end
      end
      if istop > 0, break, end
end

%     End of iteration loop.
%     Print the stopping condition.

if show
   disp(' ')
   disp('LSQR finished')
   disp(msg(istop+1,:))
   disp(' ')
   str1 = sprintf( 'istop =%8g   r1norm =%8.1e',   istop, r1norm );
   str2 = sprintf( 'anorm =%8.1e   arnorm =%8.1e', anorm, arnorm );
   str3 = sprintf( 'itn   =%8g   r2norm =%8.1e',     itn, r2norm );
   str4 = sprintf( 'acond =%8.1e   xnorm  =%8.1e', acond, xnorm  );
   disp([str1 '   ' str2])
   disp([str3 '   ' str4])
   disp(' ')
end

%-----------------------------------------------------------------------
% End of lsqr.m
%-----------------------------------------------------------------------

function z = Aprod(x,mode)
   if mode == 1
      if isnumeric(A), z = A*x;
      else             z = A(x,1);
      end
   else
      if isnumeric(A), z = (x'*A)';
      else             z = A(x,2);
      end
   end
end % function Aprod

end


function [x,g,j,gg] = v_kmeans(d,k,x0,l)
%V_KMEANS Vector quantisation using K-means algorithm [X,ESQ,J]=(D,K,X0,L)
%
%  Inputs:
%
%    D(N,P)  contains N data vectors of dimension P
%    K       is number of centres required
%    X0(K,P) are the initial centres (optional)
%
%      or alternatively
%
%    X0      gives the initialization method
%            'f'   pick K random elements of D as the initial centres [default]
%            'p'   randomly divide D into K sets and choose the centroids
%    L       gives max number of iterations (use 0 if you just want to calculate G and J)
%
%  Outputs:
%
%    X(K,P)  is output row vectors (omitted if L=0)
%    G       is mean square error
%    J(N)    indicates which centre each data vector belongs to
%    GG(L)   gives the mean square error at the start of each iteration (omitted if L=0)
%
% It is often a good idea to scale the input data so that it has equal variance in each
% dimension before calling V_KMEANS.

%  Originally based on a routine by Chuck Anderson, anderson@cs.colostate.edu, 1996

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

memsize=voicebox('memsize');
[n,p] = size(d);
nb=min(n,max(1,floor(memsize/(8*p*k))));    % block size for testing data points
nl=ceil(n/nb);                  % number of blocks
if nargin<4
    l=300;                  % very large max iteration count
    if nargin<3
        x0='f';             % use 'f' initialization mode
    end
end
if ischar(x0)
    if k<n
        if any(x0)=='p'                  % Initialize using a random partition
            ix=ceil(rand(1,n)*k);       % allocate to random clusters
            ix(rnsubset(k,n))=1:k;      % but force at least one point per cluster
            x=zeros(k,p);
            for i=1:k
                x(i,:)=mean(d(ix==i,:),1);
            end
        else                                % Forgy initialization: choose k random points [default]
            x=d(rnsubset(k,n),:);         % sample k centres without replacement
        end
    else
        x=d(mod((1:k)-1,n)+1,:);    % just include all points several times
    end
else
    x=x0;
end
m=zeros(n,1);           % minimum distance to a centre
j=zeros(n,1);           % index of closest centre
gg=zeros(l,1);
wp=ones(1,p);
kk=1:p;
kk=kk(ones(n,1),:);
kk=kk(:);

if l>0
    for ll=1:l                 % loop until x==y causes a break

        % find closest centre to each data point [m(:),j(:)] = distance, index

        ix=1;
        jx=n-nl*nb;
        for il=1:nl
            jx=jx+nb;        % increment upper limit
            ii=ix:jx;
            z = disteusq(d(ii,:),x,'x');
            [m(ii),j(ii)] = min(z,[],2);
            ix=jx+1;
        end
        y = x;              % save old centre list

        % calculate new centres as the mean of their assigned data values (or zero for unused centres)

        nd=full(sparse(j,1,1,k,1));         % number of points allocated to each centre
        md=max(nd,1);                       % remove zeros
        jj=j(:,wp);
        x=full(sparse(jj(:),kk,d(:),k,p))./md(:,wp);    % calculate the new means
        fx=find(nd==0);

        % if any centres are unused, assign them to data values that are not exactly on centres
        % choose randomly if there are more such points than needed

        if ~isempty(fx)
            q=find(m~=0);
            if length(q)<=length(fx)
                x(fx(1:length(q)),:)=d(q,:);
            else
                if length(fx)>1
                    [rr,ri]=sort(rand(length(q),1));
                    x(fx,:)=d(q(ri(1:length(fx))),:);
                else
                    x(fx,:) = d(q(ceil(rand(1)*length(q))),:);
                end
            end
        end

        % quit if the centres are unchanged

        gg(ll)=sum(m,1);
        if x==y
            break
        end
    end
    gg=gg(1:ll)/n;
%     ll % *** DEBUG ***
%     gg' % *** DEBUG ***
    g=gg(end);
else            % if l==0 then just calculate G and J (but rename as X and G)
    ix=1;
    jx=n-nl*nb;
    for il=1:nl
        jx=jx+nb;        % increment upper limit
        ii=ix:jx;
        z = disteusq(d(ii,:),x,'x');
        [m(ii),j(ii)] = min(z,[],2);
        ix=jx+1;
    end
    x=sum(m,1)/n;
    g=j;
end


% eeg_interp_sph_sline_test - script to test eeg_interp_sph_spline
%

clear

p = eeg_toolbox_defaults;

p.volt.path = 'e:\matlab\eeg_toolbox\eeg_example_data\';
p.volt.file = 'eeg_124ch_simulatedEMSE.txt'; % (400 rows x 125 columns)
p.volt.type = 'ascii';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% load electrode co-ordinates

p = elec_open(p);
p.elec.plot = 1;

x = p.elec.data.x;
y = p.elec.data.y;
z = p.elec.data.z;

% % create spherical electrode positions
% r = 10
% fprintf('...generating spherical interpolation points\n');
% [x,y,z] = elec_sphere_points(16,24,r);
%
% p.elec.data.x = x;
% p.elec.data.y = y;
% p.elec.data.z = z;
%
% p.elec.data.Xsp = x;
% p.elec.data.Ysp = y;
% p.elec.data.Zsp = z;
% p.elec.data.Rsp = [r r r];
%
% p.elec.n = 337;
%elec_plot(p)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% create simulated potential data

p.volt.sampleHz = 1000;
p.volt.sampleMsec = 1;
p.volt.sampleTime = 1 * p.volt.sampleMsec;
p.volt.samplePoint = 1;
p.volt.epochStart = 0;
p.volt.epochEnd = 1;

p.volt.var = [];
p.volt.timeArray = 0:1:10;
p.volt.points = 10;
p.volt.channels = length(x);
p.volt.epochStart = 0;
p.volt.epochEnd = 10;
p.volt.sweeps = 1;
p.volt.peaks = [];

p.volt.data = repmat(p.elec.data.x',10,1);

V = p.volt.data(p.volt.samplePoint,:);

p.clickTimePoint = 0;
p = eeg_contours_engine(p);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% spherical spline interpolation

FV = eeg_interp_sph_spline(V,[x y z]);

p.elec.n = length(FV.vertices);
p.elec.data.x = FV.vertices(:,1);
p.elec.data.y = FV.vertices(:,2);
p.elec.data.z = FV.vertices(:,3);
p.elec.data.Xsp = FV.vertices(:,1);
p.elec.data.Ysp = FV.vertices(:,2);
p.elec.data.Zsp = FV.vertices(:,3);

p.volt.data = repmat(FV.Cdata,10,1);

p.volt.file = 'spherical spline interpolation';

p = eeg_contours_engine(p);

return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% scd interpolation

FV = eeg_interp_sph_spline_scd(V,[x y z]);

p.elec.n = length(FV.vertices);
p.elec.data.x = FV.vertices(:,1);
p.elec.data.y = FV.vertices(:,2);
p.elec.data.z = FV.vertices(:,3);
p.elec.data.Xsp = FV.vertices(:,1);
p.elec.data.Ysp = FV.vertices(:,2);
p.elec.data.Zsp = FV.vertices(:,3);

p.volt.data = repmat(FV.Cdata,10,1);

p.volt.file = 'spherical spline scd interpolation';

p = eeg_contours_engine(p);


function [M,P,K,MU,S,LH] = ghkf_update(M,P,Y,h,R,h_param,p)
% GHKF_UPDATE - Gauss-Hermite Kalman filter update step
%
% Syntax:
%   [M,P,K,MU,S,LH] = GHKF_UPDATE(M,P,Y,h,R,param,p)
%
% In:
%   M  - Mean state estimate after prediction step
%   P  - State covariance after prediction step
%   Y  - Measurement vector.
%   h  - Measurement model function as a matrix H defining
%        linear function h(x) = H*x, inline function,
%        function handle or name of function in
%        form h(x,param)
%   R  - Measurement covariance
%   h_param - Parameters of h
%   p  - Degree of approximation (number of quadrature points)
%
% Out:
%   M  - Updated state mean
%   P  - Updated state covariance
%   K  - Computed Kalman gain
%   MU - Predictive mean of Y
%   S  - Predictive covariance Y
%   LH - Predictive probability (likelihood) of measurement.
%
% Description:
%   Perform additive form Gauss-Hermite Kalman filter (GHKF)
%   measurement update step. Assumes additive measurement
%   noise.
%
%   Function h(.) should be such that it can be given a
%   DxN matrix of N sigma Dx1 points and it returns
%   the corresponding measurements for each sigma
%   point. This function should also make sure that
%   the returned sigma points are compatible such that
%   there are no 2pi jumps in angles etc.
%
% Example:
%   h = inline('atan2(x(2,:)-s(2),x(1,:)-s(1))','x','s');
%   [M2,P2] = ghkf_update(M1,P1,Y,h,R,S);
%
% See also:
%   GHKF_PREDICT, GHRTS_SMOOTH, GH_TRANSFORM

% History:
%   Jun 18, 2009 - Initial version
%   May 24, 2010 - Fixed parameter input and description (asolin)
%   Aug 5,  2010 - Renamed from 'gh_update' to 'ghkf_update' (asolin)

% Copyright (C) 2009 Hartikainen, Särkkä, Solin
%
% $Id: gh_update.m,v 1.2 2009/07/01 06:34:41 ssarkka Exp $
%
% This software is distributed under the GNU General Public
% Licence (version 2 or later); please refer to the file
% Licence.txt, included with the software, for details.
%%

  %
  % Check that all arguments are there
  %
  if nargin < 5
     error('Too few arguments');
  end
  if nargin < 6
     h_param = [];
  end
  if nargin < 7
     p = [];
  end
  if isempty(p)
     p = 3;
  end

  %
  % Do the transform and make the update
  %
  tr_param = {p};
  [MU,S,C,X] = gh_transform(M,P,h,h_param,tr_param);
  S = S + R;
  K = C / S;
  M = M + K * (Y - MU);
  P = P - K * S * K';

  if nargout > 5
    LH = gauss_pdf(Y,MU,S);
  end


function [ weights CompMat fuzzyTFN ] = FuzzyAHP( CompMat )
%FUZZYAHP Fuzzy AHP
%   Fuzzy AHP selection algorithm
%
% AUTHOR:
%           F. Ozgur CATAK
% CREATED:
%           October, 2011

% fuzzy tfn and inverse fuzzy tfn constants
fuzzyTFN = {[1     1     1  ] 	[1      1    1  ]
            [1/2   3/4   1  ] 	[1      4/3  2  ]
            [2/3   1     3/2] 	[2/3    1    3/2]
            [1     3/2   2  ] 	[1/2    2/3  1  ]
            [3/2   2     5/2] 	[2/5    1/2  2/3]
            [2     5/2   3  ] 	[1/3    2/5  1/2]
            [5/2   3     7/2] 	[2/7    1/3  2/5]
            [3     7/2   4  ] 	[1/4    2/7  1/3]
            [7/2   4     9/2] 	[2/9    1/4  2/7]};

fuzzyCompMatCell={};

%%
% convert ordinal numbers to
% triangular fuzzy number using fuzzyTFN matrix
[m n] = size(CompMat);

for i=1:m
    for j=i+1:m
       CompMat(j,i) = 1 / CompMat(i,j);
    end
end

for i=1:m
    for j=1:n
        criteria = CompMat(i,j);
        if criteria >= 1
           fuzzyCompMatCell{i,j} = fuzzyTFN{ criteria ,1 };
        else
           fuzzyCompMatCell{i,j} = fuzzyTFN{ round(criteria^-1) ,2 };
        end
    end
end

%%
% find sum of every l,m,u values for triangular fuzzy number
for i=1:m
    vec = [fuzzyCompMatCell{i,:}];
    mExtendAnalysis{1,i} = sum(reshape(vec,3,[])');
end

vec = [mExtendAnalysis{1,:}];
mExtendAnalysisSum = sum(reshape(vec,3,[])');

for i=1:m
    vec = [mExtendAnalysis{1,i}];
    for j=1:3
        val = mExtendAnalysisSum(1,j);
        %valSum(1,j) = val*vec(1,j);
        valSum(1,j) = (vec(1,j))*(1/val);
    end
    mExtendAnalysis{1,i} = valSum;
end

%%
% degree of possibility calculation
%              /---
%              | 1    if m2>=m1
%              |
%              | 0    if l1>=l2
% V(M2>=M1) = <
%              |     l1-u2
%              | --------------- otherwise
%              | (m1-u2)-(m1-l1)
%              \---
degreeOfPossibility = zeros(m*(m-1),3);
rowIndex = 1;
for i=1:m
    for j=1:m
        if i~=j
            degreeOfPossibility(rowIndex,[1 2]) = [i j];
            M1 = mExtendAnalysis{1,i};
            M2 = mExtendAnalysis{1,j};
            if M1(1,2) >= M2(1,2)
                degreeOfPossibility(rowIndex,3) = 1;
            elseif M2(1,1) >= M1(1,3)
                degreeOfPossibility(rowIndex,3) = 0;
            else
                degreeOfPossibility(rowIndex,3) = (M2(1,1)-M1(1,3))/((M1(1,2)-M1(1,3))-(M2(1,2)-M2(1,1)));
            end
            rowIndex = rowIndex + 1;
        end
    end
end
%%
% normalized weight calculation
weights = zeros(1,m);
for i=1:m,
    weights(1,i) = min(degreeOfPossibility([find(degreeOfPossibility(:,1) == i)], [3]));
end
weights = weights/sum(weights);

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   PARAMETERS Returns a data structure containing the parameters of an example serial arm
%
%   Author: Arturo Gil. Universidad Miguel Hernandez de Elche.
%   email: arturo.gil@umh.es date:   20/11/2013
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 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 robot = parameters()

robot.name= 'Serial example';

robot.DH.theta= '[ ]';
robot.DH.d='[]';
robot.DH.a='[]';
robot.DH.alpha= '[]';

robot.J=[];

robot.inversekinematic_fn = 'inversekinematic(robot, T)';

%number of degrees of freedom
robot.DOF = 6;

%rotational: 0, translational: 1
robot.kind=['R' 'R' 'R' 'R' 'R' 'R'];

%minimum and maximum rotation angle in rad
robot.maxangle =[deg2rad(-165) deg2rad(165); %Axis 1, minimum, maximum
                deg2rad(-110) deg2rad(110); %Axis 2, minimum, maximum
                deg2rad(-110) deg2rad(70); %Axis 3
                deg2rad(-160) deg2rad(160); %Axis 4:
                deg2rad(-120) deg2rad(120); %Axis 5
                deg2rad(-400) deg2rad(400)]; %Axis 6:

%maximum absolute speed of each joint rad/s or m/s
robot.velmax = [deg2rad(250); %Axis 1, rad/s
                deg2rad(90); %Axis 2, rad/s
                deg2rad(90); %Axis 3, rad/s
                deg2rad(150); %Axis 4, rad/s
                deg2rad(120); %Axis 5, rad/s
                deg2rad(190)];%Axis 6, rad/s

robot.accelmax=robot.velmax/0.1; % 0.1 is here an acceleration time
            % end effectors maximum velocity
robot.linear_velmax = 1.0; %m/s, unavailable from datasheet

%base reference system
robot.T0 = eye(4);

%INITIALIZATION OF VARIABLES REQUIRED FOR THE SIMULATION
%position, velocity and acceleration
robot=init_sim_variables(robot);
robot.path = pwd;

% GRAPHICS
robot.graphical.has_graphics=1;
robot.graphical.color = [255 102 51]./255;
%for transparency
robot.graphical.draw_transparent=0;
%draw DH systems
robot.graphical.draw_axes=1;
%DH system length and Font size, standard is 1/10. Select 2/20, 3/30 for
%bigger robots
robot.graphical.axes_scale=1;
%adjust for a default view of the robot
robot.axis=[-1 1 -1 1 0 1.5];
%read graphics files
robot = read_graphics(robot);

%DYNAMICS
robot.has_dynamics=0;


function [hPa] = MPa2hPa(MPa)
% Convert pressure from megapascals to hectopascals.
% Chad Greene 2012
hPa = MPa*10000.0;


%% Setting of the problem
global s
pde = fracLapdata9;
% pde = checkboarddata;
pde.L = 1;
option.maxIt = 4;
option.maxN = 1e6;
option.elemType = 'P1P1';
option.solver = 'mg';
option.gNquadorder = 4;
option.tol = 1e-6;
[node,elem] = squaremesh([-1 1 -1 1],0.5);
bdFlag = setboundary(node,elem,'Dirichlet');

%% s = 0.2
s = 0.2;
femfracLap(node,elem,pde,bdFlag,option);

%% s = 0.4
s = 0.4;
femfracLap(node,elem,pde,bdFlag,option);

%% s = 0.6
s = 0.6;
femfracLap(node,elem,pde,bdFlag,option);

%% s = 0.8
s = 0.8;
femfracLap(node,elem,pde,bdFlag,option);


function d = dell2nose_pair(trx,fly1,fly2,istry)

nsamples = 20;

% initialize
d = nan(1,trx(fly1).nframes);

% get start and end frames of overlap
t0 = max(trx(fly1).firstframe,trx(fly2).firstframe);
t1 = min(trx(fly1).endframe,trx(fly2).endframe);

% no overlap
if t1 < t0,
  return;
end

% position of nose2
xnose = trx(fly2).x_mm + 2*trx(fly2).a_mm.*cos(trx(fly2).theta_mm);
ynose = trx(fly2).y_mm + 2*trx(fly2).a_mm.*sin(trx(fly2).theta_mm);

% ellipse 1
x_mm1 = trx(fly1).x_mm;
y_mm1 = trx(fly1).y_mm;
a_mm1 = trx(fly1).a_mm;
b_mm1 = trx(fly1).b_mm;
theta_mm1 = trx(fly1).theta_mm;

off1 = trx(fly1).off;
off2 = trx(fly2).off;

if nargin < 4,
  tstry = t0:t1;
else
  tstry = istry(:)' - off1;
end

for t = tstry,
  i = t + off1;
  j = t + off2;
  d(i) = ellipsedist_hack(x_mm1(i),y_mm1(i),...
    2*a_mm1(i),2*b_mm1(i),theta_mm1(i),...
    xnose(j),ynose(j),nsamples);
end


function [KGain,XNEW,PT1,PT2,PT3,YNEW] = MSMTUPDT(Xinew1,Xinew2,Xinew3,Yinew1,Yinew2,Yinew3,Xbark,Ybark,Pbark,Wco,Wci,Wcin,XNEW,VTt);

PYbarkYbark1 = Wco*(Yinew1 - Ybark)*(Yinew1 - Ybark)';
PYbarkYbark2 = Wci*(Yinew2 - Ybark)*(Yinew2 - Ybark)';
PYbarkYbark3 = Wcin*(Yinew3 - Ybark)*(Yinew3 - Ybark)';

PXbarkYbark1 = Wco*(Xinew1 - Xbark)*(Yinew1 - Ybark)';
PXbarkYbark2 = Wci*(Xinew2 - Xbark)*(Yinew2 - Ybark)';
PXbarkYbark3 = Wcin*(Xinew3 - Xbark)*(Yinew3 - Ybark)';

DENUP = sum(sum(PYbarkYbark1) + sum(PYbarkYbark2) + sum(PYbarkYbark3));
NUMUP = sum(sum(PXbarkYbark1) + sum(PXbarkYbark2) + sum(PXbarkYbark3));

KGain = NUMUP/DENUP;

YNEW = (sin(XNEW)).^2 + exp(VTt);
XNEW = Xbark + KGain*(YNEW - Ybark);

PT1 = Pbark - KGain*PYbarkYbark1*KGain;
PT2 = Pbark - KGain*PYbarkYbark2*KGain;
PT3 = Pbark - KGain*PYbarkYbark3*KGain;


function [J, Ybus, Yf, Yt] = makeJac(baseMVA, bus, branch, gen, fullJac)
%MAKEJAC  Forms the power flow Jacobian.
%   J = MAKEJAC(MPC)
%   J = MAKEJAC(MPC, FULLJAC)
%   J = MAKEJAC(BASEMVA, BUS, BRANCH, GEN)
%   J = MAKEJAC(BASEMVA, BUS, BRANCH, GEN, FULLJAC)
%   [J, YBUS, YF, YT] = MAKEJAC(MPC)
%
%   Returns the power flow Jacobian and, optionally, the system admittance
%   matrices. Inputs can be a MATPOWER case struct or individual BASEMVA,
%   BUS, BRANCH and GEN values. Bus numbers must be consecutive beginning
%   at 1 (i.e. internal ordering). If the FULLJAC argument is present and
%   true, it returns the full Jacobian (sensitivities of all bus injections
%   w.r.t all voltage angles/magnitudes) as opposed to the reduced version
%   used in the Newton power flow updates. The units for all quantities are
%   in per unit with radians for voltage angles.
%
%   Note: This function builds the Jacobian from scratch, rebuilding the
%         YBUS matrix in the process. You probably don't want to use this
%         in performance critical code.
%
%   See also MAKEYBUS, EXT2INT

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

if nargin < 4
    mpc     = baseMVA;
    if nargin > 1
        fullJac = bus;
    else
        fullJac = 0;
    end
    baseMVA = mpc.baseMVA;
    bus     = mpc.bus;
    branch  = mpc.branch;
    gen     = mpc.gen;
elseif nargin < 5
    fullJac = 0;
end

%% define named indices into bus, gen, branch matrices
[PQ, PV, REF, NONE, BUS_I, BUS_TYPE, PD, QD, GS, BS, BUS_AREA, VM, ...
    VA, BASE_KV, ZONE, VMAX, VMIN, LAM_P, LAM_Q, MU_VMAX, MU_VMIN] = idx_bus;
[GEN_BUS, PG, QG, QMAX, QMIN, VG, MBASE, GEN_STATUS, PMAX, PMIN, ...
    MU_PMAX, MU_PMIN, MU_QMAX, MU_QMIN, PC1, PC2, QC1MIN, QC1MAX, ...
    QC2MIN, QC2MAX, RAMP_AGC, RAMP_10, RAMP_30, RAMP_Q, APF] = idx_gen;

%% build Ybus
[Ybus, Yf, Yt] = makeYbus(baseMVA, bus, branch);

%% extract voltage
V = bus(:, VM) .* exp(1j * pi/180 * bus(:, VA));

%% make sure we use generator setpoint voltage for PV and slack buses
on = find(gen(:, GEN_STATUS) > 0);      %% which generators are on?
gbus = gen(on, GEN_BUS);                %% what buses are they at?
k = find(bus(gbus, BUS_TYPE) == PV | bus(gbus, BUS_TYPE) == REF);
V(gbus(k)) = gen(on(k), VG) ./ abs(V(gbus(k))).* V(gbus(k));

%% build Jacobian
[dSbus_dVa, dSbus_dVm] = dSbus_dV(Ybus, V);
if fullJac
    j11 = real(dSbus_dVa);
    j12 = real(dSbus_dVm);
    j21 = imag(dSbus_dVa);
    j22 = imag(dSbus_dVm);
else
    %% get bus index lists of each type of bus
    [ref, pv, pq] = bustypes(bus, gen);

    j11 = real(dSbus_dVa([pv; pq], [pv; pq]));
    j12 = real(dSbus_dVm([pv; pq], pq));
    j21 = imag(dSbus_dVa(pq, [pv; pq]));
    j22 = imag(dSbus_dVm(pq, pq));
end

J = [   j11 j12;
        j21 j22;    ];


function y = vl_nnglobalpool(x, varargin)
%VL_NNGLOBALPOOL CNN global poolinng.
%   Y = VL_NNGLOBALPOOL(X) applies the pooling operator to all
%   spatial locations of the data X. X is a SINGLE array of dimension
%   H x W x C x N where (H,W) are the height and width of the map stack,
%   C is the number of feature channels and N the number of of images
%   in the batch.
%
%   DZDX = VL_NNGLOBALPOOL(X, POOL, DZDY) computes the derivatives of the
%   block projected onto DZDY. DZDX and DZDY have the same dimensions
%   as X and Y respectively.
%
%   VL_NNGLOBALPOOL(..., 'option', value, ...) takes the following option:
%
%   `method`:: 'avg'
%     Specify method of pooling. It can be either 'max' (retain max value
%     over all spatial locations per channel) or 'avg' (compute the average
%     value over all spatial locations per channel).
%
%   The output a is a SINGLE array of dimensions 1 x 1 x C x N.
%
%   The derivative DZDY has the same dimension of the output Y and
%   The derivative DZDX has the same dimension as the input X.
%
% Copyright (C) 2016 Samuel Albanie and Andrea Vedaldi
% Licensed under The MIT License [see LICENSE.md for details]

  opts.method = 'avg' ;
  [opts, dzdy] = vl_argparsepos(opts, varargin) ;

  if nargin <= 1 || isempty(dzdy)
    switch opts.method
      case 'avg', y = mean(mean(x, 1), 2) ;
      case 'max', y = max(max(x, [], 1), [], 2) ;
      otherwise, error('Pooling method %s not recognized', opts.method) ;
    end
  else
    base = 1 / (size(x,1) * size(x,2)) * ones(size(x), 'like', x) ;
    y = bsxfun(@times, base, dzdy{1}) ;
  end


function [n,edges,nbedges,xechan] = hist_ic(x,crit)

%HIST_IC  optimal Histogram based on IC information criterion
%
%   [N,EDGES,NBEDGES,XECHAN] = HIST-IC(X,CRIT)
%	bins the elements of X into an optimal number of bins according
%	to a cost function based on Akaike's Criterion.
%
%
%   CRIT = 1 | 2 | 3  (choose one of the 3 possible criterium)  (default 3)
%          4          (returns the initial histogram instead of the optimal one)
%
%
%   N = cell array containing the distribution of each column of X
%   (or a vector if X is a column vector)
%   EDGES = cell array containing the bin edges of each column of X
%   (or a vector if X is a column vector)
%   NBEDGES = vector containing the number of bin edges for each column of X
%   (or a number if X is a column vector)
%   XECHAN = discretized version of X
%
%   Ref : O. Colot et al., Information Criteria and Abrupt Changes in
%         Probability Laws, Signal Processing VII: Theory and Applications
%	  pp.1855-18858, September 1994
%
%   F. El-Matouat, O. Colot 2000 (first version)
%   Revised 01-06-2001 by Ph. Leray - philippe.leray@univ-nantes.fr
%
%
%   Things to do :
%	* Call criteron by a name ('aic','xxx', ...) instead of a number
%

if nargin == 0
    error('Requires one or two arguments.')
end

if nargin == 1
    crit = 3;
end;

if min(size(x))==1, x = x(:); end

if isstr(x)
    error('Input argument must be numeric.')
end

if isempty(x),
	error('No elements to count')
end

[nb_l,nb_c]=size(x);

% Outputs declaration
xechan=zeros(nb_l,nb_c);

edges=cell(nb_c,1);
% Local variables
maxi = max(x);
mini = min(x);

%% Erreur ? ancien code :
%% nb_clas_ini=2*round(sqrt(nb_l)-1);	% article Fatima

nb_clas_ini=round(2*sqrt(nb_l)-1);

pas_ini=(maxi-mini)/nb_clas_ini;	% initial step

for j=1:nb_c,

	% optimal histogram for each column of X
	histo_ini =hist(x(:,j),nb_clas_ini);		% initial histogram

	if (crit~=4)
		[hist_opt,pas_opt]=hist1_ic(histo_ini,nb_l,pas_ini(j),nb_clas_ini,crit);
	else
		fprintf('Histo initial\n');
		hist_opt=histo_ini;
		pas_opt=ones(1,nb_clas_ini)*pas_ini(j);
	end;
	nbedges(j)=size(hist_opt,2);
	edges{j}=mini(j)+cumsum(pas_opt(1:nbedges(j)-1)); %+1e-7;
        [n{j} xechan(:,j)]=histc(x(:,j),[-inf edges{j} inf]);
	n{j}=n{j}(1:end-1);
end

if (nb_c==1)
	n=n{1}; edges=edges{1};
end

% ============================== subfunctions

function [hist_opt,step_opt]=hist1_ic(histo,nb,step_ini,m,critere);

%HIST1_IC  optimal Histogram based on IC information criterion
%
%   [HIST_OPT, STEP_OPT] = HIST1_IC(HISTO, NB, STEP_INI, NBSTEP_INI, CRIT)
%       fusion of an 1D histogramme (HISTO) according to an IC criterion (CRIT)
%
%   This function is mainly an internal function used by HIST_IC
%
%   Ref : O. Colot et al., Information Criteria and Abrupt Changes in
%         Probability Laws, Signal Processing VII: Theory and Applications
%         pp.1855-18858, September 1994
%
%   F. El-Matouat, O. Colot 2000 (first version)
%   Revised 11-06-2001 by Ph. Leray
%
%
%   Things to do :
%       * Call criteron by a name ('aic','xxx', ...) instead of a number
%

aic=[];
aic2=[];

% Initialisation
histt=histo;
teta = histt/nb;
pas = step_ini*ones(1,m);

% Calcul de l'ensemble des histogrammes optimaux

for z=1:m

	% Calcul de AIC pour l'union entre hist(indice,u) et hist(indice,u+1)
	aic2 = [aic2 cal_aic(nb,teta,pas,m+1-z,critere)];

	if (z~=m)
		% Calcul des couples de classes adjacentes
		if critere==1
			penalite=(2*(m-z)-1)/nb;
		elseif critere==2
			penalite=(m-z-1)*(1+log(nb))/nb;
		else
			penalite=(m-z)*(1+log(log(nb)))/nb;
		end
 		aic=cla_adj(nb,teta,pas,step_ini,m-z+1,histt,penalite,aic);
		% Recherche de la valeur min du crit�re pour les classes adjacentes
		[min_aic classe]=min(aic(1:(m-z)));

		% Fusion de hist(classe) et hist(classe+1)
		nb_pas1=pas(classe)/step_ini;
		nb_pas2=pas(classe+1)/step_ini;

		ess=round( nb_pas1*histt(classe)+nb_pas2*histt(classe+1) );
		teta(classe)=ess / nb;
		histt(classe)=ess / (nb_pas1+nb_pas2);
		pas(classe)=pas(classe)+pas(classe+1);

		% Cr�ation du nouvel histogramme
		itemp = setdiff(1:m+1-z,classe+1);
		histt = histt(itemp);
		pas = pas(itemp);
		teta = teta(itemp);
	end
end

% Recherche du crit�re minimun AIC
[min_AIC fusion]=min(aic2(1:m));

% Initialisation de histo
histt=histo;
teta = histt/nb;
pas = step_ini*ones(1,m);

% Calcul de l'histogramme optimal

for z=1:fusion-1

	% Calcul des couples de classes adjacentes
	if critere==1
		penalite=(2*(m-1)-1)/nb;
	elseif critere==2
		penalite=(m-2)*(1+log(nb))/nb;
	else
		penalite=(m-1)*(1+log(log(nb)))/nb;
	end

	aic=cla_adj(nb,teta,pas,step_ini,m,histt,penalite,aic);

	% Recherche de la valeur min du crit�re pour les classes adjacentes
	[min_aic classe]=min(aic(1:m-1));

	% Fusion de hist(indice,classe) et hist(indice,classe+1)

	nb_pas1=pas(classe)/step_ini;
	nb_pas2=pas(classe+1)/step_ini;

	teta(classe)=(round(nb_pas1*histt(classe)+nb_pas2*histt(classe+1)))/nb;
	histt(classe)=(nb_pas1*histt(classe)+nb_pas2*histt(classe+1))/(nb_pas1+nb_pas2);
	pas(classe)=pas(classe)+pas(classe+1);

	% Cr�ation du nouvel histogramme

	itemp=setdiff(1:m,classe+1);
	histt = histt(itemp);
	pas = pas(itemp);
	teta = teta(itemp);
	%aic=zeros(1,m-1);

	m=m-1;
end
hist_opt=histt;
step_opt=pas;

%=====================================================
% Calcul du Critere pour l'ensemble des classes

function akaike=cal_aic(size_ech,teta,pas,m,critere);

if critere==1
	a=(2*m-1)/size_ech;
elseif critere==2
	a=(m-1)*(1+log(size_ech))/size_ech;
else
	a=m*(1+log(log(size_ech)))/size_ech;
end

indu = find(teta);
akaike = a - 2*sum(teta(indu).*log(teta(indu)./pas(indu)));

%=====================================================
% Cla_adj.m
% aic=cla_adj(taille,indice,teta,pas,pas_ini,m,hist,penalite,aic)
% taille=nombre d'�l�ments dans chacune des hypotheses;
% indice=numero de la classe;
% Calcul du critere de Akaike pour l'histogramme totale avec
% fusion de deux classes adjacentes u et (u+1).

function aic=cla_adj(size_ech,teta,pas,pas_ini,m,hist,penalite,aic);

for u=1:m-1

	cumul=0;

	% This loop is faster than a sum of a vectorised computation !
	for x=1:m
		if x~=u & x~=u+1 & teta(x)~=0
			cumul=cumul+teta(x)*log(teta(x)/pas(x));
		end
	end

	nb_pas1=pas(u)/pas_ini;
	nb_pas2=pas(u+1)/pas_ini;

	b=( round(nb_pas1*hist(u)+nb_pas2*hist(u+1) ) ) / size_ech;

	if b~=0
		c=2*b*log( b / ( pas(u) + pas(u+1) ) );
	else
		c=0;
	end

	aic(u)=penalite-2*cumul-c;
end


%FASTHOUGHTRANSFORM  Calculates 2D Fast Hough transform of an image
%
%     dst = cv.FastHoughTransform(src)
%     dst = cv.FastHoughTransform(src, 'OptionName',optionValue, ...)
%
% ## Input
% * __src__ The source (input) image.
%
% ## Output
% * __dst__ The destination image, result of transformation.
%
% ## Options
% * __DDepth__ The depth of destination image. Default `int32`.
% * __Op__ The operation to be applied. This specifies binary operations, that
%   is such ones which involve two operands. Formally, a binary operation `f`
%   on a set `S` is a binary relation that maps elements of the Cartesian
%   product `SxS` to `S`: `f: SxS -> S`. Default 'Addition'. One of
%   * __Minimum__ Binary minimum operation. The constant specifies the binary
%     minimum operation `f` that is defined as follows: `f(x, y) = min(x, y)`.
%   * __Maximum__ Binary maximum operation. The constant specifies the binary
%     maximum operation `f` that is defined as follows: `f(x, y) = max(x, y)`.
%   * __Addition__ Binary addition operation. The constant specifies the binary
%     addition operation `f` that is defined as follows: `f(x, y) = x + y`.
%   * __Average__ Binary average operation. The constant specifies the binary
%     average operation `f` that is defined as follows: `f(x, y) = (x + y)/2`.
% * __AngleRange__ The part of Hough space to calculate. Each member specifies
%   primarily direction of lines (horizontal or vertical) and the direction of
%   angle changes. Direction of angle changes is from multiples of 90 to odd
%   multiples of 45. The image considered to be written top-down and
%   left-to-right. Angles are started from vertical line and go clockwise.
%   Separate quarters and halves are written in orientation they should be in
%   full Hough space. Default `ARO_315_135`. One of:
%   * **ARO_0_45** Vertical primarily direction and clockwise angle changes.
%   * **ARO_45_90** Horizontal primarily direction and counterclockwise angle
%     changes.
%   * **ARO_90_135** Horizontal primarily direction and clockwise angle
%     changes.
%   * **ARO_315_0** Vertical primarily direction and counterclockwise angle
%     changes.
%   * **ARO_315_45** Vertical primarily direction.
%   * **ARO_45_135** Horizontal primarily direction.
%   * **ARO_315_135** Full set of directions.
%   * **ARO_CTR_HOR** `90 +/- atan(0.5)`, interval approximately from `64.5`
%     to `116.5` degrees. It is used for calculating Fast Hough Transform for
%     images skewed by `atan(0.5)`.
%   * **ARO_CTR_VER** `0 +/- atan(0.5)`, interval approximately from `333.5`
%     (`-26.5`) to `26.5` degrees. It is used for calculating Fast Hough
%     Transform for images skewed by `atan(0.5)`.
% * __MakeSkew__ Specifies to do or not to do skewing of Hough transform
%   image. The enum specifies to do or not to do skewing of Hough transform
%   image so it would be no cycling in Hough transform image through borders
%   of image. Default 'Deskew'. One of:
%   * __Raw__ Use raw cyclic image.
%   * __Deskew__ Prepare deskewed image.
%
% The function calculates the fast Hough transform for full, half or quarter
% range of angles.
%
% See also: cv.HoughPoint2Line, cv.HoughLines, hough, houghlines, houghpeaks
%


function s32 = i4_to_s32 ( i4 )

%*****************************************************************************80
%
%% I4_TO_S32 converts an I4 to an S32.
%
%  Discussion:
%
%    An I4 is a 32 bit integer.
%
%    An S32 is a 32 character string.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer I4, the integer to be coded.
%
%    Output, string S32, the string.
%
  s32 = [];

  i4_copy = abs ( i4 );
%
%  Binary digits:
%
  for i = 32 : -1 : 2

    if ( mod ( i4_copy, i4_two ) == 1 )
      s32 = strcat ( '1', s32 );
    else
      s32 = strcat ( '0', s32 );
    end

    i4_copy = floor ( i4_copy / 2 );

  end
%
%  Sign bit
%
  s32 = strcat ( '0', s32 );
%
%  If original number was negative, then reverse all bits.
%
  if ( i4 < 0 )
    for i = 1 : 32
      if ( s32(i) == '0' )
        s32(i) = '1';
      else
        s32(i) = '0';
      end
    end
  end

  return
end


function bpab_test ( )

%*****************************************************************************80
%
%% BPAB_TEST tests BPAB.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 October 2008
%
%  Author:
%
%    John Burkardt
%
  n = 10;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'BPAB_TEST\n' );
  fprintf ( 1, '  BPAB evaluates Bernstein polynomials.\n' );
  fprintf ( 1, '\n' );

  x = 0.3;
  a = 0.0;
  b = 1.0;
  bern = bpab ( n, x, a, b );

  fprintf ( 1, '  The Bernstein polynomials of degree %d\n', n );
  fprintf ( 1, '  based on the interval from %f\n', a );
  fprintf ( 1, '  to %f\n', b );
  fprintf ( 1, '  evaluated at X = %f\n', x );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   I        Bern(I,X)\n' );
  fprintf ( 1, '\n' );

  for i = 0 : n
    fprintf ( 1, '  %2d  %12f\n', i, bern(i+1) );
  end

  return
end


function days = month_length_common ( y, m )

%*****************************************************************************80
%
%% MONTH_LENGTH_COMMON returns the number of days in a Common month.
%
%  Discussion:
%
%    The "common" calendar is meant to be the calendar which is Julian up to
%    day JED = 2299160, and Gregorian from day JED = 2299161 and after.
%
%    The routine knows that February has 28 days, except in leap years,
%    when it has 29.
%
%    In the Common calendar, October 1582 had only 21 days
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 June 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, the year in which the month occurred.
%
%    Input, integer M, the number of the month.
%
%    Output, integer DAYS, the number of days
%    in the month.
%
  mdays = [ 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 ];
%
%  Check the input.
%
  [ y2, m2, ierror ] = ym_check_common ( y, m );

  if ( ierror ~= 0 )
    days = 0;
    return
  end
%
%  Take care of the special case.
%
  if ( y2 == 1582 )
    if ( m2 == 10 )
      days = 21;
      return
    end
  end
%
%  Get the number of days in the month.
%
  days = mdays(m2);
%
%  If necessary, add 1 day for February 29.
%
  if ( m2 == 2 && year_is_leap_common ( y2 ) )
    days = days + 1;
  end

  return
end


% VL_LBP  Local Binary Patterns
%   F = VL_LBP(IM, CELLSIZE) computes the Local Binary Pattern (LBP)
%   features for image I.
%
%   IM is divided in cells of size CELLSIZE. F is a three-dimensional
%   array containing one histograms of quantized LBP features per
%   cell. The witdh of F is FLOOR(WIDTH/CELLSIZE), where WIDTH is the
%   width of the image. The same for the height. The third dimension
%   is 58.
%
%   See also: LBP, VL_LBPFLIPLR(),
%   VL_HELP().

% Copyright (C) 2013 Andrea Vedaldi.
% Copyright (C) 2010-11 Andrea Vedaldi.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).


function CrowdDis = CrowdingDistance(PopObj,FrontNo)
%CrowdingDistance - Calculate the crowding distances of solutions front
%by front.
%
%   CD = CrowdingDistance(F) calculates the crowding distances of solutions
%   according to their objective values in F.
%
%   CD = CrowdingDistance(F,FrontNo) calculates the crowding distances of
%   solutions in each non-dominated front, where FrontNo is the front
%   numbers of solutions.
%
%   Example:
%       CrowdDis = CrowdingDistance(PopObj,FrontNo)

%------------------------------- Reference --------------------------------
% S. Kukkonen and K. Deb, Improved pruning of non-dominated solutions based
% on crowding distance for bi-objective optimization problems, Proceedings
% of the IEEE Congress on Evolutionary Computation, 2006, 1179-1186.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    [N,M] = size(PopObj);
    if nargin < 2
        FrontNo = ones(1,N);
    end
    CrowdDis = zeros(1,N);
    Fronts   = setdiff(unique(FrontNo),inf);
    for f = 1 : length(Fronts)
        Front = find(FrontNo==Fronts(f));
        Fmax  = max(PopObj(Front,:),[],1);
        Fmin  = min(PopObj(Front,:),[],1);
        for i = 1 : M
            [~,Rank] = sortrows(PopObj(Front,i));
            CrowdDis(Front(Rank(1)))   = inf;
            CrowdDis(Front(Rank(end))) = inf;
            for j = 2 : length(Front)-1
                CrowdDis(Front(Rank(j))) = CrowdDis(Front(Rank(j)))+(PopObj(Front(Rank(j+1)),i)-PopObj(Front(Rank(j-1)),i))/(Fmax(i)-Fmin(i));
            end
        end
    end
end


%FIGURE_Critical_Angle

MoI = logspace(-3,0,100);
Mu = [0,0.05,0.1,0.5,1,5,10,inf];
P.m = 1;
P.g = 1;
P.L = 1;

Data = cell(length(Mu),0);
for i=1:length(Mu)
    Data(i).idxF = false(size(MoI));
    Data(i).idxB = false(size(MoI));
    Data(i).fail = false(size(MoI));
    Data(i).th = zeros(size(MoI));
    P.u = Mu(i);
    disp(['Running u = ' num2str(P.u)]);
    for j=1:length(MoI)
        P.I = MoI(j);
        C = ToppleFromRest(P);
        if ~isempty(C.th)
            if strcmp(C.exit,'SlipForwards')
               Data(i).idxF(j) = true;
            elseif strcmp(C.exit,'SlipBackwards')
                Data(i).idxB(j) = true;
            end
            Data(i).th(j) = C.th;
        else
            Data(i).fail(j) = true;
        end
    end
end

style = {'k--','b--','r--','m--','g--',...
    'k-','b-','r-','m-','g-'...
    'k:','b:','r:','m:','g:'};
LINEWIDTH = 3;
FontSize.Title = 16;
FontSize.label = 12;
names = cell(1,length(Mu));
for i=1:length(Mu)
    names{i} = num2str(['u = ' num2str(Mu(i))]);
end

figH = figure(400); clf;
set(figH,'Name','CriticalAngleLim','NumberTitle','off')
IDX = false(2,length(Mu));
for i=1:length(Mu)
    sty = style{mod(i-1,length(style))+1};
    idx = Data(i).idxB;  IDX(1,i) = sum(idx)~=0;
    subplot(2,1,1);  hold on;
    semilogx(MoI(idx),Data(i).th(idx)*180/pi,sty,'LineWidth',LINEWIDTH);
    idx = Data(i).idxF;  IDX(2,i) = sum(idx)~=0;
    subplot(2,1,2);  hold on;
    semilogx(MoI(idx),Data(i).th(idx)*180/pi,sty,'LineWidth',LINEWIDTH);
end

subplot(2,1,1);
 title('Backwards Slip (falling forward)','FontSize',FontSize.Title)
    xlabel('Moment of Inertia','FontSize',FontSize.label)
    ylabel('Critical Angle (deg)','FontSize',FontSize.label)
    legend(names(IDX(1,:)),'Location','NorthEast');
    set(gca,'Xscale','log')

    subplot(2,1,2);
 title('Forwards Slip (falling forward)','FontSize',FontSize.Title)
    xlabel('Moment of Inertia','FontSize',FontSize.label)
    ylabel('Critical Angle (deg)','FontSize',FontSize.label)
    legend(names(IDX(2,:)),'Location','NorthWest');
    set(gca,'Xscale','log')

save('DATA_Critical_Fall.mat','Data');
save2pdf('../WriteUp/Figures/Critical_Fall.pdf',figH,600);

%Later on, compute the acceleration of the tip at the onset of sliding.


function test_sep
%TEST_SEP test cs_sep, and compare with Gilbert's meshpart vtxsep
% (requires MESHPART).
%
% Example:
%   test_sep
%
% See also: testall

%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

clear functions

index = UFget ;
[ignore f] = sort (max (index.nrows, index.ncols)) ;

clf

for k = 1:length(f)

    i = f (k) ;
    Prob = UFget (i) ;
    disp (Prob) ;
    A = spones (Prob.A) ;
    [m n] = size (A) ;
    if (m ~= n)
        A = A'*A ;
    end

    A = A|A' ;

    p = symrcm (A) ;

    n = size (A,1) ;
    n2 = fix (n/2) ;
    a = p (1:n2) ;
    b = p ((n2+1):n) ;

    clf

    subplot (2,3,1) ; spy (A) ;
    subplot (2,3,2) ; spy (A (p,p)) ;

    hold on
    plot ([.5 n2+.5 n2+.5 .5 .5], [.5 .5 n2+.5 n2+.5 .5], 'r', 'LineWidth', 2) ;
    hold off

    subplot (2,3,3) ; spy (A (a,b)) ; title ('edge sep') ;
    subplot (2,3,6) ; cs_dmspy (A (a,b)) ; title ('node sep') ;

    [s as bs] = vtxsep (A,a,b) ;                                        %#ok
    [s2 a2 b2] = cs_sep (A,a,b) ;

    p2 = [a2 b2 s2] ;
    B = A (p2,p2) ;
    subplot (2,3,5) ; spy (B) ;
    hold on

    px = [s2 a2 b2] ;
    if (any (sort (px) ~= 1:n))
        px      %#ok
        n       %#ok
        error ('!') ;
    end

    na = length (a2) ;
    nb = length (b2) ;
    ns = length (s2) ;                                                  %#ok

    nab = na + nb ;

    plot ([.5 na+.5 na+.5 .5 .5], [.5 .5 na+.5 na+.5 .5], 'r', 'LineWidth', 2) ;

    plot ([na nab nab na na]+0.5, [na na nab nab na]+0.5, 'r', 'LineWidth', 2) ;

    plot ([.5 nab+.5 nab+.5 .5 .5], [.5 .5 nab+.5 nab+.5 .5], 'g', 'LineWidth', 1) ;

    hold off

    nz1 = nnz (A (a2,b2)) ;
    if (nz1 ~= 0)
        nz1     %#ok
        error ('!') ;
    end

    nz2 = nnz (A (a2,b2)) ;
    if (nz2 ~= 0)
        nz2     %#ok
        error ('!') ;
    end

    if (length (s) ~= length (s2))
        fprintf ('lengths differ: %d %d\n', length (s), length (s2)) ;
    end

    drawnow
    % pause

end


function [ less, equal, more ] = r8r8r8vec_index_search ( n, x, y, z, ...
  indx, xval, yval, zval )

%*****************************************************************************80
%
%% R8R8R8VEC_INDEX_SEARCH searches for an R8R8R8 value in an indexed sorted list.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the size of the current list.
%
%    Input, real X(N), Y(N), Z(N), the list.
%
%    Input, integer INDX(N), the sort index of the list.
%
%    Input, real XVAL, YVAL, ZVAL, the value to be sought.
%
%    Output, integer LESS, EQUAL, MORE, the indexes in INDX of the
%    entries of X that are just less than, equal to, and just greater
%    than XVAL.  If XVAL does not occur in X, then EQUAL is zero.
%    If XVAL is the minimum entry of X, then LESS is 0.  If XVAL
%    is the greatest entry of X, then MORE is N+1.
%
  if ( n <= 0 )
    less = 0;
    equal = 0;
    more = 0;
    return
  end

  lo = 1;
  hi = n;

  xlo = x(indx(lo));
  ylo = y(indx(lo));
  zlo = z(indx(lo));

  xhi = x(indx(hi));
  yhi = y(indx(hi));
  zhi = z(indx(hi));

  compare = r8r8r8_compare ( xval, yval, zval, xlo, ylo, zlo );

  if ( compare == -1 )
    less = 0;
    equal = 0;
    more = 1;
    return
  elseif ( compare == 0 )
    less = 0;
    equal = 1;
    more = 2;
    return
  end

  compare = r8r8r8_compare ( xval, yval, zval, xhi, yhi, zhi );

  if ( compare == 1 )
    less = n;
    equal = 0;
    more = n + 1;
    return
  elseif ( compare == 0 )
    less = n - 1;
    equal = n;
    more = n + 1;
    return
  end

  while ( 1 )

    if ( lo + 1 == hi )
      less = lo;
      equal = 0;
      more = hi;
      return
    end

    mid = round ( ( lo + hi ) / 2 );
    xmid = x(indx(mid));
    ymid = y(indx(mid));
    zmid = z(indx(mid));

    compare = r8r8r8_compare ( xval, yval, zval, xmid, ymid, zmid );

    if ( compare == 0 )
      equal = mid;
      less = equal - 1;
      more = equal + 1;
      return
    elseif ( compare == -1 )
      hi = mid;
    elseif ( compare == +1 )
      lo = mid;
    end

  end

  return
end


function order = strong_elim_order(G, node_sizes, partial_order)
% STRONG_ELIM_ORDER Find an elimination order to produce a strongly triangulated graph.
% order = strong_elim_order(moral_graph, node_sizes, partial_order)
%
% partial_order(i,j)=1 if we must marginalize i *after* j
% (so i will be nearer the strong root).
% e.g., if j is a decision node and i is its information set:
%   we cannot maximize j if we have marginalized out some of i
% e.g., if j is a continuous child and i is its discrete parent:
%   we want to integrate out the cts nodes before the discrete ones,
%   so that the marginal is strong.
%
% For details, see
% - Jensen, Jensen and Dittmer, "From influence diagrams to junction trees", UAI 94.
% - Lauritzen, "Propgation of probabilities, means, and variances in mixed graphical
%   association models", JASA 87(420):1098--1108, 1992.
%
% On p369 of the Jensen paper, they state "the reverse of the elimination order must be some
% extension of [the partial order] to a total order".
% We make no attempt to find the best such total ordering, in the sense of minimizing the weight
% of the resulting cliques.

% Example from the Jensen paper:
% Let us number the nodes in Fig 1 from top to bottom, left to right,
% so a=1,b=2,D1=3,c=4,...,l=14,j=15,k=16.
% The elimination ordering they propose on p370 is [14 15 16 11 12 1 4 5 10 8 13 9 7 6 3 2];

if 0
  total_order = topological_sort(partial_order);
  order = total_order(end:-1:1); % no attempt to find an optimal constrained ordering!
  return;
end

% The following implementation is due to Ilya Shpitser and seems to give wrong
% results on cg1

n = length(G);
MG = G; % copy the original graph
uneliminated = ones(1,n);
order = zeros(1,n);

for i=1:n
  roots = [];
  k = 1;
  for j=1:n
    if sum(partial_order(j,:)) == 0
      roots(k) = j;
      k = k + 1;
    end
  end
  U = find(uneliminated);
  valid = myintersect(U, roots);
  % Choose the best node from the set of valid candidates
  score1 = zeros(1,length(valid));
  score2 = zeros(1,length(valid));
  for j=1:length(valid)
    k = valid(j);
    ns = myintersect(neighbors(G, k), U);
    l = length(ns);
    M = MG(ns,ns);
    score1(j) = l^2 - sum(M(:)); % num. added edges
    score2(j) = prod(node_sizes([k ns])); % weight of clique
  end
  j1s = find(score1==min(score1));
  j = j1s(argmin(score2(j1s)));
  k = valid(j);
  uneliminated(k) = 0;
  order(i) = k;
  ns = myintersect(neighbors(G, k), U);
  if ~isempty(ns)
    G(ns,ns) = 1;
    G = setdiag(G,0);
  end
  partial_order(:,k) = 0;
end


function [dataout] = mmse_det(ch_and_rx)
% mmse_det  User-defined function for MMSE detector.
% Input: 'ch_and_rx'
%    - first elements are MIMO parameters (STBC, Nss, etc.)
%    - next 16*NST elements are channel estimates.
%    - next (NSTS+1)*NST elements are reference training symbols.
%    - next (NSTS+1)*NST elements are received training symbols.
%    - remaining x*NST elements are received data symbols.
% Output: 'detout'
%    - x*NST elements are MMSE detector outputs.
%    - x*NST elements are Viterbi metric weights.
global PER_snr snr_idx ch_type;

% Initialize vectors
Clk = ch_and_rx(1);
STBC = ch_and_rx(2);
Nss  = ch_and_rx(3);
Nsts = Nss + STBC;
Nltf = Nsts;
if (Nsts==3) % use 4 TRN symbols for NSTS=3
   Nltf = 4;
end

Nrx  = ch_and_rx(4);
ch_est  = ch_and_rx(4+(1:16*56));
ref_trn = ch_and_rx(4+16*56+(1:4*56*(Nltf)));
rx_trn  = ch_and_rx(4+16*56+4*56*(Nltf)+(1:4*56*(Nltf)));
rx_data = ch_and_rx((5+16*56+2*4*56*(Nltf)):end);

%%%% Form channel matrix
idx = 0;
H = zeros(4,4,56);
for c=1:4
    for r=1:4
        H(r,c,:) = ch_est(idx+(1:56));
        idx=idx+56;
    end
end
H = H(1:Nrx,1:Nsts,:);  %% channel matrix is Nrx x Nsts

%%%% Train data symbols...
idx = 0;
nsymtrn = (length(ref_trn)/4/56);
trnRef = zeros(Nrx,56,nsymtrn);
for n=1:nsymtrn
    for m=1:Nsts
        trnRef(m,:,n) = ref_trn(idx+(m-1)*56+(1:56));
        trnRx(m,:,n)  =  rx_trn(idx+(m-1)*56+(1:56));
    end
    idx=idx+4*56;
end

%%%% Rx data symbols...
idx = 0;
nsymdata = (length(rx_data)/4/56);
rdata = zeros(4,56,nsymdata);
for n=1:nsymdata
    for m=1:Nrx
        rdata(m,:,n) = rx_data(idx+(m-1)*56+(1:56));
    end
    idx=idx+4*56;
end

%%%%
%%%% STBC or MMSE detection...
%%%%
if (STBC)
    %%%% Form space-time decoding matrices
    for k=1:56
        if (STBC==1 && Nss==1)
            % Heff matrix (assumes channel stationary for 2 symbols)
            Heff(:,:,k) = [      H(1,1,k)      -H(1,2,k); ...
                            conj(H(1,2,k)) conj(H(1,1,k)) ];
        elseif (STBC==1 && Nss==2)
            % Heff matrix (assumes channel stationary for 2 symbols)
            Heff(:,:,k) = [      H(1,1,k)      -H(1,2,k)       H(1,3,k)              0;  ...
                            conj(H(1,2,k)) conj(H(1,1,k))             0  conj(H(1,3,k)); ...
                                 H(2,1,k)      -H(2,2,k)       H(2,3,k)              0;  ...
                            conj(H(2,2,k)) conj(H(2,1,k))             0  conj(H(2,3,k)) ];
        elseif (STBC==1 && Nss==3)
            % Heff matrix (assumes channel stationary for 2 symbols)
            Heff(:,:,k) = [      H(1,1,k)      -H(1,2,k)       H(1,3,k)              0        H(1,4,k)              0;  ...
                            conj(H(1,2,k)) conj(H(1,1,k))             0  conj(H(1,3,k))              0  conj(H(1,4,k)); ...
                                 H(2,1,k)      -H(2,2,k)       H(2,3,k)              0        H(2,4,k)              0;  ...
                            conj(H(2,2,k)) conj(H(2,1,k))             0  conj(H(2,3,k))              0  conj(H(2,4,k)); ...
                                 H(3,1,k)      -H(3,2,k)       H(3,3,k)              0        H(3,4,k)              0;  ...
                            conj(H(3,2,k)) conj(H(3,1,k))             0  conj(H(3,3,k))              0  conj(H(3,4,k)) ];
        elseif (STBC==2 && Nss==2)
            % Heff matrix (assumes channel stationary for 2 symbols)
            Heff(:,:,k) = [      H(1,1,k)      -H(1,2,k)       H(1,3,k)      -H(1,4,k);  ...
                            conj(H(1,2,k)) conj(H(1,1,k)) conj(H(1,4,k)) conj(H(1,3,k)); ...
                                 H(2,1,k)      -H(2,2,k)       H(2,3,k)      -H(2,4,k);  ...
                            conj(H(2,2,k)) conj(H(2,1,k)) conj(H(2,4,k)) conj(H(2,3,k)) ];
        end
    end

    %%%% Compute noise power...
    N_o = 0;
    for k=1:56
        He = Heff(:,:,k);
        N_o = N_o + (1/56)*sum(sum(He.*conj(He)))/Nrx;     % Signal power
    end
    snr_val = PER_snr(snr_idx);
    N_o = N_o * 10^(-snr_val/10);     % Adjust by SNR

    %%%% Compute space-time detector coeff's
    C = zeros(Nss*2,Nss*2,56);
    for k=1:56
        %%%% MMSE coeff's
        He = Heff(:,:,k);
        C(:,:,k) = inv(He'*He + N_o*eye(Nss*2))*He';
    end

    %%%% Compute space-time decoding outputs, Viterbi metric weights
    idx = 0;
    detout = zeros(4*56*nsymdata,1);
    metout = zeros(4*56*nsymdata,1);
    for n=1:2:nsymdata
        for k=1:56
            if (STBC==1 && Nss==1)
                % MMSE detector outputs
                detset = C(:,:,k) * [ rdata(1,k,n); ...
                                      conj(rdata(1,k,n+1)) ];
                detout(k+idx+0*56) = detset(1);         % first symbol in time, ss1
                detout(k+idx+4*56) = conj(detset(2));   % second symbol in time, ss1
                % Viterbi metric weights
                metset = eps+abs(1-diag(C(:,:,k)*Heff(:,:,k)));
                metout(k+idx+0*56) = 1/metset(1);
                metout(k+idx+4*56) = 1/metset(2);
            elseif (STBC==1 && Nss==2)
                % MMSE detector outputs
                detset = C(:,:,k) * [ rdata(1,k,n); ...
                                      conj(rdata(1,k,n+1)); ...
                                      rdata(2,k,n); ...
                                      conj(rdata(2,k,n+1)) ];
                detout(k+idx+0*56) = detset(1);         % first symbol in time, ss1
                detout(k+idx+4*56) = conj(detset(2));   % second symbol in time, ss1
                detout(k+idx+1*56) = detset(3);         % first symbol in time, ss2
                detout(k+idx+5*56) = conj(detset(4));   % second symbol in time, ss2
                % Viterbi metric weights
                metset = eps+abs(1-diag(C(:,:,k)*Heff(:,:,k)));
                metout(k+idx+0*56) = 1/metset(1);
                metout(k+idx+4*56) = 1/metset(2);
                metout(k+idx+1*56) = 1/metset(3);
                metout(k+idx+5*56) = 1/metset(4);
            elseif (STBC==1 && Nss==3)
                % MMSE detector outputs
                detset = C(:,:,k) * [ rdata(1,k,n); ...
                                      conj(rdata(1,k,n+1)); ...
                                      rdata(2,k,n); ...
                                      conj(rdata(2,k,n+1)); ...
                                      rdata(3,k,n); ...
                                      conj(rdata(3,k,n+1)) ];
                detout(k+idx+0*56) = detset(1);         % first symbol in time, ss1
                detout(k+idx+4*56) = conj(detset(2));   % second symbol in time, ss1
                detout(k+idx+1*56) = detset(3);         % first symbol in time, ss2
                detout(k+idx+5*56) = conj(detset(4));   % second symbol in time, ss2
                detout(k+idx+2*56) = detset(5);         % first symbol in time, ss3
                detout(k+idx+6*56) = conj(detset(6));   % second symbol in time, ss3
                % Viterbi metric weights
                metset = eps+abs(1-diag(C(:,:,k)*Heff(:,:,k)));
                metout(k+idx+0*56) = 1/metset(1);
                metout(k+idx+4*56) = 1/metset(2);
                metout(k+idx+1*56) = 1/metset(3);
                metout(k+idx+5*56) = 1/metset(4);
                metout(k+idx+2*56) = 1/metset(5);
                metout(k+idx+6*56) = 1/metset(6);
            elseif (STBC==2 && Nss==2)
                % MMSE detector outputs
                detset = C(:,:,k) * [ rdata(1,k,n); ...
                                      conj(rdata(1,k,n+1)); ...
                                      rdata(2,k,n); ...
                                      conj(rdata(2,k,n+1)) ];
                detout(k+idx+0*56) = detset(1);         % first symbol in time, ss1
                detout(k+idx+4*56) = conj(detset(2));   % second symbol in time, ss1
                detout(k+idx+1*56) = detset(3);         % first symbol in time, ss2
                detout(k+idx+5*56) = conj(detset(4));   % second symbol in time, ss2
                % Viterbi metric weights
                metset = eps+abs(1-diag(C(:,:,k)*Heff(:,:,k)));
                metout(k+idx+0*56) = 1/metset(1);
                metout(k+idx+4*56) = 1/metset(2);
                metout(k+idx+1*56) = 1/metset(3);
                metout(k+idx+5*56) = 1/metset(4);
            end
        end
        idx=idx+8*56;
    end

else

    %%%% Compute noise power...
    N_o = 0;
    for k=1:56
        He = H(:,:,k);
        N_o = N_o + (1/56)*sum(sum(He.*conj(He)))/Nrx;     % Signal power
    end
    snr_val = PER_snr(snr_idx);
    N_o = N_o * 10^(-snr_val/10);     % Adjust by SNR

    %%%%% MMSE linear detector (no STBC)
    C = zeros(Nsts,Nrx,56);
    for k=1:56
        %%%% MMSE coeff's
        He = H(:,:,k);
        C(:,:,k) = inv(He'*He + N_o*eye(Nrx)) * He';
    end

    %%%% MMSE detector outputs, Viterbi metric weights
    idx = 0;
    detset = zeros(4,1);
    detout = zeros(4*56*nsymdata,1);
    metset = eps+zeros(4,1);
    metout = zeros(4*56*nsymdata,1);
    for n=1:nsymdata
        for k=1:56
            % MMSE detector outputs
            detset(1:Nsts) = C(:,:,k)*rdata(1:Nrx,k,n);
            detout(k+idx+0*56) = detset(1);
            detout(k+idx+1*56) = detset(2);
            detout(k+idx+2*56) = detset(3);
            detout(k+idx+3*56) = detset(4);
            % Viterbi metric weights
            metset(1:Nsts) = eps+abs(1-diag(C(:,:,k)*H(:,:,k)));
            metout(k+idx+0*56) = 1/metset(1);
            metout(k+idx+1*56) = 1/metset(2);
            metout(k+idx+2*56) = 1/metset(3);
            metout(k+idx+3*56) = 1/metset(4);
        end
        idx=idx+4*56;
    end

end

% Adjust metric scaling range (0 - least confident, 7 - most confident)
% (based on use of 4-bit data, soft decisions, for Viterbi module)
max_metout = max(metout(find(metout<1e14)));
log_metout = log(metout + 1e-6);
log_max_metout = log(max_metout + 1e-6);
metout = floor(7.99 * log_metout/log_max_metout);
metout = (metout >= 7).*7 + (metout < 7).*metout;

% MMSE module output
dataout = [detout; metout];


function pde = Helmholtzdata3
%% HELMHOTLZDATA4 trigonometric  data for PML Helmholtz equation
% -laplace u -k^2 u = f;
% The Intresting domain is [0,1]^2; The computing domain including the PML
%domain is [-0.1,1.1]^2;
% boundary condition
% u =0 on the PML boundary;
% Created by Jie Zhou.
% The data come from
% Advances in Iterative Methods and Preconditioners for the Helmholtz Equation
% Copyright (C)  Long Chen. See COPYRIGHT.txt for details.

pde = struct('f',@f,'k2',@k2,'g_D',@g_D,'d',@d);

    % load data (right hand side function)
    function rhs =  f(p)

      x = p(:,1); y = p(:,2);
     k0 = size(p,1);
    rhs = zeros(k0,1);
     ii = find(abs(x-0.5)<=1.0e-2&abs(y-0.5)<=1.0e-2);
    rhs(ii) = 10000;
    end

    function PMLp = d(p)
    global omegal k  c
    c = 340;
    omegal = k*340;
          i = sqrt(-1);
          x = p(:,1); y = p(:,2);
      size0 = size(x,1);
     sigma1 = ones(size0,1);
     sigma2 = ones(size0,1);
    index = find((x<0)|(x>1)|(y>1)|(y<0));
    sigma1(index) = x(index);
    sigma2(index) = y(index);

    s1 = 1 + sigma1/((i)*omegal);
    s2 = 1 + sigma2/((i)*omegal);

    PMLp = [s2./s1 s1./s2];
    end
    function s =  g_D(p)
    s = zeros(size(p,1),1);
    end
    function wavenumber = k2(p)
    global k omegal  c
    omegal = k*340;
          i = sqrt(-1);
          x = p(:,1); y = p(:,2);
      size0 = size(x,1);
     sigma1 = ones(size0,1);
     sigma2 = ones(size0,1);
    index = find((x<0)|(x>1)|(y>1)|(y<0));
    sigma1(index) = x(index);
    sigma2(index) = y(index);

    s1 = 1 + sigma1/((i)*omegal);
    s2 = 1 + sigma2/((i)*omegal);
    wavenumber = k.^2.*s1.*s2;
    end

end


function h = max2(f, g, dims)
%MAX2   Maximum value of a CHEBFUN3 in two directions.
%   MAX2(F) returns a 1D CHEBFUN representing the maximum of the CHEBFUN3
%   object F along the y and z directions, i.e,
%                                          MAX2(F) = @(z) max(F( :, :, z)).
%
%   MAX2(F, [], dims) returns a CHEBFUN representing the maximum of F along
%   the dimensions DIMS, where DIMS = [1, 2] means along the x and y
%   directions, etc.
%
%   WARNING: This function is not always accurate to the expected precision.
%
%   For the global maximum use MAX3.
%
% See also CHEBFUN3/MAX and CHEBFUN3/MAX3.

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

% Empty check:
if ( isempty(f) )
    error('CHEBFUN:CHEBFUN3:max2:input', 'CHEBFUN3 is empty');
end

% Default to max2 of one chebfun3:
if ( nargin < 2 )
    g = [];
end

% Default to maximum along the x and y directions:
if ( nargin < 3 )
    dims = [1, 2];
end

% Do not allow max(F, G):
if ( nargin > 1 && ~isempty(g) )
    error('CHEBFUN:CHEBFUN3:max2:twoCHEBFUN3Inputs', ...
        'Unable to maximize two CHEBFUN3 objects.');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% We have no idea how to achieve this in an efficient way. This
% is an attempt to return a result, but typically it won't be accurate to
% more than 4-5 digits.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

dom = f.domain;
n = 129;
if ( all(dims == [1, 2]) || all(dims == [2, 1]) )
    vals = sample(f, n, n, n);
    temp = chebfun3.unfold(vals, [3]);
    h = chebfun(max(temp, [], 2), dom(5:6), 'splitting', 'on');
    h = simplify(h);
elseif ( all(dims == [1, 3]) || all(dims == [3, 1]) )
    vals = sample(f, n, n, n);
    temp = chebfun3.unfold(vals, [2]);
    h = chebfun(max(temp, [], 2), dom(3:4), 'splitting', 'on');
    h = simplify(h);
elseif ( all(dims == [2, 3]) || all(dims == [3, 2]) )
    vals = sample(f, n, n, n);
    temp = chebfun3.unfold(vals, [1]);
    h = chebfun(max(temp, [], 2), dom(1:2), 'splitting', 'on');
    h = simplify(h);
elseif ( dims == 0 )
    error('CHEBFUN:CHEBFUN3:max2:dims', ...
        'Dimension arguments must be two positive integer scalars within indexing range.')
else
   % return the CHEBFUN3. This is analogous to that MAX() command in
   % MATLAB.
   h = f;
end

end


function [f,s] = is_planar(V,epsilon)
  % IS_PLANAR Return whether the 3D mesh is planar
  %
  % [f,s] = is_planar(V)
  % [f,s] = is_planar(V,epsilon)
  %
  % Inputs:
  %   V  #V x 3 matrix of vertex coordinates
  %   epsilon  optional parameter used as threshold for smallest eigen value of
  %     pca, {1e-10}
  % Outputs:
  %   f  flag whether mesh is planar
  %   s  smallest principle component variance
  %
  %

  if ~exist('epsilon','var')
    epsilon = 1e-10;
  end

  if size(V,2) == 2
    f = true;
    s = 0;
    return;
  end

  % This seems to have changed during matlab. For large V this is
  % infeasible.
  [c,l] = pcacov(V);

  s = min(l);

  f = s < epsilon;

end