%%
clc, clear, close all

frame_rate = 30;

video_name = '14-36-04_still_30fps_lowres.mp4';
time = clock;
out_name = ['video_out_' num2str(time(4)) '-' num2str(time(5)) '-' num2str(time(6))];
cutout_name = ['section_' num2str(time(4)) '-' num2str(time(5)) '-' num2str(time(6))];
cutout_video = VideoWriter(cutout_name);
txt_out = ['forehead_' num2str(time(4)) '-' num2str(time(5)) '-' num2str(time(6)) '.txt'];
video = VideoReader(video_name);
out_video = VideoWriter(out_name);

cutout_video.FrameRate = frame_rate;
out_video.FrameRate = frame_rate;
open(out_video);
open(cutout_video);

% Get the frame from the video
iFrame = readFrame(video);
iFrame = readFrame(video);
iFrame = readFrame(video);
iFrame = readFrame(video);

% imshow(iFrame)
video.NumFrames

%% Detect faces
faceDetector = vision.CascadeObjectDetector();

faceLocation = step(faceDetector, iFrame);
faceLocation = faceLocation(1,:);
% iFrame = insertShape(iFrame, 'Rectangle', faceLocation);
% figure; imshow(iFrame); title('Detected face');

rectanglePoints = bbox2points(faceLocation(1,:));
forehead = [(max(rectanglePoints(:,1))+min(rectanglePoints(:,1)))/2, min(rectanglePoints(:,2))+(max(rectanglePoints(:,2))-min(rectanglePoints(:,2)))/5];
forehead_pos = forehead;
featurePoints = detectMinEigenFeatures(rgb2gray(iFrame), 'ROI', faceLocation(1,:));

pointTracker = vision.PointTracker('MaxBidirectionalError', 2);

featurePoints = featurePoints.Location;

initialize(pointTracker, featurePoints, iFrame);

left = 100;
bottom = 100;
width = size(iFrame, 2);
height = size(iFrame, 1);
videoPlayer = vision.VideoPlayer('Position', [left bottom width height]);

prevPoints = featurePoints;

while hasFrame(video)

    iFrame = readFrame(video);
    iFrame2 = iFrame;
    [featurePoints, isFound] = step(pointTracker, iFrame);
    newPoints = featurePoints(isFound,:);
    oldPoints = prevPoints(isFound,:);

    if size(newPoints, 1) >= 2
        [transRect, oldPoints, newPoints] = estimateGeometricTransform(oldPoints, newPoints, 'similarity', 'MaxDistance', 4);
        rectanglePoints = transformPointsForward(transRect, rectanglePoints);

        %% Calculating the point to be measured
        r1 = rectanglePoints(1,:);      % Get the top two corners of the rectangle
        r2 = rectanglePoints(2,:);
        r3 = rectanglePoints(3,:);
        dist_x = norm(r1-r2);           % Lenght of the horizontal line
        dist_y = norm(r2-r3);           % Lenght of the vertical line

        x = dist_x/2;           % Move half way between r1 and r2
        y = dist_y/5;           % Move 1/5 down the y coordinate
        r11 = [x-20;y-20];
        r22 = [x-20;y+20];
        r33 = [x+20;y+20];
        r44 = [x+20;y-20];

        alpha = atan(abs(r2(2)-r1(2))/(abs(r2(1)-r1(1))));  % Get the angle by which we rotate the point
        forehead1 = [cos(alpha)  -sin(alpha)
                    sin(alpha) cos(alpha)] * r11 + r1';
        forehead2 = [cos(alpha)  -sin(alpha)
                    sin(alpha) cos(alpha)] * r22 + r1';
        forehead3 = [cos(alpha)  -sin(alpha)
                    sin(alpha) cos(alpha)] * r33 + r1';
        forehead4 = [cos(alpha)  -sin(alpha)
                    sin(alpha) cos(alpha)] * r44 + r1';

        newPoints = [newPoints; forehead1'; forehead2'; forehead3'; forehead4' ];
        forehead_pos = [forehead_pos; forehead1'; forehead2'; forehead3'; forehead4'];

        reshapedRect = reshape(rectanglePoints', 1, []);
        iFrame = insertShape(iFrame, 'Polygon', reshapedRect, 'LineWidth', 1);
    
        iFrame = insertMarker(iFrame, newPoints, '+', 'Color', 'White');
        
        prevPoints = newPoints;
        setPoints(pointTracker, prevPoints);
    end
   %% Create a cutout
    outFrame = uint8(zeros(20,20,3));
    % Get the angle
    alpha = atan(abs(forehead1(2)-forehead4(2))/(abs(forehead1(1)-forehead4(1))));  % Get the angle by which we rotate the point
    beta = pi/2-alpha;  % Downward angle, because we're cutting out a square

    for ycoord = 1:21
        pivot = [cos(alpha)  -sin(alpha)
                 sin(alpha) cos(alpha)] * [0; -ycoord-1] + forehead1;
        for xcoord = 1:21
            % Calculate position of the pixels to be cut out
            pixel = [cos(alpha)  -sin(alpha)
                 sin(alpha) cos(alpha)] * [xcoord-1; 0] + pivot;

            outFrame(ycoord,xcoord,:) = iFrame2(round(pixel(2)),round(pixel(1)),:);
        end
    end
    writeVideo(cutout_video,outFrame)
    
    %% Show player and save frame with points
    step(videoPlayer, iFrame);
    writeVideo(out_video,iFrame)
end
close(out_video);
close(cutout_video);
writematrix(forehead_pos, txt_out);
release(videoPlayer);
release(faceDetector);
release(pointTracker);



