// SPDX-FileCopyrightText: 2024 Filip Leonarski, Paul Scherrer Institute <filip.leonarski@psi.ch>
// SPDX-License-Identifier: GPL-3.0-only

#include <bitset>

#include "ImageSpotFinderCPU.h"
#include "StrongPixelSet.h"

ImageSpotFinderCPU::ImageSpotFinderCPU(int32_t in_width, int32_t in_height)
    : ImageSpotFinder(in_width, in_height) {}

std::vector<DiffractionSpot> ImageSpotFinderCPU::Run(const SpotFindingSettings &settings,
                                                     const std::vector<bool> &res_mask) {
    for (int i = 0; i < OutputSize(); i++)
        output_buffer[i] = 0;

    std::bitset<32> out = 0;

    if (settings.signal_to_noise_threshold <= 0.0) {
        if (settings.photon_count_threshold > 0) {
            for (int pxl = 0; pxl < height * width; pxl++) {
                int32_t bit = pxl % 32;

                int32_t pxl_val = input_buffer[pxl];
                if (pxl_val == INT32_MAX || (pxl_val > settings.photon_count_threshold && pxl_val != INT32_MIN))
                    out.set(bit);

                if (bit == 31) {
                    output_buffer[pxl / 32] = out.to_ulong();
                    out.reset();
                }
            }
        }
    } else {
        float strong2 = settings.signal_to_noise_threshold * settings.signal_to_noise_threshold;

        // Sum and sum of squares of (2*NBY+1) vertical elements
        // These are updated after each line is finished
        // 64-bit integer guarantees calculations are made without rounding errors
        std::vector<int64_t> sum_vert(width, 0);
        std::vector<int64_t> sum2_vert(width, 0);
        std::vector<uint16_t> valid_vert(width, 0);

        for (int line = 0; line < NBX; line++) {
            for (int col = 0; col < width; col++) {
                auto pxl = line * width + col;

                if (input_buffer[pxl] != INT32_MAX && input_buffer[pxl] != INT32_MIN) {
                    int64_t tmp = input_buffer[pxl];
                    sum_vert[col] += tmp;
                    sum2_vert[col] += tmp * tmp;
                    valid_vert[col] += 1;
                }
            }
        }

        for (int line = 0; line < height; line++) {
            for (int col = 0; col < width; col++) {
                if (line < height - NBX) {
                    auto pxl = (line + NBX) * width + col;

                    if (input_buffer[pxl] != INT32_MAX && input_buffer[pxl] != INT32_MIN) {
                        int64_t tmp = input_buffer[pxl];
                        sum_vert[col] += tmp;
                        sum2_vert[col] += tmp * tmp;
                        valid_vert[col] += 1;
                    }
                }

                if (line >= NBX + 1) {
                    auto pxl = (line - (NBX + 1)) * width + col;
                    if (input_buffer[pxl] != INT32_MAX && input_buffer[pxl] != INT32_MIN) {
                        int64_t tmp = input_buffer[pxl];
                        sum_vert[col] -= tmp;
                        sum2_vert[col] -= tmp * tmp;
                        valid_vert[col] -= 1;
                    }
                }
            }

            int64_t sum = 0;
            int64_t sum2 = 0;
            int64_t valid = 0;

            for (int col = 0; col < NBX; col++) {
                sum += sum_vert[col];
                sum2 += sum2_vert[col];
                valid += valid_vert[col];
            }

            for (int col = 0; col < width; col++) {
                if (col < width - NBX) {
                    sum += sum_vert[col + NBX];
                    sum2 += sum2_vert[col + NBX];
                    valid += valid_vert[col + NBX];
                }

                if (col >= NBX + 1) {
                    sum -= sum_vert[col - NBX - 1];
                    sum2 -= sum2_vert[col - NBX - 1];
                    valid -= valid_vert[col - NBX - 1];
                }
                const int32_t pxl = line * width + col;

                int32_t pxl_val = input_buffer[pxl];
                int64_t sum_local = sum - pxl_val;
                int64_t sum2_local = sum2 - pxl_val * pxl_val;
                int64_t valid_local = valid - 1;

                int64_t var = valid_local * sum2_local - (sum_local * sum_local);
                int64_t in_minus_mean = pxl_val * valid_local - sum_local;

                const int32_t bit = pxl % 32;

                if ((pxl_val == INT32_MAX) // saturated pixel is accepted always
                    || ((pxl_val != INT32_MIN && // pixel is not bad pixel
                         valid_local > MIN_VALID_PIXELS && // too many bad pixels around will give poor statistics
                         (pxl_val > settings.photon_count_threshold) && // pixel is above count threshold
                         (in_minus_mean > 0) && // pixel value is larger than mean
                         (in_minus_mean * in_minus_mean > static_cast<int64_t>(std::ceil(var * strong2))))))
                    // pixel is above SNR threshold
                    out.set(bit);

                if (bit == 31) {
                    output_buffer[pxl / 32] = out.to_ulong();
                    out.reset() ;
                }
            }
        }
    }

    if (height * width % 32 != 0)
        output_buffer[OutputSize() - 1] = out.to_ulong();

    return ExtractSpots(settings, res_mask);
}