% Questa funzione rappresenta il caso in cui viene applicata
% l'OTTIMIZZAZIONE QUADRATICA. Viene eseguito un training iniziale sui
% punti in esame per ottenere il primo iperpiano ottimo utilizzato poi per
% le successive classificazioni. L'ottimizzazione quadratica effettuata
% consente poi di ottimizzare l'iperpiano nei passi successivi seguendo la
% dinamica dei punti considerati, senza la necessità di effettuare nuovi
% training. UNSUPERVISED.

% INPUT --> dinamica dei punti (complessiva e relativa, punti rossi e punti
% blu) + etichette iniziali ad essi assegnate
% OUTPUT --> stati stimati ad ogni ciclo + distanze minime SV/retta
% (iperpiano ottimale)

function [beta_tot dist_min] = fun_sqp(P,P_r,P_b,y_label) 

global pp N t n_f n N_tot x Q coeff var
addpath('libsvm-mat-3.0-1');
t = 1;

% comando utilizzato per la creazione del video della sequenza
% --> aviobj = avifile('fun_sqp_20lin_001_1000.avi','fps',5);

%% SVM Train
% richiamo il metodo svmtrain:
% INPUT = etichette iniziali, posizioni iniziali
% OUTPUT = modello --> SV, stato iniziale, iperpiano iniziale
model = svmtrain(y_label,P(:,1:2,1),'-t 0');

% calcolo stato iniziale per trovare iperpiano iniziale
beta_0_iniz = -model.rho;
beta_iniz = zeros(n_f,1);
N_sv = model.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.SVs(s,q);
    end
    beta_iniz = beta_iniz + model.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 = [beta; beta_0];

d_min_1 = abs(b(1)*sv_tmp(1,1) + b(2)*sv_tmp(1,2) + b(3))/sqrt(b(1)^2 + b(2)^2);
d_min_2 = abs(b(1)*sv_tmp(2,1) + b(2)*sv_tmp(2,2) + b(3))/sqrt(b(1)^2 + b(2)^2);

% memorizzazione delle distanze minime relative ottenute ad ogni passo
dist_min = [];                      % inizializzazione distanze punto_minimo/retta
dist_min = [dist_min; d_min_1 d_min_2];

% Plot iniziale
% plot punti in esame (rossi e blu)
figure(2)
plot(P_r(:,1,1),P_r(:,2,1),'o','MarkerEdgeColor','r','MarkerFaceColor','r');
hold on
title('Algoritmo a ottimizzazione quadratica','fontsize',12)
plot(P_b(:,1,1),P_b(:,2,1),'o','MarkerEdgeColor','b','MarkerFaceColor','b');
axis([-100 200 -100 200])
grid on
pause(0.2)
hold on

% plot SV + iperpiano ottimo
plot(sv_tmp(:,1),sv_tmp(:,2),'o','MarkerEdgeColor','g','MarkerFaceColor','g');
plot(x,retta,'k');
pause(0.1)
hold off

% comandi utilizzati per la creazione del video della sequenza
% --> fig = figure(t);
% --> F = getframe(fig);
% --> aviobj = addframe(aviobj,F);


%% Dati e inizializzazione variabili (C.I. per la classificazione)
b_ott = [beta_iniz; beta_0_iniz];   % stato iniziale ottimo
beta_tot = [beta; beta_0];          % memorizzazione stati


%% SVM Classify
% Qui inizia la parte di algoritmo Unsupervised in cui ad ogni ciclo
% l'iperpiano ottimo viene ricalcolato tramite ottimizzazione quadratica
% della funzione di costo considerata

disp('Scegliere il numero di linearizzazioni da eseguire nell algoritmo:')
n_lin = input('n° linearizzazioni: ');
disp(' ')
disp('Varianza del rumore: ')
q = input('Varianza di beta: ');
q_0 = input('Varianza di beta_0: ');
Q = [q*eye(n_f) zeros(n_f,1); zeros(1,n_f) q_0];
disp(' ')

for t = 2:N
    
    % l'iperpiano ottimo calcolato al ciclo precedente viene utilizzato per
    % effettuare la classificazione dei nuovi punti e che poi verrà
    % ottimizzato in base a questi
    if t > 2
        retta = retta_new;
    end
    
    % plot iperpiano ottimo ottenuto al passo precedente ricolorato in NERO
    figure(2)
    plot(x,retta,'k');
    axis([-200 300 -200 300])
    title('Algoritmo a ottimizzazione quadratica','fontsize',12)
    grid on
    pause(pp);
    hold on
    
    % Inizializzazione parametri temporanei utilizzati in ogni ciclo
    f_i = zeros(N_tot,1);       % f(x) = beta_0 + beta'*x
    ro = 0;                     % n° pti classificati come rossi (+)
    bl = 0;                     % n° pti classificati come blu (-)
    P_r_tmp = [];               % Posizioni rossi tmp (dopo classificaz.)
    P_b_tmp = [];               % Posizioni blu tmp (dopo classificaz.)
    y_r = zeros(1);             % label pti rossi
    y_b = zeros(1);             % label pti blu
        
    % ad ogni ciclo viene ricreata f(x) = beta_0 + beta'*x (x = coordinate 
    % punti) e le etichette in base alla classificazione dei punti rispetto
    % all'iperpiano calcolato al ciclo precedente
    
    
    for i = 1:N_tot
        % calcolo f
        f_i(i) = b_ott(1:n_f)'*P(i,:,t)' + b_ott(n);
        % in base ad f(i) riassegno le etichette nuove (classificazione)
        y_label(i) = sign(f_i(i));
        % in base al segno di queste etichette si classificano i nuovi
        % punti come rossi (1) o come blu (-1)
        % creo i due vettori con label positive (rosso) e negative(blu)
        % aggiorno P_r_tmp e P_b_tmp posizioni nuove post-classificaz.
        if y_label(i) == 1
            ro = ro + 1;
            y_r(ro) = y_label(i);
            P_r_tmp = [P_r_tmp; P(i,:,t)];
        else
            bl = bl + 1;
            y_b(bl) = y_label(i);
            P_b_tmp = [P_b_tmp; P(i,:,t)];
        end
    end

    
    % plot punti relativi al nuovo frame classificati in base all'iperpiano
    % considerato
    plot(P_r_tmp(:,1),P_r_tmp(:,2),'o','MarkerEdgeColor','r','MarkerFaceColor','r');
    plot(P_b_tmp(:,1),P_b_tmp(:,2),'o','MarkerEdgeColor','b','MarkerFaceColor','b');
    pause(pp)
 
   
    % 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 10 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:N_tot
                f_i(i) = b_ott(1:n_f)'*P(i,:,t)' + b_ott(n);
            end
        end
        
        % calcolo Gradiente (D) ed Hessiano (H) con la funzione linearize()
        [D,H] = linearize_new(P(:,:,t),y_label,f_i,N_tot,n);
      
        if j == 1
            sigma_Beta = (H^-1) + Q;
        else
            sigma_Beta = (H^-1);
        end
        
        var = H + sigma_Beta^-1;
        var = [var(1,1) 0 0; 0 var(2,2) 0; 0 0 var(3,3)];
        b_ott = b_ott - (var^-1)*D;
    
    end
    
    % memorizzazione stati ottimali ottenuti ad ogni passo
    % si salva sempre l'ultimo uscito dal ciclo di linearizzazioni
    beta_tot = [beta_tot b_ott];
    
    % calcolo del nuovo iperpiano ottimale
    retta_new = -(b_ott(1)/b_ott(2)*x + b_ott(3)/b_ott(2));
    m1 = -(b_ott(1)/b_ott(2));      % coefficiente angolare
        
    % Calcolo della distanza punto/retta (iperpiano ottimo) di ogni punto e
    % memorizzazione di quella minima
    % distanza punto/retta --> |ax + by + c|/sqrt(a^2 + b^2)
    % dove (x,y) sono le coordinate del punto considerato
    % e a,b,c sono i coefficienti della retta in esame
    % nel plot poi coloro di verde i punti a distanza minore dalla nuova
    % retta ottenuta
    d_r = zeros(length(y_r),1);
    d_b = zeros(length(y_b),1);
    b = b_ott;
    
    % minimo dei ROSSI
    for i = 1:length(y_r)
        d_r(i) = abs(b(1)*P_r_tmp(i,1) + b(2)*P_r_tmp(i,2) + b(3))/sqrt(b(1)^2 + b(2)^2);
        % al primo passo prendo come dist. minima quella corrente per poi
        % confrontarla con le successive
        if i == 1
            d_min_1 = d_r(i);
            x_min_1 = P_r_tmp(i,:);
        else
            % se la nuova distanza calcolata è minore di quella salvata
            % viene aggiornato d_min e vengono salvate le coordinate del
            % punto corrispondente
            if d_r(i) < d_min_1
                x_min_1 = P_r_tmp(i,:);
                d_min_1 = d_r(i);
            end
        end
    end
        
    % minimo dei BLU (stesso procedimento usato sopra)
    for i = 1:length(y_b)
        d_b(i) = abs(b(1)*P_b_tmp(i,1) + b(2)*P_b_tmp(i,2) + b(3))/sqrt(b(1)^2 + b(2)^2);
        if i == 1
            d_min_2 = d_b(i);
            x_min_2 = P_b_tmp(i,:);
        else
            if d_b(i) < d_min_2
                x_min_2 = P_b_tmp(i,:);
                d_min_2 = d_b(i);
            end
        end
    end
    
    % memorizzazione delle distanze minime relative ad ogni passo
    dist_min = [dist_min; d_min_1 d_min_2];
    
    % memorizzazione delle coordinate dei punti più vicini all'iperpiano
    x_min = [x_min_1;x_min_2];
        
   
    % Operazioni eseguite per il plot
    
    % Calcolo dei punti per disegnare il segmento punto_minimo/retta
    x_perp1 = (b_ott(2)^2*x_min_1(1) - b_ott(1)*b_ott(2)*x_min_1(2) - b_ott(3)*b_ott(1))/(b_ott(1)^2+b_ott(2)^2);
    y_perp1 = m1*x_perp1 - b_ott(3)/b_ott(2);
    x_perp2 = (b_ott(2)^2*x_min_2(1) - b_ott(1)*b_ott(2)*x_min_2(2) - b_ott(3)*b_ott(1))/(b_ott(1)^2+b_ott(2)^2);
    y_perp2 = m1*x_perp2 - b_ott(3)/b_ott(2);
    
    % margini SVM (rette parallele all'iperpiano ottimo, passanti per i
    % punti a distanza minima calcolati sopra)
    margine1 = m1*(x - x_min_1(1)) + x_min_1(2);    % rosso
    margine2 = m1*(x - x_min_2(1)) + x_min_2(2);    % blu
    
    % impostazioni grafico conseguenti alla distanza punto/retta
    if d_min_1 < 10
        color = 'r';
        width = 1.5;
    else
        color = 'g';
        width = 1;
    end
    
%     % creazione dell'area colorata tra i due margini
%     x1 = -100;
%     x2 = 200;
%     p1 = m1*(x1 - x_min_1(1)) + x_min_1(2);
%     p2 = m1*(x1 - x_min_2(1)) + x_min_2(2);
%     p3 = m1*(x2 - x_min_2(1)) + x_min_2(2);
%     p4 = m1*(x2 - x_min_1(1)) + x_min_1(2);
%     area = patch([x1 x1 x2 x2],[p1 p2 p3 p4],'g');
    

    % plot iperpiano ottimo calcolato + margini
    plot(x,retta_new,color,'LineWidth',width);
    plot(x_min(:,1),x_min(:,2),'o','MarkerEdgeColor','g','MarkerFaceColor','g');
    plot([x_min_1(1) x_perp1],[x_min_1(2) y_perp1],'--');
    plot([x_min_2(1) x_perp2],[x_min_2(2) y_perp2],'--');
%     set(area,'EdgeColor','g','facealpha',0.1);
    plot(x,margine1,'--g','LineWidth',1);
    plot(x,margine2,'--g','LineWidth',1);
    pause(pp)
    hold off
    
    
    % comando utilizzato per la creazione del video della sequenza
    % --> fig = figure(t);
    % --> F = getframe(fig);
    % --> aviobj = addframe(aviobj,F);

    
end

% comando utilizzato per la creazione del video della sequenza
% --> aviobj = close(aviobj);
