import os

import PySimpleGUI as sg
import serial
import numpy as np
import time

# sg.preview_all_look_and_feel_themes()  #to display all themes

# open serial communication
ser = serial.Serial("COM7", 9600)
# Waiting for arduino to start sketch
time.sleep(2)

slider_width = 50
slider_height = 40
button_width = 10
button_height = 2

stepper_range = ((0, 360), (-115, 115), (-115, 115), (-115, 115))
stepper_default_values_rel = [0, 0, 0, 0]
stepper_og_values_rel = [0, 0, 0, 0]
stepper_moveTo_angels_rel = [0, 0, 0, 0]
stepper_actual_angels_rel = [0, 0, 0, 0]

segments_length = [15, 50, 40, 30]
end_effector_actual_pos = [0, 0, 0]  # y,z coordination of end effector and rotation angel theta
# ik_m3_actual_pos = [0, 0, 0]  # y,z coordination of m3 and rotation angel theta
end_effector_move_to_pos = [0, 0, 0]  # y,z coordination of end effector and rotation angel theta
ik_step_size = 5  # default step size 5 cm

arm_isCalibrated = 0
arm_calibrate = 0

FK_slider_values = [0, 0, 0, 0]


# Definition of function to calculate angels of a triangle with known side lengths using the law of cosine
def calculate_triangle_angles(a, b, c):
    angle_a = np.rad2deg(np.arccos((b ** 2 + c ** 2 - a ** 2) / (2 * b * c)))
    angle_b = np.rad2deg(np.arccos((c ** 2 + a ** 2 - b ** 2) / (2 * c * a)))
    angle_c = 180 - angle_a - angle_b  # The sum of angles in a triangle is 180 degrees

    return angle_a, angle_b, angle_c


# Definition of function to calculate joints angels relative to horizon
def calculate_angels_to_horizon(relative_angels_stepper):
    absolute_angels = [0, 0, 0, 0]
    absolute_angels[0] = relative_angels_stepper[0]
    absolute_angels[1] = absolute_angels[0] + relative_angels_stepper[1]
    absolute_angels[2] = absolute_angels[1] + relative_angels_stepper[2]
    absolute_angels[3] = absolute_angels[2] + relative_angels_stepper[3]

    for j in range(4):
        if absolute_angels[j] > 360:
            absolute_angels[j] = absolute_angels[j] - 360
    print("relative:", relative_angels_stepper,
          "absolute:", absolute_angels)
    return absolute_angels


# Definition of function to calculate joints angels relative to another, based on their angels relative to horizon
def calculate_angels_relative(absolute_angels_stepper):
    relative_angels = [0, 0, 0, 0]
    relative_angels[0] = absolute_angels_stepper[0]
    relative_angels[1] = absolute_angels_stepper[1] - relative_angels[0]
    relative_angels[2] = absolute_angels_stepper[2] - relative_angels[1]
    relative_angels[3] = absolute_angels_stepper[3] - relative_angels[2]

    for j in range(4):
        if relative_angels[j] > 360:
            relative_angels[j] = relative_angels[j] - 360

    return relative_angels


# Definition of function to calculate coordination of end effector and Motor3
def fk_calculate_position(stepper_angels_rel):
    alpha_rel = np.array(stepper_angels_rel) * (np.pi / 180)
    alpha = calculate_angels_to_horizon(alpha_rel)
    start_x = 0
    start_y = 0

    z0 = np.cos(alpha[0]) * segments_length[0]
    y0 = np.sin(alpha[0]) * segments_length[0]

    y1 = y0 + np.cos(alpha[1]) * segments_length[1]
    z1 = z0 + np.sin(alpha[1]) * segments_length[1]

    y2 = y1 + np.cos(alpha[2]) * segments_length[2]
    z2 = z1 + np.sin(alpha[2]) * segments_length[2]

    y3 = y2 + np.cos(alpha[3]) * segments_length[3]
    z3 = z2 + np.sin(alpha[3]) * segments_length[3]

    end_effector_pos_2d = [y3, z3, stepper_angels_rel[0]]
    m3_pos_2d = [y2, z2, stepper_angels_rel[0]]

    return end_effector_pos_2d


# Definition of function to calculate stepper angels to move end effector to desired coordination by changing M2 and M3
def ik_calculate_angels(end_effector_position):
    z0 = segments_length[0]
    y0 = segments_length[1]

    alpha = [90, -90, 0, 0]  # relative stepper angels to reach desired coordination
    beta = [0, 0, 90]  # angels of right triangle

    y = end_effector_position[0] - y0
    z = end_effector_position[1] - z0

    l2 = segments_length[2]
    l3 = segments_length[3]
    l_h = np.sqrt(np.square(y) + np.square(z))  # calculating hypothesis

    gamma = calculate_triangle_angles(l3, l2, l_h)

    beta[0] = np.rad2deg(np.arctan2(z, y))
    beta[1] = np.rad2deg(np.arctan2(y, z))

    alpha[2] = gamma[0] + beta[0]
    alpha[3] = -(180 - gamma[2])

    return alpha


# Define function to update slider values to match actual stepper values
def update_slider_values(slider_updated_values):
    window['-Slider_Stepper0_FK-'].update(slider_updated_values[0])
    window['-Slider_Stepper1-'].update(slider_updated_values[1])
    window['-Slider_Stepper2-'].update(slider_updated_values[2])
    window['-Slider_Stepper3_FK-'].update(slider_updated_values[3])


# Define function to get chosen step size
def get_step_size():
    if values['-Radio_Option1-']:
        step_size = 1
    elif values['-Radio_Option2-']:
        step_size = 5
    elif values['-Radio_Option3-']:
        step_size = 10

    return step_size


# Calculate Default angels and position
end_effector_actual_pos = fk_calculate_position(stepper_actual_angels_rel)

# Definition of GUI Layout
FK_column = [
    [sg.Text("Forward Kinematics:")],
    [sg.Text("")],
    [sg.Text("Stepper motor 0")],
    [sg.Text(""),
     sg.Slider(range=(0, 360), size=(slider_width, slider_height), default_value=stepper_default_values_rel[0],
               orientation="h", key="-Slider_Stepper0_FK-"), sg.Text("")],
    [sg.Text("Stepper motor 1")],
    [sg.Text(""),
     sg.Slider(range=(-115, 115), size=(slider_width, slider_height), default_value=stepper_default_values_rel[1],
               orientation="h", key="-Slider_Stepper1-"), sg.Text("")],
    [sg.Text("Stepper motor 2")],
    [sg.Text(""),
     sg.Slider(range=(-115, 115), size=(slider_width, slider_height), default_value=stepper_default_values_rel[2],
               orientation="h", key="-Slider_Stepper2-"), sg.Text("")],
    [sg.Text("Stepper motor 3")],
    [sg.Text(""),
     sg.Slider(range=(-115, 115), size=(slider_width, slider_height), default_value=stepper_default_values_rel[3],
               orientation="h", key="-Slider_Stepper3_FK-"), sg.Text("")],
    [sg.Button("Submit", key="-FK_Set_Button-", size=(button_width, button_height)),
     sg.Button("Reset", key="-FK_Reset_Button-", size=(button_width, button_height)),
     sg.Button("Calibrate", key="-FK_Calibrate_Button-", size=(button_width, button_height))]

]

IK_column = [
    [sg.Text("Inverse Kinematics:")],
    [sg.Text("")],
    [sg.Text("Choose Step size:"),
     sg.Radio('1 cm', "RADIO1", key="-Radio_Option1-"),
     sg.Radio('5 cm', "RADIO1", key="-Radio_Option2-", default=True, ),
     sg.Radio('10 cm', "RADIO1", key="-Radio_Option3-")],
    [sg.Text(""), sg.Text(""), sg.Text(""),
     sg.Button("Up", key="-IK_Up_Button-", size=(button_width, button_height))],
    [sg.Button("Backward", key="-IK_Backward_Button-", size=(button_width, button_height)),
     sg.Button("Forward", key="-IK_Forward_Button-", size=(button_width, button_height))],
    [sg.Text(""), sg.Text(""), sg.Text(""),
     sg.Button("Down", key="-IK_Down_Button-", size=(button_width, button_height))],
    [sg.Text("")],
    [sg.Text("")],
    # [sg.Text(ik_m3_actual_pos, key="-m3_pos-")],
    [sg.Text("")]
]

layout = [
    [
        sg.Column(FK_column),
        sg.VSeparator(),
        sg.Column(IK_column),  # vertical_alignment="Top"
        sg.VSeparator(),
    ]
]
window = sg.Window("Robot Control", layout)

# Loop for GUI feedback and Serial Communication

while True:
    event, values = window.read()
    if event == "EXIT" or event == sg.WIN_CLOSED:
        break

    elif event == "-FK_Set_Button-" and arm_isCalibrated == 1:
        FK_slider_values[0] = values["-Slider_Stepper0_FK-"]
        FK_slider_values[1] = values["-Slider_Stepper1-"]
        FK_slider_values[2] = values["-Slider_Stepper2-"]
        FK_slider_values[3] = values["-Slider_Stepper3_FK-"]

        stepper_moveTo_angels_rel = np.array(stepper_actual_angels_rel) - np.array(FK_slider_values)
        stepper_actual_angels_rel = np.array(FK_slider_values)

    elif event == "-FK_Calibrate_Button-":
        update_slider_values(stepper_og_values_rel)

        arm_calibrate = 1
        arm_isCalibrated = 1
        stepper_actual_angels_rel = stepper_og_values_rel

    elif event == "-FK_Reset_Button-":
        update_slider_values(stepper_default_values_rel)

    # Do not forget maximal reachable coordination!
    elif event == "-IK_Up_Button-" and arm_isCalibrated == 1:
        ik_step_size = get_step_size()
        end_effector_move_to_pos = np.array(end_effector_actual_pos) + np.array([0, ik_step_size, 0])
        end_effector_move_to_pos[2] = values["-Slider_Stepper0_FK-"]

        stepper_moveTo_angels_rel = np.array(stepper_actual_angels_rel) - np.array(
            ik_calculate_angels(end_effector_move_to_pos))

        stepper_actual_angels_rel = np.array(ik_calculate_angels(end_effector_move_to_pos))
        update_slider_values(stepper_actual_angels_rel)
        ik_m3_actual_pos = end_effector_move_to_pos

    elif event == "-IK_Down_Button-" and arm_isCalibrated == 1:
        ik_step_size = get_step_size()
        end_effector_move_to_pos = np.array(end_effector_actual_pos) - np.array([0, ik_step_size, 0])
        end_effector_move_to_pos[2] = values["-Slider_Stepper0_FK-"]

        stepper_moveTo_angels_rel = np.array(stepper_actual_angels_rel) - np.array(
            ik_calculate_angels(end_effector_move_to_pos))
        stepper_actual_angels_rel = np.array(ik_calculate_angels(end_effector_move_to_pos))
        update_slider_values(stepper_actual_angels_rel)
        end_effector_actual_pos = end_effector_move_to_pos

    elif event == "-IK_Forward_Button-" and arm_isCalibrated == 1:
        ik_step_size = get_step_size()
        end_effector_move_to_pos = np.array(end_effector_actual_pos) + np.array([ik_step_size, 0, 0])
        end_effector_move_to_pos[2] = values["-Slider_Stepper0_FK-"]

        stepper_moveTo_angels_rel = np.array(stepper_actual_angels_rel) - np.array(
            ik_calculate_angels(end_effector_move_to_pos))
        stepper_actual_angels_rel = np.array(ik_calculate_angels(end_effector_move_to_pos))
        update_slider_values(stepper_actual_angels_rel)
        end_effector_actual_pos = end_effector_move_to_pos

    elif event == "-IK_Backward_Button-" and arm_isCalibrated == 1:
        ik_step_size = get_step_size()
        end_effector_move_to_pos = np.array(end_effector_actual_pos) - np.array([ik_step_size, 0, 0])
        end_effector_move_to_pos[2] = values["-Slider_Stepper0_FK-"]

        stepper_moveTo_angels_rel = np.array(stepper_actual_angels_rel) - np.array(
            ik_calculate_angels(end_effector_move_to_pos))
        stepper_actual_angels_rel = np.array(ik_calculate_angels(end_effector_move_to_pos))
        update_slider_values(stepper_actual_angels_rel)
        end_effector_actual_pos = end_effector_move_to_pos

    stepper_actual_angels_abs = calculate_angels_to_horizon(stepper_actual_angels_rel)
    end_effector_actual_pos = fk_calculate_position(stepper_actual_angels_abs)

    # Formate data before sending
    # str(arm_calibrate)
    data = "<" + str(0) + "," + str(stepper_moveTo_angels_rel[0]) + "," + str(stepper_moveTo_angels_rel[1]) \
           + "," + str(stepper_moveTo_angels_rel[2]) + "," + str(stepper_moveTo_angels_rel[3]) + ">"
    print(data)
    ser.write(data.encode())

    received_string = ser.readline().decode()
    print(received_string)

    stepper_moveTo_angels_rel = [0, 0, 0, 0]
    arm_calibrate = 1

ser.close()
window.close()