class_name tcp5_p28 extends Node

## Реализация "Протокол информационного сопряжения.
## СПО АСУ изделия МП-550 с СПО 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' ]

const CU_STATE: Dictionary = {
    0: 2,                     # Подготовка к выполнению
    1: 1,                     # Выполняется
    2: 3,                     # Ошибка в команде
    3: 7,                     # Занят выполнением другого ЦУ
    4: 2,                     # Неисправность
    }

class TCP5P28 extends unit.Unit:
    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

    const PR_STATE: Dictionary = {
        'уарэп-эмс':     [1, 1],
        'уарэп-яу07-1н': [9, 8],
        'уарэп-яу07-1в': [5, 4],
        'уарэп-яу07-1к': [13, 12],
        'уарэп-яу07-2н': [10, 9],
        'уарэп-яу07-2в': [6, 5],
        'уарэп-яу07-2к': [14, 13],
        'уарэп-яу07-3н': [11, 10],
        'уарэп-яу07-3в': [7, 6],
        'уарэп-яу07-3к': [15, 14],
        'уарэп-яу07-4н': [12, 11],
        'уарэп-яу07-4в': [8, 7],
        'уарэп-яу07-4к': [16, 15] }

    signal data_sended()
    signal disconnected(rc: Array)
    signal connected(rc: Array)
    signal get_threats(unit: TCP5P28)                 ## Сигнал запрос целей.
    signal get_interfers(unit: TCP5P28)               ## Запрос состояния выполнения целеуказания.

    var uarep_state: int = 1           ## Флаги состояния прибора, 0 бит, состояние ПО
    var uarep_line: int = 3            ## Состояние линии связи с прибором
    var ext_cu: bool = false
    var tx_queue: Array    ## Очередь пакетов для отправки
    var tx_mutex: Mutex    ## Мютекс для очереди передачи
    var thread: Thread     ## Рабочий поток
    var tctl_mutex: Mutex  ## Мютекс для управления потоком
    var tctl_run: bool     ## Управление потоком

    enum STREAM_STATE {
        IDLE,
        CONNECT,
        WAIT,
        RX_START,
        RX_DONE,
        TX_START,
        TX_DONE,
        ERROR
    }


    func _init(nm) -> void:
        super._init(nm)
        ext_cu = ProjectSettings.get_setting('application/config/Внешнее управление', false)
        ProjectSettings.connect('settings_changed', on_setting_changed)
        tx_mutex = Mutex.new()
        tctl_mutex = Mutex.new()
        connect('data_received', on_data_received)
        connect('data_sended', on_data_sended)
        connect('connected', on_connected)
        connect('disconnected', on_disconnected)
        init_state()

    func on_connected(rc: Array):
        log.info('Клиент 5П-28 подключен: %s:%s' % [ rc[0], rc[1]] )

    func on_data_sended(): pass

    func on_data_received(data: PackedByteArray):
        var tick: = Time.get_ticks_msec()
        parse(data, tick)

    func on_disconnected(rc: Array):
        log.info('Клиент 5П-28 отключен: %s:%s' % [ rc[0], rc[1]] )
        if online:
            online = false
            emit_signal('line_changed', self)
        tx_mutex.lock()
        tx_queue.clear()
        tx_mutex.unlock()

    func init_state():
        for key in PR_STATE.keys():
            var unit_pribor = network.get_unit_instance(key)
            unit_pribor.connect('line_changed', Callable(self, 'pribor_line_changed').bind(key))

    func open(host: String, port: int):
        tctl_run = true
        thread = Thread.new()
        thread.start(thread_proc.bind(host, port))

    func close():
        tctl_mutex.lock()
        tctl_run = false
        tctl_mutex.unlock()

    func thread_proc(host: String, port: int):
        var sock = TCPServer.new()
        var stream: = StreamPeerTCP.new()
        var fsm: = STREAM_STATE.CONNECT
        var rx_buff: PackedByteArray
        var rx_bytes: int = 0
        var head_len: int = 8           ## Длина заголовка команды.
        var len_place: int = 4          ## Номер начального байта с длинной блока данных.
        var status: = StreamPeerTCP.Status.STATUS_NONE
        var rc: = sock.listen(port, host)
        var cl_host: String
        var cl_port: int
        while true:
            stream.poll()
            var new_status = stream.get_status()
            if status != new_status:
                status = new_status
                match status:
                    stream.STATUS_NONE:
                        call_deferred('emit_signal', 'disconnected', [cl_host, cl_port])
                        fsm = STREAM_STATE.ERROR
                    stream.STATUS_CONNECTED:
                        cl_host = stream.get_connected_host()
                        cl_port = stream.get_connected_port()
                        call_deferred('emit_signal', 'connected', [cl_host, cl_port])
                    stream.STATUS_ERROR:
                        fsm = STREAM_STATE.ERROR

            if fsm == STREAM_STATE.CONNECT:
                if sock.is_connection_available(): # Проверить, если кто то пытается подключиться
                    stream = sock.take_connection() # Принять соединение
                new_status = stream.get_status()
                if new_status == stream.STATUS_CONNECTED:
                    fsm = STREAM_STATE.WAIT
                else:
                    fsm = STREAM_STATE.ERROR
            elif fsm == STREAM_STATE.WAIT:
                if status == stream.STATUS_CONNECTED:
                    fsm = STREAM_STATE.RX_START
                else:
                    OS.delay_msec(50)
                    fsm = STREAM_STATE.CONNECT
            elif fsm == STREAM_STATE.ERROR:
                stream.disconnect_from_host()
                fsm = STREAM_STATE.WAIT
                OS.delay_msec(1000)
            elif fsm == STREAM_STATE.IDLE:
                OS.delay_msec(50)
                fsm = STREAM_STATE.RX_START
            elif fsm == STREAM_STATE.RX_START:
                rx_buff.clear()
                rx_bytes = 0
                var sz = stream.get_available_bytes()
                if sz <= 0:
                    fsm = STREAM_STATE.TX_START
                    continue
                var rxd = stream.get_data(head_len)
                if rxd[0] == Error.OK:
                    rx_buff.append_array(rxd[1])
                else:
                    fsm = STREAM_STATE.ERROR
                    continue
                var pay: = rx_buff.decode_u32(len_place)
                rx_bytes = pay + head_len
                if rx_buff.size() == rx_bytes:
                    fsm = STREAM_STATE.RX_DONE
                else:
                    rxd = stream.get_data(rx_bytes - rx_buff.size())
                    rc = rxd[0]
                    if rc == Error.OK:
                        rx_buff.append_array(rxd[1])
                        fsm = STREAM_STATE.RX_DONE
                    else:
                        fsm = STREAM_STATE.ERROR
            elif fsm == STREAM_STATE.RX_DONE:
                var rx_data: = rx_buff.duplicate()
                call_deferred('emit_signal', 'data_received', rx_data)
                fsm = STREAM_STATE.TX_START
            elif fsm == STREAM_STATE.TX_START:
                tx_mutex.lock()
                if not tx_queue.size():
                    fsm = STREAM_STATE.IDLE
                    tx_mutex.unlock()
                    continue
                var data = tx_queue.pop_front()
                tx_mutex.unlock()
                rc = stream.put_data(data)
                if rc == Error.OK:
                    fsm = STREAM_STATE.TX_DONE
                else:
                    fsm = STREAM_STATE.ERROR
            elif fsm == STREAM_STATE.TX_DONE:
                call_deferred('emit_signal', 'data_sended')
                fsm = STREAM_STATE.RX_START

    func pribor_line_changed(u, key):
        uarep_state = tools.set_bit(uarep_state, PR_STATE[key][0], u.online)
        uarep_line = tools.set_bit(uarep_line, PR_STATE[key][1], u.online)

    func _to_string() -> String: return String('5П-28: "%s" %s ' % [self.name, ['отключен', 'на связи'][int(online)]])

    func parse(data: PackedByteArray, tick: int):
        rx_tick = 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')
            if cmd_type == CMD_TYPE_STATE:
                emit_signal('get_interfers')
            if cmd_type == CMD_TYPE_IN and ext_cu:
                set_interfer(data)

    func queue_packet(packet: PackedByteArray):
        tx_mutex.lock()
        tx_queue.append(packet)
        tx_mutex.unlock()

    func pack_threats(ths: Dictionary):
        var data = PackedByteArray()
        var data_len = 4 + len(ths) * 80           # длинна блока данных в байтах
        var tick0 = Time.get_ticks_msec()
        data.resize(4)
        data.encode_u16(0, 0)                      # Код ошибки
        data.encode_u16(2, len(ths))               # Количество целей
        for th in ths.values():
            data.append_array(get_threats_data(th, tick0))
        var data_to_send: PackedByteArray = create_header(data_len)
        data_to_send.append_array(data)
        queue_packet(data_to_send)

    func pack_interfers(interfers: Dictionary, ecms: Dictionary):
        var data = PackedByteArray()
        var data_len = 12                                # длинна блока данных в байтах
        data.resize(data_len)
        data.encode_u16(0, 0)                            # Код ошибки
        data.encode_u16(2, interfers.size())             # Количество целей
        data.encode_u32(4, uarep_state)                  # Состояние УА-РЕП
        data.encode_u32(8, uarep_line)                   # Состояние линий связи
        var cu_data: PackedByteArray = []
        cu_data.resize(4)
        for id in interfers.values():
            if not ecms.has(id):
                push_error('не обнаружен ключ: %s, в словаре воздействий' %id)
                continue
            var cu = ecms[id]
            cu_data.encode_u16(0, cu.ispp)
            var state: int = CU_STATE[cu.svk] if CU_STATE.has(cu.svk) else 8
            cu_data.encode_u8(2, state)                    # Состояние выполнения команды
            cu_data.encode_u8(3, 0xFF)                     # Эффективность работы помехи
            data.append_array(cu_data)
        var data_to_send: PackedByteArray = create_header(data_len + 4 * interfers.size())
        data_to_send.append_array(data)
        queue_packet(data_to_send)

    func create_header(data_len: int):
        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: threats.Threat, tick0: int):
        var th_data = PackedByteArray()
        th_data.resize(80)                             # Количество байт в массиве
        th_data.encode_u64(0, tick0)
        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['pow']) << 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.pow))           # Мощность излучения объекта
        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: PackedByteArray):
        var cp866_proto: PackedByteArray = proto.to_ascii_buffer()
        cp866_proto.resize(36)
        for i in cp866_proto.size():
            th_data[28 + i] = cp866_proto[i]

    func set_interfer(data: PackedByteArray):
        var mode = data.decode_u8(9)
        var id = data.decode_u16(10)
        var aoa = float(data.decode_u16(12)) / 10
        var ela = float(data.decode_u16(14)) / 10
        var freq = data.decode_u16(16)
        var width = data.decode_u16(18)
        var interfer_params: Array = [mode, id, aoa, ela, freq, width]
        signaller.emit_signal('interfer_rcv', interfer_params)

    func on_interfer_accept(err: bool):
        var data_tx = PackedByteArray()
        var data_len = 2                                  # длинна блока данных в байтах
        data_tx.resize(data_len)
        data_tx.encode_u16(0, err)                        # Код ошибки
        var data_to_send: PackedByteArray = create_header(data_len)
        data_to_send.append_array(data_tx)
        queue_packet(data_to_send)

    func on_setting_changed():
        ext_cu = ProjectSettings.get_setting('application/config/Внешнее управление', false)
