// Copyright (2019-2023) Paul Scherrer Institute

#include "hls_jfjoch.h"

struct add_multipixel_packet {
    ap_uint<512> data;
    ap_uint<1> user;
    ap_uint<1> last;
};

void add_multipixel_in_stream(STREAM_512 &data_in,
                              hls::stream<add_multipixel_packet> &data_out) {
    packet_512_t packet_in;
    data_in >> packet_in;
    data_out << add_multipixel_packet{.data = packet_in.data, .user = packet_in.user, .last = packet_in.last};
    data_in >> packet_in;
    while (!packet_in.user) {
#pragma HLS PIPELINE II=1
        data_out << add_multipixel_packet{.data = packet_in.data, .user = packet_in.user, .last = packet_in.last};
        data_in >> packet_in;
    }
    data_out << add_multipixel_packet{.data = packet_in.data, .user = packet_in.user, .last = packet_in.last};
}

void add_multipixel_cols(hls::stream<add_multipixel_packet> &data_in,
                         hls::stream<add_multipixel_packet> &data_out) {
#pragma HLS INTERFACE axis port=data_in
#pragma HLS INTERFACE axis port=data_out
    add_multipixel_packet packet_in, packet_out;
    data_in >> packet_in;
    data_out << packet_in;
    ap_uint<1> add_multipixel = ((ACT_REG_MODE(packet_in.data) & MODE_ADD_MULTIPIXEL)) ? 1 : 0;

    data_in >> packet_in;
    if (!add_multipixel) {
        while (!packet_in.user) {
#pragma HLS PIPELINE II=1
            data_out << packet_in;
            data_in >> packet_in;
        }
    } else {
        while (!packet_in.user) {
#pragma HLS PIPELINE II=33

            packet_out.last = 0;
            packet_out.user = 0;

            ap_uint<512> save = packet_in.data;
            data_in >> packet_in;
            for (int i = 1; i < 8; i++) {
                packet_out.data(511-8,0) = save(511,16);
                packet_out.data(511,512-16) = packet_in.data(15, 0);
                data_out << packet_out;
                save = packet_in.data;
                data_in >> packet_in;
            }
            packet_out = packet_in;
            packet_out.data = save(511,16);
            data_out << packet_out;

            save = 0;
            for (int k = 1; k < 4; k++) {
                for (int i = 0; i < 8; i++) {
                    packet_out.data(511, (k * 2 - 1) * 16) = packet_in.data(511 - (k * 2 - 1) * 16, 0);
                    packet_out.data((k * 2 - 1) * 16 - 1, 0) = save;
                    save = packet_in.data(511, 512 - (2 * k - 1) * 16);
                    data_out << packet_out;
                    data_in >> packet_in;
                }
            }

            packet_out.data = save;
            packet_out.last = 0;
            packet_out.user = 0;
            data_out << packet_out;
        }
    }
    data_out << packet_in;
}

void add_multipixel_line(hls::stream<add_multipixel_packet> &data_in,
                         hls::stream<add_multipixel_packet> &data_out) {
    add_multipixel_packet packet_in, packet_out;
    data_in >> packet_in;
    data_out << packet_in;
    ap_uint<1> add_multipixel = ((ACT_REG_MODE(packet_in.data) & MODE_ADD_MULTIPIXEL)) ? 1 : 0;

    data_in >> packet_in;
    if (!add_multipixel) {
        while (!packet_in.user) {
#pragma HLS PIPELINE II=1
            data_out << packet_in;
            data_in >> packet_in;
        }
    } else {
        while (!packet_in.user) {
            for (int i = 0; i < 255 * 33; i++) {
#pragma HLS PIPELINE II=1
                if (i >= 33)
                    data_out << packet_in;
                data_in >> packet_in;
            }

            for (int k = 0; k < 2; k++) {
#pragma HLS PIPELINE II=66
                ap_uint<512> packet[33];
                for (int i = 0; i < 33; i++) {
                    packet[i] = packet_in.data;
                    data_out << packet_in;
                    data_in >> packet_in;
                }
                for (int i = 0; i < 33; i++) {
                    data_out << add_multipixel_packet{.data = packet[i], .user = 0, .last = 0};
                }
            }

            for (int i = 0; i < 255 * 33; i++) {
#pragma HLS PIPELINE II=1
                if (i < 254*33)
                    data_out << packet_in;
                data_in >> packet_in;
            }
        }
    }
    data_out << packet_in;
}

void add_multipixel_output(hls::stream<add_multipixel_packet> &data_in,
                           STREAM_512 &data_out) {
    add_multipixel_packet packet_in;
    packet_512_t packet_out;
    data_in >> packet_in;
    data_out << packet_512_t{.data = packet_in.data, .user = packet_in.user, .last = packet_in.last};

    ap_uint<1> add_multipixel = ((ACT_REG_MODE(packet_in.data) & MODE_ADD_MULTIPIXEL)) ? 1 : 0;

    data_in >> packet_in;
    if (!add_multipixel) {
        while (!packet_in.user) {
            for (int i = 0; i < 512 * 32; i++) {
#pragma HLS PIPELINE II=1
                data_out << packet_512_t{.data = packet_in.data, .user = packet_in.user, .last = (i == 512 * 32 - 1)};
                data_in >> packet_in;
            }
        }
    } else {
        while (!packet_in.user) {
            for (int i = 0; i < 64; i++) {
                packet_out.user = 0;
                packet_out.last = 0;

                for (int j = 0; j < 32; j++) {
#pragma HLS PIPELINE II=1
                    data_out << packet_512_t{.data = packet_in.data, .user = packet_in.user, .last = 0};
                    data_in >> packet_in;
                }
                ap_uint<512-64> save = 0;

                for (int k = 1; k < 8; k++) {
#pragma HLS PIPELINE OFF
                	for (int j = 0; j < 33; j++) {
#pragma HLS PIPELINE II=1
                    	if (j == 0) {
                    		save(64 * k - 1, 64 * (k-1)) = packet_in.data(64, 0);
							data_in >> packet_in;
                    	} else {
                    	packet_out.data(64 * k - 1, 0) = save(64 * k - 1, 0);
                        packet_out.data(511, 64 * k) = packet_in.data(511 - 64 * k, 0);
                        save(64 * k - 1, 0) = packet_in.data(511, 512 - 64 * k);
                        data_out << packet_out;
                        data_in >> packet_in;
                    	}
                    }
                }
                packet_out.data(64 * 7, 0) = save;
                packet_out.data(511, 512 - 64) = packet_in.data(63, 0);
                data_out << packet_out;
                data_in >> packet_in;
            }
        }
    }
    data_out << packet_512_t{.data = packet_in.data, .user = packet_in.user, .last = packet_in.last};
}

void add_multipixel(STREAM_512 &data_in, STREAM_512 &data_out) {
#pragma HLS INTERFACE axis port=data_in
#pragma HLS INTERFACE axis port=data_out
#pragma HLS DATAFLOW
    hls::stream<add_multipixel_packet, 2> stream_0;
    hls::stream<add_multipixel_packet, 2> stream_1;
    hls::stream<add_multipixel_packet, 2> stream_2;
    add_multipixel_in_stream(data_in, stream_0);
    add_multipixel_cols(stream_0, stream_1);
    add_multipixel_line(stream_1, stream_2);
    add_multipixel_output(stream_2, data_out);
}