
punti_prova_cross;
load('movimento_punti_3D_100frames');
misure=points;
    C = [1 0 0 0 0 0;
         0 0 1 0 0 0;
         0 0 0 0 1 0];

P0 = 1*ones(6,6); %P0 e X0 andranno inizializzate con le precedenti nel caso della sliding window

[~, n_marker, n_istanti]=size(misure);

%pairs initialization:
T2Mpair(:,n_istanti)=zeros(n_marker,1);
% T2Mpair(:,1) = [1,2,3,4,5,6,7,8]';
T2Mpair(:,1) = [1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16]';

% xhat = zeros(6,n); %X0

%inizializzo vettore di tutti gli stati
X=zeros(6,n_marker);
Y=zeros(3,n_marker);
P(:,:,n_marker)=zeros(6,6);
B(:,:,n_marker)=zeros(3,3);


X(1,:) = misure(1,:,1);
X(3,:) = misure(2,:,1);
X(5,:) = misure(3,:,1);

for time=2:n_istanti
    
    xold=X
    Pold=P
    
    %predizione per ogni marker
    for cont=1:n_marker %per ogni marker
        [X(:,cont),P(:,:,cont)] = kalman_step( xold(:,cont), Pold(:,:,cont), misure(:,cont,time) );
        TMP = [X(1,cont);X(3,cont);X(5,cont)]
        Y(:,cont) = TMP
        B(:,:,cont) = C * P(:,:,cont) * C';
    end
    
    % calcola la distanza di ogni marker dalla sua predizione
    dist_table=zeros(n_marker,n_marker);
    female_pref_table=zeros(n_marker,n_marker);
    
    for l = 1:n_marker
        for j = 1:n_marker
            dist_table(l,j) = sqrt( (Y(1,l)-misure(1,j,time))^2 + ...
                                    (Y(2,l)-misure(2,j,time))^2 + ...
                                    (Y(3,l)-misure(3,j,time))^2  );
                                %è simmetrica!
        end
        
        %ordino la riga
        tmp = dist_table(l,:);
        for k = 1:n_marker
            intvar = find(max(tmp(1,:)) == tmp(1,:) , 1);
            female_pref_table(l,k) = intvar;
            tmp(intvar)=-1;
        end
    end

    
    
    
    %calcola da probabilità ogni marker dalla predizione
    % calcola la distanza di ogni marker dalla sua predizione
    prob_table=zeros(n_marker,n_marker);
    male_pref_table=zeros(n_marker,n_marker);
    
    for l = 1:n_marker
        for j = 1:n_marker
            % Gaussian multivariate evaluation       
            prob_table(l,j) = sqrt((2*pi)^6 * det(B(:,:,cont))) * ...
                exp(-0.5* (misure(:,l,time)-Y(:,j))'...
                *  B(:,:,cont) * (misure(:,l,time)-Y(:,j))   );
        end
        
        %ordino la riga
        tmp = prob_table(l,:);
        for k = 1:n_marker
            intvar = find(max(tmp(1,:)) == tmp(1,:) , 1);
            male_pref_table(l,k) = intvar;
            tmp(intvar)=-1;
        end
    end
    
    
    T2Mpair(:,time) = stable_marriage(male_pref_table, female_pref_table);
end
    


    
   



 