clc
clear all
close all

theta = 0.4*pi;

l1 = 1;
l2 = 1.2;
l3 = .8;
l4 = 1.1;
phi4 = .1*pi;

P1 = [0 0]';

% Loop-closure equation
syms l1 l2 l3 l4 theta phi2 phi3 phi4
eq1 = l1*cos(theta) + l2*cos(phi2) + l3*cos(phi3) + l4*cos(phi4);
% solve([eq1;eq2],phi2)
% solve(eq1,phi2)
% solve(eq1,phi3)
eq3 = phi3 == acos((l2*cos(phi2) + l4*cos(phi4) + l1*cos(theta))/l3)

% solve([eq2;eq3],phi2)
eq4 = l1*sin(theta) + l2*sin(phi2) + l3*sin(acos((l2*cos(phi2) + l4*cos(phi4) + l1*cos(theta))/l3)) + l4*sin(phi4);
solve(eq4,phi2)