# Implementation of the module {gcode_read}.
# Last edited on 2021-03-12 21:11:22 by jstolfi

import gcode_read
import path
import move
import move_parms
import rn
import re
import sys
from math import sqrt, sin, cos, floor, ceil, pi, inf, nan

class Printer_State_IMP:
  # An object that holds the deduced state of the printer.
  # All coordinates are metric (mm, s, C) even when the
  # G-code uses imperial units or per-minute feed rates.

  # ??? Add these parameters: ???
  #   print_material
  #   filament_retract_distance
  #   filament_retract_speed
  #   jump_nozzle_lift
  #   nozzle_diameter

  def __init__(self):
    # Printer parameters specified eternally:
    self.fdiam = None  # Filament diameter, mm.
    self.ac = None     # Acceleration, mm/s^2.
    self.ud = None     # Trace/jump transition penalty, s.
    self.zstep = None  # Layer thickness, mm.
    # Printer state (set from G-code):
    self.unit = None   # Unit of G-code in mm (1 for metric, 25.4 for imperial).
    self.pabs = None   # Coordinates in G-code are absolute (true) or relative (false).
    self.eabs = None   # Filament position in G-code is absolute (true) or relative (false).
    self.xpos = None   # {X} coordinate, mm.
    self.ypos = None   # {Y} coordinate, mm.
    self.zpos = None   # {Z} coordinate, mm.
    self.epos = None   # Filament position, mm.
    self.eret = None   # Filament retraction distance, mm.
    self.sp = None     # Feedrate (cruise speed), mm/s.
    self.fan = None    # Integer: cooling fan speed, 0..255.
    self.ntemp = None  # Nozzle temperature (C).
    self.btemp = None  # Bed temperature (C).
    # Internal work fields:
    self.mps = []      # List of {Move_Parms} objects already created.
    self.mps_n = []    # List of number of moves that used each elem of {self.mps} was used.
    self.mps_d = []    # List with total length of moves that used each elem of {self.mps}.
    self.fname = None  # File name, for warnings and errors.
    self.lnum = None   # Line number, for warnings and errors.

def make_state(fdiam, zstep, ac, ud, fname):
  state = gcode_read.Printer_State()
  state.fdiam = fdiam  # Filament diameter.
  state.ac = ac        # Acceleration (should be given somehow).
  state.ud = ud        # Trace/jump transition penalty.
  state.zstep = zstep  # Layer thickness.
  state.fname = fname  # File name for messages.
  return state
  # ----------------------------------------------------------------------

def set_state(state, unit, pabs, eabs, xyzpos, epos, eret, sp, fan,ntemp,btemp, lnum):
  state.unit = unit       # Unit of G-code in mm (1 for metric, 25.4 for imperial).
  state.pabs = pabs       # Coordinates in G-code are absolute (true) or relative (false).
  state.eabs = eabs       # Filament position in G-code is absolute (true) or relative (false).
  state.xpos = xyzpos[0]  # {X} coordinate, mm.
  state.ypos = xyzpos[1]  # {Y} coordinate, mm.
  state.zpos = xyzpos[2]  # {Z} coordinate, mm.
  state.epos = epos       # Filament position, mm.
  state.eret = eret       # Filament retraction distance, mm.
  state.sp = sp           # Feedrate (cruise speed), mm/s.
  state.fan = fan         # Cooling fan speed, 0..255.
  state.ntemp = ntemp     # Nozzle temperature (C).
  state.btemp = btemp     # Bed temperature (C).
  state.lnum = lnum       # Number of lines read so far.
  return
  # ----------------------------------------------------------------------

def slice(rd, state):
  moves = [] # List of moves.
  assert state.zpos != None
  for line in rd:
    state.lnum += 1
    line = line.strip()
    clean = re.sub(r' *[;].*$', '', line) # Remove comment fields
    clean = re.sub(r'^ +', '', clean) # Remove leading blanks
    if clean == '':
      # Line was just comments; print it:
      print_warning(state, "comment = '%s'" % line)
    elif re.match(r'[(].*[)]$', clean):
      # Title line:
      title = re.sub(r'^[(]', '', re.sub('[)]$', '', clean))
      print_warning(state, "title = '%s'" % title)
    else:
      # Split into operator and operands:
      ops = re.split(r'([A-Z]+[ ]*[-+]?[.0-9]+)', clean)
      ops = [ x for x in ops if x.strip() != '' ]
      sys.stderr.write("ops = %s\n" % str(ops))
      if len(ops) != 0:
        ok = process_ops(state,ops,moves)
        if not ok:
          print_warning(state, "unimplemented opcode: %s" % str(ops))
  
  ph = path.from_moves(moves)
  return ph
  # ----------------------------------------------------------------------

def show_state(wr, state):
  wr.write("----------------------------------------------------------------------\n")
  wr.write("Printer parameters (specified eternally):\n")
  wr.write("%-40s = %8.3f %s\n" % ("Filament diameter", state.fdiam, "mm"))
  wr.write("%-40s = %8.3f %s\n" % ("Acceleration", state.ac, "mm/s^2"))
  wr.write("%-40s = %8.3f %s\n" % ("Trace/jump transition penalty", state.ud, "s"))
  wr.write("%-40s = %8.3f %s\n" % ("Layer thickness", state.zstep, "mm"))
  wr.write("Printer state (set from G-code):\n")
  wr.write("%-40s = %8.3f %s\n" % ("Unit of G-code coords", state.unit, "mm"))
  wr.write("%-40s = %8s\n"      % ("G-code coords are absolute", str(state.pabs)))
  wr.write("%-40s = %8s\n"      % ("G-code filament pos is absolute", str(state.eabs)))
  wr.write("%-40s = %8.3f %s\n" % ("X coordinate", state.xpos, "mm"))
  wr.write("%-40s = %8.3f %s\n" % ("Y coordinate", state.ypos, "mm"))
  wr.write("%-40s = %8.3f %s\n" % ("Z coordinate", state.zpos, "mm"))
  wr.write("%-40s = %8.3f %s\n" % ("Filament position", state.epos, "mm"))
  wr.write("%-40s = %8.3f %s\n" % ("Filament retraction amount", state.eret, "mm"))
  wr.write("%-40s = %8.3f %s\n" % ("Feedrate (cruise speed)", state.sp, "mm/s"))
  wr.write("%-40s = %8d/255\n"   % ("Cooling fan speed", state.fan))
  wr.write("%-40s = %8.1f %s\n"  % ("Nozzle temperature", state.ntemp, "C"))
  wr.write("%-40s = %8.1f %s\n"  % ("Bed temperature", state.btemp, "C"))
  wr.write("Internal work fields\n")
  wr.write("%-40s = %s\n"       % ("File name", state.fname))
  wr.write("%-40s = %8d\n"      % ("Line number", state.lnum))
  nmp = len(state.mps)
  if nmp > 0:
    wr.write("Move parameters records\n")
    for k in range(nmp):
      wr.write("  n: %8d d:%10.3f  " % (state.mps_n[k], state.mps_d[k]))
      move_parms.show(wr, state.mps[k])
  wr.write("----------------------------------------------------------------------\n")
  return 
  # ----------------------------------------------------------------------

def process_ops(state,ops,moves):
  ok = True
  op = ops[0];
  if op == 'G0':
    # Rapid move:
    do_move(state, ops[1:], moves)
  elif op == 'G1':
    # Regular move:
    do_move(state, ops[1:], moves)
  elif op == 'G21':
    # Metric units:
    assert len(ops) == 1
    state.unit = 1
  elif op == 'G90':
    # Use absolute position coordinates:
    assert len(ops) == 1
    state.pabs = True
  elif op == 'G91':
    # Use relative position coordinates:
    assert len(ops) == 1
    state.pabs = False
  elif op == 'G92':
    # Set the absolute coordinates of current positon:
    set_position(state, ops[1:])
  elif op == "M140" or op == 'M190':
    # Set for bed temperature:
    assert len(ops) == 2
    set_temperature(state, ops[1], None, True)
  elif op == 'M104' or op == 'M109':
    # Set extruder temperature:
    assert len(ops) == 2 or len(ops) == 3
    arg1 = ops[1]
    arg2 = ops[2] if len(ops) == 3 else None
    set_temperature(state, arg1, arg2, False)
  elif op == 'M106':
    # Set cooling fan speed:
    assert len(ops) == 2
    set_fan_speed(state, ops[1])
  elif op == 'M107':
    # Cooling fan off:
    assert len(ops) == 1
    state.fan = 0
  elif op == 'M82':
    # Extrusion coordinates are absolute:
    assert len(ops) == 1
    state.eabs = True
  elif op == 'G28':
    # Go to home position:
    if len(ops) == 1:
      # Home all three axes:
      state.xpos = 0
      state.ypos = 0
      state.zpos = 0
    else:
      # Home selected axes:
      for axis in ops[1:]:
        if axis == 'X':
          state.xpos = 0
        elif axis == 'Y':
          state.ypos = 0
        elif axis == 'Z':
          state.zpos = 0
        else:
          print_error(state, "invalid axis '%s'" % axis)
  else:
    # Unrecognized opcode:
    ok = False
  return ok
  # ----------------------------------------------------------------------

def do_move(state, ops, moves):
  # Simulates a 'G0' (fast move) or 'G1' (accurate move) command.
  # If in the same layer, appends it to {moves}.
  # if in another layer, appends the moves to 
  x_ant = state.xpos
  y_ant = state.ypos
  z_ant = state.zpos
  e_ant = state.epos
  sp_ant = state.sp
  for arg in ops:
    axis, val = parse_arg(state, arg)
    if axis == 'X':
      state.xpos = convert_coord(val, state.unit, state.xpos, state.pabs)
    elif axis == 'Y':
      state.ypos = convert_coord(val, state.unit, state.ypos, state.pabs)
    elif axis == 'Z':
      state.zpos = convert_coord(val, state.unit, state.zpos, state.pabs)
    elif axis == 'E':
      state.epos = convert_coord(val, state.unit, state.epos, state.eabs)
    elif axis == 'F':
      state.sp = convert_coord(val/60, state.unit, state.sp, True)
    else:
      print_error(state, "invalid coordinate code '%s'" % axis)
  if state.zpos != z_ant and z_ant != None and z_ant != 0:
    print_error(state, "a Z motion inside a layer '%s'" % axis)
  dxy = (state.xpos - x_ant, state.ypos - y_ant)
  xydist = rn.norm(dxy)
  edist = state.epos - e_ant  # Amount filament moved.
  # Account for previous filament retraction
  if edist > state.eret:
    # Some material was extruded:
    edist = edist - state.eret # Amount actually extruded.
    state.eret = 0             # Filament now at the nozzle.
  else:
    # Filament still retracted:
    state.eret = state.eret - edist;
    edist = 0
  mp = get_move_parms(state, xydist, edist, state.sp)
  if mp != None:
    mv = move.make((x_ant, y_ant), (state.xpos, state.ypos), mp)
    moves.append(mv)
  return
  # ----------------------------------------------------------------------
  
def set_position(state, ops):
  # Simulates a 'G92' (set position) command.
  for arg in ops:
    axis, val = parse_arg(state, arg)
    if axis == 'X':
      state.xpos = convert_coord(val, state.unit, 0, True)
    elif axis == 'Y':
      state.ypos = convert_coord(val, state.unit, 0, True)
    elif axis == 'Z':
      state.zpos = convert_coord(val, state.unit, 0, True)
    elif axis == 'E':
      state.epos = convert_coord(val, state.unit, 0, True)
    else:
      print_error(state, "invalid coordinate code '%s'" % axis)
  return
  # ----------------------------------------------------------------------
  
def get_move_parms(state, xydist, edist, sp):
  # Obtains a {Move_Parms} record with the given attributes from {state.mps},
  # creating one if needed.   Assumes that {edist} is the amount of filament
  # that was actually extruded (after discounting previous retractions):
  
  # Compute the volume {evol} of material that was extruded:
  evol = edist*pi*(state.fdiam**2)/4

  # Compute the nominal trace width:
  if xydist != 0:
    # Assume that a sausage was deposited, minus the starting button.
    wd = evol/(xydist*state.zstep) 
  else:
    if evol > 0:
      # Assume that a roundish button was deposited.
      wd = sqrt((4/pi)*(evol/state.zstep)) 
      print_warning(state, "filament extruded by %.3f mm (%.3f mm^3) while still" % (edist,evol))
    else:
      # Bogus move with no motion and no extrusion:
      wd = 0
      print_warning(state, "zero length, zero net extrusion jump")
  
  # Round {wd} to a reasonable quantum:
  wd_unit = 0.02
  wd = wd_unit*floor(wd/wd_unit + 0.5)

  # Locate or create the parameter record {mp}:
  if xydist == 0 and wd == 0:
    # Null move:
    mp = None
  else:
    # Move walked some distance and/or extruded some material:
    ac = state.ac
    ud = state.ud if wd == 0 else 0
    sys.stderr.write("xydist: %.3f edist: %.3f" % (xydist,edist))
    sys.stderr.write(" wd: %.3f ac: %.3f sp: %.3f ud: %.3f" % (wd,ac,sp,ud))

    # Looks for a  previously created {Move_Parms} record with "same" values:
    mp = None
    for k in range(len(state.mps)):
      mpk = state.mps[k]
      wdk = move_parms.width(mpk)
      ack, spk, udk = move_parms.dynamics(mpk)
      # sys.stderr.write("old move parms: wdk: %.3f ack: %.3f spk: %.3f udk: .3f\n" % (wdk,ack,spk,udk))
      assert ack == state.ac
      if (abs(wdk - wd) < 0.5e-3) and (abs(spk - sp) < 0.5e-3) and (abs(udk - ud) < 0.5e-3):
        mp = mpk; 
        state.mps_n[k] += 1
        state.mps_d[k] += xydist
        break
    if mp == None:
      # No similar {Move_Parms} record; create a new one:
      sys.stderr.write(" -- NEW")
      mp = move_parms.make(wd, state.ac, sp, ud)
      state.mps.append(mp)
      state.mps_n.append(1)
      state.mps_d.append(xydist)
    sys.stderr.write("\n")
  return mp
  # ----------------------------------------------------------------------

def set_temperature(state, arg1, arg2, bed):
  # Sets {state.ntemp} (if {bed} is false) or {state.btemp} 
  # (if {bed} is true) from operand {arg1}.   If {arg2} is not {None},
  # it must be "T0".
  axis, val = parse_arg(state, arg1)
  if axis == 'S' or axis == 'R':
    if bed:
      state.btemp = val
    else:
      state.ntemp == val
  else:
    print_error(state, "bad temperature argument '%s'" % axis)
  if arg2 != None and arg2 != "T0":
    print_error(state, "invalid second temperature arg '%s'" % arg2)
  return
  # ----------------------------------------------------------------------
  
def set_fan_speed(state, arg):
  # Sets {state.fan} from operand {arg}.
  axis, val = parse_arg(state, arg)
  if axis == 'S':
    state.fan == val
  else:
    print_error(state, "bad fan_speed argument '%s'" % axis)
  return
  # ----------------------------------------------------------------------
  
def parse_arg(state, arg):
  # Receives a parameter that is "{axis}{val}" where {axis} is a capital letter
  # and {val} is a decimal number, possibly fractional and/or signed.
  # Splits them and returns two results: (1) the letter {axis} and (2) 
  # the value {val} converted to float.
  m = re.match(r'^([A-Z]) *([-+]?[.0-9]+)$', arg)
  if m:
    axis = m.group(1)
    val = m.group(2)
    if re.match(r'^[-+]?[0-9]*([0-9]|[.][0-9]+)$', val):
      val = float(val)
    else: 
      print_error(state, "bad number format '%s'" % val)
  else:
    print_error(state, "bad argument format '%s'" % arg)
  return axis,val
  # ----------------------------------------------------------------------

def convert_coord(val, unit, val_cur, absolute):
  # Converts the float value {val} as read from G-code to an absolute
  # coordinate in mm.  
  if unit == None:
    print_error("coordinate with undefined unit system")
  val = val * unit
  if absolute == None:
    print_error("coordinate with undefined absolute/relative option")
  if not absolute: 
    if val_cur == None:
      print_error("coordinate relative to undefined position")
    val += val_cur
  return val
  # ----------------------------------------------------------------------

def print_warning(state,msg):
  sys.stderr.write("%s:%d: %s\n" % (state.fname, state.lnum, msg))
  return 
  # ----------------------------------------------------------------------

def print_error(state,msg):
  print_warning(state,msg)
  assert False
  # ----------------------------------------------------------------------