clear;clc;close all;

robotBuilder

syms v1 v2 v3 v4;

GammaV = sym(zeros(nJeff, nJeff));
for i = 1:nJeff;
    for j = 1:nJeff;
        for k = 1:nJeff
    GammaV(i,k) = GammaV(i,k) + Gamma(i,j,k)*thetaDot(j);
        end;
    end;
end;

Term2 = sym(zeros(nJeff, nJeff));
for i = 1:nJeff;
    for j = 1:nJeff;
        for k = 1:nJeff;
            for l = 1:nJeff;
                Term2(i,j) = Term2(i,j) + MJeff(k,l)*GammaV(i,k)*GammaV(j,l);
            end;
        end;
    end;
end;

MJeff1 = MJeff + Term2;

MJeff2 = sym(zeros(nJeff, nJeff));
for i = 1:nJeff;
    for j = 1:nJeff;
        for k = 1:nJeff;
            for l = 1:nJeff
                MJeff2(i,j) = MJeff2(i,j) + MJeff(k,j)*Gamma(i,l,k)*thetaDot(l);
            end;
        end;
    end;
end;

MJeff3 = sym(zeros(nJeff, nJeff));
for i = 1:nJeff;
    for j = 1:nJeff;
        for k = 1:nJeff;
            for l = i:nJeff;
                MJeff3(i,j) = MJeff3(i,j) + MJeff(i,k)*Gamma(j,l,k)*thetaDot(l);
            end;
        end;
    end;
end;

MJeffS = vpa([MJeff1, MJeff2; MJeff3, MJeff],5)
MJeffSInv = inv(MJeffS);
vpa(MJeffSInv,5);

MJeffDot = sym(zeros(nJeff, nJeff, nJeff));
for l = 1:nJeff
    for i = 1:nJeff;
        for j = 1:nJeff
            MJeffDot(i,j,l) = diff(MJeff(i,j),Theta(l));
        end;
    end;
end

MJeffDotV = sym(zeros(nJeff, nJeff));
for l = 1:nJeff
    for i = 1:nJeff
        for j = 1:nJeff
            MJeffDotV(i,j) = MJeffDot(i,j,l)*thetaDot(l);
        end;
    end;
end

MJeffDotV;

Omega = vpa([MJeffDotV, MJeff; -MJeff, zeros(nJeff,nJeff)],5)

A = vpa(MJeffSInv*Omega,5); % Need a g^S-polar decomposition of A = JP; J is then the almost-complex structure

% Z = chol(MJeffS,"nocheck"); % Write g^S as Z.'*Z to throw g^S inner product onto standard inner product
% A_std = Z*A*Z.'; % Throw g^S inner product onto standard inner product
% 
% PSq_std = A_std.'*A_std; % Standard symmetrization of A
% P_std = sqrtm(PSq_std); Take standard square root of P
% P = inv(Z)*P_std*inv(Z.'); % Throw standard square root of P back onto g^S-square root of P
% 
% Pinv = inv(P); % Take inverse of P
% J = A*Pinv % A = JP in g^S-polar decomosition, so J = A*P^(-1)

J = vpa(A,5) % I have strong numerical evidence to believe P = I, which makes J = A 
 
% check1 = round(vpa(P.'-P,5),3) % P should be symmetric, so this should be 0
% check2 = round(vpa(P*MJeffS*P - PMJeffSSq,5),3) % P should be a g^S-square root of P, so this should be 0
% check3 = round(vpa(J.'*MJeffS*J - MJeffS,5),3) % J should be g^S-orthogonal, so this should be 0
% check4 = round(vpa(J*J,5),3) % J should be an almost-complex structure so this should be -I

% The above was initially thought by me to be too complicated to do with elementary symbolic functions, so
% we evaluate at a point in the tangent bundle.

% Th2Val = 0;
% thetaDot1Val = 0;
% thetaDot2Val = 0;
% 
% MJeffSTh2 = subs(MJeffS,Theta2,Th2Val);
% MJeffSTh2V1 = vpa(subs(MJeffSTh2,thetaDot1,thetaDot1Val),5);
% MJeffSTh2SV1V2 = vpa(subs(MJeffSTh2V1,thetaDot2,thetaDot2Val),5);
% 
% ATh2 = subs(A,Theta2,Th2Val); 
% ATh2V1 = vpa(subs(ATh2,thetaDot1,thetaDot1Val),5); 
% ATh2V1V2 = vpa(subs(ATh2V1,thetaDot2,thetaDot2Val),5);
% 
% ZTh2V1V2 = chol(MJeffSTh2SV1V2,"nocheck"); % Write g^S as Z.'*Z to throw g^S inner product onto standard inner product
% ATh2V1V2_std = ZTh2V1V2*ATh2V1V2*ZTh2V1V2.'; % Throw g^S-A onto standard-A
% 
% PTh2V1V2_stdSq = ATh2V1V2_std.'*ATh2V1V2_std; % Standard symmetrization of A
% PTh2V1V2_std = sqrtm(PTh2V1V2_stdSq); % Take standard square root of P
% PTh2AV1V2 = inv(ZTh2V1V2)*PTh2V1V2_std*inv(ZTh2V1V2.'); % Throw standard square root of P back onto g^S-square root of P
% 
% PTh2V1V2inv = inv(PTh2AV1V2); % Take inverse of P
% 
% JTh2V1V2 = round(vpa(ATh2V1V2*PTh2V1V2inv,5),3) % A = JP in polar decomosition, so J = A*P^(-1)

% check1 = round(vpa(PTh2V1V2.'-PTh2AV1V2,5),3) % P should be symmetric, so this should be 0
% check2 = round(vpa(PTh2V1V2*MJeffSTh2SV1V2*PTh2AV1V2 - MJeffSTh2SV1V2,5),3) % P should be a g^S-square root of P, so this should be 0
% check3 = round(vpa(JTh2V1V2.'*MJeffSTh2SV1V2*JTh2AV1V2 - MJeffSTh2SV1V2,5),3) % J should be g^S-orthogonal, so this should be 0
% check4 = round(vpa(JTh2V1V2*JTh2AV1V2,5),3) % J should be an almost-complex structure so this should be -I