classdef BBBmessage < Sock
    %BBBMEASSAGE Communication protocol for Server running on
    %BeagleBoneBlack
        
    properties
        comppresion = 0;
        TIMEOUTms = 200;       
        PROJECTOR_SCREEN_SIZE = [360 640 3];
    end
    
    properties (SetObservable = true)
        PinPROJ_ON_EXT = 0;
        DISP_CTRLconf = 0;
    end

    
    methods
        function obj = BBBmessage(openOnInit,Port, Ip)
            %UNTITLED Construct an instance of this class
            %   Detailed explanation goes here
            obj.Port = 8888;
            obj.Host = '192.168.1.50';
            if nargin > 1
                obj.Port = Port;
            end
            if nargin > 2
                obj.Host = Ip;
            end
            if nargin > 0 && openOnInit
                obj.open();
            end
        end
        function Status = SendImgSafe(obj,inImg)
            if ~isa(inImg,'uint8')
                inImg = im2uint8(inImg);
            end
            if any([size(inImg,1),size(inImg,2),size(inImg,3)]  ~= obj.PROJECTOR_SCREEN_SIZE)
                inImg = obj.resizeImg(inImg);
            end
            Status = obj.SendImg(inImg);
        end
        function Status = SendImg(obj,inImg)
            %SendImg Sends input image of size [360 640 3] to BBB
            %Framebuffer
            if any(size(inImg) ~= obj.PROJECTOR_SCREEN_SIZE)
                error('Wrong image size, call resizeImg first')
                Status = 1;
            end
            % Convert to BGRA image format
            r = inImg(:,:,1)';
            g = inImg(:,:,2)';
            b = inImg(:,:,3)';
            a = ones(size(r),'uint8')*255;
            
            D = horzcat(b(:),g(:),r(:),a(:))';
            
            Status = obj.SendMessage(D(:)');
        end
        
        function Status = SendCommands(obj)
            % Sends blank message (4 len bytes + 1 status byte)
            Status = obj.SendMessage(uint8(''));
        end
        
        function Status = UpdateStates(obj,StateMsg)
            % status byte parsing 
            % [0, 0, 0, 0, 0, 0, DISP_CTRLconf, PinPROJ_ON_EXT]
            obj.PinPROJ_ON_EXT = bitget(StateMsg,1);
            obj.DISP_CTRLconf = bitget(StateMsg,2);
            Status = 0;
        end
        
        function Status = TurnONProjector(obj)
            % Configure projector to display BBB framebuffer
            % turn on PinPROJ_ON_EXT
            if ~obj.PinPROJ_ON_EXT 
                obj.PinPROJ_ON_EXT = 1;
                obj.SendCommands();
                pause(2);
            end
            % send I2C configuration command
            obj.DISP_CTRLconf = 1;
            Status = obj.SendCommands();               
        end
        
        function Status = TurnOFFProjector(obj)
            % turn off PinPROJ_ON_EXT
            obj.PinPROJ_ON_EXT = 0;
            Status = obj.SendCommands();
        end
        
        function Status = SendMessage(obj,inputMessage)
            if obj.isopen
                % length of message - uint32 -> 4x uint8, Network Byte order
                lenMsg = fliplr(typecast(uint32(length(inputMessage)),'uint8'));
                cmdByte = uint8(bin2dec(num2str([0 0 0 0 0 0 obj.DISP_CTRLconf obj.PinPROJ_ON_EXT])));
                obj.send([lenMsg,cmdByte,inputMessage]);
                
                i=0;
                while (i < obj.TIMEOUTms)
                    pause(0.001);              
                    n = obj.poolRead();
                    if (n > 0)
                        Status = obj.read(1);
                        obj.UpdateStates(Status);
                        return;
                    end                    
                    i=i+1;
                end
                error('Did not recieved response');   
                Status = 1;
            else
                error('Connection not open');
                Status = 1;
            end
        end
        
        function imgOut = resizeImg(obj,imgIn)
            % Inbput image will be resized to [360 640 3]
            img = imgIn;
            % scale img to exactly match one side
            if any([size(img,1),size(img,2),size(img,3)] ~= obj.PROJECTOR_SCREEN_SIZE)
                imgScales = obj.PROJECTOR_SCREEN_SIZE./size(img);
                imgScale = min(imgScales(1:2));
                img = imresize(img, imgScale);
            end
            % fill empty space with black pixels, fill to 3dimenstions
            if any(size(img) < [obj.PROJECTOR_SCREEN_SIZE(1:2) 1])
                imgR=img;
                imgSizeR = size(imgR);
                toFill = [obj.PROJECTOR_SCREEN_SIZE(1:2) 1] - imgSizeR;
                img = zeros([obj.PROJECTOR_SCREEN_SIZE(1:2) imgSizeR(3)],'uint8');
                for i = 1:3
                    img(1+floor(toFill(1)/2):obj.PROJECTOR_SCREEN_SIZE(1)-ceil(toFill(1)/2),...
                        1+floor(toFill(2)/2):obj.PROJECTOR_SCREEN_SIZE(2)-ceil(toFill(2)/2),i) = imgR(:,:,i);
                end
            end
            
            imgOut = img;
        end
    end
end

