function [f,pre] = MLS_getDeform(control_points,deformed_position_of_control_points,points_of_evaluation,alpha,type,precomputed,size_of_img)
%UNTITLED2 Summary of this function goes here
%   type:
%        Affine = translation, rotation, nonuniform scaling and shear
%        Similarity = translation, rotation and uniform scaling
%        Rigid = translation, rotation
%   alpha: for weigth function (precompute)
%   p_i,q_i,v_i are row vectros eg. [x,y] so p,q,v = [x1,y1;x2,y2;x3,y3;...]
%   size_of_img = [height,width]
p = control_points;
q = deformed_position_of_control_points;
v = points_of_evaluation;

pre = precomputed;
f = [];
% switch the MLS deformation type
if strcmp(type,'Affine')
    f = MLS_Affine(q,v,pre,size_of_img);
elseif strcmp(type,'Similarity')
    f = MLS_Similarity(q,v,pre,size_of_img);
elseif strcmp(type,'Rigid')
     f = MLS_Rigid(q,v,pre,size_of_img);
else
    %ME = MException('type:PassedWrongVariable', ...
    %    'Variable %s should be one of following: Affine, Similarity, Rigid',type);
    %throw(ME)
    fprintf('PassedWrongVariable')
end
end
%MLS deformation
%% rigid deformation
function f = MLS_Rigid(q_i,V,pre,size_of_img)
f = zeros([size_of_img,2]);
% TO DO:
% f = zeros(size(V,1),2);
% f(v(1),v(2),[1,2]) --> f(i,:)
% fx = reshape(f(1,:),num(gridX),num(gridY))
% fy = reshape(f(2,:),num(gridX),num(gridY))

% loop through evaluation points v
for i = 1:size(V,1)
    v = V(i,:);
    % get param for given evaluation point v
    w_i = pre.w_i(:,i);
    sum_w_i = pre.sum_w_i(i);
    Ai= pre.Rigid(:,2*i-1:2*i);   
    p_ast = pre.p_ast(i,:);
    [q_ast,q_hat_i] = MLS_q(w_i,sum_w_i,q_i);
    
    % loop through control points
    f_vec=zeros(1,2);
    for j = 1:size(q_i,1)
        
        f_vec = f_vec + q_hat_i(j,:)*Ai(2*j-1:2*j,:);
    end
    len_f_vec = sqrt(sum(f_vec.^2));
  
    f_r = sqrt(sum((v-p_ast).^2)) * f_vec / len_f_vec + q_ast;
    
    % save deformation for given control point
    f(v(1),v(2),[1,2]) = f_r;
end
end
%% similatrity defromation
function f = MLS_Similarity(q_i,V,pre,size_of_img)
f = zeros([size_of_img,2]);

% loop through evaluation points v
for i = 1:size(V,1)
    v = V(i,:);
    % get param for given evaluation point v
    w_i = pre.w_i(:,i);
    sum_w_i = pre.sum_w_i(i);
    Ai_mu= pre.Similarity(:,2*i-1:2*i);   
    
    [q_ast,q_hat_i] = MLS_q(w_i,sum_w_i,q_i);
    
    % loop through control points
    sum_f=[0,0];
    for j = 1:size(q_i,1)        
        sum_f = sum_f + q_hat_i(j,:)*Ai_mu(2*j-1:2*j,:);
    end
    
    sum_f = sum_f + q_ast;
    
    % save deformation for given control point
    f(v(1),v(2),[1,2]) = sum_f;
end
end

% %% affine deformation
function f = MLS_Affine(q_i,V,pre,size_of_img)
f = zeros([size_of_img,2]);
% loop through evaluation points v
for i = 1:size(V,1)
    v = V(i,:);
    % get param for given evaluation point v
    w_i = pre.w_i(:,i);
    sum_w_i = pre.sum_w_i(i);
    Aj = pre.Affine(:,i);    

    [q_ast,q_hat_i] = MLS_q(w_i,sum_w_i,q_i);
    
    % loop through control points
    sum_f=[0,0];
    for j = 1:size(q_i,1)
        sum_f = sum_f + Aj(j)*q_hat_i(j,:);
    end
    sum_f = sum_f + q_ast;
    % save deformation for given control point
    %if isequal(v,p)
    f(v(1),v(2),[1,2]) = sum_f;   
end
end

%% parameters for deformation function, q = deformed positions of the control points p
function [q_ast,q_hat_i] = MLS_q(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



