classdef ImgProcess_CheckerBoard < handle
    %TESTIMGGEN Colection of methods for detecting checkerboard in image
    
    
    properties
        pS
        gridM
    end
    properties (Access = private)
        
%         Gmag
%         Gdir
        I
        minThetaSep
        maxRCM;
        cornerMetricAreaLIM
        
        
    end
    
    methods
        function obj = ImgProcess_CheckerBoard(inputArg1,inputArg2,inputArg3)
            %TESTIMGGEN Construct an instance of this class
            if nargin > 0
                obj.C = inputArg1;
            end
            
        end
        function Circ = genBinCirc(~,r,M)
            % ideal r = 5 for 640x480 img (empirically in [1])
            if ~mod(r,2)
                error('Provide odd number as radius')
            end
            
            % number of samples
            if nargin < 3, M = round( (16/5 * r)/4)*4; end % sampling angle rate approximately as in [1]
            if M == 0, M = ceil(2*pi*r);end
            
            % calculate circle
            i = 0:M-1;
            x = round( r*cos(2*pi*i/M) ); %  + pi/4 --> theta =/= theta + pi/2
            y = round( r*sin(2*pi*i/M) ); %      +0 --> fft not oscilating +-90
            
            %             x = round( r*cos(2*pi*i/M ) );
            %             y = round( r*sin(2*pi*i/M  ));
            
            % delete duplicate points
            P = unique([x',y'], 'rows', 'stable');
            
            x = P(:,1);
            y = P(:,2);
            
            if ispolycw(x,y)
                error('Not ccw')
            end
            
            Circ.x = x;
            Circ.y = y;
            Circ.r = r;
            Circ.M = M;
        end
        
        function [R,phaseF2,ampF0,ampF1,ampF2] = detectPointsFFT(~,I,Circ)
            %resize image
            %I = imresize(I,640/length(I));
            
            % allocate outputs
            R = ones( size(I)).*0;
            phaseF2 = R; ampF0= R; ampF1=R; ampF2=R;
            
            % loop through all points
            [w, h] = size(I);
            r = Circ.r;
            for px = (r+1):w-(r+1)
                for py = (r+1):h-(r+1)
                    % take sample
                    pidx = sub2ind([w,h],px+Circ.x,py+Circ.y);
                    sample = I(pidx);
                    
                    % apply fft
                    f = fft(sample)./Circ.M;
                    
                    % extract interesting amplitudes and pahases
                    ampF0(px,py) = abs(f(1));
                    ampF1(px,py) = abs(f(2));
                    ampF2(px,py) = abs(f(3));
                    phaseF2(px,py) = angle(f(3));
                end
            end
            R = ampF2 - ampF1;
        end
        
        function [cornerMetric,cornerMetricAreaLIM] = getCornerMetric(obj,I,x,y,theta,phaseF2rd, MV, MG, cornerMetricAreaFractionLim,imAdjustAllow)
            %  | MV |  MG |  MV
            %  1 1 1 0 0 0 1 1 1
            %  1 1 1 0 0 0 1 1 1
            %  1 1 1 0 0 0 1 1 1
            %  0 0 0 0 0 0 0 0 0
            %  0 0 0 0 0 0 0 0 0
            %  0 0 0 0 0 0 0 0 0
            %  1 1 1 0 0 0 1 1 1
            %  1 1 1 0 0 0 1 1 1
            %  1 1 1 0 0 0 1 1 1
            %
            if nargin < 10
                imAdjustAllow = true;
            end
            
            [w, h] = size(I);
            
            cornerMetric = zeros(length(x),1);
            cornerMetricAreaLIM = (MV^2 * cornerMetricAreaFractionLim)^2;
            
            % Prepare correlation pattern
            c = MG/2 -1/2;
            roiW = floor(2*sqrt(2)*(MV+c));
            
            vzor = zeros(2*roiW+1);
            vzor( (roiW+1-MV -c) : (roiW -c), (roiW+1-MV-c) : (roiW -c ) ) = 1;
            vzor = vzor - rot90(vzor);
            
            for i = 1:length(x)
                % indexes for ROI sample from Image
                ix = (-roiW:roiW) + x(i);
                ix(ix<1) = []; ix(ix>h) = [];
                bx = roiW - x(i) + ix(1);
                
                iy = (-roiW:roiW) + y(i);
                iy(iy<1) = []; iy(iy>w) = [];
                by = roiW - y(i) + iy(1);
                
                % take ROI Sample
                roi = ones(2*roiW+1).*NaN;
                roi(by + (1:length(iy)),bx + (1:length(ix))) = I(iy,ix);
                
                % make sure it doesnt contain NaNs
                roi(isnan(roi)) = mean(roi(:),'omitnan');
                if imAdjustAllow
                   roi = histeq(roi);
                end
                 %                roi = imadjust(roi);
                
                % rotate sample by theta
                roiR = imrotate(roi,theta(i),'bicubic','crop');
                
                % calculate corner metric
                if imAdjustAllow
                cornerMetric(i) = max( [conv2(roiR,vzor,'valid') * conv2(roiR,-fliplr(vzor),'valid'),...
                    conv2(imbinarize(roiR),vzor,'valid') * conv2(imbinarize(roiR),-fliplr(vzor),'valid'),...
                    conv2(roi,vzor,'valid') * conv2(roi,-fliplr(vzor),'valid') ]);
                else
                    cornerMetric(i) = conv2(roiR,vzor,'valid') * conv2(roiR,-fliplr(vzor),'valid');
                end
                % if angle of F=2 for 2 raduises differs more than 15°
                if abs(phaseF2rd(sub2ind([w,h],y(i),x(i)))) >  15/180*pi
                    cornerMetric(i) = -cornerMetric(i);
                end
            end
            
        end
        
        function [x,y,theta,phaseF2rd] = findCorners(obj,I,lowThreshold)
            [w, h] = size(I);            
            
            R= ones(size(I));
            ampF11 = R;
            ampF22 = R;
            phaseF2rd = repmat(R,[1 1 2]);
            i = 1;
            for r = 7:-2:5
                % extract characteristic response
                Circ = obj.genBinCirc(r);
                [Rt,phaseF2,ampF0,ampF1,ampF2] = obj.detectPointsFFT(I,Circ);
                phaseF2rd(:,:,i) = phaseF2; i = i+1;
                ampF11 = ampF11 .* ampF1;
                ampF22 = ampF22 .* ampF2;
                % R = R .* Rt;
            end
            R = ampF22-ampF11;
            phaseF2rd(:,:,1) = angdiff(phaseF2rd(:,:,1), phaseF2rd(:,:,2)); phaseF2rd(:,:,2) = [];
            % detect peaks
            centroids = FastPeakFind(Rt,lowThreshold);
            x = centroids(1:2:end);y = centroids(2:2:end);
            
            %threshold > 0
            Rtt = (Rt(sub2ind([w,h],y,x)));
            %A2tt = (ampF2(sub2ind([w,h],y,x)));
            %A0tt = (ampF0(sub2ind([w,h],y,x)));
            if sum(Rtt>lowThreshold ) >= 3
                
                x(Rtt<=lowThreshold ) = []; y(Rtt<=lowThreshold ) = [];
            else
                [~,i]  = maxk(Rtt,3);
                x = x(i); y = y(i);
            end
            % extract orientation of possible checkerboard corner
            theta = ((phaseF2(sub2ind([w,h],y,x)) +pi/2) ./pi.*180)/2;
        end
     
        
        function sample = takeCircSamples(obj,I,x,y,Circ)
            [w, h] = size(I);
            for i = 1:length(Circ)
                sample(i).Circ = Circ(i);
                % get sample
                try
                    pidx = sub2ind([w,h],y+Circ(i).y, x+Circ(i).x);
                catch ME
                    if (strcmp(ME.identifier,'MATLAB:sub2ind:IndexOutOfRange'))
                        sample(i).period = NaN;
                        continue;
                    end
                end
                sample(i).period = I(pidx);
                sample(i).x = x;
                sample(i).y = y;
            end
        end
        
        %% checkerboard joining
        function cornerValidationInit(obj, I, x, y, theta, cornerMetric,cornerMetricAreaLIM)
            % copy x,y,theta to structure
            obj.pS = struct('x',num2cell(x));
            temp = num2cell(y); [obj.pS.y] = temp{:};
            temp = num2cell(theta); [obj.pS.theta] = temp{:};
            temp = num2cell(cornerMetric); [obj.pS.cornerMetric] = temp{:};
            
            %distance to 20 closest points
            for i = 1:length(x)
                Pdist = sqrt((x - x(i)).^2 + (y - y(i)).^2);
                % select 20 closest (1st is point itself)
                [PdistN, iN] = mink(Pdist,21);
                PdistN(1) = []; iN(1) = [];
                obj.pS(i).dist2p = [iN,PdistN] ;
            end
            
            
            % image
            obj.I = I;
            
            
            
            
            % theta separation based on histogram of comfirmed corners
            % thetas
            obj.maxRCM = find(cornerMetric==max(cornerMetric));
            
            obj.minThetaSep = min(theta((theta > mean(theta(obj.maxRCM))) & cornerMetric==max(cornerMetric))) ...
                - max(theta((theta < mean(theta(obj.maxRCM))) & cornerMetric==max(cornerMetric))) ;
            
            obj.cornerMetricAreaLIM = cornerMetricAreaLIM;
            
            
        end
        
        
        function idOut = findSeedCorners(obj)
            while true
                % pick random seed point
                i = obj.maxRCM(randi(length(obj.maxRCM)));
                %     i = 803; % dark corner
                %     i = 616; i = 596; %i = 495;
                %i = 998
                %                 i = 834
                %i=108;
                %hold off, imshow(I); hold on ;  plot(x(obj.maxRCM),y(obj.maxRCM),'r.'); plot(x(i),y(i),'ro'), plot(x(i),y(i),'r*'); drawnow; pause(1)
                
                idOut = []; alpha = [];
                [idOut(1),alpha(1)] = obj.findNextCornerInDir( i, 0, obj.maxRCM,true);
                
                for j = [1,2,3]
                    %hold off, imshow(I); hold on ;  plot(x(obj.maxRCM),y(obj.maxRCM),'r.'); plot(x(i),y(i),'ro'), plot(x(i),y(i),'r*'); drawnow; pause(1)
                    if idOut(end) < 1
                        break;
                    end
                    [idOut(end+1),alpha(end+1)] = obj.findNextCornerInDir( idOut(end), j, obj.maxRCM,true);
                end
                
                if ~any(idOut<1) && (sum(abs(alpha)) - 2*pi < pi/36) && idOut(4) == i
                    %                     hold off, imshow(obj.I); hold on; plot(x(idOut),y(idOut),'b*')
                    break
                else
                    continue;
                end
            end
        end
        
        function gridM = searchForCorners(obj,idOut)
            % randomly pick point with max score and connect it to seed
            % marix
            if nargin < 2
                idOut = findSeedCorners(obj);
            end
            % 2x2 seed matrix
            obj.gridM = [idOut(1) idOut(2); idOut(4) idOut(3)];
            obj.pS(idOut(1)).nodeNeigbours = [idOut(2) idOut(4) 0 0];
            obj.pS(idOut(2)).nodeNeigbours = [0 idOut(3) idOut(1) 0];
            obj.pS(idOut(3)).nodeNeigbours = [0 0 idOut(4) idOut(2)];
            obj.pS(idOut(4)).nodeNeigbours = [idOut(3) 0 0 idOut(2)];
            
            
            % while new points > 0 (atlest one new point on this iteration)
            newPoints = [1,2,3,4];
            lastTry = true;
            while lastTry
                % grow checkerboard right=1, bottom=2, left=3, top=4
                checkDir = newPoints;
                if all(isempty(newPoints)) && lastTry
                    lastTry = false;
                    checkDir = 1:4;
                end
                newPoints = [];  % while loop
                
                for dirCheck = 1:length(checkDir)
                    i = checkDir(dirCheck);
                    
                    % rotate grid to have growing side on top
                    tempGridM = rot90(obj.gridM,i);
                    S = size(tempGridM,2);
                    % structure for new cornres
                    newCorners = struct([]);
                    % find first known corner in each collumn
                    [~, B] = max(tempGridM>0,[],1);
                    
                    % for each collumn in row of known corners
                    for j = 1:S
                        newCorners(j).fromID = tempGridM(B(j),j);
                        newCorners(j).ID = -1;
                        
                        if obj.pS(newCorners(j).fromID).nodeNeigbours(i) == -1
                            % cornern in given direction already unsuccesfuly
                            % searched for -> skip this time
                            newCorners(j).t = -1;
                            newCorners(j).candidates = {[]};
                        else
                            % search for new corner in top* direction, return
                            % candidates with ranking (s) and id of best candidate (t)
                            [t,~,s] = obj.findNextCornerInDir( tempGridM(B(j),j), i,  1:length(obj.pS), true);
                            newCorners(j).t = t;
                            newCorners(j).candidates = {s};
                        end
                    end
                    %cell2mat({s(1).candidates{s(1).originID ~= -1}}')
                    
                    %
                    
                    % if any candidate corner found
                    if any([newCorners.t] ~= -1) > 0
                        % validating between new/candidate corners :
                        % if corner(s) next to us has good gradient
                        % metric - (best candidate (t) was returned as not 0, -1)
                        % and is at same grid row / level
                        for j = 1:S-1
                            % look from it back in our (j) direction from
                            % corner to the right
                            if newCorners(j+1).t(1) > 0 && B(j) == B(j+1)
                                
                                [t,~,s] = obj.findNextCornerInDir( newCorners(j+1).t(1), i-1,  1:length(obj.pS), true);
                                newCorners(j).t(end+1) = t;
                                newCorners(j).fromID(end+1) = newCorners(j+1).t(1);
                                newCorners(j).candidates{end+1} = s;
                            end
                        end
                        
                        for j = 2:S
                            % if corner to the left (j-1) has good gradint
                            % metric and looking back from right didn't help /
                            % wasn't possible look from corner to the left
                            if newCorners(j-1).t(1) > 0 && B(j) == B(j-1) && (any(any(newCorners(j).t == [0 ; -1])) || (length(unique(newCorners(j).t)) > 1) || (length(newCorners(j).t) < 2) )
                                [t,~,s] = obj.findNextCornerInDir( newCorners(j-1).t(1), i+1,  1:length(obj.pS), true);
                                newCorners(j).t(end+1) = t;
                                newCorners(j).fromID(end+1) = newCorners(j-1).t(1);
                                newCorners(j).candidates{end+1} = s;
                            end
                        end
                        
                        % combine rating from all directions
                        newCorners = obj.combineNewCornersCandidates(newCorners,S);
                    end
                    
                    % add found coreners to matrix
                    if any(B([newCorners.ID]>0) == 1)
                        % new row create + add
                        tempGridM = [zeros(1,S);tempGridM];
                        tempGridM(sub2ind(size(tempGridM),B(find([newCorners.ID]>-2)),find([newCorners.ID]>-2))) = [newCorners.ID];
                        newPoints(end+1)=i;
                    else
                        % write to existing row
                        tempGridM(sub2ind(size(tempGridM),B(B>1)-1,find(B>1))) = [newCorners(B>1).ID];
                        
                        if any([newCorners.ID]>0)
                            newPoints(end+1)=i;
                        end
                    end
                    
                    
                    % write neigbouring corners relation to structure
                    for j = 1:S
                        % write to bottom (origin point)
                        
                        obj.pS(newCorners(j).fromID(1)).nodeNeigbours(i) = newCorners(j).ID;
                        % if newCorner write its neigbours
                        if newCorners(j).ID > 0
                            % indexing
                            ij = [-1,1] + j;
                            ij(ij<1) = []; ij(ij>S) = [];
                            
                            nN = [zeros(1,3), newCorners(j).fromID(1)];
                            %[left 0 right bottom]
                            nN(2 + ij - j) = [newCorners(ij).ID];
                            obj.pS(newCorners(j).ID).nodeNeigbours = circshift(nN,-2-i);
                        end
                    end
                    
                    
                    
                    
                    
                    obj.gridM = rot90(tempGridM,-i);
%                     subplot(1,2,1)
%                    
%                     
%                     plot([obj.pS.x],[obj.pS.y],'y.'),
%                     plot([obj.pS([obj.pS(:).cornerMetric]>0).x],[obj.pS([obj.pS(:).cornerMetric]>0).y],'y*'),
%                     plot([obj.pS([obj.pS(:).cornerMetric]>obj.cornerMetricAreaLIM).x],[obj.pS([obj.pS(:).cornerMetric]>obj.cornerMetricAreaLIM).y],'r.'),
%                     
%                     plot([obj.pS(obj.maxRCM).x],[obj.pS(obj.maxRCM).y],'r*'),
%                     
%                     plot([obj.pS([newCorners([newCorners.ID]>0).ID]).x],[obj.pS([newCorners([newCorners.ID]>0).ID]).y],'g*')
%                     drawnow;
                    %                     plot(x(cornerMetric > cornerMetricAreaLIM),y(cornerMetric > cornerMetricAreaLIM),'r.')
                    %                     plot(x(obj.maxRCM),y(obj.maxRCM),'r*');
                    %
                    %                     plot(x(obj.gridM(obj.gridM>0)),y(obj.gridM(obj.gridM>0)),'b*')
                    %                     plot(x(newCorners(newCorners>1)),y(newCorners(newCorners>1)),'g*')
                    %                     drawnow; pause(1)
                    
                    
                end
                
                
                
                % after each expansion cycle check for empty points
                [w,h] = size(obj.gridM);
                k = find(obj.gridM == 0);
                for jj = 1:length(k)
                    if isempty(k(jj)), continue;end
                    % get neigouring corners (checking matrix borders)
                    [x,y] = ind2sub([w,h], k(jj));
                    ix = (-1:1) + x;
                    ix(ix<1) = []; ix(ix>w) = [];
                    bx = 1 - x + ix(1);
                    
                    iy = (-1:1) + y;
                    iy(iy<1) = []; iy(iy>h) = [];
                    by = 1 - y + iy(1);
                    %
                    roi = zeros(3);
                    roi(bx + (1:length(ix)),by + (1:length(iy))) = obj.gridM(ix,iy);
                    
                    dID = roi([8, 6, 2, 4]);
                    dID = circshift(dID,2);
                    if sum(dID > 0) > 1
                        newCorners = struct([]);
                        newCorners(1).ID = 0;
                        newCorners.fromID = [];
                        newCorners.t = [];
                        newCorners.candidates = {};
                        for j = 1:length(dID)
                            if dID(j) > 0
                                newCorners.fromID(end+1) = dID(j);
                                [t,~,s] = obj.findNextCornerInDir( dID(j), j,  1:length(obj.pS), true);
                                newCorners.t(end+1) = t;
                                newCorners.candidates(end+1) = {s};
                            end
                        end
                        
                        newCorners = obj.combineNewCornersCandidates(newCorners,1);
                        
                        if newCorners.ID > 0
                            obj.gridM(x,y) = newCorners.ID;
                            
                            obj.pS(newCorners.ID).nodeNeigbours = circshift(dID,-2);
                            
                            for j = 1:length(dID)
                                if dID(j) > 0
                                    obj.pS(dID(j)).nodeNeigbours( j ) = newCorners.ID;
                                end
                            end
                        end
                    end
                    if roi(6) == -1
                        %                             tempGridM(y,x) = -1;
                        
                        
                        
                    end
                    
                end
                
                % attempt to fill gaps (by using surrounding points)
                disp(obj.gridM)
                 hold off; imshow(obj.I);hold on ;
                plot([obj.pS(obj.gridM(obj.gridM>0)).x],[obj.pS(obj.gridM(obj.gridM>0)).y],'b*'),
                drawnow;
            end
            gridM = obj.gridM;
        end
        
        
        function newCorners = combineNewCornersCandidates(obj,newCorners,S)
            % combine rating from all directions
            
            if nargin < 3
                S = length(newCorners);
            end
            
            for j = 1:S
                if any(unique(newCorners(j).t) > -1)
                    newCorners(j).ID = 0;
                end
                % candidate corners from multiple (2+) corners has good gradient metric and all match
                if length(unique(newCorners(j).t)) == 1 && unique(newCorners(j).t) > 0 && (sum(newCorners(j).t > 0 ) > 1)
                    % all agree -> save
                    tempNC = newCorners([1:(j-1),(j+1):end]);
                    if isempty(tempNC)
                        tempNC = -1;
                    else
                        tempNC = [tempNC.t];
                    end
                    uC = unique(newCorners(j).t);
                    uC(ismember(uC,obj.gridM) | ismember(uC,tempNC)) = 0;
                    newCorners(j).ID = uC;
                    
                    
                    % average all corner gradient metric taken from multiple
                    % (2+) directions
                elseif sum(newCorners(j).t > -1 ) > 1
                    % take all posible candidates
                    candidates = cell2mat(newCorners(j).candidates');
                    % combine data
                     tempNC = newCorners([1:(j-1),(j+1):end]);
                    if isempty(tempNC)
                        tempNC = -1;
                    else
                        tempNC = [tempNC.t];
                    end
                    
                    uC = unique(candidates(:,1)) ;
                    
                    
                    dC = uC(ismember(uC,obj.gridM) | ismember(uC,tempNC) ); % discard already known corners
                   
                    for jj = 1:length(dC)
                        uC(uC == dC(jj)) = [];
                        candidates(candidates==dC(jj),:) = [];
                    end
                    
                    for jj = 1:length(uC)
                        uniqueCandidates = [uC(jj),...
                            mean(candidates(candidates==uC(jj),2))./ length(candidates(candidates==uC(jj),2)),...
                            sum(abs(candidates(candidates==uC(jj),3))) ./ length(newCorners(j).fromID)];
                        candidates(candidates==uC(jj),:) = [];
                        candidates(end+1,:)  = uniqueCandidates;
                    end
                    % select one with combined maximum gradient metric
                    [~,id] = max(candidates(:,3));
                    % valid corner if sum > 1
                    newCorners(j).ID = candidates(id,1) .* (candidates(id,3) > 1);
                    
                    % if 3+ starting corners and still not above 1 than
                    % decide based on angle difference, number of starting
                    % points seen from and grad metric
                    if isempty(uC)
                         newCorners(j).ID = 0;
                    end
                    
                    if length(newCorners(j).fromID) > 3 && newCorners(j).ID == 0
                        [~,id] = max(candidates(:,3)./ candidates(:,2));
                        newCorners(j).ID = candidates(id,1) ;
                    end
                    
                  
                end
            end
        end
%         function connectFromNeighbours(MatrixIn, MatrixId, MatrixOrientation, directions)
%             
%         end
        %% checkerboard joining low-level
        function [idOut, alpha, s] = findNextCornerInDir(obj, i, direction, iXY, roiIDsM)
            % direction 0 = top, 1 = right, 2 = down, 3 = left,
            fowAngle = pi/4;
            
            %             % distance to other points (index constrain available)
            %             Pdist = sqrt(([obj.pS(iXY).x] - obj.pS(i).x).^2 + ([obj.pS(iXY).y] - obj.pS(i).y).^2);
            %
            %             % check Nclosest points to create seed rectangle
            %             [PdistN, iN] = mink(Pdist,20);
            %             % remove seed point
            %             PdistN(1) = []; iN(1) = [];
            %
            iN = obj.pS(i).dist2p(:,1);
            PdistN = obj.pS(i).dist2p(:,2);
            if length(obj.pS) > length(iXY)
                iN = find(sum(iXY == iN',2));
            end
            %%%%%%%%%%%%%%%%%%%%%%%%%%%
            
            % angle in image reference
            alphaN = atan2([obj.pS(iXY(iN)).y] - obj.pS(i).y,[obj.pS(iXY(iN)).x] - obj.pS(i).x);
            %corner orientation
            thetaN = [obj.pS(iXY(iN)).theta];
            
            idOut = -1;
            alpha = -1;
            % different corner orientation constrain
            i90deg = find(abs(thetaN - obj.pS(i).theta) > obj.minThetaSep);
            if isempty(i90deg), return; end
            % seed corner tilt
            tilt = [angdiff(deg2rad(obj.pS(i).theta),pi/2), angdiff(deg2rad(obj.pS(i).theta),0)];
            [~,id] = min(abs(tilt)); tilt = tilt(id);
            
            % difference from selected direction
            direction = wrapToPi(repmat( (direction-1)*pi/2 - tilt ,1,length(i90deg)));
            adiff = abs(angdiff(alphaN(i90deg), direction));
            adiff(adiff > fowAngle) = Inf;
            % sort output by ang diference
            [adiff,idTemp] = sort(adiff);
            idOut = iXY(iN(i90deg( idTemp(~isinf(adiff)) )));
            alpha = alphaN(i90deg( idTemp(~isinf(adiff)) ));
            
            
%             % draw FOW
%             subplot(1,2,1)
%             for k = [-fowAngle,fowAngle]
%                 if abs(wrapToPi(direction(1)+k)) > pi/2
%                     plot([0:-1:-50]+obj.pS(i).x,tan(wrapToPi(direction(1)+k))*[0:-1:-50]+obj.pS(i).y,'g')
%                 else
%                     plot([0:50]+obj.pS(i).x,tan(wrapToPi(direction(1)+k))*[0:50]+obj.pS(i).y,'g')
%                 end
%             end
            
            if size(idOut,1) < size(idOut,2)
                idOut = idOut';
            end
            %create candidate matrix
            s = [idOut,[adiff(~isinf(adiff))]'];
            
            % handle empty output & if requested, validate by gradient
            if isempty(idOut)
                idOut = -1;
                alpha = -1;
            elseif nargin > 4
                
                [idOut, alpha, temp] = obj.validateCornerInDir(idOut, alpha,  i);
                s(:,end+1) = temp;
                
            end
            % %             direction = [direction(1) + [-fowAngle,fowAngle]];
            
        end
        
        function [idOut, alpha, alongGrad] = validateCornerInDir(obj, cP, alpha, sP)
            
            %             fprintf('theta: %f°, dir: [%f, %f]\n',obj.pS(sP).theta);
            
            
            %             % +- 90° - top/botom swith
            %             k = -pi/2;
            %             if abs(angdiff(deg2rad(obj.pS(sP).theta),pi/2)) < abs(angdiff(deg2rad(obj.pS(sP).theta),0))
            %                 k = pi/2;
            %             end
            %
            %             % -180° - +180° wrap, select edges with gradient +- tolerance
            %             if all(wrapToPi(direction+k)  > fliplr(wrapToPi(direction+k) ) == [0 , 1])
            %                 dGmag = obj.Gmag .* (obj.Gdir >rad2deg(wrapToPi(direction(1)+k))  & obj.Gdir < rad2deg(wrapToPi(direction(2)+k)));
            %             else
            %                 dGmag = obj.Gmag .* (obj.Gdir <rad2deg(min(wrapToPi(direction+k)) )  | obj.Gdir > rad2deg( max(wrapToPi(direction+k)) ) );
            %             end
            %             dGmag = imbinarize(dGmag);
%             subplot(1,2,2)
% %              hold off; imshow(obj.I);hold on;
% %             plot([obj.pS(obj.gridM(obj.gridM>0)).x],[obj.pS(obj.gridM(obj.gridM>0)).y],'b*')
            alongGrad = zeros(length(cP),1);
            Gc = {};
            Gp = {};
            for i = 1:length(cP)
% %                  hold on; plot([obj.pS(cP(i)).x],[obj.pS(cP(i)).y],'r*'), plot(obj.pS(sP).x,obj.pS(sP).y,'b*'); text([obj.pS(cP(i)).x],[obj.pS(cP(i)).y],num2str(i),'Color','red')
                % calculate precentage of path to next corner that is along
                % edge (gradient)
                [alongGrad(i),Gc{i},Gp{i}] = obj.recursiveGradFind([obj.pS(sP).x,obj.pS(sP).y], [obj.pS(cP(i)).x,obj.pS(cP(i)).y], obj.I);
            end
            drawnow;
            alongGrads = abs(alongGrad) > 1;
            if all(~alongGrads)
                idOut = 0;
                alpha = 0;
            else
                [~,tid] =max(abs(alongGrad) .* alongGrads);
                
                idOut = cP(tid);
                alpha = alpha(tid);
                % % %                 fprintf('Slected corner (%d): %f - metric\n',tid,obj.pS(idOut).cornerMetric)
                
            end
            %             if obj.pS(idOut).cornerMetric < 81
            %                 pause(0.1), drawnow;
            %             end
        end
        
        function [alongGrad,Gcolinear,Gperpendicular] = recursiveGradFind(obj,p1,p2,dGmag)
            %              if n <= 0
            %                 r = 1;
            %              else
            %                 r = n * obj.recursiveGradFind(n-1);
            %              end
            % pro dvojci
            
            % find endpoints of perpendicular line segment
            v = p2-p1;
            midP = v./2 + p1;
            
            m = max(norm(v)*0.1 , 6); % width of perpendicular line segmet max(0.1*L,2)
            
            k = fliplr(v) .* [-1 , 1] ./norm(v); % unit perpendicular vector
            
            k1 = k*m + midP; % endpoinds of perpendicuar vector
            k2 = -k*m + midP;
            
            % sample dGmag along perpendicular line segment
            d = improfile(dGmag,[k1(1), k2(1)],[k1(2),k2(2)]);
            ld = length(d);
            %d = gradient(d);%%%%
            % find peak closest to center
            %[~, i, w] = findpeaks(d);
            %[~,i] = max(abs(d));
            d(isnan(d)) = -10; d(isinf(d)) = 10;
            i = midcross(d);
            %%%%i = i + w/2;
            dpeak = mink(i-ld/2,1,'ComparisonMethod','abs');
            
            if abs(dpeak) > max(norm(v)*0.1 , 2)
                dpeak = sign(dpeak)*max(norm(v)*0.1 , 2);
            end
            %dpeak = mink(i-ld/2,);
% %              plot([p1(1),p2(1)],[p1(2),p2(2)])
% %              plot([k1(1), k2(1)],[k1(2),k2(2)])
            
            %
            if isempty(dpeak) || abs(dpeak) <= 2
                % distLSegment = norm(v);
                samples = improfile(dGmag,[p1(1), p2(1)],[p1(2),p2(2)]);
                
                samplesR = improfile(dGmag,[p1(1), p2(1)]+k(1),[p1(2),p2(2)]+k(2),length(samples));
                samplesL = improfile(dGmag,[p1(1), p2(1)]-k(1),[p1(2),p2(2)]-k(2),length(samples));
                
                Gcolinear = samples;
                Gperpendicular = samplesR-samplesL;
                %samples(samples > 1) = 1;
                % alongGrad = sum(samples)/distLSegment;
                %                  alongGrad = max(abs(gradient(samples)));
                %                  distLSegment = abs(d(i));
                %                  figure; plot(d); hold on; plot(length(d)/2,1,'*')
            else
                % if peak more than 2px from center
                
                newP = midP + (k2-midP)./norm(k2-midP).*dpeak;
%                 plot(newP(1),newP(2),'r.')
                
                % call recursion
                [~,Gc1,Gp1] = obj.recursiveGradFind(p1,newP,dGmag) ;
                [~,Gc2,Gp2] =  obj.recursiveGradFind(newP,p2,dGmag);
                
                Gcolinear = [Gc1;Gc2];
                Gperpendicular = [Gp1;Gp2];
                
                %distLSegment = sum([dLS1,dLS2]);
                %alongGrad = (aG1*dLS1 + aG2*dLS2)/distLSegment;
                %                  distLSegment = mean([dLS1,dLS2]);
                %                  alongGrad = max([aG1,aG2]);
                
            end
            % pokud vrhchol 2px od stredu
            % navzorkovat gradient podel primky
            % return % delky
            %else
            % recursive call f(1 ,0.5) + f(0.5 , 0)
            % figure; plot(d); hold on; plot(length(d)/2,1,'*')
            alongGrad =  mean(Gperpendicular) / std(Gcolinear);
        end
        
    end
end

