import os
from pathlib import Path
import sys
import time
import queue
import random
import threading
from xarm import version
from xarm.wrapper import XArmAPI
from HardwarePositions import HardwarePositions
from PLCInterface import PLCInterface
from RobotController import RobotController

if __name__ == '__main__':
    # build path of the calibration file
    '''
    calibration_file_path = os.path.join(Path(__file__).parent, "config.txt")
    print(calibration_file_path)
    calibration_file = open(calibration_file_path, "r+")
    calibration_file.seek(0, 0)
    print(calibration_file.read())
    calibration_file.write("hello ")
    calibration_file.close()
    '''
    '''
    while not inspect_box.iterator_end():
        print(str(inspect_box.iterator) + ': ' + inspect_box.get_next_position().str())
    '''

    RobotController.pprint('xArm-Python-SDK Version:{}'.format(version.__version__))

    hardware_positions = HardwarePositions()
    robot_arm = XArmAPI('192.168.1.206', baud_checkset=False)
    robot_controller = RobotController(robot_arm, hardware_positions)
    plc_interface = PLCInterface(robot_controller)
    robot_controller.init(plc_interface)
    
    robot_controller.run()
