WPT_POS = {}
--更新航路
Waypoints = {}
local WPT_SIZE = get_param_handle('WPT_SIZE')
local target_wpt_handle = get_param_handle('TARGET_WPT')
local current_wpt = target_wpt_handle:get()
if current_wpt == 0 then
    -- 若句柄未初始化，按原逻辑赋初值
    if #Waypoints > 1 then
        current_wpt = 2
    else
        current_wpt = 1
    end
end
local param_h = {
    TARGET_WPT = {get_param_handle('TARGET_WPT'), current_wpt},
    TARGET_WPT_tbl = {get_param_handle('TARGET_WPT_TBL'),0},
}
Routes = {}
local routeTblOffset = 0
local unit = 0
RoutesTbl_param_h = {}
local function initRoute_param()
    for i = 1, 5, 1 do
        RoutesTbl_param_h[#RoutesTbl_param_h+1] = {
            idx = get_param_handle(string.format('WPT_PLAN_TBL_%.0f_IDX',i)),
            info = get_param_handle(string.format('WPT_PLAN_TBL_%.0f_INFO',i)),
            cru_ias = get_param_handle(string.format('WPT_PLAN_TBL_%.0f_CRU_IAS',i)),
            lat = get_param_handle(string.format('WPT_PLAN_TBL_%.0f_LAT',i)),
            lon = get_param_handle(string.format('WPT_PLAN_TBL_%.0f_LON',i)),
            alt = get_param_handle(string.format('WPT_PLAN_TBL_%.0f_ALT',i)),
            pre_HH = get_param_handle(string.format('WPT_PLAN_TBL_%.0f_PRE_HH',i)),
            pre_mm = get_param_handle(string.format('WPT_PLAN_TBL_%.0f_PRE_mm',i)),
            pre_ss = get_param_handle(string.format('WPT_PLAN_TBL_%.0f_PRE_ss',i)),
            pre_dd = get_param_handle(string.format('WPT_PLAN_TBL_%.0f_PRE_dd',i)),
        }
    end
end
function Init_waypoint()
    Waypoints = get_mission_route() or {}
    WPT_SIZE:set(#Waypoints)
    if #Waypoints > 1 then
        param_h.TARGET_WPT[2] = 2
    else
        param_h.TARGET_WPT[2] = 1
    end
	param_h.TARGET_WPT[1]:set(param_h.TARGET_WPT[2])
    initRoute_param()
    for i = 1, 100, 1 do
        WPT_POS[i] = {
            N_LAT = {get_param_handle(string.format('WPT_%02.0f_NEXT_REL_LAT',i)),0},
            N_LON = {get_param_handle(string.format('WPT_%02.0f_NEXT_REL_LON',i)),0},
            NEXT = {get_param_handle(string.format('WPT_%02.0f_NEXT',i)),0},
            LAT = {get_param_handle(string.format('WPT_%02.0f_LAT',i)),0},
            LON = {get_param_handle(string.format('WPT_%02.0f_LON',i)),0},
            NUM = {get_param_handle(string.format('WPT_%02.0f_NUM',i)),0},
            ALT = {get_param_handle(string.format('WPT_%02.0f_ALT',i)),0},
            ALT_TYPE = {get_param_handle(string.format('WPT_%02.0f_ALT_TYPE',i)),0},
            SPD = {get_param_handle(string.format('WPT_%02.0f_SPD',i)),0},
            TYPE = {get_param_handle(string.format('WPT_%02.0f_TYPE',i)),0},--0转向点;1着陆;2攻击
        }
    end
end
-- 通用：设置当前导航点并自动更新类型
function SetCurrentWpt(idx)
    param_h.TARGET_WPT[2] = idx
    -- 自动更新类型
    if idx > 0 and idx <= #Waypoints then
        local wptType = 0
        if Waypoints[idx].type == 'Land' then
            wptType = 1
        elseif Waypoints[idx].type == 'Attack' then
            wptType = 2
        end
        WPT_POS[idx].TYPE[2] = wptType
        WPT_POS[idx].TYPE[1]:set(wptType)
        get_param_handle('TARGET_WPT_TYPE'):set(wptType)
    end
end
local function autoNextWpt()
    local myLatM = GPS_LAT_M:get()
    local myLonM = GPS_LON_M:get()
    if #Waypoints > 1 then
        local target_route_coord = lo_to_geo_coords(Waypoints[param_h.TARGET_WPT[2]].x, Waypoints[param_h.TARGET_WPT[2]].y)
        local target_lat, target_lon = Terrain.convertLatLonToMeters(target_route_coord.lat, target_route_coord.lon)
        local lat_sub = math.abs(target_lat - myLatM)
        local lon_sub = math.abs(target_lon - myLonM)
        if lat_sub < 1000 and lon_sub < 1000 and param_h.TARGET_WPT[2] < #Waypoints then
            --param_h.TARGET_WPT[2] = param_h.TARGET_WPT[2] + 1
            SetCurrentWpt(param_h.TARGET_WPT[2] + 1)
        end
        if param_h.TARGET_WPT[2] - routeTblOffset > 5 then
            routeTblOffset = routeTblOffset + 1 --当目标航路点大于HSI下方航路表展示上限时，将展示数据偏移1个航路点
        end
    end
end

local function updateRoute()
    local routeDistance = 0
    local myLatM = GPS_LAT_M:get()
    local myLonM = GPS_LON_M:get()
    for i = 1, #Waypoints, 1 do
        local last_lat,last_lon
        if param_h.TARGET_WPT[2] > 0 and i>param_h.TARGET_WPT[2] then
            local last_route_coord = lo_to_geo_coords(Waypoints[i-1].x, Waypoints[i-1].y)
            last_lat,last_lon = Terrain.convertLatLonToMeters(last_route_coord.lat, last_route_coord.lon)
        else
            last_lat,last_lon = myLatM,myLonM
        end
        local route_coord = lo_to_geo_coords(Waypoints[i].x, Waypoints[i].y)
        local lat, lon = Terrain.convertLatLonToMeters(route_coord.lat, route_coord.lon)
        local lat_sub = lat-last_lat
        local lon_sub = lon-last_lon
        local distance
        if lat_sub == 0 and lon_sub == 0 then
            distance = 0
        else
            distance = math.sqrt(lat_sub^2+lon_sub^2)--根据勾股定理计算距离
            --DebugPrint(math.deg(angle))
        end
        local eta
        if (BASE_SENSOR.IAS:get()*MS_TO_KMH < 280 and param_h.TARGET_WPT[2]==i) or param_h.TARGET_WPT[2] > i then
            eta = -1
        else
            if param_h.TARGET_WPT[2]<i then
                if Waypoints[i].ETA <=0 then
                    if Waypoints[i].speed<1 then
                        eta = 147539
                    else
                        eta = distance/Waypoints[i].speed
                    end
                    Waypoints[i].ETA = eta
                else
                    eta = Waypoints[i].ETA
                end
            else
                local gs=GetGroundSpeed()
                if gs<1 then
                    eta = 147539--99天23小时59分59秒
                else
                    eta = distance/GetGroundSpeed()--预计到达时间，秒数做单位
                end
            end
        end
        if routeTblOffset < i and i< routeTblOffset+6 then
            RoutesTbl_param_h[i-routeTblOffset].idx:set(i-1-routeTblOffset)
            if i == 1 then
                RoutesTbl_param_h[i-routeTblOffset].info:set('起点')
            elseif type(Waypoints[i].info)=='string' then
                RoutesTbl_param_h[i-routeTblOffset].info:set(Waypoints[i].info)
            else
                RoutesTbl_param_h[i-routeTblOffset].info:set('无')
            end
            if unit == 0 then
                RoutesTbl_param_h[i-routeTblOffset].cru_ias:set(string.format('%.0fKMH',Waypoints[i].speed*MS_TO_KMH))
                RoutesTbl_param_h[i-routeTblOffset].alt:set(string.format('%.0fM',Waypoints[i].alt))
            else
                RoutesTbl_param_h[i-routeTblOffset].cru_ias:set(string.format('%.0fKnot',Waypoints[i].speed*MS_TO_KNOTS))
                RoutesTbl_param_h[i-routeTblOffset].alt:set(string.format('%.0fFT',Waypoints[i].alt*METER_TO_FEET))
            end
            RoutesTbl_param_h[i-routeTblOffset].lat:set(DD_to_DMS(route_coord.lat,1))
            RoutesTbl_param_h[i-routeTblOffset].lon:set(DD_to_DMS(route_coord.lon,2))
            if eta > 0 then
                --预计到达时间的秒数格式化成HH:mm:ss +dd
                local eta_day,etaH,eta_m,eta_s = SecondsTo_ddHHmmss(eta)
                RoutesTbl_param_h[i - routeTblOffset].pre_HH:set(etaH)
                RoutesTbl_param_h[i - routeTblOffset].pre_mm:set(eta_m)
                RoutesTbl_param_h[i - routeTblOffset].pre_ss:set(eta_s)
                RoutesTbl_param_h[i - routeTblOffset].pre_dd:set(eta_day)
            end
        end
    end
end
local function updateWpt()
    local scale = 40/MaxRange_h:get()
    for i = 1, #Waypoints, 1 do
        local route_coord = lo_to_geo_coords(Waypoints[i].x, Waypoints[i].y)
        local lat, lon = Terrain.convertLatLonToMeters(route_coord.lat, route_coord.lon)
        WPT_POS[i].LAT[2] = lat * scale
        WPT_POS[i].LON[2] = lon * scale
        WPT_POS[i].NUM[2] = i
        WPT_POS[i].ALT[2] = Waypoints[i].alt
        WPT_POS[i].ALT_TYPE[2] = Waypoints[i].alt_type
        WPT_POS[i].SPD[2] = Waypoints[i].speed
        if Waypoints[i].type == 'Land' then
            WPT_POS[i].TYPE[2] = 1
        elseif Waypoints[i].type == 'Attack' then
            WPT_POS[i].TYPE[2] = 2
        else
            WPT_POS[i].TYPE[2] = 0
        end
        if i < #Waypoints then
            WPT_POS[i].NEXT[2] = 1
            route_coord = lo_to_geo_coords(Waypoints[i + 1].x, Waypoints[i + 1].y)
            local n_lat, n_lon = Terrain.convertLatLonToMeters(route_coord.lat, route_coord.lon)
            WPT_POS[i].N_LAT[2] = (n_lat - lat) * scale
            WPT_POS[i].N_LON[2] = (n_lon - lon) * scale
        else
            WPT_POS[i].NEXT[2] = 0
            WPT_POS[i].N_LAT[2] = 0
            WPT_POS[i].N_LON[2] = 0
        end
    end
end


local wpt_form = {
    lat = get_param_handle('WPT_FORM_LAT'),
    lon = get_param_handle('WPT_FORM_LON'),
    id = get_param_handle('WPT_FORM_ID'),
    alt = get_param_handle('WPT_FORM_ALT'),
    altType = get_param_handle('WPT_FORM_ALT_TYPE'),
    speed = get_param_handle('WPT_FORM_SPD'),
    type = get_param_handle('WPT_FORM_TYPE'),
    ok = get_param_handle('WPT_FORM_OK'),
}
local function syncWpt()
    if wpt_form.ok:get() == 1 then
        local i = wpt_form.id:get()
        if i> #Waypoints then
            Waypoints[i] = {
                speed_locked = true,
                type = 'Turning Point',
                action = 'Turning Point',
                ETA = 0,
                ETA_locked = false
            }
        end
        if wpt_form.altType:get() == 1 then
            Waypoints[i].alt_type = 'RADIO'
        else
            Waypoints[i].alt_type = 'BARO'
        end
        if wpt_form.type:get()==0 then
            Waypoints[i].type='Turning Point'
        elseif wpt_form.type:get() == 1 then
            Waypoints[i].type='Land'
        elseif wpt_form.type:get() == 2 then
            Waypoints[i].type='Attack'
        end
        if unit == 1 then
            Waypoints[i].alt = wpt_form.alt:get()/METER_TO_FEET
            Waypoints[i].speed = wpt_form.speed:get()/MS_TO_KNOTS
        else
            Waypoints[i].alt = wpt_form.alt:get()
            Waypoints[i].speed = wpt_form.speed:get()/MS_TO_KMH
        end

        local lo = geo_to_lo_coords(wpt_form.lat:get(),wpt_form.lon:get())
        Waypoints[i].y = lo.z
        Waypoints[i].x = lo.x

        wpt_form.alt:set(0)
        wpt_form.type:set(0)
        wpt_form.altType:set(0)
        wpt_form.speed:set(0)
        wpt_form.lat:set(0)
        wpt_form.lon:set(0)
        wpt_form.ok:set(0)
        wpt_form.id:set(0)
        WPT_SIZE:set(#Waypoints)
        if param_h.TARGET_WPT[2] == 0 then
            param_h.TARGET_WPT[2] = 1
        end
    end
end

function UpdateWaypoint()
    param_h.TARGET_WPT[2] = target_wpt_handle:get()
    updateWpt()--处理航路点在HSI的缩放影响
    updateRoute()--计算ETA和航路点坐标
    autoNextWpt()--自动切换下一个航路点
    syncWpt()
    local unit_h = get_param_handle('UNIT')
    if type(unit_h)~="nil" then
        unit=unit_h:get()
    end
    param_h.TARGET_WPT_tbl[2] = param_h.TARGET_WPT[2] - routeTblOffset
    for k, v in pairs(WPT_POS) do
        for k1, v1 in pairs(v) do
            v1[1]:set(v1[2])
        end
    end
    for k, v in pairs(param_h) do
        if type(v[3])~="nil" then
            if v[2] > v[3] then
                v[3] = Limit(v[3]+v[4],0,1)
            elseif v[2]< v[3] then
                v[3] = Limit(v[3]-v[4],0,1)
            end
            v[1]:set(v[3])
        else
            v[1]:set(v[2])
        end
    end
end

function SetWptIdx(value)
    --param_h.TARGET_WPT[2] = param_h.TARGET_WPT[2] + value
    local newIdx = param_h.TARGET_WPT[2] + value
    if newIdx < 1 then newIdx = 1 end
    if newIdx > #Waypoints then newIdx = #Waypoints end
    SetCurrentWpt(newIdx)
end

function GetTargetWpt()
    if param_h.TARGET_WPT[2] > #Waypoints then
        param_h.TARGET_WPT[2] = 1
    end
    if param_h.TARGET_WPT[2] > 0 and type(Waypoints[param_h.TARGET_WPT[2]])~='nil' then
        local wpt=Waypoints[param_h.TARGET_WPT[2]]
        local myLatM = GPS_LAT_M:get()
        local myLonM = GPS_LON_M:get()
        local target_route_coord = lo_to_geo_coords(wpt.x, wpt.y)
        local target_lat, target_lon = Terrain.convertLatLonToMeters(target_route_coord.lat, target_route_coord.lon)
        local lat_sub = target_lat-myLatM
        local lon_sub = target_lon-myLonM
        if lat_sub == 0 and lon_sub == 0 then
            return 0,0,0,0,string.format('航路点%.0f',param_h.TARGET_WPT[2])
        end
        local offset = 0
        local az,el,range
        range = math.sqrt(lat_sub^2+lon_sub^2)
        az = math.atan2(lon_sub,lat_sub)
        if az < 0 then
            az = math.rad(360)+az
        end
        el = math.atan((wpt.alt - BASE_SENSOR.BAROALT:get())/range)
        return el,az,range,offset,string.format('航路点%.0f',param_h.TARGET_WPT[2])
    end
end