clc 
clear all
close all

addpath('PALLA_BIANCA');
addpath('libsvm-mat-3.0-1');
format long;

%% DATI
n_f = 3;                % numero di features (R G B)
n = n_f + 1;            % dimensione sistema=dim(w)
N = 150;                % numero di frames
Q = [1*eye(n_f) zeros(n_f,1); zeros(1,n_f) 10];           % varianza rumore random-walk
DD = zeros(n,1);
HH = zeros(n);
% si fissano le soglie per allenare il classificatore inizialmente
red  = 15;
green = 15;
blue = 15;

% comandi utilizzati per la creazione del video della sequenza
% --> aviobj1 = avifile('white.avi','fps',10);
% --> aviobj2 = avifile('red.avi','fps',10);



%% PROCEDURA

% Ad ogni ciclo si riceve in ingresso un nuovo frame del video scelto e lo
% si trasforma in una matrice in cui ogni riga è un pixel e ogni colonna è
% l'informazione relativa a ogni pixel (intensità di R, G e B
% rispettivamente)
% Dopodichè si eseguono le solite operazioni di Training iniziale (sul
% primo frame) e di conseguente Classification (sui frame rimanenti).

for t = 1:N
    
    %% Preparazione immagine
    G = imread(sprintf('%05d.jpg',t));
    
    figure(1)
    imshow(G);
    hold on;
    
    % comandi utilizzati per la creazione del video della sequenza
    % --> fig = figure(1);
    % --> F = getframe(fig);
    % --> aviobj1 = addframe(aviobj1,F);
    
    dim = size(G);
    dim_tot = dim(1)*dim(2);            % numero pixel complessivi
    pixel_RGB = zeros(dim_tot,3);       % inizializzazione della matrice dei pixel
    row = 1;                            % contatore di riga (n° pixel)
    for r = 1:dim(1)                    % costruzione matrice pixel
        for c = 1:dim(2)
            for m = 1:3
                pixel_RGB(row,m) = G(r,c,m);
            end
        row = row + 1;
        end
    end

    x = pixel_RGB;         % pixel dell'immagine corrente - campioni correnti
    M_t = dim_tot;         % numero pixel immagine
    f_i = zeros(M_t,1);
    
    % LA PRIMA IMMAGINE NON LA CLASSIFICO XKE' LA USO X IL TRAINING
    % Dal Training si ottiene l'iperpiano ottimo (Beta e Beta_0) iniziale
    % che serve per le successive classificazioni
         
    
    %% FASE 1 - SVM TRAIN + Operazioni eseguite SOLO al 1° CICLO del for
    
    if t == 1
    
    % Si costruiscono le etichette iniziali manualmente da fornire in
    % ingresso al comando 'svmtrain'
    % Nel caso di features = R G B, le etichette vengono create in base a
    % delle soglie: vengono imposte delle soglie di colore in base al
    % colore del soggetto che si vuole inseguire nell'immagine e, in base 
    % al risultato del confronto tra le soglie e i dati relativi ai pixel,
    % viene applicata ad ogni pixel l'etichetta 1 se le soglie sono
    % rispettate (foreground) e -1 se non lo sono (background).
    
    Group = zeros(dim_tot,1);
    for r = 1:dim_tot
        if pixel_RGB(r,1) > red && pixel_RGB(r,2) > green && pixel_RGB(r,3) > blue
            Group(r) = 1;
        else
            Group(r) = -1;
        end
    end
    
    
    % SVM TRAIN
    model = svmtrain(Group,pixel_RGB,'-t 0'); 
    y = Group;
    
    % Creazione iperpiano ottimo a partire dai dati ottenuti dal TRAIN
    beta_0_iniz = -model.rho;
    beta_iniz = zeros(n_f,1);
    N_sv = model.totalSV;
    
    for s = 1:N_sv
        sv_tmp = zeros(n_f,1);
        for q = 1:n_f
            sv_tmp(q) = model.SVs(s,q);
        end
        beta_iniz = beta_iniz + sv_tmp*model.sv_coef(s);
    end
    
    b_ott = [beta_iniz; beta_0_iniz];
    sigma_Beta = var(b_ott) + Q;
    
    % In base all'iperpiano ottenuto si colorano di rosso i pixel
    % dell'immagine etichettati come 1 e si lasciano invariati gli altri
    G_mod = G;          % copia dell'immagine corrente
    a = 0;              % contatore pixel nell'immagine copiata
    
     while a < M_t
        for r = 1:dim(1)
            for c = 1:dim(2)
                a = a + 1;
                if y(a)>0
                    G_mod(r,c,:) = [255 0 0];
                end
            end
        end
     end
     
     
     figure(2)
     imshow(G_mod);
     hold on;
     
   
    %% FASE 2 - Classificazione
    % Nei cicli successivi al primo si esegue la classificazione e si
    % calcola il nuovo iperpiano ottimale in base alla classificazione
    % appena eseguita, attraverso l'ottimizzazione quadratica.
    else
        
        n_lin = 3;
        
        for j = 1:n_lin
        
            for i = 1:M_t
                f_i(i) = b_ott(1:n_f)'*x(i,:)' + b_ott(n);
                if j == 1
                    y(i) = sign(f_i(i));
                end
            end
        
            [D,H] = linearize_new(x(:,:),y,f_i,M_t,n);
            
            if j == 1
                sigma_Beta = (H^-1) + Q;
            else
                sigma_Beta = (H^-1);
            end
        
            var = H + sigma_Beta^-1;
            % se ne considera solo la diagonale (ponendo a 0 le
            % correlazioni)
            var = [var(1,1) 0 0 0; 0 var(2,2) 0 0; 0 0 var(3,3) 0; 0 0 0 var(4,4)];

            b_ott = b_ott - (var^-1)*D;
    
        end
    
        % Group classificazione con beta e beta_0 ottimi precedenti
        % e verifico cosa ho ottenuto dalla classificazione
        G_mod = G;
        a = 0;      % contatore pixel nell'immagine
        
        while a < M_t
            for r = 1:dim(1)
                for c = 1:dim(2)
                    a = a + 1;
                    %f_i(a) = b_ott(1:n_f)'*x(a,:)' + b_ott(n);
                    if y(a)>0
                        G_mod(r,c,:) = [255 0 0];
                    end
                end
            end
        end
     
     figure(2)
     imshow(G_mod);
     hold on;
     
     % comandi utilizzati per la creazione del video della sequenza
     % --> fig = figure(2);
     % --> L = getframe(fig);
     % --> aviobj2 = addframe(aviobj2,L);     

     end
end

% comandi utilizzati per la creazione del video della sequenza (chiusura)
% --> aviobj1 = close(aviobj1);
% --> aviobj2 = close(aviobj2);