%--------------------------------------------------------- variable init --
% -- images
% perspective image load
im = imread('.\perspcam_persp.png');
[Xi,Yi,Zi] = size(im);

% Bird-eye image definition
Xbe = 501;
Ybe = 501;
gridplot=1;

% -- math
% camera location and orientation
c = [0 70 -330]';       % 70 'pixels' high and 330 'pixels' back of 0,0,0
theta = [-pi/8 0 0]';   % a 22.5 degree angle downwards

% viewer location w.r.t projection plane (defines image resolution/zoom)
e = [0,0,550];

%----------------------------------------------------------- rotation mat -
rotMat1 = [1 0 0; ...
    0 cos(-theta(1)) sin(-theta(1)); ...
    0 -sin(-theta(1)) cos(-theta(1))];

rotMat2 = [cos(-theta(2)) 0 -sin(-theta(2)); ...
    0 1 0; ...
    sin(-theta(2)) 0 cos(-theta(2))];

rotMat3 = [ cos(-theta(3)) sin(-theta(3)) 0; ...
    -sin(-theta(3)) cos(-theta(3)) 0; ...
    0 0 1];

RM = rotMat1*rotMat2*rotMat3;


%-------------------------------------------------------- unwrMat create --
%unwrMat init
unwrMat = ones(Ybe,Xbe,3);

for xbe = 1:Xbe
    for ybe = 1:Ybe

        % world position x,y,z
        a = [(xbe-Xbe/2) 0 (Ybe/2-ybe)]'; 

        % position relative to camera world position
        a_c = (a-c);   
        
        % position relative to camera orientation
%         d = RM * a_cam;  
        
        d(1) = a_c(1)*(cos(theta(2))*cos(theta(3))) + a_c(2)*(-cos(theta(2))*sin(theta(3))) + a_c(3)*(sin(theta(2)));
        d(2) = a_c(1)*(sin(theta(1))*sin(theta(2))*cos(theta(3))+cos(theta(1))*sin(theta(3))) + a_c(2)*(-sin(theta(1))*sin(theta(2))*sin(theta(3))+cos(theta(1))*cos(theta(3))) + a_c(3)*(-sin(theta(1))*cos(theta(2)));
        d(3) = a_c(1)*(-cos(theta(1))*sin(theta(2))*cos(theta(3))+sin(theta(1))*sin(theta(3))) + a_c(2)*(-cos(theta(1))*sin(theta(2))*cos(theta(3))+sin(theta(1))*sin(theta(3))) + a_c(3)*(cos(theta(1))*cos(theta(2)));

        % x,y position in perspective view
        x = round(-(d(2)-e(2)) * (e(3)/d(3)) + Xi/2);
        y = round((d(1)-e(1)) * (e(3)/d(3))  + Yi/2);

        if x>1 && x<Xi
            if y>1 && y<Yi
                % unwrMat creation
                unwrMat(ybe,xbe,1) = sub2ind([Xi,Yi,3],x,y,1);
                unwrMat(ybe,xbe,2) = unwrMat(ybe,xbe,1)+Xi*Yi;
                unwrMat(ybe,xbe,3) = unwrMat(ybe,xbe,2)+Xi*Yi;
            end
        end
    end
end


%---------------------------------------------------------------- plot ----
figure;imshow(im(unwrMat));
if gridplot
    hold on;
    a = [-5  5 5 -5 -5 5  5 -5 -5  5  5 -5 -5 -3 -3 -1 -1  1  1  3  3  5  5;
        0  0 0  0  0 0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0  0;
        5  5 3  3  1 1 -1 -1 -3 -3 -5 -5  5  5 -5 -5  5  5 -5 -5  5  5 -5]/5;
    a = a *Xbe/2;

    plot(a(3,:)+Ybe/2,a(1,:)+Xbe/2,'-');
    hold off;
end

figure;imshow(im);
if gridplot
    hold on;
    for index =1:size(a,2)
        d = RM *(a(:,index)-c);
        xx(index) = round(-(d(2)-e(2)) * (e(3)/d(3)) + Xi/2);
        yy(index) = round((d(1)-e(1)) * (e(3)/d(3))  + Yi/2);
    end
    plot(yy,xx,'-');axis equal;
    hold off;
end