% initial file open
fid = fopen('hdmi/scene3-640x480-60-425M-64M-40M.dat', 'r', 'ieee-le');
iq = fread(fid, [2, inf], 'single=>single');  fclose(fid);
scene3_425 = complex(iq(1,:), iq(2,:));

% image size constants
x_d = 640;             % image width (display)
y_d = 480;             % image height (display)

x_t = 800;             % image width (total)
y_t = 525;             % image height (total)

% frequencies
f_c = 425000000;       % center frequency

f_s = 64000000;        % sample frequency
f_p = 25175000;        % pixel frequency

f_h = round(f_p/x_t);  % line rate
f_v = round(f_h/y_t);  % frame rate

% §Image assuming perfect frequency
% interpolate
scene3_425_int = interp1(linspace(0,1,f_s),scene3_425,linspace(0,1,f_p));
% reshape
img = reshape(scene3_425_int(1:x_t*y_t), [x_t, y_t]);
% save
imwrite(rescale(abs(img')), "output/initial.png");

% §Manual shear correction
% frequencies
f_s = 64000000;        % sample frequency
f_p = 25200000;        % pixel frequency
f_h = round(f_p/x_t);  % line rate
f_v = round(f_h/y_t);  % frame rate
% interpolate and reshape
scene3_425_int = interp1(linspace(0,1,f_s),scene3_425,linspace(0,1,f_p));
img = reshape(scene3_425_int(1:x_t*y_t), [x_t, y_t]);
% save
imwrite(rescale(abs(img')), "output/manual_shear_correction.png");

% §Manual placement correction
% offsets
off_cols = 447;
off_rows = 259;

off_pixels = off_rows*x_t+off_cols;
frame_pixels = x_t*y_t;

% interpolate and reshape
scene3_425_int = interp1(linspace(0,1,f_s),scene3_425,linspace(0,1,f_p));
img = reshape(scene3_425_int(1+off_pixels:off_pixels+frame_pixels), [x_t, y_t]);
% save
imwrite(rescale(abs(img')), "output/manual_placement_correction.png");

% §Manual shear correction
f_p = 25200000;        % pixel frequency
num_frames = 59;

% interpolate and reshape
scene3_425_int = interp1(linspace(0,1,f_s),scene3_425,linspace(0,1,f_p));
img_frames = reshape(scene3_425_int(1+off_pixels:off_pixels+frame_pixels*num_frames), [x_t, y_t, num_frames]);

img_0_8 = [img_frames(1:end,1:y_d/2,1) img_frames(1:end,y_d/2+1:end,9)];
imwrite(rescale(abs(img_0_8')), "output/img_0_8_25200.png");

img_0_16 = [img_frames(1:end,1:y_d/2,1) img_frames(1:end,y_d/2+1:end,17)];
imwrite(rescale(abs(img_0_16')), "output/img_0_16_25200.png");

img_0_32 = [img_frames(1:end,1:y_d/2,1) img_frames(1:end,y_d/2+1:end,33)];
imwrite(rescale(abs(img_0_32')), "output/img_0_32_25200.png");

img_0_58 = [img_frames(1:end,1:y_d/2,1) img_frames(1:end,y_d/2+1:end,59)];
imwrite(rescale(abs(img_0_58')), "output/img_0_58_25200.png");

% new frequency
f_p = 25200097;

f_h = round(f_p/x_t);  % line rate
f_v = round(f_h/y_t);  % frame rate

% interpolate and reshape
scene3_425_int = interp1(linspace(0,1,f_s),scene3_425,linspace(0,1,f_p));
img_frames = reshape(scene3_425_int(1+off_pixels:off_pixels+frame_pixels*num_frames), [x_t, y_t, num_frames]);

img_0_58 = [img_frames(1:end,1:y_d/2,1) img_frames(1:end,y_d/2+1:end,59)];
imwrite(rescale(abs(img_0_58')), "output/img_0_58_new.png");

% §Automatic frequency identification
% autocorrelation

[autocor,lags] = xcorr(scene3_425, ceil(f_s/20000), 'coeff');
[pks,lag_locs] = findpeaks(real(autocor),'MinPeakProminence',0.95);

f_h = f_s/lags(lag_locs(end));
f_p = round(f_h*x_t);

scene3_425_int = interp1(linspace(0,1,f_s),scene3_425,linspace(0,1,f_p));
img_frames = reshape(scene3_425_int(1+off_pixels:off_pixels+frame_pixels*num_frames), [x_t, y_t, num_frames]);

img_0_58_auto_fh = [img_frames(1:end,1:y_d/2,1) img_frames(1:end,y_d/2+1:end,59)];
imwrite(rescale(abs(img_0_58_auto_fh')), "output/img_0_58_auto_fh.png");

[autocor,lags] = xcorr(scene3_425, ceil(f_s/30), 'coeff');
[pks,lag_locs] = findpeaks(real(autocor),'MinPeakProminence',0.95);

f_v = f_s/lags(lag_locs(end));
f_p = round(f_v*x_t*y_t);

scene3_425_int = interp1(linspace(0,1,f_s),scene3_425,linspace(0,1,f_p));
img_frames = reshape(scene3_425_int(1+off_pixels:off_pixels+frame_pixels*num_frames), [x_t, y_t, num_frames]);

img_0_58_auto_fv = [img_frames(1:end,1:y_d/2,1) img_frames(1:end,y_d/2+1:end,59)];
imwrite(rescale(abs(img_0_58_auto_fv')), "output/img_0_58_auto_fv.png");

% reset to the manual values
f_p = 25200097;

f_h = round(f_p/x_t);  % line rate
f_v = round(f_h/y_t);  % frame rate

scene3_425_int = interp1(linspace(0,1,f_s),scene3_425,linspace(0,1,f_p));
img_frames = reshape(scene3_425_int(1+off_pixels:off_pixels+frame_pixels*num_frames), [x_t, y_t, num_frames]);

% §Averaging frames
img_super_frame = mean(abs(img_frames), 3);
imwrite(rescale(img_super_frame'), "output/img_super_frame.png");

% §Using phase data
hsv_frames = rescale(angle(img_frames));
hsv_frames(:,:,:,2) = repmat(0.5, x_t, y_t, num_frames);
hsv_frames(:,:,:,3) = rescale(abs(img_frames));

hsv_img = squeeze(hsv_frames(:,:,1,:));
img_phase_0_hsv = hsv2rgb(hsv_img);
imwrite(permute(img_phase_0_hsv,[2,1,3]), "output/img_phase_0_hsv.png");

hsv_img = squeeze(hsv_frames(:,:,59,:));
img_phase_59_hsv = hsv2rgb(hsv_img);
imwrite(permute(img_phase_59_hsv,[2,1,3]), "output/img_phase_59_hsv.png");

% §Unrotating the periodic phase change

f_u = 370.875; % increase to go left, decrease to go right

t = linspace(0, 1, f_s);
scene3_425_unrotated = scene3_425 .* exp(-2i*pi*(-f_u)*t);

scene3_425_int = interp1(linspace(0,1,f_s),scene3_425_unrotated,linspace(0,1,f_p));
img_frames = reshape(scene3_425_int(1+off_pixels:off_pixels+frame_pixels*num_frames), [x_t, y_t, num_frames]);
img_frames_cropped = img_frames(1:x_d,1:y_d,:,:);

hsv_frames = rescale(angle(img_frames));
hsv_frames(:,:,:,2) = repmat(1.0, x_t, y_t, num_frames);
hsv_frames(:,:,:,3) = rescale(abs(img_frames));

hsv_img = squeeze(hsv_frames(:,:,1,:));
img_phase_0_unrotated_hsv = hsv2rgb(hsv_img);
imwrite(permute(img_phase_0_unrotated_hsv,[2,1,3]), "output/img_phase_0_unrotated_hsv.png");

hsv_img = squeeze([hsv_frames(:,1:y_d/2,1,:) hsv_frames(:,y_d/2+1:end,17,:)]);
img_phase_0_17_unrotated_hsv = hsv2rgb(hsv_img);
imwrite(permute(img_phase_0_17_unrotated_hsv,[2,1,3]), "output/img_phase_0_17_unrotated_hsv.png");

hsv_img = squeeze([hsv_frames(:,1:y_d/2,1,:) hsv_frames(:,y_d/2+1:end,33,:)]);
img_phase_0_33_unrotated_hsv = hsv2rgb(hsv_img);
imwrite(permute(img_phase_0_33_unrotated_hsv,[2,1,3]), "output/img_phase_0_33_unrotated_hsv.png");

hsv_img = squeeze([hsv_frames(:,1:y_d/2,1,:) hsv_frames(:,y_d/2+1:end,59,:)]);
img_phase_0_58_unrotated_hsv = hsv2rgb(hsv_img);
imwrite(permute(img_phase_0_58_unrotated_hsv,[2,1,3]), "output/img_phase_0_58_unrotated_hsv.png");

% §Finer periodic phase change adjustments

f_u = 3401649 - 855000;
scene3_425_unrotated = scene3_425 .* exp(-2i*pi*(-f_u)*t);

spectrogram(scene3_425_unrotated,[],[],[],f_s,'yaxis');

%scene3_425_int = interp1(linspace(0,1,f_s),scene3_425_unrotated,linspace(0,1,f_p));
%img_frames = reshape(scene3_425_int(1+off_pixels:off_pixels+frame_pixels*num_frames), [x_t, y_t, num_frames]);
%img_frames_cropped = img_frames(1:x_d,1:y_d,:,:);

% §Coherent periodic averaging

img_super_frame = mean(img_frames_cropped, 3);

hsv_super_frame = rescale(angle(img_super_frame));
hsv_super_frame(:,:,2) = repmat(0.5, x_d, y_d);
hsv_super_frame(:,:,3) = rescale(abs(img_super_frame));

img_super_frame_coherent = hsv2rgb(hsv_super_frame);

imwrite(permute(img_super_frame_coherent, [2,1,3]), "output/img_super_frame_coherent.png");

%Automatic unrotation

k = 18;
center = k*f_p-f_c;

% move the center to 0Hz and filter to 1MHz
scene3_425_cz = scene3_425 .* exp(-2i*pi*center*t);
[b,a] = butter(1, 1000000/(f_s/2));
scene3_425_lp = filter(b, a, scene3_425_cz);

% fm demodulate
dzdt = [diff(scene3_425_lp), zeros(1)];
scene3_425_fm = (dzdt./scene3_425_lp)/(2*pi*1i);