% Si usa la funzione fun_sqp precedentemente creata per fare un test su un
% database creato in precedenza.
% La funzione prevede un training effettuato, invece che sulla totalità di
% dati a disposizione, su un sottogruppo di questi, e una successiva
% ottimizzazione dell'iperpiano trovato, analizzando i restanti campioni.
% Si vuole verificare se effettivamente questa soluzione funziona
% correttamente e conduce ad un iperpiano simile a quello che si otterrebbe
% facendo un Training sull'intero insieme di dati a disposizione.
% Con questa funzione l'onere computazionale è nettamente inferiore.


function [] = test_statico(P_r,P_b,y_r,y_b) 

addpath('libsvm-mat-3.0-1');
global pp n_f n x Q n_lin b_ideale
x = -1:10;

disp('Scegliere la procedura che si vuole usare: ')
disp('(1) - SVM Train TOTALE con metodo CLASSICO')
disp('(2) - SVM Train TOTALE con metodo SQP')
disp('(3) - SVM Train ITERATO con metodo SQP')
scelta = input('Metodo scelto: ');
disp(' ')

%% PARTE 1 - SVM Train TOTALE con svmtrain
if scelta == 1

    y_label = [y_r; y_b];
    P = [P_r; P_b];
    % si richiama il metodo svmtrain:
    model_tot = svmtrain(y_label,P,'-t 0');
    
    % calcolo stato iniziale per trovare iperpiano iniziale
    beta_0_iniz = -model_tot.rho;
    beta_iniz = zeros(n_f,1);
    N_sv = model_tot.totalSV;
    sv_tmp = zeros(N_sv,n_f);
    
    % costruzione Support Vector (SV) a partire dal model ottenuto
    for s = 1:N_sv
        for q = 1:n_f
            sv_tmp(s,q) = model_tot.SVs(s,q);
        end
        beta_iniz = beta_iniz + model_tot.sv_coef(s)*sv_tmp(s,:)';
    end
    
    % calcolo iperpiano ottimo
    beta = beta_iniz;
    beta_0 = beta_0_iniz;
    retta = -(beta(1)/beta(2)*x + beta_0/beta(2));
    b_ideale = [beta; beta_0];
    
    % Plot iniziale
    % plot punti in esame (rossi e blu)
    figure(1)
    plot(P_r(:,1),P_r(:,2),'o','MarkerEdgeColor','r','MarkerFaceColor','r');
    hold on
    title('Test su database (statico) - SVM Train TOTALE con metodo CLASSICO','fontsize',12)
    title('Caso dati linearmente separabili')
    plot(P_b(:,1),P_b(:,2),'o','MarkerEdgeColor','b','MarkerFaceColor','b');
    axis([-1 10 -1 10])
    grid on
    pause(pp)
    hold on
    
    % plot SV + iperpiano ottimo
    % plot(sv_tmp(:,1),sv_tmp(:,2),'o','MarkerEdgeColor','g','MarkerFaceColor','g');
    plot(x,retta,'k');
    pause(pp)
    hold off
    
%     dim_group = 0;

    
%% PARTE 2 - SVM Train TOTALE con SQP
elseif scelta == 2

    b_ott = zeros(n,1);
    y_label = [y_r; y_b];
    P = [P_r; P_b];
    dim_tot = length(y_label);
    f_i = zeros(dim_tot,1); 


    for i = 1:dim_tot
        % calcolo f(x) = beta_0 + beta'*x
        f_i(i) = b_ott(1:n_f)'*P(i,:)' + b_ott(n);
    end
    
    % Input da tastiera: scelta numero di linearizzazioni effettuate
    % dall'algoritmo per la ricerca del minimo (b_ott)
    disp('Numero LINEARIZZAZIONI eseguite dall algoritmo SQP')
    n_lin = input ('Numero linearizzazioni: ');
    disp(' ')

    figure(2)
    plot(P_r(:,1),P_r(:,2),'o','MarkerEdgeColor','r','MarkerFaceColor','r');
    hold on
    title('TRAIN SVM Train TOTALE con metodo SQP','fontsize',12)
    plot(P_b(:,1),P_b(:,2),'o','MarkerEdgeColor','b','MarkerFaceColor','b');
    axis([-1 10 -1 10])
    grid on
    pause(pp)
    hold on

    
    for j = 1:n_lin
        
        % nelle linearizzazioni successive alla prima si deve ricalcolare
        % la f(x) calcolandola nel parametro ottimale parziale ottenuto
        % dall'ultima linearizzazione effettuata
        if j > 1
            for i = 1:dim_tot
                f_i(i) = b_ott(1:n_f)'*P(i,:)' + b_ott(n);
            end
        end
        
        % calcolo Gradiente (D) ed Hessiano (H) con la funzione linearize()
        [D,H] = linearize_new(P(:,:),y_label,f_i,dim_tot,n);
        
        sigma_Beta = (H^-1);        % Q = 0
        var = H + sigma_Beta^-1;
        % se ne prende solo la diagonale (ponendo a 0 le correlazioni)
        var = [var(1,1) 0 0; 0 var(2,2) 0; 0 0 var(3,3)];

        b_ott = b_ott - (var^-1)*D;
        
               
    end
    
    % nuovo iperpiano ottimo
    retta_new = -(b_ott(1)/b_ott(2)*x + b_ott(3)/b_ott(2));
    
    % plot iperpiano ottimo calcolato + margini
    plot(x,retta_new,'k');
    pause(pp)
    hold off
    
%     dim_group = 0;

%% PARTE 3 - SVM Train PARZIALE
elseif scelta == 3
    
    % TRAINING INIZIALE con SQP
    % A partire da un dataset, si utilizzano due sottogruppi di campioni dalle
    % etichette 1 e -1 per effettuare un piccolo Training iniziale con il
    % comando svmtrain
    
    disp('Inserire il numero di campioni che si vogliono utilizzare per il training iniziale')
    dati_iniz = input('n° campioni train iniziale: ');
    disp(' ')
    
    % comando utilizzato per creare il video
    % --> aviobj = avifile('trainparz_sqp_20lin_NONSEP.avi','fps',5);

    y_train = [y_r(1:dati_iniz); y_b(1:dati_iniz)];     % etichette ad essi relative
    P_train = [P_r(1:dati_iniz,:); P_b(1:dati_iniz,:)];
    
    % Training realizzato con SQP sul primo gruppo di punti
    % Inizializzazione parametri da utilizzare in seguito
    b_ott = zeros(n,1);
    dim_tot = length(y_train);
    f_i = zeros(dim_tot,1);

    for i = 1:dim_tot
        % calcolo f(x) = beta_0 + beta'*x
        f_i(i) = b_ott(1:n_f)'*P_train(i,:)' + b_ott(n);
    end
    
    % Input da tastiera: scelta numero di linearizzazioni effettuate
    % dall'algoritmo per la ricerca del minimo (b_ott)
    disp('Numero LINEARIZZAZIONI eseguite dall algoritmo SQP')
    n_lin = input ('Numero linearizzazioni: ');
    disp(' ')

    for j = 1:n_lin
        
        % nelle linearizzazioni successive alla prima si deve ricalcolare
        % la f(x) calcolandola nel parametro ottimale parziale ottenuto
        % dall'ultima linearizzazione effettuata
        if j > 1
            for i = 1:dim_tot
                f_i(i) = b_ott(1:n_f)'*P_train(i,:)' + b_ott(n);
            end
        end
        
        % calcolo Gradiente (D) ed Hessiano (H) con la funzione linearize()
        [D,H] = linearize_new(P_train(:,:),y_train,f_i,dim_tot,n);
        
        sigma_Beta = (H^-1) + Q;
        var = H + sigma_Beta^-1;
        % se ne prende solo la diagonale (ponendo a 0 le correlazioni)
        var = [var(1,1) 0 0; 0 var(2,2) 0; 0 0 var(3,3)];
        b_ott = b_ott - (var^-1)*D;
        
    end
    
    % iperpiano ottimo ottenuto dal TRAINING iniziale con SQP
    retta = -(b_ott(1)/b_ott(2)*x + b_ott(3)/b_ott(2));

    % Plot iniziale
    % plot punti in esame (rossi e blu)
    figure(3)
    plot(P_r(1:dati_iniz,1),P_r(1:dati_iniz,2),'o','MarkerEdgeColor','r','MarkerFaceColor','r');
    hold on
    title('SVM Train ITERATO con metodo SQP - Train INIZIALE','fontsize',12)
    plot(P_b(1:dati_iniz,1),P_b(1:dati_iniz,2),'o','MarkerEdgeColor','b','MarkerFaceColor','b');
    axis([-1 10 -1 10])
    grid on
    pause(pp)
    hold on
        
    % plot iperpiano ottimo calcolato + margini
    plot(x,retta,'k');
    pause(pp)
    hold off
    
    
    %% Sistemazione dell'iperpiano ottimo (train con ottimizz quadratica)
    % Ora viene utilizzato l'iperpiano creato con il Training iniziale e
    % vengono considerati i restanti sottogruppi della totalità dei dati per
    % migliorare l'iperpiano ottenuto con il primo training effettuato,
    % sempre utilizzando l'ottimizzazione quadratica
    
    % Analizzo i restanti dati in sottogruppi di dimensioni fissate a piacere
    disp('Inserire il numero di campioni per i training successivi:')
    dim_group = input('n° campioni per training successivi: ');
    disp(' ')
    
    dim_tot = 2*dim_group;
    data_class = length(P_r) - dati_iniz;
    cicli_class = data_class/dim_group;
    
    % questi dati verranno aggiornati di volta in volta ad ogni esecuzione di
    % un ciclo del for
    first = dati_iniz + 1;              % primo dato del gruppo di 20 campioni considerato
    last = first + (dim_group - 1);     % ultimo dato del gruppo di 20 campioni considerato
    
    
    for t = 1:cicli_class
        
        % l'iperpiano ottimo calcolato al ciclo precedente viene di volta
        % in volta modificato e ottimizzato in base ai punti considerati al
        % ciclo presente
        
        if t >= 2
            retta = retta_new;
        end
        
        % plot iperpiano ottimo ottenuto al passo precedente ricolorato in NERO
        figure(t)
        plot(x,retta,'k');
        axis([-1 10 -1 10])
        title('SVM Train ITERATO con metodo SQP - Train successivi','fontsize',12)
        grid on
        hold on
        
        plot(P_r(first:last,1),P_r(first:last,2),'o','MarkerEdgeColor','r','MarkerFaceColor','r');
        plot(P_b(first:last,1),P_b(first:last,2),'o','MarkerEdgeColor','b','MarkerFaceColor','b');
        pause(pp)
        
        
        % Inizializzazione parametri temporanei utilizzati in ogni ciclo
        f_i = zeros(2*dim_group,1);               % f(x) = beta_0 + beta'*x
        P_train_parz = [P_r(first:last,:); P_b(first:last,:)];     % dati relativi al gruppo considerato
        y_train_parz = [y_r(first:last); y_b(first:last)];         % vettore usato per memorizzare le etichette correnti
        
        % ad ogni ciclo viene ricreata f(x) = beta_0 + beta'*x (x = coordinate
        % punti) utilizzando il b_ott ottenuto al ciclo precedente e poi si
        % procede alla nuova ottimizzazione dell'iperpiano
        
        for i = 1:dim_tot
            % calcolo f
            f_i(i) = b_ott(1:n_f)'*P_train_parz(i,:)' + b_ott(n);
        end
        
        
        % Dopo aver eseguito la nuova classificazione, si cerca l'iperpiano
        % ottimo che divide i punti al passo corrente, utilizzando
        % l'ottimizzazione quadratica, attorno allo stato ottimo calcolato al
        % passo precedente, della funzione di costo considerata
        % Si eseguono 20 linearizzazioni della funzione per poter trovare in
        % modo più efficace il minimo di questa (stato ottimo)
        for j = 1:n_lin
            
            % nelle linearizzazioni successive alla prima si deve ricalcolare
            % la f(x) calcolandola nel parametro ottimale parziale ottenuto
            % dall'ultima linearizzazione effettuata
            if j > 1
                for i = 1:dim_tot
                    f_i(i) = b_ott(1:n_f)'*P_train_parz(i,:)' + b_ott(n);
                end
            end
            
            % calcolo Gradiente (D) ed Hessiano (H) con la funzione linearize()
            [D,H] = linearize_new(P_train_parz(:,:),y_train_parz,f_i,dim_tot,n);
            
            sigma_Beta = (H^-1) + Q;
            var = H + sigma_Beta^-1;
            % se ne considera la diagonale (ponendo a 0 le correlazioni)
            var = [var(1,1) 0 0; 0 var(2,2) 0; 0 0 var(3,3)];
            
            b_ott = b_ott - (var^-1)*D;
            
        end
        
        % calcolo del nuovo iperpiano ottimale
        retta_new = -(b_ott(1)/b_ott(2)*x + b_ott(3)/b_ott(2));
        
        % plot iperpiano ottimo calcolato + margini
        plot(x,retta_new,'g');
        pause(pp)
        hold off
        
        % comando utilizzato per la creazione del video
        % -- fig = figure(t);
        % --> F = getframe(fig);
        % --> aviobj = addframe(aviobj,F);

        
        % Si aggiornano le variabili first e last per analizzare i successivi
        % 20 campioni
        first = last + 1;
        last = first + dim_group - 1;
        
        
    end

end
    
% comando utilizzato per la creazione del video (chiude il video .avi)
% --> aviobj = close(aviobj);


%% Comandi utilizzati per valutare le prestazioni dell'algoritmo
% diff_b = abs(b_ideale - b_ott);
% 
% m_ideale = - b_ideale(1)/b_ideale(2);     % coeff angolare dell'iperpiano ideale
% ang_ideale = atan(m_ideale)*45;
% 
% m_test = - b_ott(1)/b_ott(2);
% ang_test = atan(m_test)*45;
% 
% diff_ang = abs(ang_ideale) - abs(ang_test);
% 
% n_train = dim_group;
% 
% figure(5)
% hold on
% bar(n_train,diff_b(2),'k')
% bar(n_train,diff_b(1),'r')
% bar(n_train,diff_b(3))
% grid on
% 
% figure(6)
% hold on
% bar(n_train,abs(diff_ang))
% grid on
