import sys
import select
from config import *
from motion import Motion_Move
from TMC2130 import TMC2130_Home
import machine

poll_obj = ''

def Comms_Init():
    global poll_obj
    
    poll_obj = select.poll()
    poll_obj.register(sys.stdin, select.POLLIN)

def Comms_Hello():
    hello_str = 'Stepper motor driver\r\n'
    hello_str += 'Software: v0.3\r\n'
    hello_str += 'Hardware: v1.0\r\n'
    hello_str += 'Author: Angoosh\r\n'
    sys.stdout.write(hello_str)

def Comms_Reset():
    machine.reset()

def Comms_GetInfo(info):
    if info == 0:
        sys.stdout.write(AXIS_ID+'\r\n')

def Comms_Read():
    global poll_obj
    
    poll_results = poll_obj.poll(1)
    
    if poll_results:
        rw = sys.stdin.readline().strip()
        rw = rw.upper()
        
        if 'G0' in rw:
            if AXIS_ID in rw:
                data = rw.split(' ')
                for i in data:
                    if AXIS_ID in i:
                        motorics = i.split(':')
                        Motion_Move(int(motorics[1]), int(motorics[2]))
                        sys.stdout.write('OK\r\n')
        elif 'G1' in rw:
            if AXIS_ID in rw:
                data = rw.split(' ')
                for i in data:
                    if AXIS_ID in i:
                        motorics = i.split(':')
                        Motion_Move(int(motorics[1]), int(motorics[2]))
                        sys.stdout.write('OK\r\n')
        elif 'G28' in rw:
            TMC2130_Home()
        elif 'RESET' in rw:
            sys.stdout.write('OK\r\n')
            Comms_Reset()
        elif 'INFO' in rw:
            data = rw.split(' ')
            Comms_GetInfo(int(data[1]))
        elif rw == '':
            pass
        else:
            sys.stdout.write('INVALID 1\r\n')