%B. Vandeportaële & D. Maestro 2017
close all
clear all
%symbolic or numeric simulation
symbolic=1

%size of the mirror in x
mirrorwidth=100;
%size of the mirror in y
mirrorheigth=130;
if exist('symbolic','var')
    tcx=sym('tcx','real');
    tcy=sym('tcy','real');
    tcz=sym('tcz','real');
    rcx=sym('rcx','real');
    rcy=sym('rcy','real');
    rcz=sym('rcz','real');
    rc=[rcx,rcy,rcz]';
else
    tcx= 190;
    tcy= 85;
    tcz=200;
    %rc=[0,1,0]'*110*pi/180;
    %rc=[0,1,0]'*110*pi/180;
    
    %set the rotation matrix from axis correspondances
    Rapprox=[0 0.7 -0.7;1 0 0; 0,-0.7 -0.7];
    det(Rapprox)
    [U,S,V] =svd(Rapprox)
    R=U*V'
    rr=rodrigues(R)
    rrangle=norm(rr)
    rraxis=rr/rrangle
    rc=rr
end

tc=[tcx,tcy,tcz]';
Tmc=[rodrigues2R(rc), tc; 0,0,0,1];

if exist('symbolic','var')
    fu=sym('fu','real');
    fv=sym('fv','real');
    pu=sym('pu','real');
    pv=sym('pv','real');
else
    camera_image_width=2560;
    camera_image_height=2048;
    pu=camera_image_width/2;
    pv=camera_image_height/2;
    horizontal_fov=70*pi/180;
    vertical_fov=60*pi/180;
    fu=(camera_image_width/2)/tan(horizontal_fov/2);
    fv=(camera_image_height/2)/tan(vertical_fov/2);
end
K=[fu 0 pu 0;
    0 fu pv 0;
    0 0 1 0];

if ~exist('symbolic','var')
    mirror_corners=[0,0;mirrorwidth,0;mirrorwidth,mirrorheigth;0,mirrorheigth];
    mirror_corners_projected=zeros(size(mirror_corners));
    for n=1:size(mirror_corners,1)
        P=[    mirror_corners(n,:),0,1]';
        PP=inv(Tmc)*P
        if (PP(3)<0)
            display('the point is behind the camera');
        end
        p=K*PP;
        p=p/p(3);
        mirror_corners_projected(n,:)=p(1:2)';
    end
end


if exist('symbolic','var')
    display_pixel_size_x=sym('display_pixel_size_x','real');
    display_pixel_size_y=sym('display_pixel_size_y','real');
else
    camera_image_boundaries=[0,0;camera_image_width,0;camera_image_width,camera_image_height;0,camera_image_height];
    display_pixel_size_x=0.205;
    display_pixel_size_y=0.205;
    display_pixel_width=2560;
    display_pixel_height=1800;
    %metric unit mm
    display_metric_width=display_pixel_size_x*display_pixel_size_x;
    display_metric_height=display_pixel_size_y*display_pixel_size_y;
end
pixel2metric=[display_pixel_size_x 0 0; 0 display_pixel_size_y 0; 0 0 1];

if exist('symbolic','var')
    tdx=sym('tdx','real');
    tdy=sym('tdy','real');
    tdz=sym('tdz','real');
    rdx=sym('rdx','real');
    rdy=sym('rdy','real');
    rdz=sym('rdz','real');
    rd=[rcx,rcy,rcz]';
else
    %set the rotation matrix from axis correspondances
    %close to perfect configuration
    %Rapprox2=[0 -0.7 -0.7;1 0 0; 0,-0.7 +0.7];
    %another configuration with 3 degrees of freedom rotation
    Rapprox2=[0 -0.7 -0.5;0.7 0.2 0.1; 0,-0.7 +0.9];
    
    [U,S,V] =svd(Rapprox2);
    R2=U*V';
    rr2=rodrigues(R2);
    rrangle2=norm(rr2);
    rraxis2=rr2/rrangle2;
    rd=rr2
    
    tdx=-64;
    tdy=-220;
    tdz=530;
end

% rd=[rdx,rdy,rdz]';
td=[tdx,tdy,tdz]';

Tmd=[rodrigues2R(rd), td; 0,0,0,1];
%Tdm=inv(Tmd);

Treflection=[1 0 0 0; 0 1 0 0 ; 0 0 -1 0; 0 0 0 1];
MMM=[1 0 0; 0 1 0; 0 0 0; 0 0 1]; %form P2 to P3 in z=0 plane
if ~exist('symbolic','var')
    display_image_boundaries=[0,0;display_pixel_width,0;display_pixel_width,display_pixel_height;0,display_pixel_height];
    display_corners_projected=zeros(size(display_image_boundaries));
    for n=1:size(mirror_corners,1)
        Pd=[    display_image_boundaries(n,:),1]'
        Pdmetric=pixel2metric*Pd;
        %PdmetricP3=[Pdmetric(1:2);0;Pdmetric(3)]; %compute location in P3 for z=0 plane
        PdmetricP3=MMM*Pdmetric;
        P=Treflection*Tmd*PdmetricP3;
        PP=inv(Tmc)*P
        if (PP(3)<0)
            display('the point is behind the camera');
        end
        p=K*PP;
        p=p/p(3);
        display_corners_projected(n,:)=p(1:2)';
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%computation of the homography between display  and camera taking
%into account the reflexion
Hcd=K*inv(Tmc)*Treflection*Tmd*MMM*pixel2metric

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if exist('symbolic','var')
   jacobian(reshape(Hcd,9,1),[rd]) 
end
if exist('symbolic','var')
   jacobian(reshape(Tmd,16,1),[rd]) 
end
if exist('symbolic','var')
    %what happen if we use Tcm instead of Tmc=> other parameterization
    tcix=sym('tcix','real');
    tciy=sym('tciy','real');
    tciz=sym('tciz','real');
    rcix=sym('rcix','real');
    rciy=sym('rciy','real');
    rciz=sym('rciz','real');
    
    rci=[rcix,rciy,rciz]';
    tci=[tcix,tciy,tciz]';
    Tcm=[rodrigues2R(rci), tci; 0,0,0,1];
    Hcd=K*Tcm*Treflection*Tmd*MMM*pixel2metric
    
    J= jacobian(reshape(Hcd,9,1),[rd',td',rci',tci']) 
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%display
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if ~exist('symbolic','var')
    figure
    hold on
    axis equal
    axis ij
    display_closed_contour(camera_image_boundaries,'b-','bx')
    display_closed_contour(mirror_corners_projected,'r-','rx')
    display_closed_contour(display_corners_projected,'g-','gx')
    
    for ud=0:100:display_pixel_width-1
        for vd=0:100:display_pixel_height-1
            p=Hcd*[ud;vd;1];
            p=p/p(3);
            plot(p(1),p(2),'g+');
        end
    end
end