/*
 *  $Id: math--gridcheck.c 29061 2026-01-02 15:01:53Z yeti-dn $
 *  Copyright (C) 2003-2024 David Necas (Yeti), Petr Klapetek.
 *  E-mail: yeti@gwyddion.net, klapetek@gwyddion.net.
 *
 *  The quicksort algorithm was copied from GNU C library, Copyright (C) 1991, 1992, 1996, 1997, 1999 Free Software
 *  Foundation, Inc.  See below.
 *
 *  This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public
 *  License as published by the Free Software Foundation; either version 2 of the License, or (at your option) any
 *  later version.
 *
 *  This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied
 *  warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
 *  details.
 *
 *  You should have received a copy of the GNU General Public License along with this program; if not, write to the
 *  Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include "config.h"
#include <string.h>
#include <stdlib.h>
#include <fftw3.h>

#include "libgwyddion/macros.h"
#include "libgwyddion/math.h"
#include "libgwyddion/threads.h"

#include "libgwyddion/omp.h"

#define IDENTITY_MATRIX { 1.0, 0.0, 0.0, 1.0 }

/* We do not need to know what is the previous neighbour in the chain. We only need to know if there is any. So
 * represent it by a flag. */
enum {
    NONEIGHBOUR  = G_MAXUINT,
};

typedef struct {
    /* Input. */
    guint n;
    guint stride;
    const gdouble *in_points;
    /* State. */
    GwyXY *points;
    GwyXY *vectors;
    guint *sizes;
    guint *neighbours;
    guint *preighbours;
    GHashTable *gridcheck;
    /* Outputs. */
    guint xres;
    guint yres;
    GwyXY v0;
    GwyXY v1;
    GwyXY offset;
    guint *point_index;
    gdouble maxdiff;
} GwyGridDetector;

static guint
estimate_regular_res(gdouble *pos, gint n, gdouble *minpos, gdouble *maxpos)
{
    gdouble maxstep = 0.0;
    guint k, res;

    gwy_math_sort(pos, n);
    *minpos = pos[0];
    *maxpos = pos[n-1];
    gwy_debug("range [%g,%g]", *minpos, *maxpos);
    if (maxpos <= minpos)
        return 0;

    for (k = 1; k < n; k++) {
        if (pos[k] - pos[k-1] > maxstep)
            maxstep = pos[k] - pos[k-1];
    }
    gwy_debug("maxstep %g", maxstep);
    res = (gint)ceil((*maxpos - *minpos)/maxstep) + 1;
    gwy_debug("estimated res %d", res);

    if (n % res != 0)
        return 0;

    return res;
}

/**
 * gwy_check_regular_2d_grid:
 * @coords: Array of @n coordinate pairs in plane.  You can also typecast #GwyXY or #GwyXYZ to doubles.
 * @stride: Actual number of double values in one block.  It must be at least 2 if @coords contains just alternating
 *          @x and @y.  If you pass an typecast #GwyXYZ array give stride as 3, etc.
 * @n: Number of items in @coords.
 * @tolerance: Relative distance from pixel center which is still considered OK.  Pass a negative value for some
 *             reasonable default. The maximum meaningful value is 0.5, beyond that the point would end up in
 *             a different pixel.
 * @xres: (out): Location where to store the number of columns.
 * @yres: (out): Location where to store the number of rows.
 * @xymin: (out): Location where to store the minimum coordinates (top left corner of the corresponding image).
 * @xystep: (out): Location where to store the pixel size.
 *
 * Detects if points in plane form a regular rectangular grid oriented along the Cartesian axes.
 *
 * Points lying in one straight line are not considered to form a rectangle.
 *
 * When the function fails, i.e. the points do not form a regular grid, the values of output arguments are undefined.
 *
 * See also gwy_check_regular_2d_lattice() which is more lenient.
 *
 * Returns: On success, a newly allocated array mapping grid indices (@i*@xres+@j) to indices in @coords.  %NULL is
 *          returned on failure.
 **/
guint*
gwy_check_regular_2d_grid(const gdouble *coords, guint stride, guint n,
                          gdouble tolerance,
                          guint *pxres, guint *pyres,
                          GwyXY *xymin, GwyXY *xystep)
{
    gdouble xmin, xmax, ymin, ymax, dx, dy;
    gint xres, yres;
    guint k;
    gdouble *pos;
    guint *map;
    gboolean *encountered;
    gdouble matx[3], rhsx[2], maty[3], rhsy[3];

    g_return_val_if_fail(stride >= 2, NULL);
    g_return_val_if_fail(coords || !n, NULL);
    g_return_val_if_fail(pxres && pyres && xymin && xystep, NULL);

    if (n < 4)
        return NULL;

    if (tolerance < 0.0)
        tolerance = 0.05;

    pos = g_new(gdouble, n);
    gwy_debug("estimating yres from rows");
    for (k = 0; k < n; k++)
        pos[k] = coords[k*stride + 1];
    yres = estimate_regular_res(pos, n, &ymin, &ymax);

    gwy_debug("estimating xres from columns");
    for (k = 0; k < n; k++)
        pos[k] = coords[k*stride];
    xres = estimate_regular_res(pos, n, &xmin, &xmax);

    g_free(pos);

    if (yres) {
        xres = n/yres;
        gwy_debug("from rows xres %u, yres %u", xres, yres);
    }
    else if (xres) {
        yres = n/xres;
        gwy_debug("from columns xres %u, yres %u", xres, yres);
    }
    else
        return NULL;

    if (xres < 2 || yres < 2)
        return NULL;

    /* XXX: We could remove this condition but callers would need some means to tell if map[i] is an actual index or
     * something else, probably by putting G_MAXUINT there.   But this is an API change and current callers simply use
     * the values as indices.  A new function is needed. */
    if (xres*yres != n)
        return NULL;

    /* Widen the stripe by at most 1/2 at each side.  For large tolerance assume there is already a spread and widen
     * it less accordingly.  For exact coordinates this means differences from pixel centres are within the interval
     * [-tolerance/2, +tolerance/2], i.e. always safely smaller than tolerance in absolute value, regardless of the
     * tolerance.  So exact grids should always pass. */
    dx = (xmax - xmin)/(xres - 1 + tolerance);
    dy = (ymax - ymin)/(yres - 1 + tolerance);
    xmin -= 0.5*(1.0 - tolerance)*dx;
    xmax += 0.5*(1.0 - tolerance)*dx;
    ymin -= 0.5*(1.0 - tolerance)*dy;
    ymax += 0.5*(1.0 - tolerance)*dy;

    gwy_debug("x: [%g..%g] step %g", xmin, xmax, dx);
    gwy_debug("y: [%g..%g] step %g", ymin, ymax, dy);
    map = g_new(guint, n);
    encountered = g_new0(gboolean, n);
    gwy_clear(matx, 3);
    gwy_clear(maty, 3);
    gwy_clear(rhsx, 2);
    gwy_clear(rhsy, 2);
    for (k = 0; k < n; k++) {
        gdouble rawx = coords[k*stride + 0];
        gdouble rawy = coords[k*stride + 1];
        gdouble y = (rawy - ymin)/dy;
        gdouble x = (rawx - xmin)/dx;
        gint i = (gint)floor(y);
        gint j = (gint)floor(x);
        gdouble t;

        gwy_debug("(%g,%g) -> (%d,%d)", x, y, j, i);
        if (i < 0 || i >= yres || j < 0 || j >= xres) {
            g_critical("Points not inside estimated region?!");
            goto fail;
        }
        if (fabs(x - j - 0.5) > tolerance || fabs(y - i - 0.5) > tolerance) {
            gwy_debug("(%g,%g) too far from (%g,%g)", x, y, j+0.5, i+0.5);
            goto fail;
        }
        if (encountered[i*xres + j])
            goto fail;

        encountered[i*xres + j] = TRUE;
        map[i*xres + j] = k;

        t = j;
        matx[1] += t;
        matx[2] += t*t;
        rhsx[0] += rawx;
        rhsx[1] += t*rawx;

        t = i;
        maty[1] += t;
        maty[2] += t*t;
        rhsy[0] += rawy;
        rhsy[1] += t*rawy;
    }
    matx[0] = maty[0] = n;
    g_free(encountered);

    xymin->x = xmin;
    xymin->y = ymin;
    xystep->x = dx;
    xystep->y = dy;
    *pxres = xres;
    *pyres = yres;

    if (gwy_math_choleski_decompose(2, matx)) {
        gwy_math_choleski_solve(2, matx, rhsx);
        xystep->x = rhsx[1];
        xymin->x = rhsx[0] - 0.5*rhsx[1];
        gwy_debug("least-squares x-grid improvement to xoff=%g, xstep=%g", xymin->x, xystep->x);
    }
    if (gwy_math_choleski_decompose(2, maty)) {
        gwy_math_choleski_solve(2, maty, rhsy);
        xystep->y = rhsy[1];
        xymin->y = rhsy[0] - 0.5*rhsy[1];
        gwy_debug("least-squares y-grid improvement to yoff=%g, ystep=%g", xymin->y, xystep->y);
    }

    return map;

fail:
    g_free(map);
    g_free(encountered);
    return NULL;
}

/* FIXME: Copied from inttrans.c because we cannot depend on libprocess here. */
static guint
smooth_upper_bound(guint n)
{
    static const guint primes[] = { 2, 3, 5, 7 };

    guint j, p, r;

    for (r = 1; ; ) {
        /* the factorable part */
        for (j = 0; j < G_N_ELEMENTS(primes); j++) {
            p = primes[j];
            while (n % p == 0) {
                n /= p;
                r *= p;
            }
        }

        if (n == 1)
            return r;

        /* gosh... make it factorable again */
        n++;
    }
}

static gint
fft_find_nice_size(gint size)
{
    if (size <= 16)
        return size;

    size = smooth_upper_bound(size);
    /* Ensure the result is even.  This helps with a number of FFTW routines. When size is odd then size+1 is even,
     * i.e. it has factor 2.  Since smooth_upper_bound() preserves all the small factors, we know we will get an even
     * number. */
    if (size % 2)
        size = smooth_upper_bound(size+1);

    return size;
}

static inline void
add_xy(GwyXY *result, const GwyXY *a, const GwyXY *b)
{
    result->x = a->x + b->x;
    result->y = a->y + b->y;
}

static inline void
subtract_xy(GwyXY *result, const GwyXY *a, const GwyXY *b)
{
    result->x = a->x - b->x;
    result->y = a->y - b->y;
}

static inline gdouble
norm_xy(const GwyXY *v)
{
    return v->x*v->x + v->y*v->y;
}

static inline void
transform_xy(GwyXY *result, const GwyXY *v, const gdouble *matrix)
{
    gdouble x = v->x, y = v->y;
    result->x = matrix[0]*x + matrix[1]*y;
    result->y = matrix[2]*x + matrix[3]*y;
}

/* For keeping track of multiple affine transformations. */
static inline void
matrix_multiply_left(gdouble *matrix, const gdouble *multwith)
{
    const gdouble m00 = multwith[0]*matrix[0] + multwith[1]*matrix[2];
    const gdouble m01 = multwith[0]*matrix[1] + multwith[1]*matrix[3];
    const gdouble m10 = multwith[2]*matrix[0] + multwith[3]*matrix[2];
    const gdouble m11 = multwith[2]*matrix[1] + multwith[3]*matrix[3];

    matrix[0] = m00;
    matrix[1] = m01;
    matrix[2] = m10;
    matrix[3] = m11;
}

static void
transform_points(GwyXY *points, guint n, const gdouble *matrix)
{
    guint i;

#ifdef _OPENMP
#pragma omp parallel for if(gwy_threads_are_enabled()) default(none) \
            private(i) \
            shared(points,n,matrix)
#endif
    for (i = 0; i < n; i++) {
        transform_xy(points+i, points+i, matrix);
    }
}

static void
matrix_from_tangent(gdouble b, gdouble *matrix)
{
    const gdouble q = sqrt(1.0 + b*b), cosphi = 1.0/q, sinphi = b/q;

    matrix[0] = matrix[3] = cosphi;
    matrix[1] = sinphi;
    matrix[2] = -sinphi;
}

static inline void
rewind_counts(guint *counts, guint n)
{
    memmove(counts+1, counts, n*sizeof(guint));
    counts[0] = 0;
}

static void
bounding_box(const GwyXY *points, guint n, GwyXY *bb0, GwyXY *bb1)
{
    gdouble xm = G_MAXDOUBLE, xM = -G_MAXDOUBLE;
    gdouble ym = G_MAXDOUBLE, yM = -G_MAXDOUBLE;
    guint i;

#ifdef _OPENMP
#pragma omp parallel for if(gwy_threads_are_enabled()) default(none) \
            reduction(min:xm,ym) \
            reduction(max:xM,yM) \
            private(i) \
            shared(points,n)
#endif
    for (i = 0; i < n; i++) {
        xm = fmin(xm, points[i].x);
        ym = fmin(ym, points[i].y);
        xM = fmax(xM, points[i].x);
        yM = fmax(yM, points[i].y);
    }

    bb0->x = xm;
    bb0->y = ym;
    bb1->x = xM;
    bb1->y = yM;
}

static void
center_on_origin(GwyXY *points, guint n, GwyXY *initial_offset)
{
    gdouble sx = 0.0, sy = 0.0;
    guint i;

#ifdef _OPENMP
#pragma omp parallel for if(gwy_threads_are_enabled()) default(none) \
            reduction(+:sx,sy) \
            private(i) \
            shared(points,n)
#endif
    for (i = 0; i < n; i++) {
        sx += points[i].x;
        sy += points[i].y;
    }

    initial_offset->x = sx /= n;
    initial_offset->y = sy /= n;

#ifdef _OPENMP
#pragma omp parallel for if(gwy_threads_are_enabled()) default(none)\
            private(i) \
            shared(points,n,sx,sy)
#endif
    for (i = 0; i < n; i++) {
        points[i].x -= sx;
        points[i].y -= sy;
    }
}

static void
invert_matrix(const gdouble *m, gdouble *invm)
{
    const gdouble D = m[0]*m[3] - m[1]*m[2];

    invm[0] = m[0]/D;
    invm[1] = -m[1]/D;
    invm[2] = -m[2]/D;
    invm[3] = m[3]/D;
    GWY_SWAP(gdouble, invm[0], invm[3]);
}

static void
invert_lattice_vectors(const GwyXY *v0, const GwyXY *v1, gdouble *invm)
{
    gdouble mat[4];

    mat[0] = v0->x;
    mat[1] = v1->x;
    mat[2] = v0->y;
    mat[3] = v1->y;
    invert_matrix(mat, invm);
}

static void
transform_to_grid(GwyXY *points, guint n, const GwyXY *v0, const GwyXY *v1, const GwyXY *origin,
                  gdouble *matrix)
{
    guint i;

#ifdef _OPENMP
#pragma omp parallel for if(gwy_threads_are_enabled()) default(none) \
            private(i) \
            shared(points,n,origin)
#endif
    for (i = 0; i < n; i++) {
        points[i].x -= origin->x;
        points[i].y -= origin->y;
    }
    invert_lattice_vectors(v0, v1, matrix);
    transform_points(points, n, matrix);
}

static gdouble
projected_size(const GwyXY *points, guint n, gdouble theta)
{
    const gdouble ctheta = cos(theta), stheta = sin(theta);
    gdouble m = G_MAXDOUBLE, M = -G_MAXDOUBLE;
    guint i;

    for (i = 0; i < n; i++) {
        gdouble t = points[i].x*ctheta - points[i].y*stheta;

        m = fmin(m, t);
        M = fmax(M, t);
    }

    return M - m;
}

/* Try to make points fit better into a rectangular bounding box, not necesarily related to the actual grid in any
 * manner. This is just to have a more predictable point density (points can't just be a stripe along diagonal). But
 * prefer small rotations (i.e. not rotating by ≈ π/2 if not necessary). */
static void
prestraighen(GwyXY *points, guint n, gdouble *matrix)
{
    enum { ntheta = 36 };
    gdouble best_size, best_theta;
    gdouble m[4], sizes[ntheta];
    gint i, best_i;

#ifdef _OPENMP
#pragma omp parallel for if(gwy_threads_are_enabled()) default(none) \
            private(i) \
            shared(sizes,points,n)
#endif
    for (i = 0; i < ntheta; i++) {
        gdouble theta = (i/(gdouble)ntheta - 0.5) * G_PI;
        sizes[i] = projected_size(points, n, theta);
    }

    best_size = G_MAXDOUBLE;
    best_i = 0;
    for (i = 0; i < ntheta; i++) {
        gdouble size = sizes[i] * sizes[(i + ntheta/2) % ntheta];
        //gdouble theta = (i/(gdouble)ntheta - 0.5) * G_PI;
        //gwy_debug("%g × %g = %g (for %g°)", sizes[i], sizes[(i + ntheta/2) % ntheta], size, gwy_rad2deg(theta));
        if (size < best_size) {
            best_size = size;
            best_i = i;
        }
    }

    best_theta = (best_i/(gdouble)ntheta - 0.5) * G_PI;
    m[0] = m[3] = cos(best_theta);
    m[1] = -sin(best_theta);
    m[2] = -m[1];
    transform_points(points, n, m);
    matrix_multiply_left(matrix, m);
}

static gboolean
poststraighten(GwyXY *points, guint n, gdouble *matrix)
{
    gdouble xmin0 = G_MAXDOUBLE, xminm = G_MAXDOUBLE, xminp = G_MAXDOUBLE;
    gdouble xmax0 = G_MINDOUBLE, xmaxm = G_MINDOUBLE, xmaxp = G_MINDOUBLE;
    gdouble range0, rangem, rangep;
    guint i;

#ifdef _OPENMP
#pragma omp parallel for if(gwy_threads_are_enabled()) default(none) \
            reduction(min:xmin0,xminm,xminp) \
            reduction(max:xmax0,xmaxm,xmaxp) \
            private(i) \
            shared(points,n)
#endif
    for (i = 0; i < n; i++) {
        gdouble x = points[i].x, y = points[i].y;

        xmin0 = fmin(xmin0, x);
        xmax0 = fmax(xmax0, x);
        xminm = fmin(xminm, x - y);
        xmaxm = fmax(xmaxm, x - y);
        xminp = fmin(xminp, x + y);
        xmaxp = fmax(xmaxp, x + y);
    }

    matrix[0] = matrix[3] = 1.0;
    matrix[1] = matrix[2] = 0.0;
    range0 = xmax0 - xmin0;
    rangem = xmaxm - xminm;
    rangep = xmaxp - xminp;
    if (rangem < fmin(range0, rangep)) {
        matrix[1] = -1;
        transform_points(points, n, matrix);
        gwy_debug("skew right");
        return TRUE;
    }
    else if (rangep < fmin(range0, rangem)) {
        matrix[1] = 1;
        transform_points(points, n, matrix);
        gwy_debug("skew left");
        return TRUE;
    }
    return FALSE;
}

/* Reshuffles points!
 *
 * We may be given a completely bonkers set of points and just simply sort them to little rectangles for further
 * processing. We kind of want them bigger than single-point. */
static guint*
sort_into_cells(GwyXY *points, guint n,
                const GwyXY *bb0, const GwyXY *bb1,
                guint *cnx, guint *cny)
{
    const gdouble Lx = bb1->x - bb0->x, Ly = bb1->y - bb0->y;
    const gdouble Lx2 = sqrt(Lx), Ly2 = sqrt(Ly);
    const gdouble Lx4 = sqrt(Lx2), Ly4 = sqrt(Ly2);
    gdouble ngest, dx, dy;
    guint nx, ny, k, i, totalwork = 0;
    GwyXY *newpoints;
    guint *cells, *cell_sizes;

    /* Clearly a 1D case, handle it later. */
    if (Lx <= 0.0 || Ly <= 0.0 || n < 3)
        return NULL;

    /* Here we do not know if the actual grid rectangles have a reasonable aspect ratio. Try a compromise between
     * assuming dx = dy and xres = yres. Make sure the rectangles contain at least one point on average. */
    dx = 1.4*Lx2*Lx4*Ly4/sqrt(n);
    dy = 1.4*Lx4*Ly2*Ly4/sqrt(n);
    ngest = Lx/dx * Ly/dy;
    if (ngest > n) {
        gdouble q = sqrt(n/ngest);
        dx *= q;
        dy *= q;
    }

    nx = (guint)ceil(Lx/dx);
    ny = (guint)ceil(Ly/dy);
    cells = g_new(guint, n);

#ifdef _OPENMP
#pragma omp parallel for if(gwy_threads_are_enabled()) default(none) \
            private(k) \
            shared(cells,points,n,nx,ny,bb0,dx,dy)
#endif
    for (k = 0; k < n; k++) {
        gdouble x = points[k].x - bb0->x, y = points[k].y - bb0->y;
        guint ii = GWY_ROUND(y/dy), jj = GWY_ROUND(x/dx);
        ii = MIN(ii, ny-1);
        jj = MIN(jj, nx-1);
        cells[k] = ii*nx + jj;
    }

    *cnx = nx;
    *cny = ny;

    cell_sizes = g_new0(guint, nx*ny+1);
    /* This is possibly a memory access disaster, but we have to suffer it at least once to create a layout with
     * a better locality. */
    for (k = 0; k < n; k++)
        cell_sizes[cells[k]]++;

    /* Estimate the total work for shortest vector finding. It also serves for detection of point sets with extreme
     * density variations – we reject these for multiple reasons. For start, the resulting grid (if points are on
     * a grid) would be huge and sparsely populated. */
    for (i = 0; i < nx*ny; i++)
        totalwork += cell_sizes[i]*cell_sizes[i];

    if (totalwork > 8*n) {
        g_free(cells);
        g_free(cell_sizes);
        return NULL;
    }

    gwy_accumulate_counts(cell_sizes, nx*ny, TRUE);
    newpoints = g_new(GwyXY, n);
    for (i = 0; i < n; i++)
        newpoints[cell_sizes[cells[i]]++] = points[i];
    gwy_assign(points, newpoints, n);
    g_free(newpoints);

    rewind_counts(cell_sizes, nx*ny);

    g_free(cells);

    return cell_sizes;
}

static inline void
add_cell_points(const GwyXY *points,
                const guint *cell_sizes, guint k,
                GArray *out)
{
    const GwyXY *block = points + cell_sizes[k];
    guint npts = cell_sizes[k+1] - cell_sizes[k];

    g_array_append_vals(out, block, npts);
}

static inline void
block_shortest_vector(const GwyXY *points, guint npts, GwyXY *sv)
{
    gdouble minnorm = G_MAXDOUBLE;
    gdouble v2;
    guint i, j, best_i, best_j;
    GwyXY v;

    if (npts < 3) {
        if (npts == 2)
            subtract_xy(sv, points, points+1);
        else
            sv->x = sv->y = 0.0;
        return;
    }

    best_i = 0;
    best_j = 1;
    for (i = 0; i < npts-1; i++) {
        for (j = i+1; j < npts; j++) {
            subtract_xy(&v, points+i, points+j);
            v2 = norm_xy(&v);
            if (v2 < minnorm) {
                best_i = i;
                best_j = j;
                minnorm = v2;
            }
        }
    }

    subtract_xy(sv, points+best_i, points+best_j);
}

static inline void
gather_block_points(const GwyXY *points,
                    guint cnx, guint cny, guint cnx1,
                    const guint *cell_sizes,
                    guint i,
                    GArray *blockpoints)
{
    const guint ci = i/cnx1;
    const guint cj = i % cnx1;
    const guint k = ci*cnx + cj;

    g_array_set_size(blockpoints, 0);
    add_cell_points(points, cell_sizes, k, blockpoints);
    if (cj+1 < cnx)
        add_cell_points(points, cell_sizes, k+1, blockpoints);
    if (ci+1 < cny) {
        add_cell_points(points, cell_sizes, k+cnx, blockpoints);
        if (cj+1 < cnx)
            add_cell_points(points, cell_sizes, k+cnx+1, blockpoints);
    }
}

static guint
find_shortest_vectors(const GwyXY *points,
                      guint cnx, guint cny, const guint *cell_sizes,
                      GwyXY *svectors)
{
    const guint cnx1 = (cnx > 1 ? cnx-1 : 1), cny1 = (cny > 1 ? cny-1 : 1);
    guint j, nvec;

    /* XXX: We cannot have default(none) here due to incompatibility in OpenMP specification.  Some versions require
     * shared(cnx1,cny1), whereas some forbid it. */
#ifdef _OPENMP
#pragma omp parallel if(gwy_threads_are_enabled()) \
            shared(points,cnx,cny,cell_sizes,svectors)
#endif
    {
        guint nfrom = gwy_omp_chunk_start(cnx1*cny1), nto = gwy_omp_chunk_end(cnx1*cny1);
        GArray *blockpoints = g_array_sized_new(FALSE, FALSE, sizeof(GwyXY), 20);
        guint i;

        for (i = nfrom; i < nto; i++) {
            gather_block_points(points, cnx, cny, cnx1, cell_sizes, i, blockpoints);
            block_shortest_vector(&g_array_index(blockpoints, GwyXY, 0), blockpoints->len, svectors + i);
        }

        g_array_free(blockpoints, TRUE);
    }

    for (j = nvec = 0; j < cnx1*cny1; j++) {
        if (svectors[j].x != 0.0 || svectors[j].y != 0.0)
            svectors[nvec++] = svectors[j];
    }

    return nvec;
}

static guint
filter_out_too_far_vectors(GwyXY *vectors, guint nvec)
{
    gdouble *d2all;
    gdouble d2, med, d2min, d2max;
    guint i, nvecfilt;

    if (nvec < 3)
        return nvec;

    d2all = g_new(gdouble, nvec);
#ifdef _OPENMP
#pragma omp parallel for if(gwy_threads_are_enabled()) default(none) \
            private(i) \
            shared(vectors,nvec,d2all)
#endif
    for (i = 0; i < nvec; i++)
        d2all[i] = norm_xy(vectors+i);

    med = gwy_math_median(d2all, nvec);
    g_free(d2all);

    d2min = 0.5*med;
    d2max = 2.25*med;

    for (i = nvecfilt = 0; i < nvec; i++) {
        d2 = norm_xy(vectors+i);
        if (d2 > d2min && d2 < d2max)
            vectors[nvecfilt++] = vectors[i];
    }

    return nvecfilt;
}

static inline gboolean
find_neighbour_in_cell(const GwyXY *points,
                       const guint *cell_sizes, guint k,
                       const GwyXY *expected, guint ipt, gdouble *maxd2,
                       guint *neighbour)
{
    const guint block_start = cell_sizes[k];
    const GwyXY *block = points + block_start;
    guint i, neigh = *neighbour, npts = cell_sizes[k+1] - block_start;
    gboolean found = FALSE;
    gdouble md2 = *maxd2;

    for (i = 0; i < npts; i++) {
        /* FIXME: This is what takes the most time, presumably due to a large number of cache faults. */
        GwyXY v = { block[i].x - expected->x, block[i].y - expected->y };
        gdouble d2 = norm_xy(&v);
        if (d2 < md2 && block_start + i != ipt) {
            md2 = d2;
            neigh = block_start + i;
            found = TRUE;
        }
    }

    *maxd2 = md2;
    *neighbour = neigh;

    return found;
}

static void
find_neighbours_along_vec(const GwyXY *points, guint n,
                          guint cnx, guint cny,
                          const guint *cell_sizes,
                          const GwyXY *vmean,
                          guint *neighbours, guint *preighbours)
{
    const enum {
        LOOK_RIGHT, LOOK_UP, LOOK_DOWN
    } look_where = ((vmean->x >= fabs(vmean->y)) ? LOOK_RIGHT : (vmean->y >= 0.0 ? LOOK_UP : LOOK_DOWN));

    gdouble maxd2 = 0.25*norm_xy(vmean);
    GwyXY vmean_local = *vmean;
    guint k;

    for (k = 0; k < n; k++) {
        neighbours[k] = NONEIGHBOUR;
        preighbours[k] = NONEIGHBOUR;
    }

    gwy_debug("look %u ", look_where);
    g_assert(vmean->x >= 0.0);

    /* Iterate by cells, not by points, to save some work and improve locality. */
#ifdef _OPENMP
#pragma omp parallel for if(gwy_threads_are_enabled()) default(none) \
            private(k) \
            shared(points,n,cnx,cny,cell_sizes,vmean_local,neighbours,preighbours,maxd2,look_where)
#endif
    for (k = 0; k < cnx*cny; k++) {
        guint ci = k/cnx, cj = k % cnx, nblocks = 0;
        guint look_blocks[6];
        const GwyXY *block = points + cell_sizes[k];
        guint j, npts = cell_sizes[k+1] - cell_sizes[k];

        if (!npts)
            continue;

        look_blocks[nblocks++] = k;
        if (cj < cnx-1)
            look_blocks[nblocks++] = k+1;
        if ((look_where == LOOK_DOWN || look_where == LOOK_RIGHT) && ci > 0) {
            look_blocks[nblocks++] = k-cnx;
            if (cj < cnx-1)
                look_blocks[nblocks++] = k-cnx+1;
        }
        if ((look_where == LOOK_UP || look_where == LOOK_RIGHT) && ci < cny-1) {
            look_blocks[nblocks++] = k+cnx;
            if (cj < cnx-1)
                look_blocks[nblocks++] = k+cnx+1;
        }
        if ((look_where == LOOK_DOWN || look_where == LOOK_UP) && cj > 0) {
            look_blocks[nblocks++] = k-1;
            if (look_where == LOOK_DOWN && ci > 0)
                look_blocks[nblocks++] = k-cnx-1;
            if (look_where == LOOK_UP && ci < cny-1)
                look_blocks[nblocks++] = k+cnx-1;
        }

        for (j = 0; j < npts; j++) {
            guint ib, neigh = NONEIGHBOUR;
            guint i = cell_sizes[k] + j;
            GwyXY expected;
            gdouble maxd2i = maxd2;
            gboolean found = FALSE;

            /* Work with the expected position of the neighbour because it simplifies the vector computation. */
            add_xy(&expected, block + j, &vmean_local);
            for (ib = 0; ib < nblocks; ib++)
                found |= find_neighbour_in_cell(points, cell_sizes, look_blocks[ib], &expected, i, &maxd2i, &neigh);

            if (found) {
                /* There is no race because there is exactly one moment when we write to neighbours[i] and that is
                 * right now. */
                neighbours[i] = neigh;
                /* There is no race because even in the (unlikely) case when we write multiple times to
                 * preighbours[neigh] it does not matter what we write there, as long as it is not NONEIGHBOUR. */
                preighbours[neigh] = i;
                /* And there is an implicit barrier at the end of the current ‘parallel for’, so the caller will
                 * see updated values. */
                //gwy_debug("NEIGH %g %g", points[neigh].x - points[i].x, points[neigh].y - points[i].y);
            }
        }
    }
}

static gdouble
estimate_mean_vector_with_axis(const GwyXY *vectors, guint nvec, gdouble theta, GwyXY *vmean)
{
    const gdouble m[4] = { cos(theta), -sin(theta), sin(theta), cos(theta) };
    gdouble x, y, t, sx, sy, resid, vx, vy, dp;
    gdouble spm, spp;
    GwyXY v;
    guint i;

    sx = sy = 0.0;
    for (i = 0; i < nvec; i++) {
        x = vectors[i].x;
        y = vectors[i].y;
        transform_xy(&v, vectors+i, m);
        if (fabs(v.y) > fabs(v.x)) {
            /* Rotate by π/2 to some of the quadrants where |x| ≥ |y| (rotated by ϑ). Note that the vector rotated
             * by ϑ is used only for classification; x and y are only moved between quadrants. Since we do not care
             * about the signs of x and y here, just increment the correct sum. */
            sx += y*y;
            sy += x*x;
        }
        else {
            sx += x*x;
            sy += y*y;
        }
    }

    vx = sqrt(sx/(sx + sy));
    vy = sqrt(sy/(sx + sy));
    spm = spp = 0.0;
    for (i = 0; i < nvec; i++) {
        x = vectors[i].x;
        y = vectors[i].y;
        transform_xy(&v, vectors+i, m);
        if (fabs(v.y) > fabs(v.x)) {
            /* Rotate by π/2 to some of the quadrants where |x| ≥ |y| (rotated by ϑ). Note that the vector rotated
             * by ϑ is used only for classification; x and y are only moved between quadrants. */
            t = x;
            x = y;
            y = -t;
        }
        if (x < 0) {
            x = -x;
            y = -y;
        }
        spp += x*vx + y*vy;
        spm += x*vx - y*vy;
    }
    if (fabs(spp) > fabs(spm)) {
        vx = copysign(vx, spp);
        vy = copysign(vy, spp);
    }
    else {
        vx = copysign(vx, spm);
        vy = -copysign(vy, spm);
    }

    sx = sy = 0.0;
    for (i = 0; i < nvec; i++) {
        x = vectors[i].x;
        y = vectors[i].y;
        transform_xy(&v, vectors+i, m);
        if (fabs(v.y) > fabs(v.x)) {
            /* Rotate by π/2 to some of the quadrants where |x| ≥ |y| (rotated by ϑ). Note that the vector rotated
             * by ϑ is used only for classification; x and y are only moved between quadrants. */
            t = x;
            x = y;
            y = -t;
        }
        if (x < 0) {
            x = -x;
            y = -y;
        }
        dp = x*vx + y*vy;
        sx += copysign(1.0, dp)*x;
        sy += copysign(1.0, dp)*y;
    }

    /* Ensure the x-component is non-negative. */
    if (sx < 0.0) {
        sx = -sx;
        sy = -sy;
    }

    vmean->x = sx/nvec;
    vmean->y = sy/nvec;

    resid = 0.0;
    for (i = 0; i < nvec; i++) {
        x = vectors[i].x;
        y = vectors[i].y;
        transform_xy(&v, vectors+i, m);
        if (fabs(v.y) > fabs(v.x)) {
            /* Rotate by π/2 to some of the quadrants where |x| ≥ |y| (rotated by ϑ). Note that the vector rotated
             * by ϑ is used only for classification; x and y are only moved between quadrants. */
            t = x;
            x = y;
            y = -t;
        }
        if (x < 0) {
            x = -x;
            y = -y;
        }
        v.x = x - vmean->x;
        v.y = y - vmean->y;
        resid += norm_xy(&v);
    }

    return resid;
}

static void
estimate_mean_vector_mod_pi2(const GwyXY *vectors, guint nvec, GwyXY *vmean)
{
    enum { ntheta = 4 };
    gdouble resid[ntheta], best;
    GwyXY vmeantheta[ntheta];
    guint i, i_best;

#ifdef _OPENMP
#pragma omp parallel for if(gwy_threads_are_enabled()) default(none) \
            private(i) \
            shared(vectors,nvec,vmeantheta,resid)
#endif
    for (i = 0; i < ntheta; i++) {
        gdouble theta = i/(gdouble)ntheta * G_PI/2.0;
        resid[i] = estimate_mean_vector_with_axis(vectors, nvec, theta, vmeantheta+i);
    }
#if 0
    for (i = 0; i < ntheta; i++) {
        gdouble theta = i/(gdouble)ntheta * G_PI/2.0;
        gwy_debug("ϑ = %g°, resid = %g, vmean = (%g, %g)",
                  gwy_rad2deg(theta), resid[i], vmeantheta[i].x, vmeantheta[i].y);
    }
#endif

    i_best = 0;
    best = resid[0];
    for (i = 1; i < ntheta; i++) {
        if (resid[i] < best) {
            best = resid[i];
            i_best = i;
        }
    }

    *vmean = vmeantheta[i_best];
}

/* Find vector which best aligns with chains of neigbours. Using entire chais is important because the error decreases
 * fast with the chain length (see grating evaluation paper), i.e. it is much better than simply averaging the vectors
 * to neighbours. It ensures the sine error (from error of the mean vector) decreases faster with the number of points
 * than the maximum rotation which would mix up different lines of points.
 *
 * It is a least squares problem with unknown origins of all chains (a_kx,a_ky) and common vector to the neighbour
 * (b_x,b_y). However, we only care about b and, furthermore, x and y equations are independent. By eliminating the
 * nuisance parameters (a_kx,a_ky) we thus get two direct expressions for b_x and b_y. */
static guint
estimate_mean_vector_from_chains(const GwyXY *points, guint n, const guint *neighbours, const guint *preighbours,
                                 GwyXY *vmean)
{
    gdouble sx = 0.0, sy = 0.0;
    guint i, Ntot = 0, sn = 0;

#ifdef _OPENMP
#pragma omp parallel for if(gwy_threads_are_enabled()) default(none) \
            reduction(+:sx,sy,sn,Ntot) \
            private(i) \
            shared(points,n,neighbours,preighbours)
#endif
    for (i = 0; i < n; i++) {
        gint N, k;
        guint j;

        j = neighbours[i];
        /* Only consider points at the start of a chain, i.e. no previous but some next point. */
        if (j == NONEIGHBOUR || preighbours[i] != NONEIGHBOUR)
            continue;

        /* Chain length (points, not vectors). */
        N = 1;
        do {
            j = neighbours[j];
            N++;
        } while (j != NONEIGHBOUR);

        /* Contributions to the least squares problem. */
        j = i;
        for (k = 0; k < N; k++) {
            sx += (2*k+1 - N)*points[j].x;
            sy += (2*k+1 - N)*points[j].y;
            j = neighbours[j];
        }
        sn += N*(N*N - 1);
        Ntot += N;
    }

    vmean->x = 6.0*sx/sn;
    vmean->y = 6.0*sy/sn;

    return Ntot;
}

/* FIXME: If the grid size is tiny in the other direction (1×N, 2×N or 3×N), this will likely fail. */
static gdouble
estimate_orthogonal_spacing(const GwyXY *points, guint n, gdouble ymin, gdouble ymax, gdouble vx)
{
    /* We know dy ≥ dx, so we have a reasonable discretisation estimate. However, if dy ≫ dx it can blow up! */
    const gdouble Ly = 2.0*(ymax - ymin);
    const gint northo = fft_find_nice_size(GWY_ROUND(fmin(12.0*Ly/vx, 4.0 + 3.0*pow(n, 0.8))));
    const gint npsdf = (northo + 1)/2;
    fftw_complex *ortho, *psdf;
    fftw_plan plan;
    gdouble *p = NULL, *d = NULL;
    gdouble pmax, dy, bestf, vy = -1.0;
    guint i, m_first, m_last;
    gint j, best_j, nrows;

    gwy_debug("Ly %g, northo %d", Ly, northo);

    if (npsdf < 6)
        return vy;

    d = g_new0(gdouble, northo);
    dy = Ly/northo;

    for (i = 0; i < n; i++) {
        j = GWY_ROUND((points[i].y + 0.5*Ly)/dy);
        if (j >= 0 && j < northo)
            d[j] += 1.0;
    }

    m_first = 0;
    for (j = 1; j < northo-1; j++) {
        if (d[j] > d[j-1] && d[j] >= d[j+1]) {
            m_first = j;
            break;
        }
    }

    m_last = northo-1;
    for (j = northo-2; j; j--) {
        if (d[j] > d[j+1] && d[j] >= d[j-1]) {
            m_last = j;
            break;
        }
    }
    gwy_debug("first peak %g, last peak %g", m_first*dy - 0.5*Ly, m_last*dy - 0.5*Ly);

    /* FIXME: In gwyprocess we would lock the planner. */
    ortho = g_new(fftw_complex, northo);
    psdf = g_new(fftw_complex, northo);
    p = g_new(gdouble, npsdf);
    plan = fftw_plan_dft_1d(northo, ortho, psdf, FFTW_FORWARD, FFTW_DESTROY_INPUT | FFTW_ESTIMATE);
    for (j = 0; j < northo; j++) {
        ortho[j][0] = d[j];
        ortho[j][1] = 0.0;
    }
    fftw_execute(plan);
    g_free(ortho);
    for (j = 0; j < npsdf; j++) {
        p[j] = psdf[j][0]*psdf[j][0] + psdf[j][1]*psdf[j][1];
    }
    g_free(psdf);

    j = 3;
    while (j+1 < npsdf && p[j+1] < p[j])
        j++;

    //gwy_debug("psdf valley %d", j);
    if (j == npsdf-1)
        goto end;

    best_j = j;
    pmax = p[j];
    while (j < npsdf-1) {
        if (p[j] > pmax) {
            //gwy_debug("psdf %g → %g at %d", pmax, p[j], j);
            best_j = j;
            pmax = p[j];
        }
        j++;
    }

    bestf = 0.0;
    gwy_math_refine_maximum_1d(p + best_j-1, &bestf);
    bestf += best_j;
    gwy_debug("psdf maximum %d (refined %g)", best_j, bestf);

    vy = Ly/bestf;
    gwy_debug("n rows %g", dy*(m_last - m_first)/vy + 1.0);
    nrows = GWY_ROUND(dy*(m_last - m_first)/vy + 1.0);
    if (nrows < 2) {
        vy = -1.0;
        goto end;
    }

    vy = dy*(m_last - m_first)/(nrows - 1);

end:
    GWY_FREE(d);
    GWY_FREE(p);

    return vy;
}

static gdouble
estimate_orthogonal_mod(const GwyXY *points, guint n, gdouble ymin, gdouble vy)
{
    gdouble m = 0.0;
    guint i;

#ifdef _OPENMP
#pragma omp parallel for if(gwy_threads_are_enabled()) default(none) \
            reduction(+:m) \
            private(i) \
            shared(points,n,ymin,vy)
#endif
    for (i = 0; i < n; i++) {
        gdouble y = points[i].y;
        m += fmod(y - ymin + 0.4*vy, vy);
    }
    m /= n;
    m -= 0.4*vy;

    return m;
}

static gdouble
estimate_line_coeff_full(const GwyXY *points, guint n, gdouble yorigin, gdouble ymax, gdouble *pvy)
{
    gdouble sxx = 0.0, sxy = 0.0, sx = 0.0, sy = 0.0, s1 = 0.0;
    const gdouble vy = *pvy;
    gdouble D, a, b;
    /* Centre k values. For some reason the results are strange when we do not do this, even though it should not
     * be that numerically unstable. */
    guint i, k0 = GWY_ROUND(0.5*(ymax - yorigin)/vy);

#ifdef _OPENMP
#pragma omp parallel for if(gwy_threads_are_enabled()) default(none) \
            reduction(+:sxx,sxy,sx,sy,s1) \
            private(i) \
            shared(points,n,yorigin,vy,k0)
#endif
    for (i = 0; i < n; i++) {
        gdouble x = points[i].x, y = points[i].y;
        gint k = GWY_ROUND((y - yorigin)/vy) - k0;

        sxx += x*x;
        sxy += x*y;
        sx += k*x;
        sy += k*y;
        s1 += k*k;
    }

    D = s1*sxx - sx*sx;
    a = (sy*sxx - sx*sxy)/D;
    b = (s1*sxy - sx*sy)/D;
    gwy_debug("a %g, b %g", a, b);
    *pvy = a;

    return b;
}

/* XXX: Reshuffles points! */
static guint*
sort_into_rows(GwyXY *points, guint n, gdouble yorigin, gdouble vy, guint *nrows)
{
    guint *row_sizes;
    GwyXY *newpoints;
    guint i, m = 0;

#ifdef _OPENMP
#pragma omp parallel for if(gwy_threads_are_enabled()) default(none) \
            reduction(max:m) \
            private(i) \
            shared(points,n,yorigin,vy)
#endif
    for (i = 0; i < n; i++) {
        gdouble y = points[i].y;
        guint k = GWY_ROUND((y - yorigin)/vy);
        m = MAX(k, m);
    }

    if (m >= G_MAXUINT/2) {
        *nrows = 0;
        return NULL;
    }

    m++;
    /* Add an extra element for block indexing. */
    row_sizes = g_new0(guint, m+1);
    for (i = 0; i < n; i++) {
        gdouble y = points[i].y;
        guint k = GWY_ROUND((y - yorigin)/vy);
        row_sizes[k]++;
    }

    gwy_accumulate_counts(row_sizes, m, TRUE);
    newpoints = g_new(GwyXY, n);
    for (i = 0; i < n; i++) {
        gdouble y = points[i].y;
        guint k = GWY_ROUND((y - yorigin)/vy);
        newpoints[row_sizes[k]++] = points[i];
    }
    gwy_assign(points, newpoints, n);
    g_free(newpoints);
    rewind_counts(row_sizes, m);

#ifdef _OPENMP
#pragma omp parallel for if(gwy_threads_are_enabled()) default(none) \
            private(i) \
            shared(points,m,row_sizes)
#endif
    for (i = 0; i < m; i++) {
        GwyXY *block = points + row_sizes[i];
        guint npts = row_sizes[i+1] - row_sizes[i];

        /* This sorts by the first double, i.e. the x-coordinate, which is exactly what we want. */
        qsort(block, npts, sizeof(GwyXY), gwy_compare_double);
    }

    *nrows = m;

    return row_sizes;
}

static gdouble
second_vector_meanx(const GwyXY *points, const guint *row_sizes, guint nrows, gdouble v0x, gdouble xoff,
                    guint *pn)
{
    gdouble m = 0.0;
    guint i, nv = 0;

    for (i = 0; i < nrows-1; i++) {
        const GwyXY *block1 = points + row_sizes[i], *block2 = points + row_sizes[i+1];
        guint npts1 = row_sizes[i+1] - row_sizes[i], npts2 = row_sizes[i+2] - row_sizes[i+1];
        guint i1 = 0, i2 = 0;

        while (i1 < npts1 && i2 < npts2) {
            gdouble d = block2[i2].x - (block1[i1].x + xoff);

            if (fabs(d) < 0.5*v0x) {
                m += d;
                nv++;
            }

            if (d <= 0.0)
                i2++;
            else
                i1++;
        }
    }

    if (pn)
        *pn = nv;

    return nv ? m/nv : 0.0;
}

static void
estimate_second_vector(const GwyXY *points, const guint *row_sizes, guint nrows, gdouble v0x,
                       gdouble *v1x)
{
    enum { noff = 5 };
    gdouble mean[noff], resid[noff], best, xbest;
    guint count[noff];
    guint ioff, best_ioff;

#ifdef _OPENMP
#pragma omp parallel for if(gwy_threads_are_enabled()) default(none) \
            private(ioff) \
            shared(points,row_sizes,nrows,mean,resid,count,v0x)
#endif
    for (ioff = 0; ioff < noff; ioff++) {
        gdouble xoff = (-0.5 + 0.25*ioff)*v0x, m, r = 0.0;
        guint i, nv = 0;

        m = second_vector_meanx(points, row_sizes, nrows, v0x, xoff, &nv);
        count[ioff] = nv;
        if (!nv) {
            mean[ioff] = 0.0;
            resid[ioff] = G_MAXDOUBLE;
            continue;
        }

        for (i = 0; i < nrows-1; i++) {
            const GwyXY *block1 = points + row_sizes[i], *block2 = points + row_sizes[i+1];
            guint npts1 = row_sizes[i+1] - row_sizes[i], npts2 = row_sizes[i+2] - row_sizes[i+1];
            guint i1 = 0, i2 = 0;

            while (i1 < npts1 && i2 < npts2) {
                gdouble d = block2[i2].x - (block1[i1].x + xoff);

                if (fabs(d) < 0.5*v0x)
                    r += (d - m)*(d - m);

                if (d <= 0.0)
                    i2++;
                else
                    i1++;
            }
        }

        mean[ioff] = m;
        resid[ioff] = sqrt(r/nv);
    }

    best = G_MAXDOUBLE;
    best_ioff = 0;
    for (ioff = 0; ioff < noff; ioff++) {
        gdouble xoff = (-0.5 + 0.25*ioff)*v0x;
        gdouble score = resid[ioff]/log(2 + count[ioff])/(1 - 0.25*fabs(xoff)/nrows);
        gwy_debug("[%u] off %g, score = %g, resid = %g, v1x/v0x = %g (%u values)",
                  ioff, xoff/v0x, score, resid[ioff], (mean[ioff] + xoff)/v0x, count[ioff]);
        if (score < best) {
            best_ioff = ioff;
            best = score;
        }
    }

    xbest = mean[best_ioff] + (-0.5 + 0.25*best_ioff)*v0x;
    *v1x = xbest + second_vector_meanx(points, row_sizes, nrows, v0x, xbest, NULL);
}

static void
estimate_longitudinal_mod(const GwyXY *points, guint *row_sizes, guint nrows,
                          const GwyXY *v0, const GwyXY *v1, GwyXY *origin)
{
    enum { noff = 4 };
    const gdouble yorigin = origin->y;
    gdouble invmat[4], mean[4], resid[4];
    guint nall = row_sizes[nrows] - row_sizes[0];
    guint ioff, best_ioff;
    gdouble best;

    invert_lattice_vectors(v0, v1, invmat);

#ifdef _OPENMP
#pragma omp parallel for if(gwy_threads_are_enabled()) default(none) \
            private(ioff) \
            shared(points,nrows,row_sizes,yorigin,invmat,mean,nall)
#endif
    for (ioff = 0; ioff < noff; ioff++) {
        gdouble m = 0.0, xoff = ioff/(gdouble)noff;
        guint k;

        for (k = 0; k < nrows; k++) {
            const GwyXY *block = points + row_sizes[k];
            gint n, npts = row_sizes[k+1] - row_sizes[k];

            for (n = 0; n < npts; n++) {
                GwyXY pt = block[n];

                pt.y -= yorigin;
                transform_xy(&pt, &pt, invmat);
                m += pt.x - floor(pt.x) - xoff;
            }
        }

        mean[ioff] = m/nall;
    }

#ifdef _OPENMP
#pragma omp parallel for if(gwy_threads_are_enabled()) default(none) \
            private(ioff) \
            shared(points,nrows,row_sizes,yorigin,invmat,mean,resid)
#endif
    for (ioff = 0; ioff < noff; ioff++) {
        gdouble s = 0.0, m = mean[ioff], xoff = ioff/(gdouble)noff;
        guint k;

        for (k = 0; k < nrows; k++) {
            const GwyXY *block = points + row_sizes[k];
            gint n, npts = row_sizes[k+1] - row_sizes[k];

            for (n = 0; n < npts; n++) {
                GwyXY pt = block[n];
                gdouble d;

                pt.y -= yorigin;
                transform_xy(&pt, &pt, invmat);
                d = pt.x - floor(pt.x) - xoff - m;
                s += d*d;
            }
        }
        resid[ioff] = s;
    }

    best_ioff = 0;
    best = resid[best_ioff];
    for (ioff = 1; ioff < noff; ioff++) {
        if (resid[ioff] < best) {
            best_ioff = ioff;
            best = resid[ioff];
        }
    }

    /* Offset of points along x at the yorigin line (with respect to lattice points). This is a number very close to
     * zero, i.e. directly usable as x origin, if used correctly. */
    origin->x = (mean[best_ioff] + best_ioff/(gdouble)noff)*v0->x;
}

static void
full_fit(const GwyXY *points, const guint *row_sizes, gint nrows,
         GwyXY *v0, GwyXY *v1, GwyXY *origin)
{
    const gdouble xorigin = origin->x, vx = v0->x, skew = v1->x;
    gdouble sn = 0.0, sk = 0.0, snn = 0.0, skk = 0.0, snk = 0.0;
    gdouble sx = 0.0, snx = 0.0, skx = 0.0, sy = 0.0, sny = 0.0, sky = 0.0;
    gdouble matrix[6], rhs[6];
    guint s1 = 0;
    gint k;

#ifdef _OPENMP
#pragma omp parallel for if(gwy_threads_are_enabled()) default(none) \
            reduction(+:s1,sn,sk,snn,snk,skk,sx,snx,skx,sy,sny,sky) \
            private(k) \
            shared(points,nrows,row_sizes,xorigin,vx,skew)
#endif
    for (k = 0; k < nrows; k++) {
        const GwyXY *block = points + row_sizes[k];
        gint n, npts = row_sizes[k+1] - row_sizes[k];
        gdouble kk = k, xorigin_row = xorigin + k*skew;
        gdouble sx_row = 0.0, sy_row = 0.0, sn_row = 0.0;

        for (n = 0; n < npts; n++) {
            gdouble x = block[n].x, y = block[n].y;
            gdouble nn = GWY_ROUND((x - xorigin_row)/vx);

            sn_row += nn;
            snn += nn*nn;
            sx_row += x;
            sy_row += y;
            snx += nn*x;
            sny += nn*y;
        }
        /* These do not depend on n in a new way so they can be updated easily from the row sums as k is a constant in
         * each row (the inner cycle). */
        s1 += npts;
        sk += kk*npts;
        skk += kk*kk*npts;
        sn += sn_row;
        snk += kk*sn_row;
        sx += sx_row;
        sy += sy_row;
        skx += kk*sx_row;
        sky += kk*sy_row;
    }

    matrix[0] = s1;
    matrix[1] = sn;
    matrix[2] = snn;
    matrix[3] = sk;
    matrix[4] = snk;
    matrix[5] = skk;
    gwy_math_choleski_decompose(3, matrix);

    rhs[0] = sx;
    rhs[1] = snx;
    rhs[2] = skx;
    gwy_math_choleski_solve(3, matrix, rhs);
    origin->x = rhs[0];
    v0->x = rhs[1];
    v1->x = rhs[2];

    rhs[0] = sy;
    rhs[1] = sny;
    rhs[2] = sky;
    gwy_math_choleski_solve(3, matrix, rhs);
    origin->y = rhs[0];
    v0->y = rhs[1];
    v1->y = rhs[2];
}

static gboolean
detect_grid(GwyGridDetector *gd)
{
    gdouble matrix1[4] = IDENTITY_MATRIX, matrix2[4] = IDENTITY_MATRIX, matrix[4] = IDENTITY_MATRIX, mat[4];
    GwyXY bb0, bb1, initial_offset, discrete_offset, translation, v0, v1, v0norm, origin, offset;
    GwyXY *points;
    guint *cell_sizes, *row_sizes, *point_index;
    guint i, cnx, cny, nsvec, nsvec_prev, itr, nrows, ncols, rowto, nnonempty, atleast;
    const guint n = gd->n, stride = gd->stride;
    gint imin, jmin, imax, jmax, xres, yres;
    gdouble b0, b1, maxdiff;
    GHashTable *gridcheck;
    gboolean grid_ok;

    points = gd->points = g_new(GwyXY, n);
    for (i = 0; i < n; i++) {
        points[i].x = gd->in_points[stride*i];
        points[i].y = gd->in_points[stride*i + 1];
    }

    /* Preprocess data to a more manageable state. If the shape is parallelogram we want to approxinately align one
     * of the sides with the x-axis. */
    center_on_origin(points, n, &initial_offset);
    prestraighen(points, n, matrix1);

    bounding_box(points, n, &bb0, &bb1);
    /* This physically shuffles the points. */
    cell_sizes = gd->sizes = sort_into_cells(points, n, &bb0, &bb1, &cnx, &cny);
    if (!cell_sizes) {
        gwy_debug("cellsize FAIL");
        return FALSE;
    }
    gwy_debug("cell grid %u %u", cnx, cny);

    gd->vectors = g_new(GwyXY, cnx*cny);
    nsvec = find_shortest_vectors(points, cnx, cny, cell_sizes, gd->vectors);

    gwy_debug("short vectors %u", nsvec);
    if (!nsvec) {
        gwy_debug("shortvec FAIL");
        return FALSE;
    }
    nsvec = filter_out_too_far_vectors(gd->vectors, nsvec);
    gwy_debug("filtered short vectors %u", nsvec);
    /* This has an obvious bias towards short vectors which, for skewed lattices, also means towards less skewed
     * points. Therefore, it is only useful as a somewhat improved initial estimate. For square grid the short vectors
     * are also a mix of two orthogonal populations, which we try to take care of by the mod π/2 thing. */
    estimate_mean_vector_mod_pi2(gd->vectors, nsvec, &v0);
    v0norm.x = v0.x/sqrt(norm_xy(&v0));
    v0norm.y = v0.y/sqrt(norm_xy(&v0));
    gwy_debug("v0 (%g,%g), v0norm (%g,%g)", v0.x, v0.y, v0norm.x, v0norm.y);
    GWY_FREE(gd->vectors);

    /* Get the first actually good estimate by finding the vector which best aligns with all chains of neighbours
     * (iteratively). This is the slowest bit. */
    itr = 0;
    nsvec = 0;
    gd->neighbours = g_new(guint, n);
    gd->preighbours = g_new(guint, n);
    do {
        nsvec_prev = nsvec;
        find_neighbours_along_vec(points, n, cnx, cny, cell_sizes, &v0, gd->neighbours, gd->preighbours);
        nsvec = estimate_mean_vector_from_chains(points, n, gd->neighbours, gd->preighbours, &v0);
        v0norm.x = v0.x/sqrt(norm_xy(&v0));
        v0norm.y = v0.y/sqrt(norm_xy(&v0));
        gwy_debug("[%u] v0 (%g,%g), v0norm (%g,%g) (%u vectors)", itr, v0.x, v0.y, v0norm.x, v0norm.y, nsvec);
        itr++;
    } while (nsvec > nsvec_prev && itr < 8);

    GWY_FREE(gd->sizes);
    cell_sizes = NULL;
    GWY_FREE(gd->preighbours);

    mat[0] = mat[3] = v0norm.x;
    mat[1] = v0norm.y;
    mat[2] = -v0norm.y;
    matrix_multiply_left(matrix1, mat);
    transform_points(points, n, mat);
    transform_xy(&v0, &v0, mat);

    gwy_debug("straightened v0 (%g, %g)", v0.x, v0.y);
    bounding_box(points, n, &bb0, &bb1);
    v1.y = estimate_orthogonal_spacing(points, n, bb0.y, bb1.y, v0.x);
    gwy_debug("vy %g", v1.y);
    if (v1.y <= 0.0) {
        gwy_debug("row-step FAIL");
        return FALSE;
    }
    origin.y = estimate_orthogonal_mod(points, n, bb0.y, v1.y) + bb0.y;
    if (GWY_ROUND((bb0.y - origin.y)/v1.y) != 0) {
        gwy_debug("y-origin FAIL");
        return FALSE;
    }

    /* Now we can classify points to rows. Fit the direction of all rows (not vector length here), rotate accordingly
     * and iterate to finally align everything. The lines should be basically horizontal, so we can fit with y = ax
     * + b without fear of infinities. */
    gwy_debug("line[1]");
    b0 = estimate_line_coeff_full(points, n, origin.y, bb1.y, &v1.y);
    matrix_from_tangent(b0, mat);
    matrix_multiply_left(matrix1, mat);
    transform_points(points, n, mat);
    bounding_box(points, n, &bb0, &bb1);
    origin.y = estimate_orthogonal_mod(points, n, bb0.y, v1.y) + bb0.y;

    gwy_debug("line[2]");
    b1 = estimate_line_coeff_full(points, n, origin.y, bb1.y, &v1.y);
    /* Geometric series acceleration. */
    b1 *= b0/(b0 - b1);
    matrix_from_tangent(b1, mat);
    matrix_multiply_left(matrix1, mat);
    transform_points(points, n, mat);
    bounding_box(points, n, &bb0, &bb1);
    origin.y = estimate_orthogonal_mod(points, n, bb0.y, v1.y) + bb0.y;

    GWY_FREE(gd->neighbours);

    /* This physically shuffles the points. */
    row_sizes = gd->sizes = sort_into_rows(points, n, origin.y, v1.y, &nrows);
    if (!row_sizes) {
        gwy_debug("rowcount FAIL");
        return FALSE;
    }
    gwy_debug("nrows %u", nrows);

    estimate_second_vector(points, row_sizes, nrows, v0.x, &v1.x);
    gwy_debug("v1x %g", v1.x);

    /* If the grid is skewed this can differ from xres quite a lot. As this point we use it for horizontal centering. */
    ncols = GWY_ROUND((bb1.x - bb0.x)/v0.x);

    nnonempty = 0;
    atleast = GWY_ROUND(log(1.0 + ncols) + 1);
    for (rowto = 0; rowto < nrows && nnonempty < 3; rowto++) {
        if (row_sizes[rowto+1] >= row_sizes[rowto] + atleast)
            nnonempty++;
    }

    estimate_longitudinal_mod(points, row_sizes, rowto, &v0, &v1, &origin);
    gwy_debug("[initial] v0 = (%g, %g), v1 = (%g, %g), origin = (%g, %g)",
              v0.x, v0.y, v1.x, v1.y, origin.x, origin.y);

    do {
        full_fit(points, row_sizes, MIN(rowto, nrows), &v0, &v1, &origin);
        gwy_debug("[%u rows] v0 = (%g, %g), v1 = (%g, %g), origin = (%g, %g)",
                  rowto+1, v0.x, v0.y, v1.x, v1.y, origin.x, origin.y);
        /* Repeat the last iteration to ensure convergence. */
        rowto = (rowto >= nrows ? rowto+1 : MIN(3*rowto, nrows));
    } while (rowto < nrows+2);

    transform_to_grid(points, n, &v0, &v1, &origin, matrix2);

    while (poststraighten(points, n, mat))
        matrix_multiply_left(matrix2, mat);

    GWY_FREE(gd->sizes);
    row_sizes = NULL;

    bounding_box(points, n, &bb0, &bb1);
    imin = GWY_ROUND(bb0.y);
    jmin = GWY_ROUND(bb0.x);
    imax = GWY_ROUND(bb1.y);
    jmax = GWY_ROUND(bb1.x);
    xres = jmax+1 - jmin;
    yres = imax+1 - imin;
    if (xres*yres > 3*(gint)n) {
        gwy_debug("xres, yres FAIL");
        return FALSE;
    }

    /* The full transformation is
     *
     *   matrix2⋅(matrix1⋅(xy - initial_offset) - origin) - discrete_offset
     *
     * So the overall matrix is M = matrix2⋅matrix1 (and the lattice vectors are columns of its inverse).
     * The origin (row = col = 0) in original coordinates is
     *
     *   initial_offset + inv(matrix1)⋅origin + inv(M)⋅discrete_offset
     */

    matrix_multiply_left(matrix, matrix1);
    matrix_multiply_left(matrix, matrix2);
    invert_matrix(matrix, mat);
    v0.x = mat[0];
    v0.y = mat[2];
    v1.x = mat[1];
    v1.y = mat[3];

    /* Calculate the forward transformation translation. */
    discrete_offset.x = jmin;
    discrete_offset.y = imin;
    translation = initial_offset;
    transform_xy(&translation, &translation, matrix1);
    add_xy(&translation, &translation, &origin);
    transform_xy(&translation, &translation, matrix2);
    add_xy(&translation, &translation, &discrete_offset);
    translation.x *= -1.0;
    translation.y *= -1.0;
    gwy_debug("translation (%g, %g)", translation.x, translation.y);

    /* Calculate the origin in original coodinates. */
    offset = initial_offset;
    invert_matrix(matrix1, matrix1);
    transform_xy(&origin, &origin, matrix1);
    add_xy(&offset, &offset, &origin);
    transform_xy(&discrete_offset, &discrete_offset, mat);
    add_xy(&offset, &discrete_offset, &discrete_offset);

    gwy_debug("in original coordinates (xres = %d, yres = %d)", xres, yres);
    gwy_debug("vx = (%g, %g)", v0.x, v0.y);
    gwy_debug("vy = (%g, %g)", v1.x, v1.y);
    gwy_debug("r0 = (%g, %g)", offset.x, offset.y);

    gridcheck = gd->gridcheck = g_hash_table_new(g_direct_hash, g_direct_equal);
    point_index = gd->point_index = g_new(guint, n);
    grid_ok = TRUE;
    maxdiff = 0.0;
    for (i = 0; i < n; i++) {
        GwyXY pt;
        gint col, row;
        gdouble d2;

        pt.x = gd->in_points[stride*i];
        pt.y = gd->in_points[stride*i + 1];
        transform_xy(&pt, &pt, matrix);
        add_xy(&pt, &pt, &translation);
        col = GWY_ROUND(pt.x);
        row = GWY_ROUND(pt.y);

        //gwy_debug("%d %d (%g %g)", col, row, pt.x, pt.y);
        if (col < 0 || col >= xres || row < 0 || row >= yres) {
            grid_ok = FALSE;
            break;
        }

        d2 = (col - pt.x)*(col - pt.x) + (row - pt.y)*(row - pt.y);
        maxdiff = fmax(d2, maxdiff);
        point_index[i] = row*xres + col;
        if (g_hash_table_lookup(gridcheck, GUINT_TO_POINTER(point_index[i]))) {
            grid_ok = FALSE;
            break;
        }
        g_hash_table_insert(gridcheck, GUINT_TO_POINTER(point_index[i]), GUINT_TO_POINTER(i+1));
    }

    if (!grid_ok) {
        gwy_debug("grid reconstruction FAIL");
        return FALSE;
    }

    gd->xres = xres;
    gd->yres = yres;
    gd->v0 = v0;
    gd->v1 = v1;
    gd->offset = offset;
    gd->maxdiff = sqrt(maxdiff);

    return TRUE;
}

/**
 * gwy_check_regular_2d_lattice:
 * @coords: Array of @n coordinate pairs in plane.  You can also typecast #GwyXY or #GwyXYZ to doubles.
 * @stride: Actual number of double values in one block.  It must be at least 2 if @coords contains just alternating
 *          @x and @y.  If you pass an typecast #GwyXYZ array give stride as 3, etc.
 * @n: Number of items in @coords.
 * @xres: (out): Location where to store the number of columns.
 * @yres: (out): Location where to store the number of rows.
 * @xymin: (out): Location where to store the minimum coordinates (top left corner of the corresponding image).
 * @vx: (out): Location where to store the pixel step vector along @x in the image.
 * @vy: (out): Location where to store the pixel step vector along @y in the image.
 * @maxdiff: (out) (optional): Location where to store the maximum difference of a point from exact lattice location, or %NULL. The
 *           value is in image pixel coordinates, i.e. value 1 corresponds to lattice step.
 *
 * Detects if points in plane form more or less a regular lattice.
 *
 * At present points lying in one straight line are not considered to form a lattice and neither are points sets with
 * duplicate points (corresponding to the same lattice point).
 *
 * Any rotation in the plane, a moderate amount of missing points, moderate skew and moderate deviations from the
 * exact lattice positions should not prevent lattice detection. Large defects may, and so may large differences
 * between lattice step sizes. The function prefers basis vectors @vx and @vy in which the point set resembles
 * a rectangle. When the goal is to render the data to an image this is normally the desired behaviour. However, for
 * an odd overal shape of the point set the result may not be useful.
 *
 * The caller can check for holes in the lattice by comparing returned @xres×@yres with @n, for skew by evaluating the
 * angle between @vx and @vy and for deviations by inspecting @maxdiff.
 *
 * When the function fails, i.e. return %NULL, the values of output arguments are undefined.
 *
 * See also gwy_check_regular_2d_grid() which is faster for simple complete Cartesian grids and may work better for
 * them if the steps in @x and @y differ a lot.
 *
 * Returns: On success, a newly allocated array mapping grid indices (@i*@xres+@j) to indices in @coords.  %NULL is
 *          returned on failure.
 **/
guint*
gwy_check_regular_2d_lattice(const gdouble *coords,
                             guint stride,
                             guint n,
                             guint *xres,
                             guint *yres,
                             GwyXY *xymin,
                             GwyXY *vx,
                             GwyXY *vy,
                             gdouble *maxdiff)
{
    GwyGridDetector gd;
    guint *point_index = NULL;

    g_return_val_if_fail(stride >= 2, NULL);
    g_return_val_if_fail(coords || !n, NULL);
    g_return_val_if_fail(xres && yres && xymin && vx && vy, NULL);

    if (n < 4)
        return NULL;

    gwy_clear1(gd);
    gd.n = n;
    gd.stride = stride;
    gd.in_points = coords;
    if (detect_grid(&gd)) {
        point_index = gd.point_index;
        *xres = gd.xres;
        *yres = gd.yres;
        *xymin = gd.offset;
        *vx = gd.v0;
        *vy = gd.v1;
        if (maxdiff)
            *maxdiff = gd.maxdiff;
    }
    else {
        GWY_FREE(gd.point_index);
    }

    GWY_FREE(gd.points);
    GWY_FREE(gd.vectors);
    GWY_FREE(gd.sizes);
    GWY_FREE(gd.neighbours);
    GWY_FREE(gd.preighbours);
    if (gd.gridcheck) {
        g_hash_table_destroy(gd.gridcheck);
        gd.gridcheck = NULL;
    }

    return point_index;
}

/* vim: set cin columns=120 tw=118 et ts=4 sw=4 cino=>1s,e0,n0,f0,{0,}0,^0,\:1s,=0,g1s,h0,t0,+1s,c3,(0,u0 : */
