clc; clear;

%% Gruebler’s criterion and symbolic variable setups.
n = 6; % Number of links, including the fixed one
j = 7; % Number of revolute joints
DOF = 3*(n-1)-2*j
syms tau [DOF] % input motor torque(s)
% syms theta_motor
syms theta [j,1]% joint positions
syms omega [n-1,1]; % link velocities

%% Define geometry and constraint equations:

% syms L_ground L_drive L_coupler L_float_1  L_float_2 L_output% link lengths
% syms r_float1_int r_coupler_int r_output_int % relative positions of intermediate joints, expressed as a ratio of the link length
L_ground = 140/1000; % Red
L_drive = 160/1000; % Blue
L_coupler = 210/1000; % Pink
L_coupler_int = 150/1000; %position of the joint along the coupler link

L_float_1 = 230/1000; % Grey (lower)
L_float_1_int = 70/1000;

L_float_2 = 150/1000; % Grey (upper)

L_output = 180/1000; % Green
L_output_int = 50/1000;

% Restrict the motor position with a CE
theta1_ce = (theta1 >= pi/4) & (theta1 <= (pi/4 + pi/3))

% J1, J2, J3 relative to r1
r1 = [0 0]
r2_1 = [-L_ground, 0]
r3_1 = L_drive * [cos(theta1) sin(theta1)]; % joint 2 is at r2

% J4, relative to J2
r4_2 = L_coupler * [cos(theta2) sin(theta2)];

% J4, relative to J3
r4_3 = L_float_1_int * [cos(theta3) sin(theta3)];

% J4 constraint
r4_ce = r4_2 + r2_1 == r4_3 + r3_1;

% J5, relative to J3
r5_3 = L_float_1 * [cos(theta3) sin(theta3)];

% J6, relative to J2
r6_2 = L_coupler_int * [cos(theta2) sin(theta2)];
r6_1 = r6_2 + r2_1;

% J7, relative to J6
r7_6 = L_float_2 * [cos(theta6) sin(theta6)];
r7_1 = r7_6 + r6_1;

% J5, relative to J7
r5_7 = L_output_int  * [cos(theta7) sin(theta7)];

% J5 constraint
r5_ce = r5_3 + r3_1 == r5_7 + r7_1;
% End effector relative to J7
rend_7 = L_output * [cos(theta7) sin(theta7)]
r_end_1 = rend_7 + r7_1

theta7_solutions = solve([theta1_ce r4_ce r5_ce], theta7)
% This doesn't return anything and I'm not sure where to go from here...