local THROTTLE_FUNC = 70
local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}

local PARAM_TABLE_KEY = 64
local PARAM_TABLE_PREFIX = "EP_"

-- bind a parameter to a variable
local function bind_param(name)
    local p = Parameter()
    assert(p:init(name), string.format('could not find %s parameter', name))
    return p
end

-- add a parameter and bind it to a variable
local function bind_add_param(name, idx, default_value)
    assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name))
    return bind_param(PARAM_TABLE_PREFIX .. name)
end

assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 8), 'could not add param table')
local ENG_ENABLE  = bind_add_param('ENABLE', 1, 0)
local ENG_THROTTLE_RCIN = bind_add_param('THR_FUNC', 2, 0)
local ENG_THROTTLE_RCIN_MIN = bind_add_param('THR_MIN',3, 1600)
local ENG_STARTER_RCIN = bind_add_param('STR_FUNC', 4, 0)
local ENG_STARTER_RCIN_MIN = bind_add_param('STR_MIN', 5, 1800)
local ENG_THROTTLE_PWM_MIN = bind_add_param('THR_PWM_MIN', 6, 1000)
local ENG_THROTTLE_PWM_MAX = bind_add_param('THR_PWM_MAX', 7, 2000)
local ENG_MESSAGE_INTERVAL = bind_add_param('MSG_INR_MS', 8, 3000)

--- @class EngineInfo
--- @field error_state integer ECU警告状态
--- @field thr_pos integer 节风门开度(%)
--- @field engine_rpm integer 发动机转速
--- @field ch_temp1 integer 1缸缸头温度
--- @field ch_temp2 integer 2缸缸头温度
--- @field eg_temp1 integer 1缸排气温度
--- @field eg_temp2 integer 2缸排气温度
--- @field fuel_pres integer 燃油压力(mbar)
--- @field inj_time_us integer 喷油持续时间(us)
--- @field it_deg integer 点火提前角(°)
--- @field at_tmp integer 环境温度
--- @field at_pres integer 环境压力(mbar)
--- @field ecu_vol integer ECU电源电压
--- @field reserve integer 预留字节
--- @field ecu_in_vol integer ECU内部5V电压
--- @field ecu_tmp integer ECU温度
--- @field fuel_con_rate integer 瞬时油耗(L/h)
--- @field total_fuel_con integer 总消耗量(L/h)
--- @field ecu_index integer ECU批次号
--- @field err_code integer 告警码
--- @field seq integer 序号
local EngineInfo = {}
EngineInfo.__index = EngineInfo

--- 构造一个EngineInfo消息结构体
--- @return EngineInfo
function EngineInfo.new()
    return setmetatable({
        error_state = 0,
        thr_pos = 0,
        engine_rpm = 0,
        ch_temp1 = 0,
        ch_temp2 = 0,
        eg_temp1 = 0,
        eg_temp2 = 0,
        fuel_pres = 0,
        inj_time_us = 0,
        it_deg = 0,
        at_tmp = 0,
        at_pres = 0,
        ecu_vol = 0,
        reserve = 0,
        ecu_in_vol = 0,
        ecu_tmp = 0,
        fuel_con_rate = 0,
        total_fuel_con = 0,
        ecu_index = 0,
        err_code = 0,
        seq = 0,
    }, EngineInfo)
end

--- 解码数据
--- @param data string
--- @return boolean
function EngineInfo:decode(data)
    local format = ">BHHhhhhhHHbHHBBhHHLHB"
    if data:len() < string.packsize(format) then
        return false
    end

---@diagnostic disable-next-line: assign-type-mismatch
    self.error_state ,self.thr_pos ,self.engine_rpm ,self.ch_temp1 ,self.ch_temp2 ,self.eg_temp1 ,self.eg_temp2 ,self.fuel_pres ,self.inj_time_us ,self.it_deg ,self.at_tmp ,self.at_pres ,self.ecu_vol ,self.reserve ,self.ecu_in_vol ,self.ecu_tmp ,self.fuel_con_rate ,self.total_fuel_con ,self.ecu_index ,self.err_code ,self.seq = string.unpack(format, data)
    return true
end

local payload_fixed_size = 39

--- @enum ProtocolParseState
local ProtocolParseState = {
	header1 = 0,
	header2 = 1,
	payload = 2,
	crc     = 3
}

--- @class ProtocolCodec 创建一个协议解析器
--- @field private uart AP_HAL__UARTDriver_ud|nil
--- @field private _payload string
--- @field private _crc integer
--- @field private _write_seq integer
--- @field private _state ProtocolParseState
local ProtocolCodec = {
    uart = nil,
    _write_seq = 0,
	_payload = "",
	_crc = 0,
	_state = ProtocolParseState.header1
}

--- 解析一个byte数据，返回数据为payload
--- @param byte integer
--- @return string|nil
function ProtocolCodec:parse(byte)
	local state = ProtocolCodec._state

	if state == ProtocolParseState.header1 then
		if byte ~= 0xAA then
			return nil
		end

		ProtocolCodec._crc = 0
		ProtocolCodec._payload = ""
		ProtocolCodec._state = ProtocolParseState.header2
	elseif state == ProtocolParseState.header2 then
		if byte ~= 0x55 then
			ProtocolCodec._state = ProtocolParseState.header1
			return nil
		end

		ProtocolCodec._state = ProtocolParseState.payload
	elseif state == ProtocolParseState.payload then
		ProtocolCodec._payload = ProtocolCodec._payload .. string.char(byte)
		ProtocolCodec._crc = ProtocolCodec._crc + byte
		if ProtocolCodec._payload:len() == payload_fixed_size then
			ProtocolCodec._state = ProtocolParseState.crc
		end
	elseif state == ProtocolParseState.crc then
		local crc = ProtocolCodec._crc & 0xFF
		if byte ~= crc then
			ProtocolCodec._state = ProtocolParseState.header1
			return
		end
		ProtocolCodec._state = ProtocolParseState.header1
		return ProtocolCodec._payload
	end

	return nil
end

--- 解析一个byte数据，返回数据Message
--- @return EngineInfo|nil
function ProtocolCodec:decode_engine_info()
    local uart = self.uart
    if uart == nil then
        return nil
    end

    local nbytes = uart:available()
    local n = nbytes:toint()

    for i = 1, n do
        local b = uart:read()
        if b < 0 then
            gcs:send_text(MAV_SEVERITY.CRITICAL, "could not read byte from UART")
            return nil
        end

        local proto = self:parse(b)
        if proto ~= nil then
            local ei = EngineInfo.new()
            if ei:decode(proto) then
                return ei
            end
        end
    end
    return nil
end

--- 编码一个节风门开度
--- @param value number range [0.0, 100.0]
--- @param starter_status integer [0/1]
--- @return string data, string crc
function ProtocolCodec:encode_throttle_position(value, starter_status)
    local seq = self._write_seq
    self._write_seq = (seq + 1) & 0xff

    local ival = math.floor(value * 10)
    local datas = string.pack(">BBHBHBB", 0x55, 0xAA, ival, starter_status, 0, 0, seq)

    local msgcrc = 0
    for i = 3, 9 do
        local b = datas:byte(i)
        msgcrc = msgcrc + b
    end
    msgcrc = msgcrc & 0xff
    return datas, string.pack(">B", msgcrc)
end

--- 状态
--- @enum StarterStatus
local StarterStatus = {
    Stopped = 0,
    Starting = 1,
    Working = 2
}

--- 当前启动机状态
--- @type StarterStatus
local engine_starter_current_status = StarterStatus.Stopped

--- 获取下一个更新状态
--- @return boolean
local function engine_starter_rcin_status()
    local rc_func = ENG_STARTER_RCIN:get() or 0
    if rc_func < 1 then
        return false
    end

    local rc_min = ENG_STARTER_RCIN_MIN:get() or 1800

    local pwm = SRV_Channels:get_output_pwm(rc_func)
    if not pwm then
        return false
    end

    if pwm >= rc_min then
        return true
    end
    return false
end

--- 发送到启动机为一的数量
--- @type integer
local engine_starter_counter = 0

--- 获取启动机下一个发送状态
--- @param started boolean
--- @return integer
local function engine_starter_next_status(started)
    if not arming:is_armed() or not started or engine_starter_current_status ~= StarterStatus.Stopped then
        if engine_starter_counter ~= 0 then
            engine_starter_counter = 0
        end
        return 0
    end

    local cnt = engine_starter_counter
    if cnt == 0 then
        gcs:send_text(MAV_SEVERITY.EMERGENCY, "EP Engine: starting engine")
    end

    if cnt == 20 then
        engine_starter_counter = 0
        return 0
    end

    engine_starter_counter = cnt + 1
    return 1
end


--- degree Celsius to kelvins
--- @param t number
--- @return number
local function c_to_kelvins(t)
    return t + 273.15
end

--- @type AP_EFI_Backend_ud|nil
local efi_backend = nil

--- if true then send SRV(THROTTLE_FUNC) value to engine, if false then send zero to engine
--- @type boolean
local engine_throttle_open = true

--- update efi state
--- @param ei EngineInfo
--- @return boolean
local function update_efi_state(ei)
    if efi_backend == nil then
        return false
    end

    local efi_state = EFI_State()
    local cylinder_status = Cylinder_Status()

    efi_state:engine_speed_rpm(uint32_t(ei.engine_rpm))

    --- set cylinder head temperature(1), degree Celsius to kelvins
    cylinder_status:cylinder_head_temperature(c_to_kelvins(ei.ch_temp1 * 0.1))

    --- set cylinder headtemperature(2) to exhaust_gas_temperature, degree Celsius to kelvins
    cylinder_status:exhaust_gas_temperature(c_to_kelvins(ei.ch_temp2 * 0.1))

    --- pressure mbar to kpa
    efi_state:fuel_pressure(ei.fuel_pres * 0.1)

    --- set ecu voltage to ignition_voltage
    efi_state:ignition_voltage(ei.ecu_vol * 0.1)

    --- set atmospheric pressure mbar to kpa
    efi_state:atmospheric_pressure_kpa(ei.at_pres * 0.1)

    --- set real(L) to cm3, real(L) = total_fuel_con(L) * 0.1
    efi_state:estimated_consumed_fuel_volume_cm3(ei.total_fuel_con * 100.0)

    --- real = fuel_con_rate(L/h) * 0.1, value(cm3/min) = real(L/h) * 60 * 1000
    --- value(cm3/min) = fuel_con_rate(L/h) * 0.1 * 60 * 1000.0
    efi_state:fuel_consumption_rate_cm3pm(ei.fuel_con_rate / 60 * 100.0)

    --- real = thr_pos * 0.1
    efi_state:throttle_position_percent(math.floor(ei.thr_pos * 0.1))

    efi_state:cylinder_status(cylinder_status)
    efi_state:last_updated_ms(millis())

    efi_backend:handle_scripting(efi_state)
    return true
end

--- @type integer[]
local error_last_sent_time_table = {}

--- @type integer
local error_current_time = 0

---发送gcs消息
---@param i integer
---@param level integer
---@param msg string
local function gcs_send_text(i, level, msg)
    local interval = ENG_MESSAGE_INTERVAL:get() or 3000
    local t = error_last_sent_time_table[i]
    if t == nil or (error_current_time - t >= interval) then
        error_last_sent_time_table[i] = error_current_time
        gcs:send_text(level, msg)
    end
end

--- send engine error text to gcs
--- @param ei EngineInfo
local function send_engine_error_to_gcs(ei)
    local err_code = ei.err_code
    engine_starter_current_status = (err_code >> 9) & 3

    if not ei.error_state then
        return
    end

    --- 更新当前时间
    error_current_time = millis():toint()

    if (err_code & 0x0001) ~= 0 then
        gcs_send_text(1, MAV_SEVERITY.WARNING, "Cylinder Head Temperature is too Low")
    end

    if (err_code & 0x0002) ~= 0 then
        gcs_send_text(2, MAV_SEVERITY.WARNING, "Cylinder Head Temperature is too High")
    end

    if (err_code & 0x0004) ~= 0 then
        gcs_send_text(3, MAV_SEVERITY.WARNING, "Cylinder Head Temperature excessive difference")
    end

    if (err_code & 0x0008) ~= 0 then
        gcs_send_text(4, MAV_SEVERITY.WARNING, "ECU Voltage too Low")
    end

    if (err_code & 0x0010) ~= 0 then
        gcs_send_text(5, MAV_SEVERITY.WARNING, "ECU Voltage too High")
    end

    if (err_code & 0x0020) ~= 0 then
        gcs_send_text(6, MAV_SEVERITY.WARNING, "Fuel Pressure too Low")
    end

    if (err_code & 0x0040) ~= 0 then
        gcs_send_text(7, MAV_SEVERITY.WARNING, "Fuel Pressure too High")
    end

    if (err_code & 0x0080) ~= 0 then
        gcs_send_text(8, MAV_SEVERITY.WARNING, "ECU Temperature too High")
    end

    if (err_code & 0x0100) ~= 0 then
        gcs_send_text(9, MAV_SEVERITY.WARNING, "Engine Communication Failed")
    end

    if (err_code & 0x0200) ~= 0 then
        gcs_send_text(10, MAV_SEVERITY.WARNING, "Engine Throttle Malfunction")
    end
end


local function uart_discard()
    local uart = ProtocolCodec.uart
    if uart == nil then
        return
    end

    local nbytes = uart:available()
    local n = nbytes:toint()
    for i = 1, n do
        uart:read()
    end
end

--- update engine info and set efi
local function update_engine_info()
    local engine_info = ProtocolCodec:decode_engine_info()
    if engine_info ~= nil then
        update_efi_state(engine_info)
        send_engine_error_to_gcs(engine_info)
    end
end

local last_sent_engine_throttle_time = millis():toint()

--- send vehicle throttle value to engine
local function update_engine_throttle()
    local uart = ProtocolCodec.uart
    if not uart then
        return
    end

    local now = millis():toint()
    if now - last_sent_engine_throttle_time < 50 then
        return
    end
    last_sent_engine_throttle_time = now

    local percent = 0
    --- if engine throttle status open, read THROTTLE_FUNC value for send
    if engine_throttle_open then
        local pwm = SRV_Channels:get_output_pwm(THROTTLE_FUNC)
        if pwm ~= nil then
            local min_pwm = ENG_THROTTLE_PWM_MIN:get() or 1000
            local max_pwm = ENG_THROTTLE_PWM_MAX:get() or 2000
            local rpwm = math.min(math.max(pwm, min_pwm), max_pwm)
            percent = (((rpwm - min_pwm) * 1.0) / ((max_pwm - min_pwm) * 1.0)) * 100.0
        else
            gcs:send_text(MAV_SEVERITY.WARNING, "EP Engine: could not read throttle pwm from function")
        end
    end

    local rcin_status = engine_starter_rcin_status()
    local next_status = engine_starter_next_status(rcin_status)
    local data, crc = ProtocolCodec:encode_throttle_position(percent, next_status)
    for i = 1, data:len() do
        if uart:write(data:byte(i)) < 0 then
            return
        end
    end

    uart:write(crc:byte(1))
end

--- update engine throttle send status
local function update_engine_throttle_status()
    local THROTTLE_RC_FUNC = ENG_THROTTLE_RCIN:get() or 0
    if THROTTLE_RC_FUNC < 1 then
        engine_throttle_open = true
        return
    end

    local PWM_MIN = ENG_THROTTLE_RCIN_MIN:get() or 1500

    local percent = SRV_Channels:get_output_pwm(THROTTLE_RC_FUNC)
    if percent == nil then
        engine_throttle_open = false
        gcs:send_text(MAV_SEVERITY.WARNING, "EP Engine: Could not get pwm from throttle lock function")
        return
    end

    --- if RCIN value is 1500 ~ 2000, send servo value to engine
    if percent > PWM_MIN then
        engine_throttle_open = true
    else
        engine_throttle_open = false
    end
end

local function update()
    if ENG_ENABLE:get() < 1 then
        return
    end

    if not efi_backend then
        efi_backend = efi:get_backend(0)
        if efi_backend == nil then
            gcs:send_text(MAV_SEVERITY.ERROR, "EP Engine: Could not get efi backend for update")
            return
        end
    end

    if ProtocolCodec.uart == nil then
        local uart = serial:find_serial(0)
        if uart == nil then
            gcs:send_text(MAV_SEVERITY.ERROR, "EP Engine: Could not find serial (0) for engine io")
            return
        end
        uart:begin(uint32_t(115200))
        ProtocolCodec.uart = uart
        uart_discard()
    end

    update_engine_info()
    update_engine_throttle_status()
    update_engine_throttle()
end

local function protected_wrap()
    local ok, err = pcall(update)
    if not ok then
        gcs:send_text(MAV_SEVERITY.CRITICAL, "Internal Error: " .. err)
        return protected_wrap, 1000
    end
    return protected_wrap, 25
end

gcs:send_text(MAV_SEVERITY.INFO, "EP Engine Drive Script Installed")
return protected_wrap()