classdef XB3ViconGeometricTemporalCalibration < handle
    %UNTITLED7 Summary of this class goes here
    %   Detailed explanation goes here
    
    properties
        viconPlotter;
        xb3Plotter;
        xb3ImageLoader;
        calibrationPoints;
        inSync;
        calibrationPointsDataPath;
        cameraParameters;
        extrinsicParameters;
        P;
        temporalMap;
    end
    
    methods
        
        function o = XB3ViconGeometricTemporalCalibration(viconData,imagePath,pointsPath,cameraParameters)
            o.xb3ImageLoader = ImageLoader(imagePath);
            o.viconPlotter = ViconPloting(viconData);
            o.xb3Plotter = ImageViewer(o.xb3ImageLoader);
            
            o.viconPlotter.createFigure('name','Vicon','NumberTitle','off','menubar','none','position',[80 80 900 900],'color',[0.3 0.3 0.3]);
            set(o.viconPlotter.axisHandler,'color',[0.3 0.3 0.3]);
            set(o.viconPlotter.figureHandler,'WindowKeyPressFcn',@o.callbackVICONKeyPressed);
            o.viconPlotter.draw();
            
            o.xb3Plotter.createFigure('name','XB3','NumberTitle','off','menubar','none','position',[1000 80 900 900]);
            set(o.xb3Plotter.figureHandler,'WindowKeyPressFcn',{@o.callbackXB3KeyPressed});
            
            o.xb3Plotter.draw();
            
            %Try to load calibration points
            o.calibrationPointsDataPath = pointsPath;
            try
                load(o.calibrationPointsDataPath)
                o.calibrationPoints = points;
                o.temporalMap = temporalMap;
                
                h=text(10,65+20,'Calibration points loaded','Parent',o.xb3Plotter.axisHandler);
                set(h,'FontSize',10,'FontWeight','bold','color',[0 0.5 1])
            catch
            end
            
            o.cameraParameters = cameraParameters;
            
            o.inSync = false;
            
            if ~isempty(o.calibrationPoints)
                o.extrinsics();
            end
        end
        
        function drawCalibrationPoints(o,xb3Iteration)
            
            h = findobj(o.xb3Plotter.axisHandler,'Tag','singleFrameCalibrationPoint');
            if ~isempty(h)
                delete(h);
            end
                
            %Get calibration points for the current iteration
            frames = [o.calibrationPoints(:).imageFrame];
            
            imagePointsMask = frames == xb3Iteration;
            
            if isempty(find(imagePointsMask,1))
                %No points to draw in this iteration
                return
            end
            
            points = o.calibrationPoints(imagePointsMask);
            
            for i=1:length(points)
                p = points(i).image;
                h=plot(o.xb3Plotter.axisHandler,p(1),p(2));
                set(h,'marker','+','markersize',10,'markerfacecolor',[0 1 0],'markeredgecolor',[0 1 0])
                set(h,'tag','singleFrameCalibrationPoint')
                h=text(p(1)+5,p(2)+8,points(i).label,'Parent',o.xb3Plotter.axisHandler);
                set(h,'color',[0 1 0],'fontsize',10,'FontWeight','bold')
                set(h,'tag','singleFrameCalibrationPoint')
            end 
        end
        
        function projectViconPoints(o,viconIteration)
            
            h = findobj(o.xb3Plotter.axisHandler,'Tag','viconProjection'); % try to find the old point
            if ~isempty(h)
                delete(h)
            end
                
            %Get all vicon points and tags
            person = o.viconPlotter.getPerson(viconIteration);
            tags = o.viconPlotter.tags;
            
%             keyboard
            pointsPixels = o.projectPoints(person);
            
            if isempty(pointsPixels)
                warning('Empty pixel points, cannot draw in image')
                return
            end
            
%             keyboard
            imageSize = size(o.xb3Plotter.loader.current_image);
            hold on
            
            for i=1:length(pointsPixels)
                
                if pointsPixels(1,i) > imageSize(2) || pointsPixels(2,i) > imageSize(1) || pointsPixels(1,i) < 1 || pointsPixels(2,i) < 1
                    continue
                end
                
                h=plot(o.xb3Plotter.axisHandler,pointsPixels(1,i),pointsPixels(2,i));
                set(h,'marker','o','markersize',5,'markerfacecolor',[0 0 1],'markeredgecolor',[0 0 1])
                set(h,'tag','viconProjection')
                tag = char(tags{i});
                h=text(pointsPixels(1,i)+5,pointsPixels(2,i)+8,tag(3:end),'Parent',o.xb3Plotter.axisHandler);
                set(h,'color',[0 0 1],'fontsize',8,'FontWeight','bold')
                set(h,'tag','viconProjection')
            end
            
            %Add all links           
            iteration = o.viconPlotter.currentIteration;
            
            for i=1:length(o.viconPlotter.connections)
                eval(sprintf('p1 = o.viconPlotter.%s(:,%d);',char(o.viconPlotter.connections(i,1)),iteration));
                eval(sprintf('p2 = o.viconPlotter.%s(:,%d);',char(o.viconPlotter.connections(i,2)),iteration));
                
                p1Image = o.projectPoints(p1);
                p2Image = o.projectPoints(p2);
                
                if p1Image(1) > imageSize(2) || p1Image(2) > imageSize(1) || p1Image(1)<1 || p1Image(2)<1
                    continue
                end
                
                if p2Image(1) > imageSize(2) || p2Image(2) > imageSize(1) || p2Image(1)<1 || p2Image(2)<1
                    continue
                end
                
                h=plot(o.xb3Plotter.axisHandler,[p1Image(1) p2Image(1)],[p1Image(2) p2Image(2)]);
                set(h,'color',[0 0 0])
                set(h,'tag','viconProjection')
            end
        end

        function pointsImage = projectPoints(o,pointsWorld)
            if isempty(pointsWorld)
                pointsImage = [];
                warning('Empty world points, cannot project')
                return;
            end
                
            % s * [x y 1] = [X Y Z 1] * [R; t] * K
            
            %Get points in world homogenous coordinates
            pointsWorld = [pointsWorld; ones(1,size(pointsWorld,2))];
            
            %Project to image
            pointsCamera = (pointsWorld'*o.P)';
            
            %Normalize all points
            for i=1:size(pointsCamera,2)
                pointsCamera(:,i) = pointsCamera(:,i) / pointsCamera(3,i);
            end
            
            pointsImage = pointsCamera;
        end
        
        function viconFrame = frameXB32Vicon(o,xb3Frame)
            
            %If we have enough points in the temporal map we should
            %interpolate
            if ~isempty(o.temporalMap) && length(o.temporalMap)>1
                x = [o.temporalMap(:).image];
                y = [o.temporalMap(:).world];

                yv = round(interp1(x,y,xb3Frame));
            end
            
            h = findobj(o.xb3Plotter.axisHandler,'Tag','interp'); % try to find the old point
            if ~isempty(h)
                delete(h);
            end
            
            if ~exist('yv','var') || isnan(yv)
                
                if ~isempty(o.temporalMap)
                    image = [o.temporalMap(:).image];
                    world = [o.temporalMap(:).world];
                    polynomXB32Vicon = polyfit(image,world,1);
                    
                    imageHeight = size(o.xb3Plotter.loader.current_image,1);
                    h = text(10,imageHeight-20,'Vicon frame obtained from polynomial regression with sync points','Parent',o.xb3Plotter.axisHandler,'tag','interp');
                    set(h,'FontSize',8,'FontWeight','bold','color',[1 0 0.5])
                else
                    %Default calibration
                    polynomXB32Vicon = [0.062506986523000   1.029308579371243]*1.0e2;
                    
                    imageHeight = size(o.xb3Plotter.loader.current_image,1);
                    h = text(10,imageHeight-20,'Vicon frame obtained from default polynomial regression','Parent',o.xb3Plotter.axisHandler,'tag','interp');
                    set(h,'FontSize',8,'FontWeight','bold','color',[1 0 0])
                end
                
                viconFrame = round(polyval(polynomXB32Vicon,xb3Frame));
                
                
            else
                viconFrame = yv;
                
                imageHeight = size(o.xb3Plotter.loader.current_image,1);
                h = text(10,imageHeight-20,'Vicon frame obtained from interpolation','Parent',o.xb3Plotter.axisHandler,'tag','interp');
                set(h,'FontSize',8,'FontWeight','bold','color',[0 0 1])
            end
        end
            
        function callbackXB3KeyPressed(o,src,eventData)
            
            o.xb3Plotter.callbackKeyPressed(src,eventData);
            o.drawCalibrationPoints(o.xb3Plotter.loader.current_frame);
            
            doSync = true;
            
            if eventData.Key=='a'
%                 keyboard
                %Check if vicon is synced
                if ~o.inSync
                    warning('Vicon is not in sync with xb3, please sync')
                    return
                end
                
                %Get current pressed point
                h = findobj(o.xb3Plotter.axisHandler,'Tag','pt'); % try to find the old point
                
                %Check if any
                if isempty(h)
                    warning('No point seletect')
                    return
                end
                
                %Get point image coordinates
                imagePoint(1) = get(h,'xdata');
                imagePoint(2) = get(h,'ydata');
                
                %Ask user for the point label
                label = inputdlg('Label for point (get from vicon)','Add new point');
                if isempty(label)
                    return
                end
                
                %Get vicon corresponding coordinates
                viconPoint = o.viconPlotter.getPoint(label,o.viconPlotter.currentIteration);
                
                %Check if label exists in current vicon iteration
                if isempty(viconPoint)
                    warning(['Label ' char(label) ' not found in vicon data'])
                    return
                end
                
                if isnan(viconPoint(1))
                   warning(['Point with label ' char(label) ' is invalid in vicon data'])
                   return
                end
                
                %Check if point with the same label already exists in the
                %current iteration
%                 keyboard
                
                frames = [o.calibrationPoints(:).imageFrame];
                pos = find(frames == o.xb3Plotter.loader.current_frame);
                for i=1:length(pos)
                    if strcmp(o.calibrationPoints(pos(i)).label,label)
                        warning('point with the same label found, replacing')
                        o.calibrationPoints(pos(i)) = [];
                        break
                    end
                end
                
                point.image = imagePoint;
                point.world = viconPoint;
                point.imageFrame = o.xb3Plotter.loader.current_frame;
                point.worldFrame = o.viconPlotter.currentIteration;
                point.label = label;
                
                if isempty(o.calibrationPoints)
                    o.calibrationPoints = point;
                else
                    o.calibrationPoints(end+1) = point;
                end
                
                o.drawCalibrationPoints(o.xb3Plotter.loader.current_frame);

            elseif eventData.Key == 't'
                viconFrame = o.viconPlotter.currentIteration;
                xb3Frame = o.xb3Plotter.loader.current_frame;
                
                o.temporalMap(end+1).image = xb3Frame;
                o.temporalMap(end).world = viconFrame;
                
                msgbox({'Added new temporal calibration point',...
                    ['image: ' num2str(xb3Frame)],...
                    ['world: ' num2str(viconFrame)]},...
                    'New sync point')
                
                doSync = false;
            elseif eventData.Key == 's'
                if isempty(o.calibrationPointsDataPath)
                    warning('Save path not specified')
                    return
                end
                
                points = o.calibrationPoints;
                temporalMap = o.temporalMap;
                save(o.calibrationPointsDataPath,'points','temporalMap')
                warndlg(o.calibrationPointsDataPath,'Calibration points saved')
                
            elseif eventData.Key == 'l'
                if isempty(o.calibrationPointsDataPath)
                    warning('Load path not specified')
                    return
                end
                
                load(o.calibrationPointsDataPath)
                o.calibrationPoints = points;
                
                if exist('temporalMap','var')
                    o.temporalMap = temporalMap;
                else
                    o.temporalMap = [];
                end
                
                warndlg(o.calibrationPointsDataPath,'Calibration points loaded')
                
            elseif eventData.Key == 'e'
                o.extrinsics();
            elseif eventData.Key == 'c'
                doClear = questdlg('Clear extrinsics?','Remove extrinsics calibration', ...
                         'Yes', 'No', 'Yes');
   
                if strcmp(doClear,'Yes')
                    o.extrinsicParameters = [];
                end
                
            elseif eventData.Key == 'p'
                h = findobj(o.viconPlotter.axisHandler,'Tag','calibrationPoint');
                h = [h findobj(o.xb3Plotter.axisHandler,'Tag','calibrationPoint')];
                
                if isempty(h)
                    o.plotAllCalibrationPointsInVicon();
                    o.plotAllCalibrationPointsInXb3();
                else
                    delete(h);
                end
            end
            
            if doSync
                viconFrame = o.frameXB32Vicon(o.xb3ImageLoader.current_frame);

                o.viconPlotter.currentIteration = viconFrame;
                o.viconPlotter.draw();

                h = findobj(o.xb3Plotter.axisHandler,'Tag','sync'); % try to find the old point
                h = [h findobj(o.xb3Plotter.axisHandler,'Tag','viconFrame')]; % try to find the old point
                if ~isempty(h)
                    delete(h);
                end

                h = text(10,40,'Synced with Vicon','Parent',o.xb3Plotter.axisHandler,'tag','sync');
                set(h,'FontSize',10,'FontWeight','bold','color',[0 1 0])

                h = text(10,65,['Vicon Frame ' num2str(viconFrame)],'Parent',o.xb3Plotter.axisHandler,'tag','viconFrame');
                set(h,'FontSize',10,'FontWeight','bold','color',[0 0 0])

                o.inSync = true;
            end
            
            if ~isempty(o.extrinsicParameters)
                o.projectViconPoints(viconFrame);
                o.drawBoundingBox(viconFrame);
            end
        end
        
        function drawBoundingBox(o,viconIteration)
            %Get all vicon points and tags
            person = o.viconPlotter.getPerson(viconIteration);
            %tags = o.viconPlotter.tags;
            
            %Get the points in pixels
            pointsPixels = o.projectPoints(person);
            
            maxX = max(pointsPixels(1,:));
            minX = min(pointsPixels(1,:));
            
            maxY = max(pointsPixels(2,:));
            minY = min(pointsPixels(2,:));
            
            factor = 0.05;
            maxX = maxX + factor*maxX;
            minX = minX - factor*minX;
            
            maxY = maxY + factor*maxY;
            minY = minY - (factor+0.03)*minY;
            
            %Check if bounding box is inside de image
            imageSize = size(o.xb3Plotter.loader.current_image);
            
            if maxX > imageSize(2) || maxX <= 0 || minX > imageSize(2) || minX <= 0 ...
                    || maxY > imageSize(2) || maxY <= 0 || minY > imageSize(2) || minY <= 0
                return
            end
            
            width = maxX-minX;
            height = maxY-minY;
            
            h = rectangle('Position',[minX minY width height],'Parent',o.xb3Plotter.axisHandler);
            set(h,'Tag','boundingBox')
        end
        
        function plotAllCalibrationPointsInVicon(o)
            
            points = o.calibrationPoints;
            
            mx = max([points(:).imageFrame]);
            map = colormap('jet');
            map = interp1(linspace(0,1,64),map,linspace(0,1,250));
            
            for i=1:length(points)
                color = map(round(points(i).imageFrame/mx*250),:);
                p = points(i).world;
                h=plot3(o.viconPlotter.axisHandler,p(1),p(2),p(3));
                set(h,'marker','+','markersize',10,'markerfacecolor',color,'markeredgecolor',color)
                set(h,'Tag','calibrationPoint')
                
                l = [char(points(i).label) ' - ' num2str(points(i).worldFrame)];
                h=text(p(1)+0.03,p(2)+0.03,p(3),l,'Parent',o.viconPlotter.axisHandler);
                set(h,'color',color,'fontsize',9,'FontWeight','bold')
                set(h,'Tag','calibrationPoint')
            end 
            
        end
        
        function plotAllCalibrationPointsInXb3(o)
            
            points = o.calibrationPoints;
            
            mx = max([points(:).imageFrame]);
            map = colormap('jet');
            map = interp1(linspace(0,1,64),map,linspace(0,1,250));
            
            for i=1:length(points)
                color = map(round(points(i).imageFrame/mx*250),:);
                p = points(i).image;
                h=plot(o.xb3Plotter.axisHandler,p(1),p(2));
                set(h,'marker','+','markersize',10,'markerfacecolor',color,'markeredgecolor',color)
                set(h,'Tag','calibrationPoint')
                
                l = [char(points(i).label) ' - ' num2str(points(i).imageFrame)];
                h=text(p(1)+0.03,p(2)+0.03,l,'Parent',o.xb3Plotter.axisHandler);
                set(h,'color',color,'fontsize',9,'FontWeight','bold')
                set(h,'Tag','calibrationPoint')
            end 
        end
        
        function reframeCalibrationPoints(o)
            %Simply overwrite world frame without changing the point
            %coordinates
            
            for i=1:length(o.calibrationPoints)
                o.calibrationPoints(i).viconFrame = o.frameXB32Vicon(o.calibrationPoints(i).imageFrame);
            end
            
            return;
            
            %Vicon-xb3 correspondence has change, must get new world
            %coordinates of corresponding points
            pointsToRemove = [];
            
            for i=1:length(o.calibrationPoints)
                
                label = o.calibrationPoints(i).label;
                
                viconFrame = o.frameXB32Vicon(o.calibrationPoints(i).imageFrame);
                
                %Get vicon corresponding coordinates
                viconPoint = o.viconPlotter.getPoint(label,viconFrame);
                
                %Check if label exists in current vicon iteration
                if isempty(viconPoint)
                    warning(['Label ' char(label) ' not found in vicon data'])
                    warning('this point must be removed from the calibration')
                    pointsToRemove(end+1) = i;
                    continue
                end
                
                if isnan(viconPoint(1))
                   warning(['Point with label ' char(label) ' is invalid in vicon data'])
                   warning('this point must be removed from the calibration')
                   pointsToRemove(end+1) = i;
                   continue
                end
                
                o.calibrationPoints(i).world = viconPoint;
                o.calibrationPoints(i).worldFrame = viconFrame;
            end
            
            o.calibrationPoints(pointsToRemove) = [];
        end
        
        function extrinsics(o)
            o.reframeCalibrationPoints();
            
            h = waitbar(0.0,'Estimating extrinsic camera paramters');
            
            imagePoints = reshape([o.calibrationPoints(:).image],2,length(o.calibrationPoints))';
            worldPoints = reshape([o.calibrationPoints(:).world],3,length(o.calibrationPoints))';

            waitbar(0.05)
            
            [o.extrinsicParameters.rotationMatrix,o.extrinsicParameters.translationVector] = extrinsics(imagePoints,worldPoints,o.cameraParameters);
            
            %Compute the transformation matrix from world to image
            o.P = [o.extrinsicParameters.rotationMatrix; o.extrinsicParameters.translationVector]*o.cameraParameters.IntrinsicMatrix;
            
            waitbar(1.00)
            pause(0.5)
            
            delete(h)
        end
        
        function callbackVICONKeyPressed(o,src,eventData)
            viconFrame = o.viconPlotter.currentIteration;
            
            o.viconPlotter.callbackKeyPressed(src,eventData);
            
            if o.viconPlotter.currentIteration == viconFrame
                %No change in the iteration
                return
            end
            
            h = findobj(o.xb3Plotter.axisHandler,'Tag','sync'); % try to find the old point
            h = [h findobj(o.xb3Plotter.axisHandler,'Tag','viconFrame')]; % try to find the old point
            if ~isempty(h)
                delete(h);
            end
            
            h = text(10,40,'Not in sync with Vicon','Parent',o.xb3Plotter.axisHandler,'tag','sync');
            set(h,'FontSize',10,'FontWeight','bold','color',[1 0 0])
            o.inSync = false;
            
            h = text(10,65,['Vicon Frame ' num2str(o.viconPlotter.currentIteration)],'Parent',o.xb3Plotter.axisHandler,'tag','viconFrame');
            set(h,'FontSize',10,'FontWeight','bold','color',[0 0 0])
            
            if ~isempty(o.extrinsicParameters)
                o.projectViconPoints(o.viconPlotter.currentIteration);
            end
        end
    end
    
end

