function [newImg,finterp] = MLS_applyDeform(sourceImg,f,v,grid_size)
%UNTITLED2 Summary of this function goes here
%   f(v) - deformation vector
%   v - point of evaluation
%   grid_size - loop info for v points
[imh,imw,imd] = size(sourceImg);
newImg = uint8(ones(imh,imw,imd)).*256; % for testing
% newImg = ones(imh,imw,imd);

% finterp.x = [];
% finterp.y = [];
% finterp.zX = [];
% finterp.zY = [];
finterp = 0;
% iterate over evaluated points
% +--------> x,i=width
% |
% |
% |
% V y,j=height
stepX = grid_size(2);
stepY = grid_size(1);

% go over grid
for i = 1:stepX:imw
    for j = 1:stepY:imh
        % next i,j to create square for bilinear interp
        ni = i+stepX;
        nj = j+stepY;
        
        % length of square sides
        x2 = stepX;
        y2 = stepY;
        
        % check image borders
        if (ni > imw), ni=imw; x2=imw-i;end
        if (nj > imh), nj=imh; y2=imh-j;end
        
        %   y A            ni,nj
        %     |            x2,y2
        % f01 |-----------+  f11
        %     |           |
        %     |           |
        %     |           |
        %     |           |
        % f00 +----------------> x
        %   x1,y1 = 0      f10
        %   i,j            ni,j
        % call interpolation
        [newImg,finterp] = Interpolation(i,j,ni,nj,x2,y2,f,finterp,newImg,imh,imw,imd,sourceImg);
        
    end
end

end
%% bilinear interpolation for 1*1 square,
function ret = bilinear_interpol(x,y,f00,f10,f01,f11)
% bilinear interpolation for 1*1 square, if you pass x=x/x2 and y=y/y2 you
% get bilinear interpolation for rectangle in begining of cordinate system
ret = (f00*(1-x) + f10*x)*(1-y) + (f01*(1-x) + f11*x)*y;
end

function ret = first_half_bilinear_interpolation(x,f00,f10,f01,f11)
ret = [(f00*(1-x) + f10*x),(f01*(1-x) + f11*x)];
end
function ret = second_half_bilinear_interpolation(y,p)
ret = p(1)*(1-y)+p(2)*y;
end
function slope = biggest_slope(A,B,C,D)
slope = abs([A-B,A-C,A-D,B-C,B-D,C-D]);
end

%% Interpolation
function [newImg,finterp] = Interpolation(i,j,ni,nj,x2,y2,f,finterp,newImg,imh,imw,imd,sourceImg)
% calculate step form slopes

% try 
step = min([x2,y2])/max([biggest_slope(f(j,i,1), f(j,ni,1), f(nj,i,1), f(nj,ni,1)),biggest_slope(f(j,i,2), f(j,ni,2), f(nj,i,2), f(nj,ni,2))]);
% catch e
%     f
%     fprintf('Size f: %f×%f×%f, i:%f, ni:%f, j:%f, nj:%f',size(f),i,ni,j,nj)
%     rethrow e
% end

for x = 0:step:x2
    % fill deformation function for area between control points (1/2)
    py = first_half_bilinear_interpolation(x/x2,f(j,i,1), f(j,ni,1), f(nj,i,1), f(nj,ni,1));
    px = first_half_bilinear_interpolation(x/x2,f(j,i,2), f(j,ni,2), f(nj,i,2), f(nj,ni,2));
    
    for y = 0:step:y2
        fy = second_half_bilinear_interpolation(y/y2,py);
        fx = second_half_bilinear_interpolation(y/y2,px);
        
        
%          finterp.x = [finterp.x;x+i];
%          finterp.y = [finterp.y;y+j];
%          finterp.zX = [finterp.zX;fx];
%          finterp.zY = [finterp.zY;fy];
         
       
        % check borders        
        nx = fx;
        ny = fy;
        if nx > imw, nx = imw; end
        if nx < 1, nx = 1; end
        
        if ny > imh, ny = imh; end
        if ny < 1, ny = 1; end
        
        % round dest. pixels
        f_nx = round(nx);
        
        f_ny = round(ny);
        
        % float from rounding
        fpx =  round(nx-(i+x))-(fx-(i+x));
        fpy =  round(ny-(j+y))-(fy-(j+y));
        
        % find closest source pixels 1*1 square
        x11 = floor(i+x+fpx);
        if x11 < 1
            x11 = 1; x22 = 1; fpx = 0;
        elseif x11 > imw
            x11 = imw; x22 = imw; fpx = 0;
        else
            x22 = ceil(i+x+fpx);
            fpx = i+x+fpx-x11; % distance on unit square for interpolation
        end
        
        
        y11 = floor(j+y+fpy);
        if y11 < 1
            y11 = 1;y22=1;fpy = 0;
        elseif y11 > imh
            y11 = imh;y22=imh;fpy = 0;
        else
            y22 = ceil(j+y+fpy)  ;
            fpy = j+y+fpy-y11;
        end
        
        
        newImg = interpolate_pixels(x11,x22,y11,y22,f_nx,f_ny,newImg, fpx, fpy, sourceImg,imw,imh,imd);
    end
end
end

function newImg = interpolate_pixels(x11,x22,y11,y22,f_nx,f_ny,newImg, fpx, fpy, sourceImg,imw,imh,imd)
if x22 > imw, x22 = imw;end
if x11 > imw, x11 = imw;end

if y11 > imh, y11 = imh;end
if y22 > imh, y22 = imh;end

for layer = 1:imd
    
    % interpolate the pixel at x+x2+float,y+y2+float and
    % move it to nx,ny
    %try
    newImg(f_ny, f_nx, layer) = bilinear_interpol(...
        fpx, fpy,...
        sourceImg(y11, x11,layer),...
        sourceImg(y11, x22,layer),...
        sourceImg(y22, x11,layer),...
        sourceImg(y22, x22,layer));
    %catch
    %    fprintf('f_ny: %f, f_nx: %f, layer: %f, x11: %f, x22: %f, y11: %f,y22: %f',f_ny,f_nx,x11,x22,y11,y22)
    %end
    end
end