#!/usr/bin/python
import sys
import hal
import time
import linuxcnc
s = linuxcnc.stat()
c = linuxcnc.command()




SO = hal.component("SO")
SO.newpin("so-stat", hal.HAL_BIT, hal.HAL_IN)
SO.newpin("so-sen", hal.HAL_BIT, hal.HAL_IN)
SO.newpin("so-stat-out", hal.HAL_BIT, hal.HAL_OUT)
SO.ready()

#!!("so-sen") IS HALL SENSOR FOR SPINDLE ORIENT!!

#initialize variables
try:
    while 1:
        def ok_for_atc():
            s.poll()
            return not s.estop and s.homed and (SO['so-stat'] is True)

        if not ok_for_atc():
            continue

        if ok_for_atc():
            c.mdi("g53 g0 z0")
            c.mdi("m5")
            c.mdi("m3 s50")
            if(SO['so-sen'] is False):
                c.mdi("m5")
                c.wait_complete()
                #!!spindle orient lock will go here!!
                SO['so-stat-out'] = 1

except KeyboardInterrupt:
    raise SystemExit