%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled
% 	fluid-structure interaction models. This version of the code is based off of
%	Peskin's Immersed Boundary Method Paper in Acta Numerica, 2002.
%
% Author: Nicholas A. Battista
% Email:  nickabattista[at]gmail.com
% IB2d Created: May 27th, 2015
% Institution: TCNJ
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs or non-invariant beams*)
% 	3. Target Points
%	4. Muscle-Model (combined Force-Length-Velocity model, "HIll+(Length-Tension)")
%   .
%   .
%   .
%
% One is able to update those Lagrangian Structure parameters, e.g., spring constants, resting lengths, etc
%
% There are a number of built in Examples, mostly used for teaching purposes.
%
% If you would like us to add a specific muscle model, please let Nick (nickabattista[at]gmail.co) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: updates the target point positions
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function targets = update_Target_Point_Positions(dt,current_time,targets)

%IDs = targets(:,1);                 % Stores Lag-Pt IDs in col vector
%xPts= targets(:,2);                 % Original x-Values of x-Target Pts.
%yPts= targets(:,3);                 % Original y-Values of y-Target Pts.
%kStiffs = targets(:,4);             % Stores Target Stiffnesses
%N_target = length(targets(:,1));    % Gives total number of target pts!

t1 = 0.01;                          % Period A->B (and B->A)
t2 = 0.02;                          % Period B->C (abd C->B)
period = 2*(t1+t2);                 % Time it takes to move to the right
t = rem(current_time,period);       % Time (moded out)

% Cubic Interpolation Information
p1 = 0.25;
p2 = 0.925;

% a COEFFICIENTS
a0 = 0;
a1 = 0;
a2 = 0;
a3 = 4.324324324324318;

% b COEFFICIENTS
b0 = 0.123456790123457;
b1 = -1.481481481481478;
b2 = 5.925925925925911;
b3 = -3.576910243576897;

% c COEFFICIENTS
c0 = -16.777777777777700;
c1 =  53.333333333333101;
c2 = -53.333333333333101;
c3 = 17.777777777777700;

%
% START THE INTERPOLATING BETWEEN STATES!
%
if t <= t1 % STATE A -> STATE B

    A = read_In_State('State_A.pts');
    B = read_In_State('State_B.pts');

    % Scaling time for appropriate use in interp. function so tTilde\in[0,1]
    tTilde = (t/t1);

    % Evaluate Pieceise Cubic Interpolation Poly
    if tTilde<=p1
        gFUNC = a0 + a1*tTilde + a2*tTilde^2 + a3*tTilde^3;
    elseif tTilde<=p1+p2
        gFUNC = b0 + b1*tTilde + b2*tTilde^2 + b3*tTilde^3;
    else
        gFUNC = c0 + c1*tTilde + c2*tTilde^2 + c3*tTilde^3;
    end

    targets(:,2) = A(:,1) + gFUNC*( B(:,1) - A(:,1) );
    targets(:,3) = A(:,2) + gFUNC*( B(:,2) - A(:,2) );

elseif t <= (t1+t2) % STATE B -> C

    B = read_In_State('State_B.pts');
    C = read_In_State('State_C.pts');

    % Scaling time for appropriate use in interp. function so tTilde\in[0,1]
    tTilde = (t-t1)/t2;

    % Evaluate Pieceise Cubic Interpolation Poly
    if tTilde<=p1
        gFUNC = a0 + a1*tTilde + a2*tTilde^2 + a3*tTilde^3;
    elseif tTilde<=p1+p2
        gFUNC = b0 + b1*tTilde + b2*tTilde^2 + b3*tTilde^3;
    else
        gFUNC = c0 + c1*tTilde + c2*tTilde^2 + c3*tTilde^3;
    end

    targets(:,2) = B(:,1) + gFUNC * ( C(:,1) - B(:,1) );
    targets(:,3) = B(:,2) + gFUNC * ( C(:,2) - B(:,2) );

elseif t <= (t1+2*t2) % STATE C -> B

    B = read_In_State('State_B.pts');
    C = read_In_State('State_C.pts');

    % Scaling time for appropriate use in interp. function so tTilde\in[0,1]
    tTilde = (t-t1-t2)/(t2);

    % Evaluate Pieceise Cubic Interpolation Poly
    if tTilde<=p1
        gFUNC = a0 + a1*tTilde + a2*tTilde^2 + a3*tTilde^3;
    elseif tTilde<=p1+p2
        gFUNC = b0 + b1*tTilde + b2*tTilde^2 + b3*tTilde^3;
    else
        gFUNC = c0 + c1*tTilde + c2*tTilde^2 + c3*tTilde^3;
    end

    targets(:,2) = C(:,1) + gFUNC * ( B(:,1) - C(:,1) );
    targets(:,3) = C(:,2) + gFUNC * ( B(:,2) - C(:,2) );

else % STATE B -> A

    A = read_In_State('State_A.pts');
    B = read_In_State('State_B.pts');

    % Scaling time for appropriate use in interp. function so tTilde\in[0,1]
    tTilde = (t-t1-2*t2)/(2*t2-2*t1);

    % Evaluate Pieceise Cubic Interpolation Poly
    if tTilde<=p1
        gFUNC = a0 + a1*tTilde + a2*tTilde^2 + a3*tTilde^3;
    elseif tTilde<=p1+p2
        gFUNC = b0 + b1*tTilde + b2*tTilde^2 + b3*tTilde^3;
    else
        gFUNC = c0 + c1*tTilde + c2*tTilde^2 + c3*tTilde^3;
    end

    targets(:,2) = B(:,1) + gFUNC * ( A(:,1) - B(:,1) );
    targets(:,3) = B(:,2) + gFUNC * ( A(:,2) - B(:,2) );

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: Reads in the # of vertex pts and all the vertex pts from the
%           .vertex file.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function PTS = read_In_State(struct_name)

filename = struct_name;  %Name of file to read in
fileID = fopen(filename);

% Read in the file, use 'CollectOutput' to gather all similar data together
% and 'CommentStyle' to to end and be able to skip lines in file.
C = textscan(fileID,'%f %f','CollectOutput',1);

fclose(fileID);     %Close the data file.

vertices = C{1};    %Stores all read in data in vertices (N+1,2) array

PTS = vertices(1:end,1:2);


function r8lib_test119 ( )

%*****************************************************************************80
%
%% R8LIB_TEST119 tests R8VEC_EVEN3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2009
%
%  Author:
%
%    John Burkardt
%
  nold = 4;
  nval = 12;

  xold = [ 0.0, 5.1, 7.0, 10.0 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8LIB_TEST119\n' );
  fprintf ( 1, '  R8VEC_EVEN3 tries to evenly interpolate new data\n' );
  fprintf ( 1, '  between old values.\n' );

  r8vec_print ( nold, xold, '  Original vector:' );

  xval = r8vec_even3 ( nold, nval, xold );

  r8vec_print ( nval, xval, '  New vector:' );

  return
end


% Eldar reconstruction

N = 3;                  % Nth order nonuniform sampling
TQ = 1;                 % Nyquist Period
L=20;                   % Resolution factor for fraction delay

T = [2*TQ 3*TQ 6*TQ];      % Decimation Periods
K = 0.5*lcm(2*T(1), 2*T(2)); K = 0.5*lcm(2*K, 2*T(3))/TQ;
capT = K*TQ; M = capT./T;
capM = lcm(M(1), M(2)); capM = lcm(capM, M(3));
excess = ceil((K-1)/capM);
maxf = K/(excess*capM+1);
TQ1 = maxf*TQ;
K1 = K/maxf;

ML = 400; % number of slices
w_c = 0.85;
NS = 100;  % Number of Sinusoids

LF = capM*2*K1+1;  %359,159,239          % min length of LF should be capM*2*K1
n = -(LF-1)/2:1:(LF-1)/2;
Hd = firpm(LF-1,[0 w_c],[0 w_c*pi],'differentiator');
delayV = (LF-1)/2;

std = [1e-6 1e-5 1e-4 1e-3 1e-2 1e-1];%5*1e-1];

serE = zeros(size(std));

MCruns = 25;
MCruns1 = 25;

for tt = 1:length(std)
    aa = 0;
    display(tt);
    for rrr = 1:MCruns1
        taus = [0 1.1+std(tt)*randn 2.2+std(tt)*randn]*TQ;
        if or(taus(2)==TQ,or(taus(2)==2*TQ,or(taus(3)==2*TQ,or(taus(2)==taus(3),taus(2)==taus(1)))))
            aa = aa+1;
            display(taus);
            continue;
        end

        tausI = sort([taus(1) taus(2) taus(3) T(1)+taus(1) T(2)+taus(2) 2*T(1)+taus(1)]);
        a = zeros(1,K);
        for p = 1:K
            a(p) = 1;
            for q = 1:K
                if q ~= p
                        a(p) = a(p)/sin(pi*(tausI(p)-tausI(q))/capT);
                end;
            end;
        end;

        for pp = 1:MCruns;
            Frq = rand(1,NS)*w_c/2;
            Amp = rand(1,NS)/(sqrt(NS)*2);
            Phi = rand(1,NS)*2*pi;
            inputN = zeros(1,ML*K1);
            input = zeros(1,ML*K);
            for k = 1:NS
              inputN = inputN + Amp(k)*sin(2*pi*Frq(k)*(0:ML*K1-1)*TQ1+Phi(k));
              input = input + Amp(k)*sin(2*pi*Frq(k)*(0:ML*K-1)*TQ+Phi(k));
            end;
        end

        y = zeros(N,ML*K1);
        for p = 1:N
            tau = taus(p)+(0:ML*M(p)-1)*T(p);
            x1 = zeros(1,ML*M(p));
            for k = 1:NS
                x1 = x1 + Amp(k)*sin(2*pi*Frq(k)*tau+Phi(k));
            end;

            y1 = upsample(x1,K1);

            LFE = M(p)*capM*2*K1+1;  %359,159,239          % min length of LF should be capM*2*K1
            nE = -(LFE-1)/2:1:(LFE-1)/2;
            h = sinc((nE/K1)-(taus(p)/T(p))).*kaiser_mine1(LFE,18,-K1*(taus(p)/T(p)));
            aaa = ones(1,M(p));
            bb = ones(M(p),LFE);
            for l = 1:M(p)
                for q = 1:N
                    if q ~= p
                        aaa(l) = aaa(l)*sin(pi*(taus(p)-taus(q)+(l-1)*T(p))/T(q));
                        bb(l,:) = bb(l,:).*sin(pi*((nE*TQ1/M(p))-taus(q)+(l-1)*T(p))/T(q));
                    end;
                end;
                bb(l,:) = bb(l,:)/aaa(l);
            end;

            bbb = zeros(M(p),LFE);
            bbn = zeros(M(p),LFE);
            for m = 1:M(p)
                for l = 1:M(p)
                    bbb(l,:) = bb(l,:)*exp(1i*(2*pi/M(p))*(m-1)*(l-1));
                end
                bbb = sum(bbb,1);
                bbn(m,:) = bbb.*exp(1i*(2*pi/M(p))*(m-1)*nE);
            end;
            bbn = sum(bbn,1);
            bbn = bbn.*h/M(p);

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            y1 = conv(y1,bbn);
            delay = (length(h)-1)/2;
            y1 = y1(1+delay:M(p):end-delay);
            y(p,:) = y1;
        end;
        y = real(sum(y,1));
        x = inputN;
        y = y(160:end-60);
        x = x(160:end-60);
        serE(tt) = serE(tt)+20*log10(norm(x,2)/norm(y-x,2));
    end
end

serE = serE/(MCruns*(MCruns1-aa));

plot(std,serE);
xlabel('Standard Deviation (\sigma)','fontsize',14,'fontweight','b');
ylabel('SNR in dB','fontsize',14,'fontweight','b');
grid on;box on;
set(gca,'fontsize',14,'fontweight','b')


function linplus_test527 ( )

%*****************************************************************************80
%
%% TEST527 tests R8PP_INDICATOR.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2009
%
%  Author:
%
%    John Burkardt
%
  n = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST527\n' );
  fprintf ( 1, '  R8PP_INDICATOR sets up a R8PP indicator matrix.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix order N = %d\n', n );
%
%  Set the matrix.
%
  a = r8pp_indicator ( n );

  r8pp_print ( n, a, '  The R8PP indicator matrix:' );

  return
end


eeg_file= fullfile(BTB.DataDir, 'demoMat', 'VPiac_10_10_13', ...
                   'calibration_CenterSpellerMVEP_VPiac');

% Load data
try
  [cnt, mrk, mnt] = file_loadMatlab(eeg_file);
catch
  error('You need to run ''demo_convert_ERPSpeller'' first');
end

nBlocks= 6;

% define 'nBlocks' intervals ranging from the first to the last marker:
inter = linspace( mrk.time(1), mrk.time(end), nBlocks+1 );
blk = struct( 'ival', [inter(1:end-1)' inter(2:end)'] );

% define marker structure: one marker every second
mrkblk = mrk_evenlyInBlocks( blk, 1000 );
% assign class labels for each marker corresponding to the block
mrkblk.y = util_classind2labels( mrkblk.event.blkno );
mrkblk.className= str_cprintf('block %d', 1:6);

% calculate spectra (average within the defined blocks)
spec = proc_segmentation( cnt, mrkblk, [0 1000] );
spec = proc_spectrum( spec, [5 40], kaiser(cnt.fs,2) );

% visualize the block-wise spectra
fig_set(1);
cmap= cmap_hsvFade(nBlocks, [0 5/6], 1, 1);
grid_plot( spec, mnt , defopt_spec, 'ColorOrder',cmap );

% increasing noise level can be observed, e.g. in channels C6 and T8


function ni = niftiCheckQto(ni)
% Changes the qto_* or/and sto_* fields in the NIFTI hearder if not properly
% set. Also changes other fields see below.
%
%   ni = niftiCheckQto(ni)
%
% We expect these fields to be set at some coordinates inside the image
% volume. If the NIFTI file was created without setting these files this
% function starts up the fields by setting the center of the coordinate
% system of the image to the middle of the image.
%
% Does a simple sanity-check on the xform. Right now, we check the origin to
% make sure it is well- within the image volume. If not, the qto xform
% fields of the NIFTI struct ni will be adjusted to set the origin to the
% center of the image. The updated struct is returned and a message is
% printed to the command line indicating the fix.
%
% it will update the follwing fields:
%  'qform_code'
%  'qto_xyz'
%  'qto_ijk'
%  'quatern_b'
%  'quatern_c'
%  'quatern_d'
%  'qoffset_x'
%  'qoffset_y'
%  'qoffset_z'
%  'qfac'
%  'pixdim'
%
% (c) Stanford Vista Team 2012

% if(isfield(ni,'data')&&~isempty(niftiGet(ni, 'data')))
%     % sanity-check ni.dim
%     sz = size(niftiGet(ni, 'data'));
%     if(any(ni.dim(1:3)~=sz(1:3)))
%         fprintf('[%s] NIFTI volume dim wrong- setting it to the actual data size.\n',mfilename);
%         ni.dim(1:3) = sz(1:3);
%     end
% end

if(niftiGet(ni, 'qform_code')==0 && niftiGet(ni, 'sform_code')~=0)
    fprintf('[%s] ni.qform_code is zero and sform_code ~=0. Setting ni.qto_* from ni.sto_*...\n',mfilename);
    ni = niftiSetQto(ni, niftiGet(ni, 'sto_xyz'));
end

qto_ijk = niftiGet(ni, 'qto_ijk');
origin = [qto_ijk(1:3,:)*[0 0 0 1]']';

nidim = niftiGet(ni, 'dim');
if(any(origin<2)||any(origin>nidim(1:3)-2))
  [t,r,s,k] = affineDecompose(qto_ijk);
  t = nidim/2;
  fprintf('[%s] NIFTI header origin is at or outside the image volume.\n',mfilename)
  fprintf('[%s] Origin to the image center [%2.3f,%2.3f,%2.3f] pix.\n',mfilename,t(1),t(2),t(3));
  ni = niftiSetQto(ni, inv(affineBuild(t,r,s,k)));
end

return;


function [specGlobal] = doa_srp(x,method, Param)
%%
if(~any(strcmp(method, {'SRP-PHAT' 'SRP-NON'})))
    error('ERROR[doa_srp]: method参数错误');
end
%% STFT
X = ssl_stft(x.',Param.window, Param.noverlap, Param.nfft, Param.fs);
X = X(2:end,:,:);
%%
if strcmp(method,'SRP-PHAT')
    specGlobal = ssl_srpPhat(X,Param);
else
    specGlobal = ssl_srp_nonlin(X,Param);
end

end

function X=ssl_stft(x,window,noverlap,nfft,fs)

% Inputs:x: nchan x nsampl  window = blackman(wlen);
% Output:X: nbin x nfram x nchan matrix

[nchan,~]=size(x);
[Xtemp,F,T,~] = spectrogram(x(1,:),window,noverlap,nfft,fs); % S nbin x nframe
nbin = length(F);
nframe = length(T);
X = zeros(nbin,nframe,nchan);
X(:,:,1) = Xtemp;
for ichan = 2:nchan
    X(:,:,ichan) = spectrogram(x(ichan,:),window,noverlap,nfft,fs);
end

end

function [specGlobal] = ssl_srpPhat(X,Param)
[~,nFrames,~] = size(X);
specInst = zeros(Param.nGrid, nFrames);

for i = 1:Param.nPairs
    spec = srpPhat_spec(X(Param.freqBins,:,Param.pairId(i,:)), Param.f(Param.freqBins), Param.tauGrid{i}); % NV % [freq x fram x local angle for each pair]
    specSampledgrid = (shiftdim(sum(spec,1)))';
    specCurrentPair = interp1q(Param.alphaSampled{i}', specSampledgrid, Param.alpha(i,:)');
    specInst(:,:) = specInst(:,:) + specCurrentPair;
end

switch Param.pooling
    case 'max'
        specGlobal = shiftdim(max(specInst,[],2));
    case 'sum'
        specGlobal = shiftdim(sum(specInst,2));
end
end

function [specGlobal] = ssl_srp_nonlin(X,Param)

alpha_meth = (10*Param.c)./(Param.d*Param.fs);
[~,nFrames,~] = size(X);
specInst = zeros(Param.nGrid, nFrames);

for i = 1:Param.nPairs
    spec = srpNonlin_spec(X(Param.freqBins,:,Param.pairId(i,:)), Param.f(Param.freqBins), alpha_meth(i), Param.tauGrid{i});
    specSampledgrid = (shiftdim(sum(spec,1)))';
    specCurrentPair = interp1q(Param.alphaSampled{i}', specSampledgrid, Param.alpha(i,:)');
    specInst = specInst + specCurrentPair;
end

switch Param.pooling
    case 'max'
        specGlobal = shiftdim(max(specInst,[],2));
    case 'sum'
        specGlobal = shiftdim(sum(specInst,2));
end
end


% Add baseline measure as Data Quality measure
% part of the ERPLAB Toolbox
% axs May 2019
%
% Format:
%  baseline_measure = dq_baseline(ERP);
%    or baseline_measure = dq_baseline(ERP,start_time, end_time, subtract_mean_flag)
%
% INPUT:      * - mandatory
%  * ERP  - an ERP structure
%
function [dq_struct, baseline_measure] = dq_baseline(ERP, start_ms, end_ms, subtract_mean_flag)

% Check input
try
    assert(isfield(ERP,'bindata'))
catch
    warning('Making Data Quality baseline measures requires an ERPSET')
    beep
    return
end

% Populate any empty args with defaults
if exist('start_ms','var') == 0 || isempty(start_ms)
    start_ms = ERP.times(1);
end
if exist('end_ms','var') == 0 || isempty(end_ms)
    end_ms = 0;
end
if exist('subtract_mean_flag','var') == 0 || isempty(subtract_mean_flag)
    subtract_mean_flag = 1;
end

% Check desired times are within availible times
good_start = start_ms >= ERP.times;
good_end = end_ms <= ERP.times;
try
    assert(any(good_start)==1)
    assert(any(good_end)==1)
catch
    beep
    warning('The times requested for Data Quality baseline are not available in this ERPset.')
    return
end

start_dp = find(abs(ERP.times-start_ms)==min(abs(ERP.times-start_ms))); % datapoint closest to desired ms
start_dp = start_dp(1); % if it's a tie, take the first element
end_dp = find(abs(ERP.times-end_ms)==min(abs(ERP.times-end_ms)));
end_dp = end_dp(1);

%

b_sd_1 =   std(ERP.bindata(1,start_dp:end_dp,1));

baseline_data = ERP.bindata(:,start_dp:end_dp,:);

if subtract_mean_flag == 1
    % Measure Standard Deviation, which removes mean
    baseline_measure = std(baseline_data,0,2);

elseif subtract_mean_flag == 0
    % use rms, not removing mean
    baseline_measure = rms(baseline_data,2);

elseif subtract_mean_flag == 2
    %Corrected SD
    nTimes = size(baseline_data,2);
    baseline_measure_biased = std(baseline_data,0,2);
    baseline_measure =  ((baseline_measure_biased * sqrt((nTimes-1)))/ sqrt((nTimes-(3/2)+(1/(8*(nTimes-1))))));

end

% quick plot of baseline sd
%imagesc(squeeze(baseline_measure))

% Prepare dq_struct
if subtract_mean_flag == 1
    dq_struct.type = 'Baseline Measure - SD';
elseif subtract_mean_flag == 0
    dq_struct.type = 'Baseline Measure - RMS';
elseif subtract_mean_flag == 2
    dq_struct.type = 'Baseline Measure - SD (Corrected)';
end

dq_struct.times = [start_ms end_ms];
dq_struct.data = baseline_measure;


function pklt( noisy_file, outfile)
%
%  Implements a perceptually-motivated subspace algorithm  [1].
%
%
%  Usage:  pklt(noisyFile, outputFile)
%
%         infile - noisy speech file in .wav format
%         outputFile - enhanced output file in .wav format
%
%
%  Example call:  pklt('sp04_babble_sn10.wav','out_pklt.wav');
%
%  References:
%   [1] Jabloun, F. and Champagne, B. (2003). Incorporating the human hearing
%   	 properties in the signal subspace approach for speech enhancement. IEEE
%   	 Trans. on Speech and Audio Processing, 11(6), 700-708.
%
% Authors: Yi Hu and Philipos C. Loizou
%
% Copyright (c) 2006 by Philipos C. Loizou
% $Revision: 0.0 $  $Date: 10/09/2006 $
%-------------------------------------------------------------------------

if nargin<2
   fprintf('Usage: pklt(noisyfile.wav,outFile.wav) \n\n');
   return;
end

vad_thre= 1.2; % 2.378 is the value from Mittal's paper
mu_vad= 0.98; % mu to use in vad

[noisy_speech, Srate, NBITS]= wavread( noisy_file);
subframe_dur= 4;  %subframe length is 4 ms
len= floor( Srate* subframe_dur/ 1000);
P= len; % sub-frame length - for 8k sampling rate, len is 32
frame_dur= 32; % frame length in msecs
N= frame_dur* Srate/ 1000;
Nover2= N/ 2; % window overlap in 50% of frame size
K= N;
frame_window= hamming( N);
subframe_window= hamming( P);
eta_v= .08; % used for gain values calc.

% ====noise covariance matrix estimation
L120=floor( 120* Srate/ 1000);
% assume the first 120ms is noise only
noise= noisy_speech( 1: L120);

noise_autoc= xcorr( noise, len- 1, 'biased');
% from -(len- 1) to (len- 1)
% obtain the autocorrelation functions
Rn= toeplitz( noise_autoc( len: end));
% form a Toeplitz matrix to obtain the noise signal covariance matrix
bartlett_win= bartlett( 2* len- 1);
n_autoc_win= noise_autoc.* bartlett_win;

for k= 0: N- 1
    Phi_w( k+ 1)= n_autoc_win( P: 2*P-1)' * 2* ...
        cos( 2* pi* k* (0: P- 1)'/ N)- n_autoc_win( P);
end
Phi_w= Phi_w';
% make Phi_w column vector

n_start= 1;

Nframes= floor( length( noisy_speech)/ (N/ 2))- 1;  % number of frames
x_overlap= zeros( Nover2, 1);

%===============================  Start Processing =====================

for n=1: Nframes

    noisy= noisy_speech( n_start: n_start+ N- 1);
    noisy_autoc= xcorr( noisy, len- 1, 'biased');
    Ry= toeplitz( noisy_autoc( len: 2* len- 1));

    % Use simple VAD algorithm to update noise cov matrix, Rn
    %
    vad_ratio= Ry(1,1)/ Rn(1,1);
    if (vad_ratio<= vad_thre) % noise dominant
        Rn= mu_vad* Rn+ (1- mu_vad)* Ry;
        noise_autoc_sc= Rn( 1, :)';
        % single sided noise autocorrelation
        noise_autoc= [flipud( noise_autoc_sc( 2: end)); ...
            noise_autoc_sc];
        n_autoc_win= noise_autoc.* bartlett_win;
        % compute Phi_w
        for k= 0: N- 1
            Phi_w( k+ 1)= n_autoc_win( P: 2*P-1)' * 2* ...
                cos( 2* pi* k* (0: P- 1)'/ N)- n_autoc_win( P);
        end
        % Phi_w is column vector
    end
    % =================

    Rx= Ry- Rn;

    [U, D]= eig( Rx);
    dD= diag( D); % retrieving diagonal elements
    dD_Q= find( dD> 0); % index for those eigenvalues greater than 0
    Lambda= dD( dD_Q);
    U1= U( :, dD_Q);
    % eigenvector for those eigenvalues greater than 0

    U1_fft= fft( U1, N);
    V= abs( U1_fft).^ 2;
    Phi_B= V* Lambda/ P;

    %==calculating masking threshold
    Phi_mask= mask( Phi_B( 1: N/ 2+ 1), N, Srate, NBITS);
    Phi_mask= [Phi_mask; flipud( Phi_mask( 2: N/ 2))];

    Theta= V'* Phi_mask/ K;
    Ksi= V'* Phi_w/ K;

    gain_vals= exp( -eta_v* Ksi./ min( Lambda, Theta));
    G= diag( gain_vals);
    H= U1* G* U1';

    % first step of synthesis for subframe
    sub_start= 1;
    sub_overlap= zeros( P/2, 1);
    for m= 1: (2*N/P- 1)
        sub_noisy= noisy( sub_start: sub_start+ P- 1);
        enhanced_sub_tmp= (H* sub_noisy).* subframe_window;
        enhanced_sub( sub_start: sub_start+ P/2- 1)= ...
            enhanced_sub_tmp( 1: P/2)+ sub_overlap;
        sub_overlap= enhanced_sub_tmp( P/2+1: P);
        sub_start= sub_start+ P/2;
    end
    enhanced_sub( sub_start: sub_start+ P/2- 1)= sub_overlap;

    xi= enhanced_sub'.* frame_window;
    xfinal( n_start: n_start+ Nover2- 1)= x_overlap+ xi( 1: Nover2);
    x_overlap= xi( Nover2+ 1: N);

    n_start= n_start+ Nover2;

end

xfinal( n_start: n_start+ Nover2- 1)= x_overlap;

wavwrite(xfinal, Srate, NBITS, outfile);

%=======================E N D ===================================

function M= mask( Sx, dft_length, Fs, nbits)
% Author: Patrick J. Wolfe
%         Signal Processing Group
%         Cambridge University Engineering Department
%         p.wolfe@ieee.org
% Johnston perceptual model initialisation
frame_overlap= dft_length/ 2;
freq_val = (0:Fs/dft_length:Fs/2)';
half_lsb = (1/(2^nbits-1))^2/dft_length;

freq= freq_val;
thresh= half_lsb;
crit_band_ends = [0;100;200;300;400;510;630;770;920;1080;1270;...
        1480;1720;2000;2320;2700;3150;3700;4400;5300;6400;7700;...
        9500;12000;15500;Inf];

% Maximum Bark frequency
%
imax = max(find(crit_band_ends < freq(end)));

% Normalised (to 0 dB) threshold of hearing values (Fletcher, 1929)
% as used  by Johnston.  First and last thresholds are corresponding
% critical band endpoint values, elsewhere means of interpolated
% critical band endpoint threshold values are used.
%
abs_thr = 10.^([38;31;22;18.5;15.5;13;11;9.5;8.75;7.25;4.75;2.75;...
        1.5;0.5;0;0;0;0;2;7;12;15.5;18;24;29]./10);
ABSOLUTE_THRESH = thresh.*abs_thr(1:imax);

% Calculation of tone-masking-noise offset ratio in dB
%
OFFSET_RATIO_DB = 9+ (1:imax)';

% Initialisation of matrices for bark/linear frequency conversion
% (loop increments i to the proper critical band)
%
num_bins = length(freq);
LIN_TO_BARK = zeros(imax,num_bins);
i = 1;
for j = 1:num_bins
    while ~((freq(j) >= crit_band_ends(i)) & ...
            (freq(j) < crit_band_ends(i+1))),
        i = i+1;
    end
    LIN_TO_BARK(i,j) = 1;
end

% Calculation of spreading function (Schroeder et al., 82)

spreading_fcn = zeros(imax);
summ = 0.474:imax;
spread = 10.^((15.81+7.5.*summ-17.5.*sqrt(1+summ.^2))./10);
for i = 1:imax
    for j = 1:imax
        spreading_fcn(i,j) = spread(abs(j-i)+1);
    end
end

% Calculation of excitation pattern function

EX_PAT = spreading_fcn* LIN_TO_BARK;

% Calculation of DC gain due to spreading function

DC_GAIN = spreading_fcn* ones(imax,1);

%Sx = X.* conj(X);

C = EX_PAT* Sx;

% Calculation of spectral flatness measure SFM_dB
%
[num_bins num_frames] = size(Sx);
k = 1/num_bins;
SFM_dB = 10.*log10((prod(Sx).^k)./(k.*sum(Sx)+eps)+ eps);

% Calculation of tonality coefficient and masked threshold offset
%
alpha = min(1,SFM_dB./-60);
O_dB = OFFSET_RATIO_DB(:,ones(1,num_frames)).*...
    alpha(ones(length(OFFSET_RATIO_DB),1),:) + 5.5;

% Threshold calculation and renormalisation, accounting for absolute
% thresholds

T = C./10.^(O_dB./10);
T = T./DC_GAIN(:,ones(1,num_frames));
T = max( T, ABSOLUTE_THRESH(:, ones(1, num_frames)));

% Reconversion to linear frequency scale

%M = 1.* sqrt((LIN_TO_BARK')*T);
M= LIN_TO_BARK'* T;


function linplus_test485 ( )

%*****************************************************************************80
%
%% TEST485 tests R8PBU_INDICATOR.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  n = 9;
  mu = 3;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST485\n' );
  fprintf ( 1, '  R8PBU_INDICATOR sets up a R8PBU indicator matrix.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix order N = %d\n', n );
  fprintf ( 1, '  Bandwidth MU = %d\n', mu );

  a = r8pbu_indicator ( n, mu );

  r8pbu_print ( n, mu, a, '  The R8PBU indicator matrix:' );

  return
end


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

function S=colormap222(v)

pts = reshape(double(v),[],3);

k1 = sqrt(3)*max(pts,[],2)./(sum(pts,2) + 1*(sum(pts,2) == 0));
pts = [k1.*pts(:,1) k1.*pts(:,2) k1.*pts(:,3)];

%%% Rotate Prism %%%%%%%%
g1 = rotvec2mat([1,1,1,pi/4]);g2 = rotvec2mat([-1,1,0,acos(1/sqrt(3))]);
g3 = rotvec2mat([0,0,1,-pi/3]);pts = pts*g1*g2*g3;pts(:,3)=pts(:,3)-1;

%%%%%% Prism --> Cone %%%%%%%%%%%
phi=atan2(pts(:,2),pts(:,1));maxphi = 2*pi; phi = mod(phi,maxphi);
rfin = hypot(pts(:,1),pts(:,2)).*sin(pi/6+mod(phi,2*pi/3))./(sin(pi/6));
pts = [sqrt(1/2).*rfin.*cos(phi) sqrt(1/2).*rfin.*sin(phi) pts(:,3)];

%%%%%% Cone --> Hemisphere %%%%%%
r = sqrt(pts(:,1).^2 + pts(:,2).^2 + pts(:,3).^2);
rad = sqrt(pts(:,1).^2 + pts(:,2).^2);

pts = [pts(:,1).*(rad - pts(:,3))./(r+1*(r==0)) ...
    pts(:,2).*(rad - pts(:,3))./(r+1*(r==0)) ...
    pts(:,3).*(rad - pts(:,3))./(r+1*(r==0))];

%%%%% Hemisphere --> Sphere

phi1 = atan2(pts(:,2),pts(:,1));phi1 = mod(phi1,2*pi);
ind1=find(phi1 < 2*pi/3 & phi1 > 0*pi/3);ind2=find(phi1 < 4*pi/3 & phi1 > 2*pi/3);
ind3=find(phi1 < 6*pi/3 & phi1 > 4*pi/3);ind = [ind1;ind2;ind3];

g4 = rotvec2mat([0,0,1,pi/3]);g5 = rotvec2mat([0,0,1,pi]);g6 = rotvec2mat([0,0,1,5*pi/3]);
pts(ind1,:) = pts(ind1,:)*g4;pts(ind2,:) = pts(ind2,:)*g5;pts(ind3,:) = pts(ind3,:)*g6;

phi2 = atan2(pts(ind,2),pts(ind,1));rad1 = hypot(pts(ind,2),pts(ind,1));
pts(ind,:) = [rad1.*cos(3*phi2/2) rad1.*sin(3*phi2/2) pts(ind,3)];

mult = 1 + 1;
phi4 = atan2(pts(ind,1),-pts(ind,3));rad4 = hypot(pts(ind,1),pts(ind,3));
pts(ind,:) = [rad4.*sin(mult*phi4) pts(ind,2) rad4.*-cos(mult*phi4)];

phi3 = atan2(pts(ind,2),pts(ind,1));rad3 = hypot(pts(ind,2),pts(ind,1));
pts(ind,:) = [rad3.*cos(2*phi3/3) rad3.*sin(2*phi3/3) pts(ind,3)];

pts(ind1,:) = pts(ind1,:)*g4';pts(ind2,:) = pts(ind2,:)*g5';pts(ind3,:) = pts(ind3,:)*g6';

g1 = rotvec2mat([0,0,1,pi/3]);
pts=pts*g1;

S = hsvsphere(pts);


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
% ##2
%==============================================================================
%
%   - data                 3d-brains, Omega=(0,20)x(0,10)x(0,20), level=3:6, m=[128,64,128]
%   - viewer               imgmontage
%   - interpolation        linearInter
%   - distance             SSD
%   - pre-registration     affine3D
%   - regularizer          mfElastic
%   - optimizer            Trust-Region
% ===============================================================================

setup3DbrainData;

% prepare the plot
FAIRplots('clear')
Dshow = @(T,R,omega,m) viewIP(abs(T-R),omega,m,'colormap',gray(256));
FAIRplots('set','Dshow',Dshow);

% finally:
% run the MultiLevel Non-Parametric Image Registration
% Trust Region (multigrid preconditioner for conjugate gradient solver)
% no pre-registration

NPIRpara = optPara('TrustRegion','preconditioner',@MGsolver)
[yc,wc,his] = MLIR(ML,'NPIRpara',NPIRpara,'parametric',0);

%==============================================================================


function ica_input = compute_spatiotemporal_ica_input(pca_filters, pca_traces, mu)

	num_PCs = size(pca_traces,1);

	% Create concatenated input for spatio-temporal ICA
	if (mu == 1) % Pure temporal
	    ica_input = pca_traces;
	elseif (mu == 0) % Pure spatial
	    ica_input = pca_filters;
	else % Spatio-temporal
	    ica_input = [(1-mu)*pca_filters, mu*pca_traces];
	    for pc_idx = 1:num_PCs % Renormalize
	        ica_input_row = ica_input(pc_idx,:);
	        ica_input(pc_idx,:) = ica_input_row / sqrt(norm(ica_input_row));
	    end
	end
end


classdef prtBrvDpMixture < prtBrvMixture
    % ds = prtDataGenOldFaithful; x = ds.getX;
    %
    % mm = prtBrvDpMixture('components',repmat(prtBrvMvn,25,1));
    % mm.vbConvergenceThreshold = 1e-6;
    % mm.vbVerboseText = true;
    % mm.vbVerbosePlot = true;
    % mm.vbVerboseMovie = true;
    % [mmLearned, training] = mm.vb(x);

    methods

        function obj = prtBrvDpMixture(varargin)
            obj.mixing = prtBrvDiscreteStickBreaking;

            if nargin < 1
                return
            end
            obj = constructorInputParse(obj,varargin{:});
        end
    end
end


function obj = get_metric(obj)

global Metric Nk Nt Nr Ns n;

V_equal = zeros(Nt,Ns);
W_equal = zeros(Nr,Ns,Nk);

if obj.V_RF == 0
    V_equal = obj.V_B;
    W_equal = obj.W_B;

else
    for k = 1:Nk
        V_equal = obj.V_RF*obj.V_B;
        W_equal(:,:,k) = obj.W_RF*obj.W_B(:,:,k);
    end
end

if (Metric.rate)
    [obj.rate(n),obj.A(:,n)] = get_scrate(V_equal,W_equal);
end

if (Metric.mse)
    obj.mse(n)= get_scmse(V_equal,W_equal);
end

if (Metric.ber)
    [obj.ber(n),obj.suberror(:,n)] = get_scber(V_equal,W_equal);
end


function b = generative_model(A,D,m,modeltype,modelvar,params,epsilon)
%GENERATIVE_MODEL   run generative model code
%
%   B = GENERATIVE_MODEL(A,D,m,modeltype,modelvar,params)
%
%   Generates synthetic networks using the models described in the study by
%   Betzel et al (2016) in Neuroimage.
%
%   Inputs:
%           A,          binary network of seed connections
%           D,          Euclidean distance/fiber length matrix
%           m,          number of connections that should be present in
%                       final synthetic network
%           modeltype,  specifies the generative rule (see below)
%           modelvar,   specifies whether the generative rules are based on
%                       power-law or exponential relationship
%                       ({'powerlaw'}|{'exponential})
%           params,     either a vector (in the case of the geometric
%                       model) or a matrix (for all other models) of
%                       parameters at which the model should be evaluated.
%           epsilon,    the baseline probability of forming a particular
%                       connection (should be a very small number
%                       {default = 1e-5}).
%
%   Output:
%           B,          m x number of networks matrix of connections
%
%
%   Full list of model types:
%   (each model type realizes a different generative rule)
%
%       1.  'sptl'          spatial model
%       2.  'neighbors'     number of common neighbors
%       3.  'matching'      matching index
%       4.  'clu-avg'       average clustering coeff.
%       5.  'clu-min'       minimum clustering coeff.
%       6.  'clu-max'       maximum clustering coeff.
%       7.  'clu-diff'      difference in clustering coeff.
%       8.  'clu-prod'      product of clustering coeff.
%       9.  'deg-avg'       average degree
%       10. 'deg-min'       minimum degree
%       11. 'deg-max'       maximum degree
%       12. 'deg-diff'      difference in degree
%       13. 'deg-prod'      product of degree
%
%
%   Example usage:
%
%       load demo_generative_models_data
%
%       % get number of bi-directional connections
%       m = nnz(A)/2;
%
%       % get cardinality of network
%       n = length(A);
%
%       % set model type
%       modeltype = 'neighbors';
%
%       % set whether the model is based on powerlaw or exponentials
%       modelvar = [{'powerlaw'},{'powerlaw'}];
%
%       % choose some model parameters
%       params = [-2,0.2; -5,1.2; -1,1.5];
%       nparams = size(params,1);
%
%       % generate synthetic networks
%       B = generative_model(Aseed,D,m,modeltype,modelvar,params);
%
%       % store them in adjacency matrix format
%       Asynth = zeros(n,n,nparams);
%       for i = 1:nparams;
%           a = zeros(n); a(B(:,i)) = 1; a = a + a';
%           Asynth(:,:,i) = a;
%       end
%
%   Reference: Betzel et al (2016) Neuroimage 124:1054-64.
%
%   Richard Betzel, Indiana University/University of Pennsylvania, 2015

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

n = length(D);
nparams = size(params,1);
b = zeros(m,nparams);

switch modeltype

    case 'clu-avg'
        clu = clustering_coef_bu(A);
        Kseed = bsxfun(@plus,clu(:,ones(1,n)),clu')/2;
        for iparam = 1:nparams
            eta = params(iparam,1);
            gam = params(iparam,2);
            b(:,iparam) = fcn_clu_avg(A,Kseed,D,m,eta,gam,modelvar,epsilon);
        end

    case 'clu-diff'
        clu = clustering_coef_bu(A);
        Kseed = abs(bsxfun(@minus,clu(:,ones(1,n)),clu'));
        for iparam = 1:nparams
            eta = params(iparam,1);
            gam = params(iparam,2);
            b(:,iparam) = fcn_clu_diff(A,Kseed,D,m,eta,gam,modelvar,epsilon);
        end

    case 'clu-max'
        clu = clustering_coef_bu(A);
        Kseed = bsxfun(@max,clu(:,ones(1,n)),clu');
        for iparam = 1:nparams
            eta = params(iparam,1);
            gam = params(iparam,2);
            b(:,iparam) = fcn_clu_max(A,Kseed,D,m,eta,gam,modelvar,epsilon);
        end

    case 'clu-min'
        clu = clustering_coef_bu(A);
        Kseed = bsxfun(@min,clu(:,ones(1,n)),clu');
        for iparam = 1:nparams
            eta = params(iparam,1);
            gam = params(iparam,2);
            b(:,iparam) = fcn_clu_min(A,Kseed,D,m,eta,gam,modelvar,epsilon);
        end

    case 'clu-prod'
        clu = clustering_coef_bu(A);
        Kseed = clu*clu';
        for iparam = 1:nparams
            eta = params(iparam,1);
            gam = params(iparam,2);
            b(:,iparam) = fcn_clu_prod(A,Kseed,D,m,eta,gam,modelvar,epsilon);
        end

    case 'deg-avg'
        kseed = sum(A,2);
        Kseed = bsxfun(@plus,kseed(:,ones(1,n)),kseed')/2;
        for iparam = 1:nparams
            eta = params(iparam,1);
            gam = params(iparam,2);
            b(:,iparam) = fcn_deg_avg(A,Kseed,D,m,eta,gam,modelvar,epsilon);
        end

    case 'deg-diff'
        kseed = sum(A,2);
        Kseed = abs(bsxfun(@minus,kseed(:,ones(1,n)),kseed'));
        for iparam = 1:nparams
            eta = params(iparam,1);
            gam = params(iparam,2);
            b(:,iparam) = fcn_deg_diff(A,Kseed,D,m,eta,gam,modelvar,epsilon);
        end

    case 'deg-max'
        kseed = sum(A,2);
        Kseed = bsxfun(@max,kseed(:,ones(1,n)),kseed');
        for iparam = 1:nparams
            eta = params(iparam,1);
            gam = params(iparam,2);
            b(:,iparam) = fcn_deg_max(A,Kseed,D,m,eta,gam,modelvar,epsilon);
        end

    case 'deg-min'
        kseed = sum(A,2);
        Kseed = bsxfun(@min,kseed(:,ones(1,n)),kseed');
        for iparam = 1:nparams
            eta = params(iparam,1);
            gam = params(iparam,2);
            b(:,iparam) = fcn_deg_min(A,Kseed,D,m,eta,gam,modelvar,epsilon);
        end

    case 'deg-prod'
        kseed = sum(A,2);
        Kseed = (kseed*kseed').*~eye(n);
        for iparam = 1:nparams
            eta = params(iparam,1);
            gam = params(iparam,2);
            b(:,iparam) = fcn_deg_prod(A,Kseed,D,m,eta,gam,modelvar,epsilon);
        end

    case 'neighbors'
        Kseed = (A*A).*~eye(n);
        for iparam = 1:nparams
            eta = params(iparam,1);
            gam = params(iparam,2);
            b(:,iparam) = fcn_nghbrs(A,Kseed,D,m,eta,gam,modelvar,epsilon);
        end

    case 'matching'
        Kseed = matching_ind(A);
        Kseed = Kseed + Kseed';
        for iparam = 1:nparams
            eta = params(iparam,1);
            gam = params(iparam,2);
            b(:,iparam) = fcn_matching(A,Kseed,D,m,eta,gam,modelvar,epsilon);
        end

    case 'sptl'
        for iparam = 1:nparams
            eta = params(iparam,1);
            b(:,iparam) = fcn_sptl(A,D,m,eta,modelvar{1});
        end

end

function b = fcn_clu_avg(A,K,D,m,eta,gam,modelvar,epsilon)
K = K + epsilon;
n = length(D);
mseed = nnz(A)/2;
A = A > 0;
mv1 = modelvar{1};
mv2 = modelvar{2};
switch mv1
    case 'powerlaw'
        Fd = D.^eta;
    case 'exponential'
        Fd = exp(eta*D);
end
switch mv2
    case 'powerlaw'
        Fk = K.^gam;
    case 'exponential'
        Fk = exp(gam*K);
end

c = clustering_coef_bu(A);
k = sum(A,2);

Ff = Fd.*Fk.*~A;
[u,v] = find(triu(ones(n),1));
indx = (v - 1)*n + u;
P = Ff(indx);

for i = (mseed + 1):m
    C = [0; cumsum(P)];
    r = sum(rand*C(end) >= C);
    uu = u(r);
    vv = v(r);
    A(uu,vv) = 1;
    A(vv,uu) = 1;
    k([uu,vv]) = k([uu,vv]) + 1;
    bu = A(uu,:);
    su = A(bu,bu);
    bv = A(vv,:);
    sv = A(bv,bv);
    bth = bu & bv;
    c(bth) = c(bth) + 2./(k(bth).^2 - k(bth));
    c(uu) = nnz(su)/(k(uu)*(k(uu) - 1));
    c(vv) = nnz(sv)/(k(vv)*(k(vv) - 1));
    c(k <= 1) = 0;
    bth([uu,vv]) = true;
    K(:,bth) = bsxfun(@plus,c(:,ones(1,sum(bth))),c(bth,:)')/2 + epsilon;
    K(bth,:) = bsxfun(@plus,c(:,ones(1,sum(bth))),c(bth,:)')'/2 + epsilon;

    switch mv2
        case 'powerlaw'
            Ff(bth,:) = Fd(bth,:).*((K(bth,:)).^gam);
            Ff(:,bth) = Fd(:,bth).*((K(:,bth)).^gam);
        case 'exponential'
            Ff(bth,:) = Fd(bth,:).*exp((K(bth,:))*gam);
            Ff(:,bth) = Fd(:,bth).*exp((K(:,bth))*gam);
    end
    Ff = Ff.*~A;
    P = Ff(indx);
end
b = find(triu(A,1));

function b = fcn_clu_diff(A,K,D,m,eta,gam,modelvar,epsilon)
K = K + epsilon;
n = length(D);
mseed = nnz(A)/2;
A = A > 0;
mv1 = modelvar{1};
mv2 = modelvar{2};
switch mv1
    case 'powerlaw'
        Fd = D.^eta;
    case 'exponential'
        Fd = exp(eta*D);
end
switch mv2
    case 'powerlaw'
        Fk = K.^gam;
    case 'exponential'
        Fk = exp(gam*K);
end

c = clustering_coef_bu(A);
k = sum(A,2);

Ff = Fd.*Fk.*~A;
[u,v] = find(triu(ones(n),1));
indx = (v - 1)*n + u;
P = Ff(indx);

for i = (mseed + 1):m
    C = [0; cumsum(P)];
    r = sum(rand*C(end) >= C);
    uu = u(r);
    vv = v(r);
    A(uu,vv) = 1;
    A(vv,uu) = 1;
    k([uu,vv]) = k([uu,vv]) + 1;
    bu = A(uu,:);
    su = A(bu,bu);
    bv = A(vv,:);
    sv = A(bv,bv);
    bth = bu & bv;
    c(bth) = c(bth) + 2./(k(bth).^2 - k(bth));
    c(uu) = nnz(su)/(k(uu)*(k(uu) - 1));
    c(vv) = nnz(sv)/(k(vv)*(k(vv) - 1));
    c(k <= 1) = 0;
    bth([uu,vv]) = true;
    K(:,bth) = abs(bsxfun(@minus,c(:,ones(1,sum(bth))),c(bth,:)')) + epsilon;
    K(bth,:) = abs(bsxfun(@minus,c(:,ones(1,sum(bth))),c(bth,:)'))' + epsilon;

    switch mv2
        case 'powerlaw'
            Ff(bth,:) = Fd(bth,:).*((K(bth,:)).^gam);
            Ff(:,bth) = Fd(:,bth).*((K(:,bth)).^gam);
        case 'exponential'
            Ff(bth,:) = Fd(bth,:).*exp((K(bth,:))*gam);
            Ff(:,bth) = Fd(:,bth).*exp((K(:,bth))*gam);
    end
    Ff = Ff.*~A;
    P = Ff(indx);
end
b = find(triu(A,1));

function b = fcn_clu_max(A,K,D,m,eta,gam,modelvar,epsilon)
K = K + epsilon;
n = length(D);
mseed = nnz(A)/2;
A = A > 0;
mv1 = modelvar{1};
mv2 = modelvar{2};
switch mv1
    case 'powerlaw'
        Fd = D.^eta;
    case 'exponential'
        Fd = exp(eta*D);
end
switch mv2
    case 'powerlaw'
        Fk = K.^gam;
    case 'exponential'
        Fk = exp(gam*K);
end

c = clustering_coef_bu(A);
k = sum(A,2);

Ff = Fd.*Fk.*~A;
[u,v] = find(triu(ones(n),1));
indx = (v - 1)*n + u;
P = Ff(indx);

for i = (mseed + 1):m
    C = [0; cumsum(P)];
    r = sum(rand*C(end) >= C);
    uu = u(r);
    vv = v(r);
    A(uu,vv) = 1;
    A(vv,uu) = 1;
    k([uu,vv]) = k([uu,vv]) + 1;
    bu = A(uu,:);
    su = A(bu,bu);
    bv = A(vv,:);
    sv = A(bv,bv);
    bth = bu & bv;
    c(bth) = c(bth) + 2./(k(bth).^2 - k(bth));
    c(uu) = nnz(su)/(k(uu)*(k(uu) - 1));
    c(vv) = nnz(sv)/(k(vv)*(k(vv) - 1));
    c(k <= 1) = 0;
    bth([uu,vv]) = true;
    K(:,bth) = bsxfun(@max,c(:,ones(1,sum(bth))),c(bth,:)') + epsilon;
    K(bth,:) = bsxfun(@max,c(:,ones(1,sum(bth))),c(bth,:)')' + epsilon;

    switch mv2
        case 'powerlaw'
            Ff(bth,:) = Fd(bth,:).*((K(bth,:)).^gam);
            Ff(:,bth) = Fd(:,bth).*((K(:,bth)).^gam);
        case 'exponential'
            Ff(bth,:) = Fd(bth,:).*exp((K(bth,:))*gam);
            Ff(:,bth) = Fd(:,bth).*exp((K(:,bth))*gam);
    end
    Ff = Ff.*~A;
    P = Ff(indx);
end
b = find(triu(A,1));

function b = fcn_clu_min(A,K,D,m,eta,gam,modelvar,epsilon)
K = K + epsilon;
n = length(D);
mseed = nnz(A)/2;
A = A > 0;
mv1 = modelvar{1};
mv2 = modelvar{2};
switch mv1
    case 'powerlaw'
        Fd = D.^eta;
    case 'exponential'
        Fd = exp(eta*D);
end
switch mv2
    case 'powerlaw'
        Fk = K.^gam;
    case 'exponential'
        Fk = exp(gam*K);
end

c = clustering_coef_bu(A);
k = sum(A,2);

Ff = Fd.*Fk.*~A;
[u,v] = find(triu(ones(n),1));
indx = (v - 1)*n + u;
P = Ff(indx);

for i = (mseed + 1):m
    C = [0; cumsum(P)];
    r = sum(rand*C(end) >= C);
    uu = u(r);
    vv = v(r);
    A(uu,vv) = 1;
    A(vv,uu) = 1;
    k([uu,vv]) = k([uu,vv]) + 1;
    bu = A(uu,:);
    su = A(bu,bu);
    bv = A(vv,:);
    sv = A(bv,bv);
    bth = bu & bv;
    c(bth) = c(bth) + 2./(k(bth).^2 - k(bth));
    c(uu) = nnz(su)/(k(uu)*(k(uu) - 1));
    c(vv) = nnz(sv)/(k(vv)*(k(vv) - 1));
    c(k <= 1) = 0;
    bth([uu,vv]) = true;
    K(:,bth) = bsxfun(@min,c(:,ones(1,sum(bth))),c(bth,:)') + epsilon;
    K(bth,:) = bsxfun(@min,c(:,ones(1,sum(bth))),c(bth,:)')' + epsilon;

    switch mv2
        case 'powerlaw'
            Ff(bth,:) = Fd(bth,:).*((K(bth,:)).^gam);
            Ff(:,bth) = Fd(:,bth).*((K(:,bth)).^gam);
        case 'exponential'
            Ff(bth,:) = Fd(bth,:).*exp((K(bth,:))*gam);
            Ff(:,bth) = Fd(:,bth).*exp((K(:,bth))*gam);
    end
    Ff = Ff.*~A;
    P = Ff(indx);
end
b = find(triu(A,1));

function b = fcn_clu_prod(A,K,D,m,eta,gam,modelvar,epsilon)
K = K + epsilon;
n = length(D);
mseed = nnz(A)/2;
A = A > 0;
mv1 = modelvar{1};
mv2 = modelvar{2};
switch mv1
    case 'powerlaw'
        Fd = D.^eta;
    case 'exponential'
        Fd = exp(eta*D);
end
switch mv2
    case 'powerlaw'
        Fk = K.^gam;
    case 'exponential'
        Fk = exp(gam*K);
end

c = clustering_coef_bu(A);
k = sum(A,2);

Ff = Fd.*Fk.*~A;
[u,v] = find(triu(ones(n),1));
indx = (v - 1)*n + u;
P = Ff(indx);

for i = (mseed + 1):m
    C = [0; cumsum(P)];
    r = sum(rand*C(end) >= C);
    uu = u(r);
    vv = v(r);
    A(uu,vv) = 1;
    A(vv,uu) = 1;
    k([uu,vv]) = k([uu,vv]) + 1;
    bu = A(uu,:);
    su = A(bu,bu);
    bv = A(vv,:);
    sv = A(bv,bv);
    bth = bu & bv;
    c(bth) = c(bth) + 2./(k(bth).^2 - k(bth));
    c(uu) = nnz(su)/(k(uu)*(k(uu) - 1));
    c(vv) = nnz(sv)/(k(vv)*(k(vv) - 1));
    c(k <= 1) = 0;
    bth([uu,vv]) = true;
    K(bth,:) = (c(bth,:)*c') + epsilon;
    K(:,bth) = (c*c(bth,:)') + epsilon;

    switch mv2
        case 'powerlaw'
            Ff(bth,:) = Fd(bth,:).*((K(bth,:)).^gam);
            Ff(:,bth) = Fd(:,bth).*((K(:,bth)).^gam);
        case 'exponential'
            Ff(bth,:) = Fd(bth,:).*exp((K(bth,:))*gam);
            Ff(:,bth) = Fd(:,bth).*exp((K(:,bth))*gam);
    end
    Ff = Ff.*~A;
    P = Ff(indx);
end
b = find(triu(A,1));

function b = fcn_deg_avg(A,K,D,m,eta,gam,modelvar,epsilon)
n = length(D);
mseed = nnz(A)/2;
k = sum(A,2);
[u,v] = find(triu(ones(n),1));
indx = (v - 1)*n + u;
D = D(indx);
mv1 = modelvar{1};
mv2 = modelvar{2};
switch mv1
    case 'powerlaw'
        Fd = D.^eta;
    case 'exponential'
        Fd = exp(eta*D);
end
K = K + epsilon;
switch mv2
    case 'powerlaw'
        Fk = K.^gam;
    case 'exponential'
        Fk = exp(gam*K);
end
P = Fd.*Fk(indx).*~A(indx);
b = zeros(m,1);
b(1:mseed) = find(A(indx));
for i = (mseed + 1):m
    C = [0; cumsum(P)];
    r = sum(rand*C(end) >= C);
    w = [u(r),v(r)];
    k(w) = k(w) + 1;
    switch mv2
        case 'powerlaw'
            Fk(:,w) = [((k + k(w(1)))/2) + epsilon, ((k + k(w(2)))/2) + epsilon].^gam;
            Fk(w,:) = ([((k + k(w(1)))/2) + epsilon, ((k + k(w(2)))/2) + epsilon].^gam)';
        case 'exponential'
            Fk(:,w) = exp([((k + k(w(1)))/2) + epsilon, ((k + k(w(2)))/2) + epsilon]*gam);
            Fk(w,:) = exp([((k + k(w(1)))/2) + epsilon, ((k + k(w(2)))/2) + epsilon]*gam)';
    end
    P = Fd.*Fk(indx);
    b(i) = r;
    P(b(1:i)) = 0;
end
b = indx(b);

function b = fcn_deg_diff(A,K,D,m,eta,gam,modelvar,epsilon)
n = length(D);
mseed = nnz(A)/2;
k = sum(A,2);
[u,v] = find(triu(ones(n),1));
indx = (v - 1)*n + u;
D = D(indx);
mv1 = modelvar{1};
mv2 = modelvar{2};
switch mv1
    case 'powerlaw'
        Fd = D.^eta;
    case 'exponential'
        Fd = exp(eta*D);
end
K = K + epsilon;
switch mv2
    case 'powerlaw'
        Fk = K.^gam;
    case 'exponential'
        Fk = exp(gam*K);
end
P = Fd.*Fk(indx).*~A(indx);
b = zeros(m,1);
b(1:mseed) = find(A(indx));
for i = (mseed + 1):m
    C = [0; cumsum(P)];
    r = sum(rand*C(end) >= C);

    w = [u(r),v(r)];
    k(w) = k(w) + 1;
    switch mv2
        case 'powerlaw'
            Fk(:,w) = (abs([k - k(w(1)), k - k(w(2))]) + epsilon).^gam;
            Fk(w,:) = ((abs([k - k(w(1)), k - k(w(2))]) + epsilon).^gam)';
        case 'exponential'
            Fk(:,w) = exp((abs([k - k(w(1)), k - k(w(2))]) + epsilon)*gam);
            Fk(w,:) = exp((abs([k - k(w(1)), k - k(w(2))]) + epsilon)*gam)';
    end
    P = Fd.*Fk(indx);
    b(i) = r;
    P(b(1:i)) = 0;
end
b = indx(b);

function b = fcn_deg_min(A,K,D,m,eta,gam,modelvar,epsilon)
n = length(D);
mseed = nnz(A)/2;
k = sum(A,2);
[u,v] = find(triu(ones(n),1));
indx = (v - 1)*n + u;
D = D(indx);
mv1 = modelvar{1};
mv2 = modelvar{2};
switch mv1
    case 'powerlaw'
        Fd = D.^eta;
    case 'exponential'
        Fd = exp(eta*D);
end
K = K + epsilon;
switch mv2
    case 'powerlaw'
        Fk = K.^gam;
    case 'exponential'
        Fk = exp(gam*K);
end
P = Fd.*Fk(indx).*~A(indx);
b = zeros(m,1);
b(1:mseed) = find(A(indx));
for i = (mseed + 1):m
    C = [0; cumsum(P)];
    r = sum(rand*C(end) >= C);
    w = [u(r),v(r)];
    k(w) = k(w) + 1;
    switch mv2
        case 'powerlaw'
            Fk(:,w) = [min(k,k(w(1))) + epsilon, min(k,k(w(2))) + epsilon].^gam;
            Fk(w,:) = ([min(k,k(w(1))) + epsilon, min(k,k(w(2))) + epsilon].^gam)';
        case 'exponential'
            Fk(:,w) = exp([min(k,k(w(1))) + epsilon, min(k,k(w(2))) + epsilon]*gam);
            Fk(w,:) = exp([min(k,k(w(1))) + epsilon, min(k,k(w(2))) + epsilon]*gam)';
    end
    P = Fd.*Fk(indx);
    b(i) = r;
    P(b(1:i)) = 0;
end
b = indx(b);

function b = fcn_deg_max(A,K,D,m,eta,gam,modelvar,epsilon)
n = length(D);
mseed = nnz(A)/2;
k = sum(A,2);
[u,v] = find(triu(ones(n),1));
indx = (v - 1)*n + u;
D = D(indx);
mv1 = modelvar{1};
mv2 = modelvar{2};
switch mv1
    case 'powerlaw'
        Fd = D.^eta;
    case 'exponential'
        Fd = exp(eta*D);
end
K = K + epsilon;
switch mv2
    case 'powerlaw'
        Fk = K.^gam;
    case 'exponential'
        Fk = exp(gam*K);
end
P = Fd.*Fk(indx).*~A(indx);
b = zeros(m,1);
b(1:mseed) = find(A(indx));
for i = (mseed + 1):m
    C = [0; cumsum(P)];
    r = sum(rand*C(end) >= C);
    w = [u(r),v(r)];
    k(w) = k(w) + 1;
    switch mv2
        case 'powerlaw'
            Fk(:,w) = [max(k,k(w(1))) + epsilon, max(k,k(w(2))) + epsilon].^gam;
            Fk(w,:) = ([max(k,k(w(1))) + epsilon, max(k,k(w(2))) + epsilon].^gam)';
        case 'exponential'
            Fk(:,w) = exp([max(k,k(w(1))) + epsilon, max(k,k(w(2))) + epsilon]*gam);
            Fk(w,:) = exp([max(k,k(w(1))) + epsilon, max(k,k(w(2))) + epsilon]*gam)';
    end
    P = Fd.*Fk(indx);
    b(i) = r;
    P(b(1:i)) = 0;
end
b = indx(b);

function b = fcn_deg_prod(A,K,D,m,eta,gam,modelvar,epsilon)
n = length(D);
mseed = nnz(A)/2;
k = sum(A,2);
[u,v] = find(triu(ones(n),1));
indx = (v - 1)*n + u;
D = D(indx);
mv1 = modelvar{1};
mv2 = modelvar{2};
switch mv1
    case 'powerlaw'
        Fd = D.^eta;
    case 'exponential'
        Fd = exp(eta*D);
end
K = K + epsilon;
switch mv2
    case 'powerlaw'
        Fk = K.^gam;
    case 'exponential'
        Fk = exp(gam*K);
end
P = Fd.*Fk(indx).*~A(indx);
b = zeros(m,1);
b(1:mseed) = find(A(indx));
for i = (mseed + 1):m
    C = [0; cumsum(P)];
    r = sum(rand*C(end) >= C);
    w = [u(r),v(r)];
    k(w) = k(w) + 1;
    switch mv2
        case 'powerlaw'
            Fk(:,w) = ([k*k(w(1)) + epsilon, k*k(w(2)) + epsilon].^gam);
            Fk(w,:) = (([k*k(w(1)) + epsilon, k*k(w(2)) + epsilon].^gam)');
        case 'exponential'
            Fk(:,w) = exp([k*k(w(1)) + epsilon, k*k(w(2)) + epsilon]*gam);
            Fk(w,:) = exp([k*k(w(1)) + epsilon, k*k(w(2)) + epsilon]*gam)';
    end
    P = Fd.*Fk(indx);
    b(i) = r;
    P(b(1:i)) = 0;
end
b = indx(b);

function b = fcn_nghbrs(A,K,D,m,eta,gam,modelvar,epsilon)
K = K + epsilon;
n = length(D);
mseed = nnz(A)/2;
A = A > 0;
mv1 = modelvar{1};
mv2 = modelvar{2};
switch mv1
    case 'powerlaw'
        Fd = D.^eta;
    case 'exponential'
        Fd = exp(eta*D);
end
switch mv2
    case 'powerlaw'
%         gam = abs(gam);
        Fk = K.^gam;
    case 'exponential'
        Fk = exp(gam*K);
end
Ff = Fd.*Fk.*~A;
[u,v] = find(triu(ones(n),1));
indx = (v - 1)*n + u;
P = Ff(indx);
for i = (mseed + 1):m
    C = [0; cumsum(P)];
    r = sum(rand*C(end) >= C);
    uu = u(r);
    vv = v(r);
    x = A(uu,:);
    y = A(:,vv);
    A(uu,vv) = 1;
    A(vv,uu) = 1;
    K(uu,y) = K(uu,y) + 1;
    K(y,uu) = K(y,uu) + 1;
    K(vv,x) = K(vv,x) + 1;
    K(x,vv) = K(x,vv) + 1;
    switch mv2
        case 'powerlaw'
            Ff(uu,y) = Fd(uu,y).*(K(uu,y).^gam);
            Ff(y,uu) = Ff(uu,y)';
            Ff(vv,x) = Fd(vv,x).*(K(vv,x).^gam);
            Ff(x,vv) = Ff(vv,x)';
        case 'exponential'
            Ff(uu,y) = Fd(uu,y).*exp(K(uu,y)*gam);
            Ff(y,uu) = Ff(uu,y)';
            Ff(vv,x) = Fd(vv,x).*exp(K(vv,x)*gam);
            Ff(x,vv) = Ff(vv,x)';
    end
    Ff(A) = 0;
    P = Ff(indx);
end
b = find(triu(A,1));

function b = fcn_matching(A,K,D,m,eta,gam,modelvar,epsilon)
K = K + epsilon;
n = length(D);
mseed = nnz(A)/2;
mv1 = modelvar{1};
mv2 = modelvar{2};
switch mv1
    case 'powerlaw'
        Fd = D.^eta;
    case 'exponential'
        Fd = exp(eta*D);
end
switch mv2
    case 'powerlaw'
        Fk = K.^gam;
    case 'exponential'
        Fk = exp(gam*K);
end
Ff = Fd.*Fk.*~A;
[u,v] = find(triu(ones(n),1));
indx = (v - 1)*n + u;
P = Ff(indx);
for ii = (mseed + 1):m

    C = [0; cumsum(P)];
    r = sum(rand*C(end) >= C);
    uu = u(r);
    vv = v(r);

    A(uu,vv) = 1;
    A(vv,uu) = 1;

    updateuu = find(A*A(:,uu));
    updateuu(updateuu == uu) = [];
    updateuu(updateuu == vv) = [];

    updatevv = find(A*A(:,vv));
    updatevv(updatevv == uu) = [];
    updatevv(updatevv == vv) = [];

    c1 = [A(:,uu)', A(uu,:)];
    for i = 1:length(updateuu)
        j = updateuu(i);
        c2 = [A(:,j)' A(j,:)];
        use = ~(~c1&~c2);
        use(uu) = 0;  use(uu+n) = 0;
        use(j) = 0;  use(j+n) = 0;
        ncon = sum(c1(use))+sum(c2(use));
        if (ncon==0)
            K(uu,j) = epsilon;
            K(j,uu) = epsilon;
        else
            K(uu,j) = (2*(sum(c1(use)&c2(use))/ncon)) + epsilon;
            K(j,uu) = K(uu,j);
        end

    end

    c1 = [A(:,vv)', A(vv,:)];
    for i = 1:length(updatevv)
        j = updatevv(i);
        c2 = [A(:,j)' A(j,:)];
        use = ~(~c1&~c2);
        use(vv) = 0;  use(vv+n) = 0;
        use(j) = 0;  use(j+n) = 0;
        ncon = sum(c1(use))+sum(c2(use));
        if (ncon==0)
            K(vv,j) = epsilon;
            K(j,vv) = epsilon;
        else
            K(vv,j) = (2*(sum(c1(use)&c2(use))/ncon)) + epsilon;
            K(j,vv) = K(vv,j);
        end
    end
    switch mv2
        case 'powerlaw'
            Fk = K.^gam;
        case 'exponential'
            Fk = exp(gam*K);
    end
    Ff = Fd.*Fk.*~A;
    P = Ff(indx);
end
b = find(triu(A,1));

function b = fcn_sptl(A,D,m,eta,modelvar)
n = length(D);
mseed = nnz(A)/2;
switch modelvar
    case 'powerlaw'
        Fd = D.^eta;
    case 'exponential'
        Fd = exp(eta*D);
end
[u,v] = find(triu(ones(n),1));
indx = (v - 1)*n + u;
P = Fd(indx).*~A(indx);
b = zeros(m,1);
b(1:mseed) = find(A(indx));
for i = (mseed + 1):m
    C = [0; cumsum(P)];
    r = sum(rand*C(end) >= C);
    b(i) = r;
    P = Fd(indx);
    P(b(1:i)) = 0;
end
b = indx(b);


filename='Arch_triangle_coarse';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance';'perimeter'};
weights = [1, 0.1];
constraint = {'volumeConstraint'};
optimizer = 'MMA'; incrementFactor = 1;
filterType = 'P1';

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

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


function Rb2s = flu2rdf

% FLU2RDF Camera body to camera sensor rotation matrix.
%   FLU2RDF computes the rotation matrix for a camera whos body
%   is in the FLU frame (x-front, y-left, z-up) and its sensor in
%   the RDF frame (x-right, y-down, z-front).

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

Rb2s = e2R([-pi/2 0 -pi/2]');

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

%   # START GPL LICENSE

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

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

%   # END GPL LICENSE


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

%       O. Lemoine - March 1996.

% We test each property of the corresponding TFR :

N=128;

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

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

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

% Time-marginal
sig=noisecg(N);
tfr=tfrpmh(sig);
ip1=abs(sig).^2;
ip2=mean(tfr)';
if any(abs(ip1-ip2)>sqrt(eps)),
 error('tfrpmh test 4 failed');
end

% Conservation of the time support (wide-sense)
sig=[zeros(N/4,1);noisecg(N/2);zeros(N/4,1)];
tfr=tfrpmh(sig);
if sum(any(abs(tfr(:,1:N/4-1))>sqrt(eps))) | ...
   sum(any(abs(tfr(:,(3*N/4+1):N))>sqrt(eps))),
 error('tfrpmh test 5 failed');
end

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

% A PMHD with a constant window is a MHD
sig=fmlin(N).*amgauss(N);
tfr1=tfrpmh(sig,1:N,N,ones(2*N+1,1));
tfr2=tfrmh(sig);
if max(max(abs(tfr1-tfr2)))>1e-5,
 error('tfrpmh test 7 failed');
end

N=131;

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

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

% Energy conservation
sig=noisecg(N);
tfr=tfrpmh(sig);
Es=norm(sig)^2;
Etfr=sum(mean(tfr));
if abs(Es-Etfr)>sqrt(eps),
 error('tfrpmh test 10 failed');
end

% Time-marginal
sig=noisecg(N);
tfr=tfrpmh(sig);
ip1=abs(sig).^2;
ip2=mean(tfr)';
if any(abs(ip1-ip2)>sqrt(eps)),
 error('tfrpmh test 11 failed');
end

% Conservation of the time support (wide-sense)
sig=[zeros(round(N/4),1);noisecg(round(N/2));zeros(round(N/4),1)];
tfr=tfrpmh(sig);
if sum(any(abs(tfr(:,1:round(N/4)-1))>sqrt(eps))) | ...
   sum(any(abs(tfr(:,(round(3*N/4)+2):N))>sqrt(eps))),
 error('tfrpmh test 12 failed');
end

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

% A PMHD with a constant window is a MHD
sig=fmlin(N).*amgauss(N);
tfr1=tfrpmh(sig,1:N,N,ones(2*N+1,1));
tfr2=tfrmh(sig);
if max(max(abs(tfr1-tfr2)))>1e-5,
 error('tfrpmh test 14 failed');
end


function y = polycomb(varargin)

% Simplified Version of Combvec (Mark Beale, 12-15-93)

y = varargin{1};
for i=2:length(varargin)
    z = varargin{i};
    y = [copy_blocked(y,size(z,2)); copy_interleaved(z,size(y,2))];
end

%=========================================================
function b = copy_blocked(m,n)

[mr,mc] = size(m);
b = zeros(mr,mc*n);
ind = 1:mc;
for i=[0:(n-1)]*mc
  b(:,ind+i) = m;
end
%=========================================================

function b = copy_interleaved(m,n)

[mr,mc] = size(m);
b = zeros(mr*n,mc);
ind = 1:mr;
for i=[0:(n-1)]*mr
  b(ind+i,:) = m;
end
b = reshape(b,mr,n*mc);


%% Epipolar Geometry
%
% In this sample:
%
% * We will learn about the basics of multiview geometry
% * We will see what is epipole, epipolar lines, epipolar constraint etc.
%
% Sources:
%
% * <https://docs.opencv.org/3.2.0/da/de9/tutorial_py_epipolar_geometry.html>
%

%% Theory
%
% When we take an image using pin-hole camera, we loose an important
% information, i.e depth of the image, or how far is each point in the image
% from the camera because it is a 3D-to-2D conversion. So it is an important
% question whether we can find the depth information using these cameras. And
% the answer is to use more than one camera. Our eyes works in similar way
% where we use two cameras (two eyes) which is called stereo vision. So let's
% see what OpenCV provides in this field.
%
% (_Learning OpenCV_ by Gary Bradsky has a lot of information in this field.)
%
% Before going to depth images, let's first understand some basic concepts in
% multiview geometry. In this section we will deal with epipolar geometry. See
% the image below which shows a basic setup with two cameras taking the image
% of same scene.
%
% <<https://docs.opencv.org/3.2.0/epipolar.jpg>>
%
% If we are using only the left camera, we can't find the 3D point
% corresponding to the point $x$ in image because every point on the line
% $OX$ projects to the same point on the image plane. But consider the right
% image also. Now different points on the line $OX$ projects to different
% points ($x'$) in right plane. So with these two images, we can triangulate
% the correct 3D point. This is the whole idea.
%
% The projection of the different points on $OX$ form a line on right plane
% (line $l'$). We call it *epiline* corresponding to the point $x$. It means,
% to find the point $x$ on the right image, search along this epiline. It
% should be somewhere on this line. (Think of it this way, to find the
% matching point in other image, you need not search the whole image, just
% search along the epiline. So it provides better performance and accuracy).
% This is called *Epipolar Constraint*. Similarly all points will have its
% corresponding epilines in the other image. The plane $XOO'$ is called
% *Epipolar Plane*.
%
% $O$ and $O'$ are the camera centers. From the setup given above, you can see
% that projection of right camera $O'$ is seen on the left image at the point,
% $e$. It is called the *epipole*. Epipole is the point of intersection of
% line through camera centers and the image planes. Similarly $e'$ is the
% epipole of the left camera. In some cases, you won't be able to locate the
% epipole in the image, they may be outside the image (which means, one camera
% doesn't see the other).
%
% All the epilines pass through its epipole. So to find the location of
% epipole, we can find many epilines and find their intersection point.
%
% So in this session, we focus on finding epipolar lines and epipoles. But to
% find them, we need two more ingredients, *Fundamental Matrix (F)* and
% *Essential Matrix (E)*. Essential Matrix contains the information about
% translation and rotation, which describe the location of the second camera
% relative to the first in global coordinates. See the image below
% (Image courtesy: _Learning OpenCV_ by Gary Bradsky):
%
% <<https://docs.opencv.org/3.2.0/essential_matrix.jpg>>
%
% But we prefer measurements to be done in pixel coordinates, right?
% Fundamental Matrix contains the same information as Essential Matrix in
% addition to the information about the intrinsics of both cameras so that we
% can relate the two cameras in pixel coordinates. (If we are using rectified
% images and normalize the point by dividing by the focal lengths, $F=E$). In
% simple words, Fundamental Matrix F, maps a point in one image to a line
% (epiline) in the other image. This is calculated from matching points from
% both the images. A minimum of 8 such points are required to find the
% fundamental matrix (while using 8-point algorithm). More points are
% preferred and use RANSAC to get a more robust result.
%

%% Code
%
% First we need to find as many possible matches between two images to find
% the fundamental matrix. For this, we use SIFT descriptors with FLANN based
% matcher and ratio test.
%
% Next we find the Fundamental Matrix from the list of best matches from both
% the images.
%
% Then we find the epilines. Epilines corresponding to the points in first
% image are drawn on second image. So mentioning of correct images are
% important here. We get an array of lines. So we define a new function to
% draw these lines on the images.
%
% Below is the result we get:
%
% <<https://docs.opencv.org/3.2.0/epiresult.jpg>>
%
% You can see in the left image that all epilines are converging at a point
% outside the image at right side. That meeting point is the epipole.
%
% For better results, images with good resolution and many non-planar points
% should be used.
%
% Notes:
%
% * One important topic is the forward movement of camera. Then epipoles will
%   be seen at the same locations in both with epilines emerging from a fixed
%   point. See
%   <http://answers.opencv.org/question/17912/location-of-epipole/ this>.
% * Fundamental Matrix estimation is sensitive to quality of matches, outliers
%   etc. It becomes worse when all selected matches lie on the same plane. See
%   <http://answers.opencv.org/question/18125/epilines-not-correct/ this>.
%

function epipolar_geometry_demo()
    %%
    % a pair of stereo images (grayscale)
    img1 = cv.imread(fullfile(mexopencv.root(),'test','books_left.jpg'), ...
        'Grayscale',true);  % query image
    img2 = cv.imread(fullfile(mexopencv.root(),'test','books_right.jpg'), ...
        'Grayscale',true); % train image

    %%
    % detect keypoints and calculate descriptors using SIFT
    obj = cv.SIFT('ConstrastThreshold',0.03);
    [kp1,desc1] = obj.detectAndCompute(img1);
    [kp2,desc2] = obj.detectAndCompute(img2);

    %%
    % match descriptors using FLANN
    matcher = cv.DescriptorMatcher('FlannBasedMatcher', ...
        'Index',{'KDTree', 'Trees',5}, 'Search',{'Checks',50});
    m = matcher.knnMatch(desc1, desc2, 2);

    %%
    % keep only "good" matches (ratio test as per Lowe's paper)
    m = cat(1, m{:});    % N-by-2 array of structs
    idx_good = ([m(:,1).distance] < 0.8*[m(:,2).distance]);
    m = m(idx_good,1);

    %%
    % extract keypoints from filtered matches
    pts1 = cat(1, kp1([m.queryIdx]+1).pt);
    pts2 = cat(1, kp2([m.trainIdx]+1).pt);
    if true
        pts1 = int32(pts1);
        pts2 = int32(pts2);
    end

    %%
    % find Fundamental matrix
    [F,mask] = cv.findFundamentalMat(pts1, pts2, 'Method','LMedS');
    mask = logical(mask);

    %%
    % select only inlier points
    pts1 = pts1(mask,:);
    pts2 = pts2(mask,:);

    %%
    % random colors to draw matching points and lines
    clrs = randi([0 255], [size(pts1,1) 3], 'uint8');
    clrs(:,4) = 0;

    %%
    % find epilines corresponding to points in right image (second image)
    % and draw its lines on left image
    lines1 = cv.computeCorrespondEpilines(pts2, F, 'WhichImage',2);
    [img11,~] = drawlines(img1, img2, lines1, pts1, pts2, clrs);

    %%
    % find epilines corresponding to points in left image (first image)
    % and draw its lines on right image
    lines2 = cv.computeCorrespondEpilines(pts1, F, 'WhichImage',1);
    [img22,~] = drawlines(img2, img1, lines2, pts2, pts1, clrs);

    %%
    % show result
    if ~mexopencv.isOctave() && mexopencv.require('images')
        imshowpair(img11, img22, 'montage')
        title('Left/Right')
    else
        figure('Position',[200 200 1200 400])
        subplot(121), imshow(img11), title('Left')
        subplot(122), imshow(img22), title('Right')
    end
end

function [img1,img2] = drawlines(img1, img2, lines, pts1, pts2, clrs)
    %DRAWLINES  Draw epilines and points on images
    %
    %     [img1,img2] = drawlines(img1, img2, lines, pts1, pts2, clrs)
    %
    % ## Input
    % * __img1__ first image
    % * __img2__ second image
    % * __lines__ epilines corresponding to `pts2` in `img2`
    % * __pts1__ points in `img1`
    % * __pts2__ points in `img2`
    % * __clrs__ color of each line and matching points
    %
    % ## Output
    % * __img1__ image with drawn points and epilines for the points in `img2`
    % * __img2__ image with drawn points
    %
    % Epilines corresponding to the points in 1st image is drawn on 2nd image.
    %
    % See also: estimateFundamentalMatrix, epipolarLine, lineToBorderPoints
    %

    % convert to RGB
    img1 = cv.cvtColor(img1, 'GRAY2RGB');
    img2 = cv.cvtColor(img2, 'GRAY2RGB');

    % epilines
    w = size(img1,2);
    N = size(lines,1);
    p1 = int32([zeros(N,1), -lines(:,3)./lines(:,2)]);
    p2 = int32([ones(N,1)*w, -(lines(:,3)+lines(:,1)*w)./lines(:,2)]);
    img1 = cv.line(img1, p1, p2, 'Colors',clrs, 'LineType','AA');

    % matching points
    img1 = cv.circle(img1, pts1, 5, 'Colors',clrs, 'Thickness','Filled');
    img2 = cv.circle(img2, pts2, 5, 'Colors',clrs, 'Thickness','Filled');
end


function angles2d
%ANGLES2D Description of functions for manipulating angles.
%
%   Angles are normalized in an interval of width 2*PI. Most geom2d
%   functions return results in the [0 2*pi] interval, but it can be
%   convenient to consider the [-pi pi] interval as well. See the
%   normalizeAngle function to switch between conventions.
%
%   Angles are usually oriented. The default orientation is the CCW
%   (Counter-Clockwise) orientation.
%
%   See also
%   normalizeAngle, angleDiff, angleAbsDiff, angleSort
%   angle2Points, angle3Points, vectorAngle, lineAngle, edgeAngle
%

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2010-03-31, using Matlab 7.4.0.287 (R2007a)
% Copyright 2010-2022 INRA - Cepia Software Platform

help('angles2d');


% Ryan Steindl based on Robotics Toolbox for MATLAB (v6 and v9)
%
% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com
function q = interp(Q1, Q2, r)
%Quaternion.interp Interpolate rotations expressed by quaternion objects
%
% QI = Q1.interp(Q2, R) is a unit-quaternion that interpolates between Q1 for R=0
% to Q2 for R=1. This is a spherical linear interpolation (slerp) that can be
% interpretted as interpolation along a great circle arc on a sphere.
%
% If R is a vector QI is a vector of quaternions, each element
% corresponding to sequential elements of R.
%
% Notes:
% - the value of r is clipped to the interval 0 to 1
%
% See also ctraj, Quaternion.scale.

    q1 = double(Q1);
    q2 = double(Q2);

    theta = acos(q1*q2');
    count = 1;

    % clip values of r
    r(r<0) = 0;
    r(r>1) = 1;

    if length(r) == 1
        if theta == 0
            q = Q1;
        else
            q = Quaternion( (sin((1-r)*theta) * q1 + sin(r*theta) * q2) / sin(theta) );
        end
    else
        for R=r(:)'
            if theta == 0
                qq = Q1;
            else
                qq = Quaternion( (sin((1-R)*theta) * q1 + sin(R*theta) * q2) / sin(theta) );
            end
            q(count) = qq;
            count = count + 1;
        end
    end
end


function tapas_hgf_jget_plotTraj(r)
% Plots the estimated trajectories for the HGF perceptual model for
% the JGET project
% Usage example:  est = tapas_fitModel(responses, inputs); tapas_hgf_plotTraj(est);
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2013 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Optional plotting of standard deviations (true or false)
plotsd = true;

% Set up display
scrsz = get(0,'screenSize');
outerpos = [0.2*scrsz(3),0.2*scrsz(4),0.8*scrsz(3),0.8*scrsz(4)];
figure(...
    'OuterPosition', outerpos,...
    'Name', 'HGF trajectories');

% Time axis
if size(r.u,2) > 1 && ~isempty(find(strcmp(fieldnames(r.c_prc),'irregular_intervals'))) && r.c_prc.irregular_intervals
    t = r.u(:,end)';
else
    t = ones(1,size(r.u,1));
end

ts = cumsum(t);
ts = [0, ts];

% Do we know the generative parameters?
if size(r.u,2) > 2
    genpar = true;
    mean   = r.u(:,2);
    sd     = r.u(:,3);
else
    genpar = false;
end

% Number of levels
try
    l = r.c_prc.n_levels;
catch
    l = length(r.p_prc.p)/8;
end

% Upper levels
for j = 1:l-1

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Left subplot (x)                       %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    subplot(l+1,2,2*j-1);

    if plotsd == true
        upperprior = r.p_prc.mux_0(l-j+1) +1.96*sqrt(r.p_prc.sax_0(l-j+1));
        lowerprior = r.p_prc.mux_0(l-j+1) -1.96*sqrt(r.p_prc.sax_0(l-j+1));
        upper = [upperprior; r.traj.mux(:,l-j+1)+1.96*sqrt(r.traj.sax(:,l-j+1))];
        lower = [lowerprior; r.traj.mux(:,l-j+1)-1.96*sqrt(r.traj.sax(:,l-j+1))];

        plot(0, upperprior, 'ob', 'LineWidth', 1);
        hold all;
        plot(0, lowerprior, 'ob', 'LineWidth', 1);
        fill([ts, fliplr(ts)], [(upper)', fliplr((lower)')], ...
             'b', 'EdgeAlpha', 0, 'FaceAlpha', 0.15);
    end
    plot(ts, [r.p_prc.mux_0(l-j+1); r.traj.mux(:,l-j+1)], 'b', 'LineWidth', 1.5);
    hold all;
    plot(0, r.p_prc.mux_0(l-j+1), 'ob', 'LineWidth', 1.5); % prior
    xlim([0 ts(end)]);
    title(['Posterior expectation of x_' num2str(l-j+1)], 'FontWeight', 'bold');
    ylabel(['\mu x_', num2str(l-j+1)]);

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Right subplot (alpha)                  %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    subplot(l+1,2,2*j);

    if plotsd == true
        upperprior = r.p_prc.mua_0(l-j+1) +1.96*sqrt(r.p_prc.saa_0(l-j+1));
        lowerprior = r.p_prc.mua_0(l-j+1) -1.96*sqrt(r.p_prc.saa_0(l-j+1));
        upper = [upperprior; r.traj.mua(:,l-j+1)+1.96*sqrt(r.traj.saa(:,l-j+1))];
        lower = [lowerprior; r.traj.mua(:,l-j+1)-1.96*sqrt(r.traj.saa(:,l-j+1))];

        plot(0, upperprior, 'ob', 'LineWidth', 1);
        hold all;
        plot(0, lowerprior, 'ob', 'LineWidth', 1);
        fill([ts, fliplr(ts)], [(upper)', fliplr((lower)')], ...
             'b', 'EdgeAlpha', 0, 'FaceAlpha', 0.15);
    end
    plot(ts, [r.p_prc.mua_0(l-j+1); r.traj.mua(:,l-j+1)], 'b', 'LineWidth', 1.5);
    hold all;
    plot(0, r.p_prc.mua_0(l-j+1), 'ob', 'LineWidth', 1.5); % prior
    xlim([0 ts(end)]);
    title(['Posterior expectation of \alpha_' num2str(l-j+1)], 'FontWeight', 'bold');
    ylabel(['\mu \alpha_', num2str(l-j+1)]);
end

% Input level
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Left subplot (x)                       %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
subplot(l+1,2,2*l-1);

if plotsd == true
    upperprior = r.p_prc.mux_0(1) +1.96*sqrt(r.p_prc.sax_0(1));
    lowerprior = r.p_prc.mux_0(1) -1.96*sqrt(r.p_prc.sax_0(1));
    upper = [upperprior; r.traj.mux(:,1)+1.96*sqrt(r.traj.sax(:,1))];
    lower = [lowerprior; r.traj.mux(:,1)-1.96*sqrt(r.traj.sax(:,1))];

    plot(0, upperprior, 'or', 'LineWidth', 1);
    hold all;
    plot(0, lowerprior, 'or', 'LineWidth', 1);
    fill([ts, fliplr(ts)], [(upper)', fliplr((lower)')], ...
         'r', 'EdgeAlpha', 0, 'FaceAlpha', 0.15);
end
plot(ts, [r.p_prc.mux_0(1); r.traj.mux(:,1)], 'r', 'LineWidth', 1.5);
hold all;
plot(0, r.p_prc.mux_0(1), 'or', 'LineWidth', 1.5); % prior
plot(ts(2:end), r.u(:,1), '.', 'Color', [0 0.6 0]); % inputs
if genpar
    plot(ts(2:end), mean, '-', 'Color', 'k', 'LineWidth', 1); % mean of input distribution
    plot(ts(2:end), mean +1.96.*sd, '--', 'Color', 'k', 'LineWidth', 1); % 95% interval of input distribution
    plot(ts(2:end), mean -1.96.*sd, '--', 'Color', 'k', 'LineWidth', 1); % 95% interval of input distribution
end
xlim([0 ts(end)]);
title(['Input u (green) and posterior expectation of x_1 (red) for \kappa_x=', ...
       num2str(r.p_prc.kax), ', \omega_x=', num2str(r.p_prc.omx)], 'FontWeight', 'bold');
ylabel('u, \mu x_1');
hold off;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Right subplot (alpha)                  %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
subplot(l+1,2,2*l);

if plotsd == true
    upperprior = r.p_prc.mua_0(1) +1.96*sqrt(r.p_prc.saa_0(1));
    lowerprior = r.p_prc.mua_0(1) -1.96*sqrt(r.p_prc.saa_0(1));
    upper = [upperprior; r.traj.mua(:,1)+1.96*sqrt(r.traj.saa(:,1))];
    lower = [lowerprior; r.traj.mua(:,1)-1.96*sqrt(r.traj.saa(:,1))];

    transupperprior = sqrt(exp(r.p_prc.kau *upperprior +r.p_prc.omu));
    translowerprior = sqrt(exp(r.p_prc.kau *lowerprior +r.p_prc.omu));
    transupper = sqrt(exp(r.p_prc.kau *upper +r.p_prc.omu));
    translower = sqrt(exp(r.p_prc.kau *lower +r.p_prc.omu));

    plot(0, transupperprior, 'or', 'LineWidth', 1);
    hold all;
    plot(0, translowerprior, 'or', 'LineWidth', 1);
    fill([ts, fliplr(ts)], [(transupper)', fliplr((translower)')], ...
         'r', 'EdgeAlpha', 0, 'FaceAlpha', 0.15);
end
transmuaprior = sqrt(exp(r.p_prc.kau *r.p_prc.mua_0(1) +r.p_prc.omu));
plot(ts, [transmuaprior; sqrt(exp(r.p_prc.kau *r.traj.mua(:,1) +r.p_prc.omu))], 'r', 'LineWidth', 1.5);
hold all;
plot(0, transmuaprior, 'or', 'LineWidth', 1.5); % prior
if genpar
    plot(ts(2:end), sd, '--', 'Color', 'k', 'LineWidth', 1);
end
xlim([0 ts(end)]);
title(['Belief on noise (red) for \kappa_\alpha=', ...
       num2str(r.p_prc.kaa), ', \omega_\alpha=', num2str(r.p_prc.oma)], 'FontWeight', 'bold');
ylabel('\mu \alpha_1');
hold off;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Decision model                         %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
subplot(l+1,2,2*l+1);

if plotsd == true
    upper = r.traj.muxhat(:,1)+1.96*sqrt(r.p_obs.ze +r.traj.saxhat(:,1));
    lower = r.traj.muxhat(:,1)-1.96*sqrt(r.p_obs.ze +r.traj.saxhat(:,1));

    fill([ts(2:end), fliplr(ts(2:end))], [(upper)', fliplr((lower)')], ...
         'r', 'EdgeAlpha', 0, 'FaceAlpha', 0.15);
    hold all;
end
plot(ts(2:end), r.traj.muxhat(:,1), 'Color', [153/256 17/256 153/256], 'LineWidth', 1.5);
hold all;
plot(ts(2:end), r.y(:,1), '.', 'Color', [1 0.65 0], 'MarkerSize', 15); % responses
if genpar
    plot(ts(2:end), mean, '-', 'Color', 'k', 'LineWidth', 1); % mean of input distribution
    plot(ts(2:end), mean +1.96.*sd, '--', 'Color', 'k', 'LineWidth', 1); % 95% interval of input distribution
    plot(ts(2:end), mean -1.96.*sd, '--', 'Color', 'k', 'LineWidth', 1); % 95% interval of input distribution
end
xlim([1 ts(end)]);
title('Decision model: prediction of decision (purple) and decision (orange)', 'FontWeight', 'bold');
ylabel('y, \^{\mu} x_1');
xlabel({'Trial number', ' '}); % A hack to get the relative subplot sizes right
hold off;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Learning rate                         %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if genpar
subplot(l+1,2,2*l+2);
[AX, H1, H2 ] = plotyy(ts(2:end), mean, ts(2:end), r.traj.lrx(:,1));
hold all;
ylim(AX(1), [min(mean -1.96.*sd-3), max(mean +1.96.*sd+3)]);
plot(AX(1), ts(2:end), mean +1.96.*sd, '--', 'Color', 'k', 'LineWidth', 1.1); % 95% interval of input distribution
plot(AX(1), ts(2:end), mean -1.96.*sd, '--', 'Color', 'k', 'LineWidth', 1.1); % 95% interval of input distribution
set(H1, 'Color', 'k', 'LineWidth', 1.1);
set(H2, 'Color', [178/256, 34/256, 34/256], 'LineWidth', 1.5);
set(AX(1), 'YColor', 'k');
set(AX(2), 'YColor', 'k');
xlim(AX(1), [1 ts(end)]);
xlim(AX(2), [1 ts(end)]);
title('Learning rate (bordeaux) and input sampling distribution (black)', 'FontWeight', 'bold');
ylabel(AX(1), 'Input');
ylabel(AX(2), '\sigma_x');
xlabel('Trial number');
hold off;
end


% tiny
%d = 10;
% medium
d = 100;
% big
%d = 500;
n = 3*d;
%n = 2*d;

if 1
  clear run
if 0
  % experiment 1
  w = randn(d,1);
  % this ensures about 16% training error
  w = sqrt(2)*w/norm(w);
  % training data
  x = randn(d,n);
  s = 1./(1+exp(-w'*x));
  y = (rand(1,n) < s);
  y = 2*y-1;
  if 0
    % test data
    xt = randn(d,1e4);
    s = 1./(1+exp(-w'*xt));
    yt = (rand(1,cols(xt)) < s);
    yt = 2*yt-1;
  end
elseif 1
  % experiment 2
  % positive data
  x = dirichlet_sample(ones(d,1),n);
  w = dirichlet_sample(ones(d,1),2);
  w = log(w(:,1)./w(:,2));

  s = 1./(1+exp(-w'*x));
  y = (rand(1,n) < s);
  y = 2*y-1;
  if 1
    % test data
    xt = dirichlet_sample(ones(d,1),n);
    s = 1./(1+exp(-w'*xt));
    yt = (rand(1,cols(xt)) < s);
    yt = 2*yt-1;
  end
else
  % Collins's generator
  x = randn(d,n);
  w = randn(d,1);
  y = sign(w'*x);
  x = x + randn(d,n)*sqrt(0.8);
end

if 0
  % shift the data
  c = 10;
  x = [x+c; ones(1,n)];
  w = [w; -c*sum(w)];
end
end
[d,n] = size(x);

i1 = find(y > 0);
i0 = find(y < 0);
figure(1)
plot(x(1,i1), x(2,i1), 'o', x(1,i0), x(2,i0), 'x')
if d == 2
  draw_line_clip(w(1),w(end),-w(2),'Color','k');
end

w0 = zeros(d,1);
xy = scale_cols(x,y);

if 0
  xyt = scale_cols(xt,yt);
  % find the best lambda
  lambdas = exp(linspace(-4,4,20));
  f = [];
  for i = 1:length(lambdas)
    w = train_newton(xy,w0,lambdas(i));
    f(i) = logProb(xyt,w);
  end
  figure(1)
  semilogx(lambdas,f)
  axis_pct;
  [dummy,i] = max(f);
  lambda = lambdas(i);
else
  lambda = 1e-2;
end
fprintf('lambda = %g\n',lambda)

if exist('run') ~= 1 | ~isfield(run,'Newton')
  disp('Newton')
  [w,run.Newton] = train_newton(xy,w0,lambda);
  wbest = w;
  ebest = run.Newton.e(end);
  % training errors
  s = 1./(1+exp(-wbest'*x));
  fprintf('%g training error\n', mean((s > 0.5) ~= (y > 0)))
end

if ~isfield(run,'Coord')
  disp('Coord')
  %[w,run.Coord] = train_newton2(xy,w0,lambda);
end
if ~isfield(run,'CG')
  disp('CG')
  [w,run.CG] = train_cg(xy,w0,lambda);
end
%disp(length(cg.run.e))
%[w,cg2.run] = train_cg2(xy,w0);
%disp(length(cg2.run.e))
if 0
  % compare by iteration
  figure(3)
  i = 1:length(cg2.run.e);
  plot(i, cg.run.e(i), i, cg2.run.e(i))
  legend('CG','CG2',4)
  return
end

if ~isfield(run,'BFGS')
  disp('BFGS')
  [w,run.BFGS] = train_bfgs(xy,w0,lambda);
end
%[w,run.lmBFGS] = train_lmbfgs(xy,w0,lambda);

%[w,run.sg] = train_sg(xy,w0);
if 0
  fprintf('max(sg.run.e) = %g\n',max(sg.run.e))
  fprintf('max(cg.run.e) = %g\n',max(cg.run.e))
  figure(1)
  plot(sg.run.step)
  figure(2)
  hold on, plot(cg.run.e,'g'), hold off
  figure(3)
  plot(sg.run.w(1,:),sg.run.w(2,:))
  hold on, plot(cg.run.w(1,:),cg.run.w(2,:),'g'), hold off
  return
end

if ~isfield(run,'FixedH')
  disp('FixedH')
  [w,run.FixedH] = train_bohning(xy,w0,lambda);
end

if ~isfield(run,'Dual') & lambda > 0
  disp('Dual')
  [w,run.Dual] = train_dual(xy,w0,lambda,1);
end
%[w,run.Dual2] = train_dual(xy,w0,lambda,1);
%[w,run.Dual2] = train_dual_cg(xy,w0,lambda);

if ~isfield(run,'MIS') & lambda == 0
  [w,run.MIS] = train_mis(xy,w0,lambda);
end
if ~isfield(run,'IS') & lambda == 0
  %[w,run.IS] = train_is(x,y,w0,lambda);
end

color.Newton = 'k';
color.CG = 'g';
color.BFGS = 'r';
color.lmBFGS = 'm';
color.cg2 = 'r';
color.sg = 'g';
color.Coord = 'm';
color.FixedH = 'c';
color.Dual = 'b';
color.Dual2 = 'm';
color.MIS = 'g';
color.IS = 'k';

linespec.Newton = 'b-.';
linespec.CG = 'g--';
linespec.Coord = 'g-';
linespec.FixedH = 'c-';
linespec.BFGS = 'r-.';
linespec.lmBFGS = 'm-.';
linespec.Dual = 'y-.';
linespec.Dual2 = 'm-.';
linespec.MIS = 'm--';
linespec.IS = 'k--';

% plot cost vs. accuracy
figure(2)
ebest = -Inf;
for f = fieldnames(run)'
  thisrun = getfield(run,char(f));
  ebest = max([ebest max(thisrun.e)]);
end
for f = fieldnames(run)'
  thisrun = getfield(run,char(f));
  %semilogx(thisrun.flops, thisrun.e, getfield(linespec,char(f)));
  thisrun.err = (ebest - thisrun.e)/n;
  run = setfield(run,char(f),thisrun);
  loglog(thisrun.flops, thisrun.err, getfield(color,char(f)));
  hold on
end
hold off
xlabel('FLOPS')
%ylabel('Log-likelihood')
ylabel('Difference from optimal log-likelihood')
axis_pct;
if 1
  ax = axis;
  ax(3) = 1e-10;
  axis(ax);
end
f = fieldnames(run);
legend(f,4)
if 0
  legend off
  f = fieldnames(run);
  h = mobile_text(f{:});
end
set(gcf,'paperpos',[0.25 2.5 8 6])
%print -dpsc expt1_d100_n300.ps
%print -dpsc expt2_d100_n300.ps
%print -dpsc expt25_d100_n300.ps
%print -dpsc expt25_d500_n1500.ps
%print -dpsc expt3_d100_n300.ps
%print -dpsc expt4_d100_n300.ps
% print -dpsc expt2_d300_n1500.ps
% save expt4.mat x y run

if 0
% cosine distance from the best solution
wbest = wbest/norm(wbest);
figure(3)
for f = fieldnames(run)'
  thisrun = getfield(run,char(f));
  thisrun.w = scale_cols(thisrun.w, 1./sqrt(sum(thisrun.w.^2,1)));
  thisrun.acc = sqrt(clip(sqdist(wbest, thisrun.w)));
  run = setfield(run,char(f),thisrun);
  loglog(thisrun.flops, thisrun.acc, getfield(color,char(f)));
  hold on
end
hold off
legend(fieldnames(run),4)
end

for f = fieldnames(run)'
  thisrun = getfield(run,char(f));
  %i = convergence(thisrun.e);
  i = min(find(thisrun.err < 1e-4));
  fprintf('%-9s %11d\n',char(f),thisrun.flops(i))
end


function a = r8sto_indicator ( n )

%*****************************************************************************80
%
%% R8STO_INDICATOR sets up a R8STO indicator matrix.
%
%  Discussion:
%
%    The R8STO storage format is used for a symmetric Toeplitz matrix.
%    It stores the N elements of the first row.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Output, real A(N), the R8STO matrix.
%
  fac = 10^( i4_log_10 ( n ) + 1 );

  i = 1;
  k = 0;
  for j = 1 : n
    k = k + 1;
    a(k) = fac * i + j;
  end

  return
end


% MP_SP_DRCHLT_L2_PROJ_UDOTN: assign the normal degrees of freedom trough an L2 projection for a multipatch geometry.
%  To be used with the 'RT' and 'NDL' spaces. The imposed condition reads   u \cdot n = h \cdot n
%
%   [vel, normal_dofs] = mp_sp_drchlt_l2_proj_udotn (space, msh, gnum, ornt, bnd_sides, bnd_func)
%
% INPUTS:
%
%    space:     space object (see sp_vector_div_transform)
%    msh:       mesh object (see msh_cartesian)
%    gnum:      global numbering of the degrees of freedom (see mp_interface_hdiv)
%    ornt:      global orientation of the degrees of freedom (see mp_interface_hdiv)
%  boundaries: array of structures containing the information for the boundaries (see mp_geo_load)
%    bnd_sides: boundary sides on which the Dirichlet condition is imposed
%    bnd_func:  the condition to be imposed (h in the equation)
%
% OUTPUT:
%
%     vel:         assigned value to the normal degrees of freedom
%     normal_dofs: global numbering of the normal basis functions
%
% Copyright (C) 2014, 2015 Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.

function [u, normal_dofs] = mp_sp_drchlt_l2_proj_udotn (space, msh, gnum, ornt, boundaries, refs, bnd_func)

  if (isa (space, 'sp_multipatch'))
    warning ('For spaces of the class SP_MULTIPATCH, using the function SP_DRCHLT_PROJ_L2_UDOTN inside the class')
    [u, dofs] = sp_drchlt_proj_l2_udotn (sp, msh, refs, bnd_func);
    return
  end

  ndof = max ([gnum{:}]);
  M = spalloc (ndof, ndof, 3*ndof);
  rhs = zeros (ndof, 1);

  normal_dofs = [];
  for iref = refs
    for bnd_side = 1:boundaries(iref).nsides
      iptc = boundaries(iref).patches(bnd_side);
      iside = boundaries(iref).faces(bnd_side);

      if (strcmpi (space{iptc}.transform, 'div-preserving'))
        msh_side = msh_eval_boundary_side (msh{iptc}, iside);
        sp_side = sp_eval_boundary_side (space{iptc}, msh_side);
      else
        error ('The function only works with div-conforming spaces')
      end

      normal_dofs = union (normal_dofs, gnum{iptc}(sp_side.dofs));
      for idim = 1:msh{iptc}.rdim
        x{idim} = reshape (msh_side.geo_map(idim,:,:), msh_side.nqn, msh_side.nel);
      end
      g = bnd_func (x{:}, iref);

      M_loc = op_u_v (sp_side, sp_side, msh_side, ones(size(x{1})));
      rhs_loc = op_fdotn_v (sp_side, msh_side, g);

      global_dofs = gnum{iptc}(sp_side.dofs);
      M(global_dofs, global_dofs) = M(global_dofs, global_dofs) + M_loc;
      rhs(global_dofs) = rhs(global_dofs) + rhs_loc;
    end
  end

  u = M(normal_dofs, normal_dofs) \ rhs(normal_dofs);

end


function V = par_read_volume(info)
% Function for reading the volume of a Philips Par / Rec  MR V4.* file
%
% volume = par_read_volume(file-header)
%
% examples:
% 1: info = par_read_header()
%    V = par_read_volume(info);
%    imshow(squeeze(V(:,:,round(end/2),1)),[]);
%
% 2: V = par_read_volume('test.par');

if(~isstruct(info)), info=par_read_header(info); end

% Open file
fid=fopen(info.FilenameREC','rb','ieee-le');
% Skip header
fseek(fid,0,'bof');

datasize=prod(info.Dimensions)*info.BitDepth/8;

% Read the Data
switch(info.BitDepth)
    case 8
        info.DataType='char';
    case 16
        info.DataType='short';
    case 32
        info.DataType='float';
    case 64
        info.DataType='double';
end

switch(info.DataType)
    case 'char'
        V = int8(fread(fid,datasize,'char=>int8'));
    case 'uchar'
        V = uint8(fread(fid,datasize,'uchar=>uint8'));
    case 'short'
        V = int16(fread(fid,datasize,'short=>int16'));
    case 'ushort'
        V = uint16(fread(fid,datasize,'ushort=>uint16'));
    case 'int'
        V = int32(fread(fid,datasize,'int=>int32'));
    case 'uint'
        V = uint32(fread(fid,datasize,'uint=>uint32'));
    case 'float'
        V = single(fread(fid,datasize,'float=>single'));
    case 'double'
        V = double(fread(fid,datasize,'double=>double'));
end

fclose(fid);
V = reshape(V,info.Dimensions);