import math

# Config
MOTOR_1_UNITS_PER_ROTATION = 6.0
MOTOR_2_UNITS_PER_ROTATION = 6.0
MOTOR_1_HOMING_OFFSET = -1.5
MOTOR_2_HOMING_OFFSET = 1.5

INPUT_NAME = 'sandify.thr'
OUTPUT_NAME = 'transformed.gcode'

# Open the files.
with open(INPUT_NAME, 'r') as infile:
  with open(OUTPUT_NAME, 'w') as outfile:

    for line in infile:

      # First, separate by content and comments
      parts = line.strip().split('#')

      if len(parts[0].strip()) == 0:
        # This line doesn't have anything that isn't a comment
        outfile.write(line.replace('#',';'))
        continue

      else:
        (theta, rho) = parts[0].split()

        # Here, we're actually doing the math.
        m1 = float(theta) + math.acos(float(rho))
        m2 = float(theta) - math.acos(float(rho))

        x = (MOTOR_1_UNITS_PER_ROTATION * m1 / (2*math.pi))+MOTOR_1_HOMING_OFFSET
        y = (MOTOR_2_UNITS_PER_ROTATION * m2 / (2*math.pi))+MOTOR_2_HOMING_OFFSET

        outfile.write("G1 X{:0.03f} Y{:0.03f}\n".format(x, y))          

    # We just moved each motor a bunch. A whole lot.
    # When we start the next file, we don't want it to unroll
    # So we will make the current coordinate something that is small, but the same.
    xmod = x % MOTOR_1_UNITS_PER_ROTATION
    ymod = y % MOTOR_2_UNITS_PER_ROTATION

    outfile.write("G92 X{:0.03f} Y{:0.03f}\n".format(xmod, ymod))