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

#include <algorithm>
#include <bit>
#include <bitset>
#include <cmath>

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

ImageSpotFinderCPU::ImageSpotFinderCPU(int32_t in_width, int32_t in_height)
    : ImageSpotFinder(in_width, in_height), first_pass_buffer(OutputSize(), 0) {}

void ImageSpotFinderCPU::Detect(const ImagePreprocessorBuffer &image,
                                const SpotFindingSettings &settings) {
    // Two passes, as ImageSpotFinderGPU::Detect does. The second recomputes every local background
    // with the pixels the first found strong taken out of it, and keeps those pixels strong. It
    // matters because a spot wide enough to reach into its own background window inflates the mean
    // and variance it is then tested against, so its outer pixels fail the SNR test on a single
    // pass. The GPU has always done this; running one pass here made the two finders return
    // different spot lists for the same frame.
    DetectPass(image, settings, nullptr, first_pass_buffer);
    DetectPass(image, settings, first_pass_buffer.data(), output_buffer);
}

namespace {

// The local-box SNR test of one pixel. sum/sum2/valid are its window's, centre pixel included.
bool StrongInWindow(int64_t pxl_val, int64_t sum, int64_t sum2, int64_t valid,
                    const SpotFindingSettings &settings, float strong2) {
    const int64_t sum_local = sum - pxl_val;
    const int64_t sum2_local = sum2 - pxl_val * pxl_val;
    const int64_t valid_local = valid - 1;

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

    return (pxl_val == INT32_MAX) // saturated pixel, or strong in the previous pass, is accepted always
           || ((pxl_val != INT32_MIN && // pixel is not bad pixel
                valid_local > ImageSpotFinder::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
}

} // namespace

void ImageSpotFinderCPU::DetectAt(const ImagePreprocessorBuffer &image, const SpotFindingSettings &settings,
                                  const std::vector<uint32_t> &candidates) {
    candidate_windows.clear();
    // The first pass's bits are read only inside the window of a candidate (and at the candidate), so
    // it tests only the pixels of the row/column blocks such a window reaches; the bits it leaves
    // unset there are never read.
    const int32_t nblocks = (width + 31) / 32;
    first_pass_needed.assign(static_cast<size_t>(height) * nblocks, 0);
    for (size_t w = 0; w < candidates.size(); w++)
        for (uint32_t bits = candidates[w]; bits; bits &= bits - 1) {
            const int64_t pxl = static_cast<int64_t>(w) * 32 + std::countr_zero(bits);
            if (pxl >= static_cast<int64_t>(width) * height)
                break;
            const int32_t line = static_cast<int32_t>(pxl / width), col = static_cast<int32_t>(pxl % width);
            for (int32_t y = std::max(line - NBX, 0); y <= std::min(line + NBX, height - 1); y++)
                for (int32_t b = std::max(col - NBX, 0) / 32; b <= std::min(col + NBX, width - 1) / 32; b++)
                    first_pass_needed[static_cast<size_t>(y) * nblocks + b] = 1;
        }
    DetectPass(image, settings, nullptr, first_pass_buffer, candidates.data());

    std::fill(output_buffer.begin(), output_buffer.end(), 0);
    const float strong2 = settings.signal_to_noise_threshold * settings.signal_to_noise_threshold;
    const auto first_pass = [&](int32_t pxl) { return (first_pass_buffer[pxl / 32] >> (pxl % 32)) & 1U; };

    for (const auto &c : candidate_windows) {
        const int32_t line = c.pxl / width;
        const int32_t col = c.pxl % width;
        int64_t sum = c.sum, sum2 = c.sum2, valid = c.valid;
        // Take out of the window what the second pass does not count: the first pass's strong pixels.
        for (int32_t y = std::max(line - NBX, 0); y <= std::min(line + NBX, height - 1); y++) {
            const int32_t first = y * width + std::max(col - NBX, 0);
            const int32_t last = y * width + std::min(col + NBX, width - 1);
            for (int32_t w = first / 32; w <= last / 32; w++) {
                uint32_t bits = first_pass_buffer[w];
                if (w == first / 32)
                    bits &= UINT32_MAX << (first % 32);
                if (w == last / 32 && last % 32 != 31)
                    bits &= (1U << (last % 32 + 1)) - 1;
                while (bits) {
                    const int32_t q = w * 32 + std::countr_zero(bits);
                    bits &= bits - 1;
                    const int64_t v = image[q];
                    if (v != INT32_MAX && v != INT32_MIN) {
                        sum -= v;
                        sum2 -= v * v;
                        valid -= 1;
                    }
                }
            }
        }
        const int64_t pxl_val = first_pass(c.pxl) ? INT32_MAX : image[c.pxl];
        if (StrongInWindow(pxl_val, sum, sum2, valid, settings, strong2))
            output_buffer[c.pxl / 32] |= 1U << (c.pxl % 32);
    }
}

void ImageSpotFinderCPU::DetectPass(const ImagePreprocessorBuffer &image,
                                    const SpotFindingSettings &settings,
                                    const uint32_t *prev_strong,
                                    std::vector<uint32_t> &out_buffer,
                                    const uint32_t *candidates) {
    for (int i = 0; i < OutputSize(); i++)
        out_buffer[i] = 0;

    // A pixel found strong by the previous pass reads as INT32_MAX, which the accumulation below
    // already skips and the acceptance test below already takes as strong - the same substitution
    // the GPU kernel makes when it reads prev_out.
    auto value_at = [&](int32_t pxl) -> int32_t {
        if (prev_strong && (prev_strong[pxl / 32] & (1U << (pxl % 32))))
            return INT32_MAX;
        return image[pxl];
    };

    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 = value_at(pxl);
                if (pxl_val == INT32_MAX || (pxl_val > settings.photon_count_threshold && pxl_val != INT32_MIN))
                    out.set(bit);

                if (bit == 31) {
                    out_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;

                int64_t tmp = value_at(pxl);
                if (tmp != INT32_MAX && tmp != INT32_MIN) {
                    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;

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

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

            if (candidates) {
                // Only the runs of 32-column blocks whose result is read (first_pass_needed; every
                // candidate lies in one). Each run starts its window from the vertical sums over the
                // columns it covers there - integers, so the same sums the running window holds at
                // that column - and slides it as below; the bits outside the runs stay 0, as below.
                const int32_t nblocks = (width + 31) / 32;
                const uint8_t *needed = first_pass_needed.data() + static_cast<size_t>(line) * nblocks;
                for (int32_t b = 0; b < nblocks;) {
                    if (!needed[b]) { b++; continue; }
                    const int32_t c0 = b * 32;
                    while (b < nblocks && needed[b]) b++;
                    const int32_t c1 = std::min(b * 32, width);
                    int64_t sum = 0, sum2 = 0, valid = 0;
                    for (int32_t c = std::max(c0 - NBX, 0); c <= std::min(c0 + NBX, width - 1); c++) {
                        sum += sum_vert[c];
                        sum2 += sum2_vert[c];
                        valid += valid_vert[c];
                    }
                    for (int32_t col = c0; col < c1; col++) {
                        if (col > c0) {
                            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;
                        if (candidates[pxl / 32] & (1U << (pxl % 32)))
                            candidate_windows.push_back({pxl, sum, sum2, valid});
                        if (StrongInWindow(value_at(pxl), sum, sum2, valid, settings, strong2))
                            out_buffer[pxl / 32] |= 1U << (pxl % 32);
                    }
                }
                continue;
            }

            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;
                const int32_t bit = pxl % 32;

                if (StrongInWindow(value_at(pxl), sum, sum2, valid, settings, strong2))
                    out.set(bit);

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

    if (height * width % 32 != 0)
        out_buffer[OutputSize() - 1] |= out.to_ulong();   // zeroed above; the candidates path sets its bits directly
}