classdef ViconPloting < handle
    %UNTITLED9 Summary of this class goes here
    %   Detailed explanation goes here
    
    properties
        t;
        tags;
        connections;
        figureHandler;
        axisHandler;
        currentIteration;
        trialLength;
    end
    
    methods
        function o = ViconPloting(dataFile)
            
            load(dataFile);
            
            o.t = t;
            o.tags = tags;
            o.connections = connections;
            
            eval(sprintf('o.trialLength = size(%s,2);',o.tags{1}));
            o.currentIteration = 1;
        end
        
        function point = getPoint(o,label,iteration)
            %check if label exists
            label = ['t.' char(label)];
            
            if ~ismember(label,o.tags)
                point = [];
                warning('Tag not found')
                return 
            end
            
            eval(sprintf('point(1) = o.%s(1,%d);',label,iteration));
            eval(sprintf('point(2) = o.%s(2,%d);',label,iteration));
            eval(sprintf('point(3) = o.%s(3,%d);',label,iteration));
        end
        
        function createFigure(o,varargin)
            
            o.figureHandler = figure;
            o.axisHandler = gca;
            
            hold on

            cx = 0; cy = 2; cz = 1;

            cube_width = 10.0;

            axis([cx-cube_width/2 cx+cube_width/2 cy-cube_width/2 cy+cube_width/2 cz-cube_width/2 cz+cube_width/2])
            % axis equal
            axis vis3d
            grid on
            view(3)
            

            cube_width = 9.8;

            ground_plane(1,:) = [cx-cube_width/2 cx+cube_width/2 cx+cube_width/2 cx-cube_width/2];
            ground_plane(2,:) = [cy-cube_width/2 cy-cube_width/2 cy+cube_width/2 cy+cube_width/2];
            ground_plane(3,:) = [0 0 0 0];

            ground_plane_color = [0.1 0.1 0.1];

            hp = fill3(ground_plane(1,:),ground_plane(2,:),ground_plane(3,:),'b');
            set(hp,'facecolor',ground_plane_color)

            hp=plot3(0,0,0);
            set(hp,'markersize',20,'marker','+','markeredgecolor',[0 0 1],'markerfacecolor',[0 0 1],'linewidth',2)

            set(gcf,'renderer','opengl')

            xlabel('x (m)'); ylabel('y (m)'); zlabel('z (m)');
            
            
            %Add the figure rotator tool to the figure
            addpath figure_rotator
            f = FigureRotator(gca, 'CameraTarget',    [0 0 0], ...
                                   'CameraPosition',  [12 12 12], ...
                                   'CameraViewAngle', 60);

            f.AttachCallback('WindowButtonDownFcn', @o.callbackPointSelection);
            set(o.figureHandler,'WindowKeyPressFcn',@o.callbackKeyPressed);
            set(o.figureHandler, 'menubar', 'none');

            set(o.figureHandler, varargin{:});
        end
        
        function person = getPerson(o,iteration)
            if isempty(iteration)
                warning('Empty iteration')
                person = [];
                return
            end
            
            if iteration > o.trialLength
                person = [];
                warning(['Iteration out of trial limit: ' num2str(iteration) ' > ' num2str(o.trialLength) ' (limit)'])
                return;
            end
            
            person=zeros(3,length(o.tags));
            for i=1:length(o.tags)
                eval(sprintf('person(1,%d) = o.%s(1,%d);',i,char(o.tags{i}),iteration));
                eval(sprintf('person(2,%d) = o.%s(2,%d);',i,char(o.tags{i}),iteration));
                eval(sprintf('person(3,%d) = o.%s(3,%d);',i,char(o.tags{i}),iteration));
            end
        end
        
        function callbackPointSelection(o,src, eventData)

            person = o.getPerson(o.currentIteration);

            point = get(o.axisHandler, 'CurrentPoint'); % mouse click position
            camPos = get(o.axisHandler, 'CameraPosition'); % camera position
            camTgt = get(o.axisHandler, 'CameraTarget'); % where the camera is pointing to

            camDir = camPos - camTgt; % camera direction
            camUpVect = get(o.axisHandler, 'CameraUpVector'); % camera 'up' vector

            % build an orthonormal frame based on the viewing direction and the 
            % up vector (the "view frame")
            zAxis = camDir/norm(camDir);    
            upAxis = camUpVect/norm(camUpVect); 
            xAxis = cross(upAxis, zAxis);
            yAxis = cross(zAxis, xAxis);

            rot = [xAxis; yAxis; zAxis]; % view rotation 

            % the point cloud represented in the view frame
            rotatedPointCloud = rot * person; 

            % the clicked point represented in the view frame
            rotatedPointFront = rot * point' ;

            % find the nearest neighbour to the clicked point 
            pointCloudIndex = dsearchn(rotatedPointCloud(1:2,:)', ... 
                rotatedPointFront(1:2));

            h = findobj(o.axisHandler,'Tag','pt'); % try to find the old point
            selectedPoint = person(:, pointCloudIndex); 

            if isempty(h) % if it's the first click (i.e. no previous point to delete)

                % highlight the selected point
                h = plot3(o.axisHandler,selectedPoint(1,:), selectedPoint(2,:), selectedPoint(3,:), 'r'); 
                set(h,'marker','o','linestyle','none','markersize',12);
                set(h,'MarkerFaceColor',[0 1 0],'markeredgecolor',[0 1 0])
                set(h,'Tag','pt'); % set its Tag property for later use   
                set(h,'UserData',o.tags{pointCloudIndex}); % set its Tag property for later use   

            else % if it is not the first click

                delete(h); % delete the previously selected point

                % highlight the newly selected point
                h = plot3(o.axisHandler,selectedPoint(1,:), selectedPoint(2,:), selectedPoint(3,:), 'r');  
                set(h,'marker','o','linestyle','none','markersize',12);
                set(h,'MarkerFaceColor',[0 1 0],'markeredgecolor',[0 1 0])
                set(h,'Tag','pt');  % set its Tag property for later use
                set(h,'UserData',o.tags{pointCloudIndex}); % set its Tag property for later use   
            end
        end
        
        function callbackKeyPressed(o,src, eventData)
            
                if eventData.Key == 'k'
                    %Change k
                    k_i = str2double(char(inputdlg('Set iteration','Change iteration')));

                    if isnan(k_i)
                        return
                    end

                    o.currentIteration = max(k_i,1);
                    o.currentIteration = min(o.currentIteration,o.trialLength);
                    o.draw();

                elseif strcmp(eventData.Key,'rightarrow') 

                    if strcmp(eventData.Modifier,'shift')
                        o.currentIteration = o.currentIteration + 10;
                    elseif strcmp(eventData.Modifier,'control')
                        o.currentIteration = o.currentIteration + 100;
                    else
                        o.currentIteration = o.currentIteration + 1;  
                    end

                    o.currentIteration = min(o.currentIteration,o.trialLength);
                    o.draw();

                elseif strcmp(eventData.Key,'leftarrow')
                    
                    if strcmp(eventData.Modifier,'shift')
                        o.currentIteration = o.currentIteration - 10;
                    elseif strcmp(eventData.Modifier,'control')
                        o.currentIteration = o.currentIteration - 100;
                    else
                        o.currentIteration = o.currentIteration - 1;  
                    end

                    o.currentIteration = max(o.currentIteration,1);
                    fprintf('k: %d\n',o.currentIteration)
                    o.draw();

                elseif strcmp(eventData.Key,'f')

                    %Change camera focus point

                    %Get the selected point
                    h = findobj(o.axisHandler,'Tag','pt'); % try to find the old point

                    if ~isempty(h) %Found
                        point(1) = get(h,'XData');
                        point(2) = get(h,'YData');
                        point(3) = get(h,'ZData');

                        set(o.axisHandler,'CameraTarget',point)
                    end

                elseif strcmp(eventData.Key,'u')
                    set(gca, 'CameraUpVector', [0 0 1]);
                end
            end

            function valid = isValid(point)
                if isempty(point)
                    valid = false;
                else
                    valid = ~any(isnan(point));
                end
            end


            function increment(increment)
                o.currentIteration = o.currentIteration + increment;
                o.currentIteration = min(o.currentIteration,o.trialLength);
                o.currentIteration = max(o.currentIteration,1);
                o.draw();
            end

            function draw(o)
                colors = [21, 72, 144
                      21, 72, 144
                      255, 0, 0
                      255, 0, 0
                      255, 102, 0
                      255, 255, 255];

                colors = colors/255;

                hl = findobj(o.axisHandler,'Tag','pt'); % try to find the old point
                if ~isempty(hl)
                    delete(hl)
                end

                person = o.getPerson(o.currentIteration);
                if isempty(person)
                    warning('Empty person data, cannot draw')
                    return
                end
                
                plotPerson(o.axisHandler,person,o.connections,o.tags,colors);

                ht = findobj(o.axisHandler,'Tag','frame_number');
                if ~isempty(ht)
                    delete(ht)
                end

                text(0,0,0.2,sprintf('k %d',o.currentIteration),'color',[1 1 1],'Tag','frame_number','Parent',o.axisHandler)
            end

    end
    
end

