xmavlink_util

defmodule MAVLink.Util.Arm do
  @moduledoc false
  
  @arm_retry_interval 3000
  
  require Logger
  alias APM.Message.CommandLong
  alias APM.Message.Heartbeat
  alias MAVLink.Router, as: MAV
  import MAVLink.Util.CacheManager, only: [msg: 1]
  import MAVLink.Util.FocusManager, only: [focus: 0]
  
  
  def arm() do
    with {:ok, {system_id, component_id, mavlink_version}} <- focus() do
      arm(system_id, component_id, mavlink_version)
    end
  end
  
  def arm(system_id, component_id, mavlink_version) do
    with {:ok, _, %Heartbeat{system_status: system_status}}
         when system_status in [:mav_state_standby, :mav_state_active, :mav_state_critical, :mav_state_emergency]
         <- msg Heartbeat do
      MAV.subscribe message: Heartbeat, source_system: system_id
    
      MAV.pack_and_send(%CommandLong{
        command: :mav_cmd_component_arm_disarm,
        confirmation: 1,
        param1: 1.0,    # Arm
        param2: 0.0,
        param3: 0.0,
        param4: 0.0,
        param5: 0.0,
        param6: 0.0,
        param7: 0.0,
        target_component: component_id,
        target_system: system_id}, mavlink_version)
      
      receive do
        %Heartbeat{base_mode: base_mode} ->
          cond do
            :mav_mode_flag_safety_armed in base_mode ->
              MAV.unsubscribe()
              Logger.info("Armed vehicle #{system_id}.#{component_id}")
              :ok
            true ->
              arm(system_id, component_id, mavlink_version)
          end
      after
          @arm_retry_interval ->
            arm(system_id, component_id, mavlink_version)
      end
    else
      %Heartbeat{system_status: invalid_system_status} ->
        Logger.warn("Cannot arm vehicle #{system_id}.#{component_id}: #{APM.describe invalid_system_status}")
        {:error, :cannot_arm_invalid_vehicle_status}
      _ ->
        Logger.warn("Could not determine current status of vehicle #{system_id}.#{component_id}")
        {:error, :could_not_determine_vehicle_status}
    end
  end
  
  
  
  def disarm() do
    with {:ok, {system_id, component_id, mavlink_version}} <- focus() do
      disarm(system_id, component_id, mavlink_version)
    end
  end
  
  def disarm(system_id, component_id, mavlink_version) do
    MAV.subscribe message: Heartbeat, source_system: system_id
  
    MAV.pack_and_send(%CommandLong{
      command: :mav_cmd_component_arm_disarm,
      confirmation: 1,
      param1: 0.0,    # Disarm
      param2: 0.0,
      param3: 0.0,
      param4: 0.0,
      param5: 0.0,
      param6: 0.0,
      param7: 0.0,
      target_component: component_id,
      target_system: system_id}, mavlink_version)
    
    receive do
      %Heartbeat{base_mode: base_mode} ->
        cond do
          :mav_mode_flag_safety_armed not in base_mode ->
            MAV.unsubscribe()
            Logger.info("Disarmed vehicle #{system_id}.#{component_id}")
            :ok
          true ->
            disarm(system_id, component_id, mavlink_version)
        end
    after
        @arm_retry_interval ->
          disarm(system_id, component_id, mavlink_version)
    end
  end

end