import argparse
import time
from gobotrpc import GobotRPC_PackageParseError
from gobotrpc.rpc_packages import GobotRPCVacumRequest
from gobotrpc.rpc_packages_head import GobotRPCDropStoneRequest, GobotRPCMoveZAxisRequest, GobotHeadZAxisPosition
from gobotrpc.rpc_packages_corexy import (
    GobotRPCHomeRequest,
    GobotRPCHomeResponse,
    GobotRPCReleaseMotorsRequest,
    GobotRPCReleaseMotorsResponse,
    GobotRPCSetPaddingRequest,
    GobotRPCSetPaddingResponse,
    GobotRPCGotoRequest,
    GobotRPCGotoResponse,
)

from uart_interface.serial import GobotRPC_CI_Hardware_Serial
from uart_interface.ci_highLevel import GobotRPC_CI
from uart_interface.ci_packages import CMDS, CI_ParserError

import threading

# Args parser
parser = argparse.ArgumentParser(description='GobotRPC')
parser.add_argument('port', type=str, help='Serial port', default="/dev/ttyACM1")


def main():
    args = parser.parse_args()

    gobotrpc_ci_hardware = GobotRPC_CI_Hardware_Serial(args.port, 115200)
    gobotrpc_ci = GobotRPC_CI(gobotrpc_ci_hardware)

    def rxThreadFn():
        while True:
            try:
                ci_package = gobotrpc_ci.rx_ci_package()
            except GobotRPC_PackageParseError as e:
                print(e)
                continue
            except CI_ParserError as e:
                print(e)
                continue
            
            if ci_package.get_cmd() != CMDS.HEARTBEAT:
                print(ci_package)

    rxThread = threading.Thread(target=rxThreadFn)
    rxThread.daemon = True
    rxThread.start()

    for i in range(4):
        gobotrpc_ci.setAddrPortMap(0x23, i)


    gobotrpc_ci.resetAdapter()
    time.sleep(0.5)
    gobotrpc_ci.node_get_info(0x23)

    input("SetPadding>")    
    gobotrpc_ci.send_gobotrpc_package(GobotRPCSetPaddingRequest(
        (1000, 2000), (5000, 6000), (200, 300), 3, 3
    ), 0x23)

    input("SetPadding>")    
    gobotrpc_ci.send_gobotrpc_package(GobotRPCHomeRequest(), 0x23)

    while True:

        x = int(input("X>") or "0")
        y = int(input("Y>") or "0") 
        gobotrpc_ci.send_gobotrpc_package(
            GobotRPCGotoRequest(x, y, False), 0x23
        )



if __name__ == "__main__":
    main()