%% Main minimi quadrati con utilizzo di pi� rette

clear all
close all
clc
set(0,'RecursionLimit',1000)
%% Telecamere
% Numero di telecamere
m = 16;

% tempo complessivo algoritmo
Tempo_Com = tic;

% Posizione riferita al centro focale della telecamera rispetto al sistema
% di riferimento assoluto
% Posizionamento del centro OTTICO rispetto al sistema di riferimento
% assoluto
% camera_pos = [x_t y_t z_t alpha beta gamma]; per ogni telecamera su ogni riga
% alpha = rotazione rispetto asse x
% beta = rotazione rispetto asse y
% gamma = rotazione rispetto asse z

% POSIZIONE TELECAMERE

% 4 telecamere
%camera_pos = [-1000 5000 5000 -pi/2 0 -pi/2; 5000 -1000 5000 -pi/2 0 0;11000 5000 5000 -pi/2 0 pi/2; 5000 11000 5000 -pi/2 0 pi];

% 8 telecamere
%camera_pos = [-1000 8000 5000 -pi/2 0 -pi/2;-1000 2000 5000 -pi/2 0 -pi/2; 2000 -1000 5000 -pi/2 0 0; 8000 -1000 5000 -pi/2 0 0;11000 2000 5000 -pi/2 0 pi/2;11000 8000 5000 -pi/2 0 pi/2; 8000 11000 5000 -pi/2 0 pi;2000 11000 5000 -pi/2 0 pi];

%16 telecamere
 camera_pos = [-1000 7000 6000 -pi/2 0 -pi/2;-1000 3000 6000 -pi/2 0 -pi/2; -1000 8000 3000 -pi/2 0 -pi/2;-1000 2000 3000 -pi/2 0 -pi/2;...
     3000 -1000 6000 -pi/2 0 0; 7000 -1000 6000 -pi/2 0 0; 3000 -1000 3000 -pi/2 0 0; 7000 -1000 3000 -pi/2 0 0;...
     11000 3000 6000 -pi/2 0 pi/2;11000 7000 6000 -pi/2 0 pi/2;11000 3000 3000 -pi/2 0 pi/2;11000 7000 3000 -pi/2 0 pi/2; ...
     
     7000 11000 6000 -pi/2 0 pi;3000 11000 6000 -pi/2 0 pi; 7000 11000 3000 -pi/2 0 pi;3000 11000 3000 -pi/2 0 pi];

% Definizione CCD in MegaPixel
def_CCD = 2000;

% Dimensione del piano immagine
dim_CCD = 10; % Larghezza CCD di un pollice [mm]

% Dimensione singolo pixel in [mm]
pixel_dim = dim_CCD/def_CCD;

% Lunghezza focale 
% fuoco dietro al piano immagine, negativo se fuoco davanti al piano
% immagine
f = dim_CCD/2; % Misura scalabile, nella realt� � met� della dimensione del CCD

% Larghezza piano immagine
heigth = 2000;

% Altezza piano immagine
width  = 2000;

% Visuale minima della telecamera
d_min_var = 1000;

% Visuale massima della telecamera
d_max_var = 8000;

% Diametro massimo del punto sul piano immagine
diam_max_punto = 50;

% Varianza dell'errore sulla ricostruzione del punto sul piano immagine
sigma = 100;

% Distanza dinima tra le rette, accettata per la ricostruzione dei punti
distanza_minima = 10;

% Paramatro per la mediazione dei punti utilizzato in ricostruzione
epsilon = 5;

%% Marker

% Numero di punti
n = 15;

% Punti da proiettare riferiti terna mondo [x_w1 x_w2 x_w3;y_w1 y_w2 y_w3, z_w1 z_w2 z_w3];
% pt_3D = [2000 5000 6000 7000 4000 6000 4800 3900;
%          2000 5000 6000 7000 6000 3000 5600 2900;
%          2000 5000 6000 7000 3000 5000 3900 1800];

 pt_3D = [2000 2200 2000 2500 3000 3300 5500 6000 6150 7000 4000 4300 8000 4000 4400;
          6000 6300 7000 7300 3000 3200 5500 6000 6600 7000 1000 1300 4000 6000 6500;
          3000 3200 6000 6400 3000 3150 5500 6000 6300 7000 5000 5300 3000 3000 3200];
      

%pt_3D = [2000 5000 6000 7000 4000 8000;2000 5000 6000 7000 1000 2000;2000 5000 6000 7000 5000 2000];

%pt_3D = [5000 7000;5000 7000;5000 7000];

%pt_3D = [7000;7000;7000];

%% Plot dell'ambiente

figure(1);
for i = 1 : n
plot3(pt_3D(1,i),pt_3D(2,i),pt_3D(3,i),'ro','MarkerFaceColor','b');
hold on
grid on
axis([-1000 11000 -1000 11000 -1000 11000]);
xlabel('Asse x [mm]'); ylabel('Asse y [mm]'); zlabel('Asse z [mm]');
title('Ambiente','Fontsize',15);
box on
end

%% Disegno della telecamera in 3D

% Punti con asse zeta passante per il centro del piano immagine 
xdata = [-150 150 150 -150 0]';
ydata = [-150 -150 150 150 0]';
zdata = [300 300 300 300 0]';

% Possibilit� di scalare la dimesione della telecamera 
size_tele = 1.5;

% Vettore dei vertici delle Faces
vertex_matrix_ob = size_tele*[xdata ydata zdata];

% Numero dei vertici utilizzati per plottare le Faces
faces_matrix  = [1 2 3 4];
faces_matrix1 = [1 2 5];
faces_matrix2 = [2 3 5];
faces_matrix3 = [3 4 5];
faces_matrix4 = [4 1 5];

% Vertici del corpo telecamera
x_corpo = [-150  150  150 -150 -150  150  150 -150]';
y_corpo = [-150 -150  150  150 -150 -150  150  150]';
z_corpo = [  0    0    0    0  -500 -500 -500 -500]';

faces_matrix_corpo = [1 2 3 4;5 6 7 8;3 7 8 4;2 6 7 3;1 2 6 5;1 5 8 4];

% Possibilit� di modificare dimensione corpo telecamera
size_corpo = 1.5;
vertex_matrix_corpo = size_corpo*[x_corpo y_corpo z_corpo];

%% Matrici di rotazione
for i = 1 : m
    
% Rotazione asse x
Rx=[1 0 0; 0 cos(camera_pos(i,4)) -sin(camera_pos(i,4)); 0 sin(camera_pos(i,4)) cos(camera_pos(i,4))];

% Rotazione asse y
Ry=[cos(camera_pos(i,5)) 0 sin(camera_pos(i,5)); 0 1 0; -sin(camera_pos(i,5)) 0 cos(camera_pos(i,5))];

% Rotazione asse z
Rz=[cos(camera_pos(i,6)) -sin(camera_pos(i,6)) 0; sin(camera_pos(i,6)) cos(camera_pos(i,6)) 0; 0 0 1];

% Matrice di rotazione
R = Rz*Ry*Rx;

% Ratazione vertici obiettivo
vertex_matrix_tmp = R*vertex_matrix_ob';

% Traslazione vertici obiettivo 
vertex_matrix_trasl = vertex_matrix_tmp + [ones(1,5)*camera_pos(i,1); ones(1,5)*camera_pos(i,2);ones(1,5)*camera_pos(i,3)];
vertex_matrix = vertex_matrix_trasl';

% Rotazione vertici corpo macchina
vertex_matrix_corpo_tmp = R*vertex_matrix_corpo';

% Traslazione dei vertici del corpo macchina
vertex_matrix_trasl_corpo = vertex_matrix_corpo_tmp + [ones(1,8)*camera_pos(i,1); ones(1,8)*camera_pos(i,2);ones(1,8)*camera_pos(i,3)];
vertex_matrix_corpo_patch = vertex_matrix_trasl_corpo';

% Disegno della telecamera complessiva (obiettivo+corpo)
p = patch('Vertices',vertex_matrix,'Faces',faces_matrix,'FaceColor','b');
p = patch('Vertices',vertex_matrix,'Faces',faces_matrix1,'FaceColor','b');
p = patch('Vertices',vertex_matrix,'Faces',faces_matrix2,'FaceColor','b');
p = patch('Vertices',vertex_matrix,'Faces',faces_matrix3,'FaceColor','b');
p = patch('Vertices',vertex_matrix,'Faces',faces_matrix4,'FaceColor','b');
p = patch('Vertices',vertex_matrix_corpo_patch,'Faces',faces_matrix_corpo,'FaceColor','b');
end

%% Simulatore 3D

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

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

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

for i = 1 : m
% Rotazione asse x
Rx=[1 0 0; 0 cos(camera_pos(i,4)) -sin(camera_pos(i,4)); 0 sin(camera_pos(i,4)) cos(camera_pos(i,4))];

% Rotazione asse y
Ry=[cos(camera_pos(i,5)) 0 sin(camera_pos(i,5)); 0 1 0; -sin(camera_pos(i,5)) 0 cos(camera_pos(i,5))];

% Rotazione asse z
Rz=[cos(camera_pos(i,6)) -sin(camera_pos(i,6)) 0; sin(camera_pos(i,6)) cos(camera_pos(i,6)) 0; 0 0 1];

% Matrice di rotazione
R = Rz*Ry*Rx;

% Matrice di traslazione delle telecamere
T = [camera_pos(i,1) camera_pos(i,2) camera_pos(i,3)]';
% 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 proiettare (no coordinate
%       omogenee)
% -K parametri intrinseci telecamera
% -f distanza focale
% -heigth,width dimensioni in pixel del piano immagine.
% OUTPUT:
% - coord 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(R,T,pt_3D,K,f,heigth,width);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end

%% Genera piano immagine con relativo errore

piano = cell([m,1]);
for i = 1 : m
    % Cordinate del piano immagine
    coord_tmp = [(coord{i}*def_CCD)/dim_CCD];
    P = coord_tmp;
    %P = [coord{i}];
    dist = [distance{i}];
    dati = [P' dist'];
    piano{i} = genera_piano_3D(dati, heigth ,width,d_min_var,d_max_var,sigma);
    % Plot dei piani immagine
%     figure
%     imagesc(0.5,0.5,piano{i})
%     colormap(gray)
%     title('Piano telecamera','Fontsize',15);
end
%% Calcola centri 
centri_x = cell([m,1]);
centri_y = cell([m,1]);
num_pixel_centri = cell([m,1]);

for i = 1 : m
    pixel_matrix = [piano{i}];
    % se pixels unitari
    %[centri_x{i}, centri_y{i}, num_pixel_centri{i}] = calc_centri_3D(pixel_matrix,1,1,n);
    % con dimensione pixel varibile
    [centri_x{i}, centri_y{i}, num_pixel_centri{i}] = calc_centri_3D(pixel_matrix,pixel_dim,pixel_dim,n*10);
end

%% Coordinate nel sistema di riferimento

x_p_as = cell([m,1]);
y_p_as = cell([m,1]);
z_p_as = cell([m,1]);

for i = 1 : m
    % Rotazione asse x
    Rx = [1 0 0; 0 cos(camera_pos(i,4)) -sin(camera_pos(i,4)); 0 sin(camera_pos(i,4)) cos(camera_pos(i,4))];

    % Rotazione asse y
    Ry = [cos(camera_pos(i,5)) 0 sin(camera_pos(i,5)); 0 1 0; -sin(camera_pos(i,5)) 0 cos(camera_pos(i,5))];

    % Rotazione asse z
    Rz = [cos(camera_pos(i,6)) -sin(camera_pos(i,6)) 0; sin(camera_pos(i,6)) cos(camera_pos(i,6)) 0; 0 0 1];

    % Matrice di rotazione
    R = Rz*Ry*Rx;
    
    x_p_rif = [centri_x{i}];
    y_p_rif = [centri_y{i}];
    coordinate = [x_p_rif; y_p_rif];
    [x_p_as{i}, y_p_as{i}, z_p_as{i}] = assoluto3D(camera_pos(i,1), camera_pos(i,2), camera_pos(i,3), R ,coordinate,dim_CCD,dim_CCD,f);                 
end   


%% retta parametrica nello spazio
% vediamo come si scrive una retta parametrica passante per 2 punti nello 
% spazio
figure(1)
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} = 'b';
color{12} = 'r';
color{13} = 'k';
color{14} = 'g';
color{15} = 'm';
color{16} = 'c';
color{17} = 'y';
color{18} = 'b';
color{19} = 'r';
color{20} = 'k';

for j = 1 : m
    [k nu] = size(x_p_as{j});
    for i = 1 : nu
        % Coordinate marker nel sistema di riferimento assoluto 
        x_p = [x_p_as{j}];
        y_p = [y_p_as{j}];
        z_p = [z_p_as{j}];
        
        x_0 = x_p(i);
        y_0 = y_p(i);
        z_0 = z_p(i);
        
        P_0 = [x_0; y_0; z_0]; %punto P_0
        
        % Posizione del centro del piano immagine telecamera
        x_1 = camera_pos(j,1);
        y_1 = camera_pos(j,2);
        z_1 = camera_pos(j,3);
        
        P_1 = [x_1; y_1; z_1]; %punto P_1
        
        m_1 = (P_0 - P_1);
        
        % retta passante per il punto P_0 ed il punto P_1 nello spazio
        t = -1000:0.1:10000;
        
        h = length(t);
        
        r_1 = P_0*ones(1,h) + m_1*t;
        plot3(r_1(1,:), r_1(2,:), r_1(3,:),color{j});
    end
end

%% Equazione retta passante per due punti
a = cell(m,1);
b = cell(m,1);
c = cell(m,1);
d = cell(m,1);


for i = 1 : m
    x_p_tmp = x_p_as{i};
    y_p_tmp = y_p_as{i};
    z_p_tmp = z_p_as{i};
    
    % Numero di centri della telecamera
    k = length(x_p_tmp);
    a_tmp = zeros(2,k);
    b_tmp = zeros(2,k);
    c_tmp = zeros(2,k);
    d_tmp = zeros(2,k);
    
    for j = 1 : k
        % Primo punto il centro sul piano immagine
        P_1 = [x_p_tmp(j) y_p_tmp(j) z_p_tmp(j)];
        % Secondo punto il centro focale
        P_2 = [camera_pos(i,1) camera_pos(i,2) camera_pos(i,3)];
        
        [a_tmp(:,j) b_tmp(:,j) c_tmp(:,j) d_tmp(:,j)] = eq_retta(P_1, P_2);
    end
    a{i} = a_tmp;
    b{i} = b_tmp;
    c{i} = c_tmp;
    d{i} = d_tmp;
end
    


%% Funzioni
% Eseguire 'Test_Associa2.m' per ricostruzione distribuita dei punti mediante minimi quadrati 

% Eseguire 'Test_Stato_dell_Arte.m' per ricostruzione punti centralizzata

