defmodule FarmbotFirmware.GCODE.Encoder do
  @moduledoc false

  alias FarmbotFirmware.{GCODE, Param}

  @doc false
  @spec do_encode(GCODE.kind(), GCODE.args()) :: binary()
  def do_encode(:report_idle, []), do: "R00"
  def do_encode(:report_begin, []), do: "R01"
  def do_encode(:report_success, []), do: "R02"
  def do_encode(:report_error, []), do: "R03"
  def do_encode(:report_error, error), do: "R03 " <> encode_error(error)
  def do_encode(:report_busy, []), do: "R04"

  def do_encode(:report_axis_state, xyz), do: "R05 " <> encode_axis_state(xyz)

  def do_encode(:report_calibration_state, xyz),
    do: "R06 " <> encode_calibration_state(xyz)

  def do_encode(:report_retry, []), do: "R07"
  def do_encode(:report_echo, [echo]), do: "R08 * #{echo} *"
  def do_encode(:report_invalid, []), do: "R09"

  def do_encode(:report_home_complete, [:x]), do: "R11"
  def do_encode(:report_home_complete, [:y]), do: "R12"
  def do_encode(:report_home_complete, [:z]), do: "R13"

  def do_encode(:report_position_change, [x: _] = arg),
    do: "R15 " <> encode_floats(arg)

  def do_encode(:report_position_change, [y: _] = arg),
    do: "R16 " <> encode_floats(arg)

  def do_encode(:report_position_change, [z: _] = arg),
    do: "R16 " <> encode_floats(arg)

  def do_encode(:report_parameters_complete, []), do: "R20"

  def do_encode(:report_parameter_value, pv), do: "R21 " <> encode_pv(pv)

  def do_encode(:report_calibration_parameter_value, pv),
    do: "R23 " <> encode_pv(pv)

  def do_encode(:report_pin_value, pv), do: "R41 " <> encode_ints(pv)

  def do_encode(:report_axis_timeout, [:x]), do: "R71"
  def do_encode(:report_axis_timeout, [:y]), do: "R72"
  def do_encode(:report_axis_timeout, [:z]), do: "R73"

  def do_encode(:report_end_stops, xxyyzz),
    do: "R81 " <> encode_end_stops(xxyyzz)

  def do_encode(:report_position, xyzs), do: "R82 " <> encode_floats(xyzs)

  def do_encode(:report_software_version, [version]), do: "R83 " <> version

  def do_encode(:report_encoders_scaled, xyz), do: "R84 " <> encode_floats(xyz)
  def do_encode(:report_encoders_raw, xyz), do: "R85 " <> encode_floats(xyz)

  def do_encode(:report_emergency_lock, []), do: "R87"
  def do_encode(:report_no_config, []), do: "R88"
  def do_encode(:report_load, uxvywz), do: "R89 " <> encode_uxvywz(uxvywz)
  def do_encode(:report_debug_message, [message]), do: "R99 " <> message

  def do_encode(:command_movement, xyzs), do: "G00 " <> encode_floats(xyzs)
  def do_encode(:command_movement_home, [:x, :y, :z]), do: "G28"

  def do_encode(:command_movement_home, [:x]),
    do: "G00 " <> encode_floats(x: 0.0)

  def do_encode(:command_movement_home, [:y]),
    do: "G00 " <> encode_floats(y: 0.0)

  def do_encode(:command_movement_home, [:z]),
    do: "G00 " <> encode_floats(z: 0.0)

  def do_encode(:command_movement_find_home, [:x]), do: "F11"
  def do_encode(:command_movement_find_home, [:y]), do: "F12"
  def do_encode(:command_movement_find_home, [:z]), do: "F13"

  def do_encode(:command_movement_calibrate, [:x]), do: "F14"
  def do_encode(:command_movement_calibrate, [:y]), do: "F15"
  def do_encode(:command_movement_calibrate, [:z]), do: "F16"

  def do_encode(:parameter_read_all, []), do: "F20"

  def do_encode(:parameter_read, [parameter]),
    do: "F21 P#{Param.encode(parameter)}"

  def do_encode(:parameter_write, pv), do: "F22 " <> encode_pv(pv)
  def do_encode(:calibration_parameter_write, pv), do: "F23 " <> encode_pv(pv)

  def do_encode(:pin_write, pv), do: "F41 " <> encode_ints(pv)
  def do_encode(:pin_read, p), do: "F42 " <> encode_ints(p)
  def do_encode(:pin_mode_write, pm), do: "F43 " <> encode_ints(pm)

  def do_encode(:servo_write, pv), do: "F61 " <> encode_ints(pv)
  def do_encode(:end_stops_read, []), do: "F81"
  def do_encode(:position_read, []), do: "F82"
  def do_encode(:software_version_read, []), do: "F83"
  def do_encode(:position_write_zero, [:x, :y, :z]), do: "F84 X1 Y1 Z1"
  def do_encode(:position_write_zero, [:x]), do: "F84 X1"
  def do_encode(:position_write_zero, [:y]), do: "F84 Y1"
  def do_encode(:position_write_zero, [:z]), do: "F84 Z1"

  def do_encode(:command_emergency_unlock, _), do: "F09"
  def do_encode(:command_emergency_lock, _), do: "E"
  def do_encode(), do: "R03"

  @spec encode_floats([{Param.t(), float()}]) :: binary()
  defp encode_floats(args) do
    Enum.map(args, fn {param, value} ->
      binary_float = :erlang.float_to_binary(value, decimals: 2)
      String.upcase(to_string(param)) <> binary_float
    end)
    |> Enum.join(" ")
  end

  defp encode_axis_state([{axis, :idle}]),
    do: String.upcase(to_string(axis)) <> "0"

  defp encode_axis_state([{axis, :begin}]),
    do: String.upcase(to_string(axis)) <> "1"

  defp encode_axis_state([{axis, :accelerate}]),
    do: String.upcase(to_string(axis)) <> "2"

  defp encode_axis_state([{axis, :cruise}]),
    do: String.upcase(to_string(axis)) <> "3"

  defp encode_axis_state([{axis, :decelerate}]),
    do: String.upcase(to_string(axis)) <> "4"

  defp encode_axis_state([{axis, :stop}]),
    do: String.upcase(to_string(axis)) <> "5"

  defp encode_axis_state([{axis, :crawl}]),
    do: String.upcase(to_string(axis)) <> "6"

  defp encode_calibration_state([{axis, :idle}]),
    do: String.upcase(to_string(axis)) <> "0"

  defp encode_calibration_state([{axis, :home}]),
    do: String.upcase(to_string(axis)) <> "1"

  defp encode_calibration_state([{axis, :end}]),
    do: String.upcase(to_string(axis)) <> "2"

  defp encode_end_stops(xa: xa, xb: xb, ya: ya, yb: yb, za: za, zb: zb) do
    "XA#{xa} XB#{xb} YA#{ya} YB#{yb} ZA#{za} ZB#{zb}"
  end

  defp encode_pv([{param, value}]) do
    param_id = Param.encode(param)
    binary_float = :erlang.float_to_binary(value, decimals: 2)
    "P#{param_id} V#{binary_float}"
  end

  def encode_uxvywz([u_value, x_value, v_value, y_value, w_value, z_value]) do
    u_int = to_string(u_value)
    x_int = to_string(x_value)
    v_int = to_string(v_value)
    y_int = to_string(y_value)
    w_int = to_string(w_value)
    z_int = to_string(z_value)
    "U#{u_int} X#{x_int} V#{v_int} Y#{y_int} W#{w_int} Z#{z_int}"
  end

  defp encode_error(error) do
    case error do
      :no_error -> "V0"
      :emergency_lock -> "V1"
      :timeout -> "V2"
      :stall_detected -> "V3"
      :calibration_error -> "V4"
      :invalid_command -> "V14"
      :no_config -> "V15"
      :stall_detected_x -> "V31"
      :stall_detected_y -> "V32"
      :stall_detected_z -> "V33"
      _ -> ""
    end
  end

  defp encode_ints(args) do
    Enum.map(args, fn {key, val} ->
      String.upcase(to_string(key)) <> to_string(val)
    end)
    |> Enum.join(" ")
  end
end