import sys
sys.path.append("/home/tim/python_me_tools")
from pgw import pgw

CLEAR_Z = .1

FEEDRATE = 7.5
SPINDLE_SPEED = 5000

TOOL_CHANGE_POS = [None, None, 3]

FEATURE_DEPTH = .77
PASS_DEPTH = .02

# convenient indices into coordinate tuple.
X = 0
Y = 1
Z = 2


# Coordinates for right side of bearing stand.
# All coordinates relative to axle bearing center.
parabola_step_coords = [
  (1.5938, -1.6875, None),
  (1.5938, -1.4063, None),
  (1.4473, -1.4063, None),
  (1.4473, -1.2188, None),
  (1.3204, -1.2188, None),
  (1.3204, -1.0313, None),
  (1.2129, -1.0313, None),
  (1.2129, -0.8438, None),
  (1.1250, -0.8438, None),
  (1.1250, -0.6563, None),
  (1.0567, -0.6563, None),
  (1.0567, -0.4688, None),
  (1.0079, -0.4688, None),
  (1.0079, -0.2813, None),
  (0.9786, -0.2813, None),
  (0.9786, -0.0938, None),
  (0.9688, -0.0938, None)]

LOWER_CLEAR_Y = -1.8125   # Y posiiton to clear work.

# Assume at call that tool is at CLEAR_Z.
# Cut contour at given depth
def contour(gc, depth):
    # Contour at current cutting depth,
    coord  = parabola_step_coords[0]
    gc.rapid_move([coord[X], LOWER_CLEAR_Y, None], c="Right lower clear position.")
    gc.rapid_move([None, None, -depth], c="New depth %f." % depth)
    for coord in parabola_step_coords:
        gc.move(coord)
    # Arc to opposite point centered around bearing center.
    gc.arc_cw([-coord[X], coord[Y], None], [0, 0, None], c="Arc around bearing center.")
    # Left side uses right side coordinates in reverse order
    # and with sign of X negative.
    for idx in range(len(parabola_step_coords)-2,-1,-1):
        coord = parabola_step_coords[idx]
        gc.move([-coord[X], coord[Y], None])
    gc.rapid_move([-coord[X], LOWER_CLEAR_Y, None], c="Left lower clear position.")
    gc.rapid_move([None, None, CLEAR_Z], c="Clear z position.")

# Tool touched off at axle bearing center and top of stock.
gc = pgw.GCodeWrapper()
gc.rapid_move(TOOL_CHANGE_POS)
gc.change_tool(5)
gc.set_length_compensation()
gc.feedrate(FEEDRATE)
gc.spindle(SPINDLE_SPEED)
gc.dwell(4)
cut_depth = PASS_DEPTH
while True:
    contour(gc, cut_depth)
    if cut_depth == FEATURE_DEPTH:
        break
    cut_depth += PASS_DEPTH
    if cut_depth > FEATURE_DEPTH:
        cut_depth = FEATURE_DEPTH

gc.spindle(0)
gc.rapid_move(TOOL_CHANGE_POS)
gc.stop()