%% Load parameters
clear all;close all;clc

% load('data/xb3_left_right_stereo_calibration_1.mat')
load('data/xb3_left_right_stereo_calibration_p2_1.mat')
leftRightParameters = parameters;

load('data/xb3_left_center_stereo_calibration_p2_1.mat')
leftCenterParameters = parameters;

load('data/xb3_center_right_stereo_calibration_p2_1.mat')
centerRightParameters = parameters;

parpool(8)
%delete(gdf)

%% Rectify short 1

close all; clc

shortLeftLoader = ImageLoader('/home/data/rosado_1_full_raw/left');
shortRightLoader = ImageLoader('/home/data/rosado_1_full_raw/center');
shortLeftRectifiedPath = '/home/data/matlab_rosado_full_p2_1/short_left_1';
shortRightRectifiedPath = '/home/data/matlab_rosado_full_p2_1/short_right_1';

shortParameters = leftCenterParameters;

parfor i=1:length(shortLeftLoader.ids)
    
    %Just check if already processed and jump
    leftFileName = sprintf('%06d.png',shortLeftLoader.ids(i));
    
    if exist(fullfile(shortLeftRectifiedPath,leftFileName), 'file')
        disp(['Image: ' fullfile(shortLeftRectifiedPath,leftFileName) ' exists'])
        continue
    end
    
    disp(['Processing: ' fullfile(shortLeftRectifiedPath,leftFileName)])
    
    left = shortLeftLoader.loadById(shortLeftLoader.ids(i));
    right = shortRightLoader.loadById(shortRightLoader.ids(i));
    
    [leftRect, rightRect] = rectifyStereoImages(left,right,shortParameters,'cubic','OutputView', 'valid');
    
    % Save images to disk
    leftFileName = sprintf('%06d.png',shortLeftLoader.ids(i));
    imwrite(leftRect,fullfile(shortLeftRectifiedPath,leftFileName),'PNG');
    
    rightFileName = sprintf('%06d.png',shortRightLoader.ids(i));
    imwrite(rightRect,fullfile(shortRightRectifiedPath,rightFileName),'PNG');
end

%% Rectify short 2

close all; clc

shortLeftLoader = ImageLoader('/home/data/rosado_1_full_raw/center');
shortRightLoader = ImageLoader('/home/data/rosado_1_full_raw/right');
shortLeftRectifiedPath = '/home/data/matlab_rosado_full_p2_1/short_left_2';
shortRightRectifiedPath = '/home/data/matlab_rosado_full_p2_1/short_right_2';

shortParameters = centerRightParameters;

parfor i=1:length(shortLeftLoader.ids)
    
    %Just check if already processed and jump
    leftFileName = sprintf('%06d.png',shortLeftLoader.ids(i));
    
    if exist(fullfile(shortLeftRectifiedPath,leftFileName), 'file')
        disp(['Image: ' fullfile(shortLeftRectifiedPath,leftFileName) ' exists'])
        continue
    end
    
    disp(['Processing: ' fullfile(shortLeftRectifiedPath,leftFileName)])
    
    left = shortLeftLoader.loadById(shortLeftLoader.ids(i));
    right = shortRightLoader.loadById(shortRightLoader.ids(i));
    
    [leftRect, rightRect] = rectifyStereoImages(left,right,shortParameters,'cubic','OutputView', 'valid');
    
    % Save images to disk
    leftFileName = sprintf('%06d.png',shortLeftLoader.ids(i));
    imwrite(leftRect,fullfile(shortLeftRectifiedPath,leftFileName),'PNG');
    
    rightFileName = sprintf('%06d.png',shortRightLoader.ids(i));
    imwrite(rightRect,fullfile(shortRightRectifiedPath,rightFileName),'PNG');
end

%% Rectify wide

close all; clc

wideLeftLoader = ImageLoader('/home/data/rosado_1_full_raw/left');
wideRightLoader = ImageLoader('/home/data/rosado_1_full_raw/right');
wideLeftRectifiedPath = '/home/data/matlab_rosado_full_p2_1/wide_left';
wideRightRectifiedPath = '/home/data/matlab_rosado_full_p2_1/wide_right';

wideParameters = leftRightParameters;

parfor i=1:length(wideLeftLoader.ids)

    %Just check if already processed and jump
    leftFileName = sprintf('%06d.png',wideLeftLoader.ids(i));
    
    if exist(fullfile(wideLeftRectifiedPath,leftFileName), 'file')
        disp(['Image: ' fullfile(wideLeftRectifiedPath,leftFileName) ' exists'])
        continue
    end
    
    disp(['Processing: ' fullfile(wideLeftRectifiedPath,leftFileName)])
    
    left = wideLeftLoader.loadById(wideLeftLoader.ids(i));
    right = wideRightLoader.loadById(wideRightLoader.ids(i));
    
    [leftRect, rightRect] = rectifyStereoImages(left,right,wideParameters,'cubic','OutputView', 'valid');
    
    % Save images to disk
    leftFileName = sprintf('%06d.png',wideLeftLoader.ids(i));
    imwrite(leftRect,fullfile(wideLeftRectifiedPath,leftFileName),'PNG');
    
    rightFileName = sprintf('%06d.png',wideRightLoader.ids(i));
    imwrite(rightRect,fullfile(wideRightRectifiedPath,rightFileName),'PNG');
end

%% Rectify wide calibration patterns

close all; clc

wideLeftLoader = ImageLoader('/home/data/matlab_stereo_calibration_2/left');
wideRightLoader = ImageLoader('/home/data/matlab_stereo_calibration_2/right');
wideLeftRectifiedPath = '/home/data/matlab_stereo_calibration_2_rectified/wide_left';
wideRightRectifiedPath = '/home/data/matlab_stereo_calibration_2_rectified/wide_right';

wideParameters = leftRightParameters;

parfor i=1:length(wideLeftLoader.ids)
    left = wideLeftLoader.loadById(wideLeftLoader.ids(i));
    right = wideRightLoader.loadById(wideRightLoader.ids(i));
    
    [leftRect, rightRect] = rectifyStereoImages(left,right,wideParameters,'cubic','OutputView', 'valid');
    
    % Save images to disk
    leftFileName = sprintf('%06d.png',wideLeftLoader.ids(i));
    imwrite(leftRect,fullfile(wideLeftRectifiedPath,leftFileName),'PNG');
    
    rightFileName = sprintf('%06d.png',wideRightLoader.ids(i));
    imwrite(rightRect,fullfile(wideRightRectifiedPath,rightFileName),'PNG');
end
