% esercizio di cinematica
clear
clc
clf

% introduzione dati geometrici
z1=1;
z4=15;
z3=4;
chi=40*pi/180;

% input
q=0

errore=5;
epsilon=0.01;

% valore inziale di z2 e phi2
%incognite_i=[1,0]';             % punto sbagliato
incognite_i=[10,-0.1]';         % punto corretto

A=z1*[cos(q),sin(q)]';

% soluzione della diade
B=[z4,0]';
zt=norm(A-B);
phit=atan2(B(2)-A(2),B(1)-A(1));

cont=1;
while errore>epsilon
    z2=incognite_i(1);
    phi2=incognite_i(2);
    phi3=phi2+pi-chi;
    J=[ cos(phi2)       -z2*sin(phi2)-z3*sin(phi3);...
        sin(phi2)       z2*cos(phi2)+z3*cos(phi3)];
    F=[ z2*cos(phi2)+z3*cos(phi3)-zt*cos(phit);...
        z2*sin(phi2)+z3*sin(phi3)-zt*sin(phit)];
    incognite_i_piu_1=incognite_i-inv(J)*F;
    errore=norm(F);
    incognite_i=incognite_i_piu_1;
    cont=cont+1
end

% calcolo di C
C=A+z2*[cos(phi2),sin(phi2)]';

% grafica
hold on
axis equal
plot([0 A(1)],[0 A(2)],'b')
plot([A(1) C(1)],[A(2) C(2)],'r')
plot([C(1) B(1)],[C(2) B(2)],'g')