clc, clear, close all

input_file = 'forehead_16-0-52.431.txt';
video_name = '14-26-40_still.mp4';
video = VideoReader(video_name);
time = clock;
out_name = ['section_' num2str(time(4)) '-' num2str(time(5)) '-' num2str(time(6))];
out_video = VideoWriter(out_name);
open(out_video);
video_size = 16;
%%
% Skip the first frame and the first line of the matrix
coords = readmatrix(input_file);
coords(1,:) = [];
iFrame = readFrame(video);

% Loop throught the footage and cut out a 20x20 rotating square such that
% it is centered in the final video
video.NumFrames-1
for i = 1:video.NumFrames-1
    iFrame = readFrame(video);
    outFrame = uint8(zeros(20,20,3));

    corner1 = coords(1+(i-1)*4,:);
    corner2 = coords(2+(i-1)*4,:);
    corner3 = coords(3+(i-1)*4,:);
    corner4 = coords(4+(i-1)*4,:);
    
    % Calculate lenghts, should be about 20 pixels
    length_x = norm(corner1-corner4); % just a sqrt((a(1)-b(1))^2+(a(2)-b(2))^2)
    length_y = norm(corner1-corner2);
    
    % Get the angle
    alpha = atan(abs(corner1(2)-corner4(2))/(abs(corner1(1)-corner4(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] + corner1';
        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,:) = iFrame(round(pixel(2)),round(pixel(1)),:);
        end
    end
writeVideo(out_video,outFrame(1:video_size, 1:video_size, :))
i
end

close(out_video);







