///|
pub(all) enum LengthUnit {
  Millimeter
  Inch
} derive(Eq, Debug)

///|
pub(all) enum CoordinateMode {
  AbsoluteCoordinates
  RelativeCoordinates
} derive(Eq, Debug)

///|
pub(all) enum ExtrusionMode {
  AbsoluteExtrusion
  RelativeExtrusion
} derive(Eq, Debug)

///|
pub(all) enum WorkingPlane {
  XYPlane
  XZPlane
  YZPlane
} derive(Eq, Debug)

///|
pub(all) enum SpindleMode {
  SpindleOff
  SpindleClockwise
  SpindleCounterclockwise
} derive(Eq, Debug)

///|
pub(all) enum CoolantMode {
  CoolantOff
  MistCoolant
  FloodCoolant
} derive(Eq, Debug)

///|
/// Deterministic machine-neutral modal state. Positions are normalized to mm.
pub struct MachineState {
  unit_value : LengthUnit
  coordinate_mode_value : CoordinateMode
  extrusion_mode_value : ExtrusionMode
  plane_value : WorkingPlane
  motion_value : MotionKind?
  position_value : Position
  work_offset_value : Position
  feed_value : Double?
  spindle_mode_value : SpindleMode
  spindle_speed_value : Double?
  coolant_value : CoolantMode
  tool_value : Int?
} derive(Eq, Debug)

///|
pub fn MachineState::initial() -> MachineState {
  {
    unit_value: Millimeter,
    coordinate_mode_value: AbsoluteCoordinates,
    extrusion_mode_value: AbsoluteExtrusion,
    plane_value: XYPlane,
    motion_value: None,
    position_value: Position::new(0.0, 0.0, 0.0, 0.0),
    work_offset_value: Position::new(0.0, 0.0, 0.0, 0.0),
    feed_value: None,
    spindle_mode_value: SpindleOff,
    spindle_speed_value: None,
    coolant_value: CoolantOff,
    tool_value: None,
  }
}

///|
pub fn MachineState::unit(self : MachineState) -> LengthUnit {
  self.unit_value
}

///|
pub fn MachineState::coordinate_mode(self : MachineState) -> CoordinateMode {
  self.coordinate_mode_value
}

///|
pub fn MachineState::extrusion_mode(self : MachineState) -> ExtrusionMode {
  self.extrusion_mode_value
}

///|
pub fn MachineState::plane(self : MachineState) -> WorkingPlane {
  self.plane_value
}

///|
pub fn MachineState::motion(self : MachineState) -> MotionKind? {
  self.motion_value
}

///|
pub fn MachineState::position(self : MachineState) -> Position {
  self.position_value
}

///|
pub fn MachineState::work_offset(self : MachineState) -> Position {
  self.work_offset_value
}

///|
pub fn MachineState::feed(self : MachineState) -> Double? {
  self.feed_value
}

///|
pub fn MachineState::spindle_mode(self : MachineState) -> SpindleMode {
  self.spindle_mode_value
}

///|
pub fn MachineState::spindle_speed(self : MachineState) -> Double? {
  self.spindle_speed_value
}

///|
pub fn MachineState::coolant(self : MachineState) -> CoolantMode {
  self.coolant_value
}

///|
pub fn MachineState::tool(self : MachineState) -> Int? {
  self.tool_value
}

///|
pub fn MachineState::display_position(self : MachineState) -> Position {
  Position::new(
    self.position_value.x() - self.work_offset_value.x(),
    self.position_value.y() - self.work_offset_value.y(),
    self.position_value.z() - self.work_offset_value.z(),
    self.position_value.e() - self.work_offset_value.e(),
  )
}

///|
pub fn MachineState::unit_scale(self : MachineState) -> Double {
  match self.unit_value {
    Millimeter => 1.0
    Inch => 25.4
  }
}

///|
pub struct StateSnapshot {
  line_value : Int
  before_value : MachineState
  after_value : MachineState
  labels_value : Array[String]
} derive(Eq, Debug)

///|
pub fn StateSnapshot::line(self : StateSnapshot) -> Int {
  self.line_value
}

///|
pub fn StateSnapshot::before(self : StateSnapshot) -> MachineState {
  self.before_value
}

///|
pub fn StateSnapshot::after(self : StateSnapshot) -> MachineState {
  self.after_value
}

///|
pub fn StateSnapshot::labels(self : StateSnapshot) -> Array[String] {
  self.labels_value.copy()
}

///|
pub fn StateSnapshot::changed(self : StateSnapshot) -> Bool {
  self.before_value != self.after_value
}

///|
pub struct StateTraceReport {
  snapshots_value : Array[StateSnapshot]
  diagnostics_value : Array[Diagnostic]
  final_state_value : MachineState
} derive(Eq, Debug)

///|
pub fn StateTraceReport::snapshots(
  self : StateTraceReport,
) -> Array[StateSnapshot] {
  self.snapshots_value.copy()
}

///|
pub fn StateTraceReport::diagnostics(
  self : StateTraceReport,
) -> Array[Diagnostic] {
  self.diagnostics_value.copy()
}

///|
pub fn StateTraceReport::final_state(self : StateTraceReport) -> MachineState {
  self.final_state_value
}

///|
pub fn StateTraceReport::changed_blocks(self : StateTraceReport) -> Int {
  let mut count = 0
  for snapshot in self.snapshots_value {
    if snapshot.changed() {
      count += 1
    }
  }
  count
}

///|
fn state_diagnostic(
  severity : DiagnosticSeverity,
  code : String,
  message : String,
  line : Int,
) -> Diagnostic {
  {
    severity_value: severity,
    code_value: code,
    message_value: message,
    line_value: line,
  }
}

///|
fn integer_code(value : Double) -> Int? {
  let integer = value.to_int()
  if integer.to_double() == value {
    Some(integer)
  } else {
    None
  }
}

///|
fn has_code(values : Array[Double], expected : Double) -> Bool {
  for value in values {
    if value == expected {
      return true
    }
  }
  false
}

///|
fn count_codes(values : Array[Double], candidates : Array[Double]) -> Int {
  let mut count = 0
  for candidate in candidates {
    if has_code(values, candidate) {
      count += 1
    }
  }
  count
}

///|
fn report_modal_conflict(
  diagnostics : Array[Diagnostic],
  block : GCodeBlock,
  group : String,
  values : Array[Double],
  candidates : Array[Double],
) -> Unit {
  if count_codes(values, candidates) > 1 {
    diagnostics.push(
      state_diagnostic(
        Error,
        "modal." + group + "_conflict",
        "block contains conflicting " + group + " commands",
        block.source_line(),
      ),
    )
  }
}

///|
fn apply_g_modes(
  state : MachineState,
  block : GCodeBlock,
  diagnostics : Array[Diagnostic],
  labels : Array[String],
) -> MachineState {
  let values = block.values_for(71)
  report_modal_conflict(diagnostics, block, "motion", values, [
    0.0, 1.0, 2.0, 3.0,
  ])
  report_modal_conflict(diagnostics, block, "units", values, [20.0, 21.0])
  report_modal_conflict(diagnostics, block, "coordinates", values, [90.0, 91.0])
  report_modal_conflict(diagnostics, block, "plane", values, [17.0, 18.0, 19.0])
  let mut next = state
  for value in values {
    if value == 0.0 {
      next = { ..next, motion_value: Some(Rapid) }
      labels.push("G0 rapid")
    } else if value == 1.0 {
      next = { ..next, motion_value: Some(Linear) }
      labels.push("G1 linear")
    } else if value == 2.0 {
      next = { ..next, motion_value: Some(ClockwiseArc) }
      labels.push("G2 clockwise arc")
    } else if value == 3.0 {
      next = { ..next, motion_value: Some(CounterclockwiseArc) }
      labels.push("G3 counterclockwise arc")
    } else if value == 17.0 {
      next = { ..next, plane_value: XYPlane }
      labels.push("G17 XY plane")
    } else if value == 18.0 {
      next = { ..next, plane_value: XZPlane }
      labels.push("G18 XZ plane")
    } else if value == 19.0 {
      next = { ..next, plane_value: YZPlane }
      labels.push("G19 YZ plane")
    } else if value == 20.0 {
      next = { ..next, unit_value: Inch }
      labels.push("G20 inch")
    } else if value == 21.0 {
      next = { ..next, unit_value: Millimeter }
      labels.push("G21 millimeter")
    } else if value == 90.0 {
      next = { ..next, coordinate_mode_value: AbsoluteCoordinates }
      labels.push("G90 absolute")
    } else if value == 91.0 {
      next = { ..next, coordinate_mode_value: RelativeCoordinates }
      labels.push("G91 relative")
    } else if value == 92.0 || value == 92.1 {
      ()
    } else {
      diagnostics.push(
        state_diagnostic(
          Warning,
          "command.g_unsupported",
          "state interpreter does not apply G" + value.to_string(),
          block.source_line(),
        ),
      )
    }
  }
  next
}

///|
fn apply_m_modes(
  state : MachineState,
  block : GCodeBlock,
  diagnostics : Array[Diagnostic],
  labels : Array[String],
) -> MachineState {
  let values = block.values_for(77)
  report_modal_conflict(diagnostics, block, "spindle", values, [3.0, 4.0, 5.0])
  report_modal_conflict(diagnostics, block, "coolant", values, [7.0, 8.0, 9.0])
  report_modal_conflict(diagnostics, block, "extrusion", values, [82.0, 83.0])
  let mut next = state
  for value in values {
    if value == 3.0 {
      next = { ..next, spindle_mode_value: SpindleClockwise }
      labels.push("M3 spindle clockwise")
    } else if value == 4.0 {
      next = { ..next, spindle_mode_value: SpindleCounterclockwise }
      labels.push("M4 spindle counterclockwise")
    } else if value == 5.0 {
      next = { ..next, spindle_mode_value: SpindleOff }
      labels.push("M5 spindle off")
    } else if value == 7.0 {
      next = { ..next, coolant_value: MistCoolant }
      labels.push("M7 mist coolant")
    } else if value == 8.0 {
      next = { ..next, coolant_value: FloodCoolant }
      labels.push("M8 flood coolant")
    } else if value == 9.0 {
      next = { ..next, coolant_value: CoolantOff }
      labels.push("M9 coolant off")
    } else if value == 82.0 {
      next = { ..next, extrusion_mode_value: AbsoluteExtrusion }
      labels.push("M82 absolute extrusion")
    } else if value == 83.0 {
      next = { ..next, extrusion_mode_value: RelativeExtrusion }
      labels.push("M83 relative extrusion")
    } else if value == 6.0 {
      labels.push("M6 tool change")
    } else {
      diagnostics.push(
        state_diagnostic(
          Warning,
          "command.m_unsupported",
          "state interpreter does not apply M" + value.to_string(),
          block.source_line(),
        ),
      )
    }
  }
  next
}

///|
fn modal_axis(
  current : Double,
  offset : Double,
  value : Double?,
  mode : CoordinateMode,
  scale : Double,
) -> Double {
  match value {
    None => current
    Some(input) =>
      match mode {
        AbsoluteCoordinates => input * scale + offset
        RelativeCoordinates => current + input * scale
      }
  }
}

///|
fn modal_extrusion(
  current : Double,
  offset : Double,
  value : Double?,
  mode : ExtrusionMode,
  scale : Double,
) -> Double {
  match value {
    None => current
    Some(input) =>
      match mode {
        AbsoluteExtrusion => input * scale + offset
        RelativeExtrusion => current + input * scale
      }
  }
}

///|
fn apply_position_words(
  state : MachineState,
  block : GCodeBlock,
  labels : Array[String],
) -> MachineState {
  let x = block.first_value(88)
  let y = block.first_value(89)
  let z = block.first_value(90)
  let e = block.first_value(69)
  if x is None && y is None && z is None && e is None {
    return state
  }
  let scale = state.unit_scale()
  let current = state.position()
  let offset = state.work_offset()
  let next_position = Position::new(
    modal_axis(current.x(), offset.x(), x, state.coordinate_mode(), scale),
    modal_axis(current.y(), offset.y(), y, state.coordinate_mode(), scale),
    modal_axis(current.z(), offset.z(), z, state.coordinate_mode(), scale),
    modal_extrusion(current.e(), offset.e(), e, state.extrusion_mode(), scale),
  )
  labels.push("position update")
  { ..state, position_value: next_position }
}

///|
fn apply_g92(
  state : MachineState,
  block : GCodeBlock,
  labels : Array[String],
) -> MachineState {
  let values = block.values_for(71)
  if has_code(values, 92.1) {
    labels.push("G92.1 reset offset")
    return { ..state, work_offset_value: Position::new(0.0, 0.0, 0.0, 0.0) }
  }
  if !has_code(values, 92.0) {
    return state
  }
  let current = state.position()
  let old = state.work_offset()
  let scale = state.unit_scale()
  let x = match block.first_value(88) {
    Some(value) => current.x() - value * scale
    None => old.x()
  }
  let y = match block.first_value(89) {
    Some(value) => current.y() - value * scale
    None => old.y()
  }
  let z = match block.first_value(90) {
    Some(value) => current.z() - value * scale
    None => old.z()
  }
  let e = match block.first_value(69) {
    Some(value) => current.e() - value * scale
    None => old.e()
  }
  labels.push("G92 set offset")
  { ..state, work_offset_value: Position::new(x, y, z, e) }
}

///|
fn apply_scalar_words(
  state : MachineState,
  block : GCodeBlock,
  diagnostics : Array[Diagnostic],
  labels : Array[String],
) -> MachineState {
  let mut next = state
  match block.first_value(70) {
    Some(value) => {
      let feed = value * state.unit_scale()
      if feed < 0.0 {
        diagnostics.push(
          state_diagnostic(
            Error,
            "feed.negative",
            "feed must not be negative",
            block.source_line(),
          ),
        )
      }
      next = { ..next, feed_value: Some(feed) }
      labels.push("feed update")
    }
    None => ()
  }
  match block.first_value(83) {
    Some(value) => {
      if value < 0.0 {
        diagnostics.push(
          state_diagnostic(
            Error,
            "spindle.negative",
            "spindle speed must not be negative",
            block.source_line(),
          ),
        )
      }
      next = { ..next, spindle_speed_value: Some(value) }
      labels.push("spindle speed update")
    }
    None => ()
  }
  match block.first_value(84) {
    Some(value) =>
      match integer_code(value) {
        Some(tool) if tool >= 0 => {
          next = { ..next, tool_value: Some(tool) }
          labels.push("tool selection")
        }
        _ =>
          diagnostics.push(
            state_diagnostic(
              Error,
              "tool.invalid",
              "tool number must be a non-negative integer",
              block.source_line(),
            ),
          )
      }
    None => ()
  }
  next
}

///|
/// Interpret common machine-neutral modal state without controlling hardware.
pub fn interpret_modal_state(
  document : GCodeDocument,
  initial? : MachineState = MachineState::initial(),
) -> StateTraceReport {
  let snapshots : Array[StateSnapshot] = []
  let diagnostics : Array[Diagnostic] = []
  let mut state = initial
  for block in document.blocks() {
    let before = state
    let labels : Array[String] = []
    for letter in block.duplicate_letters() {
      // G and M may occur more than once when they belong to different modal groups.
      if letter != 71 && letter != 77 {
        diagnostics.push(
          state_diagnostic(
            Warning,
            "word.duplicate",
            "letter " +
            letter.unsafe_to_char().to_string() +
            " appears more than once",
            block.source_line(),
          ),
        )
      }
    }
    if !block.checksum_valid() {
      diagnostics.push(
        state_diagnostic(
          Error,
          "checksum.invalid",
          "block checksum does not match source",
          block.source_line(),
        ),
      )
    }
    state = apply_g_modes(state, block, diagnostics, labels)
    state = apply_m_modes(state, block, diagnostics, labels)
    state = apply_scalar_words(state, block, diagnostics, labels)
    if has_code(block.values_for(71), 92.0) ||
      has_code(block.values_for(71), 92.1) {
      state = apply_g92(state, block, labels)
    } else {
      state = apply_position_words(state, block, labels)
    }
    snapshots.push({
      line_value: block.source_line(),
      before_value: before,
      after_value: state,
      labels_value: labels,
    })
  }
  {
    snapshots_value: snapshots,
    diagnostics_value: diagnostics,
    final_state_value: state,
  }
}

///|
fn unit_name(unit : LengthUnit) -> String {
  match unit {
    Millimeter => "mm"
    Inch => "inch"
  }
}

///|
fn plane_name(plane : WorkingPlane) -> String {
  match plane {
    XYPlane => "XY"
    XZPlane => "XZ"
    YZPlane => "YZ"
  }
}

///|
pub fn render_state_trace(report : StateTraceReport) -> String {
  let mut output = "snapshots=" + report.snapshots().length().to_string()
  output = output + ", changed=" + report.changed_blocks().to_string()
  output = output +
    ", diagnostics=" +
    report.diagnostics().length().to_string() +
    "\n"
  for snapshot in report.snapshots() {
    if snapshot.changed() {
      let state = snapshot.after()
      let position = state.display_position()
      output = output + "line " + snapshot.line().to_string()
      output = output + " unit=" + unit_name(state.unit())
      output = output + " plane=" + plane_name(state.plane())
      output = output +
        " position=" +
        position.x().to_string() +
        "," +
        position.y().to_string() +
        "," +
        position.z().to_string()
      output = output +
        " events=" +
        snapshot.labels().length().to_string() +
        "\n"
    }
  }
  output
}