classdef MLSdeformation < handle
    %UNTITLED2 Summary of this class goes here
    %   Detailed explanation goes here
    
    properties %(Hidden = true)
        mlsImgSize %[x,y,z]
        mlsVStep % [x,y] distance between evaluation points v
        mlsType % deformation method (by assumed defromation function restrction)
        mlsAlpha
        
        % Evaluation grid
        mlsV
        mlsVy
        mlsVx
        
        mlsX
        mlsY
        
        % Precomputation
        p_i
        w_i
        sum_w_i
        p_ast
        Ai
        Ai_mu
        mls_pp % (temp)
        
        % Computation
        mlsFx
        mlsFy
    end
    
    methods
        %% (re)load
        % Set method properties fresh start or load from old class
        % either during class initialization or after by calling respective method
        function obj = MLSdeformation(varargin)
            % Construct an instance of this class
            if nargin == 1
                % read properties from passed object
                mlsReLoad(obj,varargin{:});
            elseif nargin > 1
                % assign values to properties
                mlsBasicInit(obj,varargin{:})
            end
        end
        function mlsReLoad(obj,varargin)
            % load from dumped obj
            if  strcmp(varargin{1},'load') &&  (nargin == 3)
                obj.Property1 = in.Property1;
                % load
            else
                obj.Property1 = varargin{1} + varargin{2};
            end
        end
        %% Initialization methods
        function mlsBasicInit(obj,inputArg1,inputArg2,inputArg3)
            %METHOD mlsBasicInit assigns values to properties mlsImgSize,mlsVStep; than calls mlsGetEvalGrid
            % expected input: mlsBasicInit([size(img)],[x,y],'string')
            if ~all((size(inputArg1) == [1 3]) & (size(inputArg2) == [1 2]))
                error('expected input: mlsBasicInit(size(img),[x,y])')
            end
            
            obj.mlsImgSize = inputArg1;
            obj.mlsVStep = inputArg2;
            
            obj.mlsAlpha = 1;
            
            obj.mlsUseMethod(inputArg3);
            obj.mlsGetEvalGrid;
        end
        function mlsGetEvalGrid(obj)
            %METHOD mlsGetEvalGrid calculates X and Y coodinates of evaluation points v
            %fprintf('mlsGetEvalGrid\n')
            
            obj.mlsX = obj.mlsFindStep(obj.mlsVStep(2),obj.mlsImgSize(2));
            obj.mlsY = obj.mlsFindStep(obj.mlsVStep(1),obj.mlsImgSize(1));
            % generate meshgrid
            [obj.mlsVx,obj.mlsVy]= meshgrid(obj.mlsX,obj.mlsY);
            % covert to vetor [y1,x1; y2,x2;...]
            obj.mlsV = [obj.mlsVy(:),obj.mlsVx(:)];
        end
        function vecOut = mlsFindStep(~,stepIn,distanceIn)
            % step upper bound
            if stepIn > 180; stepIn = 180;end
            % numer of steps
            N = round((distanceIn-1)/stepIn);
            % round linspace between 1 and distanceIn, uneven spacing (int)
            vecOut = round(linspace(1,distanceIn,N+1));
        end
        
        function mlsUseMethod(obj,inputArg)
            %METHOD mlsUseMethod allows to select calculation method
            if strcmpi(inputArg,'Similarity')
                obj.mlsType = 1;
            elseif strcmpi(inputArg,'Rigid')
                obj.mlsType = 2;
            else
                error('Wrong argument mlsUseMethod');
            end
        end
        %% Precomputation orchestrator
        function mlsPrecomupte(obj,p_i)
            % check that we are initialized
            if isempty(obj.mlsV) || isempty(obj.mlsAlpha) || isempty(obj.mlsImgSize) || isempty(obj.mlsType)
                error('Class MLSdeformation not properly initialized');
            end
            % check input - p
            obj.p_i = p_i;
            obj.p_i = obj.mlsCheckBoundaries(obj.p_i,[1 1],obj.mlsImgSize(1:2));
            obj.mlsCheckPneqV();
            
            obj.mlsPreAllocate(); 
            
            
            
            obj.mlsCommonPrecompute();
            % swith method
            % call
        end
        %% Precomputation - allocate + input check
        function mlsPreAllocate(obj)
            % check size of all properties and if needed allocate them
            s_pi = size(obj.p_i,1);
            s_vi = size(obj.mlsV,1);
            
            if isempty(obj.w_i) || any(size(obj.w_i) ~= [s_pi,s_vi])
                obj.w_i = zeros(s_pi,s_vi);
            end
            
            if isempty(obj.sum_w_i) || any(size(obj.sum_w_i) ~= [s_vi,1])
                obj.sum_w_i = zeros(s_vi,1);
            end
            
            if isempty(obj.p_ast) || any(size(obj.p_ast) ~= [s_vi,2])
                obj.p_ast = zeros(s_vi,2);
            end
            
            if isempty(obj.Ai) || any(size(obj.Ai) ~= [2*s_pi,2*s_vi])
                obj.Ai = zeros(2*s_pi,2*s_vi);
            end
            
            if isempty(obj.Ai_mu) || any(size(obj.Ai_mu) ~= [2*s_pi,2*s_vi])
                obj.Ai_mu = zeros(2*s_pi,2*s_vi);
            end
            
            if isempty(obj.mlsFx) || any(size(obj.mlsFx)~= size(obj.mlsVx))
                obj.mlsFx = zeros(size(obj.mlsVx));
            end
            
            if isempty(obj.mlsFy) || any(size(obj.mlsFy)~= size(obj.mlsVy))
                obj.mlsFy = zeros(size(obj.mlsVy));
            end
        end
        function vect = mlsCheckBoundaries(~,vect,low,up)
            % if below boundary replace with boundary
            i = vect < low;
            vect(i & [1 0]) = low(1);
            vect(i & [0 1]) = low(2);
            
            % if above boundary replace with boundary
            i = vect > up;
            vect(i & [1 0]) = up(1);
            vect(i & [0 1]) = up(2);
        end
        function mlsCheckPneqV(obj)
            % check that control points aren't too near to evaluation points
            % if needed move them by an offset, in direction that
            % lets them stay within image border
            
            offset = 0.0001;
            % if p == q
            ip = ismember(round(obj.p_i,3),round(obj.mlsV,3),'rows');
            % if substracting from x | y < lower boundary
            iadd = (obj.p_i - offset < 1);
            % than we must add
            obj.p_i(ip&iadd) = obj.p_i(ip&iadd) + offset;
            % else we substract
            obj.p_i(ip&~iadd) = obj.p_i(ip&~iadd) - offset;
        end
        %% Precomputation - calculation
        function mlsCommonPrecompute(obj)
            % part of MLS calculation is depending only on control points p,
            % not deformed position (of control points) q
            
            % interating across all evaluation points in grid
            for eval = 1:size(obj.mlsV,1)
                v = obj.mlsV(eval,:);
                % calculate weight for each cotrol point based on distance
                % from current evaluation point
                tw_i = ones(size(obj.p_i,1),1) ./ (sqrt(sum(( obj.p_i-repmat(v,size(obj.p_i,1),1) ).^2,2)).^(2*obj.mlsAlpha));
                obj.w_i(:,eval) = tw_i;
                % sum all weights
                tsum_w_i = sum(tw_i);
                obj.sum_w_i(eval,1) = tsum_w_i;
                % weighted centroid (~centre of mass)
                tp_ast = sum(tw_i.*obj.p_i, 1) ./ tsum_w_i;
                obj.p_ast(eval,:) = tp_ast;
                % if any(any(isnan(tp_ast))); error('Error Check w_i and PneqV, try offset++'); end
                
                % p^_i for substitution
                p_hat_i = obj.p_i - repmat(tp_ast,size(obj.p_i,1),1);
                
                obj.precompute_SimilarityAndRigid(p_hat_i,tp_ast,tw_i,v,eval);
            end
        end
        function precompute_SimilarityAndRigid(obj,p_hat_i,tp_ast,tw_i,v,eval)
            % calculate mu_s (substitution)
            obj.mls_pp = (p_hat_i * p_hat_i') .* eye(length(p_hat_i));
            mu_s = sum(obj.mls_pp*tw_i);
            
            % transposed part of right hand equation for A_i 
            Matrix_T = [(v-tp_ast); (-1)*(obj.perpend_vec(v-tp_ast))]';
            
            % the part depending on p only
            for i = 1:length(tw_i)
                tA_i = tw_i(i) ...
                    * [p_hat_i(i,:) ; (-1)*obj.perpend_vec(p_hat_i(i,:))]...
                    * Matrix_T;
                
                obj.Ai_mu(2*i-1:2*i,2*eval-1:2*eval) = tA_i / mu_s; % similarity
                obj.Ai(2*i-1:2*i,2*eval-1:2*eval) = tA_i; % rigid
            end
            
        end
        function vec = perpend_vec(~,vec)
            % support function for precompute_SimilarityAndRigid
            % (x,y) -> (-y,x)
            temp = vec(1);
            vec(1) = (-1)*vec(2);
            vec(2) = temp;
        end
        %% Computation
        function mlsDeform(obj,q,p)
            if nargin > 2
                if ~all(all(p==obj.p_i))
                    error('Precompute first')
                end
            end
            
            q = obj.mlsCheckBoundaries(q,[1 1],obj.mlsImgSize(1:2));
            
            switch obj.mlsType
                case 1
                    obj.mlsSimilarity(q)
                case 2
                    obj.mlsRigid(q)
                otherwise
                    error('Initialize first')
            end
            
        end
        function mlsRigid(obj,q_i)
            % iterate over evaluation points
            for i = 1:size(obj.mlsV,1)
                v = obj.mlsV(i,:);
                % calculate substitutions of q - deformated/target position of
                % control point
                [q_ast,q_hat_i] = obj.mlsq(obj.w_i(:,i), obj.sum_w_i(i), q_i);
                
                f_vec=zeros(1,2);
                for j = 1:size(q_i,1)
                    % each target point q add to final position of
                    % evaluation point
                    f_vec = f_vec + q_hat_i(j,:)*obj.Ai(2*j-1:2*j,2*i-1:2*i);
                end
                len_f_vec = sqrt(sum(f_vec.^2));
                % scaling by |v-p*| and normalizing of f_r + translation by q*
                f_r = sqrt(sum((v-obj.p_ast(i,:)).^2)) * f_vec / len_f_vec + q_ast;
                
                obj.mlsFy(i) = f_r(1);
                obj.mlsFx(i) = f_r(2);
            end
        end        
        function mlsSimilarity(obj,q_i)
            % iterate over evaluation points
            for i = 1:size(obj.mlsV,1)
                % calculate substitutions of q - deformated/target position of
                % control point
                [q_ast,q_hat_i] = obj.mlsq(obj.w_i(:,i), obj.sum_w_i(i), q_i);
                
                sum_f=[0,0];
                for j = 1:size(q_i,1)
                    % each target point q adds to final position of
                    % evaluation point
                    sum_f = sum_f + q_hat_i(j,:)*obj.Ai_mu(2*j-1:2*j,2*i-1:2*i);
                end
                % translation by q*
                sum_f = sum_f + q_ast;
                
                obj.mlsFy(i) = sum_f(1);
                obj.mlsFx(i) = sum_f(2);
            end
        end
        function [q_ast,q_hat_i] = mlsq(~,w_i,sum_w_i,q_i)
            % [w_i,w_i] to make matlab coder happy
            q_ast = sum([w_i,w_i].*q_i, 1) ./ sum_w_i;
            q_hat_i = q_i - repmat(q_ast,size(q_i,1),1);
        end
        %% Interpolation
        function newImg = interpolate1(obj,sourceImg,newImg)
            iter = size(obj.mlsVx)-1;
            x2 = diff(obj.mlsX);
            y2 = diff(obj.mlsY);
            
            x1 = obj.mlsX;
            y1 = obj.mlsY;
            % iterate over evaluated points
            % +--------> x,i=width
            % |
            % |
            % |
            % V y,j=height
            for i = 1:iter(2) % x
                for j = 1:iter(1) % y
                    Fx = obj.mlsFx(j:j+1,i:i+1);
                    Fy = obj.mlsFy(j:j+1,i:i+1);
                    newImg = InterpolateOverSquare(x1(i),y1(j),x2(i),y2(j),Fx,Fy,obj.mlsImgSize,sourceImg,newImg);
                end
            end            
        end
        
        function newImg = interpolateFromSaved(obj,s,sourceImg,newImg)
            imgSize = [size(sourceImg,1),size(sourceImg,2),size(sourceImg,3)];
            if nargin < 4
                newImg = zeros(imgSize,'uint8');
            end
            
            iter = size(s.mlsVx)-1;
            x2 = diff(s.mlsX);
            y2 = diff(s.mlsY);
            
            x1 = s.mlsX;
            y1 = s.mlsY;
            % iterate over evaluated points
            % +--------> x,i=width
            % |
            % |
            % |
            % V y,j=height
            for i = 1:iter(2) % x
                for j = 1:iter(1) % y
                    Fx = s.mlsFx(j:j+1,i:i+1);
                    Fy = s.mlsFy(j:j+1,i:i+1);
                    newImg = InterpolateOverSquare(x1(i),y1(j),x2(i),y2(j),Fx,Fy,imgSize,sourceImg,newImg);
                end
            end  
        end
        
        function s = saveDeformationStruct(obj)
            s.mlsVx = obj.mlsVx;
            s.mlsX = obj.mlsX;
            s.mlsY = obj.mlsY;
            s.mlsFx = obj.mlsFx;
            s.mlsFy = obj.mlsFy;
        end
        
        function pt = MLSinterpolatePoints(obj,p)
            pt = zeros(size(p));
            % grid step
             x2 = diff(obj.mlsX);
             y2 = diff(obj.mlsY);
                
            for i = 1:size(p,1)
                % finding interpolation square
                idx1 = find(obj.mlsX < p(i,2),1,'last'); 
                idy1 = find(obj.mlsY < p(i,1),1,'last'); 
                
                if isempty(idx1)
                    idx1= 1; idx2 = 1; tx = 0;
                else
                    idx2 = idx1+1;
                    tx = (p(i,2)-obj.mlsX(idx1))/x2(idx1); % to unit square conversion
                end
                
                if isempty(idy1)
                    idy1= 1; idy2 = 1; ry = 0;
                else
                    idy2 = idy1+1;
                    ry = (p(i,1)-obj.mlsY(idy1))/y2(idy1); % to unit square conversion
                end
                              
                if idx2 > length(obj.mlsX), idx2 = idx1; tx = 1; end
                if idy2 > length(obj.mlsY), idy2 = idy1; ry = 1; end   
                % getting vlues for interpolation
                Fx = obj.mlsFx([idy1,idy2],[idx1,idx2]);
                Fy = obj.mlsFy([idy1,idy2],[idx1,idx2]);
               
                % interpolatig over unit square
                py = [(Fy(1)*(1-tx) + Fy(3)*tx),(Fy(2)*(1-tx) + Fy(4)*tx)];
                px = [(Fx(1)*(1-tx) + Fx(3)*tx),(Fx(2)*(1-tx) + Fx(4)*tx)];
                
                fy = py(1)*(1-ry)+py(2)*ry;
                fx = px(1)*(1-ry)+px(2)*ry;
                
                pt(i,:) = [fy,fx];
            end                
        end
        
    end    
end

