// 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 pixel_sqrt(STREAM_768 &data_in,
                STREAM_768 &data_out) {
#pragma HLS INTERFACE register both axis port=data_in
#pragma HLS INTERFACE register both axis port=data_out
#pragma HLS INTERFACE ap_ctrl_none port=return

    packet_768_t packet_in;
    {
#pragma HLS PROTOCOL fixed
        data_in >> packet_in;
        ap_wait();
        data_out << packet_in;
        ap_wait();
    }

    ap_uint<32> data_collection_mode = ACT_REG_MODE(packet_in.data);
    ap_uint<1> apply_sqroot = ((data_collection_mode & MODE_SQROOT) ? 1 : 0);
    ap_uint<9> sqrtmult = ACT_REG_SQRTMULT(packet_in.data) * ACT_REG_SQRTMULT(packet_in.data);

    data_in >> packet_in;

    while (!packet_in.user) {
#pragma HLS PIPELINE II=1
        if (apply_sqroot) {
            ap_int<24> pixel[32];
            unpack32(packet_in.data, pixel);
            for (int i = 0; i < 32; i++) {
                ap_uint<32> tmp = pixel[i] * sqrtmult;
                if ((pixel[i] == INT24_MAX) || (pixel[i] == INT24_MIN)) {
                    pixel[i] = pixel[i];
                } else if (pixel[i] < 0)
                    pixel[i] = 0;
                else
                    pixel[i] = hls::sqrt(tmp);
            }
            packet_in.data = pack32(pixel);
        }

        data_out << packet_in;
        data_in >> packet_in;
    }
    data_out << packet_in;
}