defmodule MAVLink.Util.SITL do
  @moduledoc """
  Provide SITL specific support e.g RC channel forwarding
  """
  
  require Logger
  alias MAVLink.Router, as: MAV
  alias APM.Message.RcChannelsRaw
  alias APM.Message.RequestDataStream
  import MAVLink.Util.FocusManager, only: [focus: 0]
  
  @resend_stream_interval 90
  
  
  def forward_rc(sitl_rc_in_port \\ 5501) do
    with {:ok, {system_id, component_id, mavlink_version}} <- focus() do
      forward_rc(system_id, component_id, mavlink_version, sitl_rc_in_port)
    end
  end
  
  def forward_rc(system_id, component_id, mavlink_version, sitl_rc_in_port) do
    Task.start_link(__MODULE__, :_connect, [system_id, component_id, mavlink_version, sitl_rc_in_port])
  end
  
  
  def _connect(system_id, component_id, mavlink_version, sitl_rc_in_port) do
    with {:ok, socket} <- :gen_udp.open(0, [:binary, ip: {127, 0, 0, 1}]),
         :ok <- MAV.subscribe message: RcChannelsRaw, source_system: system_id, source_component: component_id do
      Logger.info("Start forwarding RC from vehicle #{system_id}.#{component_id} to SITL rc-in port #{sitl_rc_in_port}}")
      _forward(system_id, component_id, mavlink_version, sitl_rc_in_port, socket, 0)
    else
      _ ->
        Logger.warn("Could not subscribe or open port to forward RC from vehicle #{system_id}.#{component_id}")
    end
  end
  
  def _forward(system_id, component_id, mavlink_version, sitl_rc_in_port, socket, 0) do
    MAV.pack_and_send(%RequestDataStream{
      target_system: system_id,
      target_component: 0,  # APM Planner sends this, doesn't work with real component id
      req_stream_id: APM.encode(:mav_data_stream_rc_channels, :mav_data_stream), # TODO Not a bitmask, where is clue in MAVlink that this field is related?
      req_message_rate: 18,
      start_stop: 1
    }, mavlink_version)
    _forward(system_id, component_id, mavlink_version, sitl_rc_in_port, socket, @resend_stream_interval)
  end
  
  def _forward(system_id, component_id, mavlink_version, sitl_rc_in_port, socket, count) do
    receive do
      %RcChannelsRaw{
        chan1_raw: c1,
        chan2_raw: c2,
        chan3_raw: c3,
        chan4_raw: c4,
        chan5_raw: c5,
        chan6_raw: c6,
        chan7_raw: c7,
        chan8_raw: c8
      } ->
        :gen_udp.send(
          socket,
          {127, 0, 0, 1}, # TODO accept destination IP as parameter
          sitl_rc_in_port,
          <<
            c1::little-unsigned-integer-size(16),
            c2::little-unsigned-integer-size(16),
            c3::little-unsigned-integer-size(16),
            c4::little-unsigned-integer-size(16),
            c5::little-unsigned-integer-size(16),
            c6::little-unsigned-integer-size(16),
            c7::little-unsigned-integer-size(16),
            c8::little-unsigned-integer-size(16)
          >>)
        _forward(system_id, component_id, mavlink_version, sitl_rc_in_port, socket, count - 1)
    end
  end
  
end