xmavlink

defmodule XMAVLink.Frame do
  @moduledoc """
  Represent and work with MAVLink v1/2 message frames
  """

  require Logger

  import XMAVLink.Utils, only: [x25_crc: 1, x25_crc: 2]

  defstruct [
    # Which raw attributes are populated?
    version: nil,
    payload_length: nil,
    # MAVLink 2 only
    incompatible_flags: 0,
    # MAVLink 2 only
    compatible_flags: 0,
    sequence_number: nil,
    source_system: nil,
    source_component: nil,
    # Default to broadcast assumed elsewhere
    target_system: 0,
    target_component: 0,
    target: nil,
    message_id: nil,
    crc_extra: nil,
    payload: nil,
    checksum: nil,
    # MAVLink 2 signing only (not implemented)
    signature: nil,
    # Original binary frame
    mavlink_1_raw: nil,
    mavlink_2_raw: nil,
    message: nil
  ]

  @type message :: XMAVLink.Message.t()
  @type version :: 1 | 2
  @type t :: %XMAVLink.Frame{
          version: version,
          payload_length: 1..255,
          incompatible_flags: non_neg_integer,
          compatible_flags: non_neg_integer,
          sequence_number: 0..255,
          source_system: 1..255,
          source_component: 1..255,
          target_system: 1..255,
          target_component: 1..255,
          target: :broadcast | :system | :system_component | :component,
          message_id: XMAVLink.Types.message_id(),
          crc_extra: XMAVLink.Types.crc_extra(),
          payload: binary,
          checksum: pos_integer,
          mavlink_1_raw: binary,
          mavlink_2_raw: binary,
          message: message
        }

  @spec binary_to_frame_and_tail(binary) ::
          {XMAVLink.Frame.t(), binary} | {nil, binary} | :not_a_frame
  # MAVLink version 1
  def binary_to_frame_and_tail(
        raw_and_rest =
          <<0xFE, payload_length::unsigned-integer-size(8),
            sequence_number::unsigned-integer-size(8), source_system::unsigned-integer-size(8),
            source_component::unsigned-integer-size(8), message_id::unsigned-integer-size(8),
            payload::binary-size(payload_length), checksum::little-unsigned-integer-size(16),
            rest::binary>>
      ) do
    {
      struct(XMAVLink.Frame,
        version: 1,
        payload_length: payload_length,
        sequence_number: sequence_number,
        source_system: source_system,
        source_component: source_component,
        message_id: message_id,
        payload: payload,
        checksum: checksum,
        mavlink_1_raw:
          binary_part(
            raw_and_rest,
            0,
            byte_size(raw_and_rest) - byte_size(rest)
          )
      ),
      rest
    }
  end

  # MAVLink version 2
  def binary_to_frame_and_tail(
        raw_and_rest =
          <<0xFD, payload_length::unsigned-integer-size(8),
            incompatible_flags::unsigned-integer-size(8),
            compatible_flags::unsigned-integer-size(8), sequence_number::unsigned-integer-size(8),
            source_system::unsigned-integer-size(8), source_component::unsigned-integer-size(8),
            message_id::little-unsigned-integer-size(24), payload::binary-size(payload_length),
            checksum::little-unsigned-integer-size(16), rest::binary>>
      ) do
    case incompatible_flags do
      0 ->
        # Vanilla MAVLink 2, we can deal with this
        {
          struct(XMAVLink.Frame,
            version: 2,
            payload_length: payload_length,
            incompatible_flags: 0,
            compatible_flags: compatible_flags,
            sequence_number: sequence_number,
            source_system: source_system,
            source_component: source_component,
            message_id: message_id,
            payload: payload,
            checksum: checksum,
            mavlink_2_raw:
              binary_part(
                raw_and_rest,
                0,
                byte_size(raw_and_rest) - byte_size(rest)
              )
          ),
          rest
        }

      _ ->
        # We don't support any incompatible flags at present
        # e.g. signing, so drop the frame
        {nil, rest}
    end
  end

  def binary_to_frame_and_tail(unfinished_mavlink_1_frame = <<0xFE, _::binary>>),
    do: {nil, unfinished_mavlink_1_frame}

  def binary_to_frame_and_tail(unfinished_mavlink_2_frame = <<0xFD, _::binary>>),
    do: {nil, unfinished_mavlink_2_frame}

  def binary_to_frame_and_tail(<<_, rest::binary>>), do: binary_to_frame_and_tail(rest)
  def binary_to_frame_and_tail(<<>>), do: :not_a_frame

  @spec validate_and_unpack(XMAVLink.Frame.t(), module) ::
          {:ok, XMAVLink.Frame.t()} | :failed_to_unpack | :checksum_invalid | :unknown_message
  def validate_and_unpack(
        frame = %XMAVLink.Frame{message_id: message_id, version: version, payload: payload},
        dialect
      ) do
    case apply(dialect, :msg_attributes, [message_id]) do
      {:ok, crc_extra, expected_length, target} ->
        if frame.checksum ==
             :binary.bin_to_list(
               %{1 => frame.mavlink_1_raw, 2 => frame.mavlink_2_raw}[frame.version],
               {1, frame.payload_length + %{1 => 5, 2 => 9}[frame.version]}
             )
             |> x25_crc()
             |> x25_crc([crc_extra]) do
          # Only used to undo MAVLink 2 payload truncation
          payload_truncated_length = 8 * (expected_length - frame.payload_length)
          # Too many ways for unpack to fail with dodgy messages...
          try do
            case apply(dialect, :unpack, [
                   message_id,
                   version,
                   payload <>
                     if(payload_truncated_length > 0 and version > 1,
                       do: <<0::size(payload_truncated_length)>>,
                       else: <<>>
                     )
                 ]) do
              {:ok, message} ->
                case target do
                  :broadcast ->
                    {:ok,
                     struct(frame,
                       message: message,
                       target_system: 0,
                       target_component: 0,
                       target: target,
                       crc_extra: crc_extra
                     )}

                  :system ->
                    {:ok,
                     struct(frame,
                       message: message,
                       target_system: message.target_system,
                       target_component: 0,
                       target: target,
                       crc_extra: crc_extra
                     )}

                  :system_component ->
                    {:ok,
                     struct(frame,
                       message: message,
                       target_system: message.target_system,
                       target_component: message.target_component,
                       target: target,
                       crc_extra: crc_extra
                     )}

                  :component ->
                    {:ok,
                     struct(frame,
                       message: message,
                       target_system: 0,
                       target_component: message.target_component,
                       target: target,
                       crc_extra: crc_extra
                     )}
                end

              _ ->
                :failed_to_unpack
            end
          rescue
            _ ->
              :ok =
                Logger.debug(
                  "validate_and_unpack: Failed to unpack #{inspect(frame)}, couldn't match payload"
                )

              :failed_to_unpack
          end
        else
          :ok = Logger.debug("validate_and_unpack: Checksum invalid #{inspect(frame)}")
          :checksum_invalid
        end

      _ ->
        :ok = Logger.debug("validate_and_unpack: Unknown message #{inspect(frame)}")
        :unknown_message
    end
  end

  # Pack message frame
  def pack_frame(frame = %XMAVLink.Frame{version: 1}) do
    payload_length = byte_size(frame.payload)

    mavlink_1_frame =
      <<payload_length::unsigned-integer-size(8), frame.sequence_number::unsigned-integer-size(8),
        frame.source_system::unsigned-integer-size(8),
        frame.source_component::unsigned-integer-size(8),
        frame.message_id::little-unsigned-integer-size(8), frame.payload::binary>>

    frame
    |> struct(
      mavlink_1_raw: <<0xFE>> <> mavlink_1_frame <> checksum(mavlink_1_frame, frame.crc_extra)
    )
  end

  def pack_frame(frame = %XMAVLink.Frame{version: 2}) do
    {truncated_length, truncated_payload} = truncate_payload(frame.payload)

    mavlink_2_frame =
      <<
        truncated_length::unsigned-integer-size(8),
        # Incompatible flags
        0::unsigned-integer-size(8),
        # Compatible flags
        0::unsigned-integer-size(8),
        frame.sequence_number::unsigned-integer-size(8),
        frame.source_system::unsigned-integer-size(8),
        frame.source_component::unsigned-integer-size(8),
        frame.message_id::little-unsigned-integer-size(24),
        truncated_payload::binary
      >>

    struct(frame,
      mavlink_2_raw: <<0xFD>> <> mavlink_2_frame <> checksum(mavlink_2_frame, frame.crc_extra)
    )
  end

  # MAVLink 2 truncate trailing 0s in payload
  defp truncate_payload(payload) do
    truncated_payload = String.replace_trailing(payload, <<0>>, "")

    if byte_size(truncated_payload) == 0 do
      # First byte of payload never truncated
      {1, <<0>>}
    else
      {byte_size(truncated_payload), truncated_payload}
    end
  end

  # Calculate checksum
  defp checksum(frame, crc_extra) do
    cs = x25_crc(frame <> <<crc_extra::unsigned-integer-size(8)>>)
    <<cs::little-unsigned-integer-size(16)>>
  end
end