classdef Model < handle & MLSdeformation & CamHelper
    %observable properties, listeners are notified on change
    properties (SetObservable = true)
        dispCtrlPoints
        closestCP
        displayedImg
        
        TCP
        improc
        %preDeformBrightnessData
        applyCorrection
    end
    % read-only properties
    properties (SetAccess = private)
        % Control Points
        ctrlPoints
        selectedCtrlPoints
        settingsCtrlPoints
        expresionList
        CtrlPointsInitDone
        movedCtrlPoint
        
        % Img management
        circBufferImg
        libraryImgs
        assigned2Imgs
        
        
        % saved data
        libraryExpression
        libraryExpressionParsed
        
        
        
        % new
%         imgSize
%         sourceImg
%         calibImg
%         calibData
%         MLSData
%         a
%         t
%         x
        % OLD
        
%         clientSocket
%         dOut
%         img_send
        
        leakPQ
        % import PQ options
        importPropPQ
        
        % HW camera
        nSnapshotDelay
        
        % Checkerboard
        checkerBoardData
    end
    properties (Hidden = true)
        % OLD
        BBBIP = '192.168.7.2';%'192.168.1.50';
        USBIP = '192.168.7.2';
        PORT = 8888;
        BBBHOST = 'root';
        BBBPSWD = 'temppwd';
    end
    events
        CtrlPointsChanged
        ClosestPointChanged
        CtrlPointsStructChanged
        ImgStructChanged
    end
    methods
        % constructor
        function obj = Model(imgPath,imgName,mlsGrid,mlsType,loadCtrlPoints)
            
            obj.leakPQ = 1; % debug MLS
            obj.importPropPQ = [1 1]; % convert double to cell, flip x,y
            
            % img managment parameters (libraryImgs + circBufferImg + selectedImg)
            %obj.selectedImg = 0;
            obj.circBufferImg.enable = true;
            obj.circBufferImg.size = 20; % keep x externaly loaded imgs;
            obj.circBufferImg.imgIDs = [];
            obj.circBufferImg.dispLastImg = true; % display last loaded img
            obj.circBufferImg.enableImgResize = true;
            obj.assigned2Imgs.ToDeform = 1;
            obj.loadImg(imgPath,imgName);
            
            %obj.searchImgDir
            %obj.selectImg(imgName)
            obj.searchExpressionDir
            obj.parseExpressionLib
            if loadCtrlPoints == 1
                %                 clobj.loadExpressionFile(obj.libraryExpressionParsed(1))
            end
            %obj.MLSInit(mlsGrid,mlsType)
            
            obj.mlsBasicInit(obj.libraryImgs(1).imgSize,mlsGrid,mlsType);
            
            obj.closestCP.limit = 7^2;
            obj.closestCP.drawID = NaN;
            obj.closestCP.draw = 0;
            %             obj.f = 2;
            %             obj.a = 2;
            %             obj.t = 0:0.01:10;
            %             obj.x = obj.a * sin(2*pi*obj.f * obj.t);
            
            
            % HW
            obj.TCP = BBBmessage();
            obj.applyCorrection = [false false false];
            % image process functions & calibration img generation
            obj.improc = ImgProcess();
            obj.nSnapshotDelay = 10;
        end
        function delete(obj)
            delete(obj.TCP);
            delete(obj.Cam);

            disp('Exit')
        end
    end
    methods
        %% Control points load
        function searchExpressionDir(obj)
            lib = dir('Model/img/expression');
            obj.libraryExpression = lib(~[lib(:).isdir]);
        end
        function parseExpressionLib(obj)
            names = {obj.libraryExpression.name};
            names = erase(names,'.mat');
            dateNames = datetime(names);
            obj.libraryExpressionParsed = sort(dateNames,'descend');
        end
        function loadExpressionFile(obj,n)
            if isdatetime(n)
                name = datestr(n,30);
                name = strcat(name,'.mat');
                p = fullfile('img','expression',name);
                loadedData= load(p);
                sFields = fieldnames(loadedData);
                %obj.ctrlPoints = getfield(loadedData,sFields{1});   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            else
                error('Pass datetime string')
            end
        end
        %% Control points add
        function id = createNewFaceCtrlPoints(obj,tag)
            id = length(obj.ctrlPoints)+1;
            if nargin < 2
                tag = 'Face Name';
            end
            obj.ctrlPoints(id).tag = tag; % Face name / ctrl points name
            obj.ctrlPoints(id).srcImg = {obj.libraryImgs(obj.assigned2Imgs.ToDeform).Name}; % source img name
            obj.ctrlPoints(id).allowEdit = 1;
            obj.ctrlPoints(id).pointGroupName = {}; % cell with names of sub group of ctrl points
            obj.ctrlPoints(id).selectedPointGroup = 1; % pointer to selected sub group of ctrl points
            obj.ctrlPoints(id).p = {}; % cell of ctrl point (groups)
            obj.ctrlPoints(id).q = {};
            
            % obj.addPointGroup();
            group = 1; % add new sub group of ctrl points
            obj.ctrlPoints(id).pointGroupName{group} = ['Point Group ',num2str(group)];
            % add new group array to cell
            obj.ctrlPoints(id).p{group} = [];
            obj.ctrlPoints(id).q{group} = [];
            obj.ctrlPoints(id).selectedPointGroup = group;
            
            obj.selectDispCtrlPoint(id) % add it to edit mode
            obj.CtrlPointsInitDone = 1;
            
        end
        function loadCtrlPoints(obj,path,file)
            id = [];
            if isempty(obj.CtrlPointsInitDone)
                id = createNewFaceCtrlPoints(obj,file);
            end
            fcont = load(fullfile(path,file));
            n = fieldnames(fcont);
            membr = ismember(n, fieldnames(obj.ctrlPoints(end)));
            if any(membr)
                if isempty(id)
                    id = createNewFaceCtrlPoints(obj,file);
                end
                for i = cell2mat({n{membr}})
                    if isa(fcont.(i), class( obj.ctrlPoints(id).(i)))
                        obj.ctrlPoints(id).(i) = fcont.(i);
                    elseif obj.importPropPQ(1) && any(strcmp(i,{'p','q'})) && isnumeric(fcont.(i))
                        %p,q input is numeric array -> enclose in cell
                        obj.ctrlPoints(id).(i) = {fcont.(i)};
                        if obj.importPropPQ(2)
                            obj.ctrlPoints(id).(i) = {flip(fcont.(i),2)};
                        end
                    end
                end
                selectDispCtrlPoint(obj,id)
            elseif ~isempty(id)
                % delete ctrl point initialized for properties
                obj.ctrlPoints(id) = [];
            end
        end
        function saveCtrlPoints(obj,path,file)
            id = obj.selectedCtrlPoints;
            %% add options
            p = flip(obj.ctrlPoints(id).p{:},2);
            q = flip(obj.ctrlPoints(id).q{:},2);
            save(fullfile(path,file),'p','q');
            obj.ctrlPoints(id).tag = file;
        end
        function addPointGroup(obj)
            id = obj.selectedCtrlPoints; % selected ctrl points
            group = obj.dispCtrlPoints(id).selectedPointGroup + 1; % add new sub group of ctrl points
            obj.dispCtrlPoints(id).pointGroupName{group} = ['Point Group ',num2str(group)];
            % add new group array to cell
            obj.dispCtrlPoints(id).p{group} = [];
            obj.dispCtrlPoints(id).q{group} = [];
            obj.dispCtrlPoints(id).selectedPointGroup = group;
        end
        function selectDispCtrlPoint(obj,id1)
            obj.selectedCtrlPoints = id1;
            obj.dispCtrlPoints = obj.ctrlPoints(id1);
            notify(obj,'CtrlPointsStructChanged')
            notify(obj,'CtrlPointsChanged')
        end
        function addCtrlPoint(obj,px,py)
            if isempty(obj.CtrlPointsInitDone)
                obj.createNewFaceCtrlPoints();
            end
            id1 = obj.selectedCtrlPoints; % selected ctrl points
            id2 = obj.dispCtrlPoints(end).selectedPointGroup; % selected sub group of ctrl points
            obj.dispCtrlPoints(end).p{id2}(end+1,:) = [px,py];
            obj.dispCtrlPoints(end).q{id2}(end+1,:) = [px,py];
            
            notify(obj,'CtrlPointsChanged')
        end
        function closestCtrlPoint(obj,mousePos)
            %mousePos;
            id1 = obj.selectedCtrlPoints; % selected ctrl points
            
            if obj.CtrlPointsInitDone
                if ~any(isempty(obj.dispCtrlPoints(end).p{:}))
                    max_dist = 7^2;
                    id2 = obj.dispCtrlPoints(end).selectedPointGroup; % selected sub group of ctrl points
                    
                    [dist_p,closest_p] = min(sum((mousePos-obj.dispCtrlPoints(end).p{id2}).^2,2));
                    [dist_q,closest_q] = min(sum((mousePos-obj.dispCtrlPoints(end).q{id2}).^2,2));
                    
                    %assign to obj
                    draw = min([dist_q,dist_p]) < obj.closestCP.limit;
                    [~,PorQ] = min([dist_p,dist_q]);
                    if PorQ == 1, drawID = closest_p;
                    else drawID = closest_q;end
                    obj.closestCP.p.id = closest_p;
                    obj.closestCP.p.dist = dist_p;
                    obj.closestCP.q.id = closest_q;
                    obj.closestCP.q.dist = dist_q;
                    if drawID ~= obj.closestCP.drawID || (obj.closestCP.draw ~= draw)
                        obj.closestCP.draw = draw;
                        obj.closestCP.drawID = drawID;
                        notify(obj,'ClosestPointChanged')
                    end
                end
            end
        end
        function deleteClosestPoint(obj)
            if obj.closestCP.draw
                id1 = obj.selectedCtrlPoints; % selected ctrl points
                if ~any(isempty(obj.dispCtrlPoints(end).p{:}))
                    id2 = obj.dispCtrlPoints(end).selectedPointGroup; % selected sub group of ctrl points
                    % if closest point is p delete both p and q
                    if obj.closestCP.p.dist <= obj.closestCP.q.dist && ~isnan(obj.closestCP.p.id)
                        obj.dispCtrlPoints(end).p{id2}(obj.closestCP.drawID,:) = [];
                        obj.dispCtrlPoints(end).q{id2}(obj.closestCP.drawID,:) = [];
                        % else delete only q
                    elseif ~isnan(obj.closestCP.q.id)
                        obj.dispCtrlPoints(end).q{id2}(obj.closestCP.drawID,:) = ...
                            obj.dispCtrlPoints(end).p{id2}(obj.closestCP.drawID,:);
                    end
                    notify(obj,'ClosestPointChanged')
                    obj.closestCP.draw = 0;
                    notify(obj,'CtrlPointsChanged')
                end
            end
        end
        function moveCtrlPoint(obj,mousePos,varargin)
            if nargin == 3 && obj.closestCP.draw
                switch varargin{1}
                    case 'hold'
                        obj.movedCtrlPoint.type = obj.closestCP.q.dist <= obj.closestCP.p.dist && ~isnan(obj.closestCP.q.id);
                        obj.movedCtrlPoint.id = obj.closestCP.drawID;
                        
                    case 'clear'
                        obj.movedCtrlPoint = [];
                end
            end
            if obj.closestCP.draw || ~isempty(obj.movedCtrlPoint)
                id1 = obj.selectedCtrlPoints; % selected ctrl points
                if ~any(isempty(obj.dispCtrlPoints(end).p{:}))
                    id2 = obj.dispCtrlPoints(end).selectedPointGroup; % selected sub group of ctrl points
                    
                    if isempty(obj.movedCtrlPoint)
                        moveQ = obj.closestCP.q.dist <= obj.closestCP.p.dist && ~isnan(obj.closestCP.q.id);
                        moveID = obj.closestCP.drawID;
                    else
                        moveQ = obj.movedCtrlPoint.type;
                        moveID = obj.movedCtrlPoint.id;
                    end
                    if moveQ
                        obj.dispCtrlPoints(end).q{id2}(moveID,:) = mousePos;
                        
                    elseif ~isnan(obj.closestCP.p.id) || ~isempty(obj.movedCtrlPoint)
                        obj.dispCtrlPoints(end).p{id2}(moveID,:) = mousePos;
                        
                    end
                    
                    
                    notify(obj,'ClosestPointChanged')
                    notify(obj,'CtrlPointsChanged')
                end
            end
        end
        function saveDispExpressions(obj,option,name)
            if obj.CtrlPointsInitDone
                id1 = obj.selectedCtrlPoints; % selected ctrl points
                if nargin > 1
                    switch option
                        case 'Rename'
                            if isempty(name), name = strcat('Expression ',num2str(id1));end
                            obj.ctrlPoints(id1).tag = name;
                        case 'Save As'
                            if isempty(name), name = strcat('Expression ',num2str(id1));end
                            obj.dispCtrlPoints(end).tag = name;
                            obj.ctrlPoints(id1+1) = obj.dispCtrlPoints(end);
                            obj.ctrlPoints(id1+1).tag = obj.dispCtrlPoints(end).tag;
                            obj.selectDispCtrlPoint(id1+1)
                        case 'Save'
                            obj.dispCtrlPoints(end).tag = obj.ctrlPoints(id1).tag;
                            obj.ctrlPoints(id1) = obj.dispCtrlPoints(end);
                            obj.ctrlPoints(id1).tag = obj.dispCtrlPoints(end).tag;
                    end
                    notify(obj,'CtrlPointsStructChanged')
                end
            else
                fprintf('Create/Load&Edit control points first\n')
            end
        end
        
        %% Control points save
        function backupExpressions(obj)
            timeString = datestr(now(),30);
            name = strcat(timeString,'.mat');
            p = fullfile('img','expression',name);
            sToSave = obj.ctrlPoints;
            save(p,'sToSave')
        end
        %% Img Load
        function loadImg(obj,path,file)
            if ischar(file)
                file = cellstr(file);
            end
            numOfImg = length(file);
            % load (all selected) img
            for i = 1:numOfImg
                id1 = obj.addImg2lib(path,file{i});
            end
            % display after load
            if obj.circBufferImg.dispLastImg
                obj.displayImg(id1);
            end
            
        end
        function id1 = addImg2lib(obj,path,file,internalimg)
            id1 = length(obj.libraryImgs)+1;
            
            % all matlab supported img formats propreties
            formatInfo = imformats;
            
            obj.libraryImgs(id1).fileName = file;
            %erase file extension and save name
            obj.libraryImgs(id1).Name = erase(file,strcat('.',[formatInfo(:).ext]));
            % keep last .* as file format
            startId = regexp(file,[formatInfo(:).ext]);
            obj.libraryImgs(id1).fileType = file(max([startId{:}]):end);
            obj.libraryImgs(id1).path = path;
            % properties for modified img only
            obj.libraryImgs(id1).children = [];
            obj.libraryImgs(id1).parent = [];
            obj.libraryImgs(id1).pqID = [];
            obj.libraryImgs(id1).filterName = [];
            obj.libraryImgs(id1).filterSettings = [];
            obj.libraryImgs(id1).img = [];
            obj.libraryImgs(id1).imgSize = [];
            obj.libraryImgs(id1).used = false;
            obj.libraryImgs(id1).usedIn = {'task0',sprintf('task%d',id1)};
            obj.libraryImgs(id1).resized = false;
            % load img
            if nargin > 3
                obj.readImg(id1,internalimg);
            else
                obj.readImg(id1)
            end
        end
        function readImg(obj,idIn,img)
            % if buffer size = 0 == disabled or is not full
            if obj.circBufferImg.enable
                if obj.circBufferImg.size < length(obj.circBufferImg.imgIDs)
                    %% implement: try to delete non used
                    [obj.libraryImgs(obj.circBufferImg.imgIDs).used];
                end
                obj.circBufferImg.imgIDs(end+1) = idIn;
                
            end
            path = fullfile(obj.libraryImgs(idIn).path,obj.libraryImgs(idIn).fileName);
            if nargin < 3
                obj.libraryImgs(idIn).img = imread(path);
            else
                obj.libraryImgs(idIn).img = img;
            end
            if obj.circBufferImg.enableImgResize
                obj.resizeImg(idIn);
            end
            
            obj.libraryImgs(idIn).imgSize = size(obj.libraryImgs(idIn).img);
            notify(obj,'ImgStructChanged')
        end
        
        function resizeImg(obj,idIn)
            
            img = obj.libraryImgs(idIn).img;
            % scale img to exactly match one side
            if any([size(img,1),size(img,2),size(img,3)]  ~= [360 640 3])
                imgScales = [360 640 3]./[size(img,1),size(img,2),size(img,3)];
                imgScale = min(imgScales(1:2));
                img = imresize(img, imgScale);
            end
            % fill empty space with black pixels, fill to 3dimenstions
            if any([size(img,1),size(img,2),size(img,3)]  < [360 640 1])
                imgR=img;
                imgSizeR = size(imgR);
                toFill = [360 640 1] - imgSizeR;
                img = uint8(zeros(360, 640, imgSizeR(3)));
                img(1+floor(toFill(1)/2):360-ceil(toFill(1)/2),...
                    1+floor(toFill(2)/2):640-ceil(toFill(2)/2),1) = imgR(:,:,1);
                img(1+floor(toFill(1)/2):360-ceil(toFill(1)/2),...
                    1+floor(toFill(2)/2):640-ceil(toFill(2)/2),2) = imgR(:,:,2);
                img(1+floor(toFill(1)/2):360-ceil(toFill(1)/2),...
                    1+floor(toFill(2)/2):640-ceil(toFill(2)/2),3) = imgR(:,:,3);
                
            end
            
            obj.libraryImgs(idIn).img = img;
        end
        function displayImg(obj,idIn)
            obj.assigned2Imgs.ToDisplay = idIn;
            obj.assigned2Imgs.ToDeform = idIn;
            obj.displayedImg = obj.libraryImgs(idIn).img;
            notify(obj,'ImgStructChanged')
        end
        function saveImgFromLib(obj,path,file)
            id = obj.assigned2Imgs.ToDisplay;
            imwrite(obj.libraryImgs(id).img,fullfile(path,file));
        end
        function renameImageInLib(obj,newName)
            idIn = obj.assigned2Imgs.ToDisplay;
            obj.libraryImgs(idIn).Name = newName;
            notify(obj,'ImgStructChanged')
        end
        function flipImgInLib(obj,fcnName)
            idIn = obj.assigned2Imgs.ToDisplay;
            switch fcnName
                case 'fliplr()'
                    obj.libraryImgs(idIn).img = fliplr(obj.libraryImgs(idIn).img);
                case 'flipud()'
                    obj.libraryImgs(idIn).img = flipud(obj.libraryImgs(idIn).img);
            end
            obj.displayedImg = obj.libraryImgs(idIn).img;
            notify(obj,'ImgStructChanged')
        end
        
        

        
        function searchImgDir(obj)
            % get directory content
            %             lib = dir(fullfile('Model','img'));
            % strip folders
            %             obj.libraryImgs = lib(~[lib(:).isdir]);
        end
        function selectImg(obj,n)
            
            %             if ischar(n) && isnan(str2double(n))
            %                 id = find(strcmp({obj.libraryImgs(:).name},n));
            %             elseif n <= length(obj.libraryImgs)
            %                 id = n;
            %             elseif length(obj.libraryImgs) < n
            %                 % allow easy loop
            %                 id = 1;
            %             else
            %                 error('No such image')
            %             end
            %             img =  imread(fullfile('Model','img',obj.libraryImgs(id).name));
            %
            %
            %
            %             if any(size(img) > [360 640 3])
            %                 imgScales = [360 640 3]./size(img);
            %                 imgScale = min(imgScales(1:2));
            %                 img = imresize(img, imgScale);
            %             end
            %
            %             if any(size(img) < [360 640 1])
            %                 imgR=img;
            %                 imgSizeR = size(imgR);
            %                 toFill = [360 640 1] - imgSizeR;
            %                 img = uint8(zeros(360, 640, imgSizeR(3)));
            %                 img(1+floor(toFill(1)/2):360-ceil(toFill(1)/2),...
            %                     1+floor(toFill(2)/2):640-ceil(toFill(2)/2),1) = imgR(:,:,1);
            %                 img(1+floor(toFill(1)/2):360-ceil(toFill(1)/2),...
            %                     1+floor(toFill(2)/2):640-ceil(toFill(2)/2),2) = imgR(:,:,2);
            %                 img(1+floor(toFill(1)/2):360-ceil(toFill(1)/2),...
            %                     1+floor(toFill(2)/2):640-ceil(toFill(2)/2),3) = imgR(:,:,3);
            %
            %             end
            %
            %             obj.imgSize = size(img);
            %             obj.displayedImg = img;
            %             obj.sourceImg.img = img;
            %             obj.sourceImg.name = obj.libraryImgs(id).name;
            %             obj.sourceImg.id = id;
        end
        
        
        
        
        %
%         function changeF(obj,f)
%             obj.f = f
%             obj.x = obj.a * sin(2*pi*obj.f * obj.t);
%             obj.notify('EventFrequencyChanged');
%             disp('f changed in model, event sent...')
%         end
%         
%         %% OLD
%         %% bbb c Server
%         function prepDLP(obj)
%             % Connects to BBB, establishes connection to projector, starts img server
%             closeDLP(obj);
%             
%             cmd = 'i2cset -y 2 0x1b 0x0b 0x00 0x00 0x00 0x00 i \n i2cset -y 2 0x1b 0x0c 0x00 0x00 0x00 0x1b i \n service lightdm stop \n TERM=linux setterm --blank force </dev/tty1 \n /home/debian/src/fbtemp &';
%             fileID = fopen('start_server.txt','w'); fprintf(fileID,cmd); fclose(fileID);
%             system(sprintf('putty %s@%s -pw %s -m ".\\start_server.txt" &',obj.BBBHOST,obj.BBBIP,obj.BBBPSWD));
%             % to verify fbtemp is running cmd: ps aux
%             pause(2);
%             tries = 0;
%             while tries < 4
%                 try
%                     openSocket(obj);
%                     break;
%                 catch e
%                     tries=tries+1;
%                     pause(1)
%                 end
%             end
%             
%         end
%         function closeDLP(obj)
%             % Connects to BBB, stops img server
%             fileID = fopen('stop_server.txt','w'); fprintf(fileID,'killall fbtemp \n service lightdm start'); fclose(fileID);
%             system('putty root@192.168.1.50 -pw temppwd  -m ".\stop_server.txt" &');
%         end
%         %% Socket
%         function openSocket(obj)
%             % java net library socket
%             import java.net.Socket
%             import java.io.DataOutputStream
%             import java.io.BufferedInputStream
%             import java.io.BufferedReader
%             import java.io.PrintWriter
%             
%             % start connection
%             obj.clientSocket = Socket(obj.BBBIP, obj.PORT);
%             obj.dOut = DataOutputStream(obj.clientSocket.getOutputStream());
%         end
%         
%         function closeSocket(obj)
%             % stop connection
%             try
%                 obj.dOut.close();
%                 obj.clientSocket.close();
%             catch e
%                 if strcmp(e.indetifier=='MATLAB:structRefFromNonStruct')
%                     fprintf('Process does not exist')
%                 else
%                     rethrow(e)
%                 end
%                 
%                 %fprintf(e)
%             end
%             % clear object
%             obj.dOut = [];
%             obj.clientSocket = [];
%         end
        %% Send img
        function [imgOut,restore] = SendImgAndSnapImg(obj, imgIn, restore)
            
            % open camera (if not open)
            stateChgCam = obj.CamOpen();
            if stateChgCam
                obj.Cam.preview;
            end
            
            % are we connected to BBB?
            if ~obj.TCP.isopen
                error('Connect to projector');
                return;
            end
            
            % turn on projector light (if needed)
            stateProjector = obj.TCP.DISP_CTRLconf;
            if ~stateProjector
                obj.TCP.TurnONProjector;
            end
            
            % send image to BBB
            obj.TCP.SendImgSafe(imgIn);
            
            pause(1) % wait for moment
            
            for i = 1:obj.nSnapshotDelay % take few images to allow for autobrightness
               imgOut = obj.Cam.snapshot();
               pause(0.2);
            end
             imgOut = obj.correctSnapshot();
                
            
            
            if all(isempty(restore))
                % for firstImg
                restore = [stateChgCam, stateProjector];
            elseif all(restore) && length(restore) == 1
                % clean up
                if stateChgCam, obj.CamClose(); end
                if ~stateProjector, obj.TCP.TurnOFFProjector; end
            elseif length(restore) == 2
                if restore(1), obj.CamClose(); end
                if ~restore(2), obj.TCP.TurnOFFProjector; end
            end
        end
        %% Dark field, clear field
        function [imgOut,restoreState] = sendImgForBrigthnessCorrect(obj,imgIn,restore,pos, multiplyDelayOnFirstImg)
            if ~isa(imgIn,'double')
                imgIn = im2double(imgIn);
            end
            
            % designate corners
            cornerPosition{1} = [1 0; 0 -1];
            cornerPosition{3} = flip( cornerPosition{1},1 );
            if isempty(pos)
                cornerPosition{4} = flip(cornerPosition{1},2);
                cornerPosition{2} = flip(cornerPosition{3},2);
            end
            
            % generate overlay for imgIn
            idCorner = find(cellfun(@isempty,cornerPosition)==0);
            cornerSize = [size(imgIn,1),size(imgIn,2)]./4;
            
            
            for i = idCorner  %add overlay
                img2Send{i} = obj.improc.addCornersToImg(imgIn, cornerSize, cornerPosition{i});
            end
            
            tSD = obj.nSnapshotDelay; % save delay
            % take snap + alter settings if neccesary
            for i = idCorner(1:end-1)
                if i == 1 && multiplyDelayOnFirstImg
                    
                    obj.nSnapshotDelay = tSD*multiplyDelayOnFirstImg;     % increase delay
                end
                
                [imgOut(i).img, r] = obj.SendImgAndSnapImg(img2Send{i},[]);
                imgOut(i).cornerPosition = cornerPosition{i};
                
                if i == 1
                    restoreState = r;
                    obj.nSnapshotDelay = tSD; % reset delay
                end
                
            end
            
            
            % take last snap + restore projector & camera if selected
            if islogical(restore) && ~all(restore)
                RestoreArg = []; % dont't restore
            elseif islogical(restore) && all(restore)
                RestoreArg = restoreState; % restore after yourself
            else
                RestoreArg = restore;
            end
            
            [imgOut(idCorner(end)).img, ~] = obj.SendImgAndSnapImg(img2Send{idCorner(end)},RestoreArg);
            imgOut(idCorner(end)).cornerPosition = cornerPosition{idCorner(end)};
            
            % match histograms J = (I,ref) between individual corner snaps
            for i = idCorner(2:end)
                imgOut(i).img = imhistmatch(imgOut(i).img,imgOut(1).img);
            end
        end
        function [darkFieldStruct,clearFieldStruct] = acqDarkFieldClearField(obj,snapshotDelayMultiply)
            if isempty(obj.improc.CamPlane)
                error('Please Run / Load Plane-Camera calibration first.')
            end
            if obj.TCP.isopen % connected to BBB
                if nargin < 2
                    snapshotDelayMultiply = 2; % takes longer to adjust expousure on first image
                end
                [darkFieldStruct,restoreState] = obj.sendImgForBrigthnessCorrect(obj.improc.DarkField_TestPattern,false,[],snapshotDelayMultiply);
                [clearFieldStruct,~] = obj.sendImgForBrigthnessCorrect(obj.improc.ClearField_TestPattern,restoreState,[],snapshotDelayMultiply);
            else % demo Only
                fprintf('Not connected to BBB, loading from demo file\n')
            end
            
            % warp and crop image (TForm saved from camera - projection plane calibration)
            for i = 1:length(darkFieldStruct)
                darkFieldStruct(i).img = obj.improc.WarpAndCropImg(darkFieldStruct(i).img);
            end
            for i = 1:length(clearFieldStruct)
                clearFieldStruct(i).img = obj.improc.WarpAndCropImg(clearFieldStruct(i).img);
            end
            
            obj.improc.darkFieldStruct = darkFieldStruct;
            obj.improc.clearFieldStruct = clearFieldStruct;
        end
        
        function [darkFieldImg,clearFieldImg] = runDarkFieldClearField(obj)
            if isempty(obj.improc.darkFieldStruct) || isempty(obj.improc.clearFieldStruct)
                error('Run acquisition first')
            end
            darkFieldStruct = obj.improc.darkFieldStruct;
            clearFieldStruct = obj.improc.clearFieldStruct;
            
            %detecting brightness calib corners
            [darkFieldStruct,darkFieldImg,HistValDark,mask, c1, c2, c3, c4] = obj.improc.findCornersInImg( darkFieldStruct);
            figure; imshow(imfuse(darkFieldImg,mask)), title('Detected brigthness calibration spots')
            
            for i = 1:length(clearFieldStruct)
                % copy masks from darkfield to clearfield to save time
                % (corners are on same spot)
                clearFieldStruct(i).mask = darkFieldStruct(i).mask;
            end
            [clearFieldStruct,clearFieldImg,HistValClear] = obj.improc.findCornersInImg( clearFieldStruct,1, c1, c2, c3, c4);
            
            % adjust clearfield to match min and max values of darkfield
            m = diff(mean(HistValDark,2))./diff(mean(HistValClear,2));
            clearFieldImg = clearFieldImg*m - (mean(HistValClear(2,:)).*m -  mean(HistValDark(2,:)));
            if max(clearFieldImg(:)) > 1
                s = (1 - max(clearFieldImg(:)));
                clearFieldImg =  clearFieldImg+s ;
                clearFieldImg(clearFieldImg<0) = 0;
                
                darkFieldImg = darkFieldImg +s;
                darkFieldImg(darkFieldImg<0) = 0;
            end
            
            % save finnished img
            obj.improc.darkfieldImg = darkFieldImg;
            obj.improc.clearfieldImg = clearFieldImg;
            
            obj.improc.darkFieldStruct = darkFieldStruct;
            obj.improc.clearFieldStruct = clearFieldStruct;
            
            % undeform img if inverse function saved
            try
            if ~isempty(obj.improc.calibOutData) && ~isempty(obj.improc.calibOutData.unDefrom)
                obj.improc.calibOutData.darkFieldCrop = obj.improc.cropNaNAndResize(obj.improc.darkfieldImg);
                obj.improc.calibOutData.clearFieldCrop = obj.improc.cropNaNAndResize(obj.improc.clearfieldImg);
                if obj.improc.calibOutData.fliplr
                    obj.improc.calibOutData.darkFieldCrop = fliplr(obj.improc.calibOutData.darkFieldCrop);
                    obj.improc.calibOutData.clearFieldCrop = fliplr(obj.improc.calibOutData.clearFieldCrop);
                end
                if obj.improc.calibOutData.flipud
                    obj.improc.calibOutData.darkFieldCrop = flipud(obj.improc.calibOutData.darkFieldCrop);
                    obj.improc.calibOutData.clearFieldCrop = flipud(obj.improc.calibOutData.clearFieldCrop);
                end                
                obj.improc.calibOutData.darkFieldCropCorr = obj.interpolateFromSaved(obj.improc.calibOutData.unDefrom,uint8(obj.improc.calibOutData.darkFieldCrop*256));
                obj.improc.calibOutData.clearFieldCropCorr = obj.interpolateFromSaved(obj.improc.calibOutData.unDefrom,uint8(obj.improc.calibOutData.clearFieldCrop*256));
            end
            catch ME
     
            end
            
            
        end
        %% QR
        function outIm = acqQRCalibCam2Plane(obj,shImg)
            % open camera (if not open)
            stateChgCam = obj.CamOpen();
            if stateChgCam
                obj.Cam.preview;
            end
            
            if nargin <2
                shImg = 1;
            end
            
            for i = 1:10 % take few images to allow for autobrightness
                outIm = obj.Cam.snapshot();
            end
            outIm = obj.correctSnapshot();
            obj.improc.CamPlane.rawImg = outIm;
            
            % close camera?
            if stateChgCam, obj.CamClose(); end
            
            if shImg
                figure, imshow(outIm); title('QR raw img')
            end
        end
        
        function outStr = runQRCalibCam2Plane(obj,shImg, ImageBinarizationSensitivity,QRMarkLineThickness, QRMarkPatternPolicy, QRMarkPatternTolerance)
            if isempty(obj.improc.CamPlane.rawImg)
                error('Run acquisition first')
            else
                img = obj.improc.CamPlane.rawImg;
            end

            if nargin < 2
                shImg = 1;
            end            
            if nargin < 3
                ImageBinarizationSensitivity = 0.2; % if lines in QR mark too thick lower sensitivity
            end
            if nargin < 4
                QRMarkLineThickness = 3; % lower if lines in QR mark are too thin and getting deleted ( red pixels)
            end
            
            % binarize image, enhance, find hollow objects
            [bwHollow, bwEnhanced, ~, RGBVisual] = obj.improc.AdaptiveBinarizeAndFilterHollowObjects(img,...
                ImageBinarizationSensitivity,QRMarkLineThickness);
            
            if shImg
                % display binarized image fused with image in false colur
                % RGBVisual visualizes operations on binarized image (for details open AdaptiveBinarizeA...)
                figure, imshowpair(imfuse(im2double(img),RGBVisual,'falsecolor'),RGBVisual,'montage');
            end
            
            if nargin < 5
                QRMarkPatternPolicy = '3of3'; % horizontal, vertical, diagonal
            end
            if nargin < 6
                QRMarkPatternTolerance = 0.6; % tolerance of thickness difference ratio (0.5 acc to QR specs)
            end
            
            % for each centroid of hollow objects take stripe and compare it to QR
            % Marker ratio [1 1 3 1 1], option of directinon xofy
            % y = 1 |; y=2 | & _; y=3 | & _  &\; x = how many direction must match
            % ratio with given accuracy (0.5)
            [labeledMask, markersPosition] = obj.improc.findMarkers(bwHollow,bwEnhanced,...
                QRMarkPatternPolicy,QRMarkPatternTolerance);
            
            if shImg
                % overlay found qr markers on image
%                 figure, imshow(imfuse(im2double(img),~labeledMask,'blend'));
                hold on, plot(markersPosition(:,1),markersPosition(:,2),'r*')
            end
            if length(markersPosition) ~=  4
                error("Can't find markers adjust binarization options / take better image")
                return
            end
            % find transformation
            [tform, refP, PlaneRectangle,imScale] = obj.improc.fitGeoTransformationPreservingPhysicalDim(markersPosition);
            
            % save TForm
            obj.improc.CamPlane.TForm = tform;
            obj.improc.CamPlane.QRMarkersPos = markersPosition;
            obj.improc.CamPlane.Border = PlaneRectangle;
            obj.improc.CamPlaneCalibrationSaved = true;
            outStr = obj.improc.CamPlane;
            
            if shImg
                % apply transformation
                [outIm2, markersOutIm2, outIm1, markersOutIm1, rectOutIm1] = obj.improc.WarpAndCropImg(img,tform,markersPosition,PlaneRectangle);
                
                % show warped image
                figure;imshow(outIm1); title('Warped image'), hold on;
                plot(markersOutIm1(:,1),markersOutIm1(:,2),'r*')
                rectangle('Position',rectOutIm1);
                
                % show warped and cropped image
                figure; imshow(outIm2); title('Warped and cropped image'),
                hold on; plot(markersOutIm2(:,1),markersOutIm2(:,2),'r*');
            end
        end
        
        
        function imgOut = calibrateProjector2Plane(obj,shImg,gridSize,delaySnap)
            %shImg = debug images
            gridSize = [15 15]; delaySnap = 2;
            obj.sendCheckerboard(gridSize,delaySnap);
            
            [dX,dY,gridM] = obj.detectCheckerboard();
            
            obj.findPreDefrom(gridM)
            
            newImg = uint8(zeros(size(I)));
            newImg = obj.interpolate1(obj.checkerBoardData.TestImg ,newImg);
            
            stateProjector = obj.TCP.DISP_CTRLconf;
            if ~stateProjector
                obj.TCP.TurnONProjector;
            end
            
            % send corrected image to BBB
            obj.TCP.SendImgSafe(fliplr(newImg));
            
            
        end
        function [TestImg,TestImg2] = sendCheckerboard(obj,gridSize,delaySnap)
            % generate X,Y, calibration image
            [TestImg, X,Y]= obj.improc.testCheckerboard(obj.improc.TestImgSize,[15 15],0);
            
            obj.checkerBoardData.gridSize = gridSize;
            obj.checkerBoardData.TestImg = TestImg;
            obj.checkerBoardData.X = X;
            obj.checkerBoardData.Y = Y;
            
            if nargin > 2
                cX = round([0.3*length(X),0.3*length(X),0.7*length(X)]);
                cY = round([0.3*length(Y),0.7*length(Y),0.3*length(Y)]);
                
                [X2,Y2,cX,cY, TestImg2] = obj.improc.CheckerboardGridAlignPoints(cX,cY,X,Y,TestImg);
                
                obj.checkerBoardData.cX = cX;
                obj.checkerBoardData.cY = cY;
                obj.checkerBoardData.X2 = X2;
                obj.checkerBoardData.Y2 = Y2;
                obj.checkerBoardData.TestImg2 = TestImg2;
                
                % send img & snap
                tSD = obj.nSnapshotDelay; % save delay
                obj.nSnapshotDelay = tSD*delaySnap;
                
                [imgOut1,r] = obj.SendImgAndSnapImg(TestImg, []);
                imgOut2 = obj.SendImgAndSnapImg(TestImg2, r);
                
                obj.nSnapshotDelay = tSD;
                
                % image adjust perspective
                imOut1 = obj.improc.WarpAndCropImg(imgOut1);
                imOut1 = im2double(rgb2gray(imOut1));
                
                imOut2 = obj.improc.WarpAndCropImg(imgOut2);
                imOut2 = im2double(rgb2gray(imOut2));
                
                obj.checkerBoardData.imgOut1 = imgOut1;
                obj.checkerBoardData.imgOut2 = imgOut2;
                obj.checkerBoardData.imOut1 = imOut1;
                obj.checkerBoardData.imOut2 = imOut2;
            else
                % turn on projector light (if needed)
                stateProjector = obj.TCP.DISP_CTRLconf;
                if ~stateProjector
                    obj.TCP.TurnONProjector;
                end
                
                % send image to BBB
                obj.TCP.SendImgSafe(TestImg);
            end
        end
        
        function [dX,dY,gridM] = detectCheckerboard(obj)
            if isempty(obj.checkerBoardData.imOut1)
                error('Do acquisition first')
            end
            [I, ~] = obj.improc.AdjustAndCropCapturedImg(obj.checkerBoardData.imOut1,1,true);
            
            % find corners
            [x,y,theta,phaseF2rd] = obj.improc.findCorners(I,0);
            
            % correlation
            [cornerMetric,cornerMetricAreaLIM] = obj.improc.getCornerMetric(I,x,y,theta,phaseF2rd, 3, 3, 0.5, true);
            
            figure;imshow(I),hold on,plot(x,y,'r.'),plot(x(cornerMetric>0),y(cornerMetric>0),'y.'), iiii =  cornerMetric > cornerMetricAreaLIM; plot(x(iiii),y(iiii),'b*');
            plot(x(cornerMetric==max(cornerMetric)),y(cornerMetric==max(cornerMetric)),'bo');
            figure; hist(theta(iiii),[2000])
            
            % grid strucutre
            obj.improc.cornerValidationInit(I, x, y, theta, cornerMetric,cornerMetricAreaLIM);
            gridM = obj.improc.searchForCorners;
            
            
            for i = 0:3
                gridM = rot90(gridM,i);
                tID = gridM(:,1);
                tID(tID<1) = [];
                if ~any([obj.improc.pS(tID).cornerMetric]==max(cornerMetric))
                    gridM(:,1) = [];
                end
                gridM(ismember(gridM,tID([obj.improc.pS(tID).cornerMetric]<cornerMetricAreaLIM))) = 0;
                gridM = rot90(gridM,-i);
            end  
            
            
            dX = ones(size(gridM)).*NaN;
            dY = dX;
            for i = 1:length([gridM(:)])
                if gridM(i) > 0
                    dX(i) = obj.improc.pS(gridM(i)).x;
                    dY(i) = obj.improc.pS(gridM(i)).y;
                end
            end
            plot(dX,dY,'b')
            plot(dX',dY','b')
            
            obj.improc.gridM = gridM;
            obj.checkerBoardData.imOut1Crop = I;
            
            
        end
        
        function stat = findPreDefrom(obj,gridM)
            if isempty(obj.checkerBoardData.imOut2)
                error('Do acquisition first')
            end
            
            if nargin < 2
                error('Pass deteceted grid as argument')
            end
            
            %FIND DEFORMATION 1
            
            % second img
            % image adaptive binarize (optional to )
            I = obj.checkerBoardData.imOut1Crop;
            [I2, stdDD] = obj.improc.AdjustAndCropCapturedImg(obj.checkerBoardData.imOut2,0,false);
            obj.checkerBoardData.imOut2Crop = I2;

            
            [x,y,theta,phaseF2rd] = obj.improc.findCorners(I2,stdDD);
            
            [cornerMetric,cornerMetricAreaLIM] = obj.improc.getCornerMetric(I2,x,y,theta,phaseF2rd, 3, 3, 0.5, true);
            figure; hist(cornerMetric,[20000])
            
            
            figure;imshow(I2),hold on,plot(x,y,'r.'),plot(x(cornerMetric>0),y(cornerMetric>0),'y.'), iiii =  cornerMetric > cornerMetricAreaLIM; plot(x(iiii),y(iiii),'b*');
            plot(x(cornerMetric==max(cornerMetric)),y(cornerMetric==max(cornerMetric)),'bo');
            
            
            % register correspondig row, cols
            if size(x,1) < size(x,2), x = x';end
            if size(y,1) < size(y,2), y = y';end
            
            [v,i] = min(sqrt(([obj.improc.pS.x]-x).^2  + ([obj.improc.pS.y]-y).^2 ),[],2);
            [~, jj]=mink(v,3); i = i(jj);
            
            cX = obj.checkerBoardData.cX;
            cY =  obj.checkerBoardData.cY;
            X2 = obj.checkerBoardData.X2;
            Y2=  obj.checkerBoardData.Y2;
            % orientation detection
            for k = 1:length(cY)
                [ccY(k,1), ccX(k,1)] = find(gridM == i(k));
                [ccY(k,2), ccX(k,2)] = find(fliplr(gridM) == i(k));
                [ccY(k,3), ccX(k,3)] = find(flipud(gridM) == i(k));
                [ccY(k,4), ccX(k,4)] = find(flipud(fliplr(gridM)) == i(k));
            end
            obj.improc.calibOutData.darkFieldCrop = obj.improc.cropNaNAndResize(obj.improc.darkfieldImg);
            obj.improc.calibOutData.clearFieldCrop = obj.improc.cropNaNAndResize(obj.improc.clearfieldImg);
            obj.improc.calibOutData.pS = obj.improc.pS;
            % find orientation relative to source image + flip
            [v,idx] = min(abs(sum([median(cY);median(cX)] - [median(ccY);median(ccX)])));
            obj.improc.calibOutData.fliplr = false;
            obj.improc.calibOutData.flipud = false;
            if ~mod(idx,2) % fliplr
                gridM = fliplr(gridM);
                temp = num2cell(-[obj.improc.calibOutData.pS.x] + max(obj.checkerBoardData.X));
                [obj.improc.calibOutData.pS.x] = temp{:};
                obj.improc.calibOutData.darkFieldCrop = fliplr(obj.improc.calibOutData.darkFieldCrop);
                obj.improc.calibOutData.clearFieldCrop = fliplr(obj.improc.calibOutData.clearFieldCrop);
                obj.checkerBoardData.imOut1Crop = fliplr(obj.checkerBoardData.imOut1Crop);
                obj.checkerBoardData.imOut2Crop = fliplr(obj.checkerBoardData.imOut2Crop);
                obj.improc.calibOutData.fliplr = true;
            end
            if idx > 2
                gridM = flipud(gridM);                
                temp = num2cell(-[obj.improc.calibOutData.pS.y] + max(obj.checkerBoardData.Y));
                [obj.improc.calibOutData.pS.y] = temp{:};
                obj.improc.calibOutData.darkFieldCrop = flipud(obj.improc.calibOutData.darkFieldCrop);
                obj.improc.calibOutData.clearFieldCrop = flipud(obj.improc.calibOutData.clearFieldCrop);
                obj.checkerBoardData.imOut1Crop = flipud(obj.checkerBoardData.imOut1Crop);
                obj.checkerBoardData.imOut2Crop = flipud(obj.checkerBoardData.imOut2Crop);
                obj.improc.calibOutData.flipud = true;
            end
            
            ccX = ccX(:,idx);
            ccY = ccY(:,idx);
                
            % trim /extend grid Y
            beforeY = unique(median(cY) - median(ccY));
            
            if beforeY > 0
                gridM = [zeros(beforeY,size(gridM,2));gridM];
            else
                gridM(1:abs(beforeY),:) = [];
            end
            
            afterY =  unique(size(X2,1) - size(gridM,1));
            if afterY > 0
                gridM = [gridM;zeros(afterY,size(gridM,2))];
            else
                gridM((end-abs(afterY)+1):end,:) = [];
            end
            
            % trim /extend grid X
            beforeX = unique(median(cX) - median(ccX));
            if beforeX > 0
                gridM = [zeros(size(gridM,1),beforeX),gridM];
            else
                gridM(:,1:abs(beforeX)) = [];
            end
            
            afterX =  unique(size(X2,2) - size(gridM,2));
            if afterX > 0
                gridM = [gridM,zeros(size(gridM,1),afterX)];
            else
                gridM(:,(end-abs(afterX)+1):end) = [];
            end
            
            obj.checkerBoardData.gridMCrop = gridM;
            
            % read corresponding x,y
            dX = ones(size(gridM)).*NaN;
            dY = dX;
            
            for i = 1:length([gridM(:)])
                if gridM(i) > 0
                    dX(i) = obj.improc.calibOutData.pS(gridM(i)).x;
                    dY(i) = obj.improc.calibOutData.pS(gridM(i)).y;
                end
            end
            
            X2 = obj.checkerBoardData.X2;
            Y2 = obj.checkerBoardData.Y2;
            % find scale -> F(1/F*p)
            kx = ( max(mean(dX,1,'omitnan')) - min(mean(dX,1,'omitnan')) ) / (max(X2(~isnan(dX))) - min(X2(~isnan(dX))));
            X2a = kx .* X2 + (min(mean(dX,1,'omitnan'))- min(X2(~isnan(dX)))*kx );
            
            ky = ( max(mean(dY,2,'omitnan')) - min(mean(dY,2,'omitnan')) ) / (max(Y2(~isnan(dY))) - min(Y2(~isnan(dY))));
            Y2a = ky .* Y2 + (min(mean(dY,2,'omitnan'))- min(Y2(~isnan(dY)))*ky );
            
            figure
            imshow(obj.checkerBoardData.TestImg),hold on; plot(X2(~isnan(dX)),Y2(~isnan(dY)),'r.'), title('Detected corners referenced to source image')
            
            figure
            imshow(obj.checkerBoardData.imOut1Crop),hold on;
            plot(X2a(~isnan(dX)),Y2a(~isnan(dY)),'g.')
            plot(dX,dY,'r.')
            quiver(dX,dY,X2a-dX,Y2a-dY,2,'w')
            legend('q','F(p)')
            title('Desired position q in deformed image')
            drawnow;
            
            p = [dY(~isnan(dY)),dX(~isnan(dX))];
            q = [Y2(~isnan(dY)),X2(~isnan(dX))];
            Fq = [Y2a(~isnan(dY)),X2a(~isnan(dX))];
            fprintf('Calculating MLS F(p)->p, this might take upto 5 min\n')
            % 1/F
            obj.mlsPrecomupte(p);
            obj.mlsSimilarity(q);
            % 1/F * p
            iFq = obj.MLSinterpolatePoints(Fq);
            
            figure
            imshow(obj.checkerBoardData.TestImg),hold on; plot(p(:,2),p(:,1),'r.'),plot(iFq(:,2),iFq(:,1),'g.')
               quiver(p(:,2),p(:,1),iFq(:,2)-p(:,2),iFq(:,1)-p(:,1),2,'y')
            legend('p','F^{-1}(q)')
            title('Pre-deform position F^{-1}(q) in src img')
            drawnow;
            
            obj.improc.calibOutData.unDefrom =  obj.saveDeformationStruct();
            obj.improc.calibOutData.darkFieldCropCorr = obj.interpolateFromSaved(obj.improc.calibOutData.unDefrom,uint8(obj.improc.calibOutData.darkFieldCrop*256));
            obj.improc.calibOutData.clearFieldCropCorr = obj.interpolateFromSaved(obj.improc.calibOutData.unDefrom,uint8(obj.improc.calibOutData.clearFieldCrop*256));
           
            fprintf('Calculating MLS p->F^-1(q), this might take upto 5 min\n')
            obj.mlsPrecomupte(q);
            obj.mlsSimilarity(iFq); % add output struct Fx,Fy,X,Y
            obj.improc.calibOutData.preDefrom = obj.saveDeformationStruct();
            stat = 0;
            fprintf('Done\n')
        end
        
        %         function sendImg(obj,varargin)
        %             % list [B G R A, B G R A, B G R A]SUD
        %             %obj.img_send = obj.MLS.imgDeformed;
        %
        %             obj.img_send = obj.displayedImg;
        %             size(obj.img_send)
        %             %     if (obj.cal.use_brightness == 1) & (obj.calibSW == 0)
        %             %         obj.img_send = uint8((double(obj.img_send)-obj.cal.dark)./(obj.cal.clear - obj.cal.dark).*obj.cal.alpha.*256);
        %             %     end
        %             if (isempty(varargin)) %& (obj.calibSW == 0)
        %                 r = obj.img_send(:,:,1)';
        %                 g = obj.img_send(:,:,2)';
        %                 b = obj.img_send(:,:,3)';
        %                 a = r*255;
        %
        %             else
        %                 r = ~obj.img_send(:,:,1)'.*200;
        %                 g = obj.img_send(:,:,2)'.*0;
        %                 g=r;
        %                 b = obj.img_send(:,:,3)'.*0;
        %                 b=r;
        %                 a = r*200;
        %             end
        %             D = horzcat(b(:),g(:),r(:),a(:))';
        %
        %             msg = D(:);
        %             obj.dOut.write(msg)
        %         end
        %% MLS
        function animateMLS(obj,parameters)
            id1 =  parameters{1};
            k1 = parameters{2};
            id2 = parameters{3};
            k2 = parameters{4};
            p = obj.ctrlPoints(id1).p{:} + k1.*(obj.ctrlPoints(id1).q{:} - obj.ctrlPoints(id1).p{:});
            q = obj.ctrlPoints(id2).p{:} + k2.*(obj.ctrlPoints(id2).q{:} - obj.ctrlPoints(id2).p{:});
            k3 = parameters{5};
            k4 = parameters{6};
            fps = parameters{7};
            t = parameters{8};
            k = k3:(k4-k3)/(t*fps):k4;
            
            % flip x and y
            p = flip(p,2);
            q = flip(q,2);
            
            
            if obj.leakPQ
                assignin('base','p',p)
                assignin('base','q',q)
            end
            
            obj.mlsPrecomupte(p);
            obj.mlsSimilarity(q);
            SourceImg = obj.libraryImgs(obj.assigned2Imgs.ToDeform);
            newImg = uint8(zeros(SourceImg.imgSize));
            
       
            newImg = obj.interpolate1(SourceImg.img,newImg);
            
            startIndex = regexp(SourceImg.Name,'-deformed');
            if length(startIndex) > 1
                startIndex = startIndex(end);
            end
            tempName = SourceImg.Name;
            tempName(startIndex:end) = [];
            tempName = strcat(tempName,'-deformed');
            if ismember(tempName,{obj.libraryImgs(:).Name})
                i = 1;
                while ismember(strcat(tempName,num2str(i)),{obj.libraryImgs(:).Name})
                    i = i+1;
                end
                tempName = strcat(tempName,num2str(i));
            end
            id1 = obj.addImg2lib('',tempName,newImg);
            obj.displayImg(id1) ;
            
            %             fprintf('Precomputation took: ')
            %             MLSPrecompute(obj,p);
            %             fprintf('\nDeformation took: ')
            %             MLSDeform(obj,p,q);
            %             fprintf('\n--------------------\n');
            
            % obj.displayedImg = obj.MLSData.imgDeformed;
            
            
        end
        
        %% projector + correction
        function sendCorrectedImage(obj,img)
            if nargin < 2
                img = obj.displayedImg;
            end
            
            if obj.applyCorrection(1)
                img = obj.interpolateFromSaved(obj.improc.calibOutData.preDefrom,img);
            end
            if obj.applyCorrection(2) || obj.applyCorrection(3)
                
                corI = (obj.improc.calibOutData.clearFieldCropCorr - obj.improc.calibOutData.darkFieldCropCorr);
                %corI = imcomplement(corI);
                [counts,binLocations] = imhist(corI);
                
                [pks,locs,w,p] = findpeaks(counts,binLocations);
                % find the peak
                [pks,locs,w,p] = findpeaks(counts,binLocations,'MinPeakProminence',max(p));
                corI2 = im2double(corI).*(256/(locs+w)); corI2(corI2>1) = 1; corI2(corI2<0) = 0;
                % widen, invert image, upper bound of peak to 1
                corI = imcomplement(im2double(corI).*(256/(locs+w)))+(locs-w)*(1/(locs+w));
                corI(corI>1) = 1; corI(corI<0) = 0; %saturation
                % correct brightness - (1-256) .* (0-1)
                if obj.applyCorrection(2)
                    img = uint8(double(img) .* corI);
                else
                    img = uint8(double(img) .* corI2);
                end
            end
            if obj.TCP.isopen
                if ~obj.TCP.DISP_CTRLconf 
                    obj.TCP.TurnONProjector;
                end            
                % send image to BBB
                obj.TCP.SendImgSafe(img);     
            else
                fprintf('Not connected to BBB yet\n')
                figure;imshow(img), title('Not connected to BBB, imshow() instead')
            end
            
        end
    end
end
