%main_example
addpath('libsvm-mat-3.0-1');

clc
clear all
close all
format long

%creazione modello di movimento
Nr=10;
Nb=10;
label_r=ones(Nr,1);
label_b=-ones(Nb,1);

%COSTRUZIONE MATRICI RANGE INIZIALI PUNTI
range_x_r=[85 95];
range_y_r=[5 15];
range_x_b=[5 15];
range_y_b=[85 95];

Po_r_x =range_x_r(1) + (range_x_r(2)-range_x_r(1)).*rand(Nr,1);
Po_r_y = range_y_r(1) + (range_y_r(2)-range_y_r(1)).*rand(Nr,1);

%POSIZIONI INIZIALI ROSSI
P_r(:,:,1)=[Po_r_x Po_r_y label_r];

Po_b_x =range_x_b(1) + (range_x_b(2)-range_x_b(1)).*rand(Nb,1);
Po_b_y =range_y_b(1) + (range_y_b(2)-range_y_b(1)).*rand(Nb,1);

%POSIZIONI INIZIALI BLU
P_b(:,:,1)=[Po_b_x Po_b_y label_b];


figure(1)
hold on
plot(P_r(:,1,1),P_r(:,2,1),'o','MarkerEdgeColor','r','MarkerFaceColor','r');
plot(P_b(:,1,1),P_b(:,2,1),'o','MarkerEdgeColor','b','MarkerFaceColor','b');
axis([0 100 0 100])
grid on

% DATI
pp = 0.2;
N = 20;     % numero frames
t = 1;
n_f = 2;
n = n_f + 1;
N_tot = Nb + Nr;
n_lin = 20;      
a = 2;
b = 2.5;
c = 3;
d = 3.5;
DD = zeros(n,1);
HH = zeros(n);
x = 1:100;
%Q = 10^-6*(-1+2*rand(3,3));                   % varianza rumore random-walk
%Q = (10^-5)*eye(n);
Q = 0*[(10^-6)*(-1+2*rand(2,2)) zeros(2,1); 0 0 0.1];  %provare con n_lin=5 e n_lin=20 per vedere la differenza         
P_pred_0 = -1+2*rand(3,3);%1*eye(n);

vr_x = -a-(b-a).*rand(Nr,1); %velocità spostamento funzione
vb_x = +a+(b-a).*-rand(Nb,1);

vr_y = +c+(d-c).*rand(Nr,1); %velocità spostamento funzione
vb_y = -c-(d-c).*-rand(Nb,1);

n_f = 2;
P = [P_r;P_b];

%% SVM TRAIN
model = svmtrain(P(:,3,1),P(:,1:2,1),'-t 0');

% disegno la retta ed evidenzio (in verde) i SV

beta_0_iniz = -model.rho;
beta_iniz = zeros(n_f,1);
N_sv = model.totalSV;
sv_tmp = zeros(N_sv,n_f);

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

beta = beta_iniz;
beta_0 = beta_0_iniz;

% pause(5)
figure(1)
hold on
retta = -(beta(1)/beta(2)*x + beta_0/beta(2));
plot(sv_tmp(:,1),sv_tmp(:,2),'o','MarkerEdgeColor','g','MarkerFaceColor','g');
plot(x,retta,'k');
hold off

%% C.I. KALMAN
w_pred_0 = [beta_iniz; beta_0_iniz];    % w_(0\-1)
%P_pred_0 = -1+2*rand(3,3);%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);


%% SVM KALMAN
for t = 2:N
    if t > 2
        retta = retta_new;
    end
    figure(1)
    plot(x,retta,'k');
    axis([0 100 0 100])
    grid on
    
    
    pause(pp);
    
    P_r(:,:,t) = [P_r(:,1,1)+vr_x*t Po_r_y+vr_y*t label_r];
    P_b(:,:,t) = [P_b(:,1,1)+vb_x*t Po_b_y+vb_y*t label_b];
    P(:,:,t) = [P_r(:,:,t); P_b(:,:,t)];
    
    hold on
    plot(P_r(:,1,t),P_r(:,2,t),'o','MarkerEdgeColor','r','MarkerFaceColor','r');
    hold on
    plot(P_b(:,1,t),P_b(:,2,t),'o','MarkerEdgeColor','b','MarkerFaceColor','b');
    axis([0 100 0 100])
    grid on
    hold off
    pause(pp)
    
    % ciclo di linearizzazioni
    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);
     
    for l = 1:n_lin-1
       
    % creazione f_i che uso poi dentro linearize_new
    f_i = zeros(N_tot,1);
    for r = 1:N_tot
       f_i(r) = w_filt_tmp(1:n_f,l)'*P(r,1:n_f,t)' + w_filt_tmp(n,l);
    end
    
    % linearizzazione
    [D,H] = linearize_new(P(:,1:n_f,t),P(:,n,t),f_i,N_tot,n);
    
    DD = [DD D];
    HH = [HH H];
    
    % AGGIORNAMENTO tmp
    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 tmp
    P_pred_tmp(:,:,l+1) = P_filt_tmp(:,:,l+1)+Q;
    w_pred_tmp(:,l+1) = w_filt_tmp(:,l+1);
    
    end
    
    % esco dal ciclo di linearizzazioni ed eseguo il Kalman finale
    % trovo il minimo di C_hat(w)+R(w) applicando kalman in forma d'info
    % AGGIORNAMENTO
    P_filt(:,:,t) = P_filt_tmp(:,:,n_lin);
    w_filt(:,t) = w_filt_tmp(:,n_lin);
    
    % PREDIZIONE
    P_pred(:,:,t) = P_pred_tmp(:,:,n_lin);
    w_pred(:,t) = w_pred_tmp(:,n_lin);
    
    
    beta = w_filt(1:n_f,t);
    beta_0 = w_filt(n,t);
    retta_new = -(beta(1)/beta(2)*x + beta_0/beta(2));
    
    hold on
    plot(x,retta_new,'g');
    pause(pp)
    hold off
    
end

close all






