clc
clear 
fprintf('Calcolo degli angoli del cinematismo: Quadrilatero Articolato \n');

% dimensione vettori del quadrilatero
z1=248.56; z2=338.66; z3=282.74; z4=198.8;

% angoli statici del quadrilatero (theta3, theta4)
t3=271; t4=181;

% range angoli theta2
initial_angle = 45;
final_angle= 90;

% Analisi posizione
for j=1:(final_angle - initial_angle)

  teta2rad=deg2rad(initial_angle+j);
  x0=[deg2rad(t3); deg2rad(t4)];
  kmax=1000; tol=1e-2; 

  f1=@(x) z2*cos(teta2rad)+z3.*cos(x(1))+z4.*cos(x(2))-z1; 
  f2=@(x) z2*sin(teta2rad)+z3.*sin(x(1))+z4.*sin(x(2));
  
  df11=@(x) -z3.*sin(x(1));
  df12=@(x) -z4.*sin(x(2));
  df21=@(x) z3.*cos(x(1));
  df22=@(x) z4.*cos(x(2));

  [x,k,info ] = newtonsis2(f1,f2,df11,df12,df21,df22,kmax,tol,x0);
  xrad=rad2deg(x);
    
  if info==0
    fprintf(' θ2 = '); disp(rad2deg(teta2rad));
    fprintf(' θ3 calcolato = '); disp(xrad(1));
    fprintf(' θ4 calcolato = '); disp(xrad(2));
    fprintf(' Numero di iterazioni '); disp(k);
    fprintf(' ------------------------------- \n'); 
    teta2(j)=rad2deg(teta2rad);
    teta3(j)=xrad(1);
    teta4(j)=xrad(2);
  else
    fprintf(' Numero massimo iterazioni raggiunto ');
  end
  
  t=deg2rad(110);
  a=[0; z2*cos(teta2rad+t); z2*cos(teta2rad+t)+z3*cos(x(1)+t); z1*cos(t)];
  b=[0; z2*sin(teta2rad+t); z2*sin(teta2rad+t)+z3*sin(x(1)+t); z1*sin(t)];

   
  plot(a,b,'r','linewidth',1)
  grid on
  grid minor
  axis([-400 200 -200 400])
  hold on
  plot(a,b,'o','color', 'r','markersize',8)
  xlabel('x [mm]')
  ylabel('y [mm]')
  title("Kinematic analysis of the front double wishbone position")
   
end

figure(2)
plot (teta2,teta3); 
grid on
grid minor
hold on
plot (teta2,teta4);
xlabel('θ2 [°]')
ylabel('θ3 - θ4 [°]')
legend("θ3","θ4")
hold off
title("θ3 and θ4 variations respective to θ2")
 
% Analisi velocita

r=input('Digita 1 per il calcolo della velocità, 2 per uscire: ');
  if r==1 
    fprintf('Calcolo delle velocità angolari del cinematismo: Quadrilatero Articolato \n');
    a1=input('Inserisci il valore di θ2 di partenza: ');
    a2=input('inserisci il valore θ2 finale: ');
    t=input('inserisci Δt in secondi: ');
    w2=deg2rad((a2-a1)/t);
     
    %cerca la posizione del valore messo nell'input a1 nell'array teta2
    for i=1:45
      if a1==teta2(i)
        i1=i;
      end
    end
 
    dftx=[-z2*sin(deg2rad(teta2(i1))); z2*cos(deg2rad(teta2(i1)))];
    dfty=[-z3*sin(deg2rad(teta3(i1))) -z4*sin(deg2rad(teta4(i1))); z3*cos(deg2rad(teta3(i1))) z4*cos(deg2rad(teta4(i1)))]; 
    w=-inv(dfty)*(dftx*w2);
    fprintf('Velocità angolare [rad/s] w2='); disp(w2);
    fprintf('Velocità angolare [rad/s] w3='); disp(w(1));
    fprintf('Velocità angolare [rad/s] w4='); disp(w(2));

  end