///|
pub struct CoordinateTransform {
  offset_x_value : Double
  offset_y_value : Double
  offset_z_value : Double
  scale_value : Double
} derive(Eq, Debug)

///|
pub fn CoordinateTransform::new(
  offset_x : Double,
  offset_y : Double,
  offset_z : Double,
  scale : Double,
) -> Result[CoordinateTransform, GCodeError] {
  if scale == 0.0 {
    return Err({
      code_value: InvalidNumber,
      message_value: "scale cannot be zero",
      location_value: SourceLocation::new(0, 0),
    })
  }
  Ok({
    offset_x_value: offset_x,
    offset_y_value: offset_y,
    offset_z_value: offset_z,
    scale_value: scale,
  })
}

///|
fn apply_position(
  transform : CoordinateTransform,
  position : Position,
) -> Position {
  Position::new(
    position.x() * transform.scale_value + transform.offset_x_value,
    position.y() * transform.scale_value + transform.offset_y_value,
    position.z() * transform.scale_value + transform.offset_z_value,
    position.e(),
  )
}

///|
pub fn transform_program(
  program : GCodeProgram,
  transform : CoordinateTransform,
) -> GCodeProgram {
  let motions : Array[Motion] = []
  for motion in program.motions() {
    motions.push({
      ..motion,
      start_value: apply_position(transform, motion.start()),
      end_value: apply_position(transform, motion.end()),
      i_value: motion.i() * transform.scale_value,
      j_value: motion.j() * transform.scale_value,
    })
  }
  { motions_value: motions, ignored_lines_value: program.ignored_lines() }
}

///|
pub fn select_motions(
  program : GCodeProgram,
  predicate : (Motion) -> Bool,
) -> GCodeProgram {
  let motions : Array[Motion] = []
  for motion in program.motions() {
    if predicate(motion) {
      motions.push(motion)
    }
  }
  { motions_value: motions, ignored_lines_value: program.ignored_lines() }
}

///|
pub fn program_equal(left : GCodeProgram, right : GCodeProgram) -> Bool {
  if left.motion_count() != right.motion_count() {
    return false
  }
  let right_motions = right.motions()
  for index, motion in left.motions() {
    let other = right_motions[index]
    if motion.kind() != other.kind() ||
      motion.start() != other.start() ||
      motion.end() != other.end() ||
      motion.i() != other.i() ||
      motion.j() != other.j() ||
      motion.feed() != other.feed() {
      return false
    }
  }
  true
}