clc
clear all
close all

addpath('pingpong');
addpath('libsvm-mat-3.0-1');

% DATI
n_f = 3;            % numero di features
n = n_f+1;          % dimensione sistema=dim(w)
N = 30;              % numero di frame
n_lin = 2;          % numero di cicli di linearizzazione
Q = 0*eye(n);    % varianza rumore random-walk
P_pred_0 = 1*eye(n);    % P_(0\-1)
DD = zeros(n,1);
HH = zeros(n);
cin  =100;
ciun =110;
cian =100;

%% PROCEDURA

for t = 1:N
    % preparazione immagine
    G = imread(sprintf('%05d.jpg',t));
    dim = size(G);
    dim_tot = dim(1)*dim(2);
    pixel_RGB = zeros(dim_tot,3);
    row = 1;
    for r = 1:dim(1)
        for c = 1:dim(2)
            for m = 1:3
                pixel_RGB(row,m) = G(r,c,m);
            end
        row = row + 1;
        end
    end


% G = imread('rgb_kalman.jpg');
% dim = size(G);
% dim_tot = dim(1)*dim(2);
% 
% pixel_RGB = zeros(dim_tot,3);
% row = 1;
% for r = 1:dim(1)
%     for c = 1:dim(2)
%         for k = 1:3
%             pixel_RGB(row,k) = G(r,c,k);
%         end
%         row = row + 1;
%     end
% end

     % LA PRIMA IMMAGINE NON LA CLASSIFICO XKE' LA USO X IL TRAINING, CHE 
     % MI DA W_(0\0} DA UTILIZZARE SULLA SECONDA IMMAGINE
     % con la prima immagine faccio il training e mi ricavo i Beta e Beta_0
     % iniziali
         
if t == 1    
        Group = zeros(dim_tot,1);
        for r = 1:dim_tot
            if pixel_RGB(r,1) > cin && pixel_RGB(r,2) > ciun && pixel_RGB(r,3) > cian
                Group(r) = 1;
            else
                Group(r) = -1;
            end
        end
    
    model = svmtrain(Group,pixel_RGB,'-t 0'); 
    
%     for r=1:dim_tot
%         if pixel_RGB(r,1) > cin && pixel_RGB(r,2) > ciun && pixel_RGB(r,3) > cian
%             plot3(pixel_RGB(r,1),pixel_RGB(r,2),pixel_RGB(r,3),'x')
%         else
%             plot3(pixel_RGB(r,1),pixel_RGB(r,2),pixel_RGB(r,3),'o')    
%         end
%     hold on
%     end
%     
    
    % CI KALMAN
    
    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
    
    
    w_pred_0 = [beta_iniz; beta_0_iniz];    % w_(0\-1)
    P_pred_0 = 1*eye(n);                    % P_(0\-1)

    w_pred(:,t) = w_pred_0;     % inizializzo matrice con tutte le w_filt(t)
    P_pred(:,:,t) = P_pred_0;
    
    w_filt(:,t) = w_pred(:,t);
    
    
else
     
     %%%% parte di classificazione dell'immagine che porge {x_i,y_i},i=1...M(t+1) %%%
     % compongo iperpiano usando w_filt(:,t) calcolato alla fine
     % dell'iteraz precedente
    
     x = pixel_RGB;         % pixel dell'immagine corrente
     M_t = dim_tot;         % numero pixel immagine
     f_i = zeros(M_t,1);
     
     % Group effettivo
     for r = 1:dim_tot
            if pixel_RGB(r,1) > cin && pixel_RGB(r,2) > ciun && pixel_RGB(r,3) > cian
                Group(r) = 1;
            else
                Group(r) = -1;
            end
     end
     y = Group;
     
     
     % ciclo di linearizzazione
     w_filt_tmp = zeros(n,1);
     w_pred_tmp = zeros(n,1);
     
 
     P_filt_tmp(:,:,1) = zeros(n);
     P_pred_tmp(:,:,1) = P_pred(:,:,t-1);
     w_filt_tmp(:,1) = w_filt(:,t-1);
     l = 0;
     
     while l < n_lin
     l = l + 1;
     % 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) = w_filt_tmp(1:n_f,l)'*x(a,:)' + w_filt_tmp(n,l);
                if f_i(a)>0 && l == n_lin
                    G_mod(r,c,:) = [255 0 0];
                end
            end
        end
     end
     
         
     % w_(t+1\t) = w_(t\t) = w_filt(:,t)
     % [D H] = linearize(Cost,w_pred(:,t)); % % calcola Gradiente ed Hessiano di C(w)
     %D = rand(n,1);
     %H = rand(n,n);
     
     % calcolo D e H
     %y = pred_Group;       % classificazione che esce dall'svmpredict
     
     
     %y = idinput(M_t,'prbs',[0,1],[-1,1]);
        
     % linearizzazione: calcolo GRADIENTE ed HESSIANO
     [D,H] = linearize_new(x,y,f_i,M_t,n);
     
     DD = [DD D];
     HH = [HH H];
     
     % calcolo parametri per C_hat(w)
     %S = H^-1;
     %m = S*D;
     
     % trovo il minimo di C_hat(w)+R(w) applicando kalman in forma d'info
     
     % AGGIORNAMENTO
     P_filt_tmp(:,:,l+1) = inv(inv(P_pred_tmp(:,:,l))+H);
     w_filt_tmp(:,l+1) = P_filt_tmp(:,:,l+1)*(inv(P_pred_tmp(:,:,l))*w_pred_tmp(:,l)+D);
     
     % PREDIZIONE
     P_pred_tmp(:,:,l+1) = P_filt_tmp(:,:,l+1)+Q;
     w_pred_tmp(:,l+1) = w_filt_tmp(:,l+1);
     
     
%      for r=1:dim_tot
%         if pixel_RGB(r,1) > cin && pixel_RGB(r,2) > ciun && pixel_RGB(r,3) > cian
%             plot3(pixel_RGB(r,1),pixel_RGB(r,2),pixel_RGB(r,3),'x')
%         else
%             plot3(pixel_RGB(r,1),pixel_RGB(r,2),pixel_RGB(r,3),'o')    
%         end
%     hold on
%     end
     
     
     end
     
     figure()
     hold on;
     imshow(G_mod);
     
     % COPIO i valori finali ottenuti dalle 10 linearizzazioni
     % AGGIORNAMENTO
     P_filt(:,:,t) = P_filt_tmp(:,:,l+1);
     w_filt(:,t) = w_filt_tmp(:,l+1);
     
     % PREDIZIONE
     P_pred(:,:,t) = P_pred_tmp(:,:,l+1);
     w_pred(:,t) = w_pred_tmp(:,l+1);
    

     
end
end
