classdef StereoCalibration < handle
    
    properties
        leftLoader;
        rightLoader;
        
        leftViewer;
        rightViewer;
        
        leftPath;
        rightPath;
        
        imagePoints;
        boardSize;
        imagesUsed;
        worldPoints;
        
        stereoParameters;
        pairsUsed;
        
        localScheduler;
    end
    
    methods
        function o = StereoCalibration(leftPath,rightPath)
            o.leftPath = leftPath;
            o.rightPath = rightPath;
            
            o.leftLoader = ImageLoader(o.leftPath);
            o.rightLoader = ImageLoader(o.rightPath);
            
            o.leftViewer = ImageViewer(o.leftLoader);
            o.rightViewer = ImageViewer(o.rightLoader);
            
            o.leftViewer.createFigure('name','Left Images','NumberTitle','off','menubar','none','position',[80 80 900 900]);
            set(o.leftViewer.figureHandler,'WindowKeyPressFcn',{@o.callbackButtonPressed});
            set(o.leftViewer.figureHandler,'WindowButtonDownFcn',{@o.callbackImagePressed});
            
            o.rightViewer.createFigure('name','Right Images','NumberTitle','off','menubar','none','position',[1000 80 900 900]);
            set(o.rightViewer.figureHandler,'WindowKeyPressFcn',{@o.callbackButtonPressed});
            set(o.rightViewer.figureHandler,'WindowButtonDownFcn',{@o.callbackImagePressed});
            
            o.leftViewer.draw();
            o.rightViewer.draw();

        end
        
        function createFigure(o,varargin)
            o.figureHandler = figure;
            
            set(o.figureHandler,'WindowKeyPressFcn',{@o.callbackKeyPressed});
            set(o.figureHandler,'WindowButtonDownFcn',{@o.callbackImagePressed});
            
            set(o.figureHandler, varargin{:});
        end
        
        function callbackButtonPressed(o,src,eventData)
            
            if strcmp(eventData.Key,'c')
                response = questdlg('Do you want to perform a global checkboard detection. This may take a while?',...
                    'Checkboard Detection','Yes','No','No');
                
                if strcmp(response,'Yes')
                    o.detectCheckerboardPoints();
                end
            end
            
            if strcmp(eventData.Key,'e')
                response = questdlg('Do you want to estimate camera parameters. This may take a while?',...
                    'Estimate Camera Parameters','Yes','No','No');
                
                if strcmp(response,'Yes')
                    if isempty(o.imagesUsed)
                        warning('Checkboard detetion was not performed yet')
                        warndlg('Please perform checkboard detection first','Cannot estimate camera parameters')
                    else
                        o.estimateCameraParameters();
                    end
                end
            end
            
            if strcmp(eventData.Key,'s')
                out_file = inputdlg('Save calibration data','Save calibration',1,'data/xb3_stereo_calibration.mat');
                
                if ~isempty(out_file)
                    params = o.stereoParameters;
                    disp(['Saving to: ' out_file])
                    save(out_file,'params')
                end
            end
            
            if strcmp(eventData.Key,'l')
                keyboard
                in_file = inputdlg('Load calibration data','Load calibration',1,{'data/xb3_stereo_calibration.mat'});
                in_file = char(in_file);
                
                if ~isempty(in_file)
                    disp(['Loading from: ' in_file])
                    load(in_file)
                    o.stereoParameters = params;
                end
            end
            
            if src == o.leftViewer.figureHandler
                o.leftViewer.callbackKeyPressed(src,eventData);
                o.drawLeftImage();
            elseif src == o.rightViewer.figureHandler
                o.rightViewer.callbackKeyPressed(src,eventData);
                o.drawRightImage();
            end 
        end
        
        function callbackImagePressed(o,src,eventData)
            if src == o.leftViewer.figureHandler
                o.leftViewer.callbackImagePressed(src,eventData);
            elseif src == o.rightViewer.figureHandler
                o.rightViewer.callbackImagePressed(src,eventData);
            end
        end
        
        function estimateCameraParameters(o)
            disp('Starting estimation')
            h = waitbar(0.01,'Estimating camera parameters');
            tic
            [o.stereoParameters, o.pairsUsed] = estimateCameraParameters(o.imagePoints,o.worldPoints,...
                'EstimateSkew',true,'NumRadialDistortionCoefficients',3,...
                'EstimateTangentialDistortion',true);
            
            disp('Estimation done')
            toc
            
            waitbar(1.0);
            
            disp('Displaying reprojection erros')
            figure('position',[120 120 1600 800])
            showReprojectionErrors(o.stereoParameters);
            set(gca,'LooseInset',[0.05 0.05 0.05 0.05])
            
            usedIds = o.leftLoader.ids(o.imagesUsed);
            set(gca,'XTick',1:length(usedIds));
            set(gca,'XTickLabel',usedIds);
            
            addpath rotateXLabels
            rotateXLabels(gca,60)
            
%             keyboard
%             set(gca,'XTick',1:length(ids))
%             set(gca,'XTickLabels',ids)
%             set(gca,'LooseInset',[0.05 0.05 0.05 0.05])
            
            pause(0.5)
            delete(h)
        end
        
        function ip = getImagePoints(o,frame,camera)
            
            if isempty(o.imagesUsed)
                ip=[];
                return
            end
            
            pos = find(o.leftLoader.ids == frame,1);
            
            if o.imagesUsed(pos)
                %This image was used for checkboard calculations
                %Mark this position
                mask = double(o.imagesUsed);
                mask(pos)=2;
                
                %Remove from the mask the zero elements
                mask = mask(mask>0);
                
                %Get the corrected position for the imagePoints
                correctedPos = find(mask == 2,1);
                ip = o.imagePoints(:,:,correctedPos,camera);
            else
                ip = [];
            end
        end
        
        function drawLeftImage(o)
            
%             disp('Failed frames:')
%             disp(o.leftLoader.ids(o.imagesUsed==0)')

            ip = o.getImagePoints(o.leftLoader.current_frame,1);
            
            if isempty(ip)
                return
            end
            
            hold on
            plot(ip(:,1),ip(:,2),'*')
        end
        
        function drawRightImage(o)
%             disp('Failed frames:')
%             disp(o.rightLoader.ids(o.imagesUsed==0)')

            ip = o.getImagePoints(o.rightLoader.current_frame,2);
            
            if isempty(ip)
                return
            end
            
            hold on
            plot(ip(:,1),ip(:,2),'*')
        end
        
        function detectCheckerboardPoints(o)
            h = waitbar(0.0,'Detecting checkboard positions');
            
            disp('Listing files and starting detection')
            
            leftFiles = {o.leftLoader.files.name};
            for i=1:length(leftFiles)
                leftFiles{i} = fullfile(o.leftLoader.main_path,leftFiles{i});
            end
            
            waitbar(0.05)
            
            rightFiles = {o.rightLoader.files.name};
            for i=1:length(rightFiles)
                rightFiles{i} = fullfile(o.rightLoader.main_path,rightFiles{i});
            end
            
            waitbar(0.1)
            
            disp('Detecting please wait ...')
            [o.imagePoints, o.boardSize, o.imagesUsed] = detectCheckerboardPoints(leftFiles, rightFiles);
            disp('Done')
            
            disp('Failed detections:')
            disp(o.leftLoader.ids(o.imagesUsed==0)')
           
            waitbar(0.95)
            
            o.worldPoints = generateCheckerboardPoints(o.boardSize, 108);
            
            waitbar(1.0)
            pause(0.5)
            
            delete(h)
        end
    end
    
    
end