%% Inizailizzazione
global dest
if m==8 %Dispone otto telecamere nella stanza. Per ogni telecamera servono 4 parametri: Traslazione lungo x, lungo y, lungo z (sarà sempre =0 nel 2d) e rotazione attorno a z.
camera_pos = [3000 -1000 0 0;7000 -1000 0 0; 11000 3000 0 pi/2; 11000 7000 0 pi/2;7000 11000 0 pi;3000 11000 0 pi;-1000 7000 0 -pi/2;-1000 3000 0 -pi/2];
else %Dispone 16 telecamere nella stanza
camera_pos = [1000 -1000 0 0; 3000 -1000 0 0; 6000 -1000 0 0; 9000 -1000 0 0; 11000 1000 0 pi/2; 11000 3000 0 pi/2;11000 6000 0 pi/2;11000 9000 0 pi/2; 9000 11000 0 pi; 6000 11000 0 pi;3000 11000 0 pi;1000 11000 0 pi; -1000 9000 0 -pi/2; -1000 6000 0 -pi/2;-1000 3000 0 -pi/2;-1000 1000 0 -pi/2; ];
end

% Definizione CCD in Pixel
def_CCD = 2000;

dim_CCD = 10; % Larghezza CCD [mm]

% Dimensione singolo pixel
pixel_dim = dim_CCD/def_CCD; 

%Distanza massina in pixels perch� venga considerato un singolo marker
% serve utilizzando la funzione calcola centri
distanza_max = 5; 

% Lunghezza focale 
f = dim_CCD/2; 

% Varianza errore per generazione piano
sigma = 30;
%% Marker

pt=pt';

%% Funzione gruppo 2

v = (def_CCD*pixel_dim)/2;
u = (def_CCD*pixel_dim)/2;

K =[   1      ,     0     , v;
       0      ,     1     , 1;
       0      ,     0     , 1];

% Inizializzo variabili dinamiche

coord    = cell([m,1]);  
hidden   = cell([m,1]); 
distance = cell([m,1]); 

%funzione camera_simulation_2d fornita dal gruppo 2
% camera_simulation: simulatore di una telecamera pin-hole
% [uv, hidden, distance]=camera_simulation(R,T,pt,K,f,heigth,width)
% 
% INPUT:
% -R matrice di rotazione della telecamera rispetto al riferimento-ambiente
% -T vettore di traslazione della telecamera rispetto al
%       riferimento-ambiente
% -pt matrice dei punti dello spazio reale da proietttare (no coordinate
%       omogenee)
% -K parametri intrinseci telecamera
% -f distanza focale
% -heigth,width dimensioni in pixel del piano immagine.

for i = 1 : m
alpha = camera_pos(i,4);
T = [camera_pos(i,1) camera_pos(i,2)]';
% - uv matrice dei punti dell' immagine,
% - hidden vettore booleano che indica quali dei punti iniziali non sono
%       visibili (0=visibile, 1=invisibile)
% - distance vettore indicante la distanza del punto dall'origine del
%       sistema telecamera (utile ad esempio per determinare la dimensione
%       apparente del marker o se considerarlo invisibile o sfocato a causa
%       della distanza 
[coord{i}, hidden{i}, distance{i}]= camera_simulation_2d(alpha,T,pt,K,f,2000);   
end


%% Genera piano immagine sporcato

%Pero ogni telecamera genera un piano immagine sporco e lo salva in delle
%celle

pixel_piano = def_CCD;
piano_gen   = cell([m,1]);

for i = 1 : m
% Riporto valori in pixel
coord_tmp = [(coord{i}*def_CCD)/dim_CCD];
distanza_tmp = [distance{i}];
% Coordinata rispetto al piano immagine, distanza dal piano immagine in mm
P = [coord_tmp' distanza_tmp'];
piano_gen{i} = genera_piano(P,pixel_piano,d_min,d_max,sigma);
end

%% Calcolo centri a partire dal piano immagine

% Inizzializzo variabili
centri = cell([m,1]); 
num_pixel_centri = cell([m,1]); 
% Inizializzo funzione calcola centri
n_tmp = 5*n;
% Ciclo per ogni singola telecamera
for i = 1 : m
    piano = [piano_gen{i}];
    [centri{i} num_pixel_centri{i}]= calc_centri(n_tmp,distanza_max,pixel_dim,piano);
end

%centri contiene le coordinate sul piano immagine del centro della
%proiezione del marker mentre num_pixel_centri contiene la dimensione della
%proiezione misurata in numero di pixel

axes(dest)

%% Funzione assoluto

% Mi riporta il punto immagine e il punto focale della telecamera nel
% sistema di riferimento assoluto per un singolo centro sul piano immagine

% Coordinate punto focale
x_f = dim_CCD/2;
y_f = -f; 

% Inizializzo le celle e il vettore dei fuochi per ogni telecamera
x_p_as = cell([m,1]);
y_p_as = cell([m,1]);
z_p_as = cell([m,1]);
x_f_as = zeros(m,1);
y_f_as = zeros(m,1);
z_f_as = zeros(m,1);
hold on;

for i = 1 : m
camera_centri = [centri{i}];
[x_p_as{i}, y_p_as{i}, z_p_as{i}, x_f_as(i) , y_f_as(i), z_f_as(i)] = assoluto(camera_pos(i,1), camera_pos(i,2), camera_pos(i,3), camera_pos(i,4),camera_centri,dim_CCD, f);
end


%% Calcolo i raggi

a = cell([m,1]);
b = cell([m,1]);
c = cell([m,1]);

for i = 1 : m
    x_p = [x_p_as{i}];
    y_p = [y_p_as{i}];
    z_p = [z_p_as{i}];
    [a{i} b{i} c{i}] = retta(x_p, y_p, x_f_as(i), y_f_as(i));
end

%% Plot delle rette sul piano di riferimento

% Distinguo il caso in cui la retta sia parallela all'asse y

% Colore diverso per i raggi assocciati alla telecamera
color = cell([2,1]);
color{1} = 'b';
color{2} = 'r';
color{3} = 'k';
color{4} = 'g';
color{5} = 'm';
color{6} = 'c';
color{7} = 'y';
color{8} = 'b';
color{9} = 'r';
color{10} = 'k';
color{11} = 'g';
color{12} = 'm';
color{13} = 'c';
color{14} = 'y';
color{15} = 'b';
color{16} = 'r';

% Area stanza di colore diverso

Area_stanza = area([0 10000 0 10000],[10000 10000 10000 10000]);
colormap white

hold on
for j = 1 : m
    a_tmp = [a{j}];
    b_tmp = [b{j}];
    c_tmp = [c{j}];
    
    for i = 1 : length(a_tmp)
        if b_tmp(i) == 0
            y = -1 : 0.1 : 11000;
            x = -(b_tmp(i)*y + c_tmp(i))/a_tmp(i);
            hold on;
            grid on;
            plot(x,y,color{j});
        else
            x = -2000 : 0.1 : 11000;
            y = -(c_tmp(i)+a_tmp(i)*x)/b_tmp(i);
            hold on;
            grid on;
            axis([-2000 12000 -2000 12000]);

            plot(x,y,color{j});
        end
    end
end


%% Plotto posizione effettiva marker nel sistema di riferimento assoluto


for i = 1 : n
plot(pt(1,i),pt(2,i),'o','MarkerSize',10);
end
xlabel('Asse x [mm]'); ylabel('Asse y [mm]');
title('Asse di riferimento assoluto','Fontsize',15);

%% Plotto disegno telecamere (da sistemare una volta concluso)

% dimesione della telecamera, si pu� scalare
size = 8;
% Punti del triangolo raffigurante la telecamera
% Valori reali, suppongo obbiettivo telecamera di dimensione 60 [mm]
xdata = [-30 0 30];
ydata = [60 0 60];

% Vertici del corpo della telecamera
x_corpo = [-30 30 30 -30];
y_corpo = [0 0 -100 -100];
vertici_corpo = [x_corpo' y_corpo'];

% Faces del corpo della telecamera
faces_corpo = [1 2 3 4];

% Scalatura dei punti
P = size*[xdata;ydata];

% Dimensione del corpo telecamera per scalatura
size_corpo = 7;

% Scalatura del corpo della telecamera
P_corpo = size_corpo*vertici_corpo;

for i = 1 : m 
% matrice di rotazione per ruotare le telecamere 
R = [cos(camera_pos(i,4)) -sin(camera_pos(i,4)); sin(camera_pos(i,4)) cos(camera_pos(i,4))];
% rotazione punti
Rot_punti = R*P; 

% Matrice di rotazione del corpo
Rot_corpo = R*P_corpo';

% Matrice di traslazione del corpo
Rot_trasl_corpo = Rot_corpo + [ones(1,4)*camera_pos(i,1); ones(1,4)*camera_pos(i,2)];

% Matrice di traslazione dell'obiettivo
Rot_trasl = Rot_punti + [ones(1,3)*camera_pos(i,1); ones(1,3)*camera_pos(i,2)];

% Plot delle varie telecamere
p = patch(Rot_trasl(1,:),Rot_trasl(2,:),'k');
% Plot dei vari obiettivi 
r = patch('Vertices',Rot_trasl_corpo','Faces',faces_corpo,'FaceColor','k');
end
axis([-2000 12000 -2000 12000])
hold off

