//=============================================================================================================
/**
 * @file     fiff_coord_trans_old.cpp
 * @author   Lorenz Esch <lesch@mgh.harvard.edu>;
 *           Matti Hamalainen <msh@nmr.mgh.harvard.edu>;
 *           Christoph Dinh <chdinh@nmr.mgh.harvard.edu>
 * @since    0.1.0
 * @date     January, 2017
 *
 * @section  LICENSE
 *
 * Copyright (C) 2017, Lorenz Esch, Matti Hamalainen, Christoph Dinh. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that
 * the following conditions are met:
 *     * Redistributions of source code must retain the above copyright notice, this list of conditions and the
 *       following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and
 *       the following disclaimer in the documentation and/or other materials provided with the distribution.
 *     * Neither the name of MNE-CPP authors nor the names of its contributors may be used
 *       to endorse or promote products derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 *
 * @brief    Definition of the FiffCoordTransOld Class.
 *
 */

//=============================================================================================================
// INCLUDES
//=============================================================================================================

#include "fiff_coord_trans_old.h"

#include <fiff/fiff_tag.h>

#include <QFile>

#include <Eigen/Dense>

//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================

using namespace Eigen;
using namespace FIFFLIB;
using namespace FIFFLIB;

#ifndef TRUE
#define TRUE 1
#endif

#ifndef FALSE
#define FALSE 0
#endif

#ifndef FAIL
#define FAIL -1
#endif

#ifndef OK
#define OK 0
#endif

#define X_20 0
#define Y_20 1
#define Z_20 2

#define FREE_20(x) if ((char *)(x) != NULL) free((char *)(x))

#define MALLOC_20(x,t) (t *)malloc((x)*sizeof(t))

static void matrix_error_20(int kind, int nr, int nc)

{
    if (kind == 1)
        printf("Failed to allocate memory pointers for a %d x %d matrix\n",nr,nc);
    else if (kind == 2)
        printf("Failed to allocate memory for a %d x %d matrix\n",nr,nc);
    else
        printf("Allocation error for a %d x %d matrix\n",nr,nc);
    if (sizeof(void *) == 4) {
        printf("This is probably because you seem to be using a computer with 32-bit architecture.\n");
        printf("Please consider moving to a 64-bit platform.");
    }
    printf("Cannot continue. Sorry.\n");
    exit(1);
}

float **mne_cmatrix_20(int nr,int nc)

{
    int i;
    float **m;
    float *whole;

    m = MALLOC_20(nr,float *);
    if (!m) matrix_error_20(1,nr,nc);
    whole = MALLOC_20(nr*nc,float);
    if (!whole) matrix_error_20(2,nr,nc);

    for(i=0;i<nr;i++)
        m[i] = whole + i*nc;
    return m;
}

#define VEC_DIFF_20(from,to,diff) {\
    (diff)[X_20] = (to)[X_20] - (from)[X_20];\
    (diff)[Y_20] = (to)[Y_20] - (from)[Y_20];\
    (diff)[Z_20] = (to)[Z_20] - (from)[Z_20];\
    }

#define ALLOC_CMATRIX_20(x,y) mne_cmatrix_20((x),(y))

#define MAXWORD 1000

#define VEC_DOT_20(x,y) ((x)[X_20]*(y)[X_20] + (x)[Y_20]*(y)[Y_20] + (x)[Z_20]*(y)[Z_20])

#define VEC_LEN_20(x) sqrt(VEC_DOT_20(x,x))

#define CROSS_PRODUCT_20(x,y,xy) {\
    (xy)[X_20] =   (x)[Y_20]*(y)[Z_20]-(y)[Y_20]*(x)[Z_20];\
    (xy)[Y_20] = -((x)[X_20]*(y)[Z_20]-(y)[X_20]*(x)[Z_20]);\
    (xy)[Z_20] =   (x)[X_20]*(y)[Y_20]-(y)[X_20]*(x)[Y_20];\
    }

#define FREE_CMATRIX_20(m) mne_free_cmatrix_20((m))

void mne_free_cmatrix_20(float **m)
{
    if (m) {
        FREE_20(*m);
        FREE_20(m);
    }
}

#define MIN_20(a,b) ((a) < (b) ? (a) : (b))

//float
Eigen::MatrixXf toFloatEigenMatrix_20(float **mat, const int m, const int n)
{
    Eigen::MatrixXf eigen_mat(m,n);

    for ( int i = 0; i < m; ++i)
        for ( int j = 0; j < n; ++j)
            eigen_mat(i,j) = mat[i][j];

    return eigen_mat;
}

void fromFloatEigenVector_20(const Eigen::VectorXf& from_vec, float *to_vec, const int n)
{
    for ( int i = 0; i < n; ++i)
        to_vec[i] = from_vec[i];
}

void fromFloatEigenMatrix_20(const Eigen::MatrixXf& from_mat, float **& to_mat, const int m, const int n)
{
    for ( int i = 0; i < m; ++i)
        for ( int j = 0; j < n; ++j)
            to_mat[i][j] = from_mat(i,j);
}

int mne_svd_20(float **mat,	/* The matrix */
            int   m,int n,	/* m rows n columns */
            float *sing,	/* Singular values (must have size
                             * MIN(m,n)+1 */
            float **uu,		/* Left eigenvectors */
            float **vv)		/* Right eigenvectors */
/*
      * Compute the SVD of mat.
      * The singular vector calculations depend on whether
      * or not u and v are given.
      *
      * The allocations should be done as follows
      *
      * mat = ALLOC_CMATRIX_3(m,n);
      * vv  = ALLOC_CMATRIX_3(MIN(m,n),n);
      * uu  = ALLOC_CMATRIX_3(MIN(m,n),m);
      * sing = MALLOC_3(MIN(m,n),float);
      *
      * mat is modified by this operation
      *
      * This simply allocates the workspace and calls the
      * LAPACK Fortran routine
      */

{
    int    udim = MIN_20(m,n);

    Eigen::MatrixXf eigen_mat = toFloatEigenMatrix_20(mat, m, n);

    //ToDo Optimize computation depending of whether uu or vv are defined
    Eigen::JacobiSVD< Eigen::MatrixXf > svd(eigen_mat ,Eigen::ComputeFullU | Eigen::ComputeFullV);

    fromFloatEigenVector_20(svd.singularValues(), sing, svd.singularValues().size());

    if (uu != NULL)
        fromFloatEigenMatrix_20(svd.matrixU().transpose(), uu, udim, m);

    if (vv != NULL)
        fromFloatEigenMatrix_20(svd.matrixV().transpose(), vv, m, n);

    return 0;
    //  return info;
}

void mne_matt_mat_mult2_20 (float **m1,float **m2,float **result,
                         int d1,int d2,int d3)
     /* Matrix multiplication
      * result(d1 x d3) = m1(d2 x d1)^T * m2(d2 x d3) */

{
    #ifdef BLAS
    char  *transa = "N";
    char  *transb = "T";
    float zero = 0.0;
    float one  = 1.0;

    sgemm (transa,transb,&d3,&d1,&d2,
            &one,m2[0],&d3,m1[0],&d1,&zero,result[0],&d3);

    return;
    #else
    int j,k,p;
    float sum;
    int  one = 1;

    for (j = 0; j < d1; j++)
        for (k = 0; k < d3; k++) {
            sum = 0.0;
        for (p = 0; p < d2; p++)
            sum = sum + m1[p][j]*m2[p][k];
        result[j][k] = sum;
    }
    return;
    #endif
}

float **mne_matt_mat_mult_20 (float **m1,float **m2,int d1,int d2,int d3)

{
  float **result = ALLOC_CMATRIX_20(d1,d3);
  mne_matt_mat_mult2_20(m1,m2,result,d1,d2,d3);
  return result;
}

static void skip_comments(FILE *in)

{
    int c;

    while (1) {
        c = fgetc(in);
        if (c == '#') {
            for (c = fgetc(in); c != EOF && c != '\n'; c = fgetc(in))
                ;
        }
        else {
            ungetc(c,in);
            return;
        }
    }
}

static int whitespace(int c)

{
    if (c == '\t' || c == '\n' || c == ' ')
        return TRUE;
    else
        return FALSE;
}

static int whitespace_quote(int c, int inquote)

{
    if (inquote)
        return (c == '"');
    else
        return (c == '\t' || c == '\n' || c == ' ');
}

static char *next_word_20(FILE *in)

{
    char *next = MALLOC_20(MAXWORD,char);
    int c;
    int  p,k;
    int  inquote;

    skip_comments(in);

    inquote = FALSE;
    for (k = 0, p = 0, c = fgetc(in); c != EOF && !whitespace_quote(c,inquote) ; c = fgetc(in), k++) {
        if (k == 0 && c == '"')
            inquote = TRUE;
        else
            next[p++] = c;
    }
    if (c == EOF && k == 0) {
        FREE_20(next);
        return NULL;
    }
    else
        next[p] = '\0';
    if (c != EOF) {
        for (k = 0, c = fgetc(in); whitespace(c) ; c = fgetc(in), k++)
            ;
        if (c != EOF)
            ungetc(c,in);
    }
#ifdef DEBUG
    if (next)
        printf("<%s>\n",next);
#endif
    return next;
}

static int get_fval_20(FILE *in, float *fval)
{
    char *next = next_word_20(in);
    if (next == NULL) {
        qWarning("bad integer");
        return FAIL;
    }
    else if (sscanf(next,"%g",fval) != 1) {
        qWarning("bad floating point number : %s",next);
        FREE_20(next);
        return FAIL;
    }
    FREE_20(next);
    return OK;
}

//=============================================================================================================
// DEFINE MEMBER METHODS
//=============================================================================================================

FiffCoordTransOld::FiffCoordTransOld()
{
}

//=============================================================================================================

FiffCoordTransOld::FiffCoordTransOld(const FiffCoordTransOld &p_FiffCoordTransOld)
{
    this->from = p_FiffCoordTransOld.from;
    this->to = p_FiffCoordTransOld.to;

    for (int j = 0; j < 3; j++) {
        this->move[j] = p_FiffCoordTransOld.move[j];
        this->invmove[j] = p_FiffCoordTransOld.invmove[j];
        for (int k = 0; k < 3; k++) {
            this->rot(j,k) = p_FiffCoordTransOld.rot(j,k);
            this->invrot(j,k) = p_FiffCoordTransOld.invrot(j,k);
        }
    }
}

//=============================================================================================================

FiffCoordTransOld *FiffCoordTransOld::catenate(FiffCoordTransOld *t1, FiffCoordTransOld *t2)
{
    FiffCoordTransOld* t = new FiffCoordTransOld;
    int j,k,p;

    t->to   = t1->to;
    t->from = t2->from;

    for (j = 0; j < 3; j++) {
        t->move[j] = t1->move[j];
        for (k = 0; k < 3; k++) {
            t->rot(j,k) = 0.0;
            t->move[j] += t1->rot(j,k)*t2->move[k];
            for (p = 0; p < 3; p++)
                t->rot(j,k) += t1->rot(j,p)*t2->rot(p,k);
        }
    }
    add_inverse(t);
    return (t);
}

//=============================================================================================================

FiffCoordTransOld::FiffCoordTransOld(int from, int to, float rot[3][3], float move[3])
{
    this->from = from;
    this->to   = to;

    for (int j = 0; j < 3; j++) {
        this->move[j] = move[j];
        for (int k = 0; k < 3; k++)
            this->rot(j,k) = rot[j][k];
    }
    add_inverse(this);
}

//=============================================================================================================

FiffCoordTransOld::~FiffCoordTransOld()
{
}

//=============================================================================================================

FiffCoordTrans FiffCoordTransOld::toNew()
{
    FiffCoordTrans t = *new FiffCoordTrans;
    MatrixXf trans = MatrixXf::Identity(4,4);
    trans.block(0,0,3,3) = this->rot;
    trans.block(0,3,3,1) = this->move;
    t.to = this->to;
    t.from = this->from;
    t.trans = trans;
    t.addInverse(t);
    return t;
}

//=============================================================================================================

int FiffCoordTransOld::add_inverse(FiffCoordTransOld *t)
{
    Matrix4f m = Matrix4f::Identity(4,4);

    m.block(0,0,3,3) = t->rot;
    m.block(0,3,3,1) = t->move;

    m = m.inverse().eval();

    t->invrot = m.block(0,0,3,3);
    t->invmove = m.block(0,3,3,1);

    return OK;
}

//=============================================================================================================

FiffCoordTransOld *FiffCoordTransOld::fiff_invert_transform() const
{
    FiffCoordTransOld* ti = new FiffCoordTransOld;
    ti->move = this->invmove;
    ti->invmove = this ->move;
    ti->rot = this->invrot;
    ti->invrot = this ->rot;
    ti->from = this->to;
    ti->to   = this->from;
    return (ti);
}

//=============================================================================================================

void FiffCoordTransOld::fiff_coord_trans(float r[], const FiffCoordTransOld *t, int do_move)
/*
 * Apply coordinate transformation
 */
{
    int j,k;
    float res[3];

    for (j = 0; j < 3; j++) {
        res[j] = (do_move ? t->move[j] :  0.0);
        for (k = 0; k < 3; k++)
            res[j] += t->rot(j,k)*r[k];
    }
    for (j = 0; j < 3; j++)
        r[j] = res[j];
}

//=============================================================================================================

FiffCoordTransOld *FiffCoordTransOld::fiff_combine_transforms(int from, int to, FiffCoordTransOld *t1, FiffCoordTransOld *t2)
/*
 * Combine two coordinate transformations
 * to yield a transform from 'from' system
 * to 'to' system.
 *
 * Return NULL if this fails
 *
 */
{
    FiffCoordTransOld* t = NULL;
    int swapped = 0;
    FiffCoordTransOld* temp;
    /*
       * We have a total of eight possibilities:
       * four without swapping and four with
       */
    while (1) {
        if (t1->to == to && t2->from == from) {
            t1 = new FiffCoordTransOld(*t1);
            t2 = new FiffCoordTransOld(*t2);
            break;
        }
        else if (t1->from == to && t2->from == from) {
            FiffCoordTransOld* tmp_t1 = t1;
            t1 = tmp_t1->fiff_invert_transform();//Memory leak here!!
            delete tmp_t1;
            t2 = new FiffCoordTransOld(*t2);
            break;
        }
        else if (t1->to == to && t2->to == from) {
            t1 = new FiffCoordTransOld(*t1);
            FiffCoordTransOld* tmp_t2 = t2;
            t2 = tmp_t2->fiff_invert_transform();//Memory leak here!!
            delete tmp_t2;
            break;
        }
        else if (t1->from == to && t2->to == from) {
            FiffCoordTransOld* tmp_t1 = t1;
            t1 = tmp_t1->fiff_invert_transform();//Memory leak here!!
            delete tmp_t1;
            FiffCoordTransOld* tmp_t2 = t2;
            t2 = tmp_t2->fiff_invert_transform();//Memory leak here!!
            delete tmp_t2;
            break;
        }
        if (swapped) {  /* Already swapped and not found */
            qCritical("Cannot combine coordinate transforms");
            return (t);
        }
        temp = t1;      /* Try it swapped as well */
        t1 = t2;
        t2 = temp;
        swapped = 1;
    }
    if (t1->from  != t2->to)    /* Transforms must match on the other side as well */
        qCritical ("Cannot combine coordinate transforms");
    else                        /* We can do it directly */
        t = catenate(t1,t2);
    FREE_20(t1); FREE_20(t2);
    return (t);
}

//=============================================================================================================

void FiffCoordTransOld::fiff_coord_trans_inv(float r[], FiffCoordTransOld *t, int do_move)
/*
 * Apply inverse coordinate transformation
 */
{
    int j,k;
    float res[3];

    for (j = 0; j < 3; j++) {
        res[j] = (do_move ? t->invmove[j] :  0.0);
        for (k = 0; k < 3; k++)
            res[j] += t->invrot(j,k)*r[k];
    }
    for (j = 0; j < 3; j++)
        r[j] = res[j];
}

//=============================================================================================================

typedef struct {
    int frame;
    const char *name;
} frameNameRec;

const char *FiffCoordTransOld::mne_coord_frame_name(int frame)
{
    static frameNameRec frames[] = {
        {FIFFV_COORD_UNKNOWN,"unknown"},
        {FIFFV_COORD_DEVICE,"MEG device"},
        {FIFFV_COORD_ISOTRAK,"isotrak"},
        {FIFFV_COORD_HPI,"hpi"},
        {FIFFV_COORD_HEAD,"head"},
        {FIFFV_COORD_MRI,"MRI (surface RAS)"},
        {FIFFV_MNE_COORD_MRI_VOXEL, "MRI voxel"},
        {FIFFV_COORD_MRI_SLICE,"MRI slice"},
        {FIFFV_COORD_MRI_DISPLAY,"MRI display"},
        {FIFFV_MNE_COORD_CTF_DEVICE,"CTF MEG device"},
        {FIFFV_MNE_COORD_CTF_HEAD,"CTF/4D/KIT head"},
        {FIFFV_MNE_COORD_RAS,"RAS (non-zero origin)"},
        {FIFFV_MNE_COORD_MNI_TAL,"MNI Talairach"},
        {FIFFV_MNE_COORD_FS_TAL_GTZ,"Talairach (MNI z > 0)"},
        {FIFFV_MNE_COORD_FS_TAL_LTZ,"Talairach (MNI z < 0)"},
        {-1,"unknown"}
    };
    int k;
    for (k = 0; frames[k].frame != -1; k++) {
        if (frame == frames[k].frame)
            return frames[k].name;
    }
    return frames[k].name;
}

//=============================================================================================================

void FiffCoordTransOld::mne_print_coord_transform_label(FILE *log, char *label, FiffCoordTransOld *t)
{
    int k,p;
    int frame;
    if (!label || strlen(label) == 0)
        fprintf(log,"Coordinate transformation: ");
    else
        fprintf(log,"%s",label);
    for (frame = t->from, k = 0; k < 2; k++) {
        if (k == 0) {
            fprintf(log,"%s -> ",mne_coord_frame_name(frame));
            frame = t->to;
        }
        else {
            fprintf(log,"%s\n",mne_coord_frame_name(frame));
            for (p = 0; p < 3; p++)
                fprintf(log,"\t% 8.6f % 8.6f % 8.6f\t% 7.2f mm\n",
                        t->rot(p,X_20),t->rot(p,Y_20),t->rot(p,Z_20),1000*t->move[p]);
            fprintf(log,"\t% 8.6f % 8.6f % 8.6f  % 7.2f\n",0.0,0.0,0.0,1.0);
        }
    }
}

//=============================================================================================================

void FiffCoordTransOld::mne_print_coord_transform(FILE *log, FiffCoordTransOld *t)
{
    mne_print_coord_transform_label(log,NULL,t);
}

//=============================================================================================================

FiffCoordTransOld *FiffCoordTransOld::mne_read_transform(const QString &name, int from, int to)
/*
          * Read the specified coordinate transformation
          */
{
    QFile file(name);
    FiffStream::SPtr stream(new FiffStream(&file));

    FiffCoordTransOld* res = NULL;
    //    fiffFile       in = NULL;
    FiffTag::SPtr t_pTag;
    //    fiffTagRec     tag;
    //    fiffDirEntry   dir;
    fiff_int_t kind, pos;
    int k;

    //    tag.data = NULL;
    //    if ((in = fiff_open(name.toUtf8().data())) == NULL)
    //        goto out;
    if(!stream->open())
        goto out;

    for (k = 0; k < stream->dir().size(); k++) {
        kind = stream->dir()[k]->kind;
        pos  = stream->dir()[k]->pos;
        if (kind == FIFF_COORD_TRANS) {
            //            if (fiff_read_this_tag (in->fd,dir->pos,&tag) == FIFF_FAIL)
            //                goto out;
            //            res = (fiffCoordTrans)tag.data;
            if (!stream->read_tag(t_pTag,pos))
                goto out;

            res = FiffCoordTransOld::read_helper( t_pTag );
            if (res->from == from && res->to == to) {
                //                tag.data = NULL;
                goto out;
            }
            else if (res->from == to && res->to == from) {
                FiffCoordTransOld* tmp_res = res;
                res = tmp_res->fiff_invert_transform();//Memory leak here!!
                delete tmp_res;
                goto out;
            }
            res = NULL;
        }
    }
    qCritical("No suitable coordinate transformation found in %s.",name.toUtf8().data());
    goto out;

out : {
        //        FREE(tag.data);
        //        fiff_close(in);
        stream->close();
        return res;
    }

    return res;
}

//=============================================================================================================

FiffCoordTransOld *FiffCoordTransOld::mne_read_transform_from_node(FiffStream::SPtr &stream, const FiffDirNode::SPtr &node, int from, int to)
/*
 * Read the specified coordinate transformation
 */
{
    FiffCoordTransOld* res = NULL;
    FiffTag::SPtr t_pTag;
    //    fiffTagRec     tag;
    //    fiffDirEntry   dir;
    fiff_int_t kind, pos;
    int k;

    //    tag.data = NULL;
    for (k = 0; k < node->nent(); k++)
        kind = node->dir[k]->kind;
    pos  = node->dir[k]->pos;
    if (kind == FIFF_COORD_TRANS) {
        //            if (fiff_read_this_tag (in->fd,dir->pos,&tag) == FIFF_FAIL)
        //                goto out;
        //            res = (fiffCoordTrans)tag.data;
        if (!stream->read_tag(t_pTag,pos))
            goto out;
        res = FiffCoordTransOld::read_helper( t_pTag );
        if (res->from == from && res->to == to) {
            //                tag.data = NULL;
            goto out;
        }
        else if (res->from == to && res->to == from) {
            FiffCoordTransOld* tmp_res = res;
            res = tmp_res->fiff_invert_transform();//Memory leak here!!
            delete tmp_res;
            goto out;
        }
        res = NULL;
    }
    printf("No suitable coordinate transformation found");
    goto out;

out : {
        //        FREE(tag.data);
        return res;
    }
}

//=============================================================================================================

FiffCoordTransOld *FiffCoordTransOld::mne_read_mri_transform(const QString &name)
/*
          * Read the MRI -> HEAD coordinate transformation
          */
{
    return mne_read_transform(name,FIFFV_COORD_MRI,FIFFV_COORD_HEAD);
}

//=============================================================================================================

FiffCoordTransOld *FiffCoordTransOld::mne_read_meas_transform(const QString &name)
/*
 * Read the MEG device -> HEAD coordinate transformation
 */
{
    return mne_read_transform(name,FIFFV_COORD_DEVICE,FIFFV_COORD_HEAD);
}

//=============================================================================================================

FiffCoordTransOld *FiffCoordTransOld::mne_read_transform_ascii(char *name, int from, int to)
/*
 * Read the Neuromag -> FreeSurfer transformation matrix
 */
{
    FILE *in = NULL;
    FiffCoordTransOld* t = NULL;
    float rot[3][3];
    float move[3];
    int   k;
    float dum;

    if ((in = fopen(name,"r")) == NULL) {
        qCritical(name);
        goto bad;
    }
    for (k = 0; k < 3; k++) {
        if (get_fval_20(in,rot[k]+X_20) == FAIL)
            goto noread;
        if (get_fval_20(in,rot[k]+Y_20) == FAIL)
            goto noread;
        if (get_fval_20(in,rot[k]+Z_20) == FAIL)
            goto noread;
        if (get_fval_20(in,move+k) == FAIL)
            goto noread;
    }
    for (k = 0; k < 4; k++) {
        if (get_fval_20(in,&dum) == FAIL)
            goto noread;
    }
    fclose(in);
    for (k = 0; k < 3; k++)
        move[k] = move[k]/1000.0;
    t  = new FiffCoordTransOld(from, to, rot, move );
    return t;

noread : {
        qCritical("Cannot read the coordinate transformation");
        goto bad;
    }

bad : {
        if(t)
            delete t;
        if (in != NULL)
            fclose(in);
        return NULL;
    }
}

//=============================================================================================================

FiffCoordTransOld *FiffCoordTransOld::mne_read_FShead2mri_transform(char *name)
{
    return mne_read_transform_ascii(name,FIFFV_COORD_HEAD,FIFFV_COORD_MRI);
}

//=============================================================================================================

FiffCoordTransOld *FiffCoordTransOld::mne_identity_transform(int from, int to)
{
    float rot[3][3] = { { 1.0, 0.0, 0.0 },
                        { 0.0, 1.0, 0.0 },
                        { 0.0, 0.0, 1.0 } };
    float move[] = { 0.0, 0.0, 0.0 };
    return new FiffCoordTransOld(from,to,rot,move);
}

//=============================================================================================================

FiffCoordTransOld * FiffCoordTransOld::fiff_make_transform_card (int from,int to,
                                                                 float *rL,
                                                                 float *rN,
                                                                 float *rR)
/* 'from' coordinate system
 * cardinal points expressed in
 * the 'to' system */

{
    FiffCoordTransOld* t = new FiffCoordTransOld();
    float ex[3],ey[3],ez[3];	/* The unit vectors */
    float alpha,alpha1,len;
    float diff1[3],diff2[3];
    int   k;
    float r0[3];

    t->from = from;
    t->to   = to;
    for (k = 0; k < 3; k++) {
        diff1[k] = rN[k] - rL[k];
        diff2[k] = rR[k] - rL[k];
    }
    alpha = VEC_DOT_20(diff1,diff2)/VEC_DOT_20(diff2,diff2);
    len = VEC_LEN_20(diff2);
    alpha1 = 1.0 - alpha;

    for (k = 0; k < 3; k++) {
        r0[k] = alpha1*rL[k] + alpha*rR[k];
        ex[k] = diff2[k]/len;
        ey[k] = rN[k] - r0[k];
        t->move[k] = r0[k];
    }

    len = VEC_LEN_20(ey);

    for (k = 0; k < 3; k++)
        ey[k] = ey[k]/len;

    CROSS_PRODUCT_20 (ex,ey,ez);

    for (k = 0; k < 3; k++) {
        t->rot(k,X_20) = ex[k];
        t->rot(k,Y_20) = ey[k];
        t->rot(k,Z_20) = ez[k];
    }

    add_inverse (t);

    return (t);
}

//=============================================================================================================

FiffCoordTransOld* FiffCoordTransOld::procrustes_align(int   from_frame,  /* The coordinate frames */
                       int   to_frame,
                       float **fromp,     /* Point locations in these two coordinate frames */
                       float **top,
                       float *w,	  /* Optional weights */
                       int   np,	  /* How many points */
                       float max_diff)	  /* Maximum allowed difference */
/*
 * Perform an alignment using the the solution of the orthogonal (weighted) Procrustes problem
 */
{
    float **from = ALLOC_CMATRIX_20(np,3);
    float **to   = ALLOC_CMATRIX_20(np,3);
    float from0[3],to0[3],rr[3],diff[3];
    int   j,k,c,p;
    float rot[3][3];
    float move[3];

    /*
     * Calculate the centroids and subtract;
     */
    for (c = 0; c < 3; c++)
        from0[c] = to0[c] = 0.0;
    for (j = 0; j < np; j++) {
        for (c = 0; c < 3; c++) {
            from0[c] += fromp[j][c];
            to0[c] += top[j][c];
        }
    }
    for (c = 0; c < 3; c++) {
        from0[c] = from0[c]/np;
        to0[c] = to0[c]/np;
    }
    for (j = 0; j < np; j++) {
        for (c = 0; c < 3; c++) {
            from[j][c] = fromp[j][c] - from0[c];
            to[j][c]   = top[j][c]    - to0[c];
        }
    }
    /*
     * Compute the solution of the orthogonal Proscrustes problem
     */
    {
        float **S;
        float **uu = ALLOC_CMATRIX_20(3,3);
        float **vv = ALLOC_CMATRIX_20(3,3);
        float **R = NULL;
        float sing[3];

        if (w) {
            /*
            * This is the weighted version which allows multiplicity of points
            */
            S = ALLOC_CMATRIX_20(3,3);
            for (j = 0; j < 3; j++) {
                for (k = 0; k < 3; k++) {
                    S[j][k] = 0.0;
                    for (p = 0; p < np; p++)
                        S[j][k] += w[p]*from[p][j]*to[p][k];
                }
            }
        }
        else
            S = mne_matt_mat_mult_20(from,to,3,np,3);
        if (mne_svd_20(S,3,3,sing,uu,vv) != 0) {
            FREE_CMATRIX_20(S);
            FREE_CMATRIX_20(uu);
            FREE_CMATRIX_20(vv);
            goto bad;
        }
        R = mne_matt_mat_mult_20(vv,uu,3,3,3);
        for (j = 0; j < 3; j++)
            for (k = 0; k < 3; k++)
                rot[j][k] = R[j][k];
        FREE_CMATRIX_20(R);
        FREE_CMATRIX_20(S);
        FREE_CMATRIX_20(uu);
        FREE_CMATRIX_20(vv);
    }
    /*
     * Now we need to generate a transformed translation vector
     */
    for (j = 0; j < 3; j++) {
        move[j] = to0[j];
        for (k = 0; k < 3; k++)
            move[j] = move[j] - rot[j][k]*from0[k];
    }
    /*
     * Test the transformation and print the results
     */
    #ifdef DEBUG
    fprintf(stderr,"Procrustes matching (desired vs. transformed) :\n");
    #endif
    for (p = 0; p < np; p++) {
        for (j = 0; j < 3; j++) {
            rr[j] = move[j];
        for (k = 0; k < 3; k++)
            rr[j] += rot[j][k]*fromp[p][k];
        }
        VEC_DIFF_20(top[p],rr,diff);
        #ifdef DEBUG
        fprintf(stderr,"\t%7.2f %7.2f %7.2f mm <-> %7.2f %7.2f %7.2f mm diff = %8.3f mm\n",
        1000*top[p][0],1000*top[p][1],1000*top[p][2],
        1000*rr[0],1000*rr[1],1000*rr[2],1000*VEC_LEN(diff));
        #endif
        if (VEC_LEN_20(diff) > max_diff) {
            printf("To large difference in matching : %7.1f > %7.1f mm", 1000*VEC_LEN_20(diff),1000*max_diff);
            goto bad;
        }
    }
    #ifdef DEBUG
    fprintf(stderr,"\n");
    #endif

    FREE_CMATRIX_20(from);
    FREE_CMATRIX_20(to);

    return new FiffCoordTransOld(from_frame,to_frame,rot,move);

    bad : {
        FREE_CMATRIX_20(from);
        FREE_CMATRIX_20(to);
        return NULL;
    }
}

//=============================================================================================================

FiffCoordTransOld *FiffCoordTransOld::read_helper( FIFFLIB::FiffTag::SPtr& tag)
{
    FiffCoordTransOld* p_FiffCoordTrans = NULL;
    if(tag->isMatrix() || tag->getType() != FIFFT_COORD_TRANS_STRUCT || tag->data() == NULL)
        return p_FiffCoordTrans;
    else
    {
        p_FiffCoordTrans = new FiffCoordTransOld;
        qint32* t_pInt32 = (qint32*)tag->data();
        p_FiffCoordTrans->from = t_pInt32[0];
        p_FiffCoordTrans->to = t_pInt32[1];

        float* t_pFloat = (float*)tag->data();
        int count = 0;
        int r, c;
        for (r = 0; r < 3; ++r) {
            p_FiffCoordTrans->move[r] = t_pFloat[11+r];
            for (c = 0; c < 3; ++c) {
                p_FiffCoordTrans->rot(r,c) = t_pFloat[2+count];
                ++count;
            }
        }

        count = 0;
        for (r = 0; r < 3; ++r) {
            p_FiffCoordTrans->invmove[r] = t_pFloat[23+r];
            for (c = 0; c < 3; ++c) {
                p_FiffCoordTrans->invrot(r,c) = t_pFloat[14+count];
                ++count;
            }
        }

        return p_FiffCoordTrans;
    }
}


/* This file is part of VoltDB.
 * Copyright (C) 2008-2020 VoltDB Inc.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License as
 * published by the Free Software Foundation, either version 3 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 Affero General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with VoltDB.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <memory>
#include <sstream>

#include <boost/algorithm/string.hpp>
#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/tokenizer.hpp>

#include "common/ValueFactory.hpp"
#include "expressions/geofunctions.h"

namespace voltdb {

static const int POINT = FUNC_VOLT_POINTFROMTEXT;
static const int POLY = FUNC_VOLT_POLYGONFROMTEXT;

static const double SPHERICAL_EARTH_MEAN_RADIUS_M = 6371008.8; // mean radius in meteres
static const double RADIUS_SQ_M = SPHERICAL_EARTH_MEAN_RADIUS_M * SPHERICAL_EARTH_MEAN_RADIUS_M;

typedef boost::tokenizer<boost::char_separator<char> > Tokenizer;

static bool isMultiPolygon(const Polygon &poly, std::stringstream *msg);
static void throwInvalidWktPoint(const std::string& input) {
    std::ostringstream oss;
    oss << "Invalid input to POINTFROMTEXT: ";
    oss << "'" << input << "', ";
    oss << "expected input of the form 'POINT(<lng> <lat>)'";
    throw SQLException(SQLException::data_exception_invalid_parameter,
                       oss.str().c_str());
}

static void throwInvalidWktPoly(const std::string& reason) {
    std::ostringstream oss;
    oss << "Invalid input to POLYGONFROMTEXT: " << reason << ".  ";
    oss << "Expected input of the form 'POLYGON((<lng> <lat>, ...), ...)'";
    throw SQLException(SQLException::data_exception_invalid_parameter,
                       oss.str().c_str());
}

static void throwInvalidMakeValidPoly(const std::string& reason) {
    std::ostringstream oss;
    oss << "Invalid input to MAKE_VALID_POLYGON: " << reason << ".";
    throw SQLException(SQLException::data_exception_invalid_parameter,
                       oss.str().c_str());
}

static void throwInvalidPointLatitude(const std::string& input) {
    std::ostringstream oss;
    oss << "Invalid input to POINTFROMTEXT: '" << input << "'";
    oss << ".  Latitude must be in the range [-90,90].";
    throw SQLException(SQLException::data_exception_invalid_parameter,
                       oss.str().c_str());
}

static void throwInvalidPointLongitude(const std::string& input) {
    std::ostringstream oss;
    oss << "Invalid input to POINTFROMTEXT: '" << input << "'";
    oss << ".  Longitude must be in the range [-180,180].";
    throw SQLException(SQLException::data_exception_invalid_parameter,
                       oss.str().c_str());
}

static void throwInvalidPolygonLatitude(const std::string& input) {
    std::ostringstream oss;
    oss << "Invalid input to POLYGONFROMTEXT: '" << input << "'";
    oss << ".  Latitude must be in the range [-90,90].";
    throw SQLException(SQLException::data_exception_invalid_parameter,
                       oss.str().c_str());
}

static void throwInvalidPolygonLongitude(const std::string& input) {
    std::ostringstream oss;
    oss << "Invalid input to POLYGONFROMTEXT: '" << input << "'";
    oss << ".  Longitude must be in the range [-180,180].";
    throw SQLException(SQLException::data_exception_invalid_parameter,
                       oss.str().c_str());
}

static void throwInvalidDistanceDWithin(const std::string& msg) {
    std::ostringstream oss;
    oss << "Invalid input to DWITHIN function: '" << msg << "'.";
    throw SQLException(SQLException::data_exception_invalid_parameter,
                       oss.str().c_str());
}

static GeographyPointValue::Coord stringToCoord(int pointOrPoly,
        const std::string& input, const std::string& val) {
    GeographyPointValue::Coord coord = 0.0;
    try {
        coord = boost::lexical_cast<GeographyPointValue::Coord>(val);
    } catch (const boost::bad_lexical_cast&) {
        if (pointOrPoly == POLY) {
            throwInvalidWktPoly("expected a number but found '" + val + "'");
        } else {
            throwInvalidWktPoint(input);
        }
    }

    return coord;
}

// function computes distance between two non-null points
// function computes distance using Haversine formula
static double getDistance(const GeographyPointValue &point1,
                          const GeographyPointValue &point2) {
    vassert(!point1.isNull());
    vassert(!point2.isNull());

    const S2LatLng latLng1 = S2LatLng(point1.toS2Point()).Normalized();
    const S2LatLng latLng2 = S2LatLng(point2.toS2Point()).Normalized();
    S1Angle distance = latLng1.GetDistance(latLng2);
    return distance.radians() * SPHERICAL_EARTH_MEAN_RADIUS_M;
}

template<> NValue NValue::callUnary<FUNC_VOLT_POINTFROMTEXT>() const {
    if (isNull()) {
        return NValue::getNullValue(ValueType::tPOINT);
    }

    int32_t textLength;
    const char* textData = getObject_withoutNull(textLength);
    std::string wkt(textData, textLength);

    // Discard whitespace, but return commas or parentheses as tokens
    Tokenizer tokens(wkt, boost::char_separator<char>(" \f\n\r\t\v", ",()"));
    Tokenizer::iterator it = tokens.begin();
    Tokenizer::iterator end = tokens.end();

    if (! boost::iequals(*it, "point")) {
        throwInvalidWktPoint(wkt);
    }
    ++it;

    if (! boost::iequals(*it, "(")) {
        throwInvalidWktPoint(wkt);
    }
    ++it;

    GeographyPointValue::Coord lng = stringToCoord(POINT, wkt, *it);
    ++it;

    GeographyPointValue::Coord lat = stringToCoord(POINT, wkt, *it);
    ++it;

    if ( lng < -180.0 || lng > 180.0) {
        throwInvalidPointLongitude(wkt);
    } else if (lat < -90.0 || lat > 90.0 ) {
        throwInvalidPointLatitude(wkt);
    }

    if (! boost::iequals(*it, ")")) {
        throwInvalidWktPoint(wkt);
    }
    ++it;

    if (it != end) {
        throwInvalidWktPoint(wkt);
    }

    NValue returnValue(ValueType::tPOINT);
    returnValue.getGeographyPointValue() = GeographyPointValue(lng, lat);

    return returnValue;
}

#undef DEBUG_POLYGONS

#if defined(DEBUG_POLYGONS)
static void printLoop(int lidx, bool is_shell, S2Loop *loop) {
    std::cout << "Loop " << lidx << ": ";
    std::cout << (is_shell ? "" : "not ") << "a shell, ";
    std::cout << "depth = " << loop->depth();
    std::cout << ", is_hole = " << loop->is_hole();
    std::cout << ", points: ";
    std::string sep("");
    for (int idx = 0; idx < loop->num_vertices(); idx += 1) {
        S2LatLng ll(loop->vertex(idx));
        std::cout << sep << "(" << ll.lng().degrees() << ", " << ll.lat().degrees() << ")";
        sep = ", ";
    }
    std::cout << "\n";
}

static void printPolygon(const std::string &label, const S2Polygon *poly) {
    std::cout << label << ":\n";
    std::cout << (poly->has_holes() ? "Has holes" : "Has no holes") << std::endl;
    for (int lidx = 0; lidx < poly->num_loops(); lidx += 1) {
        S2Loop *loop = poly->loop(lidx);
        printLoop(lidx, !loop->is_hole(), loop);
    }
    std::cout << std::flush;
}
#else
#define printLoop(lidx, is_shell, loop)
#define printPolygon(label, poly)
#endif

static void readLoop(bool is_shell,
        const std::string &wkt,
        Tokenizer::iterator &it,
        const Tokenizer::iterator &end,
        S2Loop *loop) {
    if (! boost::iequals(*it, "(")) {
        throwInvalidWktPoly("expected left parenthesis to start a ring");
    }
    ++it;

    std::vector<S2Point> points;
    while (it != end && *it != ")") {
        GeographyPointValue::Coord lng = stringToCoord(POLY, wkt, *it);

        if (lng < -180 || lng > 180) {
            throwInvalidPolygonLongitude(*it);
        }
        ++it;
        GeographyPointValue::Coord lat = stringToCoord(POLY, wkt, *it);
        if (lat < -90 || lat > 90) {
            throwInvalidPolygonLatitude(*it);
        }
        ++it;

        // Note: This is S2.  It takes latitude, longitude, not
        //       longitude, latitude.
        points.push_back(S2LatLng::FromDegrees(lat, lng).ToPoint());

        if (*it == ",") {
            ++it;
            // continue to next lat long pair
        } else if (*it != ")") {
            throwInvalidWktPoly("unexpected token: '" + (*it) + "'");
        }
    }

    if (it == end) {
        // we hit the end of input before the closing parenthesis
        throwInvalidWktPoly("unexpected end of input");
    }

    vassert(*it == ")");

    // Advance iterator to next token
    ++it;

    if (points.size() < 4) {
        throwInvalidWktPoly("A polygon ring must contain at least 4 points (including repeated closing vertex)");
    }

    const S2Point& first = points.at(0);
    const S2Point& last = points.at(points.size() - 1);

    if (first != last) {
        throwInvalidWktPoly("A polygon ring's first vertex must be equal to its last vertex");
    }

    // S2 format considers the closing vertex in a loop to be
    // implicit, while in WKT it is explicit.  Remove the closing
    // vertex here to reflect this.
    points.pop_back();
    // The first is a shell.  All others are holes.  We need to reverse
    // the order of the vertices for holes.
    if (!is_shell) {
        // Don't touch the first point.  We don't want to
        // cycle the vertices.
        std::reverse(++(points.begin()), points.end());
    }
    loop->Init(points);
}

static NValue polygonFromText(const std::string &wkt, bool doRepairs) {
    // Discard whitespace, but return commas or parentheses as tokens
    Tokenizer tokens(wkt, boost::char_separator<char>(" \f\n\r\t\v", ",()"));
    Tokenizer::iterator it = tokens.begin();
    Tokenizer::iterator end = tokens.end();

    if (! boost::iequals(*it, "polygon")) {
        throwInvalidWktPoly("does not start with POLYGON keyword");
    }
    ++it;

    if (! boost::iequals(*it, "(")) {
        throwInvalidWktPoly("missing left parenthesis after POLYGON keyword");
    }
    ++it;

    bool is_shell = true;
    // This is the length of the polygon when serialized.
    // We could get this with Polygon::serializedLenth.  But
    // that would require traversing the loops twice, and
    // who has time for that?
    std::size_t length = Polygon::serializedLengthNoLoops();
    std::vector<std::unique_ptr<S2Loop> > loops;
    while (it != end) {
        loops.push_back(std::unique_ptr<S2Loop>(new S2Loop()));
        readLoop(is_shell, wkt, it, end, loops.back().get());
        // Only the first loop is a shell.
        is_shell = false;
        length += Loop::serializedLength(loops.back()->num_vertices());
        if (*it == ",") {
            ++it;
        } else if (*it == ")") {
            ++it;
            break;
        } else {
            throwInvalidWktPoly("unexpected token: '" + (*it) + "'");
        }
    }

    if (it != end) {
        // extra stuff after input
        throwInvalidWktPoly("unrecognized input after WKT: '" + (*it) + "'");
    }

    NValue nval = ValueFactory::getUninitializedTempGeographyValue(length);
    char* storage = const_cast<char*>(ValuePeeker::peekObjectValue(nval));

    Polygon poly;
    poly.init(&loops, doRepairs); // polygon takes ownership of loops here.
    if (doRepairs) {
        std::stringstream validReason;
        if (!poly.IsValid(&validReason) || isMultiPolygon(poly, &validReason)) {
            throwInvalidWktPoly(validReason.str());
        }
    }
    SimpleOutputSerializer output(storage, length);
    poly.saveToBuffer(output);
    return nval;
}

template<> NValue NValue::callUnary<FUNC_VOLT_POLYGONFROMTEXT>() const {
    if (isNull()) {
        return NValue::getNullValue(ValueType::tGEOGRAPHY);
    }

    int32_t textLength;
    const char* textData = getObject_withoutNull(textLength);
    const std::string wkt(textData, textLength);

    return polygonFromText(wkt, false);
}

template<> NValue NValue::callUnary<FUNC_VOLT_VALIDPOLYGONFROMTEXT>() const {
    if (isNull()) {
        return NValue::getNullValue(ValueType::tGEOGRAPHY);
    }

    int32_t textLength;
    const char* textData = getObject_withoutNull(textLength);
    const std::string wkt(textData, textLength);

    return polygonFromText(wkt, true);
}

template<> NValue NValue::call<FUNC_VOLT_CONTAINS>(const std::vector<NValue>& arguments) {
    if (arguments[0].isNull() || arguments[1].isNull())
        return NValue::getNullValue(ValueType::tBOOLEAN);

    Polygon poly;
    poly.initFromGeography(arguments[0].getGeographyValue());
    S2Point pt = arguments[1].getGeographyPointValue().toS2Point();
    return ValueFactory::getBooleanValue(poly.Contains(pt));
}

template<> NValue NValue::callUnary<FUNC_VOLT_POLYGON_NUM_INTERIOR_RINGS>() const {
    if (isNull()) {
        return NValue::getNullValue(ValueType::tINTEGER);
    }

    Polygon poly;
    poly.initFromGeography(getGeographyValue());

    NValue retVal(ValueType::tINTEGER);
    // exclude exterior ring
    retVal.getInteger() = poly.num_loops() - 1;
    return retVal;
}

template<> NValue NValue::callUnary<FUNC_VOLT_POLYGON_NUM_POINTS>() const {
    if (isNull()) {
        return NValue::getNullValue(ValueType::tINTEGER);
    }

    Polygon poly;
    poly.initFromGeography(getGeographyValue());

    // the OGC spec suggests that the number of vertices should
    // include the repeated closing vertex which is implicit in S2's
    // representation.  So add an extra vertex for each loop.
    int32_t numPoints = poly.num_vertices() + poly.num_loops();

    NValue retVal(ValueType::tINTEGER);
    retVal.getInteger() = numPoints;
    return retVal;
}

template<> NValue NValue::callUnary<FUNC_VOLT_POINT_LATITUDE>() const {
    if (isNull()) {
        return NValue::getNullValue(ValueType::tDOUBLE);
    }
    const GeographyPointValue point = getGeographyPointValue();
    NValue retVal(ValueType::tDOUBLE);
    retVal.getDouble() = point.getLatitude();
    return retVal;
}

template<> NValue NValue::callUnary<FUNC_VOLT_POINT_LONGITUDE>() const {
    if (isNull()) {
        return NValue::getNullValue(ValueType::tDOUBLE);
    }
    const GeographyPointValue point = getGeographyPointValue();
    NValue retVal(ValueType::tDOUBLE);
    retVal.getDouble() = point.getLongitude();
    return retVal;
}

template<> NValue NValue::callUnary<FUNC_VOLT_POLYGON_CENTROID>() const {
    if (isNull()) {
        return NValue::getNullValue(ValueType::tPOINT);
    }

    Polygon polygon;
    polygon.initFromGeography(getGeographyValue());
    const GeographyPointValue point(polygon.GetCentroid());
    NValue retVal(ValueType::tPOINT);
    retVal.getGeographyPointValue() = point;
    return retVal;
}

template<> NValue NValue::callUnary<FUNC_VOLT_POLYGON_AREA>() const {
    if (isNull()) {
        return NValue::getNullValue(ValueType::tDOUBLE);
    }

    Polygon polygon;
    polygon.initFromGeography(getGeographyValue());

    NValue retVal(ValueType::tDOUBLE);
    // area is in steradians which is a solid angle. Earth in the calculation is treated as sphere
    // and area of sphere can be calculated as steradians * radius^2
    retVal.getDouble() = polygon.GetArea() * RADIUS_SQ_M;
    return retVal;
}

template<> NValue NValue::call<FUNC_VOLT_DISTANCE_POLYGON_POINT>(const std::vector<NValue>& arguments) {
    vassert(arguments[0].getValueType() == ValueType::tGEOGRAPHY);
    vassert(arguments[1].getValueType() == ValueType::tPOINT);

    if (arguments[0].isNull() || arguments[1].isNull()) {
        return NValue::getNullValue(ValueType::tDOUBLE);
    }

    Polygon polygon;
    polygon.initFromGeography(arguments[0].getGeographyValue());
    GeographyPointValue point = arguments[1].getGeographyPointValue();
    NValue retVal(ValueType::tDOUBLE);
    // distance is in radians, so convert it to meters
    retVal.getDouble() = polygon.getDistance(point) * SPHERICAL_EARTH_MEAN_RADIUS_M;
    return retVal;
}

template<> NValue NValue::call<FUNC_VOLT_DISTANCE_POINT_POINT>(const std::vector<NValue>& arguments) {
    vassert(arguments[0].getValueType() == ValueType::tPOINT);
    vassert(arguments[1].getValueType() == ValueType::tPOINT);

    if (arguments[0].isNull() || arguments[1].isNull()) {
        return NValue::getNullValue(ValueType::tDOUBLE);
    } else {
        NValue retVal(ValueType::tDOUBLE);
        retVal.getDouble() = getDistance(arguments[0].getGeographyPointValue(), arguments[1].getGeographyPointValue());
        return retVal;
    }
}

template<> NValue NValue::callUnary<FUNC_VOLT_ASTEXT_GEOGRAPHY_POINT>() const {
    vassert(getValueType() == ValueType::tPOINT);
    if (isNull()) {
        return NValue::getNullValue(ValueType::tVARCHAR);
    } else {
        const std::string pointAsText = getGeographyPointValue().toWKT();
        return getTempStringValue(pointAsText.c_str(), pointAsText.length());
    }
}

template<> NValue NValue::callUnary<FUNC_VOLT_ASTEXT_GEOGRAPHY>() const {
    vassert(getValueType() == ValueType::tGEOGRAPHY);
    if (isNull()) {
        return NValue::getNullValue(ValueType::tVARCHAR);
    } else {
        const std::string polygonAsText = getGeographyValue().toWKT();
        return getTempStringValue(polygonAsText.c_str(), polygonAsText.length());
    }
}

//
// Return true if poly has more than one shell, or has shells
// inside holes.
//
static bool isMultiPolygon(const Polygon &poly, std::stringstream *msg) {
    auto nloops = poly.num_loops();
    int nouters = 0;
    for (int idx = 0; idx < nloops; idx += 1) {
        S2Loop *loop = poly.loop(idx);
        switch (loop->depth()) {
            case 0:
                nouters += 1;
                break;
            case 1:
                break;
            default:
                if (msg != NULL) {
                    (*msg) << "Polygons can only be shells or holes";
                } else {
                    VOLT_TRACE("Polygons can only be shells or holes.");
                }
                return true;
        }
        if (!loop->IsNormalized(msg)) {
            return true;
        }
    }
    if (nouters != 1) {
        if (msg != NULL) {
            (*msg) << "Polygons can have only one shell, not " << nouters;
        } else {
            VOLT_TRACE("Polygons can have only one shell, not %d.", nouters);
        }
        return true;
    } else {
        return false;
    }
}

/*
 * This and FUNC_VOLT_POLYGON_INVALID_REASON are suspiciously
 * close to the same thing.  Maybe they could be unified?
 */
template<> NValue NValue::callUnary<FUNC_VOLT_IS_VALID_POLYGON>() const {
    vassert(getValueType() == ValueType::tGEOGRAPHY);
    if (isNull()) {
        return NValue::getNullValue(ValueType::tBOOLEAN);
    }
    // Be optimistic.
    bool returnval = true;
    // Extract the polygon and check its validity.
    Polygon poly;
    poly.initFromGeography(getGeographyValue());
    if (!poly.IsValid() || isMultiPolygon(poly, NULL)) {
        returnval = false;
    }
    return ValueFactory::getBooleanValue(returnval);
}

template<> NValue NValue::callUnary<FUNC_VOLT_POLYGON_INVALID_REASON>() const {
    vassert(getValueType() == ValueType::tGEOGRAPHY);
    if (isNull()) {
        return NValue::getNullValue(ValueType::tVARCHAR);
    }
    // Extract the polygon and check its validity.
    std::stringstream msg;
    Polygon poly;
    poly.initFromGeography(getGeographyValue());
    if (poly.IsValid(&msg)) {
        isMultiPolygon(poly, &msg);
    }
    std::string res (msg.str());
    if (res.size() == 0) {
        res = std::string("Valid Polygon");
    }
    return getTempStringValue(res.c_str(),res.length());
}

template<> NValue NValue::callUnary<FUNC_VOLT_MAKE_VALID_POLYGON>() const {
    vassert(getValueType() == ValueType::tGEOGRAPHY);
    if (isNull()) {
        return NValue::getNullValue(ValueType::tGEOGRAPHY);
    }
    // Extract the polygon and check its validity.
    std::stringstream msg;
    Polygon poly;
    poly.initFromGeography(getGeographyValue(), true);
    for (int idx = 0; idx < poly.num_loops(); idx += 1) {
        S2Loop *loop = poly.loop(idx);
        if ( ! loop->IsNormalized() ) {
            std::cout << "Not normalized loop in make_valid_polygon: " << idx << "\n";
        }
    }
    if ( ! poly.IsValid(&msg)) {
        std::string res (msg.str());
        vassert(res.size() > 0);
        throwInvalidMakeValidPoly(res);
        // No return from here.
    }
    else if ( isMultiPolygon(poly, &msg)) {
        std::string res (msg.str());
        vassert(res.size() > 0);
        throwInvalidMakeValidPoly(res);
        // No return from here.
    }
    // Ok, so the polygon either was valid before or else
    // we repaired it, and it is not a multi polygon.
    // So, msg will not be the empty string, and we can package
    // this polygon up.
    //
    vassert(msg.str().size() == 0);
    int length = poly.serializedLength();
    NValue nval = ValueFactory::getUninitializedTempGeographyValue(length);
    char* storage = const_cast<char*>(ValuePeeker::peekObjectValue(nval));
    SimpleOutputSerializer output(storage, length);
    poly.saveToBuffer(output);
    return nval;
}

template<> NValue NValue::call<FUNC_VOLT_DWITHIN_POLYGON_POINT>(const std::vector<NValue>& arguments) {
    vassert(arguments[0].getValueType() == ValueType::tGEOGRAPHY);
    vassert(arguments[1].getValueType() == ValueType::tPOINT);
    vassert(isNumeric(arguments[2].getValueType()));

    if (arguments[0].isNull() || arguments[1].isNull() || arguments[2].isNull()) {
        return NValue::getNullValue(ValueType::tBOOLEAN);
    }

    Polygon polygon;
    polygon.initFromGeography(arguments[0].getGeographyValue());
    GeographyPointValue point = arguments[1].getGeographyPointValue();
    double withinDistanceOf = arguments[2].castAsDoubleAndGetValue();
    if (withinDistanceOf < 0) {
        throwInvalidDistanceDWithin("Value of DISTANCE argument must be non-negative");
    }
    double polygonToPointDistance = polygon.getDistance(point) * SPHERICAL_EARTH_MEAN_RADIUS_M;
    return ValueFactory::getBooleanValue(polygonToPointDistance <= withinDistanceOf);
}

template<> NValue NValue::call<FUNC_VOLT_DWITHIN_POINT_POINT>(const std::vector<NValue>& arguments) {
    vassert(arguments[0].getValueType() == ValueType::tPOINT);
    vassert(arguments[1].getValueType() == ValueType::tPOINT);
    vassert(isNumeric(arguments[2].getValueType()));

    if (arguments[0].isNull() || arguments[1].isNull() || arguments[2].isNull()) {
        return NValue::getNullValue(ValueType::tBOOLEAN);
    }

    double withinDistanceOf = arguments[2].castAsDoubleAndGetValue();
    if (withinDistanceOf < 0) {
        throwInvalidDistanceDWithin("Value of DISTANCE argument must be non-negative");
    }
    double pointToPointDistance = getDistance(arguments[0].getGeographyPointValue(), arguments[1].getGeographyPointValue());
    return ValueFactory::getBooleanValue(pointToPointDistance <= withinDistanceOf);
}
} // end namespace voltdb


// Copyright (C) 2007-2018 The Trustees of Indiana University.

// Use, modification and distribution is subject to the Boost Software
// License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//  Authors: Thejaka Kanewala
//           Nicholas Edmonds
//           Douglas Gregor
//           Andrew Lumsdaine

#ifndef BOOST_GRAPH_DELTA_STEPPING_SHORTEST_PATHS_NUMA_HPP
#define BOOST_GRAPH_DELTA_STEPPING_SHORTEST_PATHS_NUMA_HPP

#ifndef BOOST_GRAPH_USE_MPI
#error "Parallel BGL files should not be included unless <boost/graph/use_mpi.hpp> has been included"
#endif

#include <am++/counter_coalesced_message_type.hpp>
#include <am++/detail/thread_support.hpp>

#include <boost/graph/distributed/priority_q_defs.hpp>
#include <boost/parallel/append_buffer.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/parallel/algorithm.hpp> // for all_reduce
#include <boost/graph/parallel/thread_support.hpp> // for compare_and_swap
#include <algorithm> // for std::min, std::max
#include <boost/format.hpp>
#include <iostream>
#include <atomic>

namespace boost { namespace graph { namespace distributed {

template<typename Graph,
         typename DistanceMap,
         typename EdgeWeightMap,
       	 typename WorkStats,
	 typename PriorityQueueGenerator = thread_priority_queue_gen,
         typename MessageGenerator =
           amplusplus::simple_generator<amplusplus::counter_coalesced_message_type_gen> >
class delta_stepping_shortest_paths_numa {
  typedef delta_stepping_shortest_paths_numa<Graph, DistanceMap, EdgeWeightMap, PriorityQueueGenerator>
    self_type;

  typedef typename boost::property_map<Graph, vertex_owner_t>::const_type OwnerMap;

  typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
  typedef typename graph_traits<Graph>::degree_size_type Degree;
  typedef typename property_traits<EdgeWeightMap>::value_type Dist;
  typedef std::pair<Vertex, Dist> vertex_distance_data;

  // Constructing bucket type ...
  struct default_comparer {
    bool operator()(const vertex_distance_data& vd1, const vertex_distance_data& vd2) {
      return vd1.second > vd2.second;
    }
  };

  typedef typename PriorityQueueGenerator::template queue<vertex_distance_data,
							  default_comparer>::type Bucket;

  // Bucket is an ordering container
  //  typedef typename PriorityQueueType Bucket;

  typedef typename std::vector<Bucket*>::size_type BucketIndex;

  struct vertex_distance_handler;

  typedef typename MessageGenerator::template call_result<vertex_distance_data, vertex_distance_handler,
				     vertex_distance_owner<OwnerMap, vertex_distance_data>,
				     amplusplus::idempotent_combination_t<boost::parallel::minimum<Dist>,
									  Dist> >::type
    RelaxMessage;

public:

  delta_stepping_shortest_paths_numa(Graph& g,
				     DistanceMap distance,
				     EdgeWeightMap weight,
				     amplusplus::transport &t,
				     Dist delta,
				     int offs,
                                     WorkStats& stats,
				     int freq,
				     MessageGenerator message_gen =
				     MessageGenerator(amplusplus::counter_coalesced_message_type_gen(1 << 12)))
    : dummy_first_member_for_init_order((amplusplus::register_mpi_datatype<vertex_distance_data>(), 0)),
      g(g), transport(t), distance(distance), weight(weight), delta(delta),
      core_offset(offs),
      work_stats(stats),
      owner(get(vertex_owner, g)),
      level_sync(false), current_level(0), buckets_processed(0), current_bucket(0),
      relax_msg(message_gen, transport, vertex_distance_owner<OwnerMap, vertex_distance_data>(owner),
		amplusplus::idempotent_combination(boost::parallel::minimum<Dist>(), std::numeric_limits<Dist>::max())),
      flushFrequency(freq)
  {
    initialize();
  }

#ifdef AMPLUSPLUS_PRINT_HIT_RATES
  typedef std::pair<unsigned long long, unsigned long long> cache_stats;
  cache_stats
  get_cache_stats() {
    cache_stats stats(0, 0);
    for(size_t i = 0; i < relax_msg.counters.hits.size(); ++i) {
      stats.first += relax_msg.counters.hits[i];
      stats.second += relax_msg.counters.tests[i];
    }
    return stats;
  }
#endif // AMPLUSPLUS_PRINT_HIT_RATES

  void set_source(Vertex s) { source = s; } // for threaded execution
  void set_level_sync() { level_sync = true; } // force level-synchronized exploration
  void operator() (int tid) { run(source, tid); }

  void run(Vertex s, int tid = 0);

  void initialize_threaded_buckets(int tid);
  void allocate_pqs();

  BucketIndex get_num_levels() { return buckets_processed; }
  time_type get_start_time() { return start_time; }

protected:
  void initialize();

  // Relax the edge (u, v), creating a new best path of distance x.
  void relax(const vertex_distance_data& vd);

  void find_next_bucket();

  const int dummy_first_member_for_init_order; // Unused

  const Graph& g;
  amplusplus::transport& transport;
  DistanceMap distance;
  EdgeWeightMap weight;
  Dist delta;
  int core_offset;
  WorkStats& work_stats;
  OwnerMap owner;
  Vertex source;
  bool level_sync;

  // Bucket data structure. The ith bucket contains all local vertices
  // with (tentative) distance in the range [i*delta,
  // (i+1)*delta).
  std::vector<shared_ptr<Bucket> > buckets;

  // Bucket to hold vertices deleted at each level
  shared_ptr<Bucket> deleted_vertices;
  BucketIndex current_level; // How many buckets have we processed?
  BucketIndex buckets_processed; // Stats tracking
  BucketIndex num_buckets;

  // Shared thread state to make sure we're all on the same page
  BucketIndex current_bucket;
  shared_ptr<amplusplus::detail::barrier> t_bar;
  RelaxMessage relax_msg;
  time_type start_time;
  int flushFrequency;
};

#define DELTA_STEPPING_SHORTEST_PATHS_NUMA_PARMS                                   \
      typename Graph, typename DistanceMap, typename EdgeWeightMap, typename WorkStats, typename PriorityQueueGenerator, typename MessageGenerator

#define DELTA_STEPPING_SHORTEST_PATHS_NUMA_TYPE                                    \
      delta_stepping_shortest_paths_numa<Graph, DistanceMap, EdgeWeightMap, WorkStats, PriorityQueueGenerator, MessageGenerator>

template<DELTA_STEPPING_SHORTEST_PATHS_NUMA_PARMS>
void
DELTA_STEPPING_SHORTEST_PATHS_NUMA_TYPE::initialize()
{
  int nthreads = transport.get_nthreads();

  relax_msg.set_handler(vertex_distance_handler(*this));

  // Setup distance map
  distance.set_consistency_model(0);
  set_property_map_role(vertex_distance, distance);

  using boost::parallel::all_reduce;
  using boost::parallel::maximum;
  using std::max;

  // Compute the maximum edge weight
  Dist max_edge_weight = 0;

  BGL_FORALL_VERTICES_T(u, g, Graph) {
    BGL_FORALL_OUTEDGES_T(u, e, g, Graph)
      max_edge_weight = max BOOST_PREVENT_MACRO_SUBSTITUTION (max_edge_weight, get(weight, e));
  }

  all_reduce<Dist, maximum<Dist> > r(transport, maximum<Dist>());
  max_edge_weight = r(max_edge_weight);

  // If delta wasn't supplied in the ctor initialize it
  if (delta == 0) {

    // Compute the maximum edge degree
    Degree max_degree = 0;
    BGL_FORALL_VERTICES_T(u, g, Graph) {
      max_degree = max BOOST_PREVENT_MACRO_SUBSTITUTION (max_degree, out_degree(u, g));
    }
    // max_degree = all_reduce(process_group(g), max_degree, maximum<Degree>());
    all_reduce<Degree, maximum<Degree> > r(transport, maximum<Degree>());
    max_degree = r(max_degree);

    // Take a guess at delta, based on what works well for random
    // graphs.
    delta = max_edge_weight / max_degree;
    if (delta == 0)
      delta = 1;
  }

  //
  // Initialize buckets data structure
  //

  // Extra bucket is so we don't try to insert into the bucket we're processing
  // when the index wraps
  num_buckets = max_edge_weight / delta;
  num_buckets += (num_buckets * delta < (BucketIndex)max_edge_weight) ? 2 : 1;

  // + 1 is for deleted_vertices bucket
  unsigned long capacity_per_bucket = (DEFAULT_HEAP_SIZE / (num_buckets + 1));

  // Declare bucket data structure and index variable
  buckets.resize(num_buckets);
  for (BucketIndex i = 0 ; i < buckets.size() ; ++i) {
    //    shared_ptr<Bucket> p(new Bucket(nthreads));
    //buckets[i].swap(p);
    buckets[i].reset(new Bucket(nthreads, capacity_per_bucket));
  }

  //shared_ptr<Bucket> p(new Bucket(nthreads));
  //deleted_vertices.swap(p);
  deleted_vertices.reset(new Bucket(nthreads, capacity_per_bucket));

  // Initialize distance labels
  BGL_FORALL_VERTICES_T(v, g, Graph) {
    put(distance, v, (std::numeric_limits<Dist>::max)());
  }
}

template<DELTA_STEPPING_SHORTEST_PATHS_NUMA_PARMS>
void
DELTA_STEPPING_SHORTEST_PATHS_NUMA_TYPE::allocate_pqs() {
  for (BucketIndex i = 0 ; i < buckets.size() ; ++i) {
    buckets[i]->allocate_pqs();
  }

  deleted_vertices->allocate_pqs();
}

template<DELTA_STEPPING_SHORTEST_PATHS_NUMA_PARMS>
void
DELTA_STEPPING_SHORTEST_PATHS_NUMA_TYPE::initialize_threaded_buckets(int tid) {
  for (BucketIndex i = 0 ; i < buckets.size() ; ++i) {
    buckets[i]->initialize(tid, false);
  }

  deleted_vertices->initialize(tid, false);
}

template<DELTA_STEPPING_SHORTEST_PATHS_NUMA_PARMS>
void
DELTA_STEPPING_SHORTEST_PATHS_NUMA_TYPE::run(Vertex s, int tid) {
  /**
   *  NOTE: We never remove a vertex from a higher bucket when
   *  placing it in a lower one because thread-safe insertion is
   *  cheap as long as we don't have to handle removal
   *  simultaneously, this will result in additional work and higher
   *  memory consumption, but the alternative is locking which is
   *  sure to be slow.
   */
  int doFlushCounter = 0;
  int count_epoch = 0 ;
  AMPLUSPLUS_WITH_THREAD_ID(tid) {
    int nthreads = transport.get_nthreads();

    if (tid == 0)
      t_bar.reset(new amplusplus::detail::barrier(nthreads));

    // This barrier acts as a temporary barrier until we can be sure t_bar is initialized
    { amplusplus::scoped_epoch epoch(transport); }

    // wait till all threads are pinned
    t_bar->wait();

    // if two processes are running on the same node, core_offset
    // is important to achieve thread affinity
    if (pin(tid+core_offset) != 0) {
      std::cerr << "[ERROR] Unable to pin current thread to "
		<< "core : " << tid << std::endl;
      assert(false);
    }

    // wait till all threads are pinned
    t_bar->wait();
    { amplusplus::scoped_epoch epoch(transport); }

    validate_thread_core_relation();

#ifdef LIB_CDS
    // Attach thread to CDS; main thread is already attached
    if (tid != 0)
      cds::threading::Manager::attachThread();
#endif

    // For each thread initialize buckets
    initialize_threaded_buckets(tid);
    t_bar->wait(); // wait till all threads initialize buckets

    // if thread id is 0
    // go through all the buckets
    // and allocate priority queues
    // cos we need to new them in the main thread in cds
    //if (tid == 0) {
    //  allocate_pqs();
    //}

    //t_bar->wait(); // wait till tid=0 allocate priority queues
    start_time = get_time();

    // Push the source onto the queue
    {
      amplusplus::scoped_epoch epoch(transport);

      if (get(owner, s) == transport.rank() && tid == 0)
        relax(vertex_distance_data(s, 0));
    }

    t_bar->wait();

    while(current_bucket != (std::numeric_limits<BucketIndex>::max)()) { // do for all buckets

      //std::cout << transport.rank()
      //	<< " processing bucket " << current_bucket << std::endl;

      unsigned long all_process_bucket_empty = 1;
      unsigned long p_bucket_full = 0;
      count_epoch = 0;
      // process current bucket
      while(all_process_bucket_empty != 0) {

	// wait till all threads reach here
	// before checking whether queue is empty we need to make sure
	// none of the threads are pushing elements to the queue.
	// Therefore make sure all threads reach here before checking whether queue is
	// empty
	t_bar->wait();

	if (!buckets[current_bucket]->empty(tid))
	  p_bucket_full = 1;
	else
	  p_bucket_full = 0;

	//t_bar->wait(); // TODO Not sure whether we really need this

	{
	  //	  std::cout << "p_bucket_not_empty : " << p_bucket_not_empty << std::endl;
	  amplusplus::scoped_epoch_value epoch(transport,
					       p_bucket_full,
					       all_process_bucket_empty);

 	  //std::cout << transport.rank()
	  //    << " all_process_bucket_empty : "
	  //    << all_process_bucket_empty << std::endl;

	  vertex_distance_data vd;
	  while(buckets[current_bucket]->pop(vd, tid)) { // extract an item from the current bucket

	    assert(all_process_bucket_empty != 0);
	    // process light edges
#ifdef PRINT_DEBUG
	    if (tid == 0)
	      std::cerr << transport.rank() << ": processing light edges in bucket "
			<< current_bucket << std::endl;
#endif
	    Vertex v = vd.first;
	    Dist dv = get(distance, v);

	    // v is updated with a better distance ! we do not need to relax
	    if (dv < delta * (Dist)current_level) {
	      continue;
            }

	    // Add v to set of deleted vertices
	    deleted_vertices->put(vd, tid);

	    // Relax light edges
	    BGL_FORALL_OUTEDGES_T(v, e, g, Graph) {
	      Vertex u = target(e, g);
	      Dist we = get(weight, e);
	      if (we <= delta) {// light edge
		relax_msg.send(vertex_distance_data(u, dv + we));
#ifdef PRINT_DEBUG
		std::cerr << "  Relax " << get(get(vertex_local, g), v) << "@"
			  << get(get(vertex_owner, g), v) << "->"
			  << get(get(vertex_local, g), u) << "@"
			  << get(get(vertex_owner, g), u) << "  weight = "
			  << we << std::endl;
#endif
	      }
	    } // end of BGL for

    	    doFlushCounter++;
	    if(doFlushCounter == flushFrequency) {
	      doFlushCounter = 0;
	      transport.get_scheduler().run_one();
	    }
	  } // end of while (bucket)
	  // TODO need to add if (level_sync) break; code; not sure still valid
	} // end of scoped epoch
	count_epoch += 1;
      } // all processors should be done with current bucket

      assert(buckets[current_bucket]->empty(tid));

      // If all processes are done with the current bucket:
      //   1. clear the current bucket
      //   2. process heavy edges
      //   3. find the next bucket to work on
      buckets[current_bucket]->clear(tid);
      t_bar->wait();

      // Process heavy edges
      // TODO: If we had a separate transport here we could do single-level termination detection
      {
	amplusplus::scoped_epoch epoch(transport);

	vertex_distance_data deleted_vd;
	while(deleted_vertices->pop(deleted_vd, tid)) {
	  Vertex v = deleted_vd.first;
	  Dist dv = get(distance, v);
	  BGL_FORALL_OUTEDGES_T(v, e, g, Graph) {
	    Vertex u = target(e, g);
	    Dist we = get(weight, e);
	    if (we > delta) // heavy edge
	      relax_msg.send(vertex_distance_data(u, dv + we));
	  } // end BGL for

	  doFlushCounter++;
	  if(doFlushCounter == flushFrequency) {
	    doFlushCounter = 0;
	    transport.get_scheduler().run_one();
	  }

	} // end while
      } // end_epoch does a thread barrier

#ifdef PRINT_DEBUG
      std::cerr << tid << "@" << transport.rank() << ": Current bucket " << current_bucket
		<< " size " << buckets[current_bucket]->size() << std::endl;
#endif
      assert(buckets[current_bucket]->size(tid) == 0);

      deleted_vertices->clear(tid);

      // find next bucket with work and update current_level
      BucketIndex old_bucket = current_bucket;

      t_bar->wait();
      if (tid == 0) {
	find_next_bucket();

	if (current_bucket != (std::numeric_limits<BucketIndex>::max)()) {
	  current_level += current_bucket - old_bucket;
	  ++buckets_processed;
	}
      }
      t_bar->wait();
    } // end of while global bucket check

#ifdef LIB_CDS
    if (tid != 0)
      cds::threading::Manager::detachThread();
#endif

  } // end of AMPP THREAD ID
} //end of run

template<DELTA_STEPPING_SHORTEST_PATHS_NUMA_PARMS>
void
DELTA_STEPPING_SHORTEST_PATHS_NUMA_TYPE::relax(const vertex_distance_data& vd) {
  Vertex v = vd.first;
  Dist d = vd.second;
  using boost::parallel::val_compare_and_swap;

  int tid = amplusplus::detail::get_thread_id();
  Dist old_dist = get(distance, v), last_old_dist;
  while (d < old_dist) {
    last_old_dist = old_dist;
    old_dist = val_compare_and_swap(&distance[v], old_dist, d);
    if (last_old_dist == old_dist) {
#ifdef PBGL2_PRINT_WORK_STATS
      if(old_dist < std::numeric_limits<Dist>::max()) {
	work_stats.increment_invalidated(tid);
      } else {
	work_stats.increment_useful(tid);
      }
#endif
      // Insert vertex into new bucket, note we don't remove it from any other
      // buckets it might be in
      BucketIndex new_index =
        static_cast<BucketIndex>((d - (current_level * delta)) / delta);

      //
      // If new_index == 0, relax all light edges and add it to deleted_vertices
      //
      if (new_index == 0) { // This vertex is bound for the current bucket
	BGL_FORALL_OUTEDGES_T(v, e, g, Graph) {
	  Vertex u = target(e, g);
	  Dist we = get(weight, e);
	  if (we <= delta) // light edge
	    relax_msg.send(vertex_distance_data(u, d + we));
	}

	deleted_vertices->put(vd, tid);

        break; // No need to insert into current bucket now
      }

      new_index = (current_bucket + new_index) % num_buckets;
      buckets[new_index]->put(vd, tid);

      // new_distance now == get(distance, v) so we exit automatically
      // but this saves us the conditional test
      break;
    }
  }

#ifdef PBGL2_PRINT_WORK_STATS
  work_stats.increment_rejected(tid);
#endif

  return;
}

template<DELTA_STEPPING_SHORTEST_PATHS_NUMA_PARMS>
void
DELTA_STEPPING_SHORTEST_PATHS_NUMA_TYPE::find_next_bucket()
{
  using boost::parallel::all_reduce;
  using boost::parallel::minimum;

  BucketIndex old_bucket = current_bucket;
  BucketIndex max_bucket = (std::numeric_limits<BucketIndex>::max)();

  current_bucket = (current_bucket + 1) % buckets.size();
  while (current_bucket != old_bucket && buckets[current_bucket]->empty())
    current_bucket = (current_bucket + 1) % buckets.size();

  if (current_bucket == old_bucket)
    current_bucket = max_bucket;

  // If we wrapped, project index past end of buckets to use min()
  if (current_bucket < old_bucket) current_bucket += buckets.size();

  all_reduce<BucketIndex, minimum<BucketIndex> > r(transport, minimum<BucketIndex>());
  current_bucket = r(current_bucket);

  // Map index back into range of buckets
  if (current_bucket != max_bucket)
    current_bucket %= buckets.size();
}

template<DELTA_STEPPING_SHORTEST_PATHS_NUMA_PARMS>
struct DELTA_STEPPING_SHORTEST_PATHS_NUMA_TYPE::
vertex_distance_handler {

  vertex_distance_handler() : self(NULL) {}
  vertex_distance_handler(delta_stepping_shortest_paths_numa& self) : self(&self) {}

  void operator() (const vertex_distance_data& data) const {

    self->work_stats.increment_edges(amplusplus::detail::get_thread_id());

    if (data.second < get(self->distance, data.first))
      self->relax(data);
#ifdef PBGL2_PRINT_WORK_STATS
    else
      self->work_stats.increment_rejected(amplusplus::detail::get_thread_id());
#endif
  }

protected:
  delta_stepping_shortest_paths_numa* self;
};

} } } // end namespace boost::graph::distributed

#endif // BOOST_GRAPH_DELTA_STEPPING_SHORTEST_PATHS_HPP