classdef appTentativaClean < matlab.apps.AppBase

    % Properties that correspond to app components
    properties (Access = public)
        UIFigure                       matlab.ui.Figure
        Panel2                         matlab.ui.container.Panel
        GetObjectPointCloudButton      matlab.ui.control.Button
        DistanceBehindObjectEditField  matlab.ui.control.NumericEditField
        DistanceBehindObjectEditFieldLabel  matlab.ui.control.Label
        GetSelectedPointsButton        matlab.ui.control.Button
        GetFullPointCloudButton        matlab.ui.control.Button
        ClosestObjectLabel             matlab.ui.control.Label
        UIAxes_3                       matlab.ui.control.UIAxes
        Panel                          matlab.ui.container.Panel
        StartStreamsButton             matlab.ui.control.Button
        DepthVideoStreamLabel          matlab.ui.control.Label
        RGBVideoStreamLabel            matlab.ui.control.Label
        UIAxes_2                       matlab.ui.control.UIAxes
        UIAxes                         matlab.ui.control.UIAxes
    end

    % Callbacks that handle component events
    methods (Access = private)

        % Button pushed function: StartStreamsButton
        function StartStreamsButtonPushed(app, event)
            %% GLOBALS
           global DEPTHDisp
           global RGBDisp
           global imgDEPTH
           global imgRGB
            
            %% RGB
            % LIGAR CAMARA RGB
            RGBDisp = videoinput('kinect',1,'RGB_640x480');

            frameRGB = getsnapshot(RGBDisp);
            imgRGB = image(app.UIAxesRGBStream,zeros(size(frameRGB),'uint16'));

            % MOSTRAR RGB STREAM
            axis(app.UIAxesRGBStream,'image');
            preview(RGBDisp,imgRGB);

            % RGB VIDEO STREAM ASPECT RATIO
            app.UIAxesRGBStream.XLim = [0,640];
            app.UIAxesRGBStream.YLim = [0,480];
            pbaspect(app.UIAxesRGBStream,[640,480,1]);

            %% DEPTH
            % LIGAR CAMARA DEPTH
            DEPTHDisp = videoinput('kinect',2,'Depth_640x480');

            frameDEPTH = getsnapshot(DEPTHDisp);
            imgDEPTH = image(app.UIAxesDEPTHStream,zeros(size(frameDEPTH),'uint16'));

            % MOSTRAR DEPTH STREAM
            axis(app.UIAxesDEPTHStream)
            preview(DEPTHDisp,imgDEPTH)

             % DEPTH VIDEO STREAM ASPECT RATIO
            app.UIAxesDEPTHStream.XLim = [0,640];
            app.UIAxesDEPTHStream.YLim = [0,480];
            pbaspect(app.UIAxesDEPTHStream,[640,480,2]);
        end

        % Button pushed function: GetFullPointCloudButton
        function GetFullPointCloudButtonPushed(app, event)
             %% GLOBALS
            global DEPTHDisp
            global RGBDisp
            global ptCloudRot
            global RGBFrame1
            global ptCloudRot
            
            %% EXTRAIR A NUVEM DE PONTOS
            RGBFrame1 = getsnapshot(RGBDisp);
            DEPTHFrame1 = getsnapshot(DEPTHDisp);

            ptCloudOriginal = pcfromkinect(DEPTHDisp, DEPTHFrame1, RGBFrame1);

            %% RODAR A NUVEM DE PONTOS PARA FICAR PARALELA COM EIXO DOS XX
            %ANGULO DE ROTAÇAO
            theta = -0.04;

            % MATRIZ DE ROTAÇAO EM X
            Rx = [1 0 0; 0 cos(theta) -sin(theta); 0 sin(theta) cos(theta)];
            trans = [0, 0, 0];
            tform = rigid3d(Rx,trans);

            % NOVA NUVEM DE PONTOS RODADA
            ptCloudRot = pctransform(ptCloudOriginal,tform);

            % MOSTART NUVEM DE PONTOS
            figure
            pcshow(ptCloudRot,"VerticalAxis","Y",VerticalAxisDir="Down");
            title('Full Point Cloud')
            xlabel('X(m)')
            ylabel('Y(m)')
            zlabel('Z(m)')
        end

        % Button pushed function: GetSelectedPointsButton
        function GetSelectedPointsButtonPushed(app, event)
             %% GLOBALS
            global zRot
            global MRot
            global ptCloudRot
            global ptCloudObjeto
            global RGBFrame1

            %% Fechar a janela da nuvem de pontos completa
            close(figure(1))

            %% ENCONTRAR VIZINHANÇA DO OBJETO MAIS PROXIMO
            point = [0 0 0];                      % PONTO ORIGEM PARA PROCEDER À VIZINHANÇA -> (0,0,0) ERA O PONTO QUE MARCAVA "CENTRO" DO OBJETO

            Z = app.DistancebehindobjectEditField.Value;

            K = numel(find(zRot<(MRot+Z)));     % Nº DE PONTOS DA VIZINHANÇA

            % LOCALIZAÇÃO DOS PONTOS DE VIZINHANÇA
            [indices,dists] = findNearestNeighbors(ptCloudRot,point,K);

            % NUVEM DE PONTOS DO OBJETO MAIS PROXIMO
            ptCloudObjeto = select(ptCloudRot,indices);

            figure
            pcshow(ptCloudRot,'VerticalAxis','Y',VerticalAxisDir='Down')
            hold on
            pcshow(ptCloudObjeto.Location,'ob','MarkerSize',5,'VerticalAxis','Y',VerticalAxisDir='Down')

            title('Selected Object Points')
            xlabel('X(m)')
            ylabel('Y(m)')
            zlabel('Z(m)')           

            %% OBTER IMAGEM DO OBJETO MAIS PRÓXIMO
            % RETIRAR A MATRIZ DE PONTOS DE LOCALIZAÇAO DOS PONTOS
            zRot = ptCloudRot.Location(:,:,3);

            % MOSTART INDICE DOS PONTOS MINIMOS DA NUVEM DE PONTOS
            [MRot,IRot] = min(min(zRot));

            % LOCALIZAÇAO DO PONTO MAIS PROXIMO
            [xRot,yRot]=find(zRot==MRot);

            % RESOLUÇÃO DA IMAGEM 200x200
            xmin = xRot(1)-100;
            xmax = xRot(1)+100;
            ymin = yRot(1)-100;
            ymax = yRot(1)+100;

            % % IMPEDIR QUE IMAGEM XMIN SEJA NEGATIVO E XMAX NAO ULTRAPASSE OS 480
            if xmin < 0
                xmin = 1;
            elseif xmax > 480
                xmax = 480;
            end

            % IMPEDIR QUE IMAGEM YMIN SEJA NEGATIVO EYXMAX NAO ULTRAPASSE OS 640
            if ymin < 0
                ymin = 1;
            elseif ymax > 640
                ymax = 640;
            end

            %%
            % FRAME RGB RECORTADO EM 200x200 PARA ELIMINAR PERTURBAÇÕES NO FUNDO
            RGBCropped = RGBFrame1(xmin:xmax,ymin:ymax,:);

            image(RGBCropped,'Parent',app.UIAxes)
            set(app.UIAxes,'visible','off');
        end

        % Button pushed function: GetObjectPointCloudButton
        function GetObjectPointCloudButtonPushed(app, event)
             %% GLOBALS
            global ptCloudObjeto

            %% FECHAR AS FIGURES ABERTAS
            close(figure(1));

            %% NUVEM DE PONTOS DO OBJETO SELECIONADO
            % MATRIZ POSIÇAO DO OBJETO
            PontosObjeto = ptCloudObjeto.Location;

            % MATRIZ CORES DO OBJETO
            CoresObjeto = ptCloudObjeto.Color;

            % NUVEM DE PONTOS DO OBJETO
            PtCloudObjeto = pointCloud(PontosObjeto,"Color",CoresObjeto);

            figure
            pcshow(PtCloudObjeto,'VerticalAxis','Y',VerticalAxisDir='Down')
            title('Object Point Cloud')
            xlabel('X(m)')
            ylabel('Y(m)')
            zlabel('Z(m)')
        end
    end

    % Component initialization
    methods (Access = private)

        % Create UIFigure and components
        function createComponents(app)

            % Create UIFigure and hide until all components are created
            app.UIFigure = uifigure('Visible', 'off');
            app.UIFigure.AutoResizeChildren = 'off';
            app.UIFigure.Position = [100 100 1123 637];
            app.UIFigure.Name = 'MATLAB App';
            app.UIFigure.Resize = 'off';

            % Create Panel
            app.Panel = uipanel(app.UIFigure);
            app.Panel.AutoResizeChildren = 'off';
            app.Panel.Title = 'Panel';
            app.Panel.Position = [13 133 391 494];

            % Create UIAxes
            app.UIAxes = uiaxes(app.Panel);
            app.UIAxes.Position = [47 264 300 185];

            % Create UIAxes_2
            app.UIAxes_2 = uiaxes(app.Panel);
            app.UIAxes_2.Position = [49 58 300 185];

            % Create RGBVideoStreamLabel
            app.RGBVideoStreamLabel = uilabel(app.Panel);
            app.RGBVideoStreamLabel.Position = [144 440 107 22];
            app.RGBVideoStreamLabel.Text = 'RGB Video Stream';

            % Create DepthVideoStreamLabel
            app.DepthVideoStreamLabel = uilabel(app.Panel);
            app.DepthVideoStreamLabel.Position = [141 236 113 22];
            app.DepthVideoStreamLabel.Text = 'Depth Video Stream';

            % Create StartStreamsButton
            app.StartStreamsButton = uibutton(app.Panel, 'push');
            app.StartStreamsButton.ButtonPushedFcn = createCallbackFcn(app, @StartStreamsButtonPushed, true);
            app.StartStreamsButton.Position = [148 23 100 22];
            app.StartStreamsButton.Text = 'Start Streams';

            % Create Panel2
            app.Panel2 = uipanel(app.UIFigure);
            app.Panel2.AutoResizeChildren = 'off';
            app.Panel2.Title = 'Panel2';
            app.Panel2.Position = [418 133 538 494];

            % Create UIAxes_3
            app.UIAxes_3 = uiaxes(app.Panel2);
            app.UIAxes_3.Position = [20 220 300 185];

            % Create ClosestObjectLabel
            app.ClosestObjectLabel = uilabel(app.Panel2);
            app.ClosestObjectLabel.Position = [129 396 84 22];
            app.ClosestObjectLabel.Text = 'Closest Object';

            % Create GetFullPointCloudButton
            app.GetFullPointCloudButton = uibutton(app.Panel2, 'push');
            app.GetFullPointCloudButton.ButtonPushedFcn = createCallbackFcn(app, @GetFullPointCloudButtonPushed, true);
            app.GetFullPointCloudButton.Position = [107 440 123 22];
            app.GetFullPointCloudButton.Text = 'Get Full Point Cloud';

            % Create GetSelectedPointsButton
            app.GetSelectedPointsButton = uibutton(app.Panel2, 'push');
            app.GetSelectedPointsButton.ButtonPushedFcn = createCallbackFcn(app, @GetSelectedPointsButtonPushed, true);
            app.GetSelectedPointsButton.Position = [369 322 246 22];
            app.GetSelectedPointsButton.Text = 'Get Selected Points';

            % Create DistanceBehindObjectEditFieldLabel
            app.DistanceBehindObjectEditFieldLabel = uilabel(app.Panel2);
            app.DistanceBehindObjectEditFieldLabel.HorizontalAlignment = 'right';
            app.DistanceBehindObjectEditFieldLabel.Position = [369 277 131 22];
            app.DistanceBehindObjectEditFieldLabel.Text = 'Distance Behind Object';

            % Create DistanceBehindObjectEditField
            app.DistanceBehindObjectEditField = uieditfield(app.Panel2, 'numeric');
            app.DistanceBehindObjectEditField.Limits = [0 Inf];
            app.DistanceBehindObjectEditField.Position = [515 277 100 22];
            app.DistanceBehindObjectEditField.Value = 0.2;

            % Create GetObjectPointCloudButton
            app.GetObjectPointCloudButton = uibutton(app.Panel2, 'push');
            app.GetObjectPointCloudButton.ButtonPushedFcn = createCallbackFcn(app, @GetObjectPointCloudButtonPushed, true);
            app.GetObjectPointCloudButton.Position = [261 126 138 22];
            app.GetObjectPointCloudButton.Text = 'Get Object Point Cloud';

            % Show the figure after all components are created
            app.UIFigure.Visible = 'on';
        end
    end

    % App creation and deletion
    methods (Access = public)

        % Construct app
        function app = appTentativaClean

            % Create UIFigure and components
            createComponents(app)

            % Register the app with App Designer
            registerApp(app, app.UIFigure)

            if nargout == 0
                clear app
            end
        end

        % Code that executes before app deletion
        function delete(app)

            % Delete UIFigure when app is deleted
            delete(app.UIFigure)
        end
    end
end