# Type help("robodk.robolink") or help("robodk.robomath") for more information
# Press F5 to run the script
# Documentation: https://robodk.com/doc/en/RoboDK-API.html
# Reference:     https://robodk.com/doc/en/PythonAPI/robodk.html
# Note: It is not required to keep a copy of this file, your Python script is saved with your RDK project
from subprocess import list2cmdline

# You can also use the new version of the API:
from robodk.robolink import ITEM_TYPE_FRAME, ITEM_TYPE_ROBOT, ITEM_TYPE_TARGET, ITEM_TYPE_OBJECT, Robolink, \
    ITEM_TYPE_TOOL
from robodk import *
from robodk.robomath import *
RDK = robolink.Robolink()

def CalculatePoseFrame2Object(frame, part):
    framePoseAbs = frame.PoseAbs()
    partPoseAbs = part.PoseAbs()

    pose = framePoseAbs.inv() * partPoseAbs
    return pose



linear_rail = RDK.Item('R3-JT7',ITEM_TYPE_ROBOT)
robot = RDK.Item('R3-P', ITEM_TYPE_ROBOT)
robot_1 = robot.getLink(ITEM_TYPE_ROBOT)

part = RDK.Item('202224_A13-13_Mix_hegesztendo', ITEM_TYPE_OBJECT)
partBase_frame = RDK.Item('part Base', ITEM_TYPE_FRAME)
workpiece_frame = RDK.Item('Workpiece Frame', ITEM_TYPE_FRAME)
camera_rail = RDK.Item('Camera-rail', ITEM_TYPE_ROBOT)
r3_rail_base = RDK.Item('R3-JT7 Base', ITEM_TYPE_FRAME)
r3_base = RDK.Item('R3 Base', ITEM_TYPE_FRAME)
r3_base_1 = robot.getLink(ITEM_TYPE_FRAME)
station = RDK.Item('modelling_')

tool = RDK.Item('s_magnet', ITEM_TYPE_TOOL)
home = RDK.Item('Home', ITEM_TYPE_TARGET)
start_t = RDK.Item('Start', ITEM_TYPE_TARGET)

if not all([robot.Valid(), part.Valid(), partBase_frame.Valid(), workpiece_frame.Valid()]):
    print("Error: One or more items not found. Check the names in your RoboDK station.")
    exit()


target_pose = CalculatePoseFrame2Object(partBase_frame, start_t)
new_target = RDK.AddTarget('TransformedTarget', workpiece_frame)#Ez a target arra való, hogy könnyen meghatározzam a célpont helyét
new_target.setPose(target_pose)
linear_rail_rel_pose_to_target = CalculatePoseFrame2Object(r3_rail_base, new_target)
new_target.Delete() #itt már nincs rá szükség
t = RDK.AddTarget('End_robot1', r3_rail_base) #
t.setAsCartesianTarget()
t.setPose(linear_rail_rel_pose_to_target)
t.setLink(robot_1)



# def move_robot_and_rail(target_pose):
#     best_rail_pos = None
#     best_joints = None
#
#     for rail_pos in range(-3500, 12500, 200):
#         linear_rail.setJoints([rail_pos])
#         robot_joints = robot.SolveIK(target_pose)
#
#         if robot_joints is not None:
#             best_rail_pos = rail_pos
#             best_joints = robot_joints
#             break
#
#     if best_joints is not None:
#         linear_rail.setJoints([best_rail_pos])
#         robot.MoveJ(best_joints)
#         print(f"Moved rail to {best_rail_pos} mm")
#         return True
#     else:
#         print("Target is unreachable.")
#         return False
#
# move_robot_and_rail(t.Pose())

# t.setParam("Recalculate")
# t.setAsJointTarget()
# new_joints = t.Joints()
#
# k = CalculatePoseFrame2Object(r3_base, t)
# #
# #
# #
# jl = linear_rail.SolveIK(k)
#
# jr = robot_1.SolveIK(k)
# current_joints = robot_1.Joints()
#
# #robot.MoveJ(current_joints)
# linear_rail.MoveJ(jl)

# robot_1.setPoseTool(tool)
jr_1 = robot_1.SolveIK(t.Pose())

robot_1.MoveJ(t)
robot_1.MoveJ(home)