extends Node

# ЯУ-07 Блок
var unit = Yau07.YaU07.new('ЯУ-07Б')
var unit_fs = Fs.FGOS.new()
var soc_unicast: Socket
var soc_brodcast: Socket
var ip_address_yau: String = Constants.ADDRESSES[0][1]
var ip_port_yau: int = Constants.ADDRESSES[0][2]
var timeout: float = 10.0
var last_update_time_yau07: float = 0.0
var last_update_time_fs: float = 0.0
var broadcast_packet: PackedByteArray
var state = Constants.STATE.WAIT
var send_array: Array = []

# ФС Блок
var client_fs_1: StreamPeer
var ip_fs_1: String = Constants.ADDRESSES[0][4]
var peerstream_fs_1: PacketPeer
var state_fs_1 = Constants.STATE.IDLE
var command_stack_1: Array
var counter_fs_1: int = 0

## 2 ФС
var client_fs_2: StreamPeer
var ip_fs_2: String = Constants.ADDRESSES[0][5]
var peerstream_fs_2: PacketPeer
var state_fs_2 = Constants.STATE.IDLE
var command_stack_2: Array
var counter_fs_2: int = 0

## 3 ФС
var client_fs_3: StreamPeer
var ip_fs_3: String = Constants.ADDRESSES[0][5]
var peerstream_fs_3: PacketPeer
var state_fs_3 = Constants.STATE.IDLE
var command_stack_3: Array
var counter_fs_3: int = 0

# Яу-07б
signal yau_status_line(status, addr)  ## Вызывается когда меняется состояние связи с ячейкой
signal yau_receive(data_from_yau)
signal yau_feedback_receive(data_from_yau)
signal yau_read_isa(unit_isa_ports)
# ФС модуль
signal data_from_fs_1(data:PackedByteArray)
signal data_from_fs_2(data:PackedByteArray)
signal data_from_fs_3(data:PackedByteArray)
signal port_fs_opened(port_name: String)
signal port_fs_closed(port_name: String)
signal port_error(error_str: String)


## Класс для отправки данных в сокет
class Socket extends PacketPeerUDP:
    func send_to(addr: String, port: int, data: PackedByteArray):
        self.set_dest_address(addr, port)
        self.put_packet(data)


func poll_receive_yau07(sock: Socket) -> bool:  ## Приёмник
    if timeout - last_update_time_yau07 > 0:
        while sock.get_available_packet_count() > 0:
            broadcast_packet = sock.get_packet()
            var addr_receive = sock.get_packet_ip()
            var port_receive = sock.get_packet_port()
            if (ip_address_yau == addr_receive) and (ip_port_yau == port_receive):
                last_update_time_yau07 = 0.0
                unit.parse(broadcast_packet)
                var data_from_yau_07 = unit.status
                emit_signal('yau_receive', data_from_yau_07)
                if  sock == soc_unicast:
                    emit_signal('yau_feedback_receive', data_from_yau_07)
            emit_signal('yau_status_line', addr_receive)
    else:
        if sock.get_available_packet_count():
            last_update_time_yau07 = 0.0
        else:
            emit_signal('yau_status_line', '')
    return false


func poll_receive_fs(client: StreamPeerTCP, peerstream: PacketPeer, ip_fs: String): ## Приемник ФС
    client.poll()
    var peer = peerstream.get_stream_peer()
    # Проверяем, существует ли соединение
    if not peer:
        emit_signal("port_error", ip_fs)
        return
    
    # Проверяем состояние подключения
    if client.get_status() != StreamPeerTCP.STATUS_CONNECTED:
        emit_signal("port_fs_closed", ip_fs)
        return
        
    while peer.get_available_bytes() > 0:
        var rx_data: PackedByteArray
        rx_data.append_array(peer.get_data(peer.get_available_bytes())[1])
        if client == client_fs_1:
            emit_signal("data_from_fs_1", rx_data)
            emit_signal("port_fs_opened", ip_fs)
            if state_fs_1 == Constants.STATE.SEND:
                state_fs_1 = Constants.STATE.DONE
        elif client == client_fs_2:
            emit_signal("data_from_fs_2", rx_data)
            emit_signal("port_fs_opened", ip_fs)
            if state_fs_2 == Constants.STATE.SEND:
                state_fs_2 = Constants.STATE.DONE
        elif client == client_fs_3:
            emit_signal("data_from_fs_3", rx_data)
            emit_signal("port_fs_opened", ip_fs)
            if state_fs_3 == Constants.STATE.SEND:
                state_fs_3 = Constants.STATE.DONE


func _ready() -> void:
    # Привязка для принятия широковещательного канала
    soc_brodcast = Socket.new()
    var rc = soc_brodcast.bind(Constants.BROADCAST_PORT, '*')
    if rc != OK:
        print_debug('Ошибка: Неудачная привязка широковещательного адреса')

    soc_unicast = Socket.new()
    rc = soc_unicast.bind(Constants.UNICAST_PORT, Constants.UNICAST_ADDRESS)
    if rc != OK:
        print_debug('Ошибка: неудачная привязка адреса: ', Constants.UNICAST_ADDRESS)

    unit.connect('command_fail', Callable(self, 'on_command_fail'))
    set_config()


func set_config():
    write_port_isa(Constants.BASE_PORTS.EMS_G+2, Constants.META.CI_2_B_UF)
    read_port_isa([Constants.BASE_PORTS.EMS_G+8])
    write_port_isa(Constants.BASE_PORTS.EMS_G+2, Constants.META.CI_3_B_UF)
    read_port_isa([Constants.BASE_PORTS.EMS_G+8])
    write_port_isa(Constants.BASE_PORTS.EMS_G+2, Constants.META.MODUL_UM_2)
    read_port_isa([Constants.BASE_PORTS.EMS_G+8])
    write_port_isa(Constants.BASE_PORTS.EMS_G+2, Constants.META.MODUL_UM_3)
    read_port_isa([Constants.BASE_PORTS.EMS_G+8])
    write_port_isa(Constants.BASE_PORTS.EMS_G+2, Constants.META.FGOZ_3)
    read_port_isa([Constants.BASE_PORTS.EMS_G+8, Constants.BASE_PORTS.UG+6, Constants.BASE_PORTS.UG+8])
    connect_fs_1()
    connect_fs_2()
    connect_fs_3()
    start_work_fs_1(Constants.ADDRESSES[0][0])
    start_work_fs_2(Constants.ADDRESSES[0][0])
    if Constants.CURRENT_MODE == Constants.MODE.PRD_K:
        start_work_fs_3(Constants.ADDRESSES[0][0])


func _process(delta: float) -> void:
    last_update_time_yau07 += delta
    last_update_time_fs += delta

    if client_fs_1:
        poll_receive_fs(client_fs_1, peerstream_fs_1, ip_fs_1)
        state_machine_fs(client_fs_1)
    if client_fs_2:
        poll_receive_fs(client_fs_2, peerstream_fs_2, ip_fs_2)
        state_machine_fs(client_fs_2)
    if client_fs_3:
        poll_receive_fs(client_fs_3, peerstream_fs_3, ip_fs_3)
        state_machine_fs(client_fs_3)

    poll_receive_yau07(soc_brodcast)
    poll_receive_yau07(soc_unicast)

    state_machine_yau07()

    match unit.process(delta):
        Error.OK: soc_unicast.send_to(ip_address_yau, ip_port_yau, unit.tx_data.slice(0, unit.tx_len))
        #Error.FAILED: print_debug('Ошибка отправки данных')


func state_machine_yau07():
    var fl_done = (unit.cmd_state == unit.CmdState.DONE or unit.cmd_state == unit.CmdState.FAIL)
    ## Режим ожидания
    match  state:
        Constants.STATE.WAIT:
            if fl_done and send_array.size():
                    state =  Constants.STATE.READ_ISA if send_array[0][0] == 'rd' else Constants.STATE.WRITE_ISA
        ## Режим чтения из ИСА
        Constants.STATE.READ_ISA:
            if fl_done:
                var ports_read = send_array.pop_front()
                unit.send_isa(unit.CmdCode.READ_ISA, ports_read[1])
                state = Constants.STATE.DONE
        ## Режим записи в ИСА
        Constants.STATE.WRITE_ISA:
            if fl_done:
                var ports_write: Array = send_array.pop_front()
                if ports_write[0] == 'wr':
                    var key = ports_write[1].keys()
                    unit.send_isa(unit.CmdCode.WRITE_ISA, [key[0], ports_write[1][key[0]]])
                state = Constants.STATE.WAIT
        Constants.STATE.DONE:
            if fl_done:
                emit_signal('yau_read_isa', unit.isa_ports)
                state = Constants.STATE.WAIT


# Записать команду в ИСА
func read_port_isa(read_ports: Array) -> void:
    send_array.append(['rd', read_ports])


# Прочитать данные из порта от ИСА
func write_port_isa(port_isa: int, data_isa: int) -> void:
    var write_data: Dictionary = {port_isa: data_isa}
    send_array.append(['wr', write_data])



## Ессли отсутствует подключение к ячейке ЯУ-07Б
func on_command_fail(_unit) -> void:
    print('Отсутствует подключение к ячейке ЯУ-07Б')


func on_command_change_device(meta_device):
    send_array = []
    ip_address_yau = meta_device[0]
    ip_port_yau = meta_device[1]
    ip_fs_1 = meta_device[3]
    ip_fs_2 = meta_device[4]
    ip_fs_3 = meta_device[5]
    connect_fs_1()
    connect_fs_2()
    if Constants.CURRENT_MODE == Constants.MODE.PRD_K:
        connect_fs_3()


func state_machine_fs(client_fs):
    ## State fs machine
    if client_fs == client_fs_1:
        if (state_fs_1 == Constants.STATE.IDLE) or (state_fs_1 == Constants.STATE.DONE):
            if command_stack_1:
                var pack: Array = command_stack_1.pop_front()
                unit_fs.send_fs_data(pack, client_fs_1, peerstream_fs_1)
                state_fs_1 = Constants.STATE.SEND
    if client_fs == client_fs_2:
        if (state_fs_2 == Constants.STATE.IDLE) or (state_fs_2 == Constants.STATE.DONE):
            if command_stack_2:
                var pack: Array = command_stack_2.pop_front()
                unit_fs.send_fs_data(pack, client_fs_2, peerstream_fs_2)
                state_fs_2 = Constants.STATE.SEND
    if client_fs == client_fs_3:
        if (state_fs_3 == Constants.STATE.IDLE) or (state_fs_3 == Constants.STATE.DONE):
            if command_stack_3:
                var pack: Array = command_stack_3.pop_front()
                unit_fs.send_fs_data(pack, client_fs_3, peerstream_fs_3)
                state_fs_3 = Constants.STATE.SEND


# Подключится к модулю ФС
func connect_fs_1():
    if client_fs_1:
        unit_fs.disconnect_fs(client_fs_1)
        state_fs_1 = Constants.STATE.IDLE
        command_stack_1 = []

    client_fs_1 = StreamPeerTCP.new()
    peerstream_fs_1 = PacketPeerStream.new()
    var rc = client_fs_1.connect_to_host(ip_fs_1, Constants.PORT_FS)
    if rc!= Error.OK:
        emit_signal("port_error", ip_fs_1)
    else:
        peerstream_fs_1.set_stream_peer(client_fs_1)


func start_work_fs_1(currecnt_mode_att):
    counter_fs_1 = 0
    unit_fs.start_session(counter_fs_1, client_fs_1)
    counter_fs_1 += 1
    unit_fs.set_gen_state(false, counter_fs_1, client_fs_1)
    counter_fs_1 += 1
    unit_fs.set_gen_state(true, counter_fs_1, client_fs_1)
    counter_fs_1 += 1
    unit_fs.get_gen_state(counter_fs_1, client_fs_1)
    counter_fs_1 += 1
    
    var set_freq = Constants.FS_FREQ.f2 if Constants.CURRENT_MODE == Constants.MODE.PRD_H else Constants.FS_FREQ.f4 if Constants.CURRENT_MODE == Constants.MODE.PRD_B else Constants.FS_FREQ.f6
    unit_fs.set_carrier(set_freq * 1_000_000, counter_fs_1, client_fs_1)
    counter_fs_1 += 1

    unit_fs.set_att_batch(Constants.FS_ATT[currecnt_mode_att][0], counter_fs_1, client_fs_1)
    counter_fs_1 += 1
    unit_fs.set_base_can_macro(set_freq, counter_fs_1, client_fs_1)
    counter_fs_1 += 1
    
    
    unit_fs.get_att_batch(counter_fs_1, client_fs_1)
    counter_fs_1 += 1
    unit_fs.get_carrier(counter_fs_1, client_fs_1)
    counter_fs_1 += 1
    unit_fs.get_base_current_temp(counter_fs_1, client_fs_1)
    counter_fs_1 += 1
    unit_fs.get_base_can_macro(counter_fs_1, client_fs_1)
    counter_fs_1 += 1


func set_fs_gen_status(state_gen):
    unit_fs.set_gen_state(state_gen, counter_fs_1, client_fs_1)
    counter_fs_1 += 1


func get_gen_state_1():
    unit_fs.get_gen_state(counter_fs_1, client_fs_1)
    counter_fs_1 += 1
    unit_fs.get_base_current_temp(counter_fs_1, client_fs_1)
    counter_fs_1 += 1


func set_fs_ferq_carrier(freq: int):
    unit_fs.set_carrier(freq, counter_fs_1, client_fs_1)
    counter_fs_1 += 1
    unit_fs.get_base_current_temp(counter_fs_1, client_fs_1)
    counter_fs_1 += 1
    unit_fs.get_carrier(counter_fs_1, client_fs_1)
    counter_fs_1 += 1


func set_fs_attenuation(attenuation: float):
    unit_fs.set_att_batch(attenuation, counter_fs_1, client_fs_1)
    counter_fs_1 += 1
    unit_fs.get_att_batch(counter_fs_1, client_fs_1)
    counter_fs_1 += 1
    unit_fs.get_base_current_temp(counter_fs_1, client_fs_1)
    counter_fs_1 += 1


func set_can_macro(can_frequency: int):
    unit_fs.set_base_can_macro(can_frequency, counter_fs_1, client_fs_1)
    counter_fs_1 += 1
    unit_fs.get_base_current_temp(counter_fs_1, client_fs_1)
    counter_fs_1 += 1
    unit_fs.get_base_can_macro(counter_fs_1, client_fs_1)
    counter_fs_1 += 1


func status_can_macro_1(state_can: bool):
    unit_fs.status_can_macro_exec(state_can, counter_fs_1, client_fs_1)
    counter_fs_1 += 1
    

func set_configure_gen_msk_1():
    unit_fs.set_configure_state_msk_modul_1(counter_fs_1, client_fs_1)
    counter_fs_1 += 1
    unit_fs.set_configure_state_msk_modul_2(counter_fs_1, client_fs_1)
    counter_fs_1 += 1


func set_configure_gen_const_1():
    unit_fs.set_configure_state_msk_const_1(counter_fs_1, client_fs_1)
    counter_fs_1 += 1


## 2 ФС коннект
func connect_fs_2():
    if client_fs_2:
        unit_fs.disconnect_fs(client_fs_2)
        state_fs_2 = Constants.STATE.IDLE
        command_stack_2 = []

    client_fs_2 = StreamPeerTCP.new()
    peerstream_fs_2 = PacketPeerStream.new()
    var rc = client_fs_2.connect_to_host(ip_fs_2, Constants.PORT_FS)
    if rc!= Error.OK:
        emit_signal("port_error", ip_fs_2)
    else:
        peerstream_fs_2.set_stream_peer(client_fs_2)


func start_work_fs_2(currecnt_mode_att):
    counter_fs_2 = 0
    unit_fs.start_session(counter_fs_2, client_fs_2)
    counter_fs_2 += 1
    unit_fs.set_gen_state(false, counter_fs_2, client_fs_2)
    counter_fs_2 += 1
    unit_fs.set_gen_state(true, counter_fs_2, client_fs_2)
    counter_fs_2 += 1
    unit_fs.set_att_batch(19.0, counter_fs_2, client_fs_2)
    counter_fs_2 += 1
    unit_fs.get_gen_state(counter_fs_2, client_fs_2)
    counter_fs_2 += 1
    
    var set_freq = Constants.FS_FREQ.f3 if Constants.CURRENT_MODE == Constants.MODE.PRD_H else Constants.FS_FREQ.f5 if Constants.CURRENT_MODE == Constants.MODE.PRD_B else Constants.FS_FREQ.f7
    unit_fs.set_carrier(set_freq * 1_000_000, counter_fs_2, client_fs_2)
    counter_fs_2 += 1
    
    unit_fs.set_att_batch(Constants.FS_ATT[currecnt_mode_att][1], counter_fs_2, client_fs_2)
    counter_fs_2 += 1
    unit_fs.set_base_can_macro(set_freq, counter_fs_2, client_fs_2)
    counter_fs_2 += 1
    
    unit_fs.get_att_batch(counter_fs_2, client_fs_2)
    counter_fs_2 += 1
    unit_fs.get_carrier(counter_fs_2, client_fs_2)
    counter_fs_2 += 1
    unit_fs.get_base_current_temp(counter_fs_2, client_fs_2)
    counter_fs_2 += 1
    unit_fs.get_base_can_macro(counter_fs_2, client_fs_2)
    counter_fs_2 += 1


func set_fs_gen_status_2(state_gen: bool):
    unit_fs.set_gen_state(state_gen, counter_fs_2, client_fs_2)
    counter_fs_2 += 1


func get_gen_state_2():
    unit_fs.get_gen_state(counter_fs_2, client_fs_2)
    counter_fs_2 += 1
    unit_fs.get_base_current_temp(counter_fs_2, client_fs_2)
    counter_fs_2 += 1


func set_fs_ferq_carrier_2(freq: int):
    unit_fs.set_carrier(freq, counter_fs_2, client_fs_2)
    counter_fs_2 += 1
    unit_fs.get_base_current_temp(counter_fs_2, client_fs_2)
    counter_fs_2 += 1
    unit_fs.get_carrier(counter_fs_2, client_fs_2)
    counter_fs_2 += 1


func set_fs_attenuation_2(attenuation: float):
    unit_fs.set_att_batch(attenuation, counter_fs_2, client_fs_2)
    counter_fs_2 += 1
    unit_fs.get_att_batch(counter_fs_2, client_fs_2)
    counter_fs_2 += 1
    unit_fs.get_base_current_temp(counter_fs_2, client_fs_2)
    counter_fs_2 += 1


## Установка CAN частоты для подстройки фильтра ФС
func set_can_macro_2(can_frequency: int):
    unit_fs.set_base_can_macro(can_frequency, counter_fs_2, client_fs_2)
    counter_fs_2 += 1
    unit_fs.get_base_current_temp(counter_fs_2, client_fs_2)
    counter_fs_2 += 1
    unit_fs.get_base_can_macro(counter_fs_2, client_fs_2)
    counter_fs_2 += 1


## Установка CAN статуса(вкл/откл) для подстройки фильтра ФС
func status_can_macro_2(state_can: bool):
    unit_fs.status_can_macro_exec(state_can, counter_fs_2, client_fs_2)
    counter_fs_2 += 1


## Установка модуляции msk на генератор ФС
func set_configure_gen_msk_2():
    unit_fs.set_configure_state_msk_modul_1(counter_fs_2, client_fs_2)
    counter_fs_2 += 1
    unit_fs.set_configure_state_msk_modul_2(counter_fs_2, client_fs_2)
    counter_fs_2 += 1


## Установка модуляции const на генератор ФС
func set_configure_gen_const_2():
    unit_fs.set_configure_state_msk_const_1(counter_fs_2, client_fs_2)
    counter_fs_2 += 1


## 3 ФС коннект
func connect_fs_3():
    if client_fs_3:
        unit_fs.disconnect_fs(client_fs_3)
        state_fs_3 = Constants.STATE.IDLE
        command_stack_3 = []

    client_fs_3 = StreamPeerTCP.new()
    peerstream_fs_3 = PacketPeerStream.new()
    var rc = client_fs_3.connect_to_host(ip_fs_3, Constants.PORT_FS)
    if rc!= Error.OK:
        emit_signal("port_error", ip_fs_3)
    else:
        peerstream_fs_3.set_stream_peer(client_fs_3)


func start_work_fs_3(currecnt_mode_att):
    counter_fs_3 = 0
    unit_fs.start_session(counter_fs_3, client_fs_3)
    counter_fs_3 += 1
    unit_fs.set_gen_state(false, counter_fs_3, client_fs_3)
    counter_fs_3 += 1
    unit_fs.set_gen_state(true, counter_fs_3, client_fs_3)
    counter_fs_3 += 1
    unit_fs.get_gen_state(counter_fs_3, client_fs_3)
    counter_fs_3 += 1
    
    var set_freq = Constants.FS_FREQ.f1
    unit_fs.set_carrier(set_freq * 1_000_000, counter_fs_3, client_fs_3)
    counter_fs_3 += 1
    
    unit_fs.set_att_batch(Constants.FS_ATT[currecnt_mode_att][2], counter_fs_3, client_fs_3)
    counter_fs_3 += 1
    
    unit_fs.set_base_can_macro(set_freq, counter_fs_3, client_fs_3)
    counter_fs_3 += 1
    
    unit_fs.get_att_batch(counter_fs_3, client_fs_3)
    counter_fs_3 += 1
    unit_fs.get_carrier(counter_fs_3, client_fs_3)
    counter_fs_3 += 1
    unit_fs.get_base_current_temp(counter_fs_3, client_fs_3)
    counter_fs_3 += 1
    unit_fs.get_base_can_macro(counter_fs_3, client_fs_3)
    counter_fs_3 += 1
    

func set_fs_gen_status_3(state_gen: bool):
    unit_fs.set_gen_state(state_gen, counter_fs_3, client_fs_3)
    counter_fs_3 += 1


func get_gen_state_3():
    unit_fs.get_gen_state(counter_fs_3, client_fs_3)
    counter_fs_3 += 1
    unit_fs.get_base_current_temp(counter_fs_3, client_fs_3)
    counter_fs_3 += 1


func set_fs_ferq_carrier_3(freq: int):
    unit_fs.set_carrier(freq, counter_fs_3, client_fs_3)
    counter_fs_3 += 1
    unit_fs.get_base_current_temp(counter_fs_3, client_fs_3)
    counter_fs_3 += 1
    unit_fs.get_carrier(counter_fs_3, client_fs_3)
    counter_fs_3 += 1


func set_fs_attenuation_3(attenuation: float):
    unit_fs.set_att_batch(attenuation, counter_fs_3, client_fs_3)
    counter_fs_3 += 1
    unit_fs.get_att_batch(counter_fs_3, client_fs_3)
    counter_fs_3 += 1
    unit_fs.get_base_current_temp(counter_fs_3, client_fs_3)
    counter_fs_3 += 1


## Установка CAN частоты для подстройки фильтра ФС
func set_can_macro_3(can_frequency: int):
    unit_fs.set_base_can_macro(can_frequency, counter_fs_3, client_fs_3)
    counter_fs_3 += 1
    unit_fs.get_base_current_temp(counter_fs_3, client_fs_3)
    counter_fs_3 += 1
    unit_fs.get_base_can_macro(counter_fs_3, client_fs_3)
    counter_fs_3 += 1


## Установка CAN статуса(вкл/откл) для подстройки фильтра ФС
func status_can_macro_3(state_can: bool):
    unit_fs.status_can_macro_exec(state_can, counter_fs_3, client_fs_3)
    counter_fs_3 += 1


## Установка модуляции msk на генератор ФС
func set_configure_gen_msk_3():
    unit_fs.set_configure_state_msk_modul_3(counter_fs_3, client_fs_3)
    counter_fs_3 += 1
    unit_fs.set_configure_state_msk_modul_3(counter_fs_3, client_fs_3)
    counter_fs_3 += 1


## Установка модуляции const на генератор ФС
func set_configure_gen_const_3():
    unit_fs.set_configure_state_msk_const_1(counter_fs_3, client_fs_3)
    counter_fs_3 += 1