// SPDX-FileCopyrightText: 2024 Filip Leonarski, Paul Scherrer Institute <filip.leonarski@psi.ch>
// SPDX-License-Identifier: CERN-OHL-S-2.0

#include "hls_jfjoch.h"

void stream512to768(STREAM_512 &data_in,
                    STREAM_768 &data_out,
                    hls::stream<axis_addr> &addr_in,
                    hls::stream<axis_addr> &addr_out,
                    volatile ap_uint<1> &idle) {
#pragma HLS INTERFACE axis register both port=data_in
#pragma HLS INTERFACE axis register both port=data_out
#pragma HLS INTERFACE axis register both port=addr_in
#pragma HLS INTERFACE axis register both port=addr_out
#pragma HLS INTERFACE ap_none register port=idle

    idle = 1;

    packet_512_t packet_in;
    packet_768_t packet_out;

    {
#pragma HLS PROTOCOL fixed
        data_in >> packet_in;
        ap_wait();
        data_out << packet_768_t{.data = packet_in.data, .user = packet_in.user, .last = packet_in.last};
        ap_wait();
        idle = 0;
        ap_wait();
    }

    ap_uint<32> data_collection_mode = ACT_REG_MODE(packet_in.data);
    ap_uint<1> conv_jf = ((data_collection_mode & MODE_CONV) ? 1 : 0);
    ap_uint<1> mode_unsigned = ((data_collection_mode & MODE_UNSIGNED) ? 1 : 0);
    ap_uint<1> mode_eiger_32bit = ((data_collection_mode & MODE_EIGER_32BIT) ? 1 : 0);
    ap_uint<1> mode_eiger_8bit = ((data_collection_mode & MODE_EIGER_8BIT) ? 1 : 0);

    ap_uint<24> val_out[32];

    axis_addr addr;
    addr_in >> addr;

    while(!addr.last) {

        if (addr.det_type == SLS_DETECTOR_TYPE_JUNGFRAU) {
            addr.eth_packet = (addr.eth_packet % 128) | (addr.row * 64);
        } else if (addr.det_type == SLS_DETECTOR_TYPE_EIGER) {
            if (mode_eiger_8bit) {
                addr.packet_length *= 2;
                if (!addr.row)
                    addr.eth_packet = (62 - (addr.eth_packet % 32) * 2) | (addr.column);
                else
                    addr.eth_packet = (64 + (addr.eth_packet % 32) * 2) | (addr.column);
            } else if (mode_eiger_32bit) {
                addr.packet_length /= 2;

                ap_uint<1> pos_within_2_lines = addr.eth_packet % 2;
                ap_uint<9> pos_of_4_line = (addr.eth_packet % 128) / 2;
                if (!addr.row)
                    addr.eth_packet = (252 - pos_of_4_line * 4) | (2 * addr.column) | pos_within_2_lines;
                else
                    addr.eth_packet = (256 + pos_of_4_line * 4) | (2 * addr.column) | pos_within_2_lines;
            } else {
                // For EIGER the goal is put modules in an order that interleaves lines between two columns
                if (!addr.row)
                    addr.eth_packet = (126 - (addr.eth_packet % 64) * 2) | (addr.column);
                else
                    addr.eth_packet = (128 + (addr.eth_packet % 64) * 2) | (addr.column);
            }
        } else
            addr.eth_packet = 0;

        addr_out << addr;

        if (mode_eiger_8bit && (addr.det_type == SLS_DETECTOR_TYPE_EIGER)) {
            for (int j = 0; j < addr.packet_length / 2; j++) {
#pragma HLS PIPELINE II=2
                ap_uint<8> val_in[64];

                data_in >> packet_in;
                unpack64(packet_in.data, val_in);

                for (int j = 0; j < 2; j++) {
                    for (int i = 0; i < 32; i++) {
                        if (val_in[32*j+i] == UINT8_MAX)
                            val_out[i] = INT24_MAX;
                        else
                            val_out[i] = val_in[32*j+i];
                    }

                    packet_out.data = pack32(val_out);
                    packet_out.last = (j == 1) ? packet_in.last : ap_uint<1>(0);
                    data_out << packet_out;
                }
            }
        } else if (mode_eiger_32bit && (addr.det_type == SLS_DETECTOR_TYPE_EIGER)) {
            // Mode EIGER 32-bit is always unsigned
            for (int j = 0; j < addr.packet_length; j++) {
#pragma HLS PIPELINE II=2
                ap_uint<32> val_in_0[16];
                ap_uint<32> val_in_1[16];

                data_in >> packet_in;
                unpack16(packet_in.data, val_in_0);
                data_in >> packet_in;
                unpack16(packet_in.data, val_in_1);

                for (int i = 0; i < 16; i++) {
                    if (val_in_0[i] >= INT24_MAX)
                        val_out[i] = INT24_MAX;
                    else
                        val_out[i] = val_in_0[i];

                    if (val_in_1[i] >= INT24_MAX)
                        val_out[16+i] = INT24_MAX;
                    else
                        val_out[16+i] = val_in_1[i];
                }

                packet_out.data = pack32(val_out);
                packet_out.last = packet_in.last;
                data_out << packet_out;
            }
        } else {
            for (int j = 0; j < addr.packet_length; j++) {
#pragma HLS PIPELINE II=1
                data_in >> packet_in;
                if (mode_unsigned || conv_jf) {
                    ap_uint<16> val_in[32];
                    unpack32(packet_in.data, val_in);
                    for (int i = 0; i < 32; i++) {
                        if (val_in[i] == UINT16_MAX)
                            val_out[i] = INT24_MAX;
                        else
                            val_out[i] = val_in[i];
                    }
                } else {
                    ap_int<16> val_in[32];
                    unpack32(packet_in.data, val_in);
                    for (int i = 0; i < 32; i++) {
                        if (val_in[i] == INT16_MAX)
                            val_out[i] = INT24_MAX;
                        else if (val_in[i] == INT16_MIN)
                            val_out[i] = INT24_MIN;
                        else
                            val_out[i] = val_in[i];
                    }
                }
                packet_out.data = pack32(val_out);
                packet_out.last = packet_in.last;
                data_out << packet_out;
            }
        }
        addr_in >> addr;
    }

    addr_out << addr;
    data_in >> packet_in;
    data_out << packet_768_t{.data = packet_in.data, .user = 1, .last = 0};
    idle = 1;
}