# 2軸 IK Test
# 2joint_ik_test.py 
# 【参考】 https://tajimarobotics.com/kinematics-two-link-model-2/

import math
import tkinter

SCREEN_WIDTH = 400
SCREEN_HEIGHT = 400

L1 = 0.4
L2 = 0.6

def draw(x, y):
    global canvas

    print('x, y:', x, y)

    t1_sub1 = math.atan2(y, x) 
    t1_sub2 = math.acos((L1 ** 2  - L2 ** 2 + x ** 2 + y ** 2) / (2 * L1 * math.sqrt(x ** 2 + y ** 2)))
    t1_0 = t1_sub1 + t1_sub2

    t2_0 = math.atan2(y - L1 * math.sin(t1_0), x - L1 * math.cos(t1_0)) - t1_0

    print('t1, t2:', t1_0, t2_0)

    x2 = math.cos(t1_0) * L1
    y2 = math.sin(t1_0) * L1

    x3 = x2 + math.cos(t1_0 + t2_0) * L2
    y3 = y2 + math.sin(t1_0 + t2_0) * L2

    canvas.delete('L1')
    canvas.delete('L2')

    cy = SCREEN_HEIGHT / 2
    canvas.create_line(0, cy, x2 * SCREEN_WIDTH, -(y2 - 0.5) * SCREEN_HEIGHT, fill = "white", width=4, tag='L1')
    canvas.create_line(x2 * SCREEN_WIDTH, -(y2 - 0.5) * SCREEN_HEIGHT, x3 * SCREEN_WIDTH, -(y3 - 0.5) * SCREEN_HEIGHT, fill = "red", width=4, tag='L2')

    canvas.pack()

def canvas_move(event):
    global mouse_press
    if not mouse_press:
        return
    
    #print("clicked at ", event.x, event.y)
    y = 1 - event.y / SCREEN_HEIGHT - 0.5
    #print("y:", y, -(y - 0.5) * SCREEN_HEIGHT)
    draw(event.x / SCREEN_WIDTH, y)

def canvas_press(event):
    global mouse_press
    mouse_press = True
    canvas_move(event)

def canvas_release(event):
    global mouse_press
    mouse_press = False

# ======================
# メイン
# ======================
if __name__ == "__main__":

    # ウィンドウ初期化
    root = tkinter.Tk()
    root.title(u"2軸 IK Test")

    root.geometry(str(SCREEN_WIDTH) + "x" + str(SCREEN_HEIGHT))   # ウインドウサイズを指定

    canvas = tkinter.Canvas(
        root,
        width = SCREEN_WIDTH,
        height = SCREEN_HEIGHT,
        bg = "black"
    )
    canvas.pack()

    canvas.bind("", canvas_press)
    canvas.bind("", canvas_release)
    canvas.bind("", canvas_move)
    mouse_press = False

    cy = SCREEN_HEIGHT / 2
    canvas.create_line(0, cy, SCREEN_WIDTH, cy, fill = "green", tag='line')

    root.mainloop()