// SPDX-FileCopyrightText: 2024 Filip Leonarski, Paul Scherrer Institute <filip.leonarski@psi.ch>
// SPDX-License-Identifier: GPL-3.0-only

#include <future>
#include <bitset>
#include <arpa/inet.h>

#include "HLSDevice.h"
#include "hls_cores.h"
#include "datamover_model.h"
#include "sls_packet.h"

struct HLSDeviceImpl {
    AXI_STREAM din_eth;
    AXI_STREAM dout_eth;

    constexpr static const size_t hbm_if_count = 32;
    constexpr static const size_t hbm_if_size = 32 * 1024 * 1024LU;

    std::vector<ap_uint<256>> hbm;

    hls::stream<ap_uint<32> > work_request_stream;
    hls::stream<ap_uint<32> > completion_stream;

    Datamover<512> datamover_in;
    Datamover<512> datamover_out;
    Datamover<256, 4> datamover_in_hbm_0;
    Datamover<256, 4> datamover_in_hbm_1;
    Datamover<256, 4> datamover_in_hbm_2;
    Datamover<256, 4> datamover_out_hbm_0;
    Datamover<256, 4> datamover_out_hbm_1;
    Datamover<256, 4> datamover_out_hbm_2;

    ap_uint<1> run_data_collection;
    ap_uint<1> cancel_data_collection;
    volatile ap_uint<1> host_writer_idle;
    std::vector<uint32_t> pixel_mask;

    HLSDeviceImpl()
            : hbm(hbm_if_size / 32 * hbm_if_count),
              pixel_mask(MAX_MODULES_FPGA*RAW_MODULE_SIZE/32),
              datamover_in(Direction::Input, nullptr),
              datamover_out(Direction::Output, nullptr),
              datamover_out_hbm_0(Direction::Output, (char *) hbm.data()),
              datamover_out_hbm_1(Direction::Output, (char *) hbm.data()),
              datamover_out_hbm_2(Direction::Output, (char *) hbm.data()),
              datamover_in_hbm_0(Direction::Input, (char *) hbm.data()),
              datamover_in_hbm_1(Direction::Input, (char *) hbm.data()),
              datamover_in_hbm_2(Direction::Input, (char *) hbm.data()) {}
};

HLSDevice::HLSDevice(std::vector<DeviceOutput *> &buffer_device) : idle(true), dma_address_table(65536) {
    impl_ = std::make_unique<HLSDeviceImpl>();

    auto in_mem_location32 = (uint32_t *) dma_address_table.data();

    for (int i = 0; i < buffer_device.size(); i++) {
        in_mem_location32[2 * i    ] = ((uint64_t) buffer_device[i]) & UINT32_MAX;
        in_mem_location32[2 * i + 1] = ((uint64_t) buffer_device[i]) >> 32;
    }
}

HLSDevice::~HLSDevice() {
    if (action_thread.joinable())
        action_thread.join();
    impl_.reset();
}

void HLSDevice::CreateFinalPacket() {
    CreateJFPacket(UINT64_MAX, 0, 0, nullptr, false);
}

void HLSDevice::SendPacket(char *buffer, int len, uint8_t user) {
    auto obuff = (ap_uint<512> *)buffer;

    for (int i = 0; i < (len + 63) / 64; i++) {
        packet_512_t packet_in;
        if (i == (len + 63) / 64 - 1) packet_in.last = 1;
        else packet_in.last = 0;
        packet_in.keep = 0xFFFFFFFFFFFFFFFF;
        packet_in.user = user;
        packet_in.data = obuff[i];
        impl_->din_eth.write(packet_in);
    }
}

#pragma GCC push_options
#pragma GCC optimize ("O0")

uint16_t checksum(const uint16_t *addr, size_t count) {
/* Compute Internet Checksum for "count" bytes
 *         beginning at location "addr".
 */
    uint64_t sum = 0;

    for (int i = 0; i < count / 2; i++)
        sum += addr[i];

/*  Add left-over byte, if any */
    if (count % 2 == 1)
        sum += ((uint8_t *) addr)[count / 2];

/*  Fold 32-bit sum to 16 bits */
    while (sum>>16)
        sum = (sum & 0xffff) + (sum >> 16);

    return ~sum;
}

void HLSDevice::CreateJFPacket(uint64_t frame_number, uint32_t eth_packet,
                                        uint32_t module_number, const uint16_t *data, int8_t adjust_axis, uint8_t user) {
    std::vector<char> buff(256*64);

    auto packet = (jf_raw_packet *)buff.data();

    packet->ether_type = htons(0x0800);
    packet->sour_mac[0] = 0x00; // module 0

    uint64_t tmp_mac = mac_addr;
    for (int i = 0; i < 6; i++)
        packet->dest_mac[i] = (tmp_mac >> (8*i)) % 256;

    uint32_t half_module = ((eth_packet >= 64) ? 1 : 0);

    packet->ipv4_header_h = htons(0x4500); // Big endian in IP header!
    packet->ipv4_header_total_length = htons(8268); // Big endian in IP header!
    packet->ipv4_header_dest_ip = ipv4_addr;
    packet->ipv4_header_sour_ip = 0;
    packet->ipv4_header_ttl_protocol = htons(0x0011);
    packet->ipv4_header_checksum = checksum( (uint16_t *) (buff.data() + 14), 20); // checksum is already in network order

    packet->udp_dest_port = htons(2 * module_number + half_module);
    packet->udp_sour_port = htons(0xDFAC);
    packet->udp_length = htons(8248);

    // JF headers are little endian
    packet->jf.detectortype = SLS_DETECTOR_TYPE_JUNGFRAU;
    packet->jf.timestamp = 0xABCDEF0000FEDCBAL;
    packet->jf.bunchid   = 0x1234567898765431L;
    packet->jf.row = half_module;
    packet->jf.column = 0;
    packet->jf.framenum = frame_number;
    packet->jf.packetnum = eth_packet % 64;
    if (data != nullptr) {
        for (int i = 0; i < 4096; i++)
            packet->jf.data[i] = data[i];
    }
    packet->udp_checksum = htons(checksum( (uint16_t *) (buff.data()+42), 8192+48));

    SendPacket(buff.data(), (130+adjust_axis)*64, user);
}

void HLSDevice::CreateEIGERPacket(uint64_t frame_number,
                                           uint32_t eth_packet, uint32_t module_number, uint32_t col, uint32_t row,
                                           const uint16_t *data) {
    std::vector<char> buff(256*64);

    auto packet = (eiger_raw_packet *)buff.data();

    packet->ether_type = htons(0x0800);
    packet->sour_mac[0] = 0x00; // module 0

    uint64_t tmp_mac = mac_addr;
    for (int i = 0; i < 6; i++)
        packet->dest_mac[i] = (tmp_mac >> (8*i)) % 256;

    packet->ipv4_header_h = htons(0x4500); // Big endian in IP header!
    packet->ipv4_header_total_length = htons(4172); // Big endian in IP header!
    packet->ipv4_header_dest_ip = ipv4_addr;
    packet->ipv4_header_sour_ip = 0x0;

    packet->ipv4_header_ttl_protocol = htons(0x0011);
    packet->ipv4_header_checksum = checksum( (uint16_t *) (buff.data() + 14), 20); // checksum is already in network order

    packet->udp_dest_port = htons(2 * module_number + row);
    packet->udp_sour_port = htons(0xDFAC);
    packet->udp_length = htons(4152);

    // JF headers are little endian
    packet->eiger.detectortype = SLS_DETECTOR_TYPE_EIGER;
    packet->eiger.timestamp = 0xABCDEF0000FEDCBAL;
    packet->eiger.bunchid   = 0x1234567898765431L;
    packet->eiger.row = (row % 2);
    packet->eiger.column = col % 2;
    packet->eiger.framenum = frame_number;
    packet->eiger.packetnum = eth_packet % 128;
    if (data != nullptr) {
        for (int i = 0; i < 2048; i++)
            packet->eiger.data[i] = data[i];
    }
    packet->udp_checksum = htons(checksum( (uint16_t *) (buff.data()+42), 4096+48));

    SendPacket(buff.data(), 66*64, 0);
}

#pragma GCC pop_options

void HLSDevice::HW_ReadActionRegister(DataCollectionConfig *job) {
    memcpy(job, &cfg, sizeof(DataCollectionConfig));
}

void HLSDevice::HW_WriteActionRegister(const DataCollectionConfig *job) {
    memcpy(&cfg, job, sizeof(DataCollectionConfig));
}

void HLSDevice::FPGA_StartAction() {
    if (action_thread.joinable())
        action_thread.join();

    run_counter += 1;
    idle = false;

    impl_->datamover_out.ClearCompletedDescriptors();

    action_thread = std::thread(&HLSDevice::HLSMainThread, this );
}

void HLSDevice::FrameGeneratorFuture(FrameGeneratorConfig config) {
    frame_generator(impl_->din_eth,
                    impl_->hbm.data(),
                    impl_->hbm.data(),
                    impl_->hbm_if_size,
                    mac_addr,
                    ipv4_addr,
                    impl_->cancel_data_collection,
                    config);
}

void HLSDevice::HW_RunInternalGenerator(const FrameGeneratorConfig &config) {
    frame_generator_future = std::async(std::launch::async, &HLSDevice::FrameGeneratorFuture, this, config);
}

void HLSDevice::FPGA_EndAction() {
    if (action_thread.joinable())
        action_thread.join();
}

bool HLSDevice::HW_ReadMailbox(uint32_t *values) {
    std::unique_lock ul(completion_mutex);

    ap_uint<32> tmp;
    bool ret = impl_->completion_stream.read_nb(tmp);
    values[0] = tmp;
    // equivalent to driver functionality
    if (ret) {
        uint32_t data_collection_id = (values[0] >> 16) & 0xFFFF;
        uint32_t handle = values[0] & 0xFFFF;
        if (handle == HANDLE_START)
            completion_count = 0;
        else if ((handle != HANDLE_END) && (data_collection_id != DATA_COLLECTION_ID_PURGE)) {
            completion_count++;
            while (completion_count * DMA_DESCRIPTORS_PER_MODULE > impl_->datamover_out.GetCompletedDescriptors())
                std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }
    }
    return ret;
}

void HLSDevice::Cancel() {
    impl_->cancel_data_collection = 1;
}

bool HLSDevice::HW_IsIdle() const {
    return idle && impl_->datamover_out.IsIdle();
}


bool HLSDevice::HW_SendWorkRequest(uint32_t handle) {
    impl_->work_request_stream.write(handle);
    return true;
}

inline uint32_t float2uint(float f) {
    float_uint32 fu;
    fu.f = f;
    return fu.u;
}

void HLSDevice::HLSMainThread() {
    ap_uint<1> clear_counters = 0;

    uint64_t packets_processed;

    std::vector<std::thread> hls_cores;

    STREAM_512 ip1("ip1"), udp1("udp1"), udp2("udp2"), icmp1("icmp1"), arp1("arp1"), ptp1("ptp1");

    STREAM_768 stream_768_00("stream_768_00");
    STREAM_768 stream_768_01("stream_768_01");
    STREAM_768 stream_768_02("stream_768_02");
    STREAM_768 stream_768_03("stream_768_03");
    STREAM_768 stream_768_04("stream_768_04");
    STREAM_768 stream_768_0("stream_768_0");
    STREAM_768 stream_768_1("stream_768_1");
    STREAM_768 stream_768_2("stream_768_2");
    STREAM_768 stream_768_3("stream_768_3");
    STREAM_768 stream_768_4("stream_768_4");
    STREAM_768 stream_768_4a("stream_768_4a");
    STREAM_768 stream_768_5("stream_768_5");
    STREAM_768 stream_768_6("stream_768_6");
    STREAM_768 stream_768_7("stream_768_7");
    STREAM_768 stream_768_8("stream_768_8");

    STREAM_512 data_0("data_0");
    STREAM_512 data_1("data_1");
    hls::stream<ap_axiu<768, 1, 1, 1>, 2>  data_2("data_2");
    STREAM_768 data_3("data_3");
    STREAM_768 data_4("data_4");
    STREAM_768 data_5("data_5");
    STREAM_768 data_6("data_6");
    STREAM_512 data_7("data_7");
    STREAM_512 data_8("data_8");
    STREAM_512 data_9("data_9");
    STREAM_512 data_10("data_10");
    STREAM_512 data_12("data_12");
    STREAM_512 data_13("data_13");

    hls::stream<axis_addr> addr0("addr0");
    hls::stream<axis_addr> addr1("addr1");
    hls::stream<axis_addr> addr2("addr2");

    hls::stream<axis_completion> axi_compl[13];

    hls::stream<ap_uint<16>> hbm_handles("hbm_handles");
    hls::stream<ap_uint<512>> adu_histo_result("adu_histo_result");

    hls::stream<ap_axiu<64,1,1,1>> integration_result_0("integration_result_0");
    hls::stream<ap_uint<512>> integration_result_1("integration_result_1");

    hls::stream<ap_axiu<32,1,1,1>> spot_finder_result_0("spot_finder_result_0");
    hls::stream<ap_axiu<32,1,1,1>> spot_finder_result_1("spot_finder_result_1");
    hls::stream<ap_axiu<32,1,1,1>> spot_finder_result_0a("spot_finder_result_0a");
    hls::stream<ap_uint<32>> spot_finder_conn_0("spot_finder_conn_0");
    hls::stream<ap_axiu<32,1,1,1>> spot_finder_result_2("spot_finder_result_2");
    hls::stream<ap_uint<512>> spot_finder_result_3("spot_finder_result_3");

    hls::stream<ap_uint<512>> roi_calc_result_0("roi_calc_result_0");
    hls::stream<ap_uint<PIXEL_COUNT_WIDTH>> pixel_count_result_0("pixel_calc_result_0");

    hls::stream<ap_uint<UDP_METADATA_STREAM_WIDTH> > udp_metadata("udp_metadata");

    volatile ap_uint<1> idle_data_collection = 1;
    ap_uint<1> load_to_hbm_idle;
    ap_uint<1> save_to_hbm_idle;
    ap_uint<1> integration_idle;
    ap_uint<1> stream512to768_idle;
    ap_uint<1> stream768to512_idle;
    ap_uint<1> frame_summation_idle;

    volatile bool done = false;
    volatile bool udp_done = false; // done AND udp_idle
    volatile bool sls_done = false; // done AND sls_idle

    // Sent gratuitous ARP message
    arp(arp1, impl_->dout_eth, mac_addr, ipv4_addr, 1);

    volatile rcv_state_t state = RCV_INIT;
    impl_->run_data_collection = 0;

    // Read state, check if ready to start
    data_collection_fsm(data_0, data_1,
                        addr0, addr1,
                        impl_->run_data_collection,
                        impl_->cancel_data_collection,
                        idle_data_collection,
                        cfg.mode,
                        float2uint(cfg.energy_kev),
                        cfg.nframes,
                        cfg.nmodules,
                        cfg.nstorage_cells,
                        cfg.nsummation ,
                        cfg.sqrtmult,
                        cfg.pxlthreshold_min,
                        cfg.pxlthreshold_max,
                        state);

    impl_->run_data_collection = 1;
    impl_->cancel_data_collection = 0;

    if (state != rcv_state_t::RCV_WAIT_FOR_START)
        throw std::runtime_error("HLS device not ready to receive data");

    // Start data collection
    data_collection_fsm(data_0, data_1,
                        addr0, addr1,
                        impl_->run_data_collection,
                        impl_->cancel_data_collection,
                        idle_data_collection,
                        cfg.mode,
                        float2uint(cfg.energy_kev),
                        cfg.nframes,
                        cfg.nmodules,
                        cfg.nstorage_cells,
                        cfg.nsummation ,
                        cfg.sqrtmult,
                        cfg.pxlthreshold_min,
                        cfg.pxlthreshold_max,
                        state);

    impl_->run_data_collection = 0;
    data_collection_fsm(data_0, data_1,
                        addr0, addr1,
                        impl_->run_data_collection,
                        impl_->cancel_data_collection,
                        idle_data_collection,
                        cfg.mode,
                        float2uint(cfg.energy_kev),
                        cfg.nframes,
                        cfg.nmodules,
                        cfg.nstorage_cells,
                        cfg.nsummation,
                        cfg.sqrtmult,
                        cfg.pxlthreshold_min,
                        cfg.pxlthreshold_max,
                        state);

    hls_cores.emplace_back([&] {
        while (!udp_done) {
            if (impl_->din_eth.empty())
                std::this_thread::sleep_for(std::chrono::microseconds(10));
            else
                ethernet(impl_->din_eth, ip1, arp1, ptp1,
                         mac_addr, 1, 0, eth_packets, clear_counters);
        }
    });

    hls_cores.emplace_back([&] {
        while (!udp_done) {
            if (ip1.empty())
                std::this_thread::sleep_for(std::chrono::microseconds(10));
            else
                ipv4(ip1, udp1, icmp1, ipv4_addr, 0);
        }
    });

    hls_cores.emplace_back([&] {
        ap_uint<1> udp_idle = 1;
        while (!done || !udp_idle) {
            if (udp1.empty())
                std::this_thread::sleep_for(std::chrono::microseconds(10));
            else
                udp(udp1, udp2, udp_metadata, udp_packets, clear_counters, udp_idle);
        }
        udp_done = true;
    });

    hls_cores.emplace_back([&] {
        ap_uint<1> sls_idle = 1;

        while (!done || !sls_idle) {
            if (udp2.empty())
                std::this_thread::sleep_for(std::chrono::microseconds (10));
            else
                sls_detector(udp2, udp_metadata, data_0, addr0, sls_packets, udp_eth_err, udp_len_err, current_pulse_id,
                    cfg.data_stream, clear_counters, sls_idle);
        }
        sls_done = true;
    });

    // 1. Parse incoming UDP packets
    hls_cores.emplace_back([&] {
        while ((state != RCV_WAIT_FOR_START) || (idle_data_collection.read() == 0) || (!data_0.empty()))  {
            data_collection_fsm(data_0, data_1,
                                addr0, addr1,
                                impl_->run_data_collection,
                                impl_->cancel_data_collection,
                                idle_data_collection,
                                cfg.mode,
                                float2uint(cfg.energy_kev),
                                cfg.nframes,
                                cfg.nmodules,
                                cfg.nstorage_cells,
                                cfg.nsummation,
                                cfg.sqrtmult,
                                cfg.pxlthreshold_min,
                                cfg.pxlthreshold_max,
                                state);
        }
        done = true;
    });

    hls_cores.emplace_back([&] { stream512to768(data_1, data_2, addr1, addr2, stream512to768_idle);});

    // 2. Cache images in HBM
    hls_cores.emplace_back([&] { save_to_hbm(addr2, axi_compl[0], hbm_handles,
                                             impl_->datamover_out_hbm_0.GetCtrlStream(),
                                             impl_->datamover_out_hbm_1.GetCtrlStream(),
                                             impl_->datamover_out_hbm_2.GetCtrlStream(),
                                             save_to_hbm_idle,
                                             impl_->hbm_if_size);});

    hls_cores.emplace_back([&] { save_to_hbm_data(data_2,
                                                  data_3,
                                                  impl_->datamover_out_hbm_0.GetDataStream(),
                                                  impl_->datamover_out_hbm_1.GetDataStream(),
                                                  impl_->datamover_out_hbm_2.GetDataStream());});

    hls_cores.emplace_back([&] {frame_summation_reorder_compl(data_3, data_4, axi_compl[0], axi_compl[1]);});

    uint32_t ignore_counter = 0;
    hls_cores.emplace_back([&] { load_from_hbm(data_4, data_5, axi_compl[1], axi_compl[3], hbm_handles,
                                               impl_->datamover_in_hbm_0.GetDataStream(),
                                               impl_->datamover_in_hbm_1.GetDataStream(),
                                               impl_->datamover_in_hbm_2.GetDataStream(),
                                               impl_->datamover_in_hbm_0.GetCtrlStream(),
                                               impl_->datamover_in_hbm_1.GetCtrlStream(),
                                               impl_->datamover_in_hbm_2.GetCtrlStream(),
                                               load_to_hbm_idle, ignore_counter,
                                               impl_->hbm_if_size);});

    // 3. Calculate histogram of ADU values
    hls_cores.emplace_back([&] { adu_histo(data_5, stream_768_01, adu_histo_result, axi_compl[3], axi_compl[4]);});

    // 4. Mask missing pixels
    hls_cores.emplace_back([&] { mask_missing(stream_768_01, stream_768_02, axi_compl[4], axi_compl[5]);});

    // 5. Handle EIGER packets properly
    hls_cores.emplace_back([&] { eiger_reorder(stream_768_02, stream_768_03, axi_compl[5], axi_compl[6]);});

    // 6. Apply pixel mask
    hls_cores.emplace_back([&] {
        pixel_mask(stream_768_03,
                   stream_768_04,
                   impl_->pixel_mask.data(),
                   axi_compl[6],
                   axi_compl[7]);
    });

    // 7. Apply pedestal & gain corrections
    hls_cores.emplace_back([&] {
        jf_conversion(stream_768_04, stream_768_0,
                      axi_compl[7], axi_compl[8],
                      impl_->hbm.data(),
                      impl_->hbm.data(),
                      impl_->hbm.data(),
                      impl_->hbm.data(),
                      impl_->hbm.data(),
                      impl_->hbm.data(),
                      impl_->hbm.data(),
                      impl_->hbm.data(),
                      impl_->hbm.data(),
                      impl_->hbm.data(),
                      impl_->hbm.data(),
                      impl_->hbm.data(),
                      impl_->hbm_if_size);
    });

    // 8. Pixel thresholding
    hls_cores.emplace_back([&] { pixel_threshold(stream_768_0, stream_768_1);});

    // 9. Frame summation
    hls_cores.emplace_back([&] { frame_summation(stream_768_1, stream_768_2, axi_compl[8], axi_compl[9], frame_summation_idle);});

    // 10. Integration of pixels
    hls_cores.emplace_back([&] { integration(stream_768_2, stream_768_3, integration_result_0, axi_compl[9], axi_compl[10],
                                             impl_->hbm.data(), impl_->hbm.data(), impl_->hbm.data(), impl_->hbm.data(), integration_idle, impl_->hbm_if_size);});
    hls_cores.emplace_back([&] { axis_64_to_512(integration_result_0, integration_result_1);});

    // 11. Spot finding
    ap_uint<32> tmp_snr_threshold = float2uint(spot_finder_parameters.snr_threshold);
    ap_uint<32> min_d = float2uint(spot_finder_parameters.min_d);
    ap_uint<32> max_d = float2uint(spot_finder_parameters.max_d);
    ap_int<32> tmp_count_threshold = spot_finder_parameters.count_threshold;
    ap_uint<32> min_pix_per_spot = spot_finder_parameters.min_pix_per_spot;

    hls_cores.emplace_back([&] {
        spot_finder_mask(stream_768_3,
                         stream_768_4,
                         axi_compl[10],
                         axi_compl[11],
                         impl_->hbm.data(),
                         impl_->hbm.data(),
                         min_d,
                         max_d,
                         impl_->hbm_if_size);
    });

    hls_cores.emplace_back([&] {
        spot_finder(stream_768_4, stream_768_4a, spot_finder_result_0, tmp_count_threshold, tmp_snr_threshold);
    });

    hls_cores.emplace_back([&] {
        while (spot_finder_result_0.read().user == 0);
    });

    hls_cores.emplace_back([&] {
        spot_finder(stream_768_4a, stream_768_5, spot_finder_result_0a, tmp_count_threshold, tmp_snr_threshold);
    });

    hls_cores.emplace_back([&] {
        spot_finder_connectivity(spot_finder_result_0a,
                                 spot_finder_result_1,
                                 spot_finder_conn_0);
    });

    hls_cores.emplace_back([&] {
        spot_finder_merge(spot_finder_result_1,
                          spot_finder_conn_0,
                          spot_finder_result_2,
                          min_pix_per_spot);
    });

    hls_cores.emplace_back([&] {
        axis_32_to_512(spot_finder_result_2, spot_finder_result_3);
    });

    // 12. ROI calculation
    hls_cores.emplace_back([&] {
        roi_calc(stream_768_5,
                 stream_768_6,
                 roi_calc_result_0,
                 axi_compl[11],
                 axi_compl[12],
                 impl_->hbm.data(),
                 impl_->hbm.data(),
                 impl_->hbm_if_size);
    });

    // 13. Calc pixel statistics
    hls_cores.emplace_back([&] { pixel_calc(stream_768_6, stream_768_7, pixel_count_result_0); });

    // 14. Pixel sq root
    hls_cores.emplace_back([&] { pixel_sqrt(stream_768_7,stream_768_8); });

    // 15. Reduce/extend 24-bit stream
    hls_cores.emplace_back([&] { stream768to512(stream_768_8, data_12, stream768to512_idle);});

    // 16. Prepare data to write to host memory
    hls_cores.emplace_back([&] {
        ap_uint<3> state;
        host_writer(data_12, adu_histo_result, integration_result_1, spot_finder_result_3, roi_calc_result_0,
                    pixel_count_result_0,
                    axi_compl[12], impl_->datamover_out.GetDataStream(),
                    impl_->datamover_out.GetCtrlStream(), impl_->work_request_stream, impl_->completion_stream,
                    dma_address_table.data(), packets_processed, impl_->host_writer_idle, impl_->cancel_data_collection, state);
    });

    for (auto &i : hls_cores)
        i.join();

    if (frame_generator_future.valid())
        frame_generator_future.get();

    // reset static counter
    arp(arp1, impl_->dout_eth, mac_addr, ipv4_addr, 0);
    try {
        while (!impl_->din_eth.empty())
            impl_->din_eth.read();

        if (!addr1.empty())
            throw std::runtime_error("Addr1 queue not empty");

        if (!addr2.empty())
            throw std::runtime_error("Addr2 queue not empty");

        if (!data_1.empty())
            throw std::runtime_error("data_1 queue not empty");

        if (!data_3.empty())
            throw std::runtime_error("data_3 queue not empty");

        if (!data_4.empty())
            throw std::runtime_error("data_4 queue not empty");

        if (!data_5.empty())
            throw std::runtime_error("data_5 queue not empty");

        if (!data_7.empty())
            throw std::runtime_error("data_7 queue not empty");

        if (!data_8.empty())
            throw std::runtime_error("data_8 queue not empty");

        if (!data_9.empty())
            throw std::runtime_error("data_9 queue not empty");

        if (!data_10.empty())
            throw std::runtime_error("data_10 queue not empty");

        if (!data_12.empty())
            throw std::runtime_error("data_12 queue not empty");

        if (!data_13.empty())
            throw std::runtime_error("data_13 queue not empty");

        for (auto &c: axi_compl) {
            if (!c.empty())
                throw std::runtime_error("Compl queue not empty");
        }

        if (!stream_768_0.empty())
            throw std::runtime_error("stream_768_0 queue not empty");

        if (!stream_768_2.empty())
            throw std::runtime_error("stream_768_2 queue not empty");

        if (!stream_768_3.empty())
            throw std::runtime_error("stream_768_3 queue not empty");

        if (!stream_768_4.empty())
            throw std::runtime_error("stream_768_4 queue not empty");

        if (!stream_768_5.empty())
            throw std::runtime_error("stream_768_5 queue not empty");

        if (!hbm_handles.empty())
            throw std::runtime_error("Handles queue not empty");

        if (!integration_result_0.empty())
            throw std::runtime_error("Integration result queue not empty");

        if (!integration_result_1.empty())
            throw std::runtime_error("Integration result queue not empty");

        if (!impl_->datamover_in.GetDataStream().empty())
            throw std::runtime_error("Datamover queue is not empty");

        while (!impl_->datamover_out.IsIdle())
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
    } catch (const std::runtime_error &e) {
        throw e;
    }
    idle = true;
}

DataCollectionStatus HLSDevice::GetDataCollectionStatus() const {
    DataCollectionStatus status{};

    union {
        uint64_t u64;
        double d;
    } pulse_id_conv;
    pulse_id_conv.u64 = current_pulse_id;

    status.ctrl_reg = ap_uint<1>(impl_->host_writer_idle) ? (1 << 4) : 0;
    status.max_modules = MAX_MODULES_FPGA;
    status.hbm_size_bytes = impl_->hbm_if_size;
    status.run_counter = run_counter;
    status.current_pulseid = pulse_id_conv.d;
    status.packets_udp = udp_packets;
    status.packets_sls = sls_packets;

    return status;
}

void HLSDevice::HW_LoadCalibration(const LoadCalibrationConfig &config) {
    int ret = load_calibration(impl_->hbm.data(), impl_->hbm.data(),
                               impl_->pixel_mask.data(),
                               config,
                               impl_->hbm_if_size,
                               impl_->datamover_in.GetCtrlStream(),
                               impl_->datamover_in.GetDataStream(),
                               dma_address_table.data());
    if (ret)
        throw std::runtime_error("Error in loading calibration " + std::to_string(ret));

    if (!impl_->datamover_in.GetDataStream().empty())
        throw std::runtime_error("Datamover queue is not empty");
}

void HLSDevice::HW_SetSpotFinderParameters(const SpotFinderParameters &params) {
    spot_finder_parameters = params;
}

void HLSDevice::CreateXfelBunchIDPacket(double pulse_id, uint32_t event_code) {

    union {
        uint64_t u64;
        double d;
    } pulse_id_conv;
    pulse_id_conv.d = pulse_id;

    bunchid_raw_packet packet{};

    packet.ether_type = htons(0x0800);
    for (int i = 0; i < 6; i++)
        packet.dest_mac[i] = 0xFF;

    packet.ipv4_header_h = htons(0x4500); // Big endian in IP header!
    packet.ipv4_header_total_length = htons(sizeof(bunchid_payload) + 8 + 20); // Big endian in IP header!
    packet.ipv4_header_dest_ip = UINT32_MAX;

    packet.ipv4_header_ttl_protocol = htons(0x0011);
    packet.ipv4_header_checksum = checksum( (uint16_t *) &packet.ipv4_header_h, 20); // checksum is already in network order

    packet.udp_length = htons(sizeof(bunchid_payload) + 8);
    packet.payload.magicn[0] = BUNCHID_MAGICN;
    packet.payload.magicn[1] = BUNCHID_MAGICN;
    packet.payload.magicn[2] = BUNCHID_MAGICN;
    packet.payload.bunchid_msb[0] = (pulse_id_conv.u64 >> 32) & UINT32_MAX;
    packet.payload.bunchid_msb[1] = (pulse_id_conv.u64 >> 32) & UINT32_MAX;
    packet.payload.bunchid_lsb[0] = pulse_id_conv.u64 & UINT32_MAX;
    packet.payload.bunchid_lsb[1] = pulse_id_conv.u64 & UINT32_MAX;
    packet.payload.montrig = event_code;

    SendPacket((char *) &packet, sizeof(bunchid_raw_packet));
}

uint64_t HLSDevice::GetUDPPackets() {
    return udp_packets;
}

uint64_t HLSDevice::GetSLSPackets() {
    return sls_packets;
}