farmbot_os/farmbot_core/lib/firmware/uart_handler/uart_handler.ex

defmodule Farmbot.Firmware.UartHandler do
  @moduledoc """
  Handles communication between farmbot and uart devices
  """

  use GenStage
  alias Nerves.UART
  require Farmbot.Logger
  import Farmbot.Config, only: [update_config_value: 4, get_config_value: 3]
  alias Farmbot.Firmware
  alias Firmware.Utils
  import Utils

  @behaviour Firmware.Handler

  def start_link do
    GenStage.start_link(__MODULE__, [])
  end

  def move_absolute(handler, pos, x_speed, y_speed, z_speed) do
    GenStage.call(handler, {:move_absolute, pos, x_speed, y_speed, z_speed})
  end

  def calibrate(handler, axis) do
    GenStage.call(handler, {:calibrate, axis})
  end

  def find_home(handler, axis) do
    GenStage.call(handler, {:find_home, axis})
  end

  def home(handler, axis) do
    GenStage.call(handler, {:home, axis})
  end

  def home_all(handler) do
    GenStage.call(handler, :home_all)
  end

  def zero(handler, axis) do
    GenStage.call(handler, {:zero, axis})
  end

  def update_param(handler, param, val) do
    GenStage.call(handler, {:update_param, param, val})
  end

  def read_param(handler, param) do
    GenStage.call(handler, {:read_param, param})
  end

  def read_all_params(handler) do
    GenStage.call(handler, :read_all_params)
  end

  def emergency_lock(handler) do
    GenStage.call(handler, :emergency_lock)
  end

  def emergency_unlock(handler) do
    GenStage.call(handler, :emergency_unlock)
  end

  def set_pin_mode(handler, pin, mode) do
    GenStage.call(handler, {:set_pin_mode, pin, mode})
  end

  def read_pin(handler, pin, pin_mode) do
    GenStage.call(handler, {:read_pin, pin, pin_mode})
  end

  def write_pin(handler, pin, pin_mode, value) do
    GenStage.call(handler, {:write_pin, pin, pin_mode, value})
  end

  def request_software_version(handler) do
    GenStage.call(handler, :request_software_version)
  end

  def set_servo_angle(handler, pin, number) do
    GenStage.call(handler, {:set_servo_angle, pin, number})
  end

  ## Private

  defmodule State do
    @moduledoc false
    defstruct [
      config_busy: false,
      nerves: nil,
      current_cmd: nil,
      tty: nil,
      hw: nil
    ]
  end

  def init([]) do
    Farmbot.Logger.debug 3, "Uart handler init."
    # If in dev environment,
    #   it is expected that this be done at compile time.
    # If in target environment,
    #   this should be done by `Farmbot.Firmware.AutoDetector`.
    error_msg = "Please configure uart handler!"
    tty = Application.get_env(:farmbot_core, :uart_handler)[:tty] || raise error_msg

    # Disable fw input logs after a reset of the
    # Fw handler if they were enabled.
    update_config_value(:bool, "settings", "firmware_input_log", false)
    hw = get_config_value(:string, "settings", "firmware_hardware")
    gen_stage_opts = [
      dispatcher: GenStage.BroadcastDispatcher,
    ]
    case open_tty(tty) do
      {:ok, nerves} ->
        {:producer, %State{nerves: nerves, tty: tty, hw: hw}, gen_stage_opts}
      {:error, reason} ->
        Farmbot.Logger.error 1, "Uart handler failed to initialize: #{inspect reason}"
        :ignore
    end
  end

  def handle_events(events, _, state) do
    state = Enum.reduce(events, state, fn(event, state_acc) ->
      handle_config(event, state_acc)
    end)

    case state do
      %State{config_busy: true} = state ->
        {:noreply, [:busy], state}
      %State{} = state ->
        {:noreply, [], state}
      _ -> state
    end
  end

  defp handle_config({:config, "settings", key, _val}, state)
    when key in ["firmware_input_log", "firmware_output_log"]
  do
    # Restart the framing to pick up new changes.
    UART.configure state.nerves, [framing: UART.Framing.None, active: false]
    configure_uart(state.nerves, true)
    %{state | config_busy: true}
  end

  defp handle_config({:config, "settings", "firmware_hardware", _val}, _state) do
    raise("FIXME")
  end

  defp handle_config(_, state) do
    state
  end

  defp open_tty(tty, nerves \\ nil) do
    Farmbot.Logger.debug 3, "Opening uart device: #{tty}"
    nerves = nerves || UART.start_link |> elem(1)
    Process.link(nerves)
    case UART.open(nerves, tty, [speed: 115_200, active: true]) do
      :ok ->
        :ok = configure_uart(nerves, true)
        # Flush the buffers so we start fresh
        :ok = UART.flush(nerves)
        loop_until_idle(nerves)
      err ->
        err
    end
  end

  defp loop_until_idle(nerves, idle_count \\ 0)

  defp loop_until_idle(nerves, 2) do
    Farmbot.Logger.success 3, "Got two idles. UART is up."
    Process.sleep(1500)
    {:ok, nerves}
  end

  defp loop_until_idle(nerves, idle_count)
    when is_pid(nerves) and is_number(idle_count)
  do
    Farmbot.Logger.debug 3, "Waiting for firmware idle."
    receive do
      {:circuits_uart, _, {:error, reason}} -> {:stop, reason}
      {:circuits_uart, _, {:partial, _}} -> loop_until_idle(nerves, idle_count)
      {:circuits_uart, _, {_, :idle}} -> loop_until_idle(nerves, idle_count + 1)
      {:circuits_uart, _, {_, {:debug_message, msg}}} ->
        if String.contains?(msg, "STARTUP") do
          Farmbot.Logger.success 3, "Got #{msg}. UART is up."
          UART.write(nerves, "F22 P2 V0")
          {:ok, nerves}
        else
          Farmbot.Logger.debug 3, "Got arduino debug while booting up: #{msg}"
          loop_until_idle(nerves, idle_count)
        end
      {:circuits_uart, _, _msg} -> loop_until_idle(nerves, idle_count)
    after
      10_000 -> {:stop, "Firmware didn't send any info for 10 seconds."}
    end
  end

  defp configure_uart(nerves, active) do
    UART.configure(
      nerves,
      framing: {Farmbot.Firmware.UartHandler.Framing, separator: "\r\n"},
      active: active,
      rx_framing_timeout: 500
    )
  end

  def terminate(reason, state) do
    Farmbot.Logger.warn 1, "UART handler died: #{inspect reason}"
    if state.nerves do
      UART.close(state.nerves)
      UART.stop(state.nerves)
    end
  end

  # if there is an error, we assume something bad has happened, and we probably
  # Are better off crashing here, and being restarted.
  def handle_info({:circuits_uart, _, {:error, :eio}}, state) do
    Farmbot.Logger.error 1, "UART device removed."
    old_env = Application.get_env(:farmbot_core, :behaviour)
    new_env = Keyword.put(old_env, :firmware_handler, Firmware.StubHandler)
    Application.put_env(:farmbot_core, :behaviour, new_env)
    {:stop, {:error, :eio}, state}
  end

  def handle_info({:circuits_uart, _, {:error, reason}}, state) do
    {:stop, {:error, reason}, state}
  end

  # Unhandled gcodes just get ignored.
  def handle_info({:circuits_uart, _, {:unhandled_gcode, code_str}}, state) do
    Farmbot.Logger.debug 3, "Got unhandled gcode: #{code_str}"
    {:noreply, [], state}
  end

  def handle_info({:circuits_uart, _, {_, {:report_software_version, v}}}, state) do
    expected = Application.get_env(:farmbot_core, :expected_fw_versions)
    if v in expected do
      {:noreply, [{:report_software_version, v}], state}
    else
      err = "Firmware version #{v} is not in expected versions: #{inspect expected}"
      Farmbot.Logger.error 1, err
      old_env = Application.get_env(:farmbot_core, :behaviour)
      new_env = Keyword.put(old_env, :firmware_handler, Firmware.StubHandler)
      Application.put_env(:farmbot_core, :behaviour, new_env)
      {:stop, :normal, state}
    end
  end

  def handle_info({:circuits_uart, _, {:echo, _}}, %{current_cmd: nil} = state) do
    {:noreply, [], state}
  end
  def handle_info({:circuits_uart, _, {:echo, {:echo, "*F43" <> _}}}, state) do
    {:noreply, [], state}
  end

  def handle_info({:circuits_uart, _, {:echo, {:echo, code}}}, state) do
    distance = String.jaro_distance(state.current_cmd, code)
    if distance > 0.85 do
      :ok
    else
      err = "Echo #{code} does not match #{state.current_cmd} (#{distance})"
      Farmbot.Logger.error 3, err
    end
    {:noreply, [], %{state | current_cmd: nil}}
  end

  def handle_info({:circuits_uart, _, {_q, :done}}, state) do
    {:noreply, [:done], %{state | current_cmd: nil, config_busy: false}}
  end

  def handle_info({:circuits_uart, _, {_q, gcode}}, state) do
    {:noreply, [gcode], state}
  end

  def handle_info({:circuits_uart, _, bin}, state) when is_binary(bin) do
    Farmbot.Logger.warn(3, "Unparsed Gcode: #{bin}")
    {:noreply, [], state}
  end

  defp do_write(bin, state, dispatch \\ []) do
    # Farmbot.Logger.debug 3, "writing: #{bin}"
    case UART.write(state.nerves, bin) do
      :ok -> {:reply, :ok, dispatch, %{state | current_cmd: bin}}
      err -> {:reply, err, [], %{state | current_cmd: nil}}
    end
  end

  def handle_call({:move_absolute, pos, x_speed, y_speed, z_speed}, _from, state) do
    cmd = "X#{fmnt_float(pos.x)} "
       <> "Y#{fmnt_float(pos.y)} "
       <> "Z#{fmnt_float(pos.z)} "
       <> "A#{fmnt_float(x_speed)} "
       <> "B#{fmnt_float(y_speed)} "
       <> "C#{fmnt_float(z_speed)}"
    wrote = "G00 #{cmd}"
    do_write(wrote, state)
  end

  def handle_call({:calibrate, axis}, _from, state) do
    num = case axis |> to_string() do
      "x" -> 14
      "y" -> 15
      "z" -> 16
    end
    do_write("F#{num}", state)
  end

  def handle_call({:find_home, axis}, _from, state) do
    cmd = case axis |> to_string() do
      "x" -> "11"
      "y" -> "12"
      "z" -> "13"
    end
    do_write("F#{cmd}", state)
  end

  def handle_call(:home_all, _from, state) do
    do_write("G28", state)
  end

  def handle_call({:home, axis}, _from, state) do
    cmd = case axis |> to_string() do
      "x" -> "X0"
      "y" -> "Y0"
      "z" -> "Z0"
    end
    do_write("G00 #{cmd}", state)
  end

  def handle_call({:zero, axis}, _from, state) do
    axis_format = case axis |> to_string() do
      "x" -> "X"
      "y" -> "Y"
      "z" -> "Z"
    end
    do_write("F84 #{axis_format}1", state)
  end

  def handle_call(:emergency_lock, _from, state) do
    r = UART.write(state.nerves, "E")
    {:reply, r, [], state}
  end

  def handle_call(:emergency_unlock, _from, state) do
    do_write("F09", state)
  end

  def handle_call({:read_param, param}, _from, state) do
    num = Farmbot.Firmware.Gcode.Param.parse_param(param)
    do_write("F21 P#{num}", state)
  end

  def handle_call({:update_param, param, val}, _from, state) do
    num = Farmbot.Firmware.Gcode.Param.parse_param(param)
    do_write("F22 P#{num} V#{fmnt_float(val)}", state)
  end

  def handle_call(:read_all_params, _from, state) do
    do_write("F20", state)
  end

  def handle_call({:set_pin_mode, pin, mode_atom}, _from, state) do
    encoded_mode = extract_set_pin_mode(mode_atom)
    do_write("F43 P#{pin} M#{encoded_mode}", state, [])
  end

  def handle_call({:read_pin, pin, mode}, _from, state) do
    encoded_mode = extract_pin_mode(mode)
    dispatch = [{:report_pin_mode, pin, mode}]
    do_write("F42 P#{pin} M#{encoded_mode}", state, dispatch)
  end

  def handle_call({:write_pin, pin, mode, value}, _from, state) do
    encoded_mode = extract_pin_mode(mode)
    dispatch = [{:report_pin_mode, pin, mode}, {:report_pin_value, pin, value}]
    do_write("F41 P#{pin} V#{value} M#{encoded_mode}", state, dispatch)
  end

  def handle_call(:request_software_version, _from, state) do
    do_write("F83", state)
  end

  def handle_call({:set_servo_angle, pin, angle}, _, state) do
    do_write("F61 P#{pin} V#{angle}", state)
  end

  def handle_call(_call, _from, state) do
    {:reply, {:error, :bad_call}, [], state}
  end

  def handle_demand(_amnt, state) do
    {:noreply, [], state}
  end
end