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 [
    version: nil,              # Which raw attributes are populated?
    payload_length: nil,
    incompatible_flags: 0,      # MAVLink 2 only
    compatible_flags: 0,        # MAVLink 2 only
    sequence_number: nil,
    source_system: nil,
    source_component: nil,
    target_system: 0,          # Default to broadcast assumed elsewhere
    target_component: 0,
    target: nil,
    message_id: nil,
    crc_extra: nil,
    payload: nil,
    checksum: nil,
    signature: nil,             # MAVLink 2 signing only (not implemented)
    mavlink_1_raw: nil,         # Original binary frame
    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
  def binary_to_frame_and_tail(raw_and_rest=<<0xfe, # MAVLink version 1
      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

  def binary_to_frame_and_tail(raw_and_rest=<<0xfd, # MAVLink version 2
      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
          payload_truncated_length = 8 * (expected_length - frame.payload_length)  # Only used to undo MAVLink 2 payload truncation
          try do  # Too many ways for unpack to fail with dodgy messages...
            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),
              0::unsigned-integer-size(8),  # Incompatible flags
              0::unsigned-integer-size(8),  # Compatible flags
              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
      {1, <<0>>}  # First byte of payload never truncated
    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