mc_lookahead = 0.03
mc_gain = 2000
mc_dt = 0.004

mc_conv_tol_s = 0.001
mc_conv_loops = 100

mc_ee_set_id = 2

MC_GROUP_STANDBY = 0
MC_GROUP_STOPPING = 2
MC_GROUP_ERROR_STOP = 3

MC_ERROR_PATHDIVERGED = 0
MC_ERROR_COLLISION = 1
MC_ERROR_JOINTLIMIT = 2
MC_ERROR_SINGULARITY = 4
MC_ERROR_PLANNINGFAILED = 16
MC_ERROR_DAEMONSTOPPED = 1024
MC_ERROR_SERVER_BUSY = 1025
MC_ERROR_NOT_INITIALIZED = 2000

MC_TOOLPATH_ERROR_NONE = 0
MC_TOOLPATH_ERROR_FILELOAD = 1
MC_TOOLPATH_ERROR_PARSE = 2
MC_TOOLPATH_ERROR_EMPTYTOOLPATH = 3
MC_TOOLPATH_ERROR_UNSUPPORTEDTYPE = 4

MC_GROUP_STATUS_INPUT = 24
MC_CURRENT_MOTION_ID_INPUT = 25
MC_ERROR_CODE_INPUT = 26
MC_DIGITAL_OUTPUTS_INPUT = 27
MC_JOINT_VALUE_INPUT = 24
MC_WATCHDOG_INPUT = 30
MC_PATH_SPEED_INPUT = 31
MC_ANALOG_OUTPUT_INPUTS = [32,33]
MC_PATH_SPEED_INPUT = 31

MC_SPEEDFACTOR_OUTPUT = 24

mc_last_watchdog = 0
mc_watchdog_counter = 0
mc_watchdog_counter_limit = 100

mc_last_motion_id = -1
mc_group_status = MC_GROUP_STOPPING
mc_server_started = False
mc_rtcp_moving = False
mc_debug_msg = False
mc_gmm_initialized = False
mc_last_digital_outputs = 0
mc_last_analog_outputs = [-1.0, -1.0]

def mc_initialize(mode, tcp, doc=6):
	mc_check_busy()
	
	if (mode == 0 and doc == 6):
		mc_ee_set_id = 0
	end
	
	if (mode == 1 and doc == 6):
		mc_ee_set_id = 2
	end
	
	if (mode == 0 and doc == 5):
		mc_ee_set_id = 1
	end
	
	if (mode == 1 and doc == 5):
		mc_ee_set_id = 3
	end
	
	mc_initialize_internal(tcp)
end

def mc_initialize_internal(tcp):
	if (mc_server_started == False):
		global mc_server = rpc_factory("xmlrpc", "127.0.0.1:7828")
		mc_server_started = True
	end

	mc_server.setDataStoreBoolean("IS_ON", "/robot/check_divergence", mc_check_divergence)
	mc_server.reset()
	mc_group_status = MC_GROUP_STANDBY
	mc_set_speed_factor(1.0)
	mc_server.setKinTransform(mc_ee_set_id, 0, tcp)
	mc_gmm_initialized = False
end

def mc_check_busy():
	enter_critical
	if (mc_rtcp_moving == False):
		mc_rtcp_moving = True
	else:
		mc_error_stop("", "", MC_ERROR_SERVER_BUSY)
	end
	exit_critical
end

def mc_add_linear(pose, a, v, r):
	mc_check_ready()
	ee_id = 0
	buffer_mode = "BLENDING_NEXT"
	transition_mode = "CORNER_DISTANCE"
	id = mc_server.moveLinearAbsolute(pose, mc_ee_set_id, ee_id, v, a, buffer_mode, transition_mode, r)

	if (id == -1):
		mc_error_stop("Compute node returned ID ", id)
	end
	
	mc_last_motion_id = id

	return id
end

def mc_add_circular(pose_via, pose_to, a, v, r, mode = 0):
	mc_check_ready()
	ee_id = 0
	buffer_mode = "BLENDING_NEXT"
	transition_mode = "CORNER_DISTANCE"
	id = mc_server.moveCircularBorderAbsolute(pose_via, pose_to, mode, mc_ee_set_id, ee_id, v, a, buffer_mode, transition_mode, r)
	
	if (id == -1):
		mc_error_stop("Compute node returned ID ", id)
	end
	
	mc_last_motion_id = id

	return id
end

def mc_add_path(path_id, a, v, r):
	mc_check_ready()
	ee_id = 0
	buffer_mode = "BLENDING_NEXT"
	transition_mode = "CORNER_DISTANCE"
	id = mc_server.movePath(path_id, mc_ee_set_id, ee_id, v, a, buffer_mode, transition_mode, r)
	
	if (id == -1):
		result = mc_server.getErrorCode()
		mc_toolpath_error_stop(result[0], result[1])
	end
	
	mc_last_motion_id = id

	return id
end

def mc_load_path(nc_file, use_feedrate = False):
	id = mc_server.loadPath(nc_file, use_feedrate)

	if (id < 0):
		result = mc_server.getErrorCode()
		mc_toolpath_error_stop(result[0], result[1])
	end

	return id
end

def mc_get_target_rtcp_speed():
	return read_input_float_register(MC_PATH_SPEED_INPUT)
end

def mc_check_ready():
	if mc_group_status != MC_GROUP_STANDBY:
		mc_error_stop("", "", MC_ERROR_NOT_INITIALIZED)
	end
end

def mc_set_pcs(pcs):
	ee_id = 0
	mc_server.setCoordinateTransform(mc_ee_set_id, ee_id, pcs, "ABORTING")
end

def mc_set_digital_outputs():
	current_do = read_input_integer_register(MC_DIGITAL_OUTPUTS_INPUT)
	current_do_bin = integer_to_binary_list(current_do)
	last_do_bin = integer_to_binary_list(mc_last_digital_outputs)
	loopcounter = 0
	while (loopcounter < 4):
		value_bit = loopcounter * 6
		port_bit_0 = value_bit + 1
		port_bit_1 = value_bit + 2
		port_bit_2 = value_bit + 3
		port_bit_3 = value_bit + 4
		port_bit_4 = value_bit + 5
		last_port = binary_list_to_integer([last_do_bin[port_bit_0], last_do_bin[port_bit_1], last_do_bin[port_bit_2], last_do_bin[port_bit_3], last_do_bin[port_bit_4]])
		current_port = binary_list_to_integer([current_do_bin[port_bit_0], current_do_bin[port_bit_1], current_do_bin[port_bit_2], current_do_bin[port_bit_3], current_do_bin[port_bit_4]])
		if (current_port != 0 and (current_port != last_port or current_do_bin[value_bit] != last_do_bin[value_bit])):
			if (current_port <= 8):
				set_standard_digital_out(current_port - 1, current_do_bin[value_bit])
			elif (current_port <= 16):
				set_configurable_digital_out(current_port - 9, current_do_bin[value_bit])
			else:
				set_tool_digital_out(current_port - 17, current_do_bin[value_bit])
			end
		end
		loopcounter = loopcounter + 1
	end
	mc_last_digital_outputs = current_do
end

def mc_set_analog_outputs():
	loopcounter = 0
	while (loopcounter < 2):
		current_ao = read_input_float_register(MC_ANALOG_OUTPUT_INPUTS[loopcounter])
		if((current_ao != mc_last_analog_outputs[loopcounter]) and (current_ao >= 0)): 
			set_standard_analog_out(loopcounter,current_ao)
			mc_last_analog_outputs[loopcounter] = current_ao
		end
      loopcounter = loopcounter + 1
	end
end

def mc_run_motion(id=-1):
	if (id == -1):
		id = mc_last_motion_id
	end
	
	mc_debugMsg("Waiting for ID ", id)

	if (mc_gmm_initialized == False):
		mc_initialize_gmm()
	end
	
	mc_current_motion_id = read_input_integer_register(MC_CURRENT_MOTION_ID_INPUT)
	mc_group_status = read_input_integer_register(MC_GROUP_STATUS_INPUT) 
	joint_targets = get_actual_joint_positions()
	while (mc_current_motion_id <= id and mc_current_motion_id != -2 and mc_group_status != MC_GROUP_STOPPING and mc_group_status != MC_GROUP_ERROR_STOP):
		mc_current_motion_id = read_input_integer_register(MC_CURRENT_MOTION_ID_INPUT)
		mc_group_status = read_input_integer_register(MC_GROUP_STATUS_INPUT)
		
		joint_targets = mc_get_joint_targets(joint_targets)
		servoj(joint_targets, t = mc_dt, lookahead_time = mc_lookahead, gain = mc_gain)
		
		mc_set_digital_outputs()

		mc_set_analog_outputs()

		mc_check_watchdog()
	end
	
	if (mc_group_status == MC_GROUP_STOPPING or mc_group_status == MC_GROUP_ERROR_STOP):
		stopj(45)

		mc_debugMsg("Stopping due to error after ID ", id)
		
		mc_server.setDataStoreBoolean("IS_RTCP_RUNNING", "/group_motion_manager", False)
		
		error_code = read_input_integer_register(MC_ERROR_CODE_INPUT)
		mc_error_stop("Compute node returned group status ", mc_group_status, error_code)

		mc_rtcp_moving = False
		mc_group_status = MC_GROUP_STOPPING
	end

	if (id == mc_last_motion_id):
		mc_debugMsg("Stopping after ID ", id)
		mc_check_convergence(joint_targets)
		
		mc_server.setDataStoreBoolean("IS_RTCP_RUNNING", "/group_motion_manager", False)
		
		mc_rtcp_moving = False
		mc_group_status = MC_GROUP_STOPPING
	end
	
	mc_debugMsg("Completed ID ", id)
end

def mc_set_speed_factor(s):
	write_output_float_register(MC_SPEEDFACTOR_OUTPUT, s)
end

def mc_initialize_gmm():
	mc_current_motion_id = read_input_integer_register(MC_CURRENT_MOTION_ID_INPUT)
	while (mc_current_motion_id != -1):
		mc_current_motion_id = read_input_integer_register(MC_CURRENT_MOTION_ID_INPUT)
		sleep(mc_dt)
		mc_check_watchdog()
	end
	
	mc_server.setDataStoreBoolean("IS_RTCP_RUNNING", "/group_motion_manager", True)

	while (mc_current_motion_id == -1):
		mc_current_motion_id = read_input_integer_register(MC_CURRENT_MOTION_ID_INPUT)
		sleep(mc_dt)
		mc_check_watchdog()
	end

	mc_gmm_initialized = True
end

def mc_check_convergence(joint_targets):
	loopcounter = 0
	converged = False
	while (converged == False and loopcounter < mc_conv_loops):
	
		joint_targets = mc_get_joint_targets(joint_targets)
		ur_targets = get_target_joint_positions()
		joint_speeds = get_actual_joint_speeds()

		position_converged = joint_targets[0] == ur_targets[0] and joint_targets[1] == ur_targets[1] and joint_targets[2] == ur_targets[2] and joint_targets[3] == ur_targets[3] and joint_targets[4] == ur_targets[4] and joint_targets[5] == ur_targets[5]
		speed_converged = joint_speeds[0] <= mc_conv_tol_s and joint_speeds[1] <= mc_conv_tol_s and joint_speeds[2] <= mc_conv_tol_s and joint_speeds[3] <= mc_conv_tol_s and joint_speeds[4] <= mc_conv_tol_s and joint_speeds[5] <= mc_conv_tol_s
		converged = position_converged and speed_converged

		servoj(joint_targets, t = mc_dt, lookahead_time = mc_lookahead, gain = mc_gain)

		loopcounter = loopcounter + 1
	end

	mc_debugMsg("Convergence loop count: ", loopcounter)
end

def mc_check_watchdog():
	current_watchdog = read_input_float_register(MC_WATCHDOG_INPUT)
	
	if (current_watchdog > mc_last_watchdog):
		mc_last_watchdog = current_watchdog
		mc_watchdog_counter = 0
	else:
		mc_watchdog_counter = mc_watchdog_counter + 1
		if (mc_watchdog_counter > mc_watchdog_counter_limit):
			mc_error_stop("", "", MC_ERROR_DAEMONSTOPPED)
		end
	end
end

def mc_get_joint_targets(joint_targets):
	joint_targets[0] = read_input_float_register(MC_JOINT_VALUE_INPUT)
	joint_targets[1] = read_input_float_register(MC_JOINT_VALUE_INPUT + 1)
	joint_targets[2] = read_input_float_register(MC_JOINT_VALUE_INPUT + 2)
	joint_targets[3] = read_input_float_register(MC_JOINT_VALUE_INPUT + 3)
	joint_targets[4] = read_input_float_register(MC_JOINT_VALUE_INPUT + 4)
	joint_targets[5] = read_input_float_register(MC_JOINT_VALUE_INPUT + 5)
	return joint_targets
end

def mc_error_stop(msg, param="", error_code=MC_ERROR_PLANNINGFAILED):
	if (msg != ""):
		textmsg(msg, param)
	end
	
	textmsg("Error code: ", error_code)
	error_msg = mc_getErrorMessage(error_code)
	textmsg(error_msg)
	
	popup(error_msg, title="Remote TCP", error=True, blocking=False)
	halt
end

def mc_toolpath_error_stop(error_code, param):
	msg = mc_getToolpathMessage(error_code)
	textmsg(msg + " - Line: ", param)
	
	popup(msg, title="Toolpath Validation", error=True, blocking=True)
	halt
end

def mc_debugMsg(msg, param=""):
	if (mc_debug_msg == True):
		textmsg(msg, param)
	end
end
