#!/usr/bin/python

import sys, os
BASE = os.path.abspath(os.path.join(os.path.dirname(sys.argv[0]), ".."))
sys.path.insert(0, os.path.join(BASE, "lib", "python"))

import hal, emc, time

feed_increment = .01
feed_max = 1.2
sleep_interval = .01
if len(sys.argv) > 1:
    feed_increment = float(sys.argv[1])
if len(sys.argv) > 2:
    feed_max = float(sys.argv[2])
if len(sys.argv) > 3:
    sleep_interval = float(sys.argv[3])

try:
    emc.nmlfile = os.environ['NMLFILE']
except KeyError:
    pass

s = emc.stat()
c = emc.command()

h = hal.component("hal_feedoverride_wheel")
h.newpin("count", hal.HAL_S32, hal.HAL_RD)
h.newpin("enable", hal.HAL_BIT, hal.HAL_RD)
h.ready()

s.poll()
last_count = 0
last_enable = False
blackout = time.time()
feedrate = None

while 1:
    time.sleep(sleep_interval)

    enable = h.enable
    count = h.count
    if enable and not last_enable:
        last_enable = enable
        last_count = count
        continue

    last_enable = enable

    if not h.enable:
        continue
    change = count - last_count
    last_count = count

    if not change: continue

    now = time.time()
    if feedrate is None or now >= blackout:
        s.poll()
        feedrate = s.feedrate
    feedrate += feed_increment * change
    if feedrate > feed_max: feedrate = feed_max
    if feedrate < 0: feedrate = 0
    c.feedrate(feedrate)
    blackout = now + 1


# vim:sw=4:sts=4:et