RobotArm

using PyPlot
R=1
PlotSteps = 52
ModVal = (PlotSteps - 2)*20

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_method(func, range, y, y1, y2, h)
    angel_array = zeros(PlotSteps)
    aI, c = 1, 1
    for t in range
        if c%ModVal == 1
            angel_array[aI] = y
            aI+=1
        end

        tmp1, tmp2, tmp3 = func(y1, y2, y, t), y2, y1

        y2 +=  h * tmp1
        y1 += h * tmp2
        y = tmp3

        c+=1
    end

    angel_array[aI] = y
    return y, angel_array
end


Ny(y1, y2, theta, t) = -2 * (y1 - theta)-1*y2+ 0.3*sin(2*pi*t)

h = 0.001
range = 0:h:50


E1y = guess[1]
E1y1 = pi/4
E1y2 = 0

E2y = guess[2]
E2y1 = 0
E2y2 = 0

a1finish, a1move = euler_method(Ny, range, E1y, E1y1, E1y2, h)
a2finish, a2move = euler_method(Ny, range, E2y, E2y1, E2y2, h)
answer = [a1finish ,a2finish]

Tf(a) = Fv(a) + Fskj

finish_point = Tf(answer)
println(finish_point)

plot(finish_point[1], finish_point[2], "o")
pause(10)

xaxis = zeros(PlotSteps)
yaxis = zeros(PlotSteps)

for i in 1:PlotSteps
    angles = [a1move[i], a2move[i]]
    point = Tf(angles)

    plot1x = [0, a1x(angles)]
    plot1y = [0, a1y(angles)]
    plot2x = [a1x(angles), ax(angles)]
    plot2y = [a1y(angles), ay(angles)]

    plot(plot1x, plot1y)
    plot(plot2x, plot2y)
    pause(0.1)

    xaxis[i] = point[1]
    yaxis[i] = point[2]
end

plot(xaxis, yaxis)
pause(20)