clc
close all
clear 


f = pwd;
cd(erase(mfilename('fullpath'),extractAfter(mfilename('fullpath'),'matlab_code'))); % make sure we are in ../matlab_code
addpath(genpath(pwd)); % add subfolders to path to search for functions
cd(f);

imSize = [360,640,3];
stepXY = [10,10];

m = ImgProcess(2,1);
mls = MLSdeformation(imSize,stepXY,'rigid');

% get image located in different (known) directory
x = erase(mfilename('fullpath'),mfilename);
testImg = imread(fullfile(x,'test','testImg.jpg'));
testImg = ~imbinarize(testImg)*256;
testImg = m.testDots(imSize,[25,25],5);

%% simulate projected image deformation
load('test/fakeDeform3')
mls.mlsPrecomupte(p)
mls.mlsSimilarity(q)
testImg2 = uint8(zeros(imSize));
testImg2 = mls.interpolate1(testImg,testImg2);
clearfield0 = mls.interpolate1(uint8(ones(imSize)*256), uint8(zeros(imSize)));

%% Simulate perspective distortion caused by camera offset from projected image
% Using: https://www.mathworks.com/matlabcentral/answers/166497-create-a-projective-homography-matrix-with-pitch-roll-yaw
yaw = 0.02; pitch = 0.05; roll = -0.05; 
%camPos = [size(testImg,2)/2; 0; -200]; % camera position along xaxis,yaxis,z-axis
camPos = [-150;-80; -200];
F = 100; % focal length
 
[im1, im2] = m.changePerspective(clearfield0,yaw,pitch,roll,camPos,F);

subplot(1,3,1);imshow(testImg2);
subplot(1,3,2);imshow(im2);
% detect corners
hold on
corners  = detectHarrisFeatures(imgaussfilt(rgb2gray(im2)));
%%
%  bb = m.detectBorder(rgb2gray(im2));
% centroid = fliplr(mean(bb{:}));
% 
% %%
% 
% 
% figure;imshow(im2);hold on
% plot(corners.Location(:,1),corners.Location(:,2),'g*')
% plot(centroid(1),centroid(2),'*r')
% 
% [x1,y1] = poly2cw(corners.Location(:,1),corners.Location(:,2));
% 
% p = [[x1,y1]-centroid];
% ss = atan(p(:,1)./p(:,2));

 corners = corners.selectStrongest(4);
 %corners(3) = [];
 
plot(corners)

% repair camera distortion
p0 = [imSize(2),imSize(1); 1,1; 1,imSize(1); imSize(2),1];
% Convert polygon contour to clockwise vertex ordering
[x1,y1] = poly2cw(corners.Location(:,1),corners.Location(:,2));
[x2,y2] = poly2cw(p0(:,1),p0(:,2));

tform = fitgeotrans([x1,y1],[x2,y2],'projective');

outputImage1 = imwarp(clearfield0,tform);
outputImage2 = imwarp(testImg2,tform);
subplot(1,3,3);imshow(outputImage2);



%% simulate noise
[darkfield,clearfield] = simulateNoise(50,0.005,70,0.001,imSize);
clearfield = (double(rgb2gray(outputImage1)) + clearfield) / (max(max(max(double(rgb2gray(outputImage1)) + clearfield)))/256);
% (clearfield - darkfield) = jasove zkresleni projektoru (+detektoru)
% darkfield = pozadí
testImg3 = double(rgb2gray(outputImage2)).*(clearfield - darkfield) + darkfield;


%% fix brigthness of image 
alpha = 1;
testImg4 = (double(testImg3)-darkfield)./(clearfield - darkfield).*alpha;

imshow(testImg3,[]);
figure;
imshow(testImg4,[])

[centers, radii, metric] =imfindcircles(testImg4,[1 10]);
hold on
plot(centers(:,1),centers(:,2), 'r+', 'MarkerSize', 5, 'LineWidth', 2);
%imshow(img1,[])

bb = m.detectBorder(clearfield);
border_points = bb{:};
[centroid,scale] = fitBiggestRectangle(imSize,border_points);
plot(border_points(:,2),border_points(:,1),'g.')

