-- Change VTX Pit Mode
-- By: ArchmageAU
-- Version: 1.1
-- Based on BF Telemetry script and VTXprog script
-- Reads VTX settings from telemetry so that only power is changed on request
-- Will only set Power to Pit Mode or 25mw.
-- If wanting to do more than 25mw, change using telemetry screen
-- Most races only want 25mw to prevent drowning out other racers.
--
-- Changelog:
-- V1.0
--  - Initial release
-- V1.1
--  - Ensure TxPitMode > 0 is ON and <= 0 is OFF
--  - Ensure changeState is reset even if no change in mode required
--  - Add 5s retest timeout in script.
--  - Change voice alert to Activated/Deactivated.
-- V1.2
--  - Remove 5s retest timeout (was interrupting BF telemetry)
--  - Initate change on change to Activate. (so Activate off/on will re-send request)
--  - Initate change on change to TxPitMode. (only send once per change to TxPitMode)

-- Load MSP smartport library
assert(loadScript("/SCRIPTS/BF/msp_sp.lua"))()

-- define inputs
-- (Activate - <=0 Inactive, >0 set VTX Pit Mode state based on TxPitMode)
-- (TxPitMode - <=0 VTX Pit Mode OFF, >0 VTX Pit Mode ON)
local input = {
  { "Activate", SOURCE},               -- source requested Activate from logical switch, global variable, channel, etc..
  { "TxPitMode", SOURCE}               -- source requested TxPitMode from logical switch, global variable, channel, etc..
}

-- Variables to store parameters last programmed to vtx
local readVTX = 0
local readPower = 0
local readBand = 0
local readChannel = 0
local readPitMode = 0

local firstrun = 1
local lastPitMode = 0
local lastActivate = nil
local dataRequested = 0
local dataRequestedTime = nil
local dataRead = 0
local changeState = 0
local REQ_TIMEOUT = 80 -- 800ms telemetry request timeout

local MSP_VTX_CONFIG = 88
local MSP_VTX_SET_CONFIG = 89

-- Returns payload to send to fc
local function VTXconfig(TxPitMode)
   local channel = (readBand-1)*8 + readChannel-1
   return { bit32.band(channel,0xFF), bit32.rshift(channel,8), readPower, TxPitMode }
end

-- Process results of request
local function processMspReply(cmd, rx_buf)
  if cmd == nil or rx_buf == nil then
    return 0
  else
    if cmd ~= MSP_VTX_CONFIG then
      return 0
    else
      -- Data, so store it
      if #(rx_buf) < 5 then
        -- Returned data is too short, so re-request
        dataRequested = 0

        return 0
      else
        -- Read returned data
        readVTX = rx_buf[1]
        readBand = rx_buf[2]
        readChannel = rx_buf[3]
        readPower = rx_buf[4]
        readPitMode = rx_buf[5]

        return 1
      end
    end
  end
end

-- Initiate change pit mode
local function InitiateChangePitMode(Activate, TxPitMode)
  lastActivate = Activate
  lastPitMode = TxPitMode
  changeState = 1
  dataRequested = 0
  dataRead = 0
end

-- This script will run continuously, so only do something if it needs doing
local function run(Activate, TxPitMode)
  -- If inactive, do nothing
  if Activate <= 0 then
    lastActivate = Activate
    return
  end

  -- Check if telemetry
  if getValue("RSSI") == 0 then
    return
  end

  -- If first time, record TxPitMode and set to change state
  if firstrun == 1 then
    firstrun = 0
    InitiateChangePitMode(Activate, TxPitMode)
  else
    if Activate ~= lastActivate then
      InitiateChangePitMode(Activate, TxPitMode)
    else
      if TxPitMode ~= lastPitMode then
        InitiateChangePitMode(Activate, TxPitMode)
      end
    end
  end

  -- if not changing state exit
  if changeState == 0 then
    return
  end

  -- Request data (if not already requested and not timed out)
  if (dataRequested == 0) or ((dataRequestedTime == nil) or (dataRequestedTime + REQ_TIMEOUT <= getTime())) then
     dataRequestedTime = getTime()
     mspSendRequest(MSP_VTX_CONFIG,{})
     dataRequested = 1
  end

  if dataRequested == 1 then
    -- Trocess the telemetry queue each time incase there is requests backed up in it
    mspProcessTxQ()
  end

  -- Get result if data requested
  if dataRead == 0 then
    dataRead = processMspReply(mspPollReply())
  end

  -- Write updated pit mode
  if dataRead == 1 then
    -- Get new pit mode
    local newPitMode = 0
    if TxPitMode > 0 then
      newPitMode = 1
    end

    -- Only change if requested pit mode state is different to existing pit mode state
    if newPitMode ~= readPitMode then
      mspSendRequest(MSP_VTX_SET_CONFIG, VTXconfig(newPitMode))
      mspProcessTxQ()
      if newPitMode == 1 then
        playFile("actvd.wav")
      else
        playFile("dactvd.wav")
      end
    end
    changeState = 0
    dataRead = 0
  end

  return
end

return {input=input, run=run}