clc,format compact
clear
close all
%% The length of the links of the mechanism
L(1) = 20;
L(2) = 17;
L(3) = 40;
L(4) = 36;
L(5) = 36;

%% Initialization of the figure
figure, hold on

%% give initial guess for the solution (value of X at which fvec~=0)
X(1) = deg2rad(120);
X(2) = deg2rad(90);
X(3) = deg2rad(100);
X(4) = 15;
ThetaRange = deg2rad(0:4:360);
[THETA2,THETA3,THETA4,THETA5,D] = deal(zeros(size(ThetaRange)));
Theta1 = deg2rad(15);
i = 1;
for ThetaCurrent = ThetaRange
    clf
    THETA2(i) = ThetaCurrent;
    X0 = X;
    options = optimset('Display','off');
    [X,fval,condition] = fsolve(@(X)FCN(L,THETA2(i),X,Theta1),X0,options);
    if condition ~=1
        warning(['Convergence Problem at Theta2 = '  num2str(rad2deg(ThetaCurrent)) ])
    end

    THETA3(i) = X(1);
    THETA4(i) = X(2);
    THETA5(i) = X(3);
    D(i)      = X(4);
%     disp([rad2deg(X(1:3))     d])

    P1 = [0 , 0];
    P2(1) = L(2)*cos(THETA2(i));
    P2(2) = L(2)*sin(THETA2(i));


    P5(1) = -L(1)*cosd(180);
    P5(2) = -L(1)*sind(180);

    P3(1) = P2(1)-L(3)*cos(THETA3(i));
    P3(2) = P2(2)-L(3)*sin(THETA3(i));

    P6(1) = P5(1)+D(i)*cosd(90);
    P6(2) = P5(2)+D(i)*sind(90);

    P4(1) = P3(1)-(L(4)/2)*cos(THETA4(i));
    P4(2) = P3(2)-(L(4)/2)*sin(THETA4(i));

    plot(P1(1),P1(2),'Or','MarkerSize',10,'LineWidth',3)
    hold on
    plot(  [P1(1),P2(1),P3(1),P4(1),P5(1)]  ,   [P1(2),P2(2),P3(2),P4(2),P5(2)]   ,'-*','LineWidth',2)
    plot(  [P3(1),P6(1)]  ,   [P3(2),P6(2)]   ,'-r*','LineWidth',2)
    plot(  [P1(1),P5(1)]  ,   [P1(2),P5(2)]   ,'-g*','LineWidth',4)
    plot(  [P5(1),P5(1)]  ,   [P1(2),100]   ,'-g*','LineWidth',4)
    text(P1(1),P1(2),'P1')
    text(P2(1),P2(2),'P2')
    text(P3(1),P3(2),'P3')
    text(P4(1),P4(2),'P4')

    axis equal
    axis([-50 120 -50 120])
    grid on
    pause(0.001)
    i = i+1;
end
%% Velocity and Acceleration Analysis
%------------------------------------
% Initialization
%---------------
[dTHETA3,dTHETA4,dTHETA5,dD,ddTHETA3,ddTHETA4,ddTHETA5,ddD] = deal(zeros(size(ThetaRange)));

dTHETA2 = 2*ones(size(ThetaRange)); % rad/s (Input angular velocity)
ddTHETA2 = 0*ones(size(ThetaRange)); % rad/s^2 (Input angular acceleration)

for i = 1:length(THETA2)
%   Velocity analysis
%   -----------------
    A = [L(3)*cos(THETA3(i))  (L(4)/2)*cos(THETA4(i))   L(5)*cos(THETA5(i))      0;
         L(3)*sin(THETA3(i))  (L(4)/2)*sin(THETA4(i))   L(5)*sin(THETA5(i))      0;
         0                     L(4)*cos(THETA4(i))      L(5)*cos(THETA5(i))     -1;
         0                     L(4)*sin(THETA4(i))      L(5)*sin(THETA5(i))      0 ];
    b = [L(2)*cos(THETA2(i))*dTHETA2(i);
         L(2)*sin(THETA2(i))*dTHETA2(i);
         0;
         0];
    dTHETA = A\b;
    dTHETA3(i) = dTHETA(1);
    dTHETA4(i) = dTHETA(2);
    dTHETA5(i) = dTHETA(3);
    dD(i)      = dTHETA(4);
%   Acceleration analysis
%   ---------------------
    C = [-L(3)*cos(THETA3(i))  -(L(4)/2)*cos(THETA4(i)) -L(5)*cos(THETA5(i))  0;
         -L(3)*sin(THETA3(i))  -(L(4)/2)*sin(THETA4(i)) -L(5)*sin(THETA5(i))  0;
         0                       L(4)*cos(THETA4(i))    L(5)*cos(THETA5(i))  -1;
         0                       L(4)*sin(THETA4(i))    L(5)*cos(THETA5(i))   0 ];

    D = [-L(2)*cos(THETA2(i))*ddTHETA2(i)+L(2)*sin(THETA2(i))*dTHETA2(i)^2-L(3)*sin(THETA3(i))*dTHETA3(i)^2-(L(4)/2)*sin(THETA4(i))*dTHETA4(i)^2-L(5)*sin(THETA5(i))*dTHETA5(i)^2;
         -L(2)*sin(THETA2(i))*ddTHETA2(i)-L(2)*cos(THETA2(i))*dTHETA2(i)^2+L(3)*cos(THETA3(i))*dTHETA3(i)^2+(L(4)/2)*cos(THETA4(i))*dTHETA4(i)^2+L(5)*cos(THETA5(i))*dTHETA5(i)^2;
         L(5)*sin(THETA5(i))*dTHETA5(i)^2+L(4)*sin(THETA4(i))*dTHETA4(i)^2;
         -L(5)*cos(THETA5(i))*dTHETA5(i)^2-L(4)*cos(THETA4(i))*dTHETA4(i)^2
         ];
    ddTHETA    = C\D;
    ddTHETA3(i) = ddTHETA(1);
    ddTHETA4(i) = ddTHETA(2);
    ddTHETA5(i) = ddTHETA(3);
    ddD(i)      = ddTHETA(4);

end

%   Velocity analysis plots
%   -----------------------
figure('Name','Joint velocities')
subplot(2,3,1)
plot(ThetaRange,dTHETA2),grid on,axis tight
xlabel('\theta_2'),title('\omega_2')
subplot(2,3,2)
plot(ThetaRange,dTHETA3),grid on,axis tight
xlabel('\theta_2'),title('\omega_3')
subplot(2,3,3)
plot(ThetaRange,dTHETA4),grid on,axis tight
xlabel('\theta_2'),title('\omega_4')
subplot(2,3,4)
plot(ThetaRange,dTHETA5),grid on,axis tight
xlabel('\theta_2'),title('\omega_5')
subplot(2,3,5)
plot(ThetaRange,dD),grid on,axis tight
xlabel('\theta_2'),title('dD')
% Acceleration analysis plots
% ---------------------------
figure('Name','Joint accelerations')
subplot(2,3,1)
plot(ThetaRange,ddTHETA2),grid on,axis tight
xlabel('\theta_2'),title('\alpha_2')
subplot(2,3,2)
plot(ThetaRange,ddTHETA3),grid on,axis tight
xlabel('\theta_2'),title('\alpha_3')
subplot(2,3,3)
plot(ThetaRange,ddTHETA4),grid on,axis tight
xlabel('\theta_2'),title('\alpha_4')
subplot(2,3,4)
plot(ThetaRange,ddTHETA5),grid on,axis tight
xlabel('\theta_2'),title('\alpha_5')
subplot(2,3,5)
plot(ThetaRange,ddD),grid on,axis tight
xlabel('\theta_2'),title('ddD')