local bit = require("bit")

all_devs, number = create_HID_table()
local the_index
for k, v in pairs(all_devs) do
   if v.vendor_id == 0x1209 and v.usage == 0 then
      the_path = v.path
      --print(v.path)
      --print(v.usage)
   end
end

local button_offset = 320
local button_hdg_sync = 1
local button_alt_sync = 7
local led_hdg  = 0x0001
local led_apr  = 0x0002
local led_nav  = 0x0004
local led_lvl  = 0x0008
local led_alt  = 0x0010
local led_fd   = 0x0020
local led_vnav = 0x0040
local led_ap   = 0x0080
local led_vs   = 0x0100
local led_at   = 0x0200
local led_flc  = 0x0400
local led_spd  = 0x0800
local led_n1   = 0x1000

local aircraft_data = {
   ["SR22"] = {["min_as"] = 64, ["max_as"] = 205, ["has_mach"] = false, ["has_csc"] = false},
   ["DR40"] = {["min_as"] = 49, ["max_as"] = 146, ["has_mach"] = false, ["has_csc"] = false},
   ["E55P"] = {["min_as"] = 89, ["max_as"] = 320, ["has_mach"] = true, ["max_mach"] = 0.78, ["has_csc"] = true},
   ["DH8D"] = {["min_as"] = 95, ["max_as"] = 290, ["has_mach"] = false, ["has_csc"] = false},
}

dataref("ac_icao", "sim/aircraft/view/acf_ICAO")

local cur_aircraft = aircraft_data[ac_icao]
if cur_aircraft == nil then
   print("Unknown Aircraft " .. ac_icao)
   cur_aircraft = {["min_as"] = 0, ["max_as"] = 999, ["has_mach"] = true, ["max_mach"] = 0.99,  ["has_csc"] = false}
else
   print("Aircraft is " .. ac_icao .. ": Min AS " .. cur_aircraft.min_as .. ", Max AS " .. cur_aircraft.max_as)
end

function table.clone(org)
  return {table.unpack(org)}
end

function send_app_report(device, hdg, leds, alt, vs, as, mach, pitch)
   type_ = 0
   if mach then
      type_ = bit.bor(type_, 0x10)
   end
   if pitch then
      type_ = bit.bor(type_, 0x20)
   end
   hid_write(device, 2, type_, bit.band(hdg, 0xff), bit.arshift(hdg, 8), bit.band(leds, 0xff), bit.arshift(leds, 8),
	     bit.band(alt, 0xff), bit.arshift(alt, 8), bit.band(vs, 0xff), bit.arshift(vs, 8),
	     bit.band(as, 0xff), bit.arshift(as, 8))
end

function build_leds()
   local val = 0
   if status_hdg == 1 then
      val = val + led_hdg
   elseif status_hdg == 2 then
      val = val + led_nav
   end
   if status_apr > 0 then
      val = val + led_apr
   end
   if status_alt > 1 then
      val = val + led_alt
   end
   if status_fd == 1 then
      val = val + led_fd
   elseif status_fd == 2 then
      val = val + led_fd + led_ap
   end
   if status_vnav > 1 then
      val = val + led_vnav
   end
   if status_altmode == 4 then
      val = val + led_vs
   elseif status_altmode == 5 then
      val = val + led_flc
   end
      
   return val
end

if the_path ~= nil then
   device = hid_open_path(the_path)
else
   print("Device not found!")
   device = hid_open(0x1209, 0x1)
end
if device == nil then
   print("No device!")
else
   hid_set_nonblocking(device, 1)
   if cur_aircraft.has_mach then
      local max_mach = math.floor(cur_aircraft.max_mach * 1000 + 0.5)
      hid_write(device, 2, 1, bit.band(cur_aircraft.min_as, 0xff), bit.arshift(cur_aircraft.min_as, 8),
		bit.band(cur_aircraft.max_as, 0xff), bit.arshift(cur_aircraft.max_as, 8),
		bit.band(max_mach, 0xff), bit.arshift(max_mach, 8), 0, 0, 0, 0)
   else
      hid_write(device, 2, 1, bit.band(cur_aircraft.min_as, 0xff), bit.arshift(cur_aircraft.min_as, 8),
		bit.band(cur_aircraft.max_as, 0xff), bit.arshift(cur_aircraft.max_as, 8), 0, 0, 0, 0, 0, 0)
   end
   dataref("ap_hdg", "sim/cockpit2/autopilot/heading_dial_deg_mag_pilot", "writable")
   dataref("mag_hdg", "sim/cockpit2/gauges/indicators/heading_AHARS_deg_mag_copilot")

   dataref("ap_alt", "sim/cockpit2/autopilot/altitude_dial_ft", "writable")
   dataref("cur_alt", "sim/cockpit2/gauges/indicators/altitude_ft_pilot")

   dataref("ap_vs", "sim/cockpit2/autopilot/vvi_dial_fpm", "writable")
   dataref("cur_vvi", "sim/cockpit2/gauges/indicators/vvi_fpm_pilot")

   dataref("ap_as", "sim/cockpit2/autopilot/airspeed_dial_kts", "writable")
   dataref("ap_as_kt_mach", "sim/cockpit2/autopilot/airspeed_dial_kts_mach", "writable")
   dataref("cur_ias", "sim/cockpit2/gauges/indicators/airspeed_kts_pilot")
   dataref("cur_mach", "sim/cockpit2/gauges/indicators/mach_pilot")
   
   dataref("status_alt", "sim/cockpit2/autopilot/altitude_hold_status")
   dataref("status_apr", "sim/cockpit2/autopilot/approach_status")
   dataref("status_hdg", "sim/cockpit2/autopilot/heading_mode")
   dataref("status_fd", "sim/cockpit2/autopilot/flight_director_mode")
   dataref("status_vnav", "sim/cockpit2/autopilot/vnav_status")
   dataref("status_altmode", "sim/cockpit2/autopilot/altitude_mode")
   dataref("status_machmode", "sim/cockpit2/autopilot/airspeed_is_mach")
   if ac_icao == "DH8D" then
      dataref("q4xp_status_altmode", "FJS/Q4XP/FMA/pitch_act")
      dataref("q4xp_ap_pitch", "sim/cockpit2/autopilot/sync_hold_pitch_deg", "writable")
      send_app_report(device, math.floor(ap_hdg + 0.5), build_leds(), math.floor(ap_alt + 0.5), -2, -2, status_machmode == 1, q4xp_status_altmode == 1)
   else
      send_app_report(device, math.floor(ap_hdg + 0.5), build_leds(), math.floor(ap_alt + 0.5), -2, -2, status_machmode == 1, false)   
   end
   local prev_status_altmode = 0
   local prev_status_machmode = status_machmode
   local prev_ap_hdg = ap_hdg
   local prev_report_ap_hdg = 0
   function usb_app()
      local send_hdg = -1
      local send_alt = -1
      local send_vs = -1
      local send_as = -1
      local app_report = {0xffff, 0xffff, 0xffff, 0xffff}
      local app_report_good = false
      local loops = 100
      -- read app report from hid device, discarding all reports except the latest one to prevent input lag
      repeat
	 local tmp_cnt
	 local tmp = {}
	 tmp_cnt, tmp[0], tmp[1], tmp[2], tmp[3], tmp[4], tmp[5], tmp[6], tmp[7], tmp[8], tmp[9], tmp[10], tmp[11] = hid_read(device, 12)
	 if tmp_cnt >= 11 and tmp[0] == 2 and bit.band(tmp[1], 0xf) == 0 then
	    app_report = {bit.bor(tmp[2], bit.lshift(tmp[3], 8)),
			  bit.bor(tmp[6], bit.lshift(tmp[7], 8)),
			  bit.bor(tmp[8], bit.lshift(tmp[9], 8)),
			  bit.bor(tmp[10], bit.lshift(tmp[11], 8))}
	    app_report_good = true
	 end
	 loops = loops - 1
      until tmp_cnt == 0 or tmp_cnt == nil or loops == 0
      -- autopilot heading
      if button(button_offset + button_hdg_sync) and not last_button(button_offset + button_hdg_sync) then
	 mag_hdg_int = math.floor(mag_hdg + 0.5)
	 ap_hdg = mag_hdg_int
	 send_hdg = mag_hdg_int
      elseif prev_ap_hdg ~= ap_hdg then
	 send_hdg = ap_hdg
      elseif app_report_good and app_report[1] ~= 0xffff and prev_report_ap_hdg ~= app_report[1] then
	 ap_hdg = app_report[1]
      end
      prev_ap_hdg = ap_hdg
      if app_report_good and app_report[1] ~= 0xffff then
	 prev_report_ap_hdg = app_report[1]
      end
      -- autopilot altitude
      if button(button_offset + button_alt_sync) and not last_button(button_offset + button_alt_sync) then
	 cur_alt_int = math.floor(cur_alt + 0.5)
	 ap_alt = cur_alt_int
	 send_alt = cur_alt_int
      end
      if app_report_good and app_report[2] ~= 0xffff then
	 ap_alt = app_report[2]
      end
      -- vertical speed or pitch
      local enter_mode_vs = false
      local exit_mode_vs = false
      local enter_mode_pitch = false
      local exit_mode_pitch = false
      local mode_vs = false
      local mode_pitch = false
      if ac_icao == "DH8D" then
	 if q4xp_status_altmode == 3 and prev_status_altmode ~= 3 then
	    enter_mode_vs = true
	 elseif q4xp_status_altmode ~= 3 and prev_status_altmode == 3 then
	    exit_mode_vs = true
	 end
	 if q4xp_status_altmode == 3 then
	    mode_vs = true
	 end
	 if q4xp_status_altmode == 1 and prev_status_altmode ~= 1 then
	    enter_mode_pitch = true
	 elseif q4xp_status_altmode ~= 1 and prev_status_altmode == 1 then
	    exit_mode_pitch = true
	 end
	 if q4xp_status_altmode == 1 then
	    mode_pitch = true
	 end
      else
	 if status_altmode == 4 and prev_status_altmode ~= 4 then
	    enter_mode_vs = true
	 elseif status_altmode ~= 4 and prev_status_altmode == 4 then
	    exit_mode_vs = true
	 end
	 if status_altmode == 4 then
	    mode_vs = true
	 end
      end
      if enter_mode_vs then
	 send_vs = math.floor(cur_vvi + 0.5)
      elseif enter_mode_pitch then
	 send_vs = math.floor(q4xp_ap_pitch + 0.5)
      elseif exit_mode_vs or exit_mode_pitch then
	 send_vs = -2
      elseif app_report_good and app_report[3] ~= 0xffff then
	 if mode_vs then
	    if bit.band(app_report[3], 0x8000) ~= 0 then
	       ap_vs = -bit.band(bit.bnot(app_report[3]), 0x7fff) - 1
	    else
	       ap_vs = app_report[3]
	    end
	 elseif mode_pitch then
	    if bit.band(app_report[3], 0x8000) ~= 0 then
	       q4xp_ap_pitch = (-bit.band(bit.bnot(app_report[3]), 0x7fff) - 1)
	    else
	       q4xp_ap_pitch = app_report[3]
	    end
	 end
      end
      -- airspeed
      local enter_mode_flc = false
      local exit_mode_flc = false
      local mode_flc = false
      if ac_icao == "DH8D" then
	 if q4xp_status_altmode == 2 and prev_status_altmode ~= 2 then
	    enter_mode_flc = true
	 elseif q4xp_status_altmode ~= 2 and prev_status_altmode == 2 then
	    exit_mode_flc = true
	 end
	 if q4xp_status_altmode == 2 then
	    mode_flc = true
	 end
      else
	 if status_altmode == 5 and prev_status_altmode ~= 5 then
	    enter_mode_flc = true
	 elseif status_altmode ~= 5 and prev_status_altmode == 5 then
	    exit_mode_flc = true
	 end
	 if status_altmode == 5 then
	    mode_flc = true
	 end
      end
      if enter_mode_flc then
	 if status_machmode == 1 then
	    send_as = math.floor(cur_mach * 1000 + 0.5)
	 else
	    if cur_ias < cur_aircraft.min_as then
	       send_as = cur_aircraft.min_as
	    else
	       send_as = math.floor(cur_ias + 0.5)
	    end
	 end
      elseif exit_mode_flc then
	 send_as = -2
      elseif mode_flc and status_machmode ~= prev_status_machmode then
	 if status_machmode == 1 then
	    send_as = math.floor(ap_as_kt_mach*1000 + 0.5)
	 else
	    send_as = math.floor(ap_as_kt_mach + 0.5)
	 end
      elseif app_report_good and app_report[4] ~= 0xffff then
	 if status_machmode == 1 then
	    ap_as_kt_mach = app_report[4] / 1000
	 else
	    ap_as_kt_mach = app_report[4]
	 end
      end
      if ac_icao == "DH8D" then
	 send_app_report(device, send_hdg, build_leds(), send_alt, send_vs, send_as, status_machmode == 1, q4xp_status_altmode == 1)
	 prev_status_altmode = q4xp_status_altmode
      else
	 send_app_report(device, send_hdg, build_leds(), send_alt, send_vs, send_as, status_machmode == 1, false)
	 prev_status_altmode = status_altmode
      end
      prev_status_machmode = status_machmode
   end

   do_every_frame("usb_app()")

   function exit_handler()
      send_app_report(device, -2, 0, -2, -2, -2, 0, 0)
   end

   do_on_exit('exit_handler()')
end