% Se uso Octave
% #! ese01 -qf
% Program starts
%
clear all
close all
%
% Exercise 2
% 
% with Octave
%
printf('Hello, this is the beginning ! \n');
%
% with MatLab
% display('Hello, this is the beginning !');
%
%  FIXED Parameters
%
l1 = 100
l2 = 30
l3 = 90
l4 = 80
%   Crank initial angle
theta2 =  30 * pi / 180;
%
% omega2 = 400 * ( 2 * pi / 60);
% alpha2 = 0;
% =========================================================================
%  Initial ITERATION
% =========================================================================
%  
%  INITIAL GUESS VALUES
%
x2 = 30;   % 30 ; 
y2 =  20 ; % 20 ; 
%
% theta3 
%  = 110 for the second configuration y > 0
%  = -70 for the second configuration
theta3 = 110 *(pi/180); % 110 *(pi/180); 
%
x3 = 40; 
% y3 = 60 first y3 =-40 second
y3 = 60; % 60; 
% theta4 (esempio)
% = -80 first = 50  second
theta4 = -80*(pi/180);  %  -80*(pi/180); 
x4 = 70; 
% y4 
% = 80 first -70 second
y4 = -70; % 80; 
%
%  INTRODUCTION OF VECTOR u (INITIAL GUESS)
%
u(1) = x2;
u(2) = y2;
u(3) = theta3;
u(4) = x3;
u(5) = y3;
u(6) = theta4;
u(7) = x4;
u(8) = y4;
%
% STARTING POSITION 
% 
figure(1)
% frame link
AX = 0;
AY = 0;
DX = 100;
DY = 0;
Xlinea = [AX,DX];
Ylinea = [AY,DY];
plot(Xlinea,Ylinea,'-oy','LineWidth',2);
hold on
% crank
A2X = x2 - (l2/2)*cos(theta2);
A2Y = y2 - (l2/2)*sin(theta2);
B2X = x2 + (l2/2)*cos(theta2);
B2Y = y2 + (l2/2)*sin(theta2);
Xlinea = [A2X,B2X];
Ylinea = [A2Y,B2Y];
plot(Xlinea,Ylinea,'-or','LineWidth',1);
% with Matlab axis equal;
% with Matlab axis([-20 120 -100 120]);
% with Octave
axis([-20 120 -100 120],"equal");
grid on;
ylabel(' Y axis');
xlabel(' X axis');  
title([' Initial guess configuration (it is not assembled well...)  ']);
set(1,'Color','w');
hold on
% coupler
B3X = x3 - (l3/2)*cos(theta3);
B3Y = y3 - (l3/2)*sin(theta3);
C3X = x3 + (l3/2)*cos(theta3);
C3Y = y3 + (l3/2)*sin(theta3);
Xlinea = [B3X,C3X];
Ylinea = [B3Y,C3Y];
plot(Xlinea,Ylinea,'-or','LineWidth',1);
hold on
% rocker
C4X = x4 - (l4/2)*cos(theta4);
C4Y = y4 - (l4/2)*sin(theta4);
D4X = x4 + (l4/2)*cos(theta4);
D4Y = y4 + (l4/2)*sin(theta4);
Xlinea = [C4X,D4X];
Ylinea = [C4Y,D4Y];
plot(Xlinea,Ylinea,'-or','LineWidth',1);
hold off
%
%  Array Psi of the constraint functions
%
Psi(1) = x2 - (1/2)*l2*cos(theta2);
Psi(2) = y2 - (1/2)*l2*sin(theta2);
Psi(3) = x3 -(l3/2)*cos(theta3) - x2 - (l2/2)*cos(theta2);
Psi(4) = y3 -(l3/2)*sin(theta3) - y2 - (l2/2)*sin(theta2);
Psi(5) = x4 -x3 -(1/2)*l3*cos(theta3) -(1/2)*l4*cos(theta4);
Psi(6) = y4 -y3 -(1/2)*l3*sin(theta3) -(1/2)*l4*sin(theta4);
Psi(7) = x4 - l1 + (1/2)*l4*cos(theta4);
Psi(8) = y4 + (1/2)*l4*sin(theta4);
%
%  Norm of Psi
%
Resti(1) = norm(Psi);
%
%  minor Psi_U of the Jacobian matrix
%
PsiU = zeros(8,8);
%
PsiU(1,1) = 1;
%
PsiU(2,2) = 1;
%
PsiU(3,1) = -1;
PsiU(3,3) = (1/2)*l3*sin(theta3);
PsiU(3,4) = 1;
%
PsiU(4,2) = -1;
PsiU(4,3) = -(1/2)*l3*cos(theta3);
PsiU(4,5) = 1;
%
PsiU(5,3) = (1/2)*l3*sin(theta3);
PsiU(5,4) = -1;
PsiU(5,6) = (1/2)*l4*sin(theta4);
PsiU(5,7) = 1;
%
PsiU(6,3) = -(1/2)*l3*cos(theta3);
PsiU(6,5) = -1;
PsiU(6,6) = -(1/2)*l4*cos(theta4);
PsiU(6,8) = 1;
%
PsiU(7,6) = -(1/2)*l4*sin(theta4);
PsiU(7,7) = 1;
%
PsiU(8,6) = (1/2)*l4*cos(theta4);
PsiU(8,8) = 1;
%
%  Evaluation of the increment of the vector u (using Newton Raphson method)
%
DeltaU = - inv(PsiU) * Psi' ;
%
%
%  iteration cycle starts
%
%   number of iterations
%
Niteraz = 8
%
for i = 2:Niteraz
    %
    u = u + DeltaU';
    %
    x2     = u(1);
    y2     = u(2);
    theta3 = u(3);
    x3     = u(4);
    y3     = u(5);
    theta4 = u(6);
    x4     = u(7);
    y4     = u(8);
    %
    Psi(1) = x2 - (1/2)*l2*cos(theta2);
    Psi(2) = y2 - (1/2)*l2*sin(theta2);
    Psi(3) = x3 -(l3/2)*cos(theta3) - x2 - (l2/2)*cos(theta2);
    Psi(4) = y3 -(l3/2)*sin(theta3) - y2 - (l2/2)*sin(theta2);
    Psi(5) = x4 -x3 -(1/2)*l3*cos(theta3) -(1/2)*l4*cos(theta4);
    Psi(6) = y4 -y3 -(1/2)*l3*sin(theta3) -(1/2)*l4*sin(theta4);
    Psi(7) = x4 - l1 + (1/2)*l4*cos(theta4);
    Psi(8) = y4 + (1/2)*l4*sin(theta4);
    %
    Resti(i) = norm(Psi);
    % 
    %
    PsiU(3,3) = (1/2)*l3*sin(theta3);
    PsiU(4,3) = -(1/2)*l3*cos(theta3);
    PsiU(5,3) = (1/2)*l3*sin(theta3);
    PsiU(5,6) = (1/2)*l4*sin(theta4);
    PsiU(6,3) = -(1/2)*l3*cos(theta3);
    PsiU(6,6) = -(1/2)*l4*cos(theta4);
    PsiU(7,6) = -(1/2)*l4*sin(theta4);
    PsiU(8,6) = (1/2)*l4*cos(theta4);
    %
    DeltaU = - inv(PsiU) * Psi' ;
end;
%
%
figure(2)
plot(Resti,'-b','LineWidth',1);
axis([0 10 0 0.1]);
ylabel(' morm of the constraint function array (it should be very small)');
xlabel(' Number of iterations');  
title([' trend of the morm of the constraint function array  ']);
set(2,'Color','w');
%
%
% final configuration
% 
figure(3)
% frame
AX = 0;
AY = 0;
DX = 100;
DY = 0;
Xlinea = [AX,DX];
Ylinea = [AY,DY];
plot(Xlinea,Ylinea,'-oy','LineWidth',2);
hold on
% crank
A2X = x2 - (l2/2)*cos(theta2);
A2Y = y2 - (l2/2)*sin(theta2);
B2X = x2 + (l2/2)*cos(theta2);
B2Y = y2 + (l2/2)*sin(theta2);
Xlinea = [A2X,B2X];
Ylinea = [A2Y,B2Y];
plot(Xlinea,Ylinea,'-or','LineWidth',1);
% with MatLab axis equal;
% with Matlab axis([-20 120 -100 120]);
% with Octave
axis([-20 120 -100 120],"equal");
grid on;
ylabel(' Y axis');
xlabel(' X axis');  
title([' Final configuration (it should be assembled well...)  ']);
set(3,'Color','w');
hold on
% coupler
B3X = x3 - (l3/2)*cos(theta3);
B3Y = y3 - (l3/2)*sin(theta3);
C3X = x3 + (l3/2)*cos(theta3);
C3Y = y3 + (l3/2)*sin(theta3);
Xlinea = [B3X,C3X];
Ylinea = [B3Y,C3Y];
plot(Xlinea,Ylinea,'-or','LineWidth',1);
hold on
% rocker
C4X = x4 - (l4/2)*cos(theta4);
C4Y = y4 - (l4/2)*sin(theta4);
D4X = x4 + (l4/2)*cos(theta4);
D4Y = y4 + (l4/2)*sin(theta4);
Xlinea = [C4X,D4X];
Ylinea = [C4Y,D4Y];
plot(Xlinea,Ylinea,'-or','LineWidth',1);
hold on
%
% with Matlab
% display(' I have done ! ')
% display(' Good bye class ! ')
% with Octave
printf('I have done! \n');
printf('Good bye class ! \n');