clear;clc;clear all;

% robotBuilder

% syms g;
g = 9.81;

syms th1 th2 th3 th4;
thJ4 = [th1; th2; th3; th4];
% thJ4 = [th1; 0; th3; th4];

thJ2 = [thJ4(1,1); thJ4(2,1)];
% thJ2 = [thJ4(1,1); 0];

thJdot2 = [thJ4(3,1); thJ4(4,1)];

% m=sym('m',[1,nJeff]);
% syms(m);
m1 = 23.142; % in kg
m2 = 23.142; % in kg

% L=sym('L',[1,nJeff]);
% syms(L);
L1 = 1; % in m
L2 = 1; % in m

Ixx1 = 0.093805; % in kg*m^2
Ixy1 = 0; % in kg*m^2
Txz1 = 0; % in kg*m^2
Iyy1 = 2.6125; % in kg*m^2
Iyz1 = 0; % in kg*m^2
Izz1 = 2.6677; % in kg*m^2

Ixx2 = 0.093805; % in kg*m^2
Ixy2 = 0; % in kg*m^2
Txz2 = 0; % in kg*m^2
Iyy2 = 2.6125; % in kg*m^2
Iyz2 = 0; % in kg*m^2
Izz2 = 2.6677; % in kg*m^2

% Ixx=sym('Ixx',[1,nJeff]);
% syms(Ixx);
% 
% Ixy=sym('Ixy',[1,nJeff]);
% syms(Ixy);
% 
% Ixz=sym('Ixz',[1,nJeff]);
% syms(Ixz);
% 
% Iyy=sym('Iyy',[1,nJeff]);
% syms(Iyy);
% 
% Iyz=sym('Iyz',[1,nJeff]);
% syms(Iyz);
% 
% Izz=sym('Izz',[1,nJeff]);
% syms(Izz);

% Theta=sym('Theta',[1,2]).';
% syms(Theta)
% 
% syms thetaDot1 thetaDot2 doubleThetaDot1 doubleThetaDot2
% 
% thetaDot = [thetaDot1; thetaDot2];
% doubleThetaDot = [doubleThetaDot1; doubleThetaDot2];

% initialPosition1 = deg2rad([0; 0]); % IC for once-around CCW with no loop-the-loops by inner link
% initialPosition2 = [2*pi;0]; % IC for once-around CCW with no loop-the-loops by the inner link with no gravity
% initialPosition = [initialPosition1(1,1); initialPosition1(2,1); initialPosition2(1,1); initialPosition2(2,1)];

initialPosition1 = deg2rad([0; 0]); % IC for once-around CCW with no loop-the-loops by second link
initialPosition2 = [-3.1; 11.4]; % IC for once-around CCW with no loop-the-loops by outer link with no gravity
initialPosition = [initialPosition1(1,1); initialPosition1(2,1); initialPosition2(1,1); initialPosition2(2,1)];

% initialPosition = deg2rad([0; 0; -400; 46.4034]); % IC for once-around CW with no loop-the-loops by second link

[tsol ysol] = ode45(@odeJeff,[0:0.1:1.0],initialPosition);
% plot(tsol, ysol(:,1:1))

ysolJeffVar = [tsol ysol]';

save('ysolJeffChristoffel01.mat','ysolJeffVar')

RobotShow2D

function dThetadt = odeJeff(t,thJ4)
  thJdot2 = [thJ4(3,1); thJ4(4,1)];

  g = 9.81;
  
  G = -Gvector(thJ4(1,1),thJ4(2,1),9.81); % Gravity vector
  % G =  -Gvector(thJ4(1,1),thJ4(2,1),0);
  % (L1,L2,Theta1,Theta2,g,m1,m2)
  % G = -Gvector(1,1,thJ4(1,1),thJ4(2,1),9.81,23.142,23.142); % Gravity vector
  % G = [0; 0; 0; 0]; % Gravity vector
  Tau = [0; 0]; % Torque input
  % Tau = [0; 0; 10*sin(thJ4(1,1)); 10*cos(thJ4(2,1))]; % Torque input
  % (Izz1,Izz2,L1,L2,Theta1,Theta2,m1,m2)
  % Minv = Minvmatrix(2.6677,2.6677,1,1,thJ4(1,1),thJ4(2,1),23.142,23.142); % Riemannian metric/mass matrix
  % Minv4 = [eye(2) zeros(2); zeros(2) Minv]; % Direct sum of identity matrix with Riemannian metric, lifting to the tangent bundle
  Minv4 = MSMatrixInv(thJ4(1,1),thJ4(3,1)); % Sasaki metric, a lift of the Riemannian metric to the tangent bundle
  Gamma = Gammamatrix(thJ4(2,1)); % Christoffel symbols of the second kind AKA Coriolis matrix
  % dThetadt = [thJ4(3,1); thJ4(4,1);0; 0] - Minv4*([0; 0; thJdot2.'*Gamma(:,:,1)*thJdot2; thJdot2.'*Gamma(:,:,2)*thJdot2] + Tau - (G));
  % (L1,L2,Theta1,Theta2,m2)
  % Gamma = Gammamatrix(1,1,thJ4(2,1),23.142); % Christoffel symbol terms AKA Centrifugal terms + Coriolis terms
  % dThetadt = [thJ4(3,1); thJ4(4,1);0; 0] - Minv4*([0; 0; Tau - [thJdot2.'*Gamma(:,:,1)*thJdot2; thJdot2.'*Gamma(:,:,2)*thJdot2] - G]); % EoM: doubleThetaDot = MJeffInv*(Tau - VmJeff - GmJeff)
  dThetadt = [thJ4(3,1); thJ4(4,1);(6.25e+33*(- 0.0056563*sin(thJ4(2,1))*thJ4(3,1)^2 + 0)*(5.6563e+32*cos(thJ4(2,1)) + 3.8281e+32))/(5.4108e+31*cos(thJ4(2,1)) + 1.9996e+64*cos(thJ4(2,1))^2 - 3.6225e+64) - (2.3926e+66*(0 + 2.0e-35*thJ4(4,1)*sin(thJ4(2,1))*(5.6563e+32*thJ4(3,1) + 2.8281e+32*thJ4(4,1))))/(5.4108e+31*cos(thJ4(2,1)) + 1.9996e+64*cos(thJ4(2,1))^2 - 3.6225e+64); (6.25e+33*(5.6563e+32*cos(thJ4(2,1)) + 3.8281e+32)*(0 + 2.0e-35*thJ4(4,1)*sin(thJ4(2,1))*(5.6563e+32*thJ4(3,1) + 2.8281e+32*thJ4(4,1))))/(5.4108e+31*cos(thJ4(2,1)) + 1.9996e+64*cos(thJ4(2,1))^2 - 3.6225e+64) - (5.625e+35*(1.2569e+31*cos(thJ4(2,1)) + 2.1076e+31)*(- 0.0056563*sin(thJ4(2,1))*thJ4(3,1)^2 + 0))/(5.4108e+31*cos(thJ4(2,1)) + 1.9996e+64*cos(thJ4(2,1))^2 - 3.6225e+64)]; % EoM: doubleThetaDot = MJeffInv*(Tau - VmJeff - GmJeff)
  % dThetadt = [thetaDot(2,1); thetaDot(2,1);0; 0] - Minv4*([0; 0; thetaDot.'*Gamma(:,:,1)*thetaDot; thetaDot.'*Gamma(:,:,2)*thetaDot] + Tau - G);
end