import numpy as np
import matplotlib.pyplot as plt

from arm2d import Robot2D


# %%%%            Instructions            %%%%%%%%%%%%%%
# %%% create functions invKin2D and evalRobot2D

# %%%%%%% Enter your code here    %%%%%%%%%%%%%%%%%%%%%


if __name__ == "__main__":
    plotRobot2D = Robot2D()

    # link length
    link_lengths = np.array([0.5, 0.5])
    # Random joint angle to start
    angles = np.random.uniform(-1, 1, size=2)

    plotRobot2D.plotRobot2D(angles, link_lengths)

    while True:
        pos = np.array(plt.ginput(n=1)).T   # (2, 1)
        plt.cla()
        plotRobot2D.draw_point(pos)  # Plot your clicked point

        # TODO: angles = Solve inverse kinematics
        angles = invKin2D(link_lengths, angles, pos, n, mode)
        plotRobot2D.plotRobot2D(angles, link_lengths)

        # Stop if the figure is closed
        if plotRobot2D.terminate:
            break