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

#include "BraggPrediction.h"
#include "../bragg_integration/SystematicAbsence.h"

BraggPrediction::BraggPrediction(int max_reflections)
    : max_reflections(max_reflections), reflections(max_reflections) {}

const std::vector<Reflection> &BraggPrediction::GetReflections() const {
    return reflections;
}

int BraggPrediction::Calc(const DiffractionExperiment &experiment, const CrystalLattice &lattice,
    const BraggPredictionSettings &settings) {

    const auto geom = experiment.GetDiffractionGeometry();
    const auto det_width_pxl = static_cast<float>(experiment.GetXPixelsNum());
    const auto det_height_pxl = static_cast<float>(experiment.GetYPixelsNum());

    const float one_over_dmax = 1.0f / settings.high_res_A;
    const float one_over_dmax_sq = one_over_dmax * one_over_dmax;

    float one_over_wavelength = 1.0f / geom.GetWavelength_A();

    const Coord Astar = lattice.Astar();
    const Coord Bstar = lattice.Bstar();
    const Coord Cstar = lattice.Cstar();
    const Coord S0 = geom.GetScatteringVector();

    std::vector<float> rot = geom.GetPoniRotMatrix().transpose().arr();

    // Precompute detector geometry constants
    float beam_x = geom.GetBeamX_pxl();
    float beam_y = geom.GetBeamY_pxl();
    float det_distance = geom.GetDetectorDistance_mm();
    float pixel_size = geom.GetPixelSize_mm();
    float F = det_distance / pixel_size;

    const float epsilon = 1e-5f;
    const float s0_sq = S0 * S0;
    const float rad_to_deg = 180.0f / static_cast<float>(M_PI);

    int i = 0;

    for (int h = -settings.max_hkl; h <= settings.max_hkl; h++) {
        // Precompute A* h contribution
        const float Ah_x = Astar.x * h;
        const float Ah_y = Astar.y * h;
        const float Ah_z = Astar.z * h;

        for (int k = -settings.max_hkl; k <= settings.max_hkl; k++) {
            // Accumulate B* k contribution
            const float AhBk_x = Ah_x + Bstar.x * k;
            const float AhBk_y = Ah_y + Bstar.y * k;
            const float AhBk_z = Ah_z + Bstar.z * k;

            for (int l = -settings.max_hkl; l <= settings.max_hkl; l++) {
                if (systematic_absence(h, k, l, settings.centering))
                    continue;

                if (i >= max_reflections)
                    continue;

                float recip_x = AhBk_x + Cstar.x * l;
                float recip_y = AhBk_y + Cstar.y * l;
                float recip_z = AhBk_z + Cstar.z * l;

                float recip_sq = recip_x * recip_x + recip_y * recip_y + recip_z * recip_z;
                if (recip_sq > one_over_dmax_sq)
                    continue;

                float S_x = recip_x + S0.x;
                float S_y = recip_y + S0.y;
                float S_z = recip_z + S0.z;
                float S_len = sqrtf(S_x * S_x + S_y * S_y + S_z * S_z);
                float dist_ewald_sphere = std::fabs(S_len - one_over_wavelength);

                if (dist_ewald_sphere <= settings.ewald_dist_cutoff ) {
                    const float s0_p0 = S0.x * recip_x + S0.y * recip_y + S0.z * recip_z;
                    const float val = s0_sq * recip_sq - s0_p0 * s0_p0;

                    float delta_phi_deg = NAN;
                    if (std::fabs(val) >= epsilon && s0_sq > epsilon) {
                        const float a_num = (s0_sq - 0.25f * recip_sq) * recip_sq;
                        if (a_num >= 0.0f) {
                            const float A = std::sqrt(a_num / val);
                            const float B = (A * s0_p0 + 0.5f * recip_sq) / s0_sq;

                            const float p_star_x = A * recip_x - B * S0.x;
                            const float p_star_y = A * recip_y - B * S0.y;
                            const float p_star_z = A * recip_z - B * S0.z;

                            const float p_star_sq = p_star_x * p_star_x + p_star_y * p_star_y + p_star_z * p_star_z;
                            const float denom = std::sqrt(p_star_sq * recip_sq);

                            if (denom >= epsilon) {
                                float c = (p_star_x * recip_x + p_star_y * recip_y + p_star_z * recip_z) / denom;
                                c = std::fmax(-1.0f, std::fmin(1.0f, c));
                                delta_phi_deg = std::acos(c) * rad_to_deg;
                            }
                        }
                    }

                    // Inlined RecipToDector with rot1 and rot2 (rot3 = 0)
                    // Apply rotation matrix transpose
                    float S_rot_x = rot[0] * S_x + rot[1] * S_y + rot[2] * S_z;
                    float S_rot_y = rot[3] * S_x + rot[4] * S_y + rot[5] * S_z;
                    float S_rot_z = rot[6] * S_x + rot[7] * S_y + rot[8] * S_z;

                    if (S_rot_z <= 0)
                        continue;

                    // Project to detector coordinates
                    // Assume detector is along x,y,z coordinates after rotation
                    float x = beam_x + F * S_rot_x / S_rot_z;
                    float y = beam_y + F * S_rot_y / S_rot_z;

                    if ((x < 0) || (x >= det_width_pxl) || (y < 0) || (y >= det_height_pxl))
                        continue;

                    float d = 1.0f / sqrtf(recip_sq);
                    reflections[i] = Reflection{
                        .h = h,
                        .k = k,
                        .l = l,
                        .delta_phi_deg = delta_phi_deg,
                        .predicted_x = x,
                        .predicted_y = y,
                        .d = d,
                        .dist_ewald = dist_ewald_sphere,
                        .rlp = 1.0,
                        .partiality = 1.0,
                        .zeta = 1.0
                    };
                    ++i;
                }
            }
        }
    }
    return i;
}