clear all; clc; close all;

%constants
l2 = .325;              %long arm length
l1 = .22;               %short arm length
l4 = .5;                %length to mass 1
m1 = 10;                %counterweight
m2 = .1;                %projectile
I1 = .00605;             %moment of inertia of arm
I2 = 2/5*m2*(.0254/2)^2;%moment of inertia of projectile
rad = pi/180;
ho = l1*(1 + sin(0));

%experimental varaiables
l3 = .2;

%loop stuff
tfinal = 100;
step = .1;
n = tfinal/step;

%initial vals

beta(1) = 180*rad;

theta(1) = 0;
phi(1) = 0;
psi(1) = -45*rad;
thetaDot(1) = 0;
phiDot(1) = 0;
psiDot(1) = 0;
thetaDdot(1) = 0;
phiDdot(1) = 0;
psiDdot(1) = 0;

for ii=1:n

    p = [theta(ii); psi(ii); phi(ii); thetaDot(ii); psiDot(ii); phiDot(ii)];
    Mmat = [1, 0, 0, 0, 0, 0, 0;
            0, 1, 0, 0, 0, 0, 0;
            0, 0, 1, 0, 0, 0, 0;
            0, 0, 0, m1*l1^2 + m2*l2^2 + I1, -m2*l2*l3*cos(p(1) - p(2)), m1*l1*l4*cos(p(1) - p(3)), -l2*cos(p(1));
            0, 0, 0, -m2*l2*l3*cos(p(1) - p(2)), m2*l3^2 + I2, 0, l3*cos(p(2));
            0, 0, 0, m1*l1*l4*cos(p(1) - p(3)), 0, m1*(l4^2 + 1), 0;
            0, 0, 0, -l2*cos(p(1)), l3*cos(p(2)), 0, 0];

    Qmat = [p(4);
            p(5);
            p(6);
            m1*l1*l4*p(6)^2*sin(p(1) - p(3)) + m2*l2*l3*p(5)^2*sin(p(1) - p(2)) - (m1*l1 - m2*l2)*9.8*cos(p(1));
           -m2*l2*l3*p(4)^2*sin(p(1) - p(2)) - m2*l3*9.8*cos(p(2));
            m1*l1*l4*p(4)^2*sin(p(1) - p(3)) - m2*l3*9.8*cos(p(3));
            -p(4)^2*l2*sin(p(1)) + l3*p(5)^2*sin(p(2))];

    temp = step*(Mmat\Qmat);
    newP = p(1:6) + .5*temp(1:6);

    Mmat = [1, 0, 0, 0, 0, 0, 0;
            0, 1, 0, 0, 0, 0, 0;
            0, 0, 1, 0, 0, 0, 0;
            0, 0, 0, m1*l1^2 + m2*l2^2 + I1, -m2*l2*l3*cos(newP(1) - newP(2)), m1*l1*l4*cos(newP(1) - newP(3)), -l2*cos(newP(1));
            0, 0, 0, -m2*l2*l3*cos(newP(1) - newP(2)), m2*l3^2 + I2, 0, l3*cos(newP(2));
            0, 0, 0, m1*l1*l4*cos(newP(1) - newP(3)), 0, m1*(l4^2 + 1), 0;
            0, 0, 0, -l2*cos(newP(1)), l3*cos(newP(2)), 0, 0];

    Qmat = [newP(4);
            newP(5);
            newP(6);
            m1*l1*l4*newP(6)^2*sin(newP(1) - newP(3)) + m2*l2*l3*newP(5)^2*sin(newP(1) - newP(2)) - (m1*l1 - m2*l2)*9.8*cos(newP(1));
           -m2*l2*l3*newP(4)^2*sin(newP(1) - newP(2)) - m2*l3*9.8*cos(newP(2));
            m1*l1*l4*newP(4)^2*sin(newP(1) - newP(3)) - m2*l3*9.8*cos(newP(3));
            -newP(4)^2*l2*sin(newP(1)) + l3*newP(5)^2*sin(newP(2))];

    qDotMat = step*(Mmat\Qmat);
    pDot = qDotMat(1:6);
    lamda = qDotMat(7);

    theta(ii+1) = p(1) + pDot(1);
    psi(ii+1) = p(2) + pDot(2);
    phi(ii+1) = p(3) + pDot(3);
    thetaDot(ii+1) = p(4) + pDot(4);
    psiDot(ii+1) = p(5) + pDot(5);
    phiDot(ii+1) = p(6) + pDot(5);

    beta(ii+1) = beta(ii) - p(1);
    x(ii) = cos(p(2))*l3;
    y(ii) = sin(p(2))*l3;

end

figure;
    plot(x, y)
    xlabel('X'); ylabel('Y'); title('X and Y position of projectile');