Jungfraujoch/image_analysis/indexing/FFTIndexer.cpp

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

#include <algorithm>
#include <numeric>
#include "../../common/JFJochMath.h"
#include "FFTIndexer.h"
#include <Eigen/Eigen>
#include "PostIndexingRefinement.h"

// Following Steller, Bolotovsky & Rossmann (1997) J. Appl. Cryst. 30, 1036-1040
FFTIndexer::FFTIndexer(const IndexingSettings &settings)
    : max_length_A(settings.GetFFT_MaxUnitCell_A()),
      min_length_A(settings.GetFFT_MinUnitCell_A()),
      min_angle_deg(settings.GetFFT_MinAngle_deg()),
      max_angle_deg(settings.GetFFT_MaxAngle_deg()),
      nDirections(settings.GetFFT_NumVectors()),
      refine_threads(static_cast<unsigned>(settings.GetRefineThreads())),
      result_fft(nDirections) {

    // Reciprocal-magnitude histogram in one_over_d = 1/d units (the internal convention -
    // the spot coordinates and histogram_spacing are 1/d too; the resolution limit converts
    // as 1/HighRes). NB in this code Q always means the powder 2*pi/d, so 1/d is named
    // one_over_d, never q. The histogram covers the data range [0, one_over_d_max] and is
    // zero-padded by OVERSAMPLING for sub-bin peak localisation (finer cell lengths than the
    // raw bin width gives). len_coeff (= 2*max_length/histogram_size) cancels the factor, so
    // recovered lengths are independent of it. The padding factor is 2*pi: a historical value
    // from when the extent was mistakenly written as 2*pi/d (the Q convention); it is kept
    // because the exact amount sets which marginal frames index - rounding it to a nearby
    // integer shifts the indexing rate ~0.5-1% (measured on the test datasets).
    const float oversampling = 2.0f * static_cast<float>(PI);
    const float one_over_d_max = 1.0f / settings.GetFFT_HighResolution_A();

    histogram_spacing = 1.0f / (2.0f * max_length_A);
    histogram_size = std::ceil(oversampling * one_over_d_max / histogram_spacing);

    input_size = histogram_size * nDirections;
    output_size = (histogram_size / 2 + 1) * nDirections;

    if (max_length_A <= settings.GetFFT_HighResolution_A())
        throw std::invalid_argument("Largest unit cell cannot be smaller than resolution");
    if (nDirections <= 1)
        throw std::invalid_argument("FFTWIndexer: number of directions must be > 1");
    if (!(max_length_A > 0.f))
        throw std::invalid_argument("FFTWIndexer: max_length_A must be > 0");
    if (!(settings.GetFFT_HighResolution_A() > 0.f))
        throw std::invalid_argument("FFTWIndexer: high resolution must be > 0");
    if (histogram_size < 1)
        throw std::invalid_argument("FFTWIndexer: histogram_size must be >= 1");
    if (histogram_size > 1000000)
        throw std::invalid_argument("FFTWIndexer: histogram_size too large");

    SetupDirectionVectors();
}

void FFTIndexer::SetupDirectionVectors() {
    direction_vectors.reserve(static_cast<size_t>(nDirections));

    const double phi = (1.0 + std::sqrt(5.0)) / 2.0; // Golden ratio
    const double golden_angle = 2.0 * PI / phi;

    for (int i = 0; i < nDirections; i++) {
        // Half-sphere distribution (z in [0,1])
        double z = static_cast<double>(i) / static_cast<double>(nDirections - 1);
        double theta = golden_angle * static_cast<double>(i);
        double radius = std::sqrt(std::max(0.0, 1.0 - z * z));

        double x = radius * std::cos(theta);
        double y = radius * std::sin(theta);

        // Add unit vector to the list
        direction_vectors.emplace_back(static_cast<float>(x),
                                       static_cast<float>(y),
                                       static_cast<float>(z));
    }
}


void FFTIndexer::SetupUnitCell(const std::optional<UnitCell> &cell) {
    reference_unit_cell = cell;
}


void Sort(Coord &A, Coord &B, Coord &C) {
    // A is the smallest, C is the largest

    if (A.Length() > B.Length())
        std::swap(A, B);

    if (B.Length() > C.Length())
        std::swap(B, C);

    if (A.Length() > B.Length())
        std::swap(A, B);
}

std::vector<CrystalLattice> FFTIndexer::ReduceResults(const std::vector<Coord> &results, bool widen) const {
    if (results.size() < 3)
        return {};

    std::vector<CrystalLattice> candidates;

    if (!widen) {
        // Standard: combine only the shortest few filtered vectors (original behaviour, unchanged).
        for (int i = 0; i < 3; i++) {
            for (int j = 0; j < 3; j++) {
                for (int k = 0; k < 3; k++) {
                    if (i + j + k + 2 >= results.size())
                        break;
                    Coord A = results[i];
                    Coord B = results[(i + j + 1)];
                    Coord C = results[(i + j + 1) + k + 1];

                    // sort vectors by length for reduction
                    Sort(A,B,C);

                    CrystalLattice raw(A, B, C);
                    CrystalLattice reduced = raw.NiggliReduce(); // Reduce cell
                    const auto uc = reduced.GetUnitCell();

                    if (uc.a < min_length_A || uc.b < min_length_A || uc.c < min_length_A)
                        continue;

                    float alpha = uc.alpha, beta = uc.beta, gamma = uc.gamma;
                    if (alpha < min_angle_deg || alpha > max_angle_deg ||
                        beta  < min_angle_deg || beta  > max_angle_deg ||
                        gamma < min_angle_deg || gamma > max_angle_deg)
                        continue;

                    // Three coplanar rows are invisible to the two tests above: they give perfectly
                    // ordinary lengths and angles - 30 to 150 deg admits any flat combination - and
                    // only the volume betrays them. Such a cell has no reciprocal basis at all
                    // (1/V -> infinity), so it cannot be refined, only crashed into.
                    if (reduced.VolumeFraction() < MIN_BASIS_VOLUME_FRACTION)
                        continue;

                    candidates.emplace_back(std::move(reduced));
                }
            }
        }
        return candidates;
    }

    // Fallback: anchor the two short axes (first 12) but let the third reach any longer axis, so a
    // large/elongated cell whose long axis sits beyond the standard window is built. Dedup because
    // most triples of one lattice Niggli-reduce to the same cell (keeps the refine set small).
    const size_t n = std::min<size_t>(results.size(), 64);
    const size_t n_short = std::min<size_t>(n, 12);
    std::vector<UnitCell> candidate_cells;   // parallel to `candidates`, see the dedup scan below
    for (size_t i = 0; i < n_short; i++) {
        for (size_t j = i + 1; j < n_short; j++) {
            for (size_t k = j + 1; k < n; k++) {
                Coord A = results[i], B = results[j], C = results[k];
                Sort(A, B, C);
                CrystalLattice reduced = CrystalLattice(A, B, C).NiggliReduce();
                const auto uc = reduced.GetUnitCell();

                if (uc.a < min_length_A || uc.b < min_length_A || uc.c < min_length_A)
                    continue;
                if (uc.alpha < min_angle_deg || uc.alpha > max_angle_deg ||
                    uc.beta  < min_angle_deg || uc.beta  > max_angle_deg ||
                    uc.gamma < min_angle_deg || uc.gamma > max_angle_deg)
                    continue;
                if (reduced.VolumeFraction() < MIN_BASIS_VOLUME_FRACTION)
                    continue;

                bool duplicate = false;
                for (const auto &c : candidate_cells)
                    if (c.is_close(uc, 0.02f, 1.0f)) { duplicate = true; break; }
                if (!duplicate) {
                    // Keep each accepted candidate's cell rather than re-deriving it on the next
                    // triple: GetUnitCell costs three acos, and this scan runs over every candidate
                    // accepted so far, for every triple.
                    candidate_cells.push_back(uc);
                    candidates.emplace_back(std::move(reduced));
                }
            }
        }
    }

    return candidates;
}

std::vector<Coord> FFTIndexer::FilterFFTResults(size_t max_vectors,
                                               std::vector<float> *magnitudes) const {
    std::multimap<float, FFTResult> fft_result_map;

    for (int i = 0; i < direction_vectors.size(); i++)
        fft_result_map.insert(std::make_pair(result_fft[i].magnitude, result_fft[i]));

    std::vector<FFTResult> fft_result_filtered;
    int count = 0;
    // `it` outlives the loop: the extra scan at the end of the function carries on from the
    // first direction the budget did not reach.
    auto it = fft_result_map.rbegin();
    for (; it != fft_result_map.rend() && count < max_vectors; ++it, ++count) {
        fft_result_filtered.emplace_back(it->second);
    }

    std::vector<Coord> ret;

    // Remove vectors less than 5 deg apart, as most likely these are colinear
    const float COS_5_DEG = std::cos(5.0f * PI / 180.0f);

    // Minimum relative amplitude to accept a shorter vector (fundamental)
    // over a longer one (harmonic).
    // 0.25 means the fundamental frequency must have at least 25% of the
    // intensity of the harmonic. If less, the odd-indexed spots are likely
    // systematic absences or noise, and the longer vector is the true cell.
    constexpr float MIN_FUNDAMENTAL_PEAK_RATIO = 0.25f;

    std::vector<bool> ignore(fft_result_filtered.size(), false);

    for (int i = 0; i < fft_result_filtered.size(); i++) {
        if (ignore[i])
            continue;

        Coord dir_i = direction_vectors.at(fft_result_filtered[i].direction);
        float len_i = fft_result_filtered[i].length;
        int best_idx = i; // Index of the vector we currently plan to keep

        for (int j = i + 1; j < fft_result_filtered.size(); j++) {
            if (ignore[j]) continue;

            Coord dir_j = direction_vectors.at(fft_result_filtered[j].direction);

            // If vectors are colinear (angle < 5 deg)
            if (std::fabs(dir_i * dir_j) > COS_5_DEG) {
                ignore[j] = true;

                // CHECK: Is the new candidate (j) shorter than current best (best_idx)?
                // We prefer shorter vectors (fundamental periodicity) over longer ones (harmonics)
                // BUT only if the shorter vector has "enough" amplitude to be real.

                if (fft_result_filtered[j].length < len_i * 0.9f) {
                    // Compare against 'i' (the strongest in the cluster) to define the noise floor.
                    // Using 'best_idx' could allow stepping down into noise if we already swapped to a weak peak.
                    float magnitude_ratio = fft_result_filtered[j].magnitude / fft_result_filtered[i].magnitude;

                    // Heuristic: If the shorter vector has at least 25% of the amplitude of the
                    // stronger (longer) vector, assume the shorter one is the true unit cell.
                    // If it's less than 25%, the shorter peak is likely noise/aliasing,
                    // and the longer vector is the true primitive cell.
                    if (magnitude_ratio > MIN_FUNDAMENTAL_PEAK_RATIO) {
                        dir_i = dir_j;
                        len_i = fft_result_filtered[j].length;
                        best_idx = j;
                    }
                }
            }
        }
        Coord best_dir = direction_vectors.at(fft_result_filtered[best_idx].direction);
        ret.push_back(best_dir * fft_result_filtered[best_idx].length);
        if (magnitudes)
            magnitudes->push_back(fft_result_filtered[best_idx].magnitude);
    }

    // Sort filtered vectors by magnitude
    if (magnitudes) {
        std::vector<size_t> order(ret.size());
        std::iota(order.begin(), order.end(), 0);
        std::sort(order.begin(), order.end(), [&](size_t a, size_t b) {
            return ret[a].Length() < ret[b].Length();
        });
        std::vector<Coord> sorted_ret;
        std::vector<float> sorted_mag;
        sorted_ret.reserve(order.size());
        sorted_mag.reserve(order.size());
        for (const auto i : order) {
            sorted_ret.push_back(ret[i]);
            sorted_mag.push_back((*magnitudes)[i]);
        }
        ret = std::move(sorted_ret);
        *magnitudes = std::move(sorted_mag);
    } else {
        std::sort(ret.begin(), ret.end(), [](const Coord &A, const Coord &B) {
           return A.Length() < B.Length();
        });
    }

    // max_vectors counts RAW search directions, but one lattice row is sampled by many neighbouring
    // directions of the 16k half-sphere, so nearly all the strongest entries belong to the same two or
    // three rows: 30 raw peaks prune down to only four or five distinct directions in practice, and
    // those are the strongest - hence the shortest, densest - rows. When a crystal's densest rows share
    // one plane, every direction that survives is coplanar, every triple ReduceResults forms from them
    // is degenerate, and the indexer returns no cell at all. Keep walking the same magnitude order for
    // a few more directions that are 5 deg clear of everything kept - the weak long axis that closes
    // such a cell sits well past where the budget stops. They are appended AFTER the sort, so the
    // entries above hold their positions and ReduceResults still forms every triple it formed before;
    // the shortlist only gains candidates at its end. Four, because the standard reduction combines
    // results[0..8] and a five-vector shortlist leaves most of that window unusable.
    constexpr int EXTRA_DISTINCT_DIRECTIONS = 4;

    for (int extra = 0; it != fft_result_map.rend() && extra < EXTRA_DISTINCT_DIRECTIONS; ++it) {
        Coord dir = direction_vectors.at(it->second.direction);

        bool distinct = true;
        for (const auto &v: ret) {
            if (std::fabs(dir * v.Normalize()) > COS_5_DEG) {
                distinct = false;
                break;
            }
        }

        if (distinct) {
            ret.push_back(dir * it->second.length);
            if (magnitudes)
                magnitudes->push_back(it->second.magnitude);
            extra++;
        }
    }

    return ret;
}


float FFTIndexer::IndexedFraction(const CrystalLattice &latt,
                                  const std::vector<Coord> &coord, size_t nspots) const {
    if (nspots == 0)
        return 0.0f;
    const Coord a = latt.Vec0(), b = latt.Vec1(), c = latt.Vec2();
    const float tol_sq = indexing_tolerance * indexing_tolerance;
    size_t indexed = 0;
    for (size_t i = 0; i < nspots; i++) {
        const Coord &s = coord[i];
        const float hf = a * s, kf = b * s, lf = c * s;   // Coord operator* = dot product = Miller index
        // std::rint, not std::round: rounding half away from zero has to be a libm call, half to even is
        // inlined. Only the squared residual is used, and the rules can differ only at an exact .5, where
        // either leaves |frac| = 0.5 - so the indexed count is the same either way.
        const float dh = hf - std::rint(hf);
        const float dk = kf - std::rint(kf);
        const float dl = lf - std::rint(lf);
        if (dh * dh + dk * dk + dl * dl < tol_sq)
            ++indexed;
    }
    return static_cast<float>(indexed) / static_cast<float>(nspots);
}

std::vector<CrystalLattice> FFTIndexer::ReduceAndRefine(const std::vector<Coord> &coord, size_t nspots,
                                                        const std::vector<Coord> &filtered, bool widen) {
    const auto r = ReduceResults(filtered, widen);

    Eigen::MatrixX3<float> oCell(r.size() * 3u, 3u);
    Eigen::VectorX<float> scores(r.size());

    for (int i = 0; i < r.size(); i++) {
        oCell(i * 3u, 0u) = r[i].Vec0().x;
        oCell(i * 3u, 1u) = r[i].Vec0().y;
        oCell(i * 3u, 2u) = r[i].Vec0().z;

        oCell(i * 3u + 1, 0u) = r[i].Vec1().x;
        oCell(i * 3u + 1, 1u) = r[i].Vec1().y;
        oCell(i * 3u + 1, 2u) = r[i].Vec1().z;

        oCell(i * 3u + 2, 0u) = r[i].Vec2().x;
        oCell(i * 3u + 2, 1u) = r[i].Vec2().y;
        oCell(i * 3u + 2, 2u) = r[i].Vec2().z;

        // Bootstrap score
        scores(i) = 0.2;
    }

    RefineParameters parameters{
        .viable_cell_min_spots = viable_cell_min_spots,
        .dist_tolerance_vs_reference = dist_tolerance_vs_reference,
        .reference_unit_cell = reference_unit_cell,
        .min_length_A = min_length_A,
        .max_length_A = max_length_A,
        .min_angle_deg = min_angle_deg,
        .max_angle_deg = max_angle_deg,
        .indexing_tolerance = indexing_tolerance,
        .refine_threads = refine_threads
    };

    return Refine(coord, nspots, oCell, scores, parameters);
}

// Vector perpendicular to a shortlist that lies in a single plane. Three vectors of one plane can
// never be reduced to a cell - every triple ReduceResults forms from them is degenerate - so a
// shortlist like that means the row that closes the cell was not among the search directions at all.
// The scatter matrix's smallest eigenvector is that plane's normal, and its eigenvalue ratio says how
// flat the set is: lambda_min/lambda_max = 2 <sin^2(out-of-plane angle)>. The threshold below is a
// 3.5 deg rms out-of-plane spread, which sits in a wide empty gap - measured over the accumulated
// clouds of ten rotation first passes, a degenerate shortlist scores 2e-5 to 3e-4 (0.2-0.8 deg) and
// every non-degenerate one 0.026 or more (7.7 deg and up).
std::optional<Coord> FFTIndexer::DegeneratePlaneNormal(const std::vector<Coord> &filtered) const {
    // Below five rows the test says nothing: three directions drawn at random pass it 23% of the time
    // and four 4.2%, against 0.7% at five and under 0.01% at eight (measured, 20000 draws each). A net
    // that really is planar contributes far more rows than five - the crystal here gives sixteen.
    if (filtered.size() < 5)
        return {};

    Eigen::Matrix3f scatter = Eigen::Matrix3f::Zero();
    for (const auto &v : filtered) {
        const Coord n = v.Normalize();
        const Eigen::Vector3f e(n.x, n.y, n.z);
        scatter += e * e.transpose();
    }

    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> es(scatter);
    if (es.info() != Eigen::Success)
        return {};

    constexpr float MAX_OUT_OF_PLANE = 0.0076f;   // 2*<sin^2 eps> at eps_rms = 3.5 deg
    if (es.eigenvalues()(0) > MAX_OUT_OF_PLANE * es.eigenvalues()(2))
        return {};

    const auto v = es.eigenvectors().col(0);
    return Coord(v(0), v(1), v(2));
}

// One more transform, with every search direction inside a cap about `axis`. The peak of a length-L
// row stays coherent only within roughly 10.7/(L * sigma_perp) degrees of its own direction (sigma_perp
// = the spread of the accumulated cloud across that direction), which for a long axis is finer than the
// ~0.43 deg the half-sphere grid leaves between a direction and its nearest node. Spending the same
// number of directions on a 3 deg cap samples it to ~0.016 deg instead, so the row is found where the
// global pass could only miss it. The count is unchanged, so the transform and its plan are untouched
// and this costs exactly one extra pass.
std::optional<Coord> FFTIndexer::SearchCap(const std::vector<Coord> &coord, size_t nspots,
                                           const Coord &axis, float half_angle_deg) {
    const Coord u = axis.Normalize();
    Coord seed = std::fabs(u.x) < 0.9f ? Coord(1.0f, 0.0f, 0.0f) : Coord(0.0f, 1.0f, 0.0f);
    const Coord e1 = (u % seed).Normalize();
    const Coord e2 = u % e1;

    const double cos_half = std::cos(half_angle_deg * PI / 180.0);
    const double golden_angle = 2.0 * PI / ((1.0 + std::sqrt(5.0)) / 2.0);

    std::vector<Coord> saved;
    saved.swap(direction_vectors);
    direction_vectors.reserve(saved.size());
    for (size_t i = 0; i < saved.size(); i++) {
        // Same spiral as SetupDirectionVectors, with z restricted to the cap instead of the half-sphere.
        const double z = 1.0 - (1.0 - cos_half) * static_cast<double>(i) / static_cast<double>(saved.size() - 1);
        const double r = std::sqrt(std::max(0.0, 1.0 - z * z));
        const double theta = golden_angle * static_cast<double>(i);
        direction_vectors.emplace_back(u * static_cast<float>(z)
                                       + e1 * static_cast<float>(r * std::cos(theta))
                                       + e2 * static_cast<float>(r * std::sin(theta)));
    }
    DirectionsChanged();

    // The search borrows a member the object keeps using afterwards, and an indexer outlives a failed
    // transform - the broker's pool logs the error and hands the same indexer the next image - so the
    // grid has to go back even when ExecuteFFT throws.
    try {
        ExecuteFFT(coord, nspots);
    } catch (...) {
        direction_vectors.swap(saved);
        DirectionsChanged();
        throw;
    }

    int best = 0;
    for (int i = 1; i < nDirections; i++)
        if (result_fft[i].magnitude > result_fft[best].magnitude)
            best = i;
    const FFTResult peak = result_fft[best];
    const Coord found = direction_vectors.at(peak.direction) * peak.length;

    direction_vectors.swap(saved);
    DirectionsChanged();

    if (peak.length < min_length_A)
        return {};
    return found;
}

std::vector<CrystalLattice> FFTIndexer::RunInternal(const std::vector<Coord> &coord, size_t nspots) {
    if (nspots > coord.size())
        nspots = coord.size();

    if (nspots < viable_cell_min_spots)
        return {};

    assert(nspots <= FFT_MAX_SPOTS);
    assert(coord.size() <= FFT_MAX_SPOTS);

    ExecuteFFT(coord, nspots);

    // Standard reduction: 30 strongest peaks, shortest-vector triples. Unchanged for the common case.
    std::vector<float> magnitudes;
    const auto shortlist = FilterFFTResults(30, &magnitudes);

    // Free ride on that shortlist: how much of a single sweep's blind cone this orientation makes
    // unrecoverable. Needs the spindle, the wavelength and the frame's own resolution, all of which
    // are already here; costs one pass over 30-odd rows.
    // Below this the row list is no longer trustworthy: measured over 22 solved crystals, the
    // fraction of severe orientations reported as harmless falls from 0.22 at 30 spots to 0.05 at 60,
    // and flat above. Fewer spots means no value, not a middling one.
    constexpr size_t SPINDLE_MIN_SPOTS = 60;
    spindle_severity = {};
    if (spindle_axis && wavelength_A > 0 && nspots >= SPINDLE_MIN_SPOTS) {
        float d_min = 0;
        {   // the spots' own resolution: the 95th percentile of |q|, so a few stray outliers do not
            // set the cone width for the whole frame
            std::vector<float> q;
            q.reserve(nspots);
            for (size_t i = 0; i < nspots; i++)
                q.push_back(coord[i].Length());
            if (!q.empty()) {
                const size_t k = std::min(q.size() - 1, static_cast<size_t>(0.95 * q.size()));
                std::nth_element(q.begin(), q.begin() + k, q.end());
                if (q[k] > 0)
                    d_min = 1.0f / q[k];
            }
        }
        if (d_min > 0) {
            const float sin_theta = std::min(1.0f, wavelength_A / (2.0f * d_min));
            const float theta_max_deg = static_cast<float>(std::asin(sin_theta) * 180.0 / M_PI);
            spindle_severity = SpindleBlindFraction(shortlist, magnitudes, *spindle_axis, theta_max_deg);
        }
    }

    auto lattices = ReduceAndRefine(coord, nspots, shortlist, false);

    // If the best cell indexes few of the (un-refined) accumulated spots, the true cell may be large/
    // elongated with a long axis beyond the standard triple window (a superstructure, or a satellite-
    // bearing modulated crystal). OFFER widened alternatives too - the raw fraction here is not a
    // reliable enough discriminator to replace, so the caller refines each candidate and picks the one
    // that indexes best after geometry refinement. A well-indexing compact crystal keeps only its
    // standard candidates (the widened pass never runs).
    const float frac = lattices.empty() ? 0.0f : IndexedFraction(lattices.front(), coord, nspots);
    if (frac < 0.5f) {
        auto filtered = FilterFFTResults(60);
        // A shortlist confined to one plane cannot close a cell whatever is done with it, and the row
        // that would close it is the one perpendicular to that plane. Look for it there, at an angular
        // resolution the global grid does not have.
        if (const auto normal = DegeneratePlaneNormal(filtered))
            if (const auto v = SearchCap(coord, nspots, *normal, 3.0f))
                filtered.push_back(*v);

        for (auto &w : ReduceAndRefine(coord, nspots, filtered, true)) {
            bool duplicate = false;
            for (const auto &l : lattices)
                if (l.GetUnitCell().is_close(w.GetUnitCell(), 0.02f, 1.0f)) { duplicate = true; break; }
            if (!duplicate)
                lattices.push_back(std::move(w));
        }
    }

    return lattices;
}