// 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;
};

ap_uint<512> divide_by_two(const ap_uint<512> &input) {
#pragma HLS PIPELINE II=1
    ap_int<16> val[32];
    unpack32(input, val);
    for (int i = 0; i < 32; i++) {
    	if ((val[i] == INT16_MIN) || (val[i] == INT16_MAX))
    		val[i] = val[i];
    	else
    		val[i] = val[i] / 2;
    }
    return pack32(val);
}

void divide_last_by_two(ap_uint<512> &input) {
#pragma HLS PIPELINE II=1
    ap_int<16> val[32];
    unpack32(input, val);
	if ((val[31] == INT16_MIN) || (val[31] == INT16_MAX))
		val[31] = val[31];
	else
		val[31] = val[31] / 2;
    input = pack32(val);
}


void divide_first_by_two(ap_uint<512> &input) {
#pragma HLS PIPELINE II=1
	ap_int<16> val[32];
	    unpack32(input, val);
		if ((val[0] == INT16_MIN) || (val[0] == INT16_MAX))
			val[0] = val[0];
		else
			val[0] = val[0] / 2;
	    input = pack32(val);
}

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_div(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=32
        	for (int i = 0; i < 32; i++) {
        		if ((i == 8 - 1) || (i == 2 * 8 - 1) || (i == 3 * 8 - 1))
        			divide_last_by_two(packet_in.data);

        		if ((i == 8) || (i == 2 * 8) || (i == 3 * 8))
        			divide_first_by_two(packet_in.data);
        		data_out << packet_in;
				data_in >> packet_in;
        	}
        }
    }
    data_out << packet_in;
}


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-16,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);
            packet_out.data(511, 512-16) = packet_out.data(511-16, 512-32);
            data_out << packet_out;


            for (int k = 1; k < 4; k++) {
            	if (k == 1) {
            		 save = packet_in.data(15, 0);
            	} else if (k == 2) {
            		save(31,16) = save(15,0);
            	} else if (k == 3) {
					save(63,48) = save(47,32);
				}
                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;
                }
                if (k == 1)
                	save(47, 32) = packet_in.data(15,0);
                else if (k == 2)
					save(79, 64) = packet_in.data(15,0);
            }

            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] = divide_by_two(packet_in.data);
                    data_out << add_multipixel_packet{.data = packet[i], .user = 0, .last = 0};
                    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;
    hls::stream<add_multipixel_packet, 2> stream_3;

#ifndef JFJOCH_HLS_NOSYNTH
    add_multipixel_in_stream(data_in, stream_0);
    add_multipixel_div(stream_0, stream_1);
    add_multipixel_cols(stream_1, stream_2);
    add_multipixel_line(stream_2, stream_3);
    add_multipixel_output(stream_3, data_out);
#else
    std::vector<std::thread> multipix_cores;

    multipix_cores.emplace_back([&] {add_multipixel_in_stream(data_in, stream_0);});
    multipix_cores.emplace_back([&] {add_multipixel_div(stream_0, stream_1);});
    multipix_cores.emplace_back([&] {add_multipixel_cols(stream_1, stream_2);});
    multipix_cores.emplace_back([&] {add_multipixel_line(stream_2, stream_3);});
    multipix_cores.emplace_back([&] {add_multipixel_output(stream_3, data_out);});

    for (auto &i : multipix_cores)
        i.join();
#endif
}