Arm

using PyPlot
R=1

a1x(g) = R*cos(g[1])
a2x(g) = R*cos(g[2])
a1y(g) = R*sin(g[1])
a2y(g) = R*sin(g[2])

a1xD(g) = -R*sin(g[1])
a2xD(g) = -R*sin(g[2])
a1yD(g) = R*cos(g[1])
a2yD(g) = R*cos(g[2])


ax(g) = a1x(g) + a2x(g)
ay(g) = a1y(g) + a2y(g)

Fskj=[1.3;1.3]
J(g) = [a1xD(g) a1yD(g); a2xD(g) a2yD(g)]
Fv(g) = [ax(g); ay(g)] - Fskj

guess=[pi/3;pi/5]
for i in 1:50
    guess = guess - inv(J(guess))\Fv(guess)
end

println(guess)
println(Fv(guess) + Fskj)

plot([1.3], [1.3], "o")

function euler_iteration(armDiff, KM, DM, increment)
    derivative = DM * armDiff + KM
    derivative *= increment
    return inv(DM) * (derivative - KM)
end

h = 0.001
r = 0:h:50

DM(a) = [0 1; -2*(1-a)  -1]
KM(t) = [0; 0.3*sin(2*pi*t)]

aArm1 = guess[1]
aArm2 = guess[2]
arm1Diff = [pi/4;0.0]
arm2Diff = [0.0;0.0]

iterations = 1
for i in r
    if iterations%100 == 1
        angles = [arm1Diff[1], arm2Diff[1]]
        arm1plotx = [0, a1x(angles)]
        arm1ploty = [0, a1y(angles)]
        arm2plotx = [a1x(angles), ax(angles)]
        arm2ploty = [a1y(angles), ay(angles)]
        plot(arm1plotx, arm1ploty)
        plot(arm2plotx, arm2ploty)

        pause(0.1)
    end


    arm1Diff = euler_iteration(arm1Diff, KM(i), DM(aArm1), h)
    arm2Diff = euler_iteration(arm2Diff, KM(i), DM(aArm2), h)

    iterations += 1
end

println(arm1Diff)
println(arm2Diff)