extends Node

## Реализация "Протокол информационного сопряжения
## СПО АСУ изделия «МП‑550» с СПО 5П-28"


const ONLINE_TIMEOUT = 5000  ## Время ожидания пакета от 5П-28, мс
const BUFFER_SIZE    = 2048  ## Размер приёмного буфера для датаграмм

## Тип модуляции для запаковки в пакет для 5П-28
const MOD_TYPES: Array = ['',
                'am',
                'ook',
                'ask',
                'fm',
                'qpsk',
                'oqpsk',
                'pi4qpsk',
                'qam16',
                'qam32',
                'qam64',
                'qam128',
                'qam256',
                'qam512',
                'cpfsk',
                'psk',
                'ofdm',]

class TCP5P28:
    const PACK_TYPE_CMD: int = 1
    const PACK_TYPE_RES: int = 2
    const CMD_TYPE_STATE: int = 1
    const CMD_TYPE_TH: int = 2
    const CMD_TYPE_IN: int = 3
    
    signal line_changed(unit:  TCP5P28)               ## Сигнал состояние связи изменилось
    signal data_received(unit: TCP5P28)               ## Сигнал данные приняты
    signal command_fail(unit:  TCP5P28)               ## Сигнал при не выполнении команды
    signal parse_error(unit:   TCP5P28, msg: String)  ## Сигнал ошибки разбора сообщения
    signal get_threats(unit: TCP5P28)                 ## Сигнал запрос целей

    var online: = false                ## Состояние связи с источником сообщений
    var name: String                   ## Имя экземпляра
    var rx_tick: = 0                   ## Тик время последнего приёма, мс
    var tx_data: = PackedByteArray()   ## Пакет для отправки
    var tx_stack: Array                ## Массив пакетов для отправки

    func _to_string() -> String: return String('капсрпб: "%s" %s ' % [self.name, ['отключен', 'на связи'][int(online)]])
    func _init(nm): self.name = nm

    func parse(data: PackedByteArray, now_tick: int):
        rx_tick = now_tick
        if not online:
            online = true
            emit_signal('line_changed', self)
        var pack_type = data.decode_u8(2)
        if pack_type == PACK_TYPE_CMD:
            var cmd_type = data.decode_u8(8)
            if cmd_type == CMD_TYPE_TH:
                emit_signal('get_threats')

    func process(now_tick: int):
        if online and ((now_tick - rx_tick) > ONLINE_TIMEOUT):
            online = false
            emit_signal('line_changed', self)
        if len(tx_stack):
            tx_data = tx_stack[0]
            tx_stack.remove_at(0)
            return Error.OK
        else:
            return Error.ERR_UNAVAILABLE
    
    func pack_threats(threats):
        var data = PackedByteArray()
        var data_len = 4 + len(threats) * 80           # длинна блока данных в байтах
        var tick = Time.get_ticks_msec()
        data.resize(4)
        data.encode_u16(0, 0)                          # Код ошибки
        data.encode_u16(2, len(threats))               # Количество целей
        for th in threats.values():
            data.append_array(get_threats_data(th, int(tick)))
        var data_to_send: PackedByteArray = create_header(data_len)
        data_to_send.append_array(data)
        tx_stack.append(data_to_send)

    func create_header(data_len):
        var head_data = PackedByteArray()
        head_data.resize(8)                            # Размер заголовка
        head_data.encode_u16(0, 0)                     # Код ошибки
        head_data.encode_u8(2, 2)                      # Тип пакета, ответ на запрос
        head_data.encode_u8(3, 5)                      # Версия протокола, всегда 5
        head_data.encode_u32(4, data_len)              # Длинна пакета данных 
        return head_data
        
    func get_threats_data(th, tick):
        var th_data = PackedByteArray()
        th_data.resize(80)                             # Количество байт в массиве
        th_data.encode_u64(0, tick)
        var flags: int = 0                             # с 0 по 7 бит рекомендованная помеха
        flags |= 0 << 8                                # Флаг рекомендуемой помехи
        flags |= int(th.fflags['aoa']) << 9            # Флаг доставерности пеленга
        flags |= int(th.fflags['alt']) << 10           # Флаг доставерности высоты
        flags |= int(th.fflags['power']) << 11         # Флаг доставерности мощности  
        flags |= int(th.fflags['baud']) << 12          # Флаг доставерности ск. мод. 
        flags |= int(th.fflags['slon']) << 13          # Флаг доставерности долготы ст. позиции
        flags |= int(th.fflags['slat']) << 14          # Флаг доставерности широты ст. позиции  
        flags |= int(th.fflags['lon']) << 15           # Флаг доставерности долготы
        flags |= int(th.fflags['lat']) << 16           # Флаг доставерности широты
        flags |= int(0) << 30                          # Флаг признак назначения помехи на цель
        flags |= int(0) << 31                          # Флаг признак назначения опасной цель    
        th_data.encode_u32(8, flags)                   # Флаги
        th_data.encode_u16(12, th.id)                  # Идентификатор объекта
        th_data.encode_u16(14, int(round(th.aoa/0.1))) # Пеленг объекта
        th_data.encode_u16(16, int(th.alt))            # Высота объекта
        th_data.encode_u16(18, int(th.freq))           # Частота объекта
        th_data.encode_u16(20, int(th.width))          # Ширина частотной полосы объекта
        th_data.encode_u8(22, int(th.power))           # Мощность излучения объекта
        var mod = MOD_TYPES.find(th.tmod)
        if mod == -1: mod = 0
        th_data.encode_u8(23, int(mod))                # Тип модуляции
        th_data.encode_u32(24, int(th.baud))            # Скорость модуляции, бод
        to_cp866(th.proto, th_data)
        var c1 = pow(2, 30) / 180.0
        th_data.encode_s32(64, int(th.slon * c1))
        th_data.encode_s32(68, int(th.slat * c1))
        th_data.encode_s32(72, int(th.lon * c1))
        th_data.encode_s32(76, int(th.lat * c1))
        return th_data
    
    func to_cp866(proto: String, th_data):
        var cp866_proto: PackedByteArray = proto.to_ascii_buffer()
        cp866_proto.resize(36)
        for i in len(cp866_proto):
            th_data[28 + i] = cp866_proto[i]