function plot_llkstest(a,time,timef,bootloops,maepi)
    % function plot_llkstes(a,time,timef,bootloops,maepi);
    % --------------------------------------------------
    % Plots Ncum observed vs. Ncum modeled for specified time windows
    % with choosing the model for the learning period and performs a KS-Test
    %
    % Input variables:
    % a         : earthquake catalog
    % time      : learning period fo fit Omori parameters
    % timef     : forecast period
    % bootloops : Number of bootstraps
    % maepi     : mainshock
    %
    % J.Woessner
    % last update: 20.07.04

    % Surpress warnings from fmincon
    warning off;

    %[m_main, main] = max(a.Magnitude);
    date_matlab = datenum(a.Date.Year,a.Date.Month,a.Date.Day,a.Date.Hour,a.Date.Minute,zeros(size(a,1),1));
    date_main = datenum(floor(maepi(3)),maepi(4),maepi(5),maepi(8),maepi(9),0);
    time_aftershock = date_matlab-date_main;

    % Aftershock catalog
    vSel1 = time_aftershock(:) > 0;
    tas = time_aftershock(vSel1);
    eqcatalogue = a.subset(vSel1);

    % Estimation of Omori parameters from learning period
    l = tas <= time;
    time_as=tas(l);
    % Times up to the forecast time
    lf = tas <= time+timef ;
    time_asf= [tas(lf) ];
    time_asf=sort(time_asf);

    % Select biggest aftershock earliest in time, but more than 1 day after
    % mainshock and in learning period
    mAfLearnCat = eqcatalogue(l,:);
    fDay = 1;
    ft_c=fDay/365; % Time not considered to find biggest aftershock
    vSel = (mAfLearnCat(:,3) > maepi(:,3)+ft_c & mAfLearnCat(:,3)<= maepi(:,3)+time/365);
    mCat = mAfLearnCat(vSel,:);
    vSel = mCat(:,6) == max(mCat(:,6));
    vBigAf = mCat(vSel,:);
    if length(mCat(:,1)) > 1
        vSel = vBigAf(:,3) == min(vBigAf(:,3));
        vBigAf = vBigAf(vSel,:);
    end

    date_biga = datenum(floor(vBigAf(3)),vBigAf(4),vBigAf(5),vBigAf(8),vBigAf(9),0);
    fT1 = date_biga - date_main; % Time of big aftershock

    % Calculate p,c,k for dataset
    prompt  = {'Enter model number (1:pck, 2:pckk, 3:ppckk, 4:ppcckk:'};
    title   = 'Model selection for fitting aftershock sequence';
    lines= 1;
    def     = {'1'};
    answer  = inputdlg(prompt,title,lines,def);
    nMod = str2double(answer{1});

    % Calculate uncertainty and mean values of p,c,and k
    [mMedModF, mStdL, loopout] = brutebootloglike_a2(time_as, time_asf, bootloops,fT1,nMod);
    pmed1 = mMedModF(1,1);
    pmed2 = mMedModF(1,3);
    cmed1 = mMedModF(1,5);
    cmed2 = mMedModF(1,7);
    kmed1 = mMedModF(1,9);
    kmed2 = mMedModF(1,11);

    % Compute model according to model choice
    if nMod == 1
        [pval1, pval2, cval1, cval2, kval1, kval2, fAIC, fL] = bruteforceloglike_a2(time_as,fT1,nMod);
    elseif nMod == 2
        [pval1, pval2, cval1, cval2, kval1, kval2, fAIC, fL] = bruteforceloglike_a2(time_as,fT1,nMod);
    elseif nMod == 3
        [pval1, pval2, cval1, cval2, kval1, kval2 , fAIC, fL] = bruteforceloglike_a2(time_as,fT1,nMod);
    else
        [pval1, pval2, cval1, cval2, kval1, kval2, fAIC, fL] = bruteforceloglike_a2(time_as,fT1,nMod);
    end

    % Start plotting
    figure_w_normalized_uicontrolunits('Numbertitle','off','Name','Aftershock modelling fit')

    % Plot the forecast ...
    cumnrf = (1:length(time_as))';
    cumnr_modelf = [];
    if nMod == 1
        for i=1:length(time_as)
            if pval1 ~= 1
                cm = kval1/(pval1-1)*(cval1^(1-pval1)-(time_as(i)+cval1)^(1-pval1));
            else
                cm = kval1*log(time_as(i)/cval1+1);
            end
            cumnr_modelf = [cumnr_modelf; cm];
        end % END of FOR on length(time_as)
    else
        for i=1:length(time_as)
            if time_as(i) <= fT1
                if pval1 ~= 1
                    cm = kval1/(pval1-1)*(cval1^(1-pval1)-(time_as(i)+cval1)^(1-pval1));
                else
                    cm = kval1*log(time_as(i)/cval1+1);
                end
                cumnr_modelf = [cumnr_modelf; cm];
            else
                if (pval1 ~= 1 & pval2 ~= 1)
                    cm = kval1/(pval1-1)*(cval1^(1-pval1)-(time_as(i)+cval1)^(1-pval1))+ kval2/(pval2-1)*(cval2^(1-pval2)-(time_as(i)-fT1+cval2)^(1-pval2));
                else
                    cm = kval1*log(time_as(i)/cval1+1) + kval2*log((time_as(i)-fT1)/cval2+1);
                end
                cumnr_modelf = [cumnr_modelf; cm];
            end %END of IF on fT1
        end % End of FOR length(time_as)
    end % End of if on nMod
    time_as=sort(time_as);
    cumnr_modelf=sort(cumnr_modelf);

    pf1 =  plot(time_as,cumnr_modelf,'g-.','Linewidth',2);
    hold on
    pf2 =  plot(time_as,cumnrf, 'b-','Linewidth',2);
    paf = plot(fT1, 0,'h','MarkerFaceColor',[1 1 0],'MarkerSize',12,'MarkerEdgeColor',[0 0 0] );

    % Calculate KSTEST2 as a measure of the goodness of fit
    [H,P,KSSTAT] = kstest2(cumnr_modelf,cumnrf)

    % Calculate RMS
    i=(1:1:length(time_as))';
    fRMS = (sum((i-cumnr_modelf).^2)/length(i))^0.5;

    % Round values for output
    pval1 = round(100*pval1)/100;
    pval2 = round(100*pval2)/100;
    cval1 = round(1000*cval1)/1000;
    cval2 = round(1000*cval2)/1000;
    kval1 = round(10*kval1)/10;
    kval2 = round(10*kval2)/10;
    pmed1 = round(100*pmed1)/100; mStdL(1,1) = round(100*mStdL(1,1))/100;
    pmed2 = round(100*pmed2)/100; mStdL(1,2) = round(100*mStdL(1,2))/100;
    cmed1 = round(1000*cmed1)/1000; mStdL(1,3) = round(1000*mStdL(1,3))/1000;
    cmed2 = round(1000*cmed2)/1000; mStdL(1,4) = round(1000*mStdL(1,4))/1000;
    kmed1 = round(10*kmed1)/10; mStdL(1,5) = round(100*mStdL(1,5))/100;
    kmed2 = round(10*kmed2)/10; mStdL(1,6)= round(100*mStdL(1,6))/100;
    fRMS = round(100*fRMS)/100;

    % Get Y limits for positioning texts
    yy = get(gca,'ylim');

    if nMod == 1
        string1=['p = ' num2str(pval1) '; c = ' num2str(cval1) '; k = ' num2str(kval1) ];
        string3=['pm = ' num2str(pmed1) '+-' num2str(mStdL(1,1)) '; cm = ' num2str(cmed1) '+-' num2str(mStdL(1,3)) '; km = ' num2str(kmed1) '+-' num2str(mStdL(1,5))];
        text(max(time_asf)*0.05,yy(2)*0.9,string1,'FontSize',10);
        text(max(time_asf)*0.05,yy(2)*0.8,string3,'FontSize',10);
    elseif nMod == 2
        string1=['p = ' num2str(pval1) '; c = ' num2str(cval1) '; k1 = ' num2str(kval1) '; k2 = ' num2str(kval2) ];
        string3=['pm = ' num2str(pmed1) '+-' num2str(mStdL(1,1)) '; cm = ' num2str(cmed1) '+-' num2str(mStdL(1,3)) '; km1 = ' num2str(kmed1) '+-' num2str(mStdL(1,5)) '; km2 = ' num2str(kmed2) '+-' num2str(mStdL(1,6))];
        text(max(time_asf)*0.05,yy(2)*0.9,string1,'FontSize',10);
        text(max(time_asf)*0.05,yy(2)*0.8,string3,'FontSize',10);
    elseif nMod == 3
        string1=['p1 = ' num2str(pval1) '; c = ' num2str(cval1) '; k1 = ' num2str(kval1) ];
        string2=['p2 = ' num2str(pval2) '; k2 = ' num2str(kval2) ];
        string3=['pm1 = ' num2str(pmed1) '+-' num2str(mStdL(1,1)) '; cm = ' num2str(cmed1) '+-' num2str(mStdL(1,3)) '; km1 = ' num2str(kmed1) '+-' num2str(mStdL(1,5))];
        string4=['pm2 = ' num2str(pmed2) '+-' num2str(mStdL(1,2)) '; km2 = ' num2str(kmed2) '+-' num2str(mStdL(1,6))];
        text(max(time_asf)*0.05,yy(2)*0.9,string1,'FontSize',10);
        text(max(time_asf)*0.05,yy(2)*0.85,string2,'FontSize',10);
        text(max(time_asf)*0.05,yy(2)*0.8,string3,'FontSize',10);
        text(max(time_asf)*0.05,yy(2)*0.75,string4,'FontSize',10);
    else
        string1=['p1 = ' num2str(pval1) '; c1 = ' num2str(cval1) '; k1 = ' num2str(kval1) ];
        string2=['p2 = ' num2str(pval2) '; c2 = ' num2str(cval2) '; k2 = ' num2str(kval2) ];
        string3=['pm1 = ' num2str(pmed1) '+-' num2str(mStdL(1,1)) '; cm1 = ' num2str(cmed1) '+-' num2str(mStdL(1,3)) '; km1 = ' num2str(kmed1) '+-' num2str(mStdL(1,5))];
        string4=['pm2 = ' num2str(pmed2) '+-' num2str(mStdL(1,2)) '; cm2 = ' num2str(cmed2) '+-' num2str(mStdL(1,4)) '; km2 = ' num2str(kmed2) '+-' num2str(mStdL(1,6))];
        text(max(time_asf)*0.05,yy(2)*0.9,string1,'FontSize',10);
        text(max(time_asf)*0.05,yy(2)*0.85,string2,'FontSize',10);
        text(max(time_asf)*0.05,yy(2)*0.8,string3,'FontSize',10);
        text(max(time_asf)*0.05,yy(2)*0.75,string4,'FontSize',10);
    end
    string=['H = ' num2str(H) ' P = ' num2str(P) ' KS-Statistic) = ' num2str(KSSTAT)];
    text(max(time_asf)*0.05,yy(2)*0.1,string,'FontSize',10);
    sAIC = ['AIC = ' num2str(fAIC)];
    text(max(time_asf)*0.05,yy(2)*0.05,sAIC,'FontSize',10);
    sRMS = ['RMS = ' num2str(fRMS)];
    text(max(time_asf)*0.05,yy(2)*0.15,sRMS,'FontSize',10);
    % Legend
    sModel = ['Model ' num2str(nMod)];
    legend([pf2 pf1 paf],'Data',sModel,'Sec. AF','location','Best');

    % Calculate fits of different models
    mRes = [];
    % Modified Omori law (pck)
    nMod = 1; [pval1, pval2, cval1, cval2, kval1, kval2, fAIC, fL] = bruteforceloglike_a2(time_as,fT1,nMod);
    mRes = [mRes; nMod, pval1, pval2, cval1, cval2, kval1, kval2, fAIC, fL];
    % MOL with secondary aftershock (pckk)
    nMod = 2; [pval1, pval2, cval1, cval2, kval1, kval2, fAIC, fL] = bruteforceloglike_a2(time_as,fT1,nMod);
    mRes = [mRes; nMod, pval1, pval2, cval1, cval2, kval1, kval2, fAIC, fL];
    % MOL with secondary aftershock (ppckk)
    nMod = 3; [pval1, pval2, cval1, cval2, kval1, kval2, fAIC, fL] = bruteforceloglike_a2(time_as,fT1,nMod);
    mRes = [mRes; nMod, pval1, pval2, cval1, cval2, kval1, kval2, fAIC, fL];
    % MOL with secondary aftershock (ppcckk)
    nMod = 4; [pval1, pval2, cval1, cval2, kval1, kval2, fAIC, fL] = bruteforceloglike_a2(time_as,fT1,nMod);
    mRes = [mRes; nMod, pval1, pval2, cval1, cval2, kval1, kval2, fAIC, fL];

    % Select best fitting model by AIC
    vSel = (mRes(:,8)==min(mRes(:,8)));
    mRes = mRes(vSel,:);
    if length(mRes(:,1)) > 1
        vSel = (mRes(:,1)==min(mRes(:,1)));
        mRes = mRes(vSel,:);
    end
    % Model to use for bootstrapping as of lowest AIC to observed data
    nMod = mRes(1,1);

    sModel1 = ['Info: Best model is ' num2str(nMod)];
    text(max(time_asf)*0.05,yy(2)*0.2,sModel1,'FontSize',10);
    % Figure settings
    set(gca,'FontSize',12,'Fontweight','bold','Linewidth',2)
    xlabel('Time [Days after mainshock]','FontSize',12,'Fontweight','bold')


%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled non-linear
% 	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:  nick.battista@unc.edu
% Date Created: May 27th, 2015
% Institution: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs)
% 	3. Target 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 (nick.battista@unc.edu) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: computes Porous Slip Velocity based on Darcy's Law,
%
%           U_porous = -alpha <F_Lag,hat{n}> / | d vec{X}_Lag /ds |
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [Por_Mat,nX,nY] = please_Compute_Porous_Slip_Velocity(ds,xLag,yLag,porous_info,F_Lag)

% xLag: vector of x-Pts associated w/ x-Lagrangian pts
% yLag: vector of y-Pts associated w/ y-Lagrangian pts
% porous_info:  col 1: lag-ids for porous media
%               col 2: x-Lag pts for lag-ids
%               col 3: y-Lag pts for lag-ids
%               col 4: porosity coefficient

% # of porous media pts.
Np = length( porous_info(:,1) );

% Initialize storage
%Por_X = zeros(length(xLag),1);
%Por_Y = Por_X;

% Compute Lagrangian Derivatives
[xL_s,yL_s] = give_Me_Lagrangian_Derivatives(ds,Np,porous_info);

% Compute Normal Vector (unit normals)
[nX,nY,sqrtNorm] = give_Me_Lagrangian_Normal_Vectors(xL_s,yL_s);

% Compute Porous Slip Velocity
Up_X = - ( porous_info(:,4) ) .* F_Lag( porous_info(:,1) ,1).*nX ./ sqrtNorm;
Up_Y = - ( porous_info(:,4) ) .* F_Lag( porous_info(:,1) ,2).*nY ./ sqrtNorm;
Por_Mat = [Up_X Up_Y];

% Store porous slip velocities in appropriate vector for adding to current Lagrangian Velocity Computation
%Por_X( porous_info(:,1) ) = Up_X;
%Por_Y( porous_info(:,1) ) = Up_Y;
%Por_Mat = [Por_X Por_Y];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: computes Lagrangian Derivatives
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xL_s,yL_s] = give_Me_Lagrangian_Derivatives(ds,Np,porous_info)

xL = porous_info(:,2);   % x-Values for Porous points
yL = porous_info(:,3);   % y-Values for Porous points
c = porous_info(:,5);    % choice of where it falls on stencil

xL_s = zeros(Np,1);
yL_s = zeros(Np,1);

for i=1:Np
   if c(i) == -2
       xL_s(i) = ( -25/12*xL(i) + 4*xL(i+1) - 3*xL(i+2) + 4/3*xL(i+3) - 1/4*xL(i+4) ) / ds;
       yL_s(i) = ( -25/12*yL(i) + 4*yL(i+1) - 3*yL(i+2) + 4/3*yL(i+3) - 1/4*yL(i+4) ) / ds;

   elseif c(i) == -1
       xL_s(i) = ( -0.25*xL(i-1) - 5/6*xL(i) + 1.5*xL(i+1) - 0.5*xL(i+2) + 1/12*xL(i+3) ) / ds;
       yL_s(i) = ( -0.25*yL(i-1) - 5/6*yL(i) + 1.5*yL(i+1) - 0.5*yL(i+2) + 1/12*yL(i+3) ) / ds;

   elseif c(i) == 0
       xL_s(i) = ( 1/12*xL(i-2) - 2/3*xL(i-1) + 2/3*xL(i+1) - 1/12*xL(i+2) ) / ds;
       yL_s(i) = ( 1/12*yL(i-2) - 2/3*yL(i-1) + 2/3*yL(i+1) - 1/12*yL(i+2) ) / ds;

   elseif c(i) == 1
       xL_s(i) = ( -1/12*xL(i-3) + 0.5*xL(i-2) - 1.5*xL(i-1) + 5/6*xL(i) + 0.25*xL(i+1) ) / ds;
       yL_s(i) = ( -1/12*yL(i-3) + 0.5*yL(i-2) - 1.5*yL(i-1) + 5/6*yL(i) + 0.25*yL(i+1) ) / ds;

   elseif c(i) == 2
       xL_s(i) = ( 0.25*xL(i-4) - 4/3*xL(i-3) + 3*xL(i-2) - 4*xL(i-1) + 25/12*xL(i) ) / ds;
       yL_s(i) = ( 0.25*yL(i-4) - 4/3*yL(i-3) + 3*yL(i-2) - 4*yL(i-1) + 25/12*yL(i) ) / ds;

   else
      fprintf('\n\n');
      error('Error: Bad file format inside your .porous file!\n');
   end
end

% IF CLOSED STRUCTURE!
% for i=1:Np
%     if i==1
%        xL_s(1) = ( xL(2) - xL(end) ) / (2*ds);
%        yL_s(1) = ( yL(2) - yL(end) ) / (2*ds);
%     elseif i<Np
%        xL_s(i) = ( xL(i+1) - xL(i-1) ) / (2*ds);
%        yL_s(i) = ( yL(i+1) - yL(i-1) ) / (2*ds);
%     else
%        xL_s(i) = ( xL(1) - xL(end-1) ) / (2*ds);
%        yL_s(i) = ( yL(1) - yL(end-1) ) / (2*ds);
%     end
% end

% Matrices for calculating derivatives
%AL2 = [1 1 1 1 1; 0 1 2 3 4; 0 1 4 9 16; 0 1 8 27 64; 0 1 16 3^4 4^4];
%AL1 = [1 1 1 1 1; -1 0 1 2 3; 1 0 1 4 9; -1 0 1 8 27; 1 0 1 16 81];
%A0 = [1 1 1 1 1; -2 -1 0 1 2; 4 1 0 1 4; -8 -1 0 1 8; 16 1 0 1 16];
%AR1 = [1 1 1 1 1; -3 -2 -1 0 1; 9 4 1 0 1; -27 -8 -1 0 1; 81 16 1 0 1];
%AR2 = [1 1 1 1 1; -4 -3 -2 -1 0; 16 9 4 1 0; -64 -27 -8 -1 0; 256 81 16 1 0];
%rhs = [0 1 0 0 0]';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %
% % FUNCTION: compute Porous Connections
% %
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% function connects_Por = give_Me_Porous_Connections(ds,Np,porous_info)
%
% xL = porous_info(:,2);
% yL = porous_info(:,3);
%
% start = 1;
% last = Np;
% prev = last;
% for i=1:Np
%
%     x0 = xL(i); y0 = yL(i);        % central node value
%     xm1= xL(prev); ym1 = yL(prev); % 'previous' values
%     xp1 =xL(i+1); yp1=yL(i+1);     % 'next' values
%
%     distL = sqrt( (x0-xm1)^2 + (y0-ym1)^2 );
%     distR = sqrt( (x0-xp1)^2 + (y0-yp1)^2 );
%
%
% end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: computes Lagrangian UNIT Normal Vectors
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [nX,nY,sqrtN] = give_Me_Lagrangian_Normal_Vectors(xL_s,yL_s)

sqrtN = sqrt( (xL_s).^2 + (yL_s).^2 );

nX = ( yL_s ) ./ sqrtN;
nY = ( -xL_s) ./ sqrtN;


%#eml
function [qdd,Fzf,Fzr,Flr,Ftr,Flf,Ftf,vlr,vtr,vlf,vtf,r0,r7,IMU,fx,hx]         = mdl_motorcycle(u,x,p)
% from nonlinear state space to generalized coordinates
qd=x(1:13);
q =x(14:26);
% coordinates
qx = q(1)  ; qxd = qd(1) ; qy = q(2)  ; qyd = qd(2) ; qz = q(3)  ; qzd = qd(3);
q0 = q(4)  ; q0d = qd(4) ; q1 = q(5)  ; q1d = qd(5) ; q2 = q(6)  ; q2d = qd(6);
q4 = q(7)  ; q4d = qd(7) ; q5 = q(8)  ; q5d = qd(8) ; q6 = q(9)  ; q6d = qd(9);
q7 = q(10) ; q7d = qd(10); qf = q(11) ; qfd = qd(11);
q8 = q(12) ; q8d = qd(12); q9 = q(13) ; q9d = qd(13);

% parameters
a1 = p(1)  ; l2 = p(11)  ; l3 = p(23) ; m5 = p(35)  ; a6 = p(47) ;
b1 = p(2)  ; m2 = p(12)  ; m3 = p(24) ; x5 = p(36)  ; b6 = p(48) ;
m1 = p(3)  ; x2 = p(13)  ; x3 = p(25) ; z5 = p(37)  ; m6 = p(49) ;
i1 = p(4)  ; i2 = p(14)  ; z3 = p(26) ; i5 = p(38)  ; i6 = p(50) ;
j1 = p(5)  ; t2 = p(15)  ; i3 = p(27) ; t5 = p(39)  ; j6 = p(51) ;
f1 = p(6)  ; b2 = p(16)  ; k3 = p(28) ; b5 = p(40)  ; f6 = p(52) ;
e1 = p(7)  ; p2 = p(17)  ; d3 = p(29) ; p5 = p(41)  ; e6 = p(53) ;
k1 = p(8)  ; d2 = p(18)  ; l4 = p(30) ; d5 = p(42)  ; k6 = p(54) ;
d1 = p(9)  ; e2 = p(19)  ; m4 = p(31) ; e5 = p(43)  ; d6 = p(55) ;
t1 = p(10) ; k2 = p(20)  ; x4 = p(32) ; k5 = p(44)  ; t6 = p(56) ;
             n2 = p(21)  ; z4 = p(33) ; n5 = p(45)  ;
             f2 = p(22)  ; i4 = p(34) ; f5 = p(46)  ;

u7=p(57); v7=p(58); w7=p(59); x7=p(60); y7=p(61); z7=p(62);
I1x=j1;I1y=i1;I1z=j1;I2x=i2;I2y=i2;I2z=i2;I3x=i3;I3y=i3;I3z=i3;
I4x=i4;I4y=i4;I4z=i4;I5x=i5;I5y=i5;I5z=i5;I6x=j6;I6y=i6;I6z=j6;

% trigonometry
s0 = sin(q0); c0 = cos(q0); s1 = sin(q1); c1 = cos(q1); s2 = sin(q2);c2 = cos(q2);
s4 = sin(q4); c4 = cos(q4); s5 = sin(q5); c5 = cos(q5); s6 = sin(q6);c6 = cos(q6);
s7 = sin(q7); c7 = cos(q7); s8 = sin(q8); c8 = cos(q8); s9 = sin(q9);c9 = cos(q9);

% basic rotations
R0 = [c0 -s0 0;s0 c0 0;0 0 1]; R0q = [-s0 -c0 0;c0 -s0 0;0 0 0]; %rear yaw
R1 = [1 0 0;0 c1 -s1;0 s1 c1]; R1q = [0 0 0;0 -s1 -c1;0 c1 -s1]; %rear roll
R2 = [c2 0 s2;0 1 0;-s2 0 c2]; R2q = [-s2 0 c2;0 0 0;-c2 0 -s2]; %rear pitch
R4 = [c4 -s4 0;s4 c4 0;0 0 1]; R4q = [-s4 -c4 0;c4 -s4 0;0 0 0]; %front yaw
R5 = [1 0 0;0 c5 -s5;0 s5 c5]; R5q = [0 0 0;0 -s5 -c5;0 c5 -s5]; %front roll
R6 = [c6 0 s6;0 1 0;-s6 0 c6]; R6q = [-s6 0 c6;0 0 0;-c6 0 -s6]; %front pitch
R7 = [c7 0 s7;0 1 0;-s7 0 c7]; R7q = [-s7 0 c7;0 0 0;-c7 0 -s7]; %swingarm angle
R8 = [c8 0 s8;0 1 0;-s8 0 c8]; R8q = [-s8 0 c8;0 0 0;-c8 0 -s8]; %rear wheel angle
R9 = [c9 0 s9;0 1 0;-s9 0 c9]; R9q = [-s9 0 c9;0 0 0;-c9 0 -s9]; %front wheel angle

% inertia matrix;
sa = m3*(x3-l3)-(m1+m2)*l3;
sb = m3*z3;
sc = m2*(x2-l2)-m1*l2;
sd = m4*x4+m5*(l4+x5)+m6*l4;
se = m4*z4+m5*z5;
sf = -m5-m6;
sg = m1*l2^2+m2*(x2-l2)^2+I2y;
sh = m6*l4+m5*(l4+x5);
si = 2*m5*z5;
sj = m4*(x4^2+z4^2)+m5*((l4+x5)^2+z5^2)+I4y+I5y+m6*l4^2;
sk = m5*(l4+x5)+m6*l4;
sl = (m1+m2)*l3^2+m3*((-l3+x3)^2+z3^2)+I3y;
sm = l3*(m1*l2+m2*(l2-x2));
sn = m3*(z3^2+x3^2)+l3*((m1+m2+m3)*l3-2*m3*x3)+I3y;
so = l3*sc;
sp = I5z-I5x+I4z-I4x;

M          = (m1+m2+m3+m4+m5+m6)*eye(13);
M(1:3,4:11)= [R0q*(R1*(R2*[sa;0;sb]+R7*[sc;0;0])) R0*(R1q*(R2*[sa;0;sb]+R7*[sc;0;0])) R0*(R1*(R2q*[sa;0;sb])) R4q*(R5*(R6*[sd;0;se+sf*qf])) R4*(R5q*(R6*[sd;0;se+sf*qf])) R4*(R5*(R6q*[sd;0;se+sf*qf])) R0*(R1*(R7q*[sc;0;0])) R4*(R5*(R6*[0;0;sf]))];
M(4,4)     = [1 1 0]*R1.^2*((m1*(R2*[-l3;0;0]+R7*[-l2;0;0]).^2+m2*(R2*[-l3;0;0]+R7*[-l2+x2;0;0]).^2+m3*(R2*[-l3+x3;0;z3]).^2))+I3y+I2y+I1y+c1^2*(I1x-I1y+I2x-I2y+I3x-I3y+c7^2*(I2z-I2x)+c2^2*(I3z-I3x));
M(4,5)     = -c1*(m1*(-c2*l3-c7*l2)*(s2*l3+s7*l2)+m2*(-c2*l3-c7*l2+c7*x2)*(s2*l3+s7*l2-s7*x2)+m3*(-c2*l3+c2*x3+s2*z3)*(s2*l3-s2*x3+c2*z3))+c1*s7*c7*(I2z-I2x)+c1*s2*c2*(I3z-I3x);
M(4,6)     = s1*(sn-cos(q2-q7)*so);
M(4,10)    = s1*(sg+cos(q2-q7)*sm);
M(4,12)    = I1y*s1;
M(5,5)     = m1*(s2*l3+s7*l2)^2+m2*(s2*l3-s7*(-l2+x2))^2+m3*(-s2*(-l3+x3)+c2*z3)^2+c7^2*(I2x-I2z)+c2^2*(I3x-I3z)+I3z+I2z+I1x;
M(6,6)     = sl;
M(6,10)    = cos(q2-q7)*sm;
M(7,7)     = [1 1 0]*(R5.^2*(m4*(R6*[x4;0;z4]).^2+m5*(R6*[l4+x5;0;-qf+z5]).^2+m6*(R6*[l4;0;-qf]).^2))+I6y+I5y+I4y+c5^2*(I6x-I6y+I5x-I5y+I4x-I4y+c6^2*sp);
M(7,8)     = -c5*(c6*s6*(m4*(z4^2-x4^2)+m5*((z5-qf)^2-(l4+x5)^2)+m6*(qf^2-l4^2))+cos(2*q6)*(m4*x4*z4+m5*(l4+x5)*(z5-qf)-m6*l4*qf))+c5*s6*c6*sp;
M(7,9)     = s5*(sj-sf*qf^2-si*qf);
M(7,11)    = s5*sk;
M(7,13)    = I6y*s5;
M(8,8)     = m4*(c6*z4-s6*x4)^2+m5*(-s6*(l4+x5)+c6*(z5-qf))^2+m6*(-s6*l4-c6*qf)^2-c6^2*sp+I4z+I5z+I6x;
M(9,9)     = sj-sf*qf^2-si*qf;
M(9,11)    = sh;
M(10,10)   = sg;
M(11,11)   = -sf;
M(12,12)   = I1y;
M(13,13)   = I6y;
M          = M.'+triu(M,1);

% Christoffel matrix
s=[m6*(-s6*l4-c6*qf)*c6;-s6*(l4+x5)+c6*(-qf+z5);c5*(c6*s6*(m5*(2*qf-2*z5)+2*m6*qf)+cos(2*q6)*(-m5*(l4+x5)-m6*l4));c6*(l4+x5)+s6*(-qf+z5);s1*sin(q2-q7)*sm;s1*sin(q2-q7)*so;s2*l3-s7*(-l2+x2);m1*(s2*l3+s7*l2)*c7*l2;c1*(sg+cos(q2-q7)*sm);-c2*l3-c7*l2+c7*x2;s2*l3+s7*l2-s7*x2;c1*(m1*s7*l2*(s2*l3+s7*l2)+m1*...
   (-c2*l3-c7*l2)*c7*l2+m2*(s7*l2-s7*x2)*(s2*l3+s7*l2-s7*x2)+m2*(-c2*l3-c7*l2+c7*x2)*(c7*l2-c7*x2));-c2*l3+c7*(-l2+x2);m6*(-s6*l4-c6*qf)*(-c6*l4+s6*qf);m5*(-s6*(l4+x5)+c6*(-qf+z5))*(-c6*(l4+x5)-s6*(-qf+z5));m4*(-s6*x4+c6*z4)*(-c6*x4-s6*z4);c5*(sj-sf*qf^2-si*qf);m4*x4*z4+m5*(l4+x5)*(-qf+z5)-m6*l4*qf;...
   m4*(z4^2-x4^2)+m5*((-qf+z5)^2-(l4+x5)^2)+m6*(qf^2-l4^2);c5*(-s6^2*(m4*(z4^2-x4^2)+m5*((-qf+z5)^2-(l4+x5)^2)+m6*(qf^2-l4^2))+c6^2*(m4*(z4^2-x4^2)+m5*((-qf+z5)^2-(l4+x5)^2)+m6*(qf^2-l4^2))-2*sin(2*q6)*(m4*x4*z4+m5*(l4+x5)*(-qf+z5)-m6*l4*qf));-c6*sd-s6*(se+sf*qf);-s6*sd+c6*(se+sf*qf);c5*(I6x-I6y+I5x...
   -I5y+I4x-I4y+c6^2*sp)*s5;s5*(m4*(-s6*x4+c6*z4)^2+m5*(-s6*(l4+x5)+c6*(-qf+z5))^2+m6*(-s6*l4-c6*qf)^2)*c5;-s2*(-l3+x3)+c2*z3;m3*(-s2*(-l3+x3)+c2*z3)*(-c2*(-l3+x3)-s2*z3);m1*(s2*l3+s7*l2)*c2*l3;c1*(sn-cos(q2-q7)*so);-c2*l3+c2*x3+s2*z3;s2*l3-s2*x3+c2*z3;c1*(m1*s2*l3*(s2*l3+s7*l2)+m1*(-c2*l3-c7*l2)*c2*...
   l3+m2*s2*l3*(s2*l3+s7*l2-s7*x2)+m2*(-c2*l3-c7*l2+c7*x2)*c2*l3+m3*(s2*l3-s2*x3+c2*z3)^2+m3*(-c2*l3+c2*x3+s2*z3)*(c2*l3-c2*x3-s2*z3));c1*(I1x-I1y+I2x-I2y+I3x-I3y+c7^2*(I2z-I2x)+c2^2*(I3z-I3x))*s1;s1*(m1*(s2*l3+s7*l2)^2+m2*(s2*l3-s7*(-l2+x2))^2+m3*(-s2*(-l3+x3)+c2*z3)^2)*c1;-s2*sa+c2*sb-s7*sc];
C=[0 0 0 (-c0*(c2*sa+s2*sb+c7*sc)-s0*s1*s(34))*q0d+c0*c1*s(34)*q1d+(-s0*(-s2*sa+c2*sb)+c0*s1*(-c2*sa-s2*sb))*q2d+(s0*s7*sc-c0*s1*c7*sc)*q7d c0*c1*s(34)*q0d-s0*s1*s(34)*q1d+s0*c1*(-c2*sa-s2*sb)*q2d-s0*c1*c7*sc*q7d (-s0*(-s2*sa+c2*sb)+c0*s1*(-c2*sa-s2*sb))*q0d+s0*c1*(-c2*sa-s2*sb)*q1d+(c0*(-c2*sa-s2*...
   sb)+s0*s1*(s2*sa-c2*sb))*q2d (-c4*(c6*sd+s6*(se+sf*qf))-s4*s5*s(22))*q4d+c4*c5*s(22)*q5d+(-s4*s(22)+c4*s5*s(21))*q6d+(-s4*s6*sf+c4*s5*c6*sf)*qfd c4*c5*s(22)*q4d-s4*s5*s(22)*q5d+s4*c5*s(21)*q6d+s4*c5*c6*sf*qfd (-s4*s(22)+c4*s5*s(21))*q4d+s4*c5*s(21)*q5d+(c4*s(21)+s4*s5*(s6*sd-c6*(se+sf*qf)))*q6d+...
   (c4*c6*sf-s4*s5*s6*sf)*qfd (s0*s7*sc-c0*s1*c7*sc)*q0d-s0*c1*c7*sc*q1d+(-c0*c7*sc+s0*s1*s7*sc)*q7d (-s4*s6*sf+c4*s5*c6*sf)*q4d+s4*c5*c6*sf*q5d+(c4*c6*sf-s4*s5*s6*sf)*q6d 0 0 ;0 0 0 (-s0*(c2*sa+s2*sb+c7*sc)+c0*s1*s(34))*q0d+s0*c1*s(34)*q1d+(c0*(-s2*sa+c2*sb)+s0*s1*(-c2*sa-s2*sb))*q2d+(-c0*s7*sc-s0*...
   s1*c7*sc)*q7d s0*c1*s(34)*q0d+c0*s1*s(34)*q1d-c0*c1*(-c2*sa-s2*sb)*q2d+c0*c1*c7*sc*q7d (c0*(-s2*sa+c2*sb)+s0*s1*(-c2*sa-s2*sb))*q0d-c0*c1*(-c2*sa-s2*sb)*q1d+(s0*(-c2*sa-s2*sb)-c0*s1*(s2*sa-c2*sb))*q2d (-s4*(c6*sd+s6*(se+sf*qf))+c4*s5*s(22))*q4d+s4*c5*s(22)*q5d+(c4*s(22)+s4*s5*s(21))*q6d+(c4*s6*sf+...
   s4*s5*c6*sf)*qfd s4*c5*s(22)*q4d+c4*s5*s(22)*q5d-c4*c5*s(21)*q6d-c4*c5*c6*sf*qfd (c4*s(22)+s4*s5*s(21))*q4d-c4*c5*s(21)*q5d+(s4*s(21)-c4*s5*(s6*sd-c6*(se+sf*qf)))*q6d+(s4*c6*sf+c4*s5*s6*sf)*qfd (-c0*s7*sc-s0*s1*c7*sc)*q0d+c0*c1*c7*sc*q1d+(-s0*c7*sc-c0*s1*s7*sc)*q7d (c4*s6*sf+s4*s5*c6*sf)*q4d-c4*...
   c5*c6*sf*q5d+(s4*c6*sf+c4*s5*s6*sf)*q6d 0 0 ;0 0 0 0 -c1*s(34)*q1d-s1*(-c2*sa-s2*sb)*q2d+s1*c7*sc*q7d -s1*(-c2*sa-s2*sb)*q1d+c1*(s2*sa-c2*sb)*q2d 0 -c5*s(22)*q5d-s5*s(21)*q6d-s5*c6*sf*qfd -s5*s(21)*q5d+c5*(s6*sd-c6*(se+sf*qf))*q6d-c5*s6*sf*qfd s1*c7*sc*q1d+c1*s7*sc*q7d -s5*c6*sf*q5d-c5*s6*sf*q6d ...
   0 0 ;0 0 0 (s(33)-s(32))*q1d+(m1*(-c2*l3-c7*l2)*s2*l3+m2*s(13)*s2*l3+m3*(c2*(-l3+x3)+s2*z3)*s(25)+1/2*s1^2*(2*s(27)+2*m2*s(7)*c2*l3+2*s(26))-c1^2*c2*(I3z-I3x)*s2)*q2d+(m1*(-c2*l3-c7*l2)*s7*l2-m2*s(13)*s7*(-l2+x2)+1/2*s1^2*(2*s(8)-2*m2*s(7)*c7*(-l2+x2))-c1^2*c7*(I2z-I2x)*s7)*q7d (s(33)-s(32))*q0d+...
   (s1*(m1*(-c2*l3-c7*l2)*(s2*l3+s7*l2)+m2*s(10)*s(11)+m3*s(29)*s(30))-s1*s7*c7*(I2z-I2x)-s1*s2*c2*(I3z-I3x))*q1d+(1/2*s(28)-1/2*s(31)+1/2*c1*c2^2*(I3z-I3x)-1/2*c1*s2^2*(I3z-I3x))*q2d+(1/2*s(9)-1/2*s(12)+1/2*c1*c7^2*(I2z-I2x)-1/2*c1*s7^2*(I2z-I2x))*q7d+1/2*I1y*c1*q8d (m1*(-c2*l3-c7*l2)*s2*l3+m2*s(13)...
   *s2*l3+m3*(c2*(-l3+x3)+s2*z3)*s(25)+1/2*s1^2*(2*s(27)+2*m2*s(7)*c2*l3+2*s(26))-c1^2*c2*(I3z-I3x)*s2)*q0d+(1/2*s(28)-1/2*s(31)+1/2*c1*c2^2*(I3z-I3x)-1/2*c1*s2^2*(I3z-I3x))*q1d+s1*sin(q2-q7)*so*q2d+(-1/2*s(5)-1/2*s(6))*q7d 0 0 0 (m1*(-c2*l3-c7*l2)*s7*l2-m2*s(13)*s7*(-l2+x2)+1/2*s1^2*(2*s(8)-2*m2*...
   s(7)*c7*(-l2+x2))-c1^2*c7*(I2z-I2x)*s7)*q0d+(1/2*s(9)-1/2*s(12)+1/2*c1*c7^2*(I2z-I2x)-1/2*c1*s7^2*(I2z-I2x))*q1d+(-1/2*s(5)-1/2*s(6))*q2d+s1*sin(q2-q7)*sm*q7d 0 1/2*I1y*c1*q1d 0 ;0 0 0 (-s(33)+s(32))*q0d+(-1/2*s(31)+1/2*c1*c2^2*(I3z-I3x)-1/2*c1*s2^2*(I3z-I3x)-1/2*s(28))*q2d+(-1/2*s(12)+1/2*c1*...
   c7^2*(I2z-I2x)-1/2*c1*s7^2*(I2z-I2x)-1/2*s(9))*q7d-1/2*I1y*c1*q8d (s(27)+m2*s(7)*c2*l3+s(26)-c2*(I3x-I3z)*s2)*q2d+(s(8)-m2*s(7)*c7*(-l2+x2)-c7*(I2x-I2z)*s7)*q7d (-1/2*s(31)+1/2*c1*c2^2*(I3z-I3x)-1/2*c1*s2^2*(I3z-I3x)-1/2*s(28))*q0d+(s(27)+m2*s(7)*c2*l3+s(26)-c2*(I3x-I3z)*s2)*q1d 0 0 0 (-1/2*s(12)...
   +1/2*c1*c7^2*(I2z-I2x)-1/2*c1*s7^2*(I2z-I2x)-1/2*s(9))*q0d+(s(8)-m2*s(7)*c7*(-l2+x2)-c7*(I2x-I2z)*s7)*q1d 0 -1/2*I1y*c1*q0d 0 ;0 0 0 (-m1*(-c2*l3-c7*l2)*s2*l3-m2*s(13)*s2*l3-m3*(c2*(-l3+x3)+s2*z3)*s(25)-1/2*s1^2*(2*s(27)+2*m2*s(7)*c2*l3+2*s(26))+c1^2*c2*(I3z-I3x)*s2)*q0d+(1/2*s(28)+1/2*s(31)-1/2*...
   c1*c2^2*(I3z-I3x)+1/2*c1*s2^2*(I3z-I3x))*q1d+(-1/2*s(6)+1/2*s(5))*q7d (1/2*s(28)+1/2*s(31)-1/2*c1*c2^2*(I3z-I3x)+1/2*c1*s2^2*(I3z-I3x))*q0d+(-s(27)-m2*s(7)*c2*l3-s(26)+c2*(I3x-I3z)*s2)*q1d 0 0 0 0 (-1/2*s(6)+1/2*s(5))*q0d+sin(q2-q7)*sm*q7d 0 0 0 ;0 0 0 0 0 0 (s(24)-s(23))*q5d+(m4*(c6*x4+s6*z4)*...
   (-s6*x4+c6*z4)+m5*s(4)*s(2)+m6*(c6*l4-s6*qf)*(-s6*l4-c6*qf)+1/2*s5^2*(2*s(16)+2*s(15)+2*s(14))-c5^2*c6*sp*s6)*q6d+(-m5*s(4)*s6-m6*(c6*l4-s6*qf)*s6+1/2*s5^2*(-2*m5*s(2)*c6-2*s(1)))*qfd (s(24)-s(23))*q4d+(s5*(c6*s6*s(19)+cos(2*q6)*s(18))-s5*s6*c6*sp)*q5d+(1/2*s(17)-1/2*s(20)+1/2*c5*c6^2*sp-1/2*c5*...
   s6^2*sp)*q6d+(1/2*c5*sk-1/2*s(3))*qfd+1/2*I6y*c5*q9d (m4*(c6*x4+s6*z4)*(-s6*x4+c6*z4)+m5*s(4)*s(2)+m6*(c6*l4-s6*qf)*(-s6*l4-c6*qf)+1/2*s5^2*(2*s(16)+2*s(15)+2*s(14))-c5^2*c6*sp*s6)*q4d+(1/2*s(17)-1/2*s(20)+1/2*c5*c6^2*sp-1/2*c5*s6^2*sp)*q5d+1/2*s5*(-2*sf*qf-si)*qfd 0 (-m5*s(4)*s6-m6*(c6*l4-s6*qf)...
   *s6+1/2*s5^2*(-2*m5*s(2)*c6-2*s(1)))*q4d+(1/2*c5*sk-1/2*s(3))*q5d+1/2*s5*(-2*sf*qf-si)*q6d 0 1/2*I6y*c5*q5d ;0 0 0 0 0 0 (-s(24)+s(23))*q4d+(-1/2*s(20)+1/2*c5*c6^2*sp-1/2*c5*s6^2*sp-1/2*s(17))*q6d+(-1/2*s(3)-1/2*c5*sk)*qfd-1/2*I6y*c5*q9d (s(16)+s(15)+s(14)+c6*sp*s6)*q6d+(-m5*s(2)*c6-s(1))*qfd ...
   (-1/2*s(20)+1/2*c5*c6^2*sp-1/2*c5*s6^2*sp-1/2*s(17))*q4d+(s(16)+s(15)+s(14)+c6*sp*s6)*q5d 0 (-1/2*s(3)-1/2*c5*sk)*q4d+(-m5*s(2)*c6-s(1))*q5d 0 -1/2*I6y*c5*q4d ;0 0 0 0 0 0 (-m4*(c6*x4+s6*z4)*(-s6*x4+c6*z4)-m5*s(4)*s(2)-m6*(c6*l4-s6*qf)*(-s6*l4-c6*qf)-1/2*s5^2*(2*s(16)+2*s(15)+2*s(14))+c5^2*c6*sp*...
   s6)*q4d+(1/2*s(17)+1/2*s(20)-1/2*c5*c6^2*sp+1/2*c5*s6^2*sp)*q5d+1/2*s5*(-2*sf*qf-si)*qfd (1/2*s(17)+1/2*s(20)-1/2*c5*c6^2*sp+1/2*c5*s6^2*sp)*q4d+(-s(16)-s(15)-s(14)-c6*sp*s6)*q5d (-sf*qf-1/2*si)*qfd 0 1/2*s5*(-2*sf*qf-si)*q4d+(-sf*qf-1/2*si)*q6d 0 0 ;0 0 0 (-m1*(-c2*l3-c7*l2)*s7*l2+m2*s(13)*s7*...
   (-l2+x2)-1/2*s1^2*(2*s(8)-2*m2*s(7)*c7*(-l2+x2))+c1^2*c7*(I2z-I2x)*s7)*q0d+(1/2*s(9)+1/2*s(12)-1/2*c1*c7^2*(I2z-I2x)+1/2*c1*s7^2*(I2z-I2x))*q1d+(-1/2*s(5)+1/2*s(6))*q2d (1/2*s(9)+1/2*s(12)-1/2*c1*c7^2*(I2z-I2x)+1/2*c1*s7^2*(I2z-I2x))*q0d+(-s(8)+m2*s(7)*c7*(-l2+x2)+c7*(I2x-I2z)*s7)*q1d (-1/2*s(5)+...
   1/2*s(6))*q0d-sin(q2-q7)*sm*q2d 0 0 0 0 0 0 0 ;0 0 0 0 0 0 (m5*s(4)*s6+m6*(c6*l4-s6*qf)*s6-1/2*s5^2*(-2*m5*s(2)*c6-2*s(1)))*q4d+(1/2*c5*sk+1/2*s(3))*q5d-1/2*s5*(-2*sf*qf-si)*q6d (1/2*c5*sk+1/2*s(3))*q4d+(m5*s(2)*c6+s(1))*q5d -1/2*s5*(-2*sf*qf-si)*q4d+(sf*qf+1/2*si)*q6d 0 0 0 0 ;0 0 0 1/2*I1y*c1*...
   q1d 1/2*I1y*c1*q0d 0 0 0 0 0 0 0 0 ;0 0 0 0 0 0 1/2*I6y*c5*q5d 1/2*I6y*c5*q4d 0 0 0 0 0 ];

% preparation on force calculation
g=9.81;
q70=-.2;
qf0=0.3;

r0d =[qxd+(-s0*(-c2*l3-c7*l2)-c0*(-c1*s1*a1-s1*(s2*l3+s7*l2-b1-c1*a1)))*q0d+s0*c1*(s2*l3+s7*l2-b1-c1*a1)*q1d+(c0*s2*l3+s0*s1*c2*l3)*q2d+(c0*s7*l2+s0*s1*c7*l2)*q7d+c0*(-b1-c1*a1)*q8d
      qyd+(c0*(-c2*l3-c7*l2)-s0*(-c1*s1*a1-s1*(s2*l3+s7*l2-b1-c1*a1)))*q0d-c0*c1*(s2*l3+s7*l2-b1-c1*a1)*q1d+(s0*s2*l3-c0*s1*c2*l3)*q2d+(s0*s7*l2-c0*s1*c7*l2)*q7d+s0*(-b1-c1*a1)*q8d
      qzd-s1*(s2*l3+s7*l2-b1-c1*a1)*q1d+c1*c2*l3*q2d+c1*c7*l2*q7d];
r7d =[qxd+(-s4*(c6*l4-s6*qf)-c4*(-c5*s5*a6-s5*(-s6*l4-c6*qf-b6-c5*a6)))*q4d+s4*c5*(-s6*l4-c6*qf-b6-c5*a6)*q5d+(c4*(-s6*l4-c6*qf)+s4*s5*(-c6*l4+s6*qf))*q6d+(-c4*s6-sin(q4)*s5*c6)*qfd+c4*(-b6-c5*a6)*q9d
      qyd+(c4*(c6*l4-s6*qf)-s4*(-c5*s5*a6-s5*(-s6*l4-c6*qf-b6-c5*a6)))*q4d-c4*c5*(-s6*l4-c6*qf-b6-c5*a6)*q5d+(s4*(-s6*l4-c6*qf)-c4*s5*(-c6*l4+s6*qf))*q6d+(-s4*s6+c4*s5*c6)*qfd+s4*(-b6-c5*a6)*q9d
      qzd-s5*(-s6*l4-c6*qf-b6-c5*a6)*q5d+c5*(-c6*l4+s6*qf)*q6d-c5*c6*qfd];

r0  = [0;0;qz+c1*(s2*l3+s7*l2-b1)-a1];
r7  = [0;0;qz-c5*(s6*l4+c6*qf+b6)-a6];
vtr =-s0*r0d(1)+c0*r0d(2);
vlr = c0*r0d(1)+s0*r0d(2)+.01;
vtf =-s4*r7d(1)+c4*r7d(2);
vlf = c4*r7d(1)+s4*r7d(2)+.01;
v1  = [c0*s2+s0*s1*c2;s0*s2-c0*s1*c2;c1*c2];
v2  = [c4*s6+s4*s5*c6;s4*s6-c4*s5*c6;c5*c6];

Rm3 = R0*(R1*R2); % frame
Rm4 = R4*R5*R6; % steering head

    % 1. find the axis about to rotate and the angle
    v1 = Rm3*[0;0;1];
    v2 = Rm4*[0;0;1];
    w1= cross(v1,v2);
    % 2. use Rodrigues rotation formula to get the new x and y vectors
    x_old = Rm3*[1;0;0];
    y_old = Rm3*[0;1;0];
    x_new = x_old*v1.'*v2+cross(w1,x_old)+w1*w1.'*x_old/(1+v1.'*v2);
    y_new = y_old*v1.'*v2+cross(w1,y_old)+w1*w1.'*y_old/(1+v1.'*v2);
    % 3. calculate angle between the vectors
    qxc = real(x_new.'*Rm4(:,1));
    qyc = real(y_new.'*Rm4(:,2));
    qxs = real(-x_new.'*Rm4(:,2));
    qys = real(y_new.'*Rm4(:,1));
% q3=atan2(qxs,qxc);
% q3=atan2(qxs,qyc);
q3=atan2(qys,qxc);
% q3=atan2(qys,qyc);

T3  = -u(2)*(v1+v2)/norm(v1+v2);

% forces/torques
Frs = k2*(q2-q7-q70)+b2*(q2d-q7d);
Ffs = k5*(qf-p5)+d5*qfd*(qfd<0)+e5*qfd*(qfd>0)-k3*(qf<b5)+k3*(qf>t5);

Frb = u(1)*(u(1)>0);
Ffb = u(1)*(u(1)<0);

Fzr = max((-k1*r0(3)-d1*r0d(3))*(r0(3)<0),0) ;
Ftr = t1*Fzr*atan(-vtr/vlr);
Flr = Fzr*min(max(f1*((b1+a1*cos(q1))*q8d/vlr-1),-1),1);

Fzf = max((-k1*r7(3)-d1*r7d(3))*(r7(3)<0),0) ;
Ftf = t6*Fzf*atan(-vtf/vlf);
Flf = Fzf*min(max(f6*((b6+a6*cos(q5))*q9d/vlf-1),-1),1);

T1  =  k3*(pi/2-dot(v1,v2))*cross(v1,v2);

%% experiment
sr    =  r0d(1:2);
Fr    =  -Fzr*(sr./((sr.'*sr)^4+1)^(1/8)+sr./((sr.'*sr)^2+1));
Flr =  c0*Fr(1)+s0*Fr(2);
Ftr = -s0*Fr(1)+c0*Fr(2);

ss    =  r7d(1:2);
Ff    =  -Fzf*(ss./((ss.'*ss)^4+1)^(1/8)+ss./((ss.'*ss)^2+1));
Flf =  c4*Ff(1)+s4*Ff(2);
Ftf = -s4*Ff(1)+c4*Ff(2);
%% end of experiment

% work done by forces/torques
Qfs = [0 0 0 0 0 0 0 0 0 0 -Ffs 0 0].';
Qrs = [0 0 0 0 0 -Frs 0 0 0 Frs 0 0 0].';
Qfb = [0 0 0 0 0 0 0 0 -Ffb 0 0 0 Ffb].';
Qrb = [0 0 0 0 0 0 0 0 0 -Frb 0 Frb 0].';
QT1 = [0 0 0 T1(3) c0*T1(1)+s0*T1(2) -s0*c1*T1(1)+c0*c1*T1(2)+s1*T1(3) -T1(3) -c4*T1(1)-s4*T1(2) s4*c5*T1(1)-c4*c5*T1(2)-s5*T1(3) 0 0 0 0].';
QT3 = [0 0 0 T3(3) c0*T3(1)+s0*T3(2) -s0*c1*T3(1)+c0*c1*T3(2)+s1*T3(3) -T3(3) -c4*T3(1)-s4*T3(2) s4*c5*T3(1)-c4*c5*T3(2)-s5*T3(3) 0 0 0 0].';
Qrz = [0 0 1 0 -s1*(s2*l3+s7*l2-b1) c1*c2*l3 0 0 0 c1*c7*l2 0 0 0                    ].'*Fzr;
Qfz = [0 0 1 0 0 0 0 s5*(s6*l4+c6*qf+b6) c5*(s6*qf-c6*l4) 0 -c5*c6 0 0               ].'*Fzf;
Qlf = [c4 s4 0 0 0 0 -s5*(s6*l4+c6*qf+b6) 0 -s6*l4-c6*qf 0 -s6 0 -c5*a6-b6           ].'*Flf;
Qlr = [c0 s0 0 -s1*(-s2*l3-s7*l2+b1) 0 s2*l3 0 0 0 s7*l2 0 -c1*a1-b1 0               ].'*Flr;
Qtf = [-s4 c4 0 0 0 0 c6*l4-s6*qf c5*(s6*l4+c6*qf+b6)+a6 s5*(c6*l4-s6*qf) 0 s5*c6 0 0].'*Ftf;
Qtr = [-s0 c0 0 -c2*l3-c7*l2 c1*(b1-s2*l3-s7*l2)+a1 -s1*c2*l3 0 0 0 -s1*c7*l2 0 0 0  ].'*Ftr;
Vq  = -g*[0 0 m1+m2+m3+m4+m5+m6 0 -m1*s1*(s2*l3+s7*l2)-m2*s1*(s2*l3-s7*(-l2+x2))-m3*s1*(-s2*(-l3+x3)+c2*z3) (m1+m2)*c1*c2*l3+m3*c1*(-c2*(-l3+x3)-s2*z3) 0 (-m4*s5*(-s6*x4+c6*z4)-m5*s5*(-s6*(l4+x5)+c6*(-qf+z5))-m6*s5*(-s6*l4-c6*qf)) m4*c5*(-c6*x4-s6*z4)+m5*c5*(-c6*(l4+x5)-s6*(-qf+z5))+m6*c5*(-c6*l4+s6*qf) m1*c1*c7*l2-m2*c1*c7*(-l2+x2) c5*c6*sf 0 0];
% Q   = Vq.'+Qfs+Qrs+Qrz+Qfz+QT1+QT3+Qlf+Qlr+Qtf;
Q   = Vq.'+Qfs+Qrs+Qrz+Qfz+QT1+QT3+Qtf+Qlf+Qlr+Qtr+Qfb+Qrb;
% Q   = Vq.'+Qfs+Qrs+Qrz+Qfz+QT1+QT3+Qflon+Qrlon+Qflat+Qrlat;

qdd = M\(Q-C*qd);

IMU = [0;0;0;0;0;0];

qxdd=qdd(1);
qydd=qdd(2);
qzdd=qdd(3);
q0dd=qdd(4);
q1dd=qdd(5);
q2dd=qdd(6);

su = sin(u7); cu = cos(u7); sv = sin(v7); cv = cos(v7); sw = sin(w7);cw = cos(w7);

% basic rotations
R0 = [c0 -s0 0;s0 c0 0;0 0 1]; R0q = [-s0 -c0 0;c0 -s0 0;0 0 0]; R0qq = [-c0 s0 0;-s0 -c0 0;0 0 0];
R1 = [1 0 0;0 c1 -s1;0 s1 c1]; R1q = [0 0 0;0 -s1 -c1;0 c1 -s1]; R1qq = [0 0 0;0 -c1 s1;0 -s1 -c1];
R2 = [c2 0 s2;0 1 0;-s2 0 c2]; R2q = [-s2 0 c2;0 0 0;-c2 0 -s2]; R2qq = [-c2 0 -s2;0 0 0;s2 0 -c2];

Ru = [cu -su 0;su cu 0;0 0 1];
Rv = [1 0 0;0 cv -sv;0 sv cv];
Rw = [cw 0 sw;0 1 0;-sw 0 cw];

% position vector
rm7 = [qx;qy;qz]+R0*(R1*(R2*[x7;y7;z7]));

% % velocity vector
% rm7d=jacobian(rm7,q)*qd
% rm7d1= [qxd;qyd;qzd]+q0d*R0q*(R1*(R2*[x7;y7;z7]))+R0*(q1d*R1q*(R2*[x7;y7;z7])+R1*(q2d*R2q*[x7;y7;z7]))
% rm7d2= [qxd;qyd;qzd]+R0q*(R1*(R2*q0d*[x7;y7;z7]))+R0*(R1q*(R2*q1d*[x7;y7;z7])+R1*(R2q*q2d*[x7;y7;z7]))
% temp=simple(rm7d1-rm7d2)
% acceleration vector
% rm7dd=(jacobian(rm7d,[q;qd])*[qd;qdd])
% rm7dd1=(jacobian(rm7d1,[q;qd])*[qd;qdd])% this vector is in global coordinates, but the sensor measures in local coordinates.
% rm7dd2=[qxdd;qydd;qzdd] ...
%     +R0qq*(R1*(R2*q0d^2*[x7;y7;z7]))   +R0q*(R1q*(R2*q0d*q1d*[x7;y7;z7])) +R0q*(R1*(R2q*q0d*q2d*[x7;y7;z7])) +R0q*(R1*(R2*q0dd*[x7;y7;z7])) ...
%     +R0q*(R1q*(R2*q0d*q1d*[x7;y7;z7])) +R0*(R1qq*(R2*q1d^2*[x7;y7;z7]))   +R0*(R1q*(R2q*q1d*q2d*[x7;y7;z7])) +R0*(R1q*(R2*q1dd*[x7;y7;z7])) ...
%     +R0q*(R1*(R2q*q0d*q2d*[x7;y7;z7])) +R0*(R1q*(R2q*q1d*q2d*[x7;y7;z7])) +R0*(R1*(R2qq*q2d^2*[x7;y7;z7]))   +R0*(R1*(R2q*q2dd*[x7;y7;z7]))
% temp=simple(rm7dd-rm7dd2)

% absolute position measured in local body coordinates
% - rm7loc = R2'*R1'*R0'*([qx;qy;qz]+R0*R1*R2*[x7;y7;z7])
% - rm7loc = R2'*R1'*R0'*[qx;qy;qz]+ R2'*R1'*R0'*R0*R1*R2*[x7;y7;z7]
% - rm7loc = R2'*R1'*R0'*[qx;qy;qz]+ [x7;y7;z7]
% rm7loc = R2.'*R1.'*R0.'*[qx;qy;qz]+ [x7;y7;z7]
% how can this be possible? this implies (or not?) that the velocity of the sensor is
% independent of its local position [x7;y7;z7].

% absolute velocity measured in local body coordinates
% rm7dloc8 = R2.'*(R1.'*(((R0.'*[qxd;qyd;qzd])) + (R0tR0q *(R1*(R2*q0d*[x7;y7;z7]))))+(R1tR1q*(R2*q1d*[x7;y7;z7])))+(R2tR2q*q2d*[x7;y7;z7])

% syms s0 s1 s2 c0 c1 c2
% rm7ddloc6 =  R2.'*R1.'*R0.'*[qxdd;qydd;qzdd]+ ...
%   [z7*(q2dd + (q1d^2*sin(2*q2))/2 + q0dd*s1 - (q0d^2*sin(2*q2)*c1^2)/2 + 2*q0d*q1d*cos(q1)*cos(q2)^2) - x7*(q0d^2*(c2^2 + s1^2*s2^2) + q1d^2*s2^2 + q2d^2 + 2*q0d*q2d*s1 + q0d*q1d*sin(2*q2)*c1) - y7*(c1*s1*s2*q0d^2 - 2*q1d*c2*s1*q0d + q1dd*s2 + q0dd*c1*c2);
%    x7*(-c1*s1*s2*q0d^2 - 2*q2d*c1*s2*q0d + q1dd*s2 + 2*q1d*q2d*c2 + q0dd*c1*c2) - y7*(q0d^2*c1^2 + q1d^2) + z7*((sin(2*q1)*c2*q0d^2)/2 + 2*q2d*c1*c2*q0d - q1dd*c2 + 2*q1d*q2d*s2 + q0dd*c1*s2);
%    y7*((sin(2*q1)*c2*q0d^2)/2 + 2*q1d*s1*s2*q0d + q1dd*c2 - q0dd*c1*s2) - z7*(q1d^2*c2^2 - q0d^2*(c1^2*c2^2 - 1) + q2d^2 + 2*q0d*q2d*s1 - q0d*q1d*sin(2*q2)*c1) - x7*(q2dd - (q1d^2*sin(2*q2))/2 + q0dd*s1 + (q0d^2*sin(2*q2)*c1^2)/2 + 2*q0d*q1d*c1*s2^2)]

rm7ddloc6 =  R2.'*(R1.'*(R0.'*[qxdd;qydd;qzdd+9.81]))+ ...
[ z7*(q2dd+q0dd*s1+c2*s2*(q1d^2-q0d^2*c1^2)+2*c1*c2^2*q0d*q1d) ...
- y7*(s2*(c1*s1*q0d^2+q1dd)+c2*(c1*q0dd-2*q1d*s1*q0d)) ...
- x7*(q1d^2*s2^2+q0d^2*s1^2*s2^2+q0d^2*c2^2+2*q0d*q2d*s1+2*c1*c2*q0d*q1d*s2+q2d^2); ...
  z7*((c1*(s1*q0d^2+2*q2d*q0d)-q1dd)*c2+(c1*q0dd+2*q1d*q2d)*s2) ...
+ x7*(-c1*s1*s2*q0d^2-2*c1*q2d*s2*q0d+q1dd*s2+c1*c2*q0dd+2*c2*q1d*q2d) ...
- y7*(c1^2*q0d^2+q1d^2); ...
  y7*(c2*(c1*s1*q0d^2+q1dd)+s2*(2*s1*q0d*q1d-c1*q0dd)) ...
- x7*(q2dd+q0dd*s1+c2*s2*(q0d^2*c1^2-q1d^2)+2*c1*q0d*q1d*s2^2) ...
- z7*(q0d^2*(1-c2^2*c1^2)+c2^2*q1d^2+2*q0d*(q2d*s1-c1*c2*q1d*s2)+q2d^2)]...
;

rm7ddloc6 = Rw.'*(Rv.'*(Ru.'*rm7ddloc6));

wrel=R2.'*R1.'*R0.'*[0;0;q0d]+R2.'*R1.'*[q1d;0;0]+R2.'*[0;q2d;0];
wrel = Rw.'*(Rv.'*(Ru.'*wrel));

IMU=[wrel;rm7ddloc6];

% from generalized coordinates to nonlinear state space
fx=[qdd;qd];
hx=[IMU];


function [y_in_x,x_in_y]=cosmo_overlap(xs,ys)
% compute overlap between vectors or cellstrings in two cells
%
% [y_in_x,x_in_y]=cosmo_overlap(xs,ys)
%
% Inputs:
%    xs         Nx1 cell with all elements either numeric arrays or
%               cells with strings
%    ys         Mx1 cell with all elements either numeric arrays or
%               cells with strings
%
% Output:
%    y_in_x     MxN cell with the (i,j)-th element indicating the ratio of
%               elements in ys{j} that are present in xs{i}
%    x_in_i     MxN cell with the (i,j)-th element indicating the ratio of
%               elements in ys{i} that are present in ys{i}
%
% Examples:
%     % Compute overlap between two cells with cellstrings
%     xs={{'a'},{'a','b'},{'a','b','c'},{}};
%     ys={{'b'},{'c','b','a'}};
%     [x_in_y,y_in_x]=cosmo_overlap(xs,ys);
%     cosmo_disp(x_in_y);
%     %|| [ 0     0.333
%     %||   1     0.667
%     %||   1         1
%     %||   0         0 ]
%     %||
%     cosmo_disp(y_in_x);
%     %|| [     0         1
%     %||     0.5         1
%     %||   0.333         1
%     %||     NaN       NaN ]
%
%     % Compute overlap between two cells with numeric arrays
%     xs={1,[1 2],1:3,[]};
%     ys={2,[3,2,1]};
%     [x_in_y,y_in_x]=cosmo_overlap(xs,ys);
%     cosmo_disp(x_in_y);
%     %|| [ 0     0.333
%     %||   1     0.667
%     %||   1         1
%     %||   0         0 ]
%     %||
%     cosmo_disp(y_in_x);
%     %|| [     0         1
%     %||     0.5         1
%     %||   0.333         1
%     %||     NaN       NaN ]
%
%
% #   For CoSMoMVPA's copyright information and license terms,   #
% #   see the COPYING file distributed with CoSMoMVPA.           #

    [xs_vec,nx,nxs,xi]=get_counts(xs);
    [ys_vec,ny,nys,yi]=get_counts(ys);

    xy_cell=[xs_vec(:);ys_vec(:)];
    xyc=cat(1,xy_cell{:});

    % index xs from 1 to nx, and ys from (nx+1) to (nx+ny)
    xyi=[xi;(nx+yi)];

    % space for histogram
    h=zeros(nx,ny);

    % position of index of last value in xs
    xs_pos_last=sum(nxs);

    % because string comparisons are slow, use their indices instead
    [unq,unused,idxs]=unique(xyc);
    nunq=numel(unq);
    [idxs_sorted,i_sorted]=sort(idxs);

    msk=[true; any(diff(idxs_sorted,1),2)];
    unq_start_end_pos=[find(msk); 1+numel(msk)];

    for j=1:nunq
        % more readible, but slower:
        %    start_pos=unq_start_end_pos(j);
        %    end_pos=unq_start_end_pos(j+1)-1;
        %    if i_sorted(start_pos)>xs_pos_last [...]
        if i_sorted(unq_start_end_pos(j))<=xs_pos_last && ...
                i_sorted(unq_start_end_pos(j+1)-1)>xs_pos_last

            start_pos=unq_start_end_pos(j);
            end_pos=unq_start_end_pos(j+1)-1;

            i=i_sorted(start_pos:(end_pos));

            first_y=find_first_greater_than(i,xs_pos_last);
            px=xyi(i(1:(first_y-1)));
            py=xyi(i(first_y:end))-nx;

            h(px,py)=h(px,py)+1;
        end
    end

    x_in_y=bsxfun(@rdivide,h,nxs);
    y_in_x=bsxfun(@rdivide,h,nys');

function i=find_first_greater_than(sorted_vs,thr)
    % using binary search, find the first position i in sorted_vs
    % so that sorted(vs)>thr
    % it is assumed that sorted_vs is sorted
    if sorted_vs(end)<=thr
        i=numel(sorted_vs)+1;
        return
    end

    first=1;
    last=numel(sorted_vs);

    while first<last
        mid=floor((first+last)/2);
        if sorted_vs(mid)<=thr
            first=mid+1;
        else
            last=mid;
        end
    end
    i=first;
    %assert(sorted_vs(i)>thr);
    %assert(i==1 || sorted_vs(i-1)<=thr);

function [xs_vec,n,c,i]=get_counts(xs)
    n=numel(xs);
    c=zeros(n,1);
    xs_vec=cell(n,1);
    for k=1:n
        xsk=xs{k};
        if ~isnumeric(xsk) && ~iscellstr(xsk)
            error('only cells with numeric or cellstr input is supported');
        end
        c(k)=numel(xsk);
        xs_vec{k}=xsk(:);
    end

    nc=sum(c);
    i=zeros(nc,1);
    pos=0;
    for k=1:n
        ck=c(k);
        idxs=pos+(1:ck);
        i(idxs)=k;
        pos=pos+ck;
    end


function [ar,e,dc]=v_lpccovar(s,p,t,w)
%V_LPCCOVAR performs covariance LPC analysis [AR,E,DC]=(S,P,T)
%
%  Inputs:  S(NS)    is the input signal
%           P        is the order (default: 12)
%           T(NF,:)  specifies the frames size details: each row specifies one frame
%                    T can be a cell array if rows have unequal numbers of values
%                       T(:,1) gives the start of the analysis interval: must be >P
%                       T(:,2) gives the end of the anaylsis interval [default: t(:+1,1)-1]
%                       subsequent pairs can be used to specify multiple disjoint segments
%                    If T is omitted, T(1,1)=P+1, T(1,2)=NS;
%                    The elements of t need not be integers.
%           W(NS)    The error at each sample is weighted by W^2 (default: 1)
%
% Outputs:  AR(NF,P+1)  are the AR coefficients with AR(:,1) = 1
%           E(NF,4)     each row is [Er Es Pr Ps] and gives the energy ("E") and power ("P")
%                       in the input signal window ("s") and in the LPC residual "r".
%                       The 'gain' of the LPC filter is g=sqrt(Pr); x=filter(g,ar,randn(:,1)) will
%                       generate noise with approximately the same power spectrum as the input s.
%           DC          is the DC component of the signal S. If this output is included,
%                       the LPC equations are modified to include a DC offset.

% Notes:
%
% (1a) If no DC output is specified AR(j,:)*S(n-(0:P)) ~ 0 or, equivalently,
%      S(n) ~ -AR(j,2:P)*S(n-(1:P)) where T(j,1) <= n <= T(j,2).
% (1b) If a DC output is specified AR(j,:)*(S(n-(0:P))-DC) ~ 0 or, equivalently,
%      S(n) ~ DC - AR(j,2:P)*(S(n-(1:P))-DC) = DC*sum(AR,j,:)) - AR(j,2:P)*S(n-(1:P))
%      where T(j,1) <= n <= T(j,2).
%
% (2) For speech processing P should be at least 2*F*L/C where F is the sampling
%     frequency, L the vocal tract length and C the speed of sound. For a typical
%     male (l=17 cm) this gives f/1000.
%
% (3) Each analysis frame should contain at least 2P samples. If note (1) is followed
%     this implies at least 2 ms of speech signal per frame.
%
% (4) It can be advantageous to restrict the analysis regions to time intervals
%     when the glottis is closed (closed-phase analysis). This can be achieved by
%     setting the T input parameter appropriately. If the closed-phase is shorter than
%     2 ms then two or more successive closed-phases should be used by defining 4 or more
%     elements in the corresponding row of T.
%
% (5) A previous version of this routine allowed T() to have a single row which would
%     be replicated for the entire file length. This has been removed because it gave rise
%     to an ambiguity.

%  Bugs: should really detect a singular matrix and reduce the order accordingly

%	   Copyright (C) Mike Brookes 1995
%      Version: $Id: v_lpccovar.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
s = s(:); % make it a column vector
if nargin < 2 p=12; end;
if nargin < 3 t=[p+1 length(s)]; end;
wq = nargin>3;
[nf,ng]=size(t);
if iscell(t)
    t{nf+1}=length(s)+1;
else
    if rem(ng,2)
        t(:,end+1)=[t(2:nf,1)-1; length(s)];
    end
end
ar=zeros(nf,p+1);
ar(:,1)=1;
e=zeros(nf,4);
dc=zeros(nf,1);
d0=nargout >2;
rs=(1:p);
for jf=1:nf
    if iscell(t)
        tj=t{jf};
        if rem(length(tj),2)
            tj(end+1)=t{jf+1}(1)-1;
        end
    else
        tj=t(jf,:);
    end

    ta = ceil(tj(1));
    tb = floor(tj(2));
    cs = (ta:tb).';
    for js=3:2:length(tj)
        ta = ceil(tj(js));
        tb = floor(tj(js+1));
        cs = [cs; (ta:tb).'];
    end
    %disp(cs([logical(1); (cs(2:end-1)~=cs(1:end-2)+1)|(cs(2:end-1)~=cs(3:end)-1); logical(1)])');
    nc = length(cs);
    pp=min(p,nc-d0);
    dm=zeros(nc,pp);	% predefine shape
    dm(:) = s(cs(:,ones(1,pp))-rs(ones(nc,1),1:pp));
    if nargout>2
        if wq
            dm = [ones(nc,1) dm].*w(cs(:,ones(1,1+pp)));
            sc=(s(cs).*w(cs));
            aa = (dm\sc).';
        else
            dm = [ones(nc,1) dm];
            sc=s(cs);
            aa = (dm\sc).';
        end
        ar(jf,2:pp+1) = -aa(2:pp+1);
        e(jf,1)=sc.'*(sc - dm*aa.');
        e(jf,2)=sc.'*sc;
        e(jf,3:4)=e(jf,1:2)/nc;
        dc(jf)=aa(1)/sum(ar(jf,:));
    else
        if wq
            dm = dm.*w(cs(:,ones(1,pp)));
            sc=(s(cs).*w(cs));
            aa = (dm\sc).';
        else
            sc=s(cs);
            aa = (dm\sc).';
        end;
        ar(jf,2:pp+1) = -aa;
        if nargout~=1
            e(jf,1)=real(sc'*(sc - dm*aa.'));
            e(jf,2)=real(sc'*sc);
            e(jf,3:4)=e(jf,1:2)/nc;
        end
    end
end
if ~nargout
    v_lpcar2ff(repmat(sqrt(e(:,3).^(-1)),1,p+1).*ar,255);
    ylabel('Power (dB)');
end


function x = merl_quadprog(varargin)
  % MERL_QUADPROG Optimize problems of the form:
  %
  % min_x 1/2 x' Q x - x' h
  %
  % s.t. x>=0
  %
  % According to the method described in "PARALLEL QUADRATIC PROGRAMMING FOR
  % IMAGE PROCESSING" [Brand & Chen 11]
  %
  % x = merl_quadprog(Q,h,'ParamterName',ParameterValue)
  %
  % Inputs:
  %   Q  n by n symmetric positive semi-definite quadratic coefficients matrix
  %   h  n by 1 linear coefficents vector (note sign)
  % Outputs:
  %   x  n by 1 solution vector
  %
  %

  Q = varargin{1};
  h = varargin{2};

  n = size(Q,1);
  assert(size(Q,2) == n);
  assert(numel(h) == n);
  h = h(:);

  max_iter = inf;
  tol = 1e-7;

  % initial guess
  x0 = ones(n,1);

  v = 3;
  while v <= numel(varargin)
    switch varargin{v}
    case 'MaxIter'
      assert((v+1)<=numel(varargin));
      v = v+1;
      max_iter = varargin{v};
    case 'X0'
      assert((v+1)<=numel(varargin));
      v = v+1;
      x0 = varargin{v};
    otherwise
      error(['Unknown parameter: ' varargin{v}]);
    end
    v = v+1;
  end

  %% "some r ∈ R^n ≥ 0"
  %r = sparse(n,1);
  % ri = max(Qii , ∑ j Q−i j )
  r = max(diag(Q),max(-Q,[],2));

  % "similarly" ... "si>0"
  s = ones(n,1);

  Qp = max(Q,0) + diag(r);
  Qm = max(-Q,0) + diag(r);
  assert(issparse(Qp));
  hp = max(h,0) + s;
  hm = max(-h,0) + s;

  x = x0;
  it = 0;
  while true
    x_prev = x;
    x = x.*(hp+Qm*x)./(hm+Qp*x);
    it = it + 1;
    diff = max(abs(x-x_prev));
    %fprintf('%d: %g\n',it,diff);
    if it >= max_iter
      return;
    end
    if diff<=tol
      return;
    end
  end

end


function T = sldlda(X, nums, varargin)
%SLDLDA Performs Direct Linear Discriminant Analysis
%
% $ Syntax $
%   - T = sldlda(X, nums)
%   - T = sldlda(X, nums, ...)
%
% $ Arguments $
%   - X:        the training sample matrix
%   - nums:     the numbers of samples in all classes
%   - T:        the solved transform matrix
%
% $ Description $
%   - T = sldlda(X, nums) performs direct LDA on the samples X using
%     default settings.
%
%   - T = sldlda(X, nums, ...) performs direct LDA on the samples X
%     with the specified properties.
%     \*
%     \t   Table 1.  The properties of Fisher Discriminant Analysis   \\
%     \h     name     &     description                                \\
%           'pdimset' &  The cell containing the arguments for determining
%                        the range space of Sb. They will be input to
%                        slrangespace for dimension determination.
%           'whiten'  &  The cell containing the arguments for computing
%                        the whitening transform in 2nd stage. They will
%                        input to slwhiten_from_cov.
%                        default = {}.       \\
%           'Sb'      &  The pre-computed between-class scattering matrix
%                        or the cell containing the arguments for
%                        computing the scatter matrix in the form
%                        {type, ...}, which is input to slscatter.     \\
%           'Sw'      &  The pre-computed within-class scattering matrix
%                        or the cell containing the arguments for
%                        computing the scatter matrix in the form
%                        {type, ...}, which is input to slscatter.     \\
%         'weights'   &  The sample weights. default = [].             \\
%     \*
%
% $ Remarks $
%   -# The function solves the transform in mainly following stages:
%      First solves the range space of between-class scattering,
%      projecting all samples onto it. Then, solve the whitening transform
%      of the projected within-class scattering.
%
%   -# If Sb or its computing rule is given, the range space is  directly
%      solved from Sb, otherwise the null space is solved from class
%      centers. If both Sb and Sw are given, then the samples are not
%      used in the function. In this cases, you can simply input an empty X.
%
%   -# If both Sb and Sw are given, the pre-pca step will not be conducted.
%      no matter whether prepca is true or false.
%
% $ History $
%   - Created by Dahua Lin on May 1st, 2006
%

%% parse and verify input arguments

if nargin < 2
    raise_lackinput('slfld', 2);
end

% check size

if ~isempty(X)
    if ndims(X) ~= 2
        error('sltoolbox:invaliddims', ...
            'The sample matrix X should be a 2D matrix');
    end
    [d, n] = size(X);

    k = length(nums);
    if ~isequal(size(nums), [1, k]);
        error('sltoolbox:invaliddims', ...
            'The nums vector should be a row vector');
    end
    if sum(nums) ~= n
        error('sltoolbox:sizmismatch', ...
            'The total number in nums is not consistent with that in X');
    end
end

% check options
opts.pdimset = {};
opts.whiten = {};
opts.Sb = {'Sb'};
opts.Sw = {'Sw'};
opts.weights = [];
opts = slparseprops(opts, varargin{:});

has_Sb = ~isempty(opts.Sb) && isnumeric(opts.Sb);
has_Sw = ~isempty(opts.Sw) && isnumeric(opts.Sw);
if has_Sb && has_Sw
    d = size(opts.Sw, 1);

    if ~isequal(size(opts.Sb), [d, d]) || ~isequal(size(opts.Sw), [d, d])
        error('sltoolbox:sizmismatch', ...
            'Size consistency in Sb and Sw');
    end

else
    if isempty(X)
        error('sltoolbox:invalidargs', ...
            'The samples cannot be empty when Sb or Sw is not pre-computed');
    end
    if (has_Sb && ~isequal(size(opts.Sb), [d, d])) || (has_Sw && ~isequal(size(opts.Sw), [d, d]))
        error('sltoolbox:sizmismatch', ...
            'Size consistency in Sb and Sw');
    end

end
w = opts.weights;

%% Step 1: Compute range space of Sb

if has_Sb
    T1 = slrangespace({'cov', opts.Sb}, opts.pdimset{:});
elseif ~isempty(opts.Sb) && ~isequal(opts.Sb, {'Sb'})
    Sb = slscatter({'cov', X}, opts.Sb{:}, 'sweights', w, 'nums', nums);
    T1 = slrangespace(Sb, opts.pdimset{:});
    clear Sb;
else
    Xc = get_weighted_centers(X, w, nums);
    T1 = slrangespace(Xc, opts.pdimset{:});
    clear Xc wc;
end

%% Step 2: Compute the whiten transform for Sw on range space

if has_Sw
    PSw = T1' * opts.Sw * T1;
else
    X = T1' * X;
    PSw = slscatter(X, opts.Sw{:}, 'sweights', w, 'nums', nums);
end
T2 = slwhiten_from_cov(PSw, opts.whiten{:});
T2 = flipdim(T2, 2);

%% Integrate the transforms

T = T1 * T2;

%% The function for computing weighted centers
function [Xc, wc] = get_weighted_centers(X, w, nums)

Xc = slmeans(X, w, nums);
if isempty(w)
    wc = nums;
else
    k = length(nums);
    [sp, ep] = slnums2bounds(nums);
    wc = zeros(1, k);
    for i = 1 : k
        wc(i) = sum(w(sp(i):ep(i)));
    end
end
wc = sqrt(max(wc, 0));
Xc = slmulvec(Xc, wc, 2);


classdef Triangle_Linear < Interpolation

    methods (Access = public)

        function obj = Triangle_Linear(cParams)
            obj.init(cParams);
            obj.computeParams();
            obj.computeCases();
        end

        function computeShapeDeriv(obj,xGauss)
            obj.computeShapes(xGauss);
            obj.computeShapeDerivatives(xGauss);
        end

    end

    methods (Access = private)

        function computeParams(obj)
            obj.ndime = 2;
            obj.nnode = 3;
            obj.pos_nodes = [0 0; 1 0; 0 1];
            obj.isoDv = 0.5;
            obj.main_loop = [3 3];
            obj.extra_cases = [];
        end

        function computeShapes(obj,posgp)
            ngaus = size(posgp,2);
            nelem = size(posgp,3);
            s = posgp(1,:,:);
            t = posgp(2,:,:);
            I = ones(size(t));
            obj.shape = zeros(obj.nnode,ngaus,nelem);
            obj.shape(1,:,:) = I-s-t;
            obj.shape(2,:,:) = s;
            obj.shape(3,:,:) = t;
        end

        function computeShapeDerivatives(obj,posgp)
            ngaus = size(posgp,2);
            nelem = size(posgp,3);
            obj.deriv = zeros(obj.ndime,obj.nnode,ngaus,nelem);
            obj.deriv(1,1,:,:) = -1;
            obj.deriv(1,2,:,:) = 1;
            obj.deriv(1,3,:,:) = 0;
            obj.deriv(2,1,:,:) = -1;
            obj.deriv(2,2,:,:) = 0;
            obj.deriv(2,3,:,:) = 1;
        end

        function computeCases(obj)
            obj.iteration = [1 2 3;
                             2 3 1];
            obj.cases(:,:,1) = [1 4 5;
                4 2 3;
                5 4 3];
            obj.cases(:,:,2) = [1 4 3;
                4 2 5;
                4 5 3];
            obj.cases(:,:,3) = [1 4 5;
                1 2 4;
                5 4 3];
            obj.selectcases =  [1     0;
                2     0;
                3     3;
                0     2;
                0     1];
        end

    end

end


% NMFLAB for Signal Processing written by A. Cichocki and R. Zdunek
% in cooperation with other members of Laboratory for Advanced Brain Signal
% Processing, BSI, RIKEN, Saitama, JAPAN

function [A,X,Distance_output]=nmf_cascade(Y,r,Index_norm,A_true,S,mc_on, NoAlts,restart_mc_on,AL,Y_true,type_alg,max_restart,no_iter, alphaA1, alphaA2, alphaX, alphaSa, alphaS, alpha)
%
%
% Non-negative Matrix Factorization (NMF) with the cascade implementation
% of some NMF algorithms
%
% [A,X]=nmf_cascade(Y, r, Index_norm, A_true, S, mc_on, NoAlts, restart_mc_on, AL, Y_true, type_alg, max_restart, no_iter,
%       alphaA1, alphaA2, alphaX, alphaSa, alphaS, alpha)
%       returns mixing matrix A of dimension [m by r],
%       and source matrix X of dimension [r by T], for the linear mixing model: AX = Y,
%       where Y is an observation matrix [m by T].
% Note: > m: number of sensors,
%       > r: number of sources,
%       > T: number of samples,
%
% INPUTS:
%       > Index_norm:    vector of 13 binary entries indicating which number
%                        divergence measures are turned on in the View Options,
%
%       > A_true:        true mixing matrix (only for synthetic data),
%       > S:             true source matrix (only for synthetic data),
%       > mc_on:         1 - Monte Carlo analysis enabled, 0 - otherwise,
%       > NoAlts:        number of alternating steps (only for Monte Carlo
%                        analysis and the option "Fixed Alternatings" is selected)
%       > restart_mc_on: 1 - restarts in Monte Carlo analysis are enabled,
%                        0 - otherwise,
%       > AL:            mixing matrix estimaed from the preceeding layer,
%       > Y_true:        the first layer mixed signals (mixtures),
%       > type_alg:      indicates the selected algorithm,
%       > max_restart:   number of restarts,
%       > no_iter:       number of inner iterations,
%       > alphaA1:       regularization parameter for computation of
%                        the first mixing matrix,
%       > alphaA2:       regularization parameter for computation of
%                        the second mixing matrix,
%       > alphaX:        regularization parameter for computation of
%                        the sources,
%       > alphaSa:       parameter of non-linear projection in computation
%                        of the mixing matrix,
%       > alphaS:        parameter of non-linear projection in computation
%                        of the sources,
%       > alpha:         parameter "alpha" in the Amari alpha-divergence,
%
% OUTPUTS:
%       > A:               estimated mixing matrix,
%       > X:               estimated source matrix,
%       > Distance_output: structures of different divergences measured between "Y" and estimated "AX" versus iterations,
%
%
% #########################################################################
A = [];
X = [];
if (nargin < 19) | isempty(alpha) | max(size(alpha) > 1)
   disp('Incorrect parameter alpha in alpha-divergence');
   return
end
if (nargin < 18) | isempty(alphaS) | max(size(alphaS) > 1)
   disp('Incorrect regularization parameter alphaS');
   return
end
if (nargin < 17) | isempty(alphaSa) | max(size(alphaSa) > 1)
   disp('Incorrect regularization parameter alphaSa');
   return
end
if (nargin < 16) | isempty(alphaX) | max(size(alphaX) > 1)
   disp('Incorrect regularization parameter alphaX');
   return
end
if (nargin < 15) | isempty(alphaA2) | max(size(alphaA2) > 1)
   disp('Incorrect regularization parameter alphaA2');
   return
end
if (nargin < 14) | isempty(alphaA1) | max(size(alphaA1) > 1)
   disp('Incorrect regularization parameter alphaA1');
   return
end
if (nargin < 13) | isempty(no_iter) | (no_iter < 1) | max(size(no_iter) > 1)
   disp('Incorrect number of iterations in the EMML algorithm');
   return
end
if (nargin < 12) | isempty(max_restart)  | (max_restart < 0) | max(size(max_restart) > 1)
   disp('Number of restarts must be given correctly');
   return
end
if (nargin < 11) | isempty(type_alg) | (type_alg < 0) | max(size(type_alg) > 1)
   disp('Unknown algorithm');
   return
end
if (nargin < 10) | isempty(Y_true)
   disp('The first layer mixed signals are unknown');
   Y_true = zeros(size(Y_true));
end
if (nargin < 9) | isempty(AL)
   disp('Mixing matrix from the preceeding layer unknown');
   AL = eye(size(Y,1));
end
if (nargin < 8) | isempty(restart_mc_on) | max(size(restart_mc_on) > 1)
   disp('Index od restarts in MC analysis unknown');
   restart_mc_on = 0;
end
if (nargin < 7) | isempty(NoAlts) | max(size(NoAlts) > 1)
   disp('Adjustable number of alternatings');
   NoAlts = [];
end
if (nargin < 6) | isempty(mc_on) | max(size(mc_on) > 1)
   disp('No Monte Carlo Analysis');
   mc_on = 0;
end
if (nargin < 5) | isempty(S)
   disp('X_true not given');
end
if (nargin < 4) | isempty(A_true)
   disp('A_true not given');
   index_fixed_A = 1;
else
   index_fixed_A = 0;
end
if (nargin < 3) | isempty(Index_norm)
   '"Index_norm" must be specified'
   return
end
if (nargin < 2) | isempty(r)
   'Rank of factorization must be given'
   return
end
if isempty(Y) | isnan(Y)
   error('No data');
   return
end
% test for negative values in Y
if min(min(Y)) < 0
    disp('Some matrix entries are changed from negative to small positive');
    Y(Y< 0) = eps;
end
if min(sum(Y,2)) == 0
    disp('Not all entries in a row can be zero');
    return
end

Y = Y + eps;
%Y = Y*Y';

if (alpha == 0) & (type_alg == 3)
    type_alg = 4;
end

[m,T]=size(Y);
niter_selected = 1000;     % maximum number of iterations for the selected sample (can be adjusted)
niter_sample = 30; % maximum number of iterations for each random sample
epsil_normA = 1E-12; % tolerance for alternating

% Monte Carlo and alternatings adjustment
if mc_on & ~restart_mc_on
    max_restart = 0;
end
if ~isempty(NoAlts)
    niter_selected = NoAlts;
end

% Declaration for A and X
A=zeros(m,r);
Ap = zeros(m);
X=zeros(r,T);
Ainit = A;
Xinit = X;
Z = zeros(m,T);
KL_outer_temp = 0;
Z_outer = 0;
nr = 0; restart_on = 0; norm_A = 10; nr_best = -1;
m_sx = 1:m; r_sx = 1:r; T_sx = 1:T; s_dist = 0;

while (nr <= max_restart)

   % Initialize random A and X
       if ~nr & (~mc_on | restart_mc_on)
          A1_init(m_sx',m_sx) = abs(repmat(.1*sin(2*pi*.1*m_sx'),1,m) + repmat(.1*cos(2*pi*.1*m_sx),m,1) + repmat(cos(2*pi*.471*m_sx'),1,m) + repmat(sin(2*pi*.471*m_sx),m,1));
          A1_init = A1_init/max(max(A1_init));

          A2_init(m_sx',r_sx) = abs(repmat(.1*sin(2*pi*.1*m_sx'),1,r) + repmat(.1*cos(2*pi*.1*r_sx),m,1) + repmat(cos(2*pi*.471*m_sx'),1,r) + repmat(sin(2*pi*.471*r_sx),m,1));
          A2_init = A2_init/max(max(A2_init));

          Xinit(r_sx',T_sx) = abs(repmat(.1*sin(2*pi*.1*r_sx'),1,T) + repmat(.1*cos(2*pi*.1*T_sx),r,1) + repmat(cos(2*pi*.471*r_sx'),1,T) + repmat(sin(2*pi*.471*T_sx),r,1));
          Xinit = Xinit/max(max(Xinit));
       else
          A1_init=rand(m,m);
          A2_init=rand(m,r);
          Xinit=rand(r,T);
       end

        % Normalization of initial guess
       A1_init = A1_init*diag(1./sum(A1_init,1));
       A2_init = A2_init*diag(1./sum(A2_init,1));

        if (nr == max_restart)&(max_restart > 0)
           A1 = A1_best;
           A2 = A2_best;
           X = X_best;
        else
           A1 = A1_init;
           A2 = A2_init;
           X = Xinit;
        end % initial guess assignment

    Yx = zeros(m,T);
    n = 0; k = 0;

while ((k <= niter_sample)&(nr < max_restart)) | ((k <= niter_selected)&(nr == max_restart)&(norm_A > epsil_normA)& isempty(NoAlts)) | ((k <= niter_selected)&(nr == max_restart)& (NoAlts > 0))

k = k + 1;

% generalized divergence-reducing NMF iterations (main algorithm)
        if no_iter == 1

            if type_alg == 1 % KL

                Ap = A1;

                Nom = (Y./(A1*A2*X + eps))*(A2*X)';
                Nom(Nom <= 0) = eps;
                A1 = (A1.*(Nom./( repmat((A2*X*ones(T,1))',m,1) + eps))).^(1 + alphaSa);
                A1 = A1*diag(1./sum(A1,1));

                Nom = A1'*(Y./(A1*A2*X + eps))*X';
                Nom(Nom <= 0) = eps;
                A2 = (A2.*(Nom./(A1'*ones(m,1)*ones(1,T)*X' + eps))).^(1 + alphaSa);
                A2 = A2*diag(1./sum(A2,1));

                Nom = (A1*A2)'*(Y./(A1*A2*X + eps));
                Nom(Nom <= 0) = eps;
                X = (X.*(Nom./(repmat((A1*A2)'*ones(m,1),1,T) + eps))).^(1 + alphaS);

            elseif type_alg == 2 % Frobenius

                Ap = A1;
                Nom = Y*(A2*X)' - alphaA1;
                Nom(Nom <= 0) = eps;
                A1 = A1.*(Nom./(A1*A2*X*(A2*X)' + eps));
                A1 = A1*diag(1./sum(A1,1));

                Nom = A1'*Y*X' - alphaA2;
                Nom(Nom <= 0) = eps;
                A2 = A2.*(Nom./(A1'*A1*A2*X*X' + eps));
                A2 = A2*diag(1./sum(A2,1));

                Nom = (A1*A2)'*Y - alphaX;
                Nom(Nom <= 0) = eps;
                X = X.*(Nom./((A1*A2)'*A1*A2*X + eps));

            elseif type_alg == 3 % Amari alpha-divergence

                Ap = A1;
                Nom = ((Y./(A1*A2*X + eps)).^alpha )*(A2*X)';
                Nom(Nom <= 0) = eps;
                A1 = (A1.*(Nom./( repmat((A2*X*ones(T,1))',m,1) + eps)).^(1/alpha) ).^(1 + alphaSa);
                A1 = A1*diag(1./sum(A1,1));

                Nom = A1'*((Y./(A1*A2*X + eps)).^alpha )*X';
                Nom(Nom <= 0) = eps;
                A2 = (A2.*(Nom./(A1'*ones(m,1)*ones(1,T)*X' + eps)).^(1/alpha) ).^(1 + alphaSa);
                A2 = A2*diag(1./sum(A2,1));

                Nom = (A1*A2)'*(Y./(A1*A2*X + eps)).^alpha;
                Nom(Nom <= 0) = eps;
                X = (X.*(Nom./(repmat((A1*A2)'*ones(m,1),1,T) + eps)).^(1/alpha) ).^(1 + alphaS);

            elseif type_alg == 4 % SMART

                Ap = A1;
                Nom = ( log(Y./(A1*A2*X + eps) +eps) )*(A2*X)';
                A1 = (A1.*exp( Nom* diag(1./(sum( A2*X,2) +eps) ) ) ).^(1 + alphaSa);
                A1 = A1*diag(1./(sum(A1,1) + eps));

                Nom = A1'*( log(Y./(A1*A2*X + eps) + eps) )*X';
                A2 = (A2.*exp( diag(1./(sum( A1,1) + eps) )*Nom*diag(1./(sum( X,2) + eps) ) ) ).^(1 + alphaSa);
                A2 = A2*diag(1./(sum(A2,1) + eps));

                Nom = (A1*A2)'*log(Y./(A1*A2*X + eps) + eps);
                X = (X.*exp(diag(1./(sum( A1*A2,1) + eps) )*Nom ) ).^(1 + alphaS);

            end % type_alg

        else

          if type_alg == 1 % KL
                   Ap = A1;
               for t = 1:no_iter
                   Nom = (Y./(A1*A2*X + eps))*(A2*X)' - alphaA1;
                   Nom(Nom <= 0) = eps;
                   A1 = (A1.*(Nom./( repmat((A2*X*ones(T,1))',m,1) + eps))).^(1 + alphaSa);
                   A1 = A1*diag(1./sum(A1,1));

               end
               for t = 1:no_iter
                    Nom = A1'*(Y./(A1*A2*X + eps))*X' - alphaA2;
                    Nom(Nom <= 0) = eps;
                    A2 = (A2.*(Nom./(A1'*ones(m,1)*ones(1,T)*X' + eps))).^(1 + alphaSa);
                    A2 = A2*diag(1./sum(A2,1));
               end
               for t = 1:no_iter
                   Nom = (A1*A2)'*(Y./(A1*A2*X + eps)) - alphaX;
                   Nom(Nom <= 0) = eps;
                   X = (X.*(Nom./(repmat((A1*A2)'*ones(m,1),1,T) + eps))).^(1 + alphaS);
               end

          elseif type_alg == 2 % Frobenius
               Ap = A1;
               for t = 1:no_iter
                   Nom = Y*(A2*X)' - alphaA1;
                   Nom(Nom <= 0) = eps;
                   A1 = (A1.*(Nom./(A1*A2*X*(A2*X)' + eps))).^(1 + alphaSa);
                   A1 = A1*diag(1./sum(A1,1));
               end
               for t = 1:no_iter
                     Nom = A1'*Y*X' - alphaA2;
                     Nom(Nom <= 0) = eps;
                     A2 = (A2.*(Nom./(A1'*A1*A2*X*X' + eps))).^(1 + alphaSa);
                     A2 = A2*diag(1./sum(A2,1));
               end
               for t = 1:no_iter
                    Nom = (A1*A2)'*Y - alphaX;
                    Nom(Nom <= 0) = eps;
                    X = (X.*(Nom./((A1*A2)'*A1*A2*X + eps))).^(1 + alphaS);
               end

          elseif type_alg == 3 % Amari alpha-divergence
               Ap = A1;
               for t = 1:no_iter
                    Nom = ((Y./(A1*A2*X + eps)).^alpha )*(A2*X)';
                    Nom(Nom <= 0) = eps;
                    A1 = (A1.*(Nom./( repmat((A2*X*ones(T,1))',m,1) + eps)).^(1/alpha) ).^(1 + alphaSa);
                    A1 = A1*diag(1./sum(A1,1));
               end
               for t = 1:no_iter
                     Nom = A1'*((Y./(A1*A2*X + eps)).^alpha )*X';
                     Nom(Nom <= 0) = eps;
                     A2 = (A2.*(Nom./(A1'*ones(m,1)*ones(1,T)*X' + eps)).^(1/alpha) ).^(1 + alphaSa);
                     A2 = A2*diag(1./sum(A2,1));
               end
               for t = 1:no_iter
                     Nom = (A1*A2)'*(Y./(A1*A2*X + eps)).^alpha;
                     Nom(Nom <= 0) = eps;
                     X = (X.*(Nom./(repmat((A1*A2)'*ones(m,1),1,T) + eps)).^(1/alpha) ).^(1 + alphaS);
               end

            elseif type_alg == 4 % SMART
               Ap = A1;
               for t = 1:no_iter
                    Nom = ( log(Y./(A1*A2*X + eps) +eps) )*(A2*X)';
                    Nom(Nom <= 0) = eps;
                    A1 = (A1.*exp( Nom* diag(1./(sum( A2*X,2) +eps) ) ) ).^(1 + alphaSa);
                    A1 = A1*diag(1./(sum(A1,1) + eps));
               end
               for t = 1:no_iter
                    Nom = A1'*( log(Y./(A1*A2*X + eps) + eps) )*X';
                    Nom(Nom <= 0) = eps;
                    A2 = (A2.*exp( diag(1./(sum( A1,1) + eps) )*Nom*diag(1./(sum( X,2) + eps) ) ) ).^(1 + alphaSa);
                    A2 = A2*diag(1./(sum(A2,1) + eps));
               end
               for t = 1:no_iter
                    Nom = (A1*A2)'*log(Y./(A1*A2*X + eps) + eps);
                    Nom(Nom <= 0) = eps;
                    X = (X.*exp(diag(1./(sum( A1*A2,1) + eps) )*Nom ) ).^(1 + alphaS);
               end

          end % type_alg

        end % if no_iter

                if (nr == max_restart)&(mod(k,50)==0)& (~mc_on | restart_mc_on)
                    norm_A = norm(abs(A1 - Ap),'fro');
                    fprintf(1, 'Restart %d,  %d-th alternating step\n',nr_best+1,k);
                end

                if sum(Index_norm)
                   if (nr == max_restart) & (((k < 50) & (mod(k,5)==0)) | ((k>49) & ((mod(k,50)==0))))

                       s_dist = s_dist + 1;
                       k_select(s_dist) = k;
                       Z = A1*A2*X + eps;
                       Z = diag(1./(sqrt(var(Z')) + eps))*Z;

                        dist_Fro(s_dist) = norm(Y - Z,'fro');
                        dist_KL(s_dist) = sum(sum(Y.*log(Y./Z + eps) - Y + Z));
                        dist_KL2(s_dist) = sum(sum(Z.*log(Z./Y + eps) + Y - Z));
                        dist_Pearson(s_dist) = sum(sum( ((Y - Z).^2)./Z ));
                        dist_Hellinger(s_dist) = sum(sum( (sqrt(Z) - sqrt(Y)).^2 ));
                        dist_JS_rel(s_dist) = sum(sum(2*Y.*log(2*Y./(Y + Z) + eps) + Z - Y));
                        dist_JS_rel2(s_dist) = sum(sum(2*Z.*log(2*Z./(Y + Z) + eps) - Z + Y));
                        Zy = Y + Z;
                        dist_JS(s_dist) = sum(sum(Y.*log(2*Y./Zy + eps) + Z.*log(2*Z./Zy + eps) ));
                        dist_AG_rel(s_dist) = sum(sum(Zy.*log(.5*Zy./Y + eps) + Y - Z));
                        dist_AG(s_dist) = sum(sum(.5*Zy.*log(.5*Zy./sqrt(Y.*Z) + eps)));
                        dist_J(s_dist) = sum(sum( .5*(Y - Z).*log(Y./Z + eps) ));
                        dist_Chi(s_dist) = sum(sum( ((Y + Z).*(Y - Z).^2)./(Y.*Z) ));
                        dist_Tria(s_dist) = sum(sum( ((Y - Z).^2)./(Y + Z) ));
                    end % if multiple
                end % if sum

end % while (k)

% Outer KL divergence
Z = AL*A1*A2*X;
Z_outer = norm(Z,'fro') + eps;
KL_outer = sum(sum(Y_true.*log((Y_true + eps)./(Z + eps)) - Y_true + Z))/Z_outer;

          if (nr == 0) | (KL_outer < KL_outer_temp)
              A1_best = A1; A2_best = A2; X_best = X; KL_outer_temp = KL_outer; nr_best = nr;
           end % multi-conditions

   nr = nr + 1;

   if nr <=max_restart
      fprintf(1, 'Restart %d, Kullback-Leibler divergence = %e\n',	nr, KL_outer);
   end

end % while (restarts)

% One-Variance scaling
X(X <= 0) = eps;
A = A1*A2;

Distance_output = cell(length(s_dist),1);
Distance_output(1) = {[]};
Distance_output(2) = {[]};
Distance_output(3) = {[]};
Distance_output(4) = {[]};
Distance_output(5) = {[]};
Distance_output(6) = {[]};
Distance_output(7) = {[]};
Distance_output(8) = {[]};
Distance_output(9) = {[]};
Distance_output(10) = {[]};
Distance_output(11) = {[]};
Distance_output(12) = {[]};
Distance_output(13) = {[]};
Distance_output(14) = {[]};

if sum(Index_norm)
   Distance_output(1) = {k_select};
   Distance_output(2) = {dist_Fro};
   Distance_output(3) = {dist_KL};
   Distance_output(4) = {dist_KL2};
   Distance_output(5) = {dist_Pearson};
   Distance_output(6) = {dist_Hellinger};
   Distance_output(7) = {dist_JS_rel};
   Distance_output(8) = {dist_JS_rel2};
   Distance_output(9) = {dist_JS};
   Distance_output(10) = {dist_AG_rel};
   Distance_output(11) = {dist_AG};
   Distance_output(12) = {dist_J};
   Distance_output(13) = {dist_Chi};
   Distance_output(14) = {dist_Tria};
end


function Mh = oprBuilderHmx(opr,k,Xunk,Mx,X,Xnrm,Yunk,My,Y,Ynrm,tol)
%+========================================================================+
%|                                                                        |
%|            OPENOPR - LIBRARY FOR SPECIFIC OPERATORS IN BEM             |
%|           openOpr is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2019.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : oprBuilderHmx.m                               |
%|    #    |   VERSION    : 0.61                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 05.09.2018                                    |
%| ( === ) |   SYNOPSIS   : Finite element builder with low-rank          |
%|  `---'  |                approximation for handle function             |
%+========================================================================+

% Initialize H-Matrix
Mh = hmx(Xunk,Yunk,tol);

% Admissibility
[isfar,Xdim,Ydim] = hmxFar(Mh);

% Compression for far distances
if isfar
    % ACA compression for quadrature matrix
    if strcmp(opr,'nxK')
        [A,B,flag] = oprACAv(opr,k,X,Xnrm,Y,Ynrm,tol);
    else
        [A,B,flag] = oprACA(opr,k,X,Xnrm,Y,Ynrm,tol);
    end

    % Finite element integration
    if flag
        % Integration
        if strcmp(opr,'H') || strcmp(opr,'T')
            for n = 1:length(Mx)
                Mx{n} = Mx{n} * A;
                My{n} = B * My{n};
            end
            A = cell2mat(Mx);
            B = cell2mat(My');
        elseif strcmp(opr,'nxK')
            A = [ Mx{1}*A(:,:,2) , - Mx{1}*A(:,:,3) , ...
                Mx{2}*A(:,:,3) , - Mx{2}*A(:,:,1) , ...
                Mx{3}*A(:,:,1) , - Mx{3}*A(:,:,2) ] ;
            B = [ B(:,:,2)*My{3} ; B(:,:,3)*My{2} ; ...
                B(:,:,3)*My{1} ; B(:,:,1)*My{3} ; ...
                B(:,:,1)*My{2} ; B(:,:,2)*My{1} ] ;
        else
            A = Mx * A;
            B = B * My;
        end

        % Recompression
        [A,B] = hmxQRSVD(A,B,tol);
    end

else
    flag = 0;
end

%%% Compression
if flag
    % Type
    Mh.typ = 1;

    % Low-rank
    Mh.dat = {A,B};

%%%% Full or sparse for smallest box (stopping criterion)
elseif (min(size(Mh)) < 100)
    % Type
    Mh.typ = 2;

    % Quadrature matrix
    if strcmp(opr,'nxK')
        Gxy = zeros(size(X,1),size(Y,1),3);
        for j = 1:size(Gxy,2)
            Gxy(:,j,:) = oprGreenKernel(opr,k,X,Xnrm,Y(j,:),Ynrm(j,:));
        end
    else
        Gxy = zeros(size(X,1),size(Y,1));
        for j = 1:size(Gxy,2)
            Gxy(:,j) = oprGreenKernel(opr,k,X,Xnrm,Y(j,:),Ynrm(j,:));
        end
    end

    % Finite element integration
    if strcmp(opr,'H') || strcmp(opr,'T')
        Mh.dat = zeros(size(Mh));
        for n = 1:length(Mx)
            Mh.dat = Mh.dat + Mx{n} * Gxy * My{n};
        end
    elseif strcmp(opr,'nxK')
        Mh.dat = Mx{1} * Gxy(:,:,2) * My{3} - Mx{1} * Gxy(:,:,3) * My{2} + ...
            Mx{2} * Gxy(:,:,3) * My{1} - Mx{2} * Gxy(:,:,1) * My{3} + ...
            Mx{3} * Gxy(:,:,1) * My{2} - Mx{3} * Gxy(:,:,2) * My{1} ;
    else
        Mh.dat = Mx * Gxy * My;
    end

%%% H-Matrix (recursion)
else
    % Type
    Mh.typ = 0;

    % Subdivision for X
    [I1,I2] = hmxSubdivide(Xunk,Xdim);
    Mh.row  = {I1 , I1 , I2 , I2};

    % Subdivision for Y
    [I1,I2] = hmxSubdivide(Yunk,Ydim);
    Mh.col  = {I1 , I2 , I1 , I2};

    % H-Matrix (recursion)
    for i = 1:4
        % Dof indices
        Ir = Mh.row{i};
        Ic = Mh.col{i};

        % Fem matrix subdivision and quadratures points indices
        [Mxchd,Ix] = oprSubdivideCell(Mx,Ir,'left');
        [Mychd,Iy] = oprSubdivideCell(My,Ic,'right');

        % Recursion
        Mh.chd{i} = oprBuilderHmx(opr,k,Xunk(Ir,:),Mxchd,X(Ix,:),Xnrm(Ix,:),...
            Yunk(Ic,:),Mychd,Y(Iy,:),Ynrm(Iy,:),tol);
    end

    % Fusion
    Mh = hmxFusion(Mh);
end
end

%         % Add H-Vector
%         Mv(Ix) = Mv(Ix) + tmp
%
%
% % Corrective Matrix-vector product for Stokes Stresslet
% if strcmp(opr,('Tc'))
%     % H-Matrix (recursion)
%     if (Mh.typ == 0)
%
%     % Compressed leaf
%     elseif (Mh.typ == 1)
%          TODO
% %         Mv = Mx\(A*sum(B,2));
%
%     % Full leaf
%     elseif (Mh.typ == 2)
%         Mv = sum(Gxy*My,2);
%     end
% end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   PARAMETERS Returns a data structure containing the parameters of the
%   IRB_4600.
%
%   Author: Arturo Gil. Universidad Miguel Hern�ndez de Elche.
%   email: arturo.gil@umh.es date:   09/01/2012
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

robot.name= 'abb_IRB_4600';

%Path where everything is stored for this robot
robot.path = 'robots/abb/IRB_4600';

robot.DH.theta= '[q(1) q(2)-pi/2 q(3) q(4) q(5) q(6)+pi]';
robot.DH.d='[0.495 0 0 0.960 0 0.135]';
robot.DH.a='[0.175 0.9 0.175 0 0 0]';
robot.DH.alpha= '[-pi/2 0 -pi/2 pi/2 -pi/2 0]';
robot.J=[];

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

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

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

%minimum and maximum rotation angle in rad
robot.maxangle =[deg2rad(-180) deg2rad(180); %Axis 1, minimum, maximum
                deg2rad(-90) deg2rad(150); %Axis 2, minimum, maximum
                deg2rad(-180) deg2rad(75); %Axis 3
                deg2rad(-400) deg2rad(400); %Axis 4: Unlimited (400� default)
                deg2rad(-125) deg2rad(120); %Axis 5
                deg2rad(-400) deg2rad(400)]; %Axis 6: Unlimited (800� default)

%maximum absolute speed of each joint rad/s or m/s
robot.velmax = [deg2rad(200); %Axis 1, rad/s
                deg2rad(200); %Axis 2, rad/s
                deg2rad(260); %Axis 3, rad/s
                deg2rad(360); %Axis 4, rad/s
                deg2rad(360); %Axis 5, rad/s
                deg2rad(450)];%Axis 6, rad/s

robot.accelmax=robot.velmax/0.1; % 0.1 is here an acceleration time

            % end effectors maximum velocity
robot.linear_velmax = 2.5; %m/s

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

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

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

%DYNAMICS
robot.has_dynamics=0;


function getVoxelHull(obj, voxelSize, varargin)
% GETVOXELHULL Compute the voxel hull of a point cloud.
% ------------------------------------------------------------------------------
% DESCRIPTION/NOTES
% * The voxel hull is a low resolution representation of the volume occupied by
%   a point cloud. For the computation of the voxel hull the object space is
%   subdivided into a voxel structure. The voxel hull of the point cloud
%   consists of all voxels which contain at least one point of the point cloud.
%
% * Only active points are considered for the computation of the voxel hull.
%
% * The voxel hull can be used to select points, which are overlapping with
%   another point cloud. An example for this task is given in the documentation
%   of the 'select' method. For this call 'help pointCloud.select' and see the
%   examples for the 'InVoxelHull' selection strategy.
% ------------------------------------------------------------------------------
% INPUT
% 1 [voxelSize]
%   Voxel size (equal to edge length) of a single voxel.
% ------------------------------------------------------------------------------
% OUTPUT
% 1 [obj]
%   The voxel hull is attached to the following properties of the ouput object:
%   * obj.voxelHull          = n-by-3 matrix containing the x, y, z coordinates
%                              of the centers of the voxels
%   * obj.voxelHullVoxelSize = edge length of voxels (given by parameter
%                              voxelSize)
% ------------------------------------------------------------------------------
% EXAMPLES
% 1 Import a point cloud and compute its voxel hull.
%   pc = pointCloud('Lion.xyz');
%   pc.getVoxelHull(5); % voxel size is 5mm
%   % The voxel hull can now be found in pc.voxelHull and pc.voxelHullVoxelSize
% ------------------------------------------------------------------------------
% philipp.glira@gmail.com
% ------------------------------------------------------------------------------

% Input parsing ----------------------------------------------------------------

p = inputParser;
p.addRequired('voxelSize', @(x) isscalar(x) && x>0);
% Undocumented
p.addParameter('Centroids', false, @islogical);
p.parse(voxelSize, varargin{:});
p = p.Results;
% Clear required inputs to avoid confusion
clear voxelSize

% Start ------------------------------------------------------------------------

procHierarchy = {'POINTCLOUD' 'GETVOXELHULL'};
msg('S', procHierarchy);
msg('I', procHierarchy, sprintf('Point cloud label = ''%s''', obj.label));

% Compute voxel hull -----------------------------------------------------------

% Lower left point of activated points
lim.min = min(obj.X(obj.act,:), [], 1);
% Round origin (voxel hulls have coincident voxel centers if mod(100, p.voxelSize) == 0)
lim.min = (floor(lim.min/100))*100;

% Indices of voxel cells in x, y and z direction (indices start with 0!)
idxXYZ = [floor( (obj.X(obj.act,1) - lim.min(1)) / p.voxelSize ) ...
          floor( (obj.X(obj.act,2) - lim.min(2)) / p.voxelSize ) ...
          floor( (obj.X(obj.act,3) - lim.min(3)) / p.voxelSize )];

% Remove multiple points to get unique voxels
[voxel, ~, ic] = unique(idxXYZ, 'rows'); % ic for centroids

% Transformation of indices to coordinate system and save to object
obj.voxelHull = [lim.min(1) + p.voxelSize/2 + voxel(:,1) * p.voxelSize ...
                 lim.min(2) + p.voxelSize/2 + voxel(:,2) * p.voxelSize ...
                 lim.min(3) + p.voxelSize/2 + voxel(:,3) * p.voxelSize];

% Round voxel hull (because e.g. -100+0.025+1982*0.05 ~= -0.875)
if p.voxelSize < 1
    noDigits = abs(floor(log10(p.voxelSize)-1)) + 2; % e.g. for 0.05 -> noDigits = 3+2 = 5 (+2 is arbitrary choice to be on the safe side)
    obj.voxelHull = round(obj.voxelHull, noDigits);
end

% Save voxel size to object
obj.voxelHullVoxelSize = p.voxelSize;

% Calculate centroids within each voxel ----------------------------------------

if p.Centroids

    % Add centroid matrix
    obj.voxelHull = [obj.voxelHull zeros(size(voxel,1),3)];

    % Centroid for each voxel
    % Var1
    % tic;
    % for i = 1:size(voxel,1), obj.voxelHull(i,4:6) = mean(obj.X(ic == i,:),1); end
    % toc;
    % c1 = obj.voxelHull(:,4:6);

    % Var2
    % tic;
    % cellX = cell(size(voxel,1),1);
    % for i = 1:size(voxel,1), cellX{i,1} = obj.X(ic == i,:); end
    % cellMean = cellfun(@(x) mean(x,1), cellX, 'UniformOutput', false);
    % obj.voxelHull(:,4:6) = vertcat(cellMean{:});
    % toc;
    % c2 = obj.voxelHull(:,4:6);

    % Var3
    % tic;
    % cellMean = arrayfun(@(x) mean(obj.X(ic == x,:),1), [1:size(voxel,1)]', 'UniformOutput', false);
    % obj.voxelHull(:,4:6) = vertcat(cellMean{:});
    % toc;
    % c3 = obj.voxelHull(:,4:6);

    % Var4
    % tic;
    [~, idx] = sort(ic);
    Xs = obj.X(idx,:);
    rowDist = diff(find([1; diff(ic(idx)); 1]));
    cellX = mat2cell(Xs, rowDist);
    cellMean = cellfun(@(x) mean(x,1), cellX, 'UniformOutput', false);
    obj.voxelHull(:,4:6) = vertcat(cellMean{:});
    % toc;
    % c4 = obj.voxelHull(:,4:6);

end

% End --------------------------------------------------------------------------

msg('E', {'POINTCLOUD' 'GETVOXELHULL'});

end


% Runs a step of non-local Bayes
%
% USAGE: [deno, aggw] = nlbayes_step(nisy, bsic, sigma, prms)
%
%  -> nisy    : noisy image
%  -> bsic    : basic estimate (can be empty for the first step)
%  -> sigma   : noise std. dev.
%  -> prms    : struc with prms (wx, px, np, r)
%
%  <- deno    : denoised image
%  <- aggw    : aggregation weights
function [deno, aggw] = nlbayes_step(nisy, bsic, sigma, prms)

% image size and channels
w = size(nisy,2);
h = size(nisy,1);
chnls = size(nisy,3);

% in the absence of the basic estimate, define basic as the noisy image
if isempty(bsic),
	bsic = nisy;
	step2 = false;
else
	step2 = true
end

% aggregation
aggw = zeros(size(nisy)); % aggregation weights
aggp = zeros(size(nisy)); % mask of already processed patches (for speed-up trick)
aggu = zeros(size(nisy)); % aggregated image
deno = zeros(size(nisy)); % denoised image (deno = aggu ./ aggw);

% step sizes
stepx = floor(prms.px/2);
stepy = floor(prms.px/2);
ii = 0; % iteration counter

% patch dimensionality
pdim = prms.px * prms.px * chnls;

% use dct basis
% U = kron(eye(chnls), dct_basis(prms.px, prms.px));
% use pca basis
U = [];

% use an aggregation window
if prms.pw,
	wx = chebwin(prms.px);
else
	wx = ones(prms.px,1);
end
wwx = repmat(reshape(wx*wx',[prms.px*prms.px 1]),[chnls prms.np]);

% main loop
for pay = [1:stepy:h - prms.px+1,h - prms.px+1],
for pax = [1:stepx:w - prms.px+1,w - prms.px+1], ii = ii + 1;

	% acceleration: skip iteration if patch has been been already denoised
	if aggp(pay,pax,1),
		continue;
	end

	% -------------------------------------------------- compute patch group

	% patches in search region
	srch_region = bsic(max(1,pay - prms.wx):min(h,pay + prms.wx + prms.px - 1),...
	                   max(1,pax - prms.wx):min(w,pax + prms.wx + prms.px - 1),:);

	srch_patches = im2col_ch(srch_region, [prms.px prms.px]);
	refe_patch = bsic(pay:pay+prms.px-1,pax:pax+prms.px-1,:);

	[distances, idx] = sort(L2_distance(refe_patch(:), srch_patches));

	% coordinates of the np nearest neighbors to the ref patch
	idx = idx(1:prms.np)';

	srch_h = size(srch_region,1) - prms.px + 1;
	patches.coords = [max(1,pax - prms.wx) + floor((idx-1)/srch_h),...
	                  max(1,pay - prms.wx) +   mod( idx-1 ,srch_h)];

	% ---------------------------------------------- extract similar patches
	if step2,
		patches.bsic = srch_patches(:,idx);

		% extract noisy patches
		srch_region = nisy(max(1,pay - prms.wx):min(h,pay + prms.wx + prms.px - 1),...
		                   max(1,pax - prms.wx):min(w,pax + prms.wx + prms.px - 1),:);
		patches.nisy = im2col_ch(srch_region, [prms.px prms.px]);
		patches.nisy = patches.nisy(:,idx);
	else
		patches.nisy = srch_patches(:,idx);
	end

	% ----------------------------------------------- compute bayes estimate
	if step2,
		aa = reshape(patches.bsic,[pdim prms.np]);
		[dd,gg] = compute_bayes_estimate(patches.nisy,aa,sigma,prms.r,'pos',U);
	else
		[dd,gg] = compute_bayes_estimate(patches.nisy,[],sigma,prms.r,'pos',U);
	end

	% ------------------------------------------- aggregate patches on image
	gg = ones(size(dd,1),1)*gg;
	aggu = aggregate_patches(aggu, wwx.*gg.*dd, [prms.px prms.px], patches.coords);
	aggw = aggregate_patches(aggw, wwx.*gg    , [prms.px prms.px], patches.coords);

	aggp(patches.coords(:,2) + (patches.coords(:,1)-1)*h) = 1+...
	           aggp(patches.coords(:,2) + (patches.coords(:,1)-1)*h);

	% draw
	if (mod(ii,50) == 1)
		nonzero = find(aggw ~= 0);
		deno(nonzero) = min(255, max(0, aggu(nonzero) ./ aggw(nonzero)));

		% draw a red box indicating limits of search region
		deno(max(1,pay-prms.wx):min(h,pay+prms.wx),max(1,pax-prms.wx),:) = 0;
		deno(max(1,pay-prms.wx):min(h,pay+prms.wx),min(w,pax+prms.wx),:) = 0;
		deno(max(1,pay-prms.wx),max(1,pax-prms.wx):min(w,pax+prms.wx),:) = 0;
		deno(min(h,pay+prms.wx),max(1,pax-prms.wx):min(w,pax+prms.wx),:) = 0;
		deno(max(1,pay-prms.wx):min(h,pay+prms.wx),max(1,pax-prms.wx),1) = 255;
		deno(max(1,pay-prms.wx):min(h,pay+prms.wx),min(w,pax+prms.wx),1) = 255;
		deno(max(1,pay-prms.wx),max(1,pax-prms.wx):min(w,pax+prms.wx),1) = 255;
		deno(min(h,pay+prms.wx),max(1,pax-prms.wx):min(w,pax+prms.wx),1) = 255;

		imagesc(max(min([deno;255-aggw],255),0)/255,[0 1]);
		axis equal, axis off,
		colormap gray
		drawnow
		pause(.01)

		% reset red pixels to 0
		deno(max(1,pay-prms.wx):min(h,pay+prms.wx),max(1,pax-prms.wx),1) = 0;
		deno(max(1,pay-prms.wx):min(h,pay+prms.wx),min(w,pax+prms.wx),1) = 0;
		deno(max(1,pay-prms.wx),max(1,pax-prms.wx):min(w,pax+prms.wx),1) = 0;
		deno(min(h,pay+prms.wx),max(1,pax-prms.wx):min(w,pax+prms.wx),1) = 0;
	end

end
end

% compute denoised image
nonzero = find(aggw ~= 0);
deno(nonzero) = min(255, max(0, aggu(nonzero) ./ aggw(nonzero)));

end