class_name navigation extends Node

## "Протокол №120-11435-09.PDF"

const ONLINE_TIMEOUT: = 5000  ## Время ожидания пакета, миллисекунды
const pow_2_30:       = pow(2.0, 30.0)
const FRACT_MAX: int = 3

## Источник данных навигации
class NaviSource extends unit.Unit:
    var cmd_tick_rx: int = 0
    var fract: int = 0
    var data: = {}         ## Принятые данные
    var source_addr: = []  ## Адрес отправителя
    const akng_mode = {0:'тестовый', 1:'основной', 2:'резервный', 3:'аварийный'}
    static var latlon: Vector2 ## Текущие координаты
    func _to_string() -> String:
        return String('NaviSource: %s %s' % [self.name, 'на связи' if online else 'отключен'])
    func _init(nm):
        self.name = nm
    func process(tick: int):
        if online and ((tick - cmd_tick_rx) > ONLINE_TIMEOUT):
            online = false
            emit_signal('line_changed', self)
            source_addr.clear()

    func parse(rx_data: PackedByteArray, tick: int) -> bool:
        cmd_tick_rx = tick
        if not online:
            online = true
            emit_signal('line_changed', self)
        if rx_data.size() < 32:
            emit_signal('parse_failed', self)
            return false
        var flags: = rx_data.decode_u16(28)  # Флаги достоверности полей структуры
        data.clear()
        data['mode'] = akng_mode[(flags >> 4) & 3]  # Режим работы АКНГ
        if flags & (1 << 0):
            data['kk'] = rx_data.decode_float(0)  # Килевая качка, градусы
            data['bk'] = rx_data.decode_float(4)  # Бортовая качка, градусы
        if flags & (1 << 1):
            data['k'] = rx_data.decode_float(8)   # Курс, градусы (0.0 - Север)
        if flags & (1 << 2):
            latlon.x = float(rx_data.decode_s32(12) * 180) / pow_2_30   # Широта, градусы
            latlon.y = float(rx_data.decode_s32(16) * 180) / pow_2_30  # Долгота, градусы
            data['latitude'] = latlon.x
            data['longitude'] = latlon.y
        if flags & (1 << 3):
            data['speed'] = rx_data.decode_float(20)  # Скорость, м/с
        data['err'] = rx_data.decode_u16(24)          #
        if fract == 0:
            emit_signal('data_received', data)
        fract += 1
        fract %= FRACT_MAX
        return true