%% Interactive geometric and temporal calibration

clear all;close all;clc

viconData = 'data/data_in_labeling.mat';
imagesPath = '/home/data/matlab_rosado_full_p2_1/wide_left';
pointsPath = 'data/extrinsic_calibration_points.mat';
% calibrationPath = 'data/xb3_left_right_stereo_calibration_p2.mat';
calibrationPath = 'data/xb3_left_post_rectified_parameters_calibration_2_1.mat';

load(calibrationPath)

% cameraParams = parameters.CameraParameters1;

calibrationMatching = XB3ViconGeometricTemporalCalibration(viconData,imagesPath,pointsPath,cameraParams);

%% Plot temporal calibration sync points and regression

image = [calibrationMatching.temporalMap(:).image];
world = [calibrationMatching.temporalMap(:).world];

figure('position',[80 80 900 900],'color',[1 1 1],'name','Temporal registration, sync points','NumberTitle','off');

plot(image,world,'o','markersize',6,'markeredgecolor',[0 0 1],'markeredgecolor',[0 0 1])

xlabel('Image Frame')
ylabel('World frame')
grid on
hold on

p = polyfit(image,world,1);
worldFit = polyval(p,image);

plot(image,worldFit,'-r')

worldResid = world - worldFit;

SSresid = sum(worldResid.^2);
SStotal = (length(world)-1)*var(world);

rsq = 1 - SSresid/SStotal; 

legend('data',sprintf('R^2 = %.8f',rsq),'location','best')

%% Make movie with xb3 frames

writerObj = VideoWriter('data/RENAME_ME.avi','Uncompressed AVI');
set(writerObj,'FrameRate',16);
% set(writerObj,'Quality',100);
open(writerObj);

set(calibrationMatching.xb3Plotter.figureHandler,'position',[80 80 1400 1000],'color',[0.1 0.1 0.1])
% set(calibrationMatching.xb3Plotter.axisHandler,'LooseInset',get(gca,'TightInset'))
axis tight

trialLength = length(calibrationMatching.xb3Plotter.loader.ids);

frame = getframe();
frameSize = size(frame.cdata)

for i=1:trialLength
    figure(calibrationMatching.xb3Plotter.figureHandler)
    frame = getframe();
    
    frame.cdata = imresize(frame.cdata,[frameSize(1) frameSize(2)]);
    writeVideo(writerObj,frame);
    
    fprintf('Added: %d of %d (%.1f %%)\n',i,trialLength,(i/trialLength)*100)
    
    %Use callback to change frame, simulate keyboard click
    eventData.Key = 'rightarrow';
    eventData.Modifier = '';
    calibrationMatching.callbackXB3KeyPressed([],eventData);

    pause(0.001)
end

close(writerObj);

    

