function [newImg] = InterpolateOverSquare(i,j,x2,y2,Fx,Fy,ImgSize,sourceImg,newImg)
%InterpolateOverSquare performs bilinear interpoleation of new pixel
% position and pixel value
%   1st step is to calculate position inside new (deformed) image
%   2nd step is to calculate value of this new pixel



%   y A            ni,nj
%     |            x2,y2
% f01 |-----------+  f11
%     |           |
%     |           |
%     |           |
%     |           |
% f00 +----------------> x
%   x1,y1 = 0      f10
%   i,j            ni,j
%
%   | f00 , f10;|   | 1 , 3;|
%   | f01 , f11 |   | 2 , 4 |
%
%
% call interpolation

% calculate loop step based on values of deformation function in verteces
% of square => prevents creation of gaps in new image
useStep = min([x2,y2])/max([biggest_slope(Fy(1),Fy(2),Fy(3),Fy(4)),biggest_slope(Fx(1),Fx(2),Fx(3),Fx(4))]);

% edge cases for source pixel position
cy = j+[0,y2]-Fy(:)+round(Fy(:));
cx = i+[0,x2]-Fx(:)+round(Fx(:));

% if edge cases are outside of image border run loop with safety variable
% checks, else run without them (5x speed improvement)
% safety margin 1 - rounding
if notInsideBorder([cy(:),cx(:)],2,ImgSize(1:2)-1) || notInsideBorder([Fy(:),Fx(:)],2,ImgSize(1:2)-1)
    
    for x = 0:useStep:x2
        % first half of bilinear interpolation:
        % linear aproximation for given x between vertices with same y value
        % p(1) = f00 * (1 - (x/x2)) + f10 * (x/x2)
        % p(2) = f01 * (1 - (x/x2)) + f11 * (x/x2)
        tx = x/x2;
        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)];
        
        for y = 0:useStep:y2
            % second half of bilinear interpolation:
            % linear aproximation for given y between two previously
            % calculated points => the new (relative) position of pixel
            % f = p(1) * (1 - (y/y2)) + p(2) * (y/y2)
            ry = y/y2;
            fy = py(1)*(1-ry)+py(2)*ry;
            fx = px(1)*(1-ry)+px(2)*ry;
            
            % new position of pixel in newImg
            nx = round(fx);
            ny = round(fy);
            
            % check if the new pixel is not Inside Border if true skip
            if nx <1 || nx > ImgSize(2) || ny <1 || ny > ImgSize(1)
                continue
            end
            
            % compensate source point for rounding
            sx = i + x - fx + nx;
            sy = j + y - fy + ny;
            
            % find unit square enclosing [sy,sx] for interpolating of pixel
            % value
            x11 = floor(sx);x22 = ceil(sx); xx = sx - x11;
            y11 = floor(sy);y22 = ceil(sy); yy = sy - y11;
            
            % check that vertecies of unit square are within borders
            if x11 < 1 || x22 < 1
                x11 = 1; x22 = 1; xx = 0;
            elseif x11 > ImgSize(2) || x22 > ImgSize(2)
                x11 = ImgSize(2); x22 = ImgSize(2); xx = 0;
            end
            if y11 < 1 || y22 < 1
                y11 = 1; y22 = 1; yy = 0;
            elseif y11 > ImgSize(1) || y22 > ImgSize(1)
                y11 = ImgSize(1); y22 = ImgSize(1); yy = 0;
            end
            
            % calculate bilinear interpolation weights
            w = [(1-xx)*(1-yy) , xx*(1-yy), (1-xx)*yy , xx*yy];
            
            % apply weights on pixels from all (R,G,B) layers
            for layer = 1:ImgSize(3)
                newImg(ny, nx, layer) = sourceImg(y11, x11,layer) * w(1)...
                    + sourceImg(y11, x22,layer) * w(2)...
                    + sourceImg(y22, x11,layer) * w(3)...
                    + sourceImg(y22, x22,layer) * w(4);
            end
        end
    end
else
    for x = 0:useStep:x2
        % first half of bilinear interpolation:
        % linear aproximation for given x between vertices with same y value
        % p(1) = f00 * (1 - (x/x2)) + f10 * (x/x2)
        % p(2) = f01 * (1 - (x/x2)) + f11 * (x/x2)
        tx = x/x2;
        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)];
        
        for y = 0:useStep:y2
            % second half of bilinear interpolation:
            % linear aproximation for given y between two previously
            % calculated points => the new (relative) position of pixel
            % f = p(1) * (1 - (y/y2)) + p(2) * (y/y2)
            ry = y/y2;
            fy = py(1)*(1-ry)+py(2)*ry;
            fx = px(1)*(1-ry)+px(2)*ry;
            
            % new position of pixel in newImg
            nx = round(fx);
            ny = round(fy);
            
            % compensate source point for rounding
            sx = i + x - fx + nx;
            sy = j + y - fy + ny;
            
            % find unit square enclosing [sy,sx] for interpolating of pixel
            % value
            x11 = floor(sx);x22 = ceil(sx); xx = sx - x11;
            y11 = floor(sy);y22 = ceil(sy); yy = sy - y11;
            
            % calculate bilinear interpolation weights
            w = [(1-xx)*(1-yy) , xx*(1-yy), (1-xx)*yy , xx*yy];
            
            % apply weights on pixels from all (R,G,B) layers
            for layer = 1:ImgSize(3)
                newImg(ny, nx, layer) = sourceImg(y11, x11,layer) * w(1)...
                    + sourceImg(y11, x22,layer) * w(2)...
                    + sourceImg(y22, x11,layer) * w(3)...
                    + sourceImg(y22, x22,layer) * w(4);
            end
        end
    end
end
end
function slope = biggest_slope(A,B,C,D)
slope = abs([A-B,A-C,A-D,B-C,B-D,C-D]);
end
function out = notInsideBorder(vec,low,up)
out = false;
if any(any(vec < low)), out = true;
elseif any(any(vec > up)), out = true;
end
end