// Copyright (2019-2024) Paul Scherrer Institute

#include "CPUSpotFinder.h"
#define NUM_PASS 2

template <int N>
void FindSpots(DeviceOutput &output,
               int big_column, int big_row,
               const SpotFindingSettings& settings,
               const float *d_array,
               float *arr_mean,
               float *arr_stddev,
               uint32_t *arr_valid_count,
               uint32_t *arr_strong_pixel) {
    auto image = (int16_t *) output.pixels;

    std::vector<uint8_t> mask(N * N, 0);

    for (int y = 0; y < N; y++) {
        size_t line = big_row * N + y;
        for (int x = 0; x < N; x++) {
            size_t col = big_column * N + x;
            size_t coord = line * RAW_MODULE_COLS + col;

            uint8_t bad_pixel = 0;
            if ((line == 255) || (line == 256)
                || (col == 255) || (col == 256)
                || (col == 511) || (col == 512)
                || (col == 767) || (col == 768))
                bad_pixel = 1;

            if ((d_array[coord] < settings.high_resolution_limit)
                || (d_array[coord] > settings.low_resolution_limit))
                bad_pixel = 1;

            if ((image[coord] == INT16_MIN) || (image[coord] > 32760))
                bad_pixel = 1;

            mask[y * N + x] = bad_pixel;
        }
    }

    for (int i = 0; i < NUM_PASS; i++) {
        int64_t sum = 0;
        int64_t sum2 = 0;
        int64_t valid_count = 0;

        for (int y = 0; y < N; y++) {
            for (int x = 0; x < N; x++) {
                size_t coord = (big_row * N + y) * RAW_MODULE_COLS + big_column * N + x;
                if (mask[y * N + x] == 0) {
                    sum += image[coord];
                    sum2 += image[coord] * image[coord];
                    valid_count++;
                }
            }
        }

        int64_t variance = valid_count * sum2 - sum * sum;
        float threshold = variance * settings.signal_to_noise_threshold * settings.signal_to_noise_threshold;

        for (int y = 0; y < N; y++) {
            size_t line = big_row * N + y;
            for (int x = 0; x < N; x++) {
                size_t col = big_column * N + x;
                size_t coord = line * RAW_MODULE_COLS + col;

                arr_valid_count[coord] = valid_count;
                if (valid_count > 0) {
                    arr_mean[coord] = static_cast<float>(sum) / static_cast<float>(valid_count);
                    arr_stddev[coord] = sqrtf(
                            static_cast<float>(variance) / static_cast<float>(valid_count * valid_count));
                } else {
                    arr_mean[coord] = -1;
                    arr_stddev[coord] = -1;
                }

                bool strong_pixel = true;

                if (mask[y * N + x])
                    strong_pixel = false;

                if ((settings.photon_count_threshold < 0)
                    && (settings.signal_to_noise_threshold <= 0))
                    strong_pixel = false;

                if ((settings.photon_count_threshold >= 0)
                    && (image[coord] <= settings.photon_count_threshold)) {
                    strong_pixel = false;
                }

                if (settings.signal_to_noise_threshold > 0) {
                    int64_t in_minus_mean = image[coord] * valid_count - sum;
                    if ((in_minus_mean * in_minus_mean <= threshold)
                        || (in_minus_mean <= 0)
                        || (valid_count <= N * N / 2))
                        strong_pixel = false;
                }

                if (strong_pixel) {
                    mask[y * N + x] = 1;
                    arr_strong_pixel[coord]++;
                    output.spot_finding_result.strong_pixel[coord / 8] |= (1 << (coord % 8));
                    output.spot_finding_result.strong_pixel_count++;
                }
            }
        }
    }
}

void FindSpots(DeviceOutput &output,
               const SpotFindingSettings& settings,
               const float *d_array,
               float *arr_mean,
               float *arr_stddev,
               uint32_t *arr_valid_count,
               uint32_t *arr_strong_pixel) {
    for (auto &i: output.spot_finding_result.strong_pixel)
        i = 0;

    for (int i = 0; i < RAW_MODULE_LINES / 32; i++) {
        for (int j = 0; j < RAW_MODULE_COLS / 32; j++)
            FindSpots<32>(output, j, i, settings, d_array, arr_mean, arr_stddev, arr_valid_count, arr_strong_pixel);
    }
}