from robolink import *
import numpy as np
import json
RDK = Robolink()
TAB = "    "

project_name = ""
robot_item = RDK.Item("", ITEM_TYPE_ROBOT)
object = RDK.Item("x_calib", ITEM_TYPE_OBJECT)
reference_frame = RDK.Item("WC", ITEM_TYPE_FRAME)
tool = RDK.Item("T9", ITEM_TYPE_TOOL)

if not robot_item.Valid():
    raise Exception('Invalid Robot.')
elif not object.Valid():
    raise Exception('Invalid Object.')
elif not reference_frame.Valid():
    raise Exception('Invalid Reference Frame.')
elif not tool.Valid():
    raise Exception('Invalid Tool.')

points, feature_name = object.GetPoints(FEATURE_POINT)
max_value = max(abs(x) for x in points[0][3:])
try:
    normal_axis = points[0][3:].index(max_value)
except:
    normal_axis = points[0][3:].index(-max_value)
print(points[0])

if normal_axis == 0:
    project_name = "C86"
elif normal_axis == 1:
    project_name = "C85"
elif normal_axis == 2:
    project_name = "C84"

milling_project = RDK.Item(f"{project_name} Settings", ITEM_TYPE_MACHINING)
if milling_project.Valid():
    milling_project.Delete()
program = RDK.Item(project_name, ITEM_TYPE_PROGRAM)
if program.Valid():
    program.Delete()

milling_project = RDK.AddMachiningProject(f"{project_name} Settings", robot_item)
milling_project.setMachiningParameters(part=object)

milling_project.setParam("ApproachRetract", "Normal 40")
milling_project.setParam("Visible", 0)

machiningsettings = milling_project.setParam("Machining")
machiningsettings["AutoUpdate"] = 0
machiningsettings["PointApproach"] = 0.01
milling_project.setParam("Machining", machiningsettings)

sign = None
sign_num = 0
if normal_axis == 0:
    if points[0][3] > 0:
        sign = "+"
        sign_num = 1
    else:
        sign = "-"
        sign_num = -1

    offset_instruction = f"1\tWOFFS.oframe.trans := [OFFS_X {sign} calib_cnt, OFFS_Y, OFFS_Z];\n\
                         1\tTPWrite \" OFF_X \" + NumtoStr(OFFS_X {sign} calib_cnt,3);"
    calib_instruction = f".oframe.trans.x := OFFS_X {sign} calib_cnt"
    calib_offset_cmd = f"calib_offx := {sign} calib_cnt;"
    sign_cmd = f"sign_x := {sign_num};"
elif normal_axis == 1:
    if points[0][4] > 0:
        sign = "+"
        sign_num = 1
    else:
        sign = "-"
        sign_num = -1

    offset_instruction = f"1\tWOFFS.oframe.trans := [OFFS_X, OFFS_Y {sign} calib_cnt, OFFS_Z];\n\
                         1\tTPWrite \" OFF_Y \" + NumtoStr(OFFS_Y {sign} calib_cnt,3);"
    calib_instruction = f".oframe.trans.y := OFFS_Y {sign} calib_cnt"
    calib_offset_cmd = f"calib_offy := {sign} calib_cnt;"
    sign_cmd = f"sign_y := {sign_num};"
else:
    if points[0][5] > 0:
        sign = "+"
        sign_num = 1
    else:
        sign = "-"
        sign_num = -1

    offset_instruction = f"1\tWOFFS.oframe.trans := [OFFS_X, OFFS_Y, OFFS_Z {sign} calib_cnt];\n\
                         1\tTPWrite \" OFF_Z \" + NumtoStr(OFFS_Z {sign} calib_cnt,3);"
    calib_instruction = f".oframe.trans.z := OFFS_Z {sign} calib_cnt"
    calib_offset_cmd = f"calib_offz := {sign} calib_cnt;"
    sign_cmd = f"sign_z := {sign_num};"
RDK.setParam(f"{project_name}_Calib_OFFS", calib_instruction)


machiningprograms = milling_project.setParam("ProgEvents")
machiningprograms["CallProgStart"] = "onStart"
machiningprograms["CallProgStartOn"] = 1
machiningprograms["CallProgFinish"] = "onFinish"
machiningprograms["CallProgFinishOn"] = 1
machiningprograms["CallPathStart"] = f"1\tonPathStart;\n\
                                      1\t{sign_cmd}\n\
                                      1\tSocketSend ags_socket1 \\Str:=\"A020510\";\n\
                                      1\tWHILE TRUE DO\n\
                                      1\tWOFFS := {reference_frame.Name()};\n\
                                      {offset_instruction}"
machiningprograms["CallPathStartOn"] = 1
machiningprograms["CallAction"] = f"1\tWaitTime \\InPos, 0.5;\n\
                                   1\tprobe_detect(1);\n\
                                   1\tSocketSend ags_socket1 \\Str:=\"A09051\";\n\
                                   1\tSocketReceive ags_socket1 \\Str:=str_buf \\ReadNoOfBytes:=1;\n\
                                   1\tstr_buf := HexToDec(str_buf);\n\
                                   1\tIF (str_buf = \"1\") THEN\n\
                                   1\tGOTO nextPart;\n\
                                   1\tENDIF\n\
                                   1\tcalib_cnt := get_calib_value();\n"
machiningprograms["CallActionOn"] = 1
machiningprograms["CallPathFinish"] = f"1\tENDWHILE\n\
                                        1\tnextPart:\n\
                                        1\tonPathFinish;\n\
                                        1\t{calib_offset_cmd}\n\
                                        1\tWOFFS := {reference_frame.Name()};"
machiningprograms["CallPathFinishOn"] = 1
milling_project.setParam("ProgEvents", machiningprograms)


milling_project.setPoseFrame(reference_frame)
milling_project.setPoseTool(tool)
milling_project.setParent(RDK.Item("settings_calib", ITEM_TYPE_FOLDER))

prog_status_out = milling_project.Update(1, 3600)  # 1表示檢查碰撞
prog_status = prog_status_out[3]
print(prog_status_out)

if prog_status > 0.0:
    program = milling_project.getLink(ITEM_TYPE_PROGRAM)
    program.setName(project_name)
    program.setParent(RDK.Item("prog_calib", ITEM_TYPE_FOLDER))
    print(f"Program {project_name} created successfully!")
else:
    if points[0][normal_axis] == 1:
        points[0][normal_axis] = -1
    else:
        points[0][normal_axis] = 1
    new_curve_object = RDK.AddPoints(points)
    new_curve_object.setName(object.Name())
    object.Delete()
    new_curve_object.setColorCurve([1, 0, 0, 1])
    new_curve_object.setParent(reference_frame)
    new_curve_object.setVisible(False)
    milling_project.setMachiningParameters(part=new_curve_object)
    milling_project.Update(1, 3600)
    program = milling_project.getLink(ITEM_TYPE_PROGRAM)
    program.setName(project_name)
    program.setParent(RDK.Item("prog_calib", ITEM_TYPE_FOLDER))
    print(f"Program {project_name} created successfully!") 

# program = milling_project.getLink(ITEM_TYPE_PROGRAM)
# program.setName(project_name)
# program.setParent(RDK.Item("programs-calib", ITEM_TYPE_FOLDER))

# Provoking an exception will display the console output if you run this script from RoboDK
# raise Exception('Program Finished')