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;
A = sym('a', [nJeff nJeff]);
B = sym('b', [nJeff^2 nJeff]);
C = sym('c', [nJeff^3 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 = [0.002 -0.001 0.001 -0.003 -0.002 -0.001]                % vector in Tp(M), x

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% 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') sym(b(1,4), 'f') sym(b(1,5), 'f') sym(b(1,6), 'f')];

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

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

for k=1:nJeff
  for i=1:nJeff
    for j=1:nJeff
      B(i+(nJeff*(k-1)),j) = (sym(1,'f')/factorial(2))*diff(A(i,j),pv(1,j));
    end
  end
end

for l=1:nJeff
  for k=1:nJeff
    for i=1:nJeff
      for j=1:nJeff
        C(i+(nJeff*(k-1)+nJeff^2*(l-1)),j) = (sym(1,'f')/factorial(3))*diff(B(i+(3*(k-1)),j),pv(1,j));
      end
    end
  end
end

x1T = [x1; x2; x3; x4; x5; x6];

n = sym('n', [nJeff nJeff^2]);

for i=1:nJeff
  for j=1:nJeff^2
    n(i,j) = 0;
  end
end

for i=1:nJeff
  for j=1:nJeff
    n(i,j+nJeff*(i-1)) = x(1,j);
  end
end

n1 = n(1,1:end);
n2 = n(2,1:end);
n3 = n(3,1:end);
n4 = n(4,1:end);
n5 = n(5,1:end);
n6 = n(6,1:end);

o = sym('o', [nJeff^2 nJeff^3]);

for i=1:nJeff^2
  for j=1:nJeff^3
    o(i,j) = 0;
  end
end

for i=1:nJeff^2
  for j=1:nJeff
    o(i,j+nJeff*(i-1)) = x(1,j);
  end
end

o01 = o(1,1:end);
o02 = o(2,1:end);
o03 = o(3,1:end);
o04 = o(4,1:end);
o05 = o(5,1:end);
o06 = o(6,1:end);
o07 = o(7,1:end);
o08 = o(8,1:end);
o09 = o(9,1:end);
o10 = o(10,1:end);
o11 = o(11,1:end);
o12 = o(12,1:end);
o13 = o(13,1:end);
o14 = o(14,1:end);
o15 = o(15,1:end);
o16 = o(16,1:end);
o17 = o(17,1:end);
o17 = o(17,1:end);
o18 = o(18,1:end);
o19 = o(19,1:end);
o20 = o(20,1:end);
o21 = o(21,1:end);
o22 = o(22,1:end);
o23 = o(23,1:end);
o24 = o(24,1:end);
o25 = o(25,1:end);
o26 = o(26,1:end);
o27 = o(27,1:end);
o28 = o(28,1:end);
o29 = o(29,1:end);
o30 = o(30,1:end);
o31 = o(31,1:end);
o32 = o(32,1:end);
o33 = o(33,1:end);
o34 = o(34,1:end);
o35 = o(35,1:end);
o36 = o(36,1:end);

T0 = [F1; F2; F3; F4; F5; F6];
T0poly = vpa(subs(T0,pv,a),10);
T0num = double(vpa(subs(T0,pv,a),10));

T1 = A*x1T;
T1 = expand(T1);
T1poly = vpa(subs(T1,pv,a),10);
T1num = double(vpa(subs(subs(T1,pv,a),x,b),10));

T21 = n1*B*x1T;
T22 = n2*B*x1T;
T23 = n3*B*x1T;
T24 = n4*B*x1T;
T25 = n5*B*x1T;
T26 = n6*B*x1T;
T21 = expand(T21);
T22 = expand(T22);
T23 = expand(T23);
T24 = expand(T24);
T25 = expand(T25);
T26 = expand(T26);

T2 = [T21; T22; T23; T24; T25; T26];
T2poly = vpa(subs(T2,pv,a),10);
T2num = double(vpa(subs(subs(T2,pv,a),x,b),10));

T311 = o01*C;
T312 = o02*C;
T313 = o03*C;
T314 = o04*C;
T315 = o05*C;
T316 = o06*C;
T321 = o07*C;
T322 = o08*C;
T323 = o09*C;
T324 = o10*C;
T325 = o11*C;
T326 = o12*C;
T331 = o13*C;
T332 = o14*C;
T333 = o15*C;
T334 = o16*C;
T335 = o17*C;
T336 = o18*C;
T341 = o19*C;
T342 = o20*C;
T343 = o21*C;
T344 = o22*C;
T345 = o23*C;
T346 = o24*C;
T351 = o25*C;
T352 = o26*C;
T353 = o27*C;
T354 = o28*C;
T355 = o29*C;
T356 = o30*C;
T361 = o31*C;
T362 = o32*C;
T363 = o33*C;
T364 = o34*C;
T365 = o35*C;
T366 = o36*C;

C2 = [T311; T312; T313; T314; T315; T316; T321; T322; T323; T324; T325; T326; T331; T332; T333; T334; T335; T336; T341; T342; T343; T344; T345; T346; T351; T352; T353; T354; T355; T356; T361; T362; T363; T364; T365; T366];
T31 = n1*C2*x1T;
T32 = n2*C2*x1T;
T33 = n3*C2*x1T;
T34 = n4*C2*x1T;
T35 = n5*C2*x1T;
T36 = n6*C2*x1T;
T31 = expand(T31);
T32 = expand(T32);
T33 = expand(T33);
T34 = expand(T34);
T35 = expand(T35);
T36 = expand(T36);

T3 = [T31; T32; T33; T34; T35; T36];
T3poly = vpa(subs(T3,pv,a),10);
T3num = double(vpa(subs(subs(T3,pv,a),x,b),10));

P3 = T0 + T1 + T2 + T3
P3poly = T0poly + T1poly + T2poly + T3poly
P3num = T0num + T1num + T2num + T3num

Fnum = double(vpa(subs(subs(subs(subs(subs(subs([F1; F2; F3; F4; F5; F6],p1,a(1,1)+b(1,1)),p2,a(1,2)+b(1,2)),p3,a(1,3)+b(1,3)),v1,a(1,4)+b(1,4)),v2,a(1,5)+b(1,5)),v3,a(1,6)+b(1,6)),10))