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

#include "BraggPredictionRotation.h"

#include <cmath>
#include <algorithm>

#include "../../common/DiffractionGeometry.h"
#include "../../common/GoniometerAxis.h"
#include "../../common/JFJochException.h"
#include "../bragg_integration/SystematicAbsence.h"

BraggPredictionRotation::BraggPredictionRotation(int max_reflections)
    : BraggPrediction(max_reflections) {}

namespace {
    inline float deg_to_rad(float deg) {
        return deg * (static_cast<float>(M_PI) / 180.0f);
    }
    inline float rad_to_deg(float rad) {
        return rad * (180.0f / static_cast<float>(M_PI));
    }

    // Solve A cos(phi) + B sin(phi) + D = 0, return solutions in [phi0, phi1]
    // Returns 0..2 solutions.
    inline int solve_trig(float A, float B, float D,
                          float phi0, float phi1,
                          float out_phi[2]) {
        if (phi1 < phi0) std::swap(phi0, phi1);

        const float R = std::sqrt(A*A + B*B);
        if (!(R > 0.0f))
            return 0;

        const float rhs = -D / R;
        if (rhs < -1.0f || rhs > 1.0f)
            return 0;

        const float phi_ref = std::atan2(B, A);
        const float delta = std::acos(rhs);

        float s1 = phi_ref + delta;
        float s2 = phi_ref - delta;

        // Shift candidates by +/- 2*pi so they land near the interval.
        const float two_pi = 2.0f * static_cast<float>(M_PI);
        auto shift_near = [&](float x) {
            while (x < phi0 - two_pi) x += two_pi;
            while (x > phi1 + two_pi) x -= two_pi;
            return x;
        };

        s1 = shift_near(s1);
        s2 = shift_near(s2);

        int n = 0;
        if (s1 >= phi0 && s1 <= phi1) out_phi[n++] = s1;
        if (s2 >= phi0 && s2 <= phi1) {
            if (n == 0 || std::fabs(s2 - out_phi[0]) > 1e-6f) out_phi[n++] = s2;
        }
        return n;
    }

    // Convert angle to fractional image_number using goniometer start/increment
    inline float phi_deg_to_image_number(float phi_deg, float start_deg, float increment_deg) {
        return (phi_deg - start_deg) / increment_deg;
    }
} // namespace

int BraggPredictionRotation::Calc(const DiffractionExperiment &experiment,
                                  const CrystalLattice &lattice,
                                  const BraggPredictionSettings &settings) {
    const auto gon_opt = experiment.GetGoniometer();
    if (!gon_opt.has_value())
        throw JFJochException(JFJochExceptionCategory::InputParameterInvalid,
                              "BraggPredictionRotationCPU requires a goniometer axis");

    const GoniometerAxis& gon = *gon_opt;
    const auto geom = experiment.GetDiffractionGeometry();
    const float det_width_pxl = static_cast<float>(experiment.GetXPixelsNum());
    const float 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;

    const float wavelength = geom.GetWavelength_A();
    const float one_over_wavelength = 1.0f / wavelength;
    const float one_over_wavelength_sq = one_over_wavelength * one_over_wavelength;

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

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

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

    // Determine prediction interval in spindle angle.
    const float start_deg = gon.GetStart_deg();
    const float inc_deg   = gon.GetIncrement_deg();
    const float wedge_deg = gon.GetWedge_deg();

    const float phi0_deg = gon.GetAngle_deg(static_cast<float>(settings.image_number));
    const float phi1_deg = phi0_deg + wedge_deg;

    const float phi0 = deg_to_rad(phi0_deg);
    const float phi1 = deg_to_rad(phi1_deg);

    // Convert mosaicity to radians (assumes settings has this field, or defaults to small value)
    const float mosaicity_rad = deg_to_rad(settings.mosaicity_deg);
    const float half_mosaicity = mosaicity_rad * 0.5f;

    const Coord w = gon.GetAxis().Normalize();

    int i = 0;

    for (int h = -settings.max_hkl; h <= settings.max_hkl; ++h) {
        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) {
            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;

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

                if (recip_sq <= 0.0f || recip_sq > one_over_dmax_sq)
                    continue;

                const float g_par_s = recip_x * w.x + recip_y * w.y + recip_z * w.z;
                const float g_par_x = w.x * g_par_s;
                const float g_par_y = w.y * g_par_s;
                const float g_par_z = w.z * g_par_s;

                const float g_perp_x = recip_x - g_par_x;
                const float g_perp_y = recip_y - g_par_y;
                const float g_perp_z = recip_z - g_par_z;

                const float g_perp2 = g_perp_x * g_perp_x + g_perp_y * g_perp_y + g_perp_z * g_perp_z;
                if (g_perp2 < 1e-12f)
                    continue;

                const float p_x = S0.x + g_par_x;
                const float p_y = S0.y + g_par_y;
                const float p_z = S0.z + g_par_z;

                const float w_x_gperp_x = w.y * g_perp_z - w.z * g_perp_y;
                const float w_x_gperp_y = w.z * g_perp_x - w.x * g_perp_z;
                const float w_x_gperp_z = w.x * g_perp_y - w.y * g_perp_x;

                const float A_trig = 2.0f * (p_x * g_perp_x + p_y * g_perp_y + p_z * g_perp_z);
                const float B_trig = 2.0f * (p_x * w_x_gperp_x + p_y * w_x_gperp_y + p_z * w_x_gperp_z);
                const float D_trig = (p_x * p_x + p_y * p_y + p_z * p_z) + g_perp2 - one_over_wavelength_sq;

                float sols[2]{};
                const int nsol = solve_trig(A_trig, B_trig, D_trig, phi0 - half_mosaicity, phi1 + half_mosaicity, sols);

                for (int si = 0; si < nsol; ++si) {
                    if (i >= max_reflections) break;

                    const float phi = sols[si];
                    
                    const float cos_p = cosf(phi);
                    const float sin_p = sinf(phi);

                    // Rodrigues' rotation formula: g = g_par + cos(phi)*g_perp + sin(phi)*(w x g_perp)
                    const float g_x = g_par_x + cos_p * g_perp_x + sin_p * w_x_gperp_x;
                    const float g_y = g_par_y + cos_p * g_perp_y + sin_p * w_x_gperp_y;
                    const float g_z = g_par_z + cos_p * g_perp_z + sin_p * w_x_gperp_z;

                    const float S_x = S0.x + g_x;
                    const float S_y = S0.y + g_y;
                    const float S_z = S0.z + g_z;

                    // Inlined RecipToDector
                    const float S_rot_x = rot[0] * S_x + rot[1] * S_y + rot[2] * S_z;
                    const float S_rot_y = rot[3] * S_x + rot[4] * S_y + rot[5] * S_z;
                    const float S_rot_z = rot[6] * S_x + rot[7] * S_y + rot[8] * S_z;

                    if (S_rot_z <= 0)
                        continue;

                    const float coeff = coeff_const / S_rot_z;
                    const float x = beam_x + S_rot_x * coeff;
                    const float y = beam_y + S_rot_y * coeff;

                    if (x < 0.0f || x >= det_width_pxl || y < 0.0f || y >= det_height_pxl)
                        continue;
                    float dist_ewald_sphere = std::fabs(sqrtf(S_x * S_x + S_y * S_y + S_z * S_z) - one_over_wavelength);
                    float d = 1.0f / sqrtf(recip_sq);
                    const float phi_deg = rad_to_deg(phi);
                    reflections[i] = Reflection{
                        .h = h,
                        .k = k,
                        .l = l,
                        .angle_deg = phi_deg,
                        .predicted_x = x,
                        .predicted_y = y,
                        .d = d,
                        .dist_ewald =  dist_ewald_sphere
                    };
                    ++i;
                }
            }
        }
    }
    return i;
}