% MAIN

clc
% close all
% clear all

global pp N t n_f n N_tot x Nr Nb

%% Creazione modello di movimento
disp('------------- Creazione modello -------------')
disp('Numero di campioni da esaminare:')
Nr = input('n° punti per ogni gruppo: ');
disp(' ')
Nb = Nr;

label_r = ones(Nr,1);           % etichette rosse iniziali (1)
label_b = -ones(Nb,1);          % etichette blu iniziali (-1)
y_label = [label_r; label_b];   % etichette raggruppate
y_r = label_r;                  % rinomimazione etichette (per fun_sqp)
y_b = label_b;

% 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 punti ROSSI
P_r(:,:,1) = [Po_r_x Po_r_y];

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 punti BLU
P_b(:,:,1) = [Po_b_x Po_b_y];

% comando utilizzato per lavorare su una dinamica fissa dei punti
% Posizioni iniziali totali
% load('traiett.mat');


%% Dati
N = 30;             % n° frames
n_f = 2;            % n° features
n = n_f + 1;        % dimensione stato
N_tot = Nb + Nr;    % n° totale campioni
a_dat = 1;          % coefficienti per la creazione della traiettoria punti
b_dat = 2;
c_dat = 8;
d_dat = 9;
x = -200:300;       % asse utilizzato per i plot
pp = 0.1;           % pausa utilizzata nei plot


%% Costruzione traiettorie punti 

% Velocità spostamento funzione
vr_x = -a_dat-(b_dat-a_dat).*rand(Nr,1); 
vb_x = +a_dat+(b_dat-a_dat).*-rand(Nb,1);

% Velocità spostamento funzione
vr_y = +c_dat+(d_dat-c_dat).*rand(Nr,1); 
vb_y = -c_dat-(d_dat-c_dat).*-rand(Nb,1);

% Posizioni punti rossi e blu (traiettoria)
for t = 1:N
    P_r(:,:,t) = [P_r(:,1,1)+vr_x*t Po_r_y+vr_y*t];
    P_b(:,:,t) = [P_b(:,1,1)+vb_x*t Po_b_y+vb_y*t];
    P(:,:,t) = [P_r(:,:,t); P_b(:,:,t)];
end

%% Algoritmi di simulazione
quit = 0;

while quit == 0

    disp('---------- Algoritmi di simulazione ----------')
    disp('Scegliere una funzione tra le seguenti:')
    disp(' ')
    disp('(1) - Funzione ideale, effettua ad ogni ciclo un nuovo Training')
    disp('(2) - Funzione SQP, effettua un Training iniziale e poi modella l iperpiano autonomamente in base ai nuovi dati')
    disp('(3) - Funzione semi-ideale, effettua un nuovo Training solo se necessario, rappresenta lo stato dell arte')
    disp(' ')
    index = input('Funzione scelta: ');
    disp(' ')
    
    if index == 1
        [beta_tot_ideale dist_ideale] = fun_ideale(P,P_r,P_b,y_label);
    elseif index == 2
        [beta_tot_sqp dist_sqp] = fun_sqp(P,P_r,P_b,y_label);
    else
        [beta_tot_trainclassify dist_tc] = fun_trainclassify(P,P_r,P_b,y_label);
    end
    
    disp('Si vuole eseguire una nuova simulazione?')
    disp('(1) - Si')
    disp('(2) - No')
    risp1 = input('Risposta: ');
    disp(' ')
    
    if risp1 == 2
        quit = 1;
    end
    
    % comando utilizzato per studiare le prestazioni dell'algoritmo sqp
    % rispetto a quello ideale (per poterlo usare si devono avere a
    % disposizione le distanze dist_sqp e dist_ideale punto/retta)
    %
    % prestazioni(dist_sqp,dist_ideale)

end

%% Confronto prestazioni
quit = 0;

while quit == 0

    disp('--------------- Prestazioni ---------------')
    disp('Si vogliono analizzare le prestazioni?')
    disp('(1) - Si')
    disp('(2) - No')
    risp2 = input('Risposta: ');
    disp(' ')
    
    if risp2 == 1
        
        disp('scegliere le prestazioni che si desiderano conoscere:')
        disp('(1) - Ottimalità algoritmo SQP: confronto distanze iperpiano/punti')
        disp('(2) - Prestazioni algoritmo SQP rispetto al caso ideale')
        prestazione = input('Scelta: ');
        disp(' ')
        
        
        if prestazione == 1
            optimality(dist_sqp);
            optimality(dist_ideale);
        else
            confr(dist_sqp,dist_ideale);
        end
        
        disp('Si vogliono conoscere ulteriori prestazioni?')
        disp('(1) - Si')
        disp('(2) - No')
        risp3 = input('Risposta: ');
        disp(' ')
        
        if risp3 == 2
            quit = 1;
        end
       
    else
        quit = 1;
    end
end
