/****************************************************************
 *
 * MODULE:     v.cluster
 *
 * AUTHOR(S):  Markus Metz
 *
 * PURPOSE:    Identifies clusters in a point cloud
 *
 * COPYRIGHT:  (C) 2015 by the GRASS Development Team
 *
 *             This program is free software under the
 *             GNU General Public License (>=v2).
 *             Read the file COPYING that comes with GRASS
 *             for details.
 *
 ****************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <grass/gis.h>
#include <grass/vector.h>
#include <grass/glocale.h>
#include <grass/kdtree.h>

#define CL_DBSCAN        1
#define CL_DBSCAN2       2
#define CL_DENSE         3
#define CL_OPTICS        4
#define CL_OPTICS2       5

#define GET_PARENT(p, c) ((p) = (int)(((c) - 2) / 3 + 1))
#define GET_CHILD(c, p)  ((c) = (int)(((p) * 3) - 1))

struct cl_pnt {
    int uid;
    int prevpnt;
    double cd;
    double reach;
    double c[3];
};

static struct cl_pnt *clp;

static int *heapidx;
static int heapsize;

int add_pt(int idx);
int drop_pt(void);

int main(int argc, char *argv[])
{
    struct Map_info In, Out;
    struct line_pnts *Points;
    struct line_cats *Cats;
    int i, j, type, cat, is3d;
    struct GModule *module;
    struct Option *input, *output, *lyr_opt;
    struct Option *dist_opt, *min_opt, *method_opt;
    struct Flag *flag_2d, *flag_topo, *flag_attr;
    int clayer;
    int npoints, nlines;
    int *cid, *idx, *renumber, OLD, NEW;
    int nclusters, noutliers;
    struct kdtree *kdt;
    struct kdtrav trav;
    double c[3];
    int uid;
    double eps;
    int ndims, minpnts;
    int clmethod;
    double *kddist;
    int kdfound, *kduid;

    /* initialize GIS environment */
    /* reads grass env, stores program name to G_program_name() */
    G_gisinit(argv[0]);

    /* initialize module */
    module = G_define_module();
    G_add_keyword(_("vector"));
    G_add_keyword(_("point cloud"));
    G_add_keyword(_("cluster"));
    G_add_keyword(_("clump"));
    G_add_keyword(_("level1"));

    module->description = _("Performs cluster identification.");

    /* Define the different options as defined in gis.h */
    input = G_define_standard_option(G_OPT_V_INPUT);

    output = G_define_standard_option(G_OPT_V_OUTPUT);

    lyr_opt = G_define_standard_option(G_OPT_V_FIELD);
    lyr_opt->label = _("Layer number or name for cluster ids");
    lyr_opt->answer = "2";

    dist_opt = G_define_option();
    dist_opt->type = TYPE_DOUBLE;
    dist_opt->key = "distance";
    dist_opt->required = NO;
    dist_opt->label = _("Maximum distance to neighbors");

    min_opt = G_define_option();
    min_opt->type = TYPE_INTEGER;
    min_opt->key = "min";
    min_opt->required = NO;
    min_opt->label = _("Minimum number of points to create a cluster");

    method_opt = G_define_option();
    method_opt->type = TYPE_STRING;
    method_opt->key = "method";
    method_opt->options = "dbscan,dbscan2,density,optics,optics2";
    method_opt->answer = "dbscan";
    method_opt->required = NO;
    method_opt->label = _("Clustering method");

    flag_2d = G_define_flag();
    flag_2d->key = '2';
    flag_2d->label = _("Force 2D clustering");

    flag_topo = G_define_standard_flag(G_FLG_V_TOPO);

    flag_attr = G_define_standard_flag(G_FLG_V_TABLE);

    /* options and flags parser */
    if (G_parser(argc, argv))
        exit(EXIT_FAILURE);

    Points = Vect_new_line_struct();
    Cats = Vect_new_cats_struct();

    Vect_check_input_output_name(input->answer, output->answer, G_FATAL_EXIT);

    if (Vect_set_open_level(1))
        G_fatal_error(_("Unable to set predetermined vector open level"));

    if (1 > Vect_open_old(&In, input->answer, NULL))
        G_fatal_error(_("Unable to open vector map <%s>"), input->answer);

    /* Check if old vector is 3D. We should preserve 3D data. */
    is3d = WITHOUT_Z;
    ndims = 2;
    if (Vect_is_3d(&In)) {
        is3d = WITH_Z;
        ndims = 3;
    }

    if (flag_2d->answer)
        ndims = 2;

    minpnts = ndims;

    if (min_opt->answer) {
        minpnts = atoi(min_opt->answer);
        if (minpnts < 2) {
            G_warning(_("Minimum number of points must be at least 2"));
            minpnts = 2;
        }
        minpnts--;
    }

    clayer = atoi(lyr_opt->answer);
    if (clayer < 1)
        G_fatal_error(_("Option %s must be positive"), lyr_opt->key);

    clmethod = CL_DBSCAN;
    if (!strcmp(method_opt->answer, "dbscan2"))
        clmethod = CL_DBSCAN2;
    else if (!strcmp(method_opt->answer, "density"))
        clmethod = CL_DENSE;
    else if (!strcmp(method_opt->answer, "optics"))
        clmethod = CL_OPTICS;
    else if (!strcmp(method_opt->answer, "optic2"))
        clmethod = CL_OPTICS2;

    /* count points */
    G_message(_("Counting input points ..."));
    npoints = nlines = 0;
    while ((type = Vect_read_next_line(&In, Points, Cats)) > 0) {
        nlines++;
        if (type == GV_POINT) {
            if (Vect_cat_get(Cats, clayer, &cat))
                G_fatal_error(_("Layer %d is not empty, choose another layer"),
                              clayer);
            npoints++;
        }
    }

    if (npoints < minpnts + 1) {
        G_warning(_("Not enough points in input, nothing to do"));
        Vect_close(&In);
        exit(EXIT_SUCCESS);
    }

    /* Open new vector for reading/writing */
    if (0 > Vect_open_new(&Out, output->answer, is3d)) {
        Vect_close(&In);
        G_fatal_error(_("Unable to create vector map <%s>"), output->answer);
    }

    /* Copy header and history data from old to new map */
    Vect_copy_head_data(&In, &Out);
    Vect_hist_copy(&In, &Out);
    Vect_hist_command(&Out);

    /* create k-d tree */
    G_message(_("Creating search index ..."));
    kdt = kdtree_create(ndims, NULL);
    cid = G_malloc((nlines + 1) * sizeof(int));
    idx = G_malloc((nlines + 1) * sizeof(int));
    Vect_rewind(&In);
    i = 0;
    cid[0] = 0;
    idx[0] = 0;
    while ((type = Vect_read_next_line(&In, Points, Cats)) > 0) {
        G_percent(i++, nlines, 4);
        cid[i] = 0;
        if (type == GV_POINT) {

            c[0] = Points->x[0];
            c[1] = Points->y[0];
            c[2] = Points->z[0];

            kdtree_insert(kdt, c, i, 0);
        }
    }
    G_percent(nlines, nlines, 4);

    kdtree_optimize(kdt, 2);

    noutliers = nclusters = 0;
    if (clmethod == CL_DBSCAN) {
        /* DBSCAN
         * the neighbors of each point
         * with at least minpnts neighbors within distance (epsilon)
         * are added to a cluster */

        /* get epsilon */
        if (dist_opt->answer) {
            eps = atof(dist_opt->answer);
            if (eps <= 0)
                G_fatal_error(_("Option %s must be a positive number"),
                              dist_opt->key);
        }
        else {
            int n;
            double dist, mean, min, max, sum, sumsq, sd;
            double *kd;
            int *ki;

            /* estimate epsilon */
            G_message(_("Estimating maximum distance ..."));
            kdtree_init_trav(&trav, kdt);
            c[2] = 0.0;
            n = 0;
            sum = sumsq = 0;
            min = INFINITY;
            max = 0;
            kd = G_malloc(minpnts * sizeof(double));
            ki = G_malloc(minpnts * sizeof(int));
            i = 0;
            while (kdtree_traverse(&trav, c, &uid)) {
                G_percent(i++, npoints, 4);

                kdfound = kdtree_knn(kdt, c, ki, kd, minpnts, &uid);
                if (kdfound) {
                    dist = sqrt(kd[kdfound - 1]);
                    sum += dist;
                    sumsq += dist * dist;
                    n++;
                    if (min > dist)
                        min = dist;
                    if (max < dist)
                        max = dist;
                }
            }
            G_percent(npoints, npoints, 4);

            G_free(kd);
            G_free(ki);

            if (!n)
                G_fatal_error(_("No neighbors found"));

            mean = sum / n;
            sd = sqrt(sumsq / n - mean * mean);
            eps = mean + 2.575829 * sd; /* 99% CI */

            if (eps > max)
                eps = max;

            G_message(_("Distance to the %d nearest neighbor:"), minpnts);
            G_message(_("Min: %g, max: %g"), min, max);
            G_message(_("Mean: %g"), mean);
            G_message(_("Standard deviation: %g"), sd);

            G_message(_("Estimated maximum distance: %g"), eps);
        }

        /* create clusters */
        G_message(_("Building clusters ..."));
        nclusters = 0;
        kdtree_init_trav(&trav, kdt);
        c[2] = 0.0;
        idx[0] = 0;
        i = 0;
        while (kdtree_traverse(&trav, c, &uid)) {
            G_percent(i++, npoints, 4);

            /* radius search */
            /* TODO: use knn search */
            kdfound = kdtree_dnn(kdt, c, &kduid, &kddist, eps, &uid);

            /* must have min neighbors within radius */
            if (kdfound >= minpnts) {

                OLD = cid[uid];
                NEW = idx[OLD];
                while (OLD != NEW) {
                    OLD = NEW;
                    NEW = idx[OLD];
                }
                cat = NEW;

                /* find latest cluster */
                for (j = 0; j < kdfound; j++) {
                    OLD = cid[kduid[j]];
                    NEW = idx[OLD];
                    while (OLD != NEW) {
                        OLD = NEW;
                        NEW = idx[OLD];
                    }
                    if (cat < NEW) {
                        cat = NEW;
                    }
                }

                if (cat == 0) {
                    /* start new cluster */
                    nclusters++;
                    cat = nclusters;
                    if (nclusters > nlines)
                        G_fatal_error(_("nlines: %d, nclusters: %d"), nlines,
                                      nclusters);
                    idx[nclusters] = nclusters;
                    cid[uid] = nclusters;
                }

                /* set or update cluster ids */
                if (cid[uid] != 0) {
                    /* relabel */
                    OLD = cid[uid];
                    NEW = idx[OLD];
                    while (OLD != NEW) {
                        OLD = NEW;
                        NEW = idx[OLD];
                    }
                    idx[NEW] = cat;
                }
                else {
                    cid[uid] = cat;
                }

                for (j = 0; j < kdfound; j++) {
                    if (cid[kduid[j]] != 0) {
                        /* relabel */
                        OLD = cid[kduid[j]];
                        NEW = idx[OLD];
                        while (OLD != NEW) {
                            OLD = NEW;
                            NEW = idx[OLD];
                        }
                        idx[NEW] = cat;
                    }
                    else {
                        cid[kduid[j]] = cat;
                    }
                }
            }
            if (kdfound) {
                G_free(kddist);
                G_free(kduid);
            }
        }
        G_percent(npoints, npoints, 4);

        if (nclusters == 0) {
            G_message(_("No clusters found, adjust option %s"), dist_opt->key);
            Vect_close(&In);
            Vect_close(&Out);
            Vect_delete(output->answer);
            exit(EXIT_SUCCESS);
        }

        /* generate a renumbering scheme */
        G_message(_("Generating renumbering scheme..."));
        G_debug(1, "%d initial clusters", nclusters);
        /* allocate final clump ID */
        renumber = (int *)G_malloc((nclusters + 1) * sizeof(int));
        renumber[0] = 0;
        cat = 1;
        G_percent(0, nclusters, 1);
        for (i = 1; i <= nclusters; i++) {
            G_percent(i, nclusters, 4);
            OLD = i;
            NEW = idx[i];
            if (OLD != NEW) {
                renumber[i] = 0;
                /* find valid clump ID */
                while (OLD != NEW) {
                    OLD = NEW;
                    NEW = idx[OLD];
                }
                idx[i] = NEW;
            }
            else
                /* set final clump id */
                renumber[i] = cat++;
        }

        nclusters = cat - 1;

        /* write cluster ids */
        G_message(_("Write out cluster ids ..."));
        Vect_rewind(&In);
        i = 0;
        noutliers = 0;
        while ((type = Vect_read_next_line(&In, Points, Cats)) > 0) {
            G_percent(i++, nlines, 4);
            if (type == GV_POINT) {
                cat = renumber[idx[cid[i]]];
                if (!cat)
                    noutliers++;
                Vect_cat_set(Cats, clayer, cat);
                Vect_write_line(&Out, GV_POINT, Points, Cats);
            }
        }
        G_percent(nlines, nlines, 4);
    }
    else if (clmethod == CL_DBSCAN2) {
        /* DBSCAN, but cluster size must be at least minpnts + 1 */
        int *clcnt;

        /* get epsilon */
        if (dist_opt->answer) {
            eps = atof(dist_opt->answer);
            if (eps <= 0)
                G_fatal_error(_("Option %s must be a positive number"),
                              dist_opt->key);
        }
        else {
            int n;
            double dist, mean, min, max, sum, sumsq, sd;
            double *kd;
            int *ki;

            /* estimate epsilon */
            G_message(_("Estimating maximum distance ..."));
            kdtree_init_trav(&trav, kdt);
            c[2] = 0.0;
            n = 0;
            sum = sumsq = 0;
            min = INFINITY;
            max = 0;
            kd = G_malloc(minpnts * sizeof(double));
            ki = G_malloc(minpnts * sizeof(int));
            i = 0;
            while (kdtree_traverse(&trav, c, &uid)) {
                G_percent(i++, npoints, 4);

                kdfound = kdtree_knn(kdt, c, ki, kd, minpnts, &uid);
                if (kdfound) {
                    dist = sqrt(kd[kdfound - 1]);
                    sum += dist;
                    sumsq += dist * dist;
                    n++;
                    if (min > dist)
                        min = dist;
                    if (max < dist)
                        max = dist;
                }
            }
            G_percent(npoints, npoints, 4);

            G_free(kd);
            G_free(ki);

            if (!n)
                G_fatal_error(_("No neighbors found"));

            mean = sum / n;
            sd = sqrt(sumsq / n - mean * mean);
            eps = mean + 2.575829 * sd; /* 99% CI */

            if (eps > max)
                eps = max;

            G_message(_("Distance to the %d nearest neighbor:"), minpnts);
            G_message(_("Min: %g, max: %g"), min, max);
            G_message(_("Mean: %g"), mean);
            G_message(_("Standard deviation: %g"), sd);

            G_message(_("Estimated maximum distance: %g"), eps);
        }

        /* create clusters */
        G_message(_("Building clusters ..."));
        clcnt = G_malloc((nlines + 1) * sizeof(int));
        for (i = 0; i <= nlines; i++)
            clcnt[i] = 0;
        nclusters = 0;
        kdtree_init_trav(&trav, kdt);
        c[2] = 0.0;
        idx[0] = 0;
        i = 0;
        while (kdtree_traverse(&trav, c, &uid)) {
            G_percent(i++, npoints, 4);

            /* radius search */
            /* TODO: use knn search */
            kdfound = kdtree_dnn(kdt, c, &kduid, &kddist, eps, &uid);

            /* any neighbor within radius */
            if (kdfound > 0) {

                OLD = cid[uid];
                NEW = idx[OLD];
                while (OLD != NEW) {
                    OLD = NEW;
                    NEW = idx[OLD];
                }
                cat = NEW;

                /* find latest cluster */
                for (j = 0; j < kdfound; j++) {
                    OLD = cid[kduid[j]];
                    NEW = idx[OLD];
                    while (OLD != NEW) {
                        OLD = NEW;
                        NEW = idx[OLD];
                    }
                    if (cat < NEW) {
                        cat = NEW;
                    }
                }

                if (cat == 0) {
                    /* start new cluster */
                    nclusters++;
                    cat = nclusters;
                    if (nclusters > nlines)
                        G_fatal_error(_("nlines: %d, nclusters: %d"), nlines,
                                      nclusters);
                    idx[nclusters] = nclusters;
                    cid[uid] = nclusters;
                    clcnt[cat] = 1;
                }

                /* set or update cluster ids */
                if (cid[uid] != 0) {
                    /* relabel */
                    OLD = cid[uid];
                    NEW = idx[OLD];
                    while (OLD != NEW) {
                        OLD = NEW;
                        NEW = idx[OLD];
                    }
                    idx[NEW] = cat;
                }
                else {
                    cid[uid] = cat;
                    clcnt[cat]++;
                }

                for (j = 0; j < kdfound; j++) {
                    if (cid[kduid[j]] != 0) {
                        OLD = cid[kduid[j]];
                        NEW = idx[OLD];
                        while (OLD != NEW) {
                            OLD = NEW;
                            NEW = idx[OLD];
                        }
                        /* relabel */
                        idx[NEW] = cat;
                    }
                    else {
                        cid[kduid[j]] = cat;
                        clcnt[cat]++;
                    }
                }
                G_free(kddist);
                G_free(kduid);
            }
        }
        G_percent(npoints, npoints, 4);

        if (nclusters == 0) {
            G_message(_("No clusters found, adjust option %s"), dist_opt->key);
            Vect_close(&In);
            Vect_close(&Out);
            Vect_delete(output->answer);
            exit(EXIT_SUCCESS);
        }

        /* generate a renumbering scheme */
        G_message(_("Generating renumbering scheme..."));
        G_debug(1, "%d initial clusters", nclusters);
        /* allocate final clump ID */
        renumber = (int *)G_malloc((nclusters + 1) * sizeof(int));
        renumber[0] = 0;
        cat = 1;
        G_percent(0, nclusters, 1);
        for (i = 1; i <= nclusters; i++) {
            G_percent(i, nclusters, 4);
            OLD = i;
            NEW = idx[i];
            if (OLD != NEW) {
                /* find valid clump ID */
                while (OLD != NEW) {
                    OLD = NEW;
                    NEW = idx[OLD];
                }
                idx[i] = NEW;
                clcnt[NEW] += clcnt[i];
            }
        }
        for (i = 1; i <= nclusters; i++) {
            OLD = i;
            NEW = idx[i];
            renumber[i] = 0;
            if (OLD == NEW && clcnt[NEW] > minpnts) {
                /* set final clump id */
                renumber[i] = cat++;
            }
        }

        nclusters = cat - 1;

        /* write cluster ids */
        G_message(_("Write out cluster ids ..."));
        Vect_rewind(&In);
        i = 0;
        noutliers = 0;
        while ((type = Vect_read_next_line(&In, Points, Cats)) > 0) {
            G_percent(i++, nlines, 4);
            if (type == GV_POINT) {
                cat = renumber[idx[cid[i]]];
                if (!cat)
                    noutliers++;
                Vect_cat_set(Cats, clayer, cat);
                Vect_write_line(&Out, GV_POINT, Points, Cats);
            }
        }
        G_percent(nlines, nlines, 4);
    }
    else if (clmethod == CL_OPTICS) {
        /* OPTICS
         * each pair of points is either directly connected or
         * connected by a chain of other points
         * for each unprocessed point p
         * mark as processed, append to output list
         * core distance of p: distance to the k-th neighbor
         * q: neighbor of p
         * reachability of q: max(dist(p, q), coredist(p))
         * -> needs epsilon, otherwise always coredist(p)
         * for each unprocessed neighbor q
         * if q has not been reached yet, put q in min heap
         * if q's reachability can be reduced, put q with new reachability in
         * min heap proceed with point with smallest reachability clusters: plot
         * x = position in output list, y = reachability clusters = valleys of
         * reachability in plot hierarchical clusters: valleys in valleys
         */

        double *kd;
        int *ki;
        int k, kdpnts;
        int *clidx;
        int *olist, nout;
        double newrd;
        int isout;

        kd = G_malloc(minpnts * sizeof(double));
        ki = G_malloc(minpnts * sizeof(int));

        clp = G_malloc((npoints + 1) * sizeof(struct cl_pnt));
        heapidx = G_malloc((npoints + 1) * sizeof(int));
        olist = G_malloc((npoints + 1) * sizeof(int));
        clidx = G_malloc((nlines + 1) * sizeof(int));

        heapsize = 0;

        /* get epsilon */
        eps = 0;
        if (dist_opt->answer) {
            eps = atof(dist_opt->answer);
            if (eps <= 0)
                G_fatal_error(_("Option %s must be a positive number"),
                              dist_opt->key);
        }

        /* loading points */
        G_message(_("Loading points ..."));
        kdtree_init_trav(&trav, kdt);
        c[2] = 0.0;
        i = 0;
        while (kdtree_traverse(&trav, c, &uid)) {
            G_percent(i, npoints, 4);

            clp[i].c[0] = c[0];
            clp[i].c[1] = c[1];
            clp[i].c[2] = c[2];
            clp[i].uid = uid;
            clp[i].cd = -1;
            clp[i].reach = -1;
            clp[i].prevpnt = -1;
            clidx[uid] = i;
            olist[i] = -1;

            i++;
        }
        G_percent(npoints, npoints, 4);
        kdpnts = i;
        G_debug(0, "%d points in k-d tree", kdpnts);

        /* reachability network */
        G_message(_("Reachability network ..."));
        nout = 0;
        for (i = 0; i < kdpnts; i++) {
            G_percent(i, kdpnts, 4);

            if (clp[i].cd > 0)
                continue;

            /* knn search */
            uid = clp[i].uid;
            kdfound = kdtree_knn(kdt, clp[i].c, ki, kd, minpnts, &uid);
            if (kdfound < minpnts)
                G_fatal_error(_("Not enough points found"));

            clp[i].cd = kd[minpnts - 1];
            /* no reachability for the seed point !!! */
            clp[i].reach = clp[i].cd; /* ok ? */
            olist[nout++] = i;

            /* initialize heap */
            newrd = clp[i].cd;
            for (j = 0; j < kdfound; j++) {
                if (clp[clidx[ki[j]]].cd < 0) {
                    /* deviation from OPTICS,
                     * creates nicer connectivity graph */
                    newrd = kd[j];
                    if (clp[clidx[ki[j]]].reach < 0 ||
                        clp[clidx[ki[j]]].reach > newrd) {
                        clp[clidx[ki[j]]].reach = newrd;
                        clp[clidx[ki[j]]].prevpnt = i;
                        add_pt(clidx[ki[j]]);
                    }
                }
            }

            while (heapsize) {
                k = drop_pt();
                if (k < 0 || k >= kdpnts)
                    G_fatal_error("Invalid index");
                if (clp[k].cd > 0)
                    continue;

                /* knn search */
                uid = clp[k].uid;
                kdfound = kdtree_knn(kdt, clp[k].c, ki, kd, minpnts, &uid);
                if (kdfound < minpnts)
                    G_fatal_error(_("Not enough points found"));

                clp[k].cd = kd[minpnts - 1];
                olist[nout++] = k;

                newrd = clp[k].cd;
                for (j = 0; j < kdfound; j++) {
                    if (heapsize >= npoints)
                        G_fatal_error("Heap is too large");
                    if (clp[clidx[ki[j]]].cd < 0) {
                        /* deviation from OPTICS,
                         * creates nicer connectivity graph */
                        newrd = kd[j];
                        if (clp[clidx[ki[j]]].reach < 0 ||
                            clp[clidx[ki[j]]].reach > newrd) {
                            clp[clidx[ki[j]]].reach = newrd;
                            clp[clidx[ki[j]]].prevpnt = k;
                            add_pt(clidx[ki[j]]);
                        }
                    }
                }
            }
        }
        G_percent(kdpnts, kdpnts, 4);
        G_debug(0, "nout: %d", nout);
        if (nout != kdpnts)
            G_fatal_error("nout != kdpnts");

        /* set cluster ids */
        G_message(_("Set cluster ids ..."));
        isout = 1;
        nclusters = 0;
        for (i = 0; i < kdpnts; i++) {
            G_percent(i, kdpnts, 4);

            if (eps > 0 && clp[olist[i]].reach > eps)
                isout = 1;
            else {
                if (isout || clp[olist[i]].prevpnt == -1) {
                    isout = 0;
                    nclusters++;
                }
                cid[clp[olist[i]].uid] = nclusters;
            }
        }
        G_percent(kdpnts, kdpnts, 4);

        /* write cluster ids */
        G_message(_("Write out cluster ids ..."));
        Vect_rewind(&In);
        i = 0;
        noutliers = 0;
        while ((type = Vect_read_next_line(&In, Points, Cats)) > 0) {
            G_percent(i++, nlines, 4);
            if (type == GV_POINT) {
                cat = cid[i];
                if (!cat)
                    noutliers++;
                Vect_cat_set(Cats, clayer, cat);
                Vect_write_line(&Out, GV_POINT, Points, Cats);
            }
        }
        G_percent(nlines, nlines, 4);
    }
    else if (clmethod == CL_OPTICS2) {
        /* OPTICS modified, create separated reachability networks
         * for each point p
         *   get p's core distance
         *   get p's neighbors
         *   for each neighbor q
         *     new reachability of q: dist(p, q)
         *     if q has been processed
         *       new reachability of q: max(coredist(q), dist(p, q))
         *     set or reduce q's reachability
         *     connect q to p if q's reachability can be updated
         */

        double *coredist;
        double *reachability;
        int *nextpnt;
        double *kd;
        int *ki;
        double newrd;

        coredist = G_malloc((nlines + 1) * sizeof(double));
        reachability = G_malloc((nlines + 1) * sizeof(double));
        nextpnt = G_malloc((nlines + 1) * sizeof(int));

        kd = G_malloc(minpnts * sizeof(double));
        ki = G_malloc(minpnts * sizeof(int));

        for (i = 0; i <= nlines; i++) {
            coredist[i] = -1;
            reachability[i] = -1;
            nextpnt[i] = -1;
            cid[i] = 0;
            idx[i] = 0;
        }

        /* reachability network */
        G_message(_("Reachability network ..."));
        kdtree_init_trav(&trav, kdt);
        c[2] = 0.0;
        i = 0;
        while (kdtree_traverse(&trav, c, &uid)) {
            G_percent(i++, npoints, 4);

            /* knn search */
            kdfound = kdtree_knn(kdt, c, ki, kd, minpnts, &uid);
            if (kdfound < minpnts)
                G_fatal_error(_("Not enough points found"));
            coredist[uid] = kd[minpnts - 1];

            for (j = 0; j < kdfound; j++) {
                /* new reachability */
                newrd = kd[j];
                if (coredist[ki[j]] > kd[j]) {
                    /* do not connect a point to its own cluster
                     * because points in its own cluster
                     * have already been connected to this point
                     * or reconnected */
                    newrd = coredist[ki[j]];
                }

                if (reachability[ki[j]] == -1 || reachability[ki[j]] > newrd) {
                    reachability[ki[j]] = newrd;
                    nextpnt[ki[j]] = uid;

                    /* no link - back link */
                    if (nextpnt[uid] == ki[j]) {
                        if (coredist[ki[j]] == -1) {
                            G_fatal_error(
                                _("Neighbor point's core dist is -1"));
                        }
                        if (coredist[ki[j]] < coredist[uid]) {
                            nextpnt[ki[j]] = -1;
                            reachability[ki[j]] = -1;
                            nextpnt[uid] = ki[j];
                        }
                        else {
                            nextpnt[uid] = -1;
                            reachability[uid] = -1;
                            reachability[uid] = -1;
                        }
                    }
                }
            }
        }
        G_percent(npoints, npoints, 4);

        /* create clusters from reachability network */
        G_message(_("Building clusters ..."));
        G_percent(0, nlines, 4);
        for (i = 1; i <= nlines; i++) {

            G_percent(i, nlines, 4);

            if (cid[i] > 0 || coredist[i] == -1 || nextpnt[i] == -1)
                continue;

            if (cid[nextpnt[i]] > 0) {
                cid[i] = idx[cid[nextpnt[i]]];
            }
            else {
                /* start new cluster */
                nclusters++;
                cid[i] = nclusters;
                idx[nclusters] = nclusters;
                uid = nextpnt[i];

                while (uid > 0) {
                    if (cid[uid] == 0) {
                        cid[uid] = nclusters;

                        uid = nextpnt[uid];
                    }
                    else {
                        /* relabel */
                        OLD = cid[uid];
                        NEW = idx[OLD];
                        while (OLD != NEW) {
                            OLD = NEW;
                            NEW = idx[OLD];
                        }
                        idx[NEW] = nclusters;
                        uid = nextpnt[uid];
                        uid = -1;
                    }
                }
            }
        }

        /* generate a renumbering scheme */
        G_message(_("Generating renumbering scheme..."));
        G_debug(1, "%d initial clusters", nclusters);
        /* allocate final clump ID */
        renumber = (int *)G_malloc((nclusters + 1) * sizeof(int));
        renumber[0] = 0;
        cat = 1;
        G_percent(0, nclusters, 1);
        for (i = 1; i <= nclusters; i++) {
            G_percent(i, nclusters, 4);
            OLD = i;
            NEW = idx[i];
            if (OLD != NEW) {
                renumber[i] = 0;
                /* find valid clump ID */
                while (OLD != NEW) {
                    OLD = NEW;
                    NEW = idx[OLD];
                }
                idx[i] = NEW;
            }
            else
                /* set final clump id */
                renumber[i] = cat++;
        }

        nclusters = cat - 1;

        /* write cluster ids */
        G_message(_("Write out cluster ids ..."));
        Vect_rewind(&In);
        i = 0;
        noutliers = 0;
        while ((type = Vect_read_next_line(&In, Points, Cats)) > 0) {
            G_percent(i++, nlines, 4);
            if (type == GV_POINT) {
                cat = renumber[idx[cid[i]]];
                if (!cat)
                    noutliers++;
                Vect_cat_set(Cats, clayer, cat);
                Vect_write_line(&Out, GV_POINT, Points, Cats);
            }
        }
        G_percent(nlines, nlines, 4);
    }
    else if (clmethod == CL_DENSE) {
        /* MATRUSKA
         * clusters in clusters in clusters ...
         * calculate core density = distance to (minpnts - 1) for each point
         * sort points ascending by core density
         * for each point in sorted list
         *   if point does not have a cluster id
         *     start new cluster, cluster reachability is core density of this
         * point connect all points within cluster reachability add all
         * connected points to list while list is not empty remove last point
         * from list connect all points within cluster reachability add all
         * connected points to list
         *       */
        int *clidx;
        double *kd;
        int *ki;
        double cd;
        int k, kdcount;
        struct ilist *CList;

        clp = G_malloc((nlines + 1) * sizeof(struct cl_pnt));
        clidx = G_malloc((nlines + 1) * sizeof(int));

        kd = G_malloc(minpnts * sizeof(double));
        ki = G_malloc(minpnts * sizeof(int));

        CList = G_new_ilist();

        for (i = 0; i <= nlines; i++) {
            clp[i].c[0] = 0;
            clp[i].c[1] = 0;
            clp[i].c[2] = 0;
            clp[i].cd = -1;
            clp[i].uid = -1;
            clidx[i] = -1;
        }

        /* core density */
        G_message(_("Core density ..."));
        kdtree_init_trav(&trav, kdt);
        c[2] = 0.0;
        i = 0;
        kdcount = 0;
        uid = -1;
        while (kdtree_traverse(&trav, c, &uid)) {
            G_percent(i++, npoints, 4);

            /* knn search */
            kdfound = kdtree_knn(kdt, c, ki, kd, minpnts, &uid);
            if (kdfound < minpnts)
                G_fatal_error(_("Not enough points found"));

            cd = kd[minpnts - 1];

            /* list insert */
            for (j = kdcount; j > 0; j--) {
                if (clp[j - 1].cd <= cd)
                    break;
                clp[j] = clp[j - 1];
                clidx[clp[j].uid] = j;
            }
            clp[j].uid = uid;
            clp[j].c[0] = c[0];
            clp[j].c[1] = c[1];
            clp[j].c[2] = c[2];
            clp[j].cd = cd;
            clidx[clp[j].uid] = j;
            kdcount++;
        }
        G_percent(npoints, npoints, 4);

        /* create clusters */
        G_message(_("Building clusters ..."));
        nclusters = 0;
        for (i = 0; i < kdcount; i++) {
            G_percent(i, kdcount, 4);

            if (cid[clp[i].uid] > 0)
                continue;

            /* knn search */
            kdfound = kdtree_knn(kdt, clp[i].c, ki, kd, minpnts, &clp[i].uid);
            if (kdfound < minpnts)
                G_fatal_error(_("Not enough points found"));

            /* start a new cluster */
            uid = clp[i].uid;
            nclusters++;
            cat = nclusters;
            cid[uid] = cat;
            cd = clp[i].cd;
            CList->n_values = 0;
            for (j = 0; j < kdfound; j++) {
                if (cid[ki[j]] == 0) {
                    G_ilist_add(CList, clidx[ki[j]]);
                    cid[ki[j]] = cat;
                }
            }
            if (CList->n_values < minpnts) {
                CList->n_values = 0;
                nclusters--;
                cid[uid] = 0;
                for (j = 0; j < kdfound; j++) {
                    if (cid[ki[j]] == cat) {
                        cid[ki[j]] = 0;
                    }
                }
            }

            while (CList->n_values) {
                /* expand cluster */
                CList->n_values--;
                k = CList->value[CList->n_values];
                if (k < 0)
                    G_fatal_error("expand cluster: k < 0");
                if (clp[k].uid < 1)
                    G_fatal_error("expand cluster: clp[k].uid < 1");

                kdfound =
                    kdtree_knn(kdt, clp[k].c, ki, kd, minpnts, &clp[k].uid);
                if (kdfound < minpnts)
                    G_fatal_error(_("Not enough points found"));

                for (j = 0; j < kdfound; j++) {
                    if (kd[j] <= cd && cid[ki[j]] == 0) {
                        cid[ki[j]] = cat;
                        if (clidx[ki[j]] < 0)
                            G_fatal_error(
                                "expand cluster ngbrs: clidx[ki[j]] < 0");
                        G_ilist_add(CList, clidx[ki[j]]);
                    }
                }
            }
        }
        G_percent(kdcount, kdcount, 4);

        /* write cluster ids */
        G_message(_("Write out cluster ids ..."));
        Vect_rewind(&In);
        i = 0;
        noutliers = 0;
        while ((type = Vect_read_next_line(&In, Points, Cats)) > 0) {
            G_percent(i++, nlines, 4);
            if (type == GV_POINT) {
                cat = cid[i];
                if (!cat)
                    noutliers++;
                Vect_cat_set(Cats, clayer, cat);
                Vect_write_line(&Out, GV_POINT, Points, Cats);
            }
        }
        G_percent(nlines, nlines, 4);
    }

    if (!flag_attr->answer)
        Vect_copy_tables(&In, &Out, 0);

    /* Build topology for vector map and close them */
    Vect_close(&In);
    if (!flag_topo->answer)
        Vect_build(&Out);
    Vect_close(&Out);

    G_message(_("%d clusters found"), nclusters);
    G_message(_("%d outliers found"), noutliers);

    exit(EXIT_SUCCESS);
}

/* min heap */

/* compare heap points */
int cmp_pnt(int a, int b)
{
    if (clp[a].reach < clp[b].reach)
        return 1;
    if (clp[a].reach > clp[b].reach)
        return 0;
    if (clp[a].uid < clp[b].uid)
        return 1;
    return 0;
}

/* standard sift-up routine for d-ary min heap */
static int sift_up(int start)
{
    register int parent, child, child_added;

    child = start;
    child_added = heapidx[child];

    while (child > 1) {
        GET_PARENT(parent, child);

        /* child smaller */
        if (cmp_pnt(child_added, heapidx[parent])) {
            /* push parent point down */
            heapidx[child] = heapidx[parent];
            child = parent;
        }
        else
            /* no more sifting up, found new slot for child */
            break;
    }

    /* put point in new slot */
    if (child < start) {
        heapidx[child] = child_added;
    }

    return 0;
}

/* add point routine for min heap */
int add_pt(int idx)
{
    /* add point to next free position */
    heapsize++;

    heapidx[heapsize] = idx;

    /* sift up: move new point towards top of heap */
    sift_up(heapsize);

    return 0;
}

/* drop point routine for min heap */
int drop_pt(void)
{
    register int child, childr, parent;
    register int i, idx;

    idx = heapidx[1];
    if (heapsize == 1) {
        heapidx[1] = -1;
        heapsize = 0;
        return idx;
    }

    /* start with root */
    parent = 1;

    /* sift down: move hole back towards bottom of heap */
    while (GET_CHILD(child, parent) <= heapsize) {

        i = child + 3;
        for (childr = child + 1; childr <= heapsize && childr < i; childr++) {
            if (cmp_pnt(heapidx[childr], heapidx[child])) {
                child = childr;
            }
        }

        /* move hole down */
        heapidx[parent] = heapidx[child];
        parent = child;
    }

    /* hole is in lowest layer, move to heap end */
    if (parent < heapsize) {
        heapidx[parent] = heapidx[heapsize];

        /* sift up last swapped point, only necessary if hole moved to heap end
         */
        sift_up(parent);
    }

    /* the actual drop */
    heapsize--;

    return idx;
}
