clear;clc;
syms p1 p2 p3 v1 v2 v3;
%syms x1 x2 x3 x4 x5 x6;
%syms s;
pv = [p1 p2 p3 v1 v2 v3];
%x = [x1 x2 x3 x4 x5 x6];
syms F1(p1,p2,p3,v1,v2,v3) F2(p1,p2,p3,v1,v2,v3) F3(p1,p2,p3,v1,v2,v3) F4(p1,p2,p3,v1,v2,v3) F5(p1,p2,p3,v1,v2,v3) F6(p1,p2,p3,v1,v2,v3);
nJeff = 6;
Asym = sym('a', [nJeff nJeff]);

F1 = v1                              % Vector field satifying the canonical flip on the double tangent bundle coordinate function
F2 = v2                              % Vector field satifying the canonical flip on the double tangent bundle coordinate function
F3 = v3                              % Vector field satifying the canonical flip on the double tangent bundle coordinate function

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% You shouldn't need to alter anything above this line %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

F4 = atan(p1)-(pi/4)                 % Vector field satifying the canonical flip on the double tangent bundle coordinate function
F5 = asin(p2)-(pi/3)                 % Vector field satifying the canonical flip on the double tangent bundle coordinate function
F6 = acos(p3)-(pi/6)                 % Vector field satifying the canonical flip on the double tangent bundle coordinate function

a = [1 sqrt(3)/2 sqrt(3)/2 0 0 0]    % point in manifold, p
%b = [00.01 -0.02 0.01]                % vector in Tp(M), x

Ts = 0.1                             % Sampling time (in seconds)

B = eye(nJeff);                       % Control Input Matrix

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% You shouldn't need to alter anything below this line %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

double(vpa(subs([F1 F2 F3 F4 F5 F6],pv,a),10)) % Check vector field is at critical point

a = [sym(a(1,1), 'f') sym(a(1,2), 'f') sym(a(1,3), 'f') sym(a(1,4), 'f') sym(a(1,5), 'f') sym(a(1,6), 'f')];
%b = [sym(b(1,1), 'f') sym(b(1,2), 'f') sym(b(1,3), 'f')];

F = [F1; F2; F3; F4; F5; F6];

for i=1:nJeff
  for j=1:nJeff
  Asym(i,j) = diff(F(i,1),pv(1,j));
  end
end

A = double(vpa(subs(Asym,pv,a),10))
D = eig(A)

B

ZJ = zeros(nJeff,2*nJeff);
QJ = [A, B; ZJ];
GBlock = expm(Ts*QJ);
G = GBlock(1:nJeff,1:nJeff)
GEig = eig(G)
abs(GEig)
H = GBlock(1:nJeff,nJeff+1:2*nJeff)
Ctrb = ctrb(G,H);
rank(Ctrb);
K = place(G,H,zeros(nJeff,1))
Deadbeat01 = G-H*K
DeadbeatEig = round(eig(Deadbeat01),4)

