close all; clear;
im = imread('gmuSquare4.JPG', 'jpg');
im0 = double(rgb2gray(im));

im = imread('gmuSquare3.JPG', 'jpg');
im1 = double(rgb2gray(im));

% load matches
load x0;
load x1;
npoints = size(x0,2);

% make composite image to visualize matches
figure;
im2 = [im1 im0];
[ydim, xdim] = size(im0);
imagesc(im2); colormap gray; hold on; axis image;
% plot matches
plot(x1(2,:), x1(1,:),'.');
plot(x0(2,:)+xdim, x0(1,:),'.');
for i=1:npoints
  line([x1(2,i) x0(2,i)+xdim],[x1(1,i), x0(1,i)]);
end;

% compute homography between the two images
%%%%%%%%%%%%%%%% your code goes here 

H = homography(x0, x1);

% warp the im1 to im0 - generate extended view where 
% for each pixel in image1 you compute the coordinates
% in im0
im10 = HwarpTwo2One(H, im1, im0);


% focal length
f = 900; 

% intrinsic camera matrix
K = [f 0 ydim/2;
     0 f xdim/2;
     0   0   1];



%

% compute the rotation between the two views
% using H and K.
% Since H can be recovered only upto scale 
% fix the scale to make the columns of the rotation 
% matrix are of unit norm (as close as possible)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%% your code goes here%%%%%%%%%%%%%%%%%%%%%%



R = (K^-1)*H*K;


c = [1 1 1]/([1 1 1]*R);
R = c*R;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

figure;
imagesc(im10); colormap gray; axis off; axis image;
title('image stich')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% generate cylindrical coordinates 

[xc, yc] = meshgrid(1:xdim, 1:ydim);

%negx = -500;
%negy = 0;
%[xc, yc] = meshgrid(1-negx:xdim-negx, 1-negy:ydim-negy);

zc = ones(size(xc))*f;


sam = randsample(min(size(im0,1),size(xc,1))*min(size(im0,2),size(xc,2)),10000);

xyz0 = R\[yc(:)';xc(:)';zc(:)'];
xyz1 = [yc(:)';xc(:)';zc(:)'];



xyzhat0(1,:) = xyz0(1,:)./sqrt(xyz0(2,:).^2 + xyz0(3,:).^2 );
xyzhat0(2,:) = xyz0(2,:)./sqrt(xyz0(2,:).^2 + xyz0(3,:).^2 );
xyzhat0(3,:) = xyz0(3,:)./sqrt(xyz0(2,:).^2 + xyz0(3,:).^2 );

xyzhat1(1,:) = xyz1(1,:)./sqrt(xyz1(2,:).^2 + xyz1(3,:).^2 );
xyzhat1(2,:) = xyz1(2,:)./sqrt(xyz1(2,:).^2 + xyz1(3,:).^2 );
xyzhat1(3,:) = xyz1(3,:)./sqrt(xyz1(2,:).^2 + xyz1(3,:).^2 );



%draw_rots;
%keyboard
%figure; colormap('gray');
%hold on
%scatter3(xyzhat0(1,sam),xyzhat0(2,sam),xyzhat0(3,sam),3,im0(sam))
%scatter3(xyzhat1(1,sam),xyzhat1(2,sam),xyzhat1(3,sam),3,im1(sam))
%hold off
%keyboard

theta0 = atan2(xyzhat0(3,:),xyzhat0(2,:));
h0 = xyzhat0(1,:);
theta1 = atan2(xyzhat1(3,:),xyzhat1(2,:));
h1 = xyzhat1(1,:);

tcent = mean([theta0,theta1]);
hcent = mean([h0,h1]);


xp0 = f*(theta0 -tcent);
yp0 = f*(h0 - hcent);
xp1 = f*(theta1 - tcent);
yp1 = f*(h1 - hcent);


xhat0 = sin(theta0-tcent);
yhat0 = h0 -hcent;
zhat0 = cos(theta0-tcent);
xhat1 = sin(theta1-tcent);
yhat1 = h1 - hcent;
zhat1 = cos(theta1-tcent);


%figure; colormap('gray');
%hold on
%scatter3(xhat0(sam),yhat0(sam),zhat0(sam),3,im0(sam))
%scatter3(xhat1(sam),yhat1(sam),zhat1(sam),3,im1(sam))
%scatter3(sin(0:pi/4:2*pi),zeros(1,9),cos(0:pi/4:2*pi),1)
%hold off
%keyboard


xi0 = f*xhat0./zhat0 + 320;
yi0 = f*yhat0./zhat0 + 240;
xi1 = f*xhat1./zhat1 + 320;
yi1 = f*yhat1./zhat1 + 240;

xim0 = reshape(xi0,[ydim,xdim]);
yim0 = reshape(yi0,[ydim,xdim]);
xim1 = reshape(xi1,[ydim,xdim]);
yim1 = reshape(yi1,[ydim,xdim]);



% for each pixels on the final cylindrical 
% image compute the pixel in the real image
% where that pixel reprojects to xim, yim
% are the pixel grids





% do backward warping - interpolate on the grid
im_cylinder0 = interp2(double(im0),xim0,yim0,'*bilinear');
im_cylinder1 = interp2(double(im1),xim1,yim1,'*bilinear');


%im_cylinder = interp2(double(im0),xim,yim,'*bilinear'); 
figure; imagesc(im_cylinder0); title('cylindrical projection 0');
colormap gray;
figure; imagesc(im_cylinder1); title('cylindrical projection 1');
colormap gray;



