/*
 # This file is part of the Astrometry.net suite.
 # Licensed under a 3-clause BSD style license - see LICENSE
 */

#include <unistd.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <assert.h>

#include "os-features.h"
#include "kdtree.h"
#include "starutil.h"
#include "mathutil.h"
#include "bl.h"
#include "bl-sort.h"
#include "matchobj.h"
#include "catalog.h"
#include "tic.h"
#include "quadfile.h"
#include "quad-utils.h"
#include "xylist.h"
#include "rdlist.h"
#include "qidxfile.h"
#include "verify.h"
#include "ioutils.h"
#include "starkd.h"
#include "codekd.h"
#include "index.h"
#include "boilerplate.h"
#include "sip.h"
#include "sip-utils.h"
#include "sip_qfits.h"
#include "log.h"
#include "fitsioutils.h"
#include "fit-wcs.h"
#include "codefile.h"
#include "solver.h"
#include "permutedsort.h"
#include "intmap.h"

#include "plotstuff.h"
#include "plotxy.h"
#include "plotindex.h"

static const char* OPTIONS = "hx:w:i:vj:X:Y:Q:s:O:";

void print_help(char* progname) {
    BOILERPLATE_HELP_HEADER(stdout);
    printf("\nUsage: %s\n"
           "   -w <WCS input file>\n"
           "   -x <xyls input file>\n"
           "   -i <index-name>\n"
           "   [-X <x-column>]: X column name\n"
           "   [-Y <y-column>]: Y column name\n"
           "   [-v]: verbose\n"
           "   [-j <pixel-jitter>]: set pixel jitter (default 1.0)\n"
           "   [-Q <quad-plot-basename>]: plot all the quads in the image, one per file.  Put a %%i in this filename!\n"
           "   [-O <obj-plot-basename>]: make a plot for each image object; Put a %%i in this filename\n"
           "   [-s <plot-scale>]: scale everything by this factor (eg, 0.25)\n"
           "\n", progname);
}


struct foundquad {
    //unsigned int stars[DQMAX];
    double codedist;
    double logodds;
    double pscale;
    int quadnum;
    MatchObj mo;
};
typedef struct foundquad foundquad_t;

static int sort_fq_by_stars(const void* v1, const void* v2) {
    const foundquad_t* fq1 = v1;
    const foundquad_t* fq2 = v2;
    int mx1=0, mx2=0;
    int i;
    for (i=0; i<DQMAX; i++) {
        mx1 = MAX(mx1, fq1->mo.field[i]);
        mx2 = MAX(mx2, fq2->mo.field[i]);
    }
    if (mx1 < mx2)
        return -1;
    if (mx1 == mx2)
        return 0;
    return 1;
}

struct correspondence {
    int star;
    int field;
    double starx, stary;
    double fieldx, fieldy;
    double dist;
};
typedef struct correspondence corr_t;

struct indexstar {
    int star;
    double starx, stary;
    pl* corrs;
    il* allquads;
};
typedef struct indexstar indexstar_t;


int main(int argc, char** args) {
    int c;
    char* xylsfn = NULL;
    char* wcsfn = NULL;

    sl* indexnames;
    pl* indexes;
    pl* qidxes;

    xylist_t* xyls = NULL;
    sip_t sip;
    int i;
    int W, H;
    double xyzcenter[3];
    double fieldrad2;
    double pixeljitter = 1.0;
    int loglvl = LOG_MSG;
    double wcsscale;

    char* xcol = NULL;
    char* ycol = NULL;
	
    double nsigma = 3.0;

    char* quadplotfn = NULL;
    char* objplotfn = NULL;
    double quadplotscale = 1.0;

    fits_use_error_system();

    indexnames = sl_new(8);

    while ((c = getopt(argc, args, OPTIONS)) != -1) {
        switch (c) {
        case 's':
            quadplotscale = atof(optarg);
            break;
        case 'Q':
            quadplotfn = optarg;
            break;
        case 'O':
            objplotfn = optarg;
            break;
        case 'X':
            xcol = optarg;
            break;
        case 'Y':
            ycol = optarg;
            break;
        case 'j':
            pixeljitter = atof(optarg);
            break;
        case 'h':
            print_help(args[0]);
            exit(0);
        case 'i':
            sl_append(indexnames, optarg);
            break;
        case 'x':
            xylsfn = optarg;
            break;
        case 'w':
            wcsfn = optarg;
            break;
        case 'v':
            loglvl++;
            break;
        }
    }
    if (optind != argc) {
        print_help(args[0]);
        exit(-1);
    }
    if (!xylsfn || !wcsfn) {
        print_help(args[0]);
        exit(-1);
    }
    log_init(loglvl);

    // read WCS.
    logmsg("Trying to parse SIP header from %s...\n", wcsfn);
    if (!sip_read_header_file(wcsfn, &sip)) {
        logmsg("Failed to parse SIP header from %s.\n", wcsfn);
    }
    // image W, H
    W = sip.wcstan.imagew;
    H = sip.wcstan.imageh;
    if ((W == 0.0) || (H == 0.0)) {
        logmsg("WCS file %s didn't contain IMAGEW and IMAGEH headers.\n", wcsfn);
        // FIXME - use bounds of xylist?
        exit(-1);
    }
    wcsscale = sip_pixel_scale(&sip);
    logmsg("WCS scale: %g arcsec/pixel\n", wcsscale);

    // read XYLS.
    xyls = xylist_open(xylsfn);
    if (!xyls) {
        logmsg("Failed to read an xylist from file %s.\n", xylsfn);
        exit(-1);
    }
    if (xcol)
        xylist_set_xname(xyls, xcol);
    if (ycol)
        xylist_set_yname(xyls, ycol);
    xylist_set_include_flux(xyls, FALSE);
    xylist_set_include_background(xyls, FALSE);



    // read indices.
    indexes = pl_new(8);
    qidxes = pl_new(8);
    for (i=0; i<sl_size(indexnames); i++) {
        char* name = sl_get(indexnames, i);
        index_t* indx;
        char* qidxfn;
        qidxfile* qidx;
        logmsg("Loading index from %s...\n", name);
        indx = index_load(name, 0, NULL);
        if (!indx) {
            logmsg("Failed to read index \"%s\".\n", name);
            exit(-1);
        }
        pl_append(indexes, indx);

        logmsg("Index name: %s\n", indx->indexname);

        qidxfn = index_get_qidx_filename(indx->indexname);
        qidx = qidxfile_open(qidxfn);
        if (!qidx) {
            logmsg("Failed to open qidxfile \"%s\".\n", qidxfn);
            exit(-1);            
        }
        free(qidxfn);
        pl_append(qidxes, qidx);
    }
    sl_free2(indexnames);

    // Find field center and radius.
    sip_pixelxy2xyzarr(&sip, W/2, H/2, xyzcenter);
    fieldrad2 = arcsec2distsq(sip_pixel_scale(&sip) * hypot(W/2, H/2));

    // Find all stars in the field.
    for (i=0; i<pl_size(indexes); i++) {
        index_t* indx;
        int nquads;
        uint32_t* quads;
        int j, k;
        qidxfile* qidx;
        il* uniqquadlist;
        bl* foundquads = NULL;

        // index stars that are inside the image.
        il* starlist;

        // index stars that have correspondences.
        il* corrstarlist;

        // quads that are at least partly-contained in the image.
        il* quadlist;

        // quads that are fully-contained in the image.
        il* fullquadlist;

        // index stars that are in partly-contained quads.
        il* starsinquadslist;

        // index stars that are in fully-contained quads.
        il* starsinquadsfull;

        // index stars that are in quads and have correspondences.
        il* corrstars;
        // the corresponding field stars
        il* corrfield;

        // quads that are fully in the image and built from stars with correspondences.
        il* corrfullquads;


        // Map from index stars to list of field stars corresponding.
        //intmap_t* corr_i2f;
        //intmap_t* corr_f2i;

        bl* corrs;

        indexstar_t* istars;
	
        dl* starxylist;
        il* corrquads;
        il* corruniqquads;
        starxy_t* xy;

        // (x,y) positions of field stars.
        double* fieldxy;

        int Nfield;
        kdtree_t* ftree;
        kdtree_t* itree;
        int Nleaf = 5;
        int dimquads, dimcodes;
        int ncorr, nindexcorr;
        double pixr2;
        il* indstarswithcorrs;

        double* xyzs = NULL;

        // Indices (in the star-kdtree) of stars that are within the image rectangle;
        // Ngood of them.
        int* starinds = NULL;

        int Nindex;
        int Ngood;

        indx = pl_get(indexes, i);
        qidx = pl_get(qidxes, i);

        corrs = bl_new(256, sizeof(corr_t));

        logmsg("Index jitter: %g arcsec (%g pixels)\n", indx->index_jitter, indx->index_jitter / wcsscale);
        pixr2 = square(indx->index_jitter / wcsscale) + square(pixeljitter);
        logmsg("Total jitter: %g pixels\n", sqrt(pixr2));

        // Read field
        xy = xylist_read_field(xyls, NULL);
        if (!xy) {
            logmsg("Failed to read xyls entries.\n");
            exit(-1);
        }
        Nfield = starxy_n(xy);
        fieldxy = starxy_to_xy_array(xy, NULL);
        logmsg("Found %i field objects\n", Nfield);

        // Find index stars.
        startree_search_for(indx->starkd, xyzcenter, fieldrad2*1.05,
                            &xyzs, NULL, &starinds, &Nindex);
        if (!Nindex) {
            logmsg("No index stars found.\n");
            exit(-1);
        }
        logmsg("Found %i index stars in range.\n", Nindex);

        starlist = il_new(16);
        corrstarlist = il_new(16);
        starxylist = dl_new(16);

        // Find which ones in range are inside the image rectangle.
        Ngood = 0;
        for (j=0; j<Nindex; j++) {
            int starnum = starinds[j];
            double x, y;
            if (!sip_xyzarr2pixelxy(&sip, xyzs + j*3, &x, &y))
                continue;
            if ((x < 0) || (y < 0) || (x >= W) || (y >= H))
                continue;
            il_append(starlist, starnum);
            dl_append(starxylist, x);
            dl_append(starxylist, y);
            // compact this array...
            starinds[Ngood] = starnum;
            Ngood++;
        }
        logmsg("Found %i index stars inside the field.\n", (int)il_size(starlist));

        assert(Ngood == il_size(starlist));
        assert(Ngood * 2 == dl_size(starxylist));

        istars = malloc(Ngood * sizeof(indexstar_t));

        // Now find correspondences between index objects and field objects.
        // Build a tree out of the field objects (in pixel space)
        // We make a copy of fieldxy because the copy gets permuted in this process.
        {
            double* fxycopy = malloc(Nfield * 2 * sizeof(double));
            memcpy(fxycopy, fieldxy, Nfield * 2 * sizeof(double));
            ftree = kdtree_build(NULL, fxycopy, Nfield, 2, Nleaf, KDTT_DOUBLE, KD_BUILD_SPLIT);
        }
        if (!ftree) {
            logmsg("Failed to build kdtree.\n");
            exit(-1);
        }

        // Search for correspondences
        /*
         corr_i2f = intmap_new(sizeof(int), 4, 256, 0);
         corr_f2i = intmap_new(sizeof(int), 4, 256, 0);
         */
        ncorr = 0;
        nindexcorr = 0;
        indstarswithcorrs = il_new(16);
        for (j=0; j<dl_size(starxylist)/2; j++) {
            double xy[2];
            kdtree_qres_t* res;
            int nn;
            double nnd2;
            corr_t c;
            xy[0] = dl_get(starxylist, j*2+0);
            xy[1] = dl_get(starxylist, j*2+1);

            // kdtree check.
            /*
             int k;
             for (k=0; k<Nfield; k++) {
             double d2 = distsq(fieldxy + 2*k, xy, 2);
             if (d2 < pixr2) {
             logverb("  index star at (%.1f, %.1f) and field star at (%.1f, %.1f)\n", xy[0], xy[1],
             fieldxy[2*k+0], fieldxy[2*k+1]);
             }
             }
             */

            nn = kdtree_nearest_neighbour(ftree, xy, &nnd2);
            logverb("  Index star at (%.1f, %.1f): nearest field star is %g pixels away.\n",
                    xy[0], xy[1], sqrt(nnd2));

            res = kdtree_rangesearch_options(ftree, xy, pixr2 * nsigma*nsigma, KD_OPTIONS_SMALL_RADIUS | KD_OPTIONS_COMPUTE_DISTS | KD_OPTIONS_SORT_DISTS);

            istars[j].star = il_get(starlist, j);
            istars[j].starx = dl_get(starxylist, 2*j+0);
            istars[j].stary = dl_get(starxylist, 2*j+1);
            istars[j].corrs = pl_new(16);
            istars[j].allquads = il_new(16);

            if (!res || !res->nres)
                continue;

            for (k=0; k<res->nres; k++) {
                corr_t* cc;
                memset(&c, 0, sizeof(c));
                c.star = il_get(starlist, j);
                c.field = res->inds[k];
                c.starx = dl_get(starxylist, 2*j+0);
                c.stary = dl_get(starxylist, 2*j+1);
                c.fieldx = fieldxy[2*res->inds[k]+0];
                c.fieldy = fieldxy[2*res->inds[k]+1];
                c.dist = sqrt(res->sdists[k]);
                // add to master list, recording ptr.
                cc = bl_append(corrs, &c);
                pl_append(istars[j].corrs, cc);
            }

            /*
             for (k=0; k<res->nres; k++) {
             intmap_append(corr_i2f, il_get(starlist, j), res->inds + k);
             }
             */

            ncorr += res->nres;
            nindexcorr++;
            kdtree_free_query(res);
            il_append(indstarswithcorrs, j);

            il_append(corrstarlist, il_get(starlist, j));
        }
        logmsg("Found %i index stars with corresponding field stars.\n", nindexcorr);
        logmsg("Found %i total index star correspondences\n", ncorr);

        {
            double* ixycopy = malloc(Ngood * 2 * sizeof(double));
            dl_copy(starxylist, 0, Ngood * 2, ixycopy);
            itree = kdtree_build(NULL, ixycopy, Ngood, 2, Nleaf, KDTT_DOUBLE, KD_BUILD_SPLIT);
        }
        if (!itree) {
            logmsg("Failed to build index kdtree.\n");
            exit(-1);
        }
        for (j=0; j<Nfield; j++) {
            double* fxy;
            int nn;
            kdtree_qres_t* res;
            double nnd2;

            fxy = fieldxy + 2*j;

            logverb("Field object %i: (%g, %g)\n", j, fxy[0], fxy[1]);

            nn = kdtree_nearest_neighbour(itree, fxy, &nnd2);
            logverb("  Nearest index star is %g pixels away.\n",
                    sqrt(nnd2));

            res = kdtree_rangesearch_options(itree, fxy, pixr2 * nsigma*nsigma, KD_OPTIONS_SMALL_RADIUS);
            if (!res || !res->nres) {
                logverb("  No index stars within %g pixels\n", sqrt(pixr2)*nsigma);
                continue;
            }
            logverb("  %i index stars within %g pixels\n", res->nres, sqrt(pixr2)*nsigma);

            /*
             for (k=0; k<res->nres; k++) {
             intmap_append(corr_f2i, j, il_access(starlist, res->inds[k]));
             }
             */
            //intmap_append(corr_f2i, res->inds[k], il_access(starlist, j));
            kdtree_free_query(res);
        }


        if (log_get_level() >= LOG_VERB) {
            // See what quads could be built from the index stars with correspondences.
            double* corrxy;
            int k, N = il_size(indstarswithcorrs);
            corrxy = malloc(N * 2 * sizeof(double));
            for (j=0; j<N; j++) {
                int ind = il_get(indstarswithcorrs, j);
                corrxy[2*j+0] = dl_get(starxylist, ind*2+0);
                corrxy[2*j+1] = dl_get(starxylist, ind*2+1);
            }
            for (j=0; j<N; j++) {
                for (k=0; k<j; k++) {
                    int m;
                    double q2 = distsq(corrxy + j*2, corrxy + k*2, 2);
                    double qc[2];
                    int nvalid;
                    qc[0] = (corrxy[j*2+0] + corrxy[k*2+0]) / 2.0;
                    qc[1] = (corrxy[j*2+1] + corrxy[k*2+1]) / 2.0;
                    nvalid = 0;
                    for (m=0; m<N; m++) {
                        if (m == j || m == k)
                            continue;
                        if (distsq(qc, corrxy + m*2, 2) < q2/4.0)
                            nvalid++;
                    }
                    logverb("  Quad diameter: %g pix (%g arcmin): %i stars in the circle.\n",
                            sqrt(q2), sqrt(q2) * wcsscale / 60.0, nvalid);
                }
            }
            free(corrxy);
        }

        uniqquadlist = il_new(16);
        quadlist = il_new(16);

        // For each index star, find the quads of which it is part.
        for (j=0; j<il_size(starlist); j++) {
            int k;
            int starnum = il_get(starlist, j);
            if (qidxfile_get_quads(qidx, starnum, &quads, &nquads)) {
                logmsg("Failed to get quads for star %i.\n", starnum);
                exit(-1);
            }
            //logmsg("star %i is involved in %i quads.\n", starnum, nquads);
            for (k=0; k<nquads; k++) {
                il_insert_ascending(quadlist, quads[k]);
                il_insert_unique_ascending(uniqquadlist, quads[k]);

                il_append(istars[j].allquads, quads[k]);
            }
        }
        logmsg("Found %i quads partially contained in the field.\n", (int)il_size(uniqquadlist));

        dimquads = quadfile_dimquads(indx->quads);
        dimcodes = dimquad2dimcode(dimquads);

        // Find quads that are fully contained in the image.
        fullquadlist = il_new(16);
        for (j=0; j<il_size(uniqquadlist); j++) {
            int quad = il_get(uniqquadlist, j);
            int ind = il_index_of(quadlist, quad);
            if (log_get_level() >= LOG_VERB) {
                int k, nin=0, ncorr=0;
                unsigned int stars[dimquads];
                for (k=0; k<dimquads; k++) {
                    if (ind+k >= il_size(quadlist))
                        break;
                    if (il_get(quadlist, ind+k) != quad)
                        break;
                    nin++;
                }
                quadfile_get_stars(indx->quads, quad, stars);
                for (k=0; k<dimquads; k++)
                    if (il_contains(corrstarlist, stars[k]))
                        ncorr++;
                debug("Quad %i has %i stars in the field (%i with correspondences).\n", quad, nin, ncorr);
            }
            if (ind + (dimquads-1) >= il_size(quadlist))
                continue;
            if (il_get(quadlist, ind+(dimquads-1)) != quad)
                continue;
            il_append(fullquadlist, quad);
        }
        logmsg("Found %i quads fully contained in the field.\n", (int)il_size(fullquadlist));


        {
            double* sortdata = startree_get_data_column(indx->starkd, "r", starinds, Ngood);
            int k;
            int* sweeps = malloc(Ngood * sizeof(int));
            int* perm = NULL;
            int* maxsweep = malloc(il_size(uniqquadlist) * sizeof(int));
            unsigned int stars[DQMAX];

            // DEBUG -- how does "sweep" correspond to column "r" ?
            // (answer: small sweep --> small r, pretty much.)
            for (k=0; k<Ngood; k++) {
                sweeps[k] = startree_get_sweep(indx->starkd, starinds[k]);
            }
            perm = permuted_sort(sweeps, sizeof(int), compare_ints_asc, NULL, Ngood);
            if (sortdata) {
                for (k=0; k<Ngood; k++) {
                    logverb("  sweep %i, r mag %g\n", sweeps[perm[k]], sortdata[perm[k]]);
                }
            }

            /*
             for (k=0; k<Ngood; k++) {
             int star = starinds[perm[k]];
             double sd = sortdata[perm[k]];
             il* lst;
             lst = (il*)intmap_find(corr_i2f, star, FALSE);
             logmsg("Index star %i (%i), sweep %i, mag %g.  %i correspondences\n", k, star, sweeps[perm[k]], sd, (lst ? il_size(lst) : 0));
             if (lst) {
             for (j=0; j<il_size(lst); j++) {
             logmsg("  field star %i\n", il_get(lst, j));
             }
             }
             }
             */


            free(perm);


            for (j=0; j<il_size(uniqquadlist); j++) {
                int quad = il_get(uniqquadlist, j);
                int ms = 0;
                quadfile_get_stars(indx->quads, quad, stars);
                for (k=0; k<dimquads; k++) {
                    int sweep = startree_get_sweep(indx->starkd, stars[k]);
                    ms = MAX(ms, sweep);
                }
                maxsweep[j] = ms;
            }
            perm = permuted_sort(maxsweep, sizeof(int), compare_ints_asc, NULL, il_size(uniqquadlist));

            logverb("\nQuads completely within the image:\n");
            for (j=0; j<il_size(uniqquadlist); j++) {
                int quad = il_get(uniqquadlist, perm[j]);
                if (!il_contains(fullquadlist, quad))
                    continue;
                quadfile_get_stars(indx->quads, quad, stars);
                logverb("(full) quad %i: made from stars with sweeps:", quad);
                for (k=0; k<dimquads; k++) {
                    int sweep = startree_get_sweep(indx->starkd, stars[k]);
                    logverb(" %i", sweep);
                }
                logverb("\n");
            }

            logverb("\nNearby quads, not completely within the image:\n");
            for (j=0; j<il_size(uniqquadlist); j++) {
                int quad = il_get(uniqquadlist, perm[j]);
                if (il_contains(fullquadlist, quad))
                    continue;
                quadfile_get_stars(indx->quads, quad, stars);
                logverb("(near) quad %i: made from stars with sweeps:", quad);
                for (k=0; k<dimquads; k++) {
                    int sweep = startree_get_sweep(indx->starkd, stars[k]);
                    logverb(" %i", sweep);
                }
                logverb("\n");
            }

            free(perm);
            free(maxsweep);

            free(sweeps);
            free(sortdata);
        }



        if (quadplotfn) {
            for (j=0; j<il_size(fullquadlist); j++) {
                char* fn;
                int SW, SH;
                int k;
                plot_args_t* pargs;
                plotxy_t* pxy;
                plotindex_t* pind;
                int quad;
                unsigned int stars[dimquads];
                double quadxy[DQMAX * 2];

                asprintf(&fn, quadplotfn, j);
                SW = (int)(quadplotscale * W);
                SH = (int)(quadplotscale * H);

                pargs = plotstuff_new();
                pargs->outformat = PLOTSTUFF_FORMAT_PNG;
                pargs->outfn = fn;
                plotstuff_set_size(pargs, SW, SH);

                plotstuff_set_color(pargs, "black");
                plotstuff_plot_layer(pargs, "fill");
                pargs->lw = 2.0;

                pxy = plot_xy_get(pargs);
                pxy->scale = quadplotscale;

                plotstuff_set_color(pargs, "red");
                plotstuff_set_markersize(pargs, 6);
                for (k=0; k<Nfield; k++)
                    plot_xy_vals(pxy, fieldxy[2*k+0], fieldxy[2*k+1]);
                plotstuff_plot_layer(pargs, "xy");

                plotstuff_set_color(pargs, "green");
                //plotstuff_set_marker(pargs, "crosshair");
                plotstuff_set_markersize(pargs, 4);
                for (k=0; k<dl_size(starxylist)/2; k++)
                    plot_xy_vals(pxy, dl_get(starxylist, 2*k+0), dl_get(starxylist, 2*k+1));
                plotstuff_plot_layer(pargs, "xy");

                pind = plot_index_get(pargs);

                quad = il_get(fullquadlist, j);
                quadfile_get_stars(indx->quads, quad, stars);
                for (k=0; k<dimquads; k++) {
                    int m = il_index_of(starlist, stars[k]);
                    quadxy[k*2+0] = dl_get(starxylist, 2*m+0) * quadplotscale;
                    quadxy[k*2+1] = dl_get(starxylist, 2*m+1) * quadplotscale;
                }

                plot_quad_xy(pargs->cairo, quadxy, dimquads);
                cairo_stroke(pargs->cairo);

                plotstuff_output(pargs);

                plotstuff_free(pargs);
                logmsg("Wrote %s\n", fn);
                free(fn);
            }
        }


        // Find the stars that are in quads.
        starsinquadslist = il_new(16);
        for (j=0; j<il_size(uniqquadlist); j++) {
            int k;
            unsigned int stars[dimquads];
            int quad = il_get(uniqquadlist, j);
            quadfile_get_stars(indx->quads, quad, stars);
            for (k=0; k<dimquads; k++)
                il_insert_unique_ascending(starsinquadslist, stars[k]);
        }
        logmsg("Found %i index stars involved in quads (with at least one star contained in the image).\n", (int)il_size(starsinquadslist));

        // Find the stars that are in quads that are completely contained.
        starsinquadsfull = il_new(16);
        for (j=0; j<il_size(fullquadlist); j++) {
            int k;
            unsigned int stars[dimquads];
            int quad = il_get(fullquadlist, j);
            quadfile_get_stars(indx->quads, quad, stars);
            for (k=0; k<dimquads; k++)
                il_insert_unique_ascending(starsinquadsfull, stars[k]);
        }
        logmsg("Found %i index stars involved in quads (with all stars contained in the image).\n", (int)il_size(starsinquadsfull));

        // For each index object involved in quads, search for a correspondence.
        corrstars = il_new(16);
        corrfield = il_new(16);
        for (j=0; j<il_size(starsinquadslist); j++) {
            int star;
            double sxyz[3];
            double sxy[2];
            kdtree_qres_t* fres;
            star = il_get(starsinquadslist, j);
            if (startree_get(indx->starkd, star, sxyz)) {
                logmsg("Failed to get position for star %i.\n", star);
                exit(-1);
            }
            if (!sip_xyzarr2pixelxy(&sip, sxyz, sxy, sxy+1)) {
                logmsg("SIP backward for star %i.\n", star);
                exit(-1);
            }
            fres = kdtree_rangesearch_options(ftree, sxy, pixr2 * nsigma*nsigma,
                                              KD_OPTIONS_SMALL_RADIUS | KD_OPTIONS_SORT_DISTS);
            if (!fres || !fres->nres)
                continue;
            if (fres->nres > 1) {
                logmsg("%i matches for star %i.\n", fres->nres, star);
            }

            il_append(corrstars, star);
            il_append(corrfield, fres->inds[0]); //kdtree_permute(ftree, fres->inds[0]));

            logverb("  star %i: dist %g to field star %i\n", star, sqrt(fres->sdists[0]), fres->inds[0]);

            /*{
             double fx, fy;
             int fi;
             fi = il_get(corrfield, il_size(corrfield)-1);
             fx = fieldxy[2*fi + 0];
             fy = fieldxy[2*fi + 1];
             logmsg("star   %g,%g\n", sxy[0], sxy[1]);
             logmsg("field  %g,%g\n", fx, fy);
             }*/
        }
        logmsg("Found %i correspondences for stars involved in quads (with at least one star in the field).\n",
               (int)il_size(corrstars));

        // Find quads built only from stars with correspondences.
        corrquads = il_new(16);
        corruniqquads = il_new(16);
        for (j=0; j<il_size(corrstars); j++) {
            int k;
            int starnum = il_get(corrstars, j);
            if (qidxfile_get_quads(qidx, starnum, &quads, &nquads)) {
                logmsg("Failed to get quads for star %i.\n", starnum);
                exit(-1);
            }
            for (k=0; k<nquads; k++) {
                il_insert_ascending(corrquads, quads[k]);
                il_insert_unique_ascending(corruniqquads, quads[k]);
            }
        }

        // Find quads that are fully contained in the image.
        logverb("Looking at quads built from stars with correspondences...\n");
        corrfullquads = il_new(16);

        for (j=0; j<il_size(corruniqquads); j++) {
            int quad = il_get(corruniqquads, j);
            int ind = il_index_of(corrquads, quad);

            if (log_get_level() >= LOG_VERB) {
                int k, nin=0;
                for (k=0; k<dimquads; k++) {
                    if (ind+k >= il_size(corrquads))
                        break;
                    if (il_get(corrquads, ind+k) != quad)
                        break;
                    nin++;
                }
                debug("  Quad %i has %i stars with correspondences.\n", quad, nin);
            }

            if (ind + (dimquads-1) >= il_size(corrquads))
                continue;
            if (il_get(corrquads, ind+(dimquads-1)) != quad)
                continue;
            il_append(corrfullquads, quad);
        }
        logmsg("Found %i quads built from stars with correspondencs, fully contained in the field.\n", (int)il_size(corrfullquads));

        foundquads = bl_new(16, sizeof(foundquad_t));

        for (j=0; j<il_size(corrfullquads); j++) {
            unsigned int stars[dimquads];
            int k;
            int ind;
            double realcode[dimcodes];
            double fieldcode[dimcodes];
            tan_t wcs;
            MatchObj mo;
            foundquad_t fq;
            double codedist;

            int quad = il_get(corrfullquads, j);

            memset(&mo, 0, sizeof(MatchObj));

            quadfile_get_stars(indx->quads, quad, stars);

            codetree_get(indx->codekd, quad, realcode);

            for (k=0; k<dimquads; k++) {
                int find;
                // position of corresponding field star.
                ind = il_index_of(corrstars, stars[k]);
                assert(ind >= 0);
                find = il_get(corrfield, ind);
                mo.quadpix[k*2 + 0] = fieldxy[find*2 + 0];
                mo.quadpix[k*2 + 1] = fieldxy[find*2 + 1];
                // index star xyz.
                startree_get(indx->starkd, stars[k], mo.quadxyz + 3*k);

                mo.star[k] = stars[k];
                mo.field[k] = find;
            }

            logmsg("\nquad #%i (quad id %i): stars", j, quad);
            for (k=0; k<dimquads; k++)
                logmsg(" %i", mo.field[k]);
            logmsg("\n");

            codefile_compute_field_code(mo.quadpix, fieldcode, dimquads);

            //logmsg(" code (index): %.3f,%.3f,%.3f,%.3f\n", realcode[0], realcode[1], realcode[2], realcode[3]);
            //logmsg(" code (field): %.3f,%.3f,%.3f,%.3f\n", fieldcode[0], fieldcode[1], fieldcode[2], fieldcode[3]);

            quad_enforce_invariants(mo.star, realcode, dimquads, dimcodes);
            quad_enforce_invariants(mo.field, fieldcode, dimquads, dimcodes);
            codedist = sqrt(distsq(realcode, fieldcode, dimcodes));
            logmsg("  code distance (normal parity): %g\n", codedist);

            quad_flip_parity(fieldcode, fieldcode, dimcodes);
            quad_enforce_invariants(mo.star, realcode, dimquads, dimcodes);
            quad_enforce_invariants(mo.field, fieldcode, dimquads, dimcodes);
            codedist = sqrt(distsq(realcode, fieldcode, dimcodes));
            logmsg("  code distance (flip parity): %g\n", codedist);

            fit_tan_wcs(mo.quadxyz, mo.quadpix, dimquads, &wcs, NULL);
            wcs.imagew = W;
            wcs.imageh = H;

            {
                double pscale = tan_pixel_scale(&wcs);
                logmsg("  quad scale: %g arcsec/pix -> field size %g x %g arcmin\n",
                       pscale, arcsec2arcmin(pscale * W), arcsec2arcmin(pscale * H));
            }

            logverb("Distances between corresponding stars:\n");
            for (k=0; k<il_size(corrstars); k++) {
                int star = il_get(corrstars, k);
                int field = il_get(corrfield, k);
                double* fxy;
                double xyz[2];
                double sxy[2];
                double d;
                anbool ok;

                startree_get(indx->starkd, star, xyz);
                ok = tan_xyzarr2pixelxy(&wcs, xyz, sxy, sxy+1);
                fxy = fieldxy + 2*field;
                d = sqrt(distsq(fxy, sxy, 2));

                logverb("  correspondence: field star %i: distance %g pix\n", field, d);
            }

            logmsg("  running verify() with the found WCS:\n");

            //log_set_level(LOG_ALL);
            log_set_level(log_get_level() + 1);

            {
                double llxyz[3];
                verify_field_t* vf;
                double verpix2 = pixr2;

                tan_pixelxy2xyzarr(&wcs, W/2.0, H/2.0, mo.center);
                tan_pixelxy2xyzarr(&wcs, 0, 0, llxyz);
                mo.radius = sqrt(distsq(mo.center, llxyz, 3));
                mo.radius_deg = dist2deg(mo.radius);
                mo.scale = tan_pixel_scale(&wcs);
                mo.dimquads = dimquads;
                memcpy(&mo.wcstan, &wcs, sizeof(tan_t));
                mo.wcs_valid = TRUE;

                vf = verify_field_preprocess(xy);

                verify_hit(indx->starkd, indx->cutnside, &mo, NULL, vf, verpix2,
                           DEFAULT_DISTRACTOR_RATIO, W, H,
                           log(1e-100), log(1e9), HUGE_VAL, TRUE, FALSE);

                verify_field_free(vf);
            }

            log_set_level(loglvl);

            logmsg("Verify log-odds %g (odds %g)\n", mo.logodds, exp(mo.logodds));


            memset(&fq, 0, sizeof(foundquad_t));
            //memcpy(fq.stars, stars, dimquads);
            fq.codedist = codedist;
            fq.logodds = mo.logodds;
            fq.pscale = tan_pixel_scale(&wcs);
            fq.quadnum = quad;
            memcpy(&(fq.mo), &mo, sizeof(MatchObj));
            bl_append(foundquads, &fq);
        }

        // Sort the found quads by star index...
        bl_sort(foundquads, sort_fq_by_stars);

        logmsg("\n\n\n");
        logmsg("Sorted by star number (ie, the order they'll be found):\n\n");
        int ngood = 0;
        double maxcodedist = 0.01;
        double minlogodds = log(1e9);
        logmsg("Assuming max code distance %g and min logodds %g\n", maxcodedist, minlogodds);

        for (j=0; j<bl_size(foundquads); j++) {
            int k;
            foundquad_t* fq = bl_access(foundquads, j);
            if (fq->codedist <= maxcodedist && fq->logodds >= minlogodds) {
                ngood++;
                logmsg("*** ");
            }
            logmsg("quad #%i: stars", fq->quadnum);
            for (k=0; k<fq->mo.dimquads; k++) {
                logmsg(" %i", fq->mo.field[k]);
            }
            logmsg("\n");
            logmsg("  codedist %g\n", fq->codedist);
            logmsg("  logodds %g (odds %g)\n", fq->logodds, exp(fq->logodds));
        }
        printf("\nTotal of %i quads that would solve the image.\n", ngood);



        if (objplotfn) {
            il* closeistars = il_new(256);
            int SW, SH;
            plot_args_t* pargs;
            sip_t scalesip;

            SW = (int)(quadplotscale * W);
            SH = (int)(quadplotscale * H);

            pargs = plotstuff_new();
            pargs->outformat = PLOTSTUFF_FORMAT_PNG;
            plotstuff_set_size(pargs, SW, SH);

            sip_copy(&scalesip, &sip);
            tan_transform(&scalesip.wcstan, &scalesip.wcstan,
                          1, scalesip.wcstan.imagew,
                          1, scalesip.wcstan.imageh,
                          quadplotscale);
            plotstuff_set_wcs_sip(pargs, &scalesip);

            for (j=0; j<Nfield; j++) {
                char* fn;
                int k,m;
                plotxy_t* pxy;
                plotindex_t* pind;
                int quad;
                unsigned int stars[dimquads];
                //double quadxy[DQMAX * 2];
                double* fxy;
                int nn;
                double nnd2;
                kdtree_qres_t* res;
                double x,y;
                int closestart;

                fxy = fieldxy + 2*j;

                logverb("Field object %i: (%g, %g)\n", j, fxy[0], fxy[1]);

                nn = kdtree_nearest_neighbour(itree, fxy, &nnd2);
                logverb("  Nearest index star is %g pixels away.\n",
                        sqrt(nnd2));

                res = kdtree_rangesearch_options(itree, fxy, pixr2 * nsigma*nsigma, KD_OPTIONS_SMALL_RADIUS);
                if (!res || !res->nres) {
                    logverb("  No index stars within %g pixels\n", sqrt(pixr2)*nsigma);
                    continue;
                }

                logverb("  %i index stars within %g pixels\n", res->nres, sqrt(pixr2)*nsigma);

                // NOTE, "closeistars" contains indices in "starlist" / "starxy" directly.

                closestart = il_size(closeistars);

                for (k=0; k<res->nres; k++) {
                    il_append(closeistars, res->inds[k]);
                }

                asprintf(&fn, objplotfn, j);
                pargs->outfn = fn;

                plotstuff_set_color(pargs, "black");
                plotstuff_plot_layer(pargs, "fill");
                pargs->lw = 2.0;

                pxy = plot_xy_get(pargs);
                pxy->scale = quadplotscale;

                plotstuff_set_color(pargs, "green");
                //plotstuff_set_marker(pargs, "crosshair");

                plotstuff_set_markersize(pargs, 5);
                for (k=0; k<il_size(closeistars); k++) {
                    int ind = il_get(closeistars, k);
                    double x,y;
                    assert(ind >= 0);
                    x = dl_get(starxylist, 2*ind+0);
                    y = dl_get(starxylist, 2*ind+1);
                    plot_xy_vals(pxy, x, y);
                }
                plotstuff_plot_layer(pargs, "xy");
                plot_xy_clear_list(pxy);

                plotstuff_set_color(pargs, "darkgreen");
                plotstuff_set_markersize(pargs, 2);
                for (k=0; k<il_size(starlist); k++) {
                    //int star = il_get(starlist, k);
                    //if (il_contains(closeistars, star))
                    if (il_contains(closeistars, k))
                        continue;
                    x = dl_get(starxylist, 2*k+0);
                    y = dl_get(starxylist, 2*k+1);
                    plot_xy_vals(pxy, x, y);
                }
                plotstuff_plot_layer(pargs, "xy");
                plot_xy_clear_list(pxy);

                pind = plot_index_get(pargs);

                // All quads incident on index stars near field stars we've looked at (cumulative)
                for (k=0; k<il_size(uniqquadlist); k++) {
                    int nclose = 0;
                    anbool thistime = FALSE;
                    quad = il_get(uniqquadlist, k);
                    quadfile_get_stars(indx->quads, quad, stars);
                    for (m=0; m<dimquads; m++) {
                        int cind;
                        int ind = il_index_of(starlist, stars[m]);
                        if (ind == -1)
                            continue;
                        cind = il_index_of(closeistars, ind);
                        if (cind < 0)
                            continue;
                        nclose++;
                        if (cind >= closestart)
                            thistime = TRUE;
                        //logverb("quad %i: star %i, starlist ind %i, close ind %i (start %i)\n", quad, stars[m], ind, cind, closestart);
                    }
                    if (!nclose)
                        continue;

                    /*
                     for (m=0; m<dimquads; m++) {
                     int ind = il_index_of(starlist, stars[m]);
                     assert(ind >= 0);
                     quadxy[m*2+0] = dl_get(starxylist, 2*ind+0) * quadplotscale;
                     quadxy[m*2+1] = dl_get(starxylist, 2*ind+1) * quadplotscale;
                     }
                     plot_quad_xy(pargs->cairo, quadxy, dimquads);
                     */

                    if (thistime)
                        logverb("Quad %i: made from %i stars we've seen so far.\n", quad, nclose);

                    if (thistime) {
                        //plotstuff_set_color(pargs, "green");
                        cairo_set_color(pargs->cairo, "green");
                    } else {
                        //plotstuff_set_color(pargs, "darkgreen");
                        cairo_set_color(pargs->cairo, "darkgreen");
                    }

                    plot_index_plotquad(pargs->cairo, pargs, pind, indx, quad, dimquads);
                }


                plotstuff_set_color(pargs, "red");
                plotstuff_set_markersize(pargs, 6);
                for (k=0; k<=j; k++)
                    plot_xy_vals(pxy, fieldxy[2*k+0], fieldxy[2*k+1]);
                plotstuff_plot_layer(pargs, "xy");
                plot_xy_clear_list(pxy);

                plotstuff_set_markersize(pargs, 10);
                plot_xy_vals(pxy, fxy[0], fxy[1]);
                plotstuff_plot_layer(pargs, "xy");
                plot_xy_clear_list(pxy);

                plotstuff_set_color(pargs, "darkred");
                plotstuff_set_markersize(pargs, 3);
                for (k=j+1; k<Nfield; k++)
                    plot_xy_vals(pxy, fieldxy[2*k+0], fieldxy[2*k+1]);
                plotstuff_plot_layer(pargs, "xy");
                plot_xy_clear_list(pxy);

                plotstuff_output(pargs);

                logmsg("Wrote %s\n", fn);
                free(fn);

                kdtree_free_query(res);
            }

            plotstuff_free(pargs);
            free(itree->data.any);
            kdtree_free(itree);
        }

        il_free(fullquadlist);
        il_free(uniqquadlist);
        il_free(quadlist);
        il_free(starlist);
        il_free(corrstarlist);
    }

    if (xylist_close(xyls)) {
        logmsg("Failed to close XYLS file.\n");
    }
    return 0;
}
