// Copyright (2019-2023) Paul Scherrer Institute

#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] <= INT16_MIN)
            tmp16[i] = UINT16_MAX;
        else if (tmp24[i] < 0)
            tmp16[i] = 0;
        else
            tmp16[i] = tmp24[i];
    }
    return pack32(tmp16);
}

void stream_24bit_conv(STREAM_768 &data_in, STREAM_512 &data_out) {
#pragma HLS INTERFACE axis register both port=data_in
#pragma HLS INTERFACE axis register both port=data_out
    
    packet_768_t packet_in;
    packet_512_t packet_out;

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

    data_out << packet_512_t{.data = packet_in.data, .user = 0, .last = 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  {
    	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};
}