%% Definição do tamanho do espaço e do tamanho do carro

%Espaço envolvente
SpaceSize=5000;         
mainImage= uint8(255*ones(SpaceSize,SpaceSize,3)); 

imshow(mainImage);

% %Atlas 
AtlasWidth=1400;
AtlasLength=3500;
CamerasWidth=1290;
CamerasLength=1400;

initPxX= (SpaceSize-AtlasWidth)/2;
initPxY= (SpaceSize-AtlasLength)/2;
CamPxX=initPxX;
CamPxY=initPxY+1400;

mainImage(initPxY:initPxY+AtlasLength,initPxX:initPxX+AtlasWidth,1:3)=125;
mainImage(CamPxY:CamPxY+CamerasLength,initPxX:initPxX+AtlasWidth,1:2)=55;


imshow(mainImage);


%% Definição dos parâmetros das câmaras

    prompt=sprintf('Insere o vetor das posições em X da câmara %d (entre %d e %d): ',i,initPxX,initPxX+1500);
    x= input(prompt); %V1[2500,2500,1900,3100,1900,3100] V2 [2500,2500,2175,2825,2175,2825] V3 [2500,2500,2500,2500,2500,2500]
    prompt=sprintf('Insere o vetor das posições em Y da câmara %d (entre %d e %d): ',i,CamPxY,CamPxY+CamerasLength-50);
    y= input(prompt); %V1[2200,3500,2600,2600,3050,3050] V2 [2200,3500,2600,2600,3050,3050] V3[2850,2850,2850,2850,2850,2850]
    prompt=sprintf('Insere o vetor dos ângulo da câmara %d: ',i);
    ang= input(prompt); %V1[90,270,180,0,180,0] %V2[90,270,180,0,180,0] %V3[90,270,180,0,180,0]
    
    for i=1:length(x)
        mainImage(y(i)-50:y(i)+50,x(i)-50:x(i)+50,3)=255;
    end
    imshow(mainImage);
    

for i=1:length(x)

    
    if ang(i)==0
        hold on; plot([x(i),5000],[y(i),y(i)+tan((30*pi)/180)*(5000-x(i))], 'b');
        hold on; plot([x(i),5000],[y(i),y(i)-tan((30*pi)/180)*(5000-x(i))], 'b');
    elseif ang(i)==90
        hold on; plot([x(i)+tan((30*pi)/180)*y(i),x(i)],[1,y(i)], 'b');
        hold on; plot([x(i)-tan((30*pi)/180)*y(i),x(i)],[1,y(i)], 'b');
    elseif ang(i)==180 
        hold on; plot([x(i),1],[y(i),y(i)+tan((30*pi)/180)*(x(i))], 'b');
        hold on; plot([x(i),1],[y(i),y(i)-tan((30*pi)/180)*(x(i))], 'b');
    elseif ang(i)==270
        hold on; plot([x(i)+tan((30*pi)/180)*(5000-y(i)),x(i)],[5000,y(i)], 'r');
        hold on; plot([x(i)-tan((30*pi)/180)*(5000-y(i)),x(i)],[5000,y(i)], 'r');
        
        i
%         x(i)=x(i)-tan((30*pi)/180)*(5000-y(i))
%         y(i)=5000
        theta = 90; % to rotate 90 counterclockwise
        R = [cosd(theta) -sind(theta); sind(theta) cosd(theta)];
        % Rotate your point(s)
        point = [2500 5000]'; % arbitrarily selected
        rotpoint = R*point;
        
        rotpoint(1)
        rotpoint(2)
        
        hold on; plot([x(i),rotpoint(1)],[y(i),rotpoint(2)], 'r');
        
        
        
    else
        
        %error('O ângulo não é válido!')
        hold on; plot([x(i)+tan((30*pi)/180)*(5000-y(i)),x(i)],[5000,y(i)], 'b');
        hold on; plot([x(i)-tan((30*pi)/180)*(5000-y(i)),x(i)],[5000,y(i)], 'b');
    end

end


