clear all;
close all;

%% Ploting the curve
%Lemniscate of Gerono

phi = linspace(0, 15, 120);
center = [0, -0.5];

lemniscate = [cos(phi) + center(1); sin(2*phi)/2 + center(2)];

%Initial angles
angles = [-pi/2, pi/4, pi/2];
segment_lengths = [0.9, 0.5, 0.3];
timesteps_num = 25;

%% Solve IK problem

for i=1:size(lemniscate,2)-1
    
    if i==1
        angles = inverse_kin_and_smoothness(angles, [lemniscate(1,i),lemniscate(2,i)], segment_lengths, timesteps_num, false);
    else
        angles = inverse_kin_and_smoothness(angles, [lemniscate(1,i),lemniscate(2,i)], segment_lengths, timesteps_num, true);
    end
end

%% Define IK solver function


function final_a = inverse_kin_and_smoothness(init_angles, target, seg_lens, ts_num, draw_line)

    assert(sqrt(target(1)^2+target(2)^2) <= sum(seg_lens));
    
    %% Define placeholders
    
    alphas = zeros(3,ts_num);
    alphas(:,1) = init_angles;
    
    a1 = alphas(1,1);
    a12 = a1 + alphas(2,1);
    a123 = a12 + alphas(3,1);
    
    E1_inverse = zeros(2, ts_num);
    E2_inverse = zeros(2, ts_num);
    E3_inverse = zeros(2, ts_num);
    
    E1_inverse(:,1) = [seg_lens(1).*cos(a1); seg_lens(1).*sin(a1)];
    E2_inverse(:,1) = [E1_inverse(1,1) + seg_lens(2).*cos(a12); E1_inverse(2,1)+seg_lens(2).*sin(a12)];
    E3_inverse(:,1) = [E2_inverse(1,1) + seg_lens(3).*cos(a123); E2_inverse(2,1)+seg_lens(3).*sin(a123)];
    
    target_difference = [(target(1)-E3_inverse(1,1))/ts_num;(target(2)-E3_inverse(2,1))/ts_num];
    
    for i= 1:ts_num-1
        
            a1 = alphas(1,i);
            a12 = a1 + alphas(2,i);
            a123 = a12 + alphas(3,i);
            
             
             J = [-seg_lens(1)*sin(a1) - seg_lens(2)*sin(a12) - seg_lens(3)*sin(a123), ...
                 - seg_lens(2)*sin(a12) - seg_lens(3)*sin(a123), ...
                 - seg_lens(3)*sin(a123);
                 seg_lens(1)*cos(a1) + seg_lens(2)*cos(a12) + seg_lens(3)*cos(a123), ...
                 seg_lens(2)*cos(a12) + seg_lens(3)*cos(a123), ...
                 seg_lens(3)*cos(a123)];
             
             delta_a = pinv(J)*target_difference;
             alphas(:,i+1) = alphas(:,i) + delta_a;
            
    end
    
    final_a = alphas(:,ts_num);
    
%     tau = linspace(0,1,ts_num);
%     beta = zeros(3,ts_num);
%     
%     beta0 = alphas(:,1);
%     betaT = alphas(:,end);
%   
%     beta = beta0+(beta0-betaT)*(15.*tau.^4-6.*tau.^5-10.*tau.^3);
    
    b1 = alphas(1,:);
    b12 = b1 + alphas(2,:);
    b123 = b12 + alphas(3,:);
    
    
    E1_smooth_jerk = [seg_lens(1).*cos(b1); seg_lens(1).*sin(b1)];
    E2_smooth_jerk = [E1_smooth_jerk(1,:) + seg_lens(2).*cos(b12); E1_smooth_jerk(2,:)+seg_lens(2).*sin(b12)];
    E3_smooth_jerk = [E2_smooth_jerk(1,:) + seg_lens(3).*cos(b123); E2_smooth_jerk(2,:)+seg_lens(3).*sin(b123)];
    
    for i = 1:ts_num
       
        arm_jerk = plot([0,E1_smooth_jerk(1,i),E2_smooth_jerk(1,i),E3_smooth_jerk(1,i)],[0,E1_smooth_jerk(2,i),E2_smooth_jerk(2,i),E3_smooth_jerk(2,i)],'Color','#D95319','LineWidth',1.2);
        hold on; 
        grid on;
        %plot(E3_smooth_jerk(1,i),E3_smooth_jerk(2,i),'k.');
        if draw_line == true
            scatter(E3_smooth_jerk(1,i),E3_smooth_jerk(2,i),'b.');
            %targ_mark = plot(target(1),target(2),'r.','Markersize', 25);
        end
        xlim([-1.5, 1.5]);
        ylim([-1.5, 1.5]);
                
        drawnow;
        delete(arm_jerk);
        %delete(targ_mark);
        
    end
    
    


end





