###############################################################################
# Init data
###############################################################################
# ROBOT_NAME = "NRMK-Indy7" # Get using grpc
# JOINT_DOF = 6             # Get using grpc
TIMEOUT = 5  # seconds
SW_VERSION = "2.4.5"
STEP_INFO = 2  # step version 2

SOURCE_OF_FRAME = 18  # Server => Client 0x12
MAX_DATA_LENGTH = 200

# ROBOT_NAME_byte = ROBOT_NAME.encode("utf-8")  # bytes(ROBOT_NAME, encoding='ascii')
SW_VERSION_byte = SW_VERSION.encode("utf-8")  # bytes(SW_VERSION, encoding='ascii')
STEP_INFO_byte = STEP_INFO.to_bytes(1, 'little')
SOURCE_OF_FRAME_byte = SOURCE_OF_FRAME.to_bytes(1, 'little')

###############################################################################
# DATA FRAME
###############################################################################
HEADER_SIZE = 52
CMD_SIZE = 4
HEADER_CMD_SIZE = HEADER_SIZE + CMD_SIZE

HEADER_ROBOT_NAME = [0, 20]  # 20 bytes
HEADER_SW_VERSION = [20, 32]  # 12 bytes
HEADER_STEP_INFO = [32, 33]  # 1 bytes
HEADER_SOURCE_OF_FRAME = [33, 34]  # 1 bytes
HEADER_INVOKE_ID = [34, 38]  # 4 bytes
HEADER_DATA_LENGTH = [38, 42]  # 4 bytes
HEADER_RESERVED = [42, 52]  # 10 bytes
HEADER_CMD = [52, 56]  # 4 bytes

###############################################################################
# CMD
###############################################################################
DIRECT_VARIABLE_ADDRESS_LIMIT = 999

CMD_CHECK = 0
CMD_EMERGENCY_STOP = 1
CMD_RESET_ROBOT = 2
CMD_SET_SERVO = 3
CMD_SET_BRAKE = 4
CMD_STOP = 5
CMD_MOVE = 6
CMD_MOVE_HOME = 7
CMD_MOVE_ZERO = 8
CMD_JOINT_MOVE_TO = 9
CMD_JOINT_MOVE_BY = 10
CMD_TASK_MOVE_TO = 11
CMD_TASK_MOVE_BY = 12
CMD_TASK_MOVE_VELOCITY = 13     # not use

CMD_START_CURRENT_PROGRAM = 14
CMD_PAUSE_CURRENT_PROGRAM = 15
CMD_RESUME_CURRENT_PROGRAM = 16
CMD_STOP_CURRENT_PROGRAM = 17
CMD_START_DEFAULT_PROGRAM = 18
CMD_REGISTER_DEFAULT_PROGRAM_IDX = 19
CMD_GET_REGISTERED_DEFAULT_PROGRAM_IDX = 20

CMD_IS_ROBOT_RUNNING = 30
CMD_IS_READY = 31
CMD_IS_EMG = 32
CMD_IS_COLLIDED = 33
CMD_IS_ERR = 34
CMD_IS_BUSY = 35
CMD_IS_MOVE_FINISHED = 36
CMD_IS_HOME = 37
CMD_IS_ZERO = 38
CMD_IS_IN_RESETTING = 39
CMD_IS_DIRECT_TEACHING = 60
CMD_IS_TEACHING = 61
CMD_IS_PROGRAM_RUNNING = 62
CMD_IS_PROGRAM_PAUSED = 63
CMD_IS_CONTY_CONNECTED = 64

CMD_CHANGE_DIRECT_TEACHING = 80
CMD_FINISH_DIRECT_TEACHING = 81

CMD_JOINT_PUSH_BACK_WAYPOINT_SET = 90
CMD_JOINT_POP_BACK_WAYPOINT_SET = 91
CMD_JOINT_CLEAR_WAYPOINT_SET = 92
CMD_JOINT_EXECUTE_WAYPOINT_SET = 94
CMD_TASK_PUSH_BACK_WAYPOINT_SET = 95
CMD_TASK_POP_BACK_WAYPOINT_SET = 96
CMD_TASK_CLEAR_WAYPOINT_SET = 97
CMD_TASK_EXECUTE_WAYPOINT_SET = 99

CMD_SET_DEFAULT_TCP = 100
CMD_RESET_DEFAULT_TCP = 101
CMD_SET_COMP_TCP = 102
CMD_RESET_COMP_TCP = 103
CMD_SET_REFFRAME = 104
CMD_RESET_REFFRAME = 105
CMD_SET_COLLISION_LEVEL = 106
CMD_SET_JOINT_BOUNDARY = 107
CMD_SET_TASK_BOUNDARY = 108
CMD_SET_JOINT_ACCELERATION = 109
CMD_SET_TASK_ACCELERATION = 110
CMD_SET_JOINT_WTIME = 111
CMD_SET_TASK_WTIME = 112
CMD_SET_TASK_CMODE = 113
CMD_SET_JOINT_BLEND_RADIUS = 116
CMD_SET_TASK_BLEND_RADIUS = 117

CMD_SET_REDUCED_MODE = 130  # not released
CMD_SET_REDUCED_SPEED_RATIO = 131  # not released
CMD_GET_REDUCED_MODE = 230  # not released
CMD_GET_REDUCED_SPEED_RATIO = 231  # not released

CMD_GET_DEFAULT_TCP = 200
CMD_GET_COMP_TCP = 201
CMD_GET_REFFRAME = 202
CMD_GET_COLLISION_LEVEL = 203
CMD_GET_JOINT_BOUNDARY = 204
CMD_GET_TASK_BOUNDARY = 205
CMD_GET_JOINT_ACCELERATION = 206
CMD_GET_TASK_ACCELERATION = 207
CMD_GET_JOINT_WTIME = 208
CMD_GET_TASK_WTIME = 209
CMD_GET_TASK_CMODE = 210
CMD_GET_JOINT_BLEND_RADIUS = 213
CMD_GET_TASK_BLEND_RADIUS = 214

CMD_GET_RUNNING_TIME = 300
CMD_GET_CMODE = 301
CMD_GET_JOINT_STATE = 302
CMD_GET_JOINT_POSITION = 320
CMD_GET_JOINT_VELOCITY = 321
CMD_GET_TASK_POSITION = 322
CMD_GET_TASK_VELOCITY = 323
CMD_GET_TORQUE = 324
CMD_GET_INV_KIN = 325

CMD_GET_LAST_EMG_INFO = 380

CMD_GET_SMART_DI = 400
CMD_GET_SMART_DIS = 401
CMD_SET_SMART_DO = 402
CMD_SET_SMART_DOS = 403
CMD_GET_SMART_AI = 404
CMD_SET_SMART_AO = 405
CMD_GET_SMART_DO = 406
CMD_GET_SMART_DOS = 407
CMD_GET_SMART_AO = 408
CMD_SET_ENDTOOL_DO = 409  # v2.3.1 added
CMD_GET_ENDTOOL_DO = 410  # v2.3.1 added

CMD_GET_EXTIO_FTCAN_ROBOT_RAW = 420
CMD_GET_EXTIO_FTCAN_ROBOT_TRANS = 421
CMD_GET_EXTIO_FTCAN_CB_RAW = 422
CMD_GET_EXTIO_FTCAN_CB_TRANS = 423

CMD_READ_DIRECT_VARIABLE = 460
CMD_READ_DIRECT_VARIABLES = 461
CMD_WRITE_DIRECT_VARIABLE = 462
CMD_WRITE_DIRECT_VARIABLES = 463

CMD_SET_SYNC_MODE = 700

CMD_FOR_EXTENDED = 800
CMD_FOR_STREAMING = 801

CMD_SEND_KEYCOMMAND = 9996
CMD_READ_MEMORY = 9997
CMD_WRITE_MEMORY = 9998
CMD_ERROR = 9999

#########################################################################
# Extended DCP command                                                  #
#########################################################################
EXT_CMD_MOVE_TRAJ_BY_DATA = 1
EXT_CMD_MOVE_TRAJ_BY_TXT_DATA = 2
EXT_CMD_MOVE_TRAJ_BY_FILE = 3
EXT_CMD_MOVE_TRAJ_BY_TXT_FILE = 4

EXT_CMD_JMOVE_ABS_WAYPOINT_SET = 11
EXT_CMD_TMOVE_ABS_WAYPOINT_SET = 12

EXT_CMD_SET_JSON_PROG = 21
EXT_CMD_SET_JSON_PROG_START = 22

#########################################################################
# Error code                                                            #
#########################################################################
ERR_NONE = 0
ERR_NO_MATCHED_ROBOT = 1
ERR_NO_MATCHED_STEP = 2
ERR_HEADER_FORMAT = 4
ERR_OVER_DATA_SIZE = 5
ERR_NOT_SUPPORT_COMMAND = 6
ERR_UNKNOWN_COMMAND = 7
ERR_UNKNOWN_DATA = 8
ERR_PROCESS_FAILED = 9
ERR_PARSE_FAILED = 10
ERR_NO_MATCHED_PARAMETER = 11
ERR_NO_MATCHED_DATA_SIZE = 12
ERR_WRONG_ASCII_FORMAT = 13
ERR_ROBOT_MOVING_STATE = 14
ERR_ROBOT_PROGRAM_RUNNING = 15
ERR_ROBOT_MOVE_FAILED = 16
ERR_NO_DEFAULT_PROGRAM = 17
ERR_NO_CURRENT_PROGRAM = 18
ERR_CURRENT_PROGRAM_STATE = 19
ERR_EMG_STATE = 20
ERR_ROBOT_STATE = 21
ERR_ROBOT_PROGRAM_LOAD_FAILED = 22
ERR_DIRECT_VARIABLE_INVALID_ADDRESS = 23
ERR_DIRECT_VARIABLE_INVALID_FORMAT = 24
ERR_DIRECT_VARIABLE_REFNUM_LIMIT = 25  # The number of access for direct variable exceeded 20
ERR_CONNECTION_EXCEPTION = 600
ERR_CONNECTION_TIMEOUT = 601

#########################################################################
# Header Status Bit                                                     #
#########################################################################
HEADER_STATUS_BIT_TASK_RUNNING = 0x80000000  # 0b 1000 0000 0000 0000 0000 0000 0000 0000
HEADER_STATUS_BIT_ROBOT_READY = 0x40000000  # 0b 0100 0000 0000 0000 0000 0000 0000 0000
HEADER_STATUS_BIT_EMG_STOPPED = 0x20000000  # 0b 0010 0000 0000 0000 0000 0000 0000 0000
HEADER_STATUS_BIT_COLLIDED = 0x10000000  # 0b 0001 0000 0000 0000 0000 0000 0000 0000
HEADER_STATUS_BIT_ERR_STATE = 0x08000000  # 0b 0000 1000 0000 0000 0000 0000 0000 0000
HEADER_STATUS_BIT_BUSY = 0x04000000  # 0b 0000 0100 0000 0000 0000 0000 0000 0000
HEADER_STATUS_BIT_MOVE_FINISHED = 0x02000000  # 0b 0000 0010 0000 0000 0000 0000 0000 0000
HEADER_STATUS_BIT_HOME = 0x01000000  # 0b 0000 0001 0000 0000 0000 0000 0000 0000
HEADER_STATUS_BIT_ZERO = 0x00800000  # 0b 0000 0000 1000 0000 0000 0000 0000 0000
HEADER_STATUS_BIT_IN_RESETTING = 0x00400000  # 0b 0000 0000 0100 0000 0000 0000 0000 0000

HEADER_STATUS_BIT_DIRECT_TEACHING = 0x00000080  # 0b 0000 0000 0000 0000 0000 0000 1000 0000
HEADER_STATUS_BIT_TEACHING = 0x00000040  # 0b 0000 0000 0000 0000 0000 0000 0100 0000
HEADER_STATUS_BIT_PROGRAM_RUNNING = 0x00000020  # 0b 0000 0000 0000 0000 0000 0000 0010 0000
HEADER_STATUS_BIT_PROGRAM_PAUSED = 0x00000010  # 0b 0000 0000 0000 0000 0000 0000 0001 0000
HEADER_STATUS_BIT_CONTY_CONNECTED = 0x00000008  # 0b 0000 0000 0000 0000 0000 0000 0000 1000

#########################################################################
# DirectVariableType                                                    #
#########################################################################
DIRECT_VAR_TYPE_ERROR = -1
DIRECT_VAR_TYPE_BYTE = 0
DIRECT_VAR_TYPE_WORD = 1
DIRECT_VAR_TYPE_DWORD = 2
DIRECT_VAR_TYPE_LWORD = 3
DIRECT_VAR_TYPE_FLOAT = 4
DIRECT_VAR_TYPE_DFLOAT = 5
DIRECT_VAR_TYPE_MODBUS_REG = 10

NUMBER_OF_ACCESS_DIRECT_VARIABLE = 20
