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

#include "hls_jfjoch.h"

ap_uint<1024> extend_24_to_32(ap_int<24*32> input) {
#pragma HLS INLINE
    ap_int<24> tmp24[32];
    ap_int<32> tmp32[32];
    unpack32(input, tmp24);
    for (int i = 0; i < 32; i++) {
        if (tmp24[i] == INT24_MAX)
            tmp32[i] = INT32_MAX;
        else if (tmp24[i] == INT24_MIN)
            tmp32[i] = INT32_MIN;
        else
            tmp32[i] = tmp24[i];
    }
    return pack32(tmp32);
}

ap_uint<1024> extend_24_to_32_unsigned(ap_int<24*32> input) {
#pragma HLS INLINE
    ap_int<24> tmp24[32];
    ap_uint<32> tmp32[32];
    unpack32(input, tmp24);
    for (int i = 0; i < 32; i++) {
        if (tmp24[i] == INT24_MAX)
            tmp32[i] = UINT32_MAX;
        else if (tmp24[i] == INT24_MIN)
            tmp32[i] = UINT32_MAX;
        else if (tmp24[i] < 0)
            tmp32[i] = 0;
        else
            tmp32[i] = tmp24[i];
    }
    return pack32(tmp32);
}

ap_uint<512> reduce_24_to_16(ap_int<24*32> input) {
#pragma HLS INLINE
    ap_int<24> tmp24[32];
    ap_int<16> tmp16[32];
    unpack32(input, tmp24);
    for (int i = 0; i < 32; i++) {
        if (tmp24[i] >= INT16_MAX)
            tmp16[i] = INT16_MAX;
        else if (tmp24[i] <= INT16_MIN)
            tmp16[i] = INT16_MIN;
        else
            tmp16[i] = tmp24[i];
    }
    return pack32(tmp16);
}

ap_uint<512> reduce_24_to_16_unsigned(ap_int<24*32> input) {
#pragma HLS INLINE
    ap_int<24> tmp24[32];
    ap_uint<16> tmp16[32];
    unpack32(input, tmp24);
    for (int i = 0; i < 32; i++) {
        if (tmp24[i] >= UINT16_MAX)
            tmp16[i] = UINT16_MAX;
        else if (tmp24[i] == INT24_MIN)
            tmp16[i] = UINT16_MAX;
        else if (tmp24[i] < 0)
            tmp16[i] = 0;
        else
            tmp16[i] = tmp24[i];
    }
    return pack32(tmp16);
}

ap_uint<256> reduce_24_to_8(ap_int<24*32> input) {
#pragma HLS INLINE
    ap_int<24> tmp24[32];
    ap_int<8> tmp8[32];
    unpack32(input, tmp24);
    for (int i = 0; i < 32; i++) {
        if (tmp24[i] >= INT8_MAX)
            tmp8[i] = INT8_MAX;
        else if (tmp24[i] <= INT8_MIN)
            tmp8[i] = INT8_MIN;
        else
            tmp8[i] = tmp24[i];
    }
    return pack32(tmp8);
}

ap_uint<256> reduce_24_to_8_unsigned(ap_int<24*32> input) {
#pragma HLS INLINE
    ap_int<24> tmp24[32];
    ap_uint<8> tmp8[32];
    unpack32(input, tmp24);
    for (int i = 0; i < 32; i++) {
        if (tmp24[i] >= UINT8_MAX)
            tmp8[i] = UINT8_MAX;
        else if (tmp24[i] == INT24_MIN)
            tmp8[i] = UINT8_MAX;
        else if (tmp24[i] < 0)
            tmp8[i] = 0;
        else
            tmp8[i] = tmp24[i];
    }
    return pack32(tmp8);
}

void stream768to512(STREAM_768 &data_in,
                    STREAM_512 &data_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 ap_none register port=idle

    idle = 1;

    packet_768_t packet_in;
    packet_512_t packet_out;

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

    ap_uint<32> data_collection_mode = ACT_REG_MODE(packet_in.data);
    ap_uint<1> write_32bit = ((data_collection_mode & MODE_32BIT_OUTPUT) ? 1 : 0);
    ap_uint<1> write_8bit  = ((data_collection_mode & MODE_8BIT_OUTPUT) ? 1 : 0);
    ap_uint<1> write_unsigned = ((data_collection_mode & MODE_UNSIGNED) ? 1 : 0);

    data_in >> packet_in;

    if (write_32bit) {
    	conv_to_32bit:
        while (!packet_in.user) {
#pragma HLS PIPELINE II=2
            ap_uint<1024> tmp;
            if (write_unsigned)
                tmp = extend_24_to_32_unsigned(packet_in.data);
            else
                tmp = extend_24_to_32(packet_in.data);

            packet_out.data = tmp(511,0);
            packet_out.user = 0;
            packet_out.last = 0;
            data_out << packet_out;

            packet_out.data = tmp(1023,512);
            packet_out.user = 0;
            packet_out.last = packet_in.last;
            data_out << packet_out;
            data_in >> packet_in;
        }
    } else if (write_8bit) {
        ap_uint<1> counter = 0;
        ap_uint<256> tmp0, tmp1;
        conv_to_8bit:
        while (!packet_in.user) {
#pragma HLS PIPELINE II=1
            if (counter == 0) {
                if (write_unsigned)
                    tmp0 = reduce_24_to_8_unsigned(packet_in.data);
                else
                    tmp0 = reduce_24_to_8(packet_in.data);
            } else {
                if (write_unsigned)
                    tmp1 = reduce_24_to_8_unsigned(packet_in.data);
                else
                    tmp1 = reduce_24_to_8(packet_in.data);
                packet_out.data = (tmp1, tmp0);
                packet_out.user = 0;
                packet_out.last = packet_in.last;
                data_out << packet_out;
            }
            data_in >> packet_in;
            counter = (counter == 0) ? 1 : 0;
        }
    } else {
    	conv_to_16bit:
        while (!packet_in.user) {
#pragma HLS PIPELINE II=1
            if (write_unsigned)
                packet_out.data = reduce_24_to_16_unsigned(packet_in.data);
            else
                packet_out.data = reduce_24_to_16(packet_in.data);
            packet_out.user = 0;
            packet_out.last = packet_in.last;
            data_out << packet_out;
            data_in >> packet_in;
        }
    }
    data_out << packet_512_t{.data = packet_in.data, .user = 1, .last = 0};
    idle = 1;
}