from robot import *
from convert_tools import *
import time
import sys
import math
ip = "192.168.0.160"
ec = {}
robot = XMateErProRobot(ip)
nDOFs_MIN = 6
DRIVER_VERSION = "RoboDK Driver for Rokae SR3 v0.1"
ROBOTCOM_UNKNOWN = -1000
ROBOTCOM_CONNECTION_PROBLEMS = -3
ROBOTCOM_DISCONNECTED = -2
ROBOTCOM_NOT_CONNECTED = -1
ROBOTCOM_READY = 0
ROBOTCOM_WORKING = 1
ROBOTCOM_WAITING = 2
STATUS = ROBOTCOM_DISCONNECTED
def print_message(message):
    print("SMS:" + message)
    sys.stdout.flush()
    
def print_debug(message):
    print("DBG:" + message)
    sys.stdout.flush()
    
def UpdateStatus(set_status=None):
    global STATUS
    if set_status is not None:
        STATUS = set_status
    if STATUS == ROBOTCOM_CONNECTION_PROBLEMS:
        print_message("Connection problems")
    elif STATUS == ROBOTCOM_DISCONNECTED:
        print_message("Disconnected")
    elif STATUS == ROBOTCOM_NOT_CONNECTED:
        print_message("Not connected")
    elif STATUS == ROBOTCOM_READY:
        print_message("Ready")
    elif STATUS == ROBOTCOM_WORKING:
        print_message("Working...")
    elif STATUS == ROBOTCOM_WAITING:
        print_message("Waiting...")
    else:
        print_message("Unknown status")
def ConnectRobot():
    global robot, ec
    try:
        robot.connectToRobot(ec)
        robot.setOperateMode(rokae.OperateMode.automatic, ec)
        robot.setPowerState(True, ec)
        UpdateStatus(ROBOTCOM_READY)
        print_debug("robot connect completed")
    except Exception as e:
        UpdateStatus(ROBOTCOM_CONNECTION_PROBLEMS)
        print_debug(f"connection Error: {e}")
def DisconnectRobot():
    global robot, ec
    try:
        robot.stop(ec)
        robot.setPowerState(False, ec)
        robot.setOperateMode(rokae.OperateMode.manual, ec)
        robot.disconnectFromRobot(ec)
        UpdateStatus(ROBOTCOM_DISCONNECTED)
        print_debug("robot disconnect")
    except Exception as e:
        print_debug(f"disconnect error: {e}")
def waitRobot(robot):
    global ec
    running = True
    while running:
        time.sleep(0.1)
        st = robot.operationState(ec)
        if st == rokae.OperationState.idle.value or st == rokae.OperationState.unknown.value:
            running = False
def RunDriver():
    try:
        for line in sys.stdin:
            RunCommand(line)
    except KeyboardInterrupt:
        print_debug("Driver stop...")
    finally:
        DisconnectRobot()
def RunCommand(linecmd):
    global robot, ec
    def line_2_values(words):
        values = []
        for word in words:
            try:
                values.append(float(word))
            except ValueError:
                pass
        return values
    def execute_movement(joints):
        UpdateStatus(ROBOTCOM_WORKING)
        try:
            ra_jnts = [math.radians(j) for j in joints]
            point_move = MoveAbsJCommand(ra_jnts, 20, 10)
            robot.moveReset(ec)
            robot.executeCommand([point_move], ec)
            robot.moveStart(ec)
            UpdateStatus(ROBOTCOM_READY)
            waitRobot(robot)
        except Exception as e:
            UpdateStatus(ROBOTCOM_CONNECTION_PROBLEMS)
            print_debug(f"Move error: {e}")
    words = linecmd.strip().split(' ')
    values = line_2_values(words)
    if not words:
        return
    command = words[0]
    if command == "CONNECT":
        ConnectRobot()
    elif command == "DISCONNECT":
        DisconnectRobot()
    elif command == "MOVJ" and len(values) >= nDOFs_MIN:
        execute_movement(values[:nDOFs_MIN])
    elif command == "MOVabsJ" and len(values) >= nDOFs_MIN:
        execute_movement(values[:nDOFs_MIN])
    elif command == "CJNT":
        jnts = robot.jointPos(ec)
        gr_jnts = [round(math.degrees(j), 2) for j in jnts]
        print_joints(gr_jnts)
    else:
        print_debug(f"Bad command: {linecmd}")
    UpdateStatus(ROBOTCOM_READY)
def print_joints(joints):
    print_debug("JNTS " + " ".join(format(x, ".2f") for x in joints))
    sys.stdout.flush()
if __name__ == "__main__":
    import atexit
    atexit.register(DisconnectRobot)
    print_message(DRIVER_VERSION)
    RunDriver()