%% 
clc;
clear;

an = pi/2;

%Ry = [cos(an) 0 sin(an); 0 1 0; -sin(an) 0 cos(an)];
R = rotaz( 0, -pi/2, 0 )
T  = [0 0 1000]';

% matrice 3xN contenente le coordinate 3d di N punti rispetto al sistema di riferimento ambiente
pt = [ 1000 0 0]'; 

f=1000;
K=eye(3);

% heigth = 1;
% width = 2000;

[uv, hidden, distance]= camera_simulation(R,T,pt,K,f)
break




















x1=0;
y1= -1000;
x2= 1000;
y2= 5000;
%[a , b , c] = retta(x1,y1,x2,y2)


% function [ R ] = rotaz( a,b,c )
% %ROTAZ Summary of this function goes here
% %   Detailed explanation goes here
%     rx=[1 0 0; 0 cos(a) -sin(a); 0 sin(a) cos(a)];
%     ry=[cos(b) 0 sin(b); 0 1 0; -sin(b) 0 cos(b)];
%     rz=[cos(c) -sin(c) 0; sin(c) cos(c) 0; 0 0 1];
% 
%     R=rz*ry*rx;
% end

% 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.
% OUTPUT:
% - uv matrice dei punti dell' immagine,
% - hidden vettore booleano che indica quali dei punti iniziali non sono
%       visibili (1=visibile, 0=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 







%% 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

% 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);

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, length_piano, f);
end

%% Provo funzione retta passante per due punti
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

% [a1 b1 c1] = retta(x_p_as1, y_p_as1, x_f_as1 , y_f_as1);

%% 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';

% Area stanza di colore diverso
Area_stanza = area([0 10000 0 10000],[10000 10000 10000 10000]);
colormap cool

figure(1);

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 : 10;
            y = -1 : 0.1 : 10000;
            x = -(b_tmp(i)*y + c_tmp(i))/a_tmp(i);
            hold on;
            grid on;
            axis([-2 11 -2 11]);
            plot1 = plot(x,y,color{j});
        else
            %x = 0 : 0.1 : 10;
            x = -1 : 0.1 : 10000;
            y = -(c_tmp(i)+a_tmp(i)*x)/b_tmp(i);
            
            hold on;
            grid on;
            %axis([-2 11 -2 11]);
            axis([-1000 11000 -1000 11000]);
            plot1 = plot(x,y,color{j});
        end
    end
% hSGroup = hggroup;
% %hCGroup = hggroup;
% set(plot1,'Parent',hSGroup)
% 
% %  set(get(get(hCGroup,'Annotation'),'LegendInformation'),...
% %      'IconDisplayStyle','on'); 
% legend('tele 1','tele 2','tele 3','tele 4');
end
xlabel('Asse x [m]'); ylabel('Asse y [m]');
title('Asse di riferimento assoluto','Fontsize',15);

% dimesione della telecamera, si pu� scalare
size = 0.5;
% Punti del triangolo raffigurante la telecamera
xdata = [0 2 4];
ydata = [0 -4 0];
% Scalatura dei punti
P = size*[xdata;ydata];

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; 

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');
end

% v = cell([m,1]);
% v_tmp_inc_a = cell([m,1]);
% v_tmp_inc_i = cell([m,1]);
% %figure(2)
% 
% for j = 1 : m
%     if j == m
%         a_tmp = [a{j}];
%         b_tmp = [b{j}];
%         c_tmp = [c{j}];
%         
%         a_tmp1 = [a{1}];
%         b_tmp1 = [b{1}];
%         c_tmp1 = [c{1}];
%         
%         for i = 1 : length(a_tmp)
%             v{i}  = [a_tmp(i) b_tmp(i);a_tmp1(i) b_tmp1(i)];
%             c_rette = [c_tmp(i) c_tmp1(i)];
%             v_tmp = inv([v{i}])*(-c_rette');
%             x = v_tmp(1);
%             y = v_tmp(2);
%             %axis([-10 11 -10 11]);
%             grid on;
%             hold on;
%             plot(x, y,'o','MarkerSize',10);
%         end
%         for i = 1 : (length(a_tmp)-1)
%             v_tmp_inc_a{i} = [a_tmp(i) b_tmp(i);a_tmp1(i+1) b_tmp1(i+1)];
%             v_tmp_inc_i{i} = [a_tmp(i+1) b_tmp(i+1);a_tmp1(i) b_tmp1(i)];
%             c_rette_a = [c_tmp(i) c_tmp1(i+1)];
%             c_rette_i = [c_tmp(i+1) c_tmp1(i)];
%             v_tmp_a = inv([v_tmp_inc_a{i}])*(-c_rette_a');
%             v_tmp_i = inv([v_tmp_inc_i{i}])*(-c_rette_i');
%             x_a = v_tmp_a(1);
%             y_a = v_tmp_a(2);
%             x_i = v_tmp_i(1);
%             y_i = v_tmp_i(2);
%             axis([-10 11 -10 11]);
%             grid on;
%             hold on;
%             plot(x_a, y_a,'o','MarkerSize',10);
%             plot(x_i, y_i,'o','MarkerSize',10);
%         end
%     else
%         a_tmp = [a{j}];
%         b_tmp = [b{j}];
%         c_tmp = [c{j}];
%         
%         a_tmp1 = [a{j+1}];
%         b_tmp1 = [b{j+1}];
%         c_tmp1 = [c{j+1}];
%         for i = 1 : length(a_tmp)
%             v{i}  = [a_tmp(i) b_tmp(i);a_tmp1(i) b_tmp1(i)];
%             c_rette = [c_tmp(i) c_tmp1(i)];
%             v_tmp = inv([v{i}])*(-c_rette');
%             x = v_tmp(1);
%             y = v_tmp(2);
%             %axis([-10 11 -10 11]);
%             grid on;
%             hold on;
%             plot(x, y,'o','MarkerSize',10);
%         end
%         for i = 1 : (length(a_tmp)-1)
%             v_tmp_inc_a{i} = [a_tmp(i) b_tmp(i);a_tmp1(i+1) b_tmp1(i+1)];
%             v_tmp_inc_i{i} = [a_tmp(i+1) b_tmp(i+1);a_tmp1(i) b_tmp1(i)];
%             c_rette_a = [c_tmp(i) c_tmp1(i+1)];
%             c_rette_i = [c_tmp(i+1) c_tmp1(i)];
%             v_tmp_a = inv([v_tmp_inc_a{i}])*(-c_rette_a');
%             v_tmp_i = inv([v_tmp_inc_i{i}])*(-c_rette_i');
%             x_a = v_tmp_a(1);
%             y_a = v_tmp_a(2);
%             x_i = v_tmp_i(1);
%             y_i = v_tmp_i(2);
%             axis([-10 11 -10 11]);
%             grid on;
%             hold on;
%             plot(x_a, y_a,'o','MarkerSize',10);
%             plot(x_i, y_i,'o','MarkerSize',10);
%         end
%         
%         if j <= m/2
%             a_tmp = [a{j}];
%             b_tmp = [b{j}];
%             c_tmp = [c{j}];
%             
%             a_tmp1 = [a{j+2}];
%             b_tmp1 = [b{j+2}];
%             c_tmp1 = [c{j+2}];
%             for i = 1 : length(a_tmp)
%                 v{i}  = [a_tmp(i) b_tmp(i);a_tmp1(i) b_tmp1(i)];
%                 c_rette = [c_tmp(i) c_tmp1(i)];
%                 v_tmp = inv([v{i}])*(-c_rette');
%                 x = v_tmp(1);
%                 y = v_tmp(2);
%                 %axis([-10 11 -10 11]);
%                 grid on;
%                 hold on;
%                 plot(x, y,'o','MarkerSize',10);
%             end
%             for i = 1 : (length(a_tmp)-1)
%                 v_tmp_inc_a{i} = [a_tmp(i) b_tmp(i);a_tmp1(i+1) b_tmp1(i+1)];
%                 v_tmp_inc_i{i} = [a_tmp(i+1) b_tmp(i+1);a_tmp1(i) b_tmp1(i)];
%                 c_rette_a = [c_tmp(i) c_tmp1(i+1)];
%                 c_rette_i = [c_tmp(i+1) c_tmp1(i)];
%                 v_tmp_a = inv([v_tmp_inc_a{i}])*(-c_rette_a');
%                 v_tmp_i = inv([v_tmp_inc_i{i}])*(-c_rette_i');
%                 x_a = v_tmp_a(1);
%                 y_a = v_tmp_a(2);
%                 x_i = v_tmp_i(1);
%                 y_i = v_tmp_i(2);
%                 axis([-10 11 -10 11]);
%                 grid on;
%                 hold on;
%                 plot(x_a, y_a,'o','MarkerSize',10);
%                 plot(x_i, y_i,'o','MarkerSize',10);
%             end
%         end
%     end
% end


