/* -*- linux-c -*- */
/* fewbody_utils.c

   Copyright (C) 2002-2004 John M. Fregeau

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2 of the License, or
   (at your option) any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_roots.h>
#include "fewbody.h"

/* allocate a vector */
double *fb_malloc_vector(int n)
{
	return((double *) malloc(n * sizeof(double)));
}

/* allocate a matrix */
double **fb_malloc_matrix(int nr, int nc)
{
	int i;
	double **m;

	m = (double **) malloc(nr * sizeof(double *));

	m[0] = (double *) malloc(nr * nc * sizeof(double));

	for (i=1; i<nr; i++) {
		m[i] = m[i-1] + nc;
	}

	return(m);
}

/* free a vector */
void fb_free_vector(double *v)
{
	free(v);
}

/* free a matrix */
void fb_free_matrix(double **m)
{
	free(m[0]);
	free(m);
}

/* a fast square function */
double fb_sqr(double x)
{
	return(x*x);
}

/* a fast cube function */
double fb_cub(double x)
{
	return(x*x*x);
}

/* the dot product of two vectors */
double fb_dot(double x[3], double y[3])
{
	return(x[0] * y[0] + x[1] * y[1] + x[2] * y[2]);
}

/* the modulus of a vector */
double fb_mod(double x[3])
{
	return(sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2]));
}

/* the cross product of two vectors */
int fb_cross(double x[3], double y[3], double z[3])
{
	z[0] = x[1] * y[2] - x[2] * y[1];
	z[1] = x[2] * y[0] - x[0] * y[2];
	z[2] = x[0] * y[1] - x[1] * y[0];

	return(0);
}

/* something to calculate the angular momentum */
int fb_angmom(fb_obj_t *star, int nstar, double L[3])
{
	int i, k;
	double **l;

	l = fb_malloc_matrix(nstar, 3);

	for (k=0; k<3; k++) {
		L[k] = 0.0;
	}

	for (i=0; i<nstar; i++) {
		fb_cross(star[i].x, star[i].v, l[i]);
		for (k=0; k<3; k++) {
			L[k] += star[i].m * l[i][k];
		}
	}

	fb_free_matrix(l);

	return(0);
}

/* something to calculate the internal angular momentum */
void fb_angmomint(fb_obj_t *star, int nstar, double L[3])
{
	int i, k;

	for (k=0; k<3; k++) {
		L[k] = 0.0;
	}

	for (i=0; i<nstar; i++) {
		for (k=0; k<3; k++) {
			L[k] += star[i].Lint[k];
		}
	}
}

/* something to calculate the internal energy */
double fb_einttot(fb_obj_t *star, int nstar)
{
	int i;
	double eint=0.0;

	for (i=0; i<nstar; i++) {
		eint += star[i].Eint;
	}

	return(eint);
}

/* calculates the total potential energy of the system */
double fb_petot(fb_obj_t *star, int nstar)
{
	int i, j, k;
	double pe=0.0, r[3];

	for (i=0; i<nstar; i++) {
		for (j=i+1; j<nstar; j++) {
			for (k=0; k<3; k++) {
				r[k] = star[j].x[k] - star[i].x[k];
			}
			pe += -star[i].m * star[j].m / fb_mod(r);
		}
	}

	return(pe);
}

/* calculates the total kinetic energy of the system */
double fb_ketot(fb_obj_t *star, int nstar)
{
	int i;
	double ke=0.0;

	for (i=0; i<nstar; i++) {
		ke += 0.5 * star[i].m * fb_dot(star[i].v, star[i].v);
	}

	return(ke);
}

/* calculates the potential energy of the bound members of the system */
double fb_outerpetot(fb_obj_t **obj, int nobj)
{
	int i, j, k;
	double pe=0.0, r[3];

	for (i=0; i<nobj; i++) {
		for (j=i+1; j<nobj; j++) {
			for (k=0; k<3; k++) {
				r[k] = obj[j]->x[k] - obj[i]->x[k];
			}
			pe += -obj[i]->m * obj[j]->m / fb_mod(r);
		}
	}

	return(pe);
}

/* calculates the kinetic energy of the bound members of the system */
double fb_outerketot(fb_obj_t **obj, int nobj)
{
	int i;
	double ke=0.0;

	for (i=0; i<nobj; i++) {
		ke += 0.5 * obj[i]->m * fb_dot(obj[i]->v, obj[i]->v);
	}

	return(ke);
}

/* solve the Kepler equation for the eccentric anomaly, given the mean anomaly and eccentricity */
double fb_kepler(double e, double mean_anom)
{
	int status, iter;
	double ecc_anom, params[2];
	gsl_function F;
	const gsl_root_fsolver_type *T;
	gsl_root_fsolver *s;

	/* set up the root solver */
	F.function = &fb_keplerfunc;
	F.params = &params;

	/* set the parameters */
	params[0] = e;
	params[1] = mean_anom;

	T = gsl_root_fsolver_brent;
	s = gsl_root_fsolver_alloc(T);
	gsl_root_fsolver_set(s, &F, 0.0, 2.0*FB_CONST_PI);

	/* get eccentric anomaly by root-finding */
	iter = 0;
	do {
		iter++;
		gsl_root_fsolver_iterate(s);
		status = gsl_root_test_interval(gsl_root_fsolver_x_lower(s), gsl_root_fsolver_x_upper(s), \
						FB_ROOTSOLVER_ABS_ACC, FB_ROOTSOLVER_REL_ACC);
	} while (status == GSL_CONTINUE && iter < FB_ROOTSOLVER_MAX_ITER);

	if (iter >= FB_ROOTSOLVER_MAX_ITER) {
		fprintf(stderr, "Root finder failed to converge.\n");
		exit(1);
	}

	/* we've got the root */
	ecc_anom = gsl_root_fsolver_root(s);

	/* free memory associated with root solver */
	gsl_root_fsolver_free(s);

	return(ecc_anom);
}

/* the Kepler function for the root finder */
double fb_keplerfunc(double ecc_anom, void *params)
{
	double e, mean_anom;

	e = ((double *)params)[0];
	mean_anom = ((double *)params)[1];

	return(ecc_anom - e * sin(ecc_anom) - mean_anom);
}

/* calculate the relative tidal acceleration */
double fb_reltide(fb_obj_t *bin, fb_obj_t *single, double r)
{
	double arel, atid;

	arel = bin->m / fb_sqr(bin->a * (1.0 + bin->e));
	/* Factor in numerator is (single->m + bin->m) instead of single->m for the case of
	   small single->m, for which we want to at some point resolve "bin" to get the
	   motion of "single".  This will eventually be fixed when we we use full collapsed
	   binary member positions in the integration, and not just the CM of the binary. */
	atid = 2.0 * (single->m + bin->m) / fb_cub(r) * bin->a * (1.0 + bin->e);

	return(atid/arel);
}


/*
 * Copyright (c) 2016-2021 lymastee, All rights reserved.
 * Contact: lymastee@hotmail.com
 *
 * This file is part of the gslib project.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#pragma once

#ifndef linequ_546582d4_5f60_4938_9ce8_1beb1678ba61_h
#define linequ_546582d4_5f60_4938_9ce8_1beb1678ba61_h

#include <gslib/type.h>
#include <gslib/std.h>
#include <gslib/error.h>

__gslib_begin__

class _trait_dimcol {};
class _trait_dimrow {};

template<class _fty, class _mty, class _trdim>
class ledimsel
{
public:
    typedef _fty type;
    typedef _mty matrix;
    typedef ledimsel<_fty, _mty, _trdim> selection;

public:
    ledimsel(matrix& mat, int cr): _mat(mat), _sel(cr) {}
    int index() const { return _sel; }
    int capacity() const { return capacity<_trdim>(); }
    int cols() const { return _mat.cols(); }
    int rows() const { return _mat.rows(); }
    type& get(int i) { return get<_trdim>(i); }
    const type& get_const(int i) const { return get_const<_trdim>(i); }
    void set(int i, const type& t) { get(i) = t; }

protected:
    template<class _trdim>
    int capacity() const;
    template<class _trdim>
    type& get(int i);
    template<class _trdim>
    const type& get_const(int i) const;
    template<>
    int capacity<_trait_dimcol>() const { return rows(); }
    template<>
    int capacity<_trait_dimrow>() const { return cols(); }
    template<>
    type& get<_trait_dimcol>(int i) { return _mat.get(i, _sel); }
    template<>
    type& get<_trait_dimrow>(int i) { return _mat.get(_sel, i); }
    template<>
    const type& get_const<_trait_dimcol>(int i) const { return _mat.get_const(i, _sel); }
    template<>
    const type& get_const<_trait_dimrow>(int i) const { return _mat.get_const(_sel, i); }

protected:
    matrix&         _mat;
    int             _sel;

public:
    template<class _op>
    ledimsel& operator*=(_op t)
    {
        for_each([&t](type& n) { n *= t; });
        return *this;
    }
    template<class _op>
    ledimsel& operator/=(_op t) { return operator*=(t); }
    template<class _opset>
    ledimsel& operator+=(const _opset& rhs)
    {
        assert(capacity() == rhs.capacity());
        int cap = capacity();
        for(int i = 0; i < cap; i ++)
            get(i) += rhs.get_const(i);
        return *this;
    }
    template<class _opset>
    ledimsel& operator-=(const _opset& rhs)
    {
        assert(capacity() == rhs.capacity());
        int cap = capacity();
        for(int i = 0; i < cap; i ++)
            get(i) -= rhs.get_const(i);
        return *this;
    }

public:
    template<class _lambda>
    void for_each(_lambda lam)
    {
        int cap = capacity();
        for(int i = 0; i < cap; i ++)
            lam(get(i));
    }
    template<class _lambda>
    void const_for_each(_lambda lam) const
    {
        int cap = capacity();
        for(int i = 0; i < cap; i ++)
            lam(get_const(i));
    }
};

/* optimize for row selector */
template<class _fty, class _mty>
class ledimsel<_fty, _mty, _trait_dimrow>
{
public:
    typedef _fty type;
    typedef _mty matrix;
    typedef ledimsel<_fty, _mty, _trait_dimrow> selection;

public:
    ledimsel(matrix& mat, int cr): _mat(mat), _sel(cr), _data(mat.data() + mat.accpos(cr, 0)) {}
    int index() const { return _sel; }
    int capacity() const { return cols(); }
    int cols() const { return _mat.cols(); }
    int rows() const { return _mat.rows(); }
    type& get(int i) { return _data[i]; }
    const type& get_const(int i) const { return _data[i]; }
    void set(int i, const type& t) { get(i) = t; }

protected:
    matrix&         _mat;
    int             _sel;
    type*           _data;

public:
    template<class _op>
    ledimsel& operator*=(_op t)
    {
        for_each([&t](type& n) { n *= t; });
        return *this;
    }
    template<class _op>
    ledimsel& operator/=(_op t) { return operator*=(t); }
    template<class _opset>
    ledimsel& operator+=(const _opset& rhs)
    {
        assert(capacity() == rhs.capacity());
        int cap = capacity();
        for(int i = 0; i < cap; i ++)
            get(i) += rhs.get_const(i);
        return *this;
    }
    template<class _opset>
    ledimsel& operator-=(const _opset& rhs)
    {
        assert(capacity() == rhs.capacity());
        int cap = capacity();
        for(int i = 0; i < cap; i ++)
            get(i) -= rhs.get_const(i);
        return *this;
    }

public:
    template<class _lambda>
    void for_each(_lambda lam)
    {
        int cap = capacity();
        for(int i = 0; i < cap; i ++)
            lam(get(i));
    }
    template<class _lambda>
    void const_for_each(_lambda lam) const
    {
        int cap = capacity();
        for(int i = 0; i < cap; i ++)
            lam(get_const(i));
    }
};

template<class _opset1, class _opset2>
static void swap_opset(_opset1& ops1, _opset2& ops2)
{
    assert(ops1.capacity() == ops2.capacity());
    int cap = ops1.capacity();
    for(int i = 0; i < cap; i ++)
        std::swap(ops1.get(i), ops2.get(i));
}

template<class _opset>
static int first_non_zero(const _opset& ops)
{
    int cap = ops.capacity();
    for(int i = 0; i < cap; i ++) {
        if(ops.get_const(i) != 0)
            return i;
    }
    return cap;
}

template<class _opset>
static int last_non_zero(const _opset& ops)
{
    for(int i = ops.capacity() - 1; i >= 0; i --) {
        if(ops.get_const(i) != 0)
            return i;
    }
    return ops.capacity();
}

template<class _fty>
class levector
{
public:
    typedef _fty type;
    typedef levector<_fty> vector;

protected:
    int             _size;
    type*           _data;

public:
    levector() { _data = 0, _size = 0; }
    levector(int s)
    {
        assert(s > 0);
        _data = 0;
        setup(s);
    }
    ~levector()
    {
        if(_data) {
            delete [] _data;
            _data = 0;
        }
    }
    void setup(int s)
    {
        if(_data != 0)
            delete [] _data;
        _size = s;
        _data = new type[s];
    }
    int capacity() const { return _size; }
    type& get(int i) { return _data[i]; }
    const type& get_const(int i) const { return _data[i]; }
    type* data() const { return _data; }
    void set(int i, const type& t) { get(i) = t; }

public:
    template<class _lambda>
    void for_each(_lambda lam)
    {
        int cap = capacity();
        for(int i = 0; i < cap; i ++)
            lam(get(i));
    }
    template<class _lambda>
    void const_for_each(_lambda lam) const
    {
        int cap = capacity();
        for(int i = 0; i < cap; i ++)
            lam(get_const(i));
    }
};

template<class _fty>
class lematrix
{
public:
    typedef _fty type;
    typedef lematrix<_fty> matrix;
    typedef levector<_fty> vector;
    typedef ledimsel<_fty, lematrix, _trait_dimrow> rowselector;
    typedef ledimsel<_fty, const lematrix, _trait_dimrow> const_rowselector;
    typedef ledimsel<_fty, lematrix, _trait_dimcol> colselector;
    typedef ledimsel<_fty, const lematrix, _trait_dimcol> const_colselector;

protected:
    int             _rows;
    int             _cols;
    type*           _data;

public:
    lematrix()
    {
        _rows = _cols = 0;
        _data = 0;
    }
    lematrix(int r, int c)
    {
        assert(r > 0 && c > 0);
        _data = 0;
        set_dim(r, c);
    }
    ~lematrix()
    {
        if(_data != 0) {
            delete [] _data;
            _data = 0;
        }
    }
    void set_dim(int r, int c)
    {
        if(_data != 0)
            delete [] _data;
        assert(r > 0 && c > 0);
        _rows = r;
        _cols = c;
        _data = new type[r * c];
    }
    int accpos(int r, int c) const
    {
        assert(r >= 0 && r < _rows && c >= 0 && c < _cols);
        int p = r * _cols + c;
        return p;
    }
    type& get(int r, int c)
    {
        assert(r >= 0 && r < _rows && c >= 0 && c < _cols);
        int p = accpos(r, c);
        return _data[p];
    }
    const type& get_const(int r, int c) const
    {
        assert(r >= 0 && r < _rows && c >= 0 && c < _cols);
        int p = accpos(r, c);
        return _data[p];
    }
    int dump_row(vector& v, int r)
    {
        int s = cols();
        v.setup(s);
        memcpy_s(v.data(), sizeof(type) * s, &get(r, 0), sizeof(type) * s);
        return s;
    }
    int dump_col(vector& v, int c)
    {
        int s = rows();
        v.setup(s);
        auto& sel = select_col(c);
        assert(s == sel.capacity());
        for(int i = 0; i < s; i ++)
            v.set(i, sel.get_const(i));
        return s;
    }
    bool gauss_elim()
    {
        assert(rows() >= 2);
        int cap = rows();
        arrange(0, cap);
        for(int i = 1; i < cap; i ++)
            gauss_elim(i);
        return row_rank() <= col_rank();
    }

public:
    int rows() const { return _rows; }
    int cols() const { return _cols; }
    type* data() const { return _data; }
    colselector select_col(int c) { return colselector(*this, c); }
    const_colselector select_col(int c) const { return const_colselector(*this, c); }
    rowselector select_row(int r) { return rowselector(*this, r); }
    const_rowselector select_row(int r) const { return const_rowselector(*this, r); }
    bool fuzzy_solvable() const { return rows() >= cols() - 1; }
    int row_rank() const { return cols() - first_non_zero(select_row(0)) - 1; }
    int col_rank() const { return last_non_zero(select_col(cols() - 1)) + 1; }

protected:
    void arrange(int start, int end)
    {
        assert(start >= 0 && end <= rows());
        if(end - start <= 1)
            return;
        struct node
        {
            int index, first;
            node(int i, int f): index(i), first(f) {}
        };
        list<node> stlist;
        for(int i = start; i < end; i ++)
            stlist.push_back(node(i, first_non_zero(select_row(i))));
        stlist.sort([](const node& n1, const node& n2)->bool { return n1.first < n2.first; });
        assert(!stlist.empty());
        matrix mt;
        mt.set_dim(end - start, cols());
        auto j = stlist.begin();
        for(int i = 0; j != stlist.end(); ++ i, ++ j) {
            auto& r = mt.select_row(i);
            memcpy_s(&r.get(0), sizeof(type)*cols(), &get_const(j->index, 0), sizeof(type)*cols());
        }
        int cpsize = sizeof(type) * cols() * (end - start);
        memcpy_s(&get(start, 0), cpsize, mt.data(), cpsize);
    }
    void gauss_elim(int i)
    {
        assert(i > 0 && i < rows());
        auto& select1 = select_row(i - 1);
        auto& select2 = select_row(i);
        int first1 = first_non_zero(select1);
        if(select2.get(first1) == 0)
            return;
        int cap = rows();
        for(int j = i; j < cap; j ++) {
            if(!gauss_elim(i - 1, j, first1))
                break;
        }
        arrange(i, cap);
    }
    bool gauss_elim(int i, int j, int first)
    {
        assert(i < j);
        auto& select1 = select_row(i);
        auto& select2 = select_row(j);
        if(select2.get(first) == 0)
            return false;
        select2 *= (select1.get_const(first) / select2.get_const(first));
        select2 -= select1;
        return true;
    }

protected:
    void trace_data() const
    {
        for(int i = 0; i < rows(); i ++) {
            auto& sel = select_row(i);
            for(int j = 0; j < sel.capacity(); j ++)
                trace(_t("%lf    "), sel.get_const(j));
            trace(_t("\r"));
        }
    }
};

template<class _fty>
class linequ
{
public:
    typedef _fty type;
    typedef linequ<_fty> myref;
    typedef levector<_fty> vector;
    typedef lematrix<_fty> matrix;
    static double qnan()
    {
        static const struct nan_struct { union { uint a[2]; double b; }; } qnan_struct = { 0xffffffff, 0x7fffffff };
        return qnan_struct.b;
    }

public:
    void set_variable_count(int c) { _variables.setup(c); }
    int get_variable_count() const { return _variables.capacity(); }
    void set_equation_count(int c) { _matrix.set_dim(c, get_variable_count() + 1); }
    int get_equation_count() const { return _matrix.rows(); }
    const vector& get_results() const { return _variables; }
    type get_variable(int i) const { return _variables.get_const(i); }
    void set_equation(int i, ...)
    {
        va_list ptr;
        va_start(ptr, i);
        auto& selection = _matrix.select_row(i);
        int cap = _matrix.cols();
        for(int j = 0; j < cap; j ++)
            selection.set(j, va_arg(ptr, type));
    }
    void set_equation(int i, const type d[], int s)
    {
        auto& selection = _matrix.select_row(i);
        if(s > selection.capacity())
            s = selection.capacity();
        for(int j = 0; j < s; j ++)
            selection.set(j, d[j]);
    }
    int solve()
    {
        if(!_matrix.fuzzy_solvable() || !_matrix.gauss_elim())
            return 0;
        set_unsolved<_fty>();
        int rkcol = _matrix.col_rank();
        int c = 0;
        for(int i = rkcol - 1; i >= 0; i --, c ++) {
            if(!solve(i))
                break;
        }
        return c;
    }

protected:
    vector          _variables;
    matrix          _matrix;

protected:
    template<class _fty>
    void set_unsolved();
    template<>
    void set_unsolved<double>() { _variables.for_each([](type& v) { v = qnan(); }); }
    template<>
    void set_unsolved<float>()
    {
        float fqnan = static_cast<float>(qnan());
        assert(sizeof(fqnan) == sizeof(int));
        memset(_variables.data(), *(int*)&fqnan, _variables.capacity());
    }
    bool solve(int c)
    {
        auto& selection = _matrix.select_row(c);
        int first = first_non_zero(selection);
        for(int i = get_variable_count() - 1; i > c; i --) {
            if((((type)qnan())) == get_variable(i))
                return false;
        }
        int cap = get_variable_count();
        type s = selection.get_const(cap);
        for(int i = c + 1; i < cap; i ++)
            s += (selection.get_const(i) * get_variable(i));
        _variables.set(c, -s / selection.get_const(c));
        return true;
    }
};

__gslib_end__

#endif


#include <config.h>
#include <stddef.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_complex.h>

#include <gsl/gsl_fft_complex.h>
#include <gsl/gsl_fft_complex_float.h>

#define BASE_DOUBLE
#include "templates_on.h"
#include "bitreverse.c"
#include "templates_off.h"
#undef  BASE_DOUBLE

#define BASE_FLOAT
#include "templates_on.h"
#include "bitreverse.c"
#include "templates_off.h"
#undef  BASE_FLOAT

#include "factorize.c"

#define BASE_DOUBLE
#include "templates_on.h"
#include "c_init.c"
#include "c_main.c"
#include "c_pass_2.c"
#include "c_pass_3.c"
#include "c_pass_4.c"
#include "c_pass_5.c"
#include "c_pass_6.c"
#include "c_pass_7.c"
#include "c_pass_n.c"
#include "c_radix2.c"
#include "templates_off.h"
#undef  BASE_DOUBLE

#define BASE_FLOAT
#include "templates_on.h"
#include "c_init.c"
#include "c_main.c"
#include "c_pass_2.c"
#include "c_pass_3.c"
#include "c_pass_4.c"
#include "c_pass_5.c"
#include "c_pass_6.c"
#include "c_pass_7.c"
#include "c_pass_n.c"
#include "c_radix2.c"
#include "templates_off.h"
#undef  BASE_FLOAT

#include <gsl/gsl_fft_halfcomplex.h>
#include <gsl/gsl_fft_halfcomplex_float.h>

#define BASE_DOUBLE
#include "templates_on.h"
#include "hc_init.c"
#include "hc_main.c"
#include "hc_pass_2.c"
#include "hc_pass_3.c"
#include "hc_pass_4.c"
#include "hc_pass_5.c"
#include "hc_pass_n.c"
#include "hc_radix2.c"
#include "hc_unpack.c"
#include "templates_off.h"
#undef  BASE_DOUBLE

#define BASE_FLOAT
#include "templates_on.h"
#include "hc_init.c"
#include "hc_main.c"
#include "hc_pass_2.c"
#include "hc_pass_3.c"
#include "hc_pass_4.c"
#include "hc_pass_5.c"
#include "hc_pass_n.c"
#include "hc_radix2.c"
#include "hc_unpack.c"
#include "templates_off.h"
#undef  BASE_FLOAT

#include <gsl/gsl_fft_real.h>
#include <gsl/gsl_fft_real_float.h>

#define BASE_DOUBLE
#include "templates_on.h"
#include "real_init.c"
#include "real_main.c"
#include "real_pass_2.c"
#include "real_pass_3.c"
#include "real_pass_4.c"
#include "real_pass_5.c"
#include "real_pass_n.c"
#include "real_radix2.c"
#include "real_unpack.c"
#include "templates_off.h"
#undef  BASE_DOUBLE

#define BASE_FLOAT
#include "templates_on.h"
#include "real_init.c"
#include "real_main.c"
#include "real_pass_2.c"
#include "real_pass_3.c"
#include "real_pass_4.c"
#include "real_pass_5.c"
#include "real_pass_n.c"
#include "real_radix2.c"
#include "real_unpack.c"
#include "templates_off.h"
#undef  BASE_FLOAT


#ifndef __MMA__
#define __MMA__

#include <petsc.h>

/* -----------------------------------------------------------------------------
Authors: Niels Aage
 Copyright (C) 2013-2020,
This MMA implementation is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This Module is distributed in the hope that it will be useful,implementation
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this Module; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
-------------------------------------------------------------------------- */
class MMA {
  public:
    // Construct using defaults subproblem penalization
    MMA(PetscInt n, PetscInt m, Vec x);
    // User defined subproblem penalization
    MMA(PetscInt n, PetscInt m, Vec x, PetscScalar* a, PetscScalar* c, PetscScalar* d);
    // Initialize with restart from itr
    MMA(PetscInt n, PetscInt m, PetscInt itr, Vec xo1, Vec xo2, Vec U, Vec L);
    // Initialize with restart and specify subproblem parameters
    MMA(PetscInt n, PetscInt m, PetscInt itr, Vec xo1, Vec xo2, Vec U, Vec L, PetscScalar* a, PetscScalar* c,
        PetscScalar* d);
    // Destructor
    ~MMA();

    // Set and solve a subproblem: return new xval
    PetscErrorCode Update(Vec xval, Vec dfdx, PetscScalar* gx, Vec* dgdx, Vec xmin, Vec xmax);

    // Return necessary data for possible restart
    PetscErrorCode Restart(Vec xo1, Vec xo2, Vec U, Vec L);

    // Set the aggresivity of the moving asymptotes
    PetscErrorCode SetAsymptotes(PetscScalar init, PetscScalar decrease, PetscScalar increase);

    // do/don't add convexity approx to constraints: default=false
    PetscErrorCode ConstraintModification(PetscBool conMod) {
        constraintModification = conMod;
        return 0;
    };

    // val=0: default, val=1: increase robustness, i.e
    // control the spacing between L < alp < x < beta < U,
    PetscErrorCode SetRobustAsymptotesType(PetscInt val);

    // Sets outer movelimits on all primal design variables
    // This is often requires to prevent the solver from oscilating
    PetscErrorCode SetOuterMovelimit(PetscScalar Xmin, PetscScalar Xmax, PetscScalar movelim, Vec x, Vec xmin,
                                     Vec xmax);

    // Return KKT residual norms (norm2 and normInf)
    PetscErrorCode KKTresidual(Vec xval, Vec dfdx, PetscScalar* gx, Vec* dgdx, Vec xmin, Vec xmax, PetscScalar* norm2,
                               PetscScalar* normInf);

    // Inf norm on diff between two vectors: SHOULD NOT BE HERE - USE BASIC
    // PETSc!!!!!
    PetscScalar DesignChange(Vec x, Vec xold);

  private:
    // Set up the MMA subproblem based on old x's and xval
    PetscErrorCode GenSub(Vec xval, Vec dfdx, PetscScalar* gx, Vec* dgdx, Vec xmin, Vec xmax);

    // Interior point solver for the subproblem
    PetscErrorCode SolveDIP(Vec xval);

    // Compute primal vars based on dual solution
    PetscErrorCode XYZofLAMBDA(Vec x);

    // Dual gradient
    PetscErrorCode DualGrad(Vec x);

    // Dual Hessian
    PetscErrorCode DualHess(Vec x);

    // Dual line search
    PetscErrorCode DualLineSearch();

    // Dual residual
    PetscScalar DualResidual(Vec x, PetscScalar epsi);

    // Problem size and iteration counter
    PetscInt n, m, k;

    // "speed-control" for the asymptotes
    PetscScalar asyminit, asymdec, asyminc;

    // do/don't add convexity constraint approximation in subproblem
    PetscBool constraintModification; // default = FALSE

    // Bool specifying if non lin constraints are included or not
    PetscBool NonLinConstraints;

    // 0: (default) span between alp L x U beta,
    // 1: increase the span for further robustness
    PetscInt RobustAsymptotesType;

    // Local vectors: penalty numbers for subproblem
    PetscScalar *a, *c, *d;

    // Local vectors: elastic variables
    PetscScalar* y;
    PetscScalar  z;

    // Local vectors: Lagrange multipliers:
    PetscScalar *lam, *mu, *s;

    // Global: Asymptotes, bounds, objective approx., constraint approx.
    Vec L, U, alpha, beta, p0, q0, *pij, *qij;

    // Local: subproblem constant terms, dual gradient, dual hessian
    PetscScalar *b, *grad, *Hess;

    // Global: Old design variables
    Vec xo1, xo2;

    // Math helpers
    PetscErrorCode Factorize(PetscScalar* K, PetscInt nn);
    PetscErrorCode Solve(PetscScalar* K, PetscScalar* x, PetscInt nn);
    PetscScalar    Min(PetscScalar d1, PetscScalar d2);
    PetscScalar    Max(PetscScalar d1, PetscScalar d2);
    PetscInt       Min(PetscInt d1, PetscInt d2);
    PetscInt       Max(PetscInt d1, PetscInt d2);
    PetscScalar    Abs(PetscScalar d1);
};

#endif


/**
 * \author Sylvain Marsat, University of Maryland - NASA GSFC
 *
 * \brief C code for structures representing a waveform as a list of modes in amplitude/phase form.
 *
 */

#define _XOPEN_SOURCE 500

#ifdef __GNUC__
#define UNUSED __attribute__ ((unused))
#else
#define UNUSED
#endif

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <complex.h>
#include <time.h>
#include <unistd.h>
#include <getopt.h>
#include <stdbool.h>
#include <string.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_bspline.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_complex.h>

#include "constants.h"
#include "struct.h"

/* Call a function that references an object */
double ObjectFunctionCall(const ObjectFunction* this,double f){return this->function(this->object,f);};

/**************************************************************/
/* Functions computing the max and min between two int */
int max (int a, int b) { return a > b ? a : b; }
int min (int a, int b) { return a < b ? a : b; }

/************** GSL error handling and I/O ********************/

/* GSL error handler */
void Err_Handler(const char *reason, const char *file, int line, int gsl_errno) {
  printf("gsl: %s:%d: %s - %d\n", file, line, reason, gsl_errno);
  exit(1);
}

/* Functions to read binary data from files */
int Read_Vector(const char dir[], const char fname[], gsl_vector *v) {
  char *path=malloc(strlen(dir)+64);
  sprintf(path,"%s/%s", dir, fname);
  FILE *f = fopen(path, "rb");
  if (!f) {
    fprintf(stderr, "Error reading data from %s\n", path);
    free(path);
    return(FAILURE);
  }
  int ret = gsl_vector_fread(f, v);
  if (ret != 0) {
    fprintf(stderr, "Error reading data from %s.\n",path);
    free(path);
    return(FAILURE);
  }
  fclose(f);
  free(path);
  return(SUCCESS);
}
int Read_Matrix(const char dir[], const char fname[], gsl_matrix *m) {
  char *path=malloc(strlen(dir)+64);
  sprintf(path,"%s/%s", dir, fname);
  FILE *f = fopen(path, "rb");
  if (!f) {
    fprintf(stderr, "Error reading data from %s\n", path);
    free(path);
    return(FAILURE);
  }
  int ret = gsl_matrix_fread(f, m);
  if (ret != 0) {
    fprintf(stderr, "Error reading data from %s\n", path);
    free(path);
    return(FAILURE);
  }
  fclose(f);
  free(path);
  return(SUCCESS);
}
/* Functions to read text data from files */
int Read_Text_Vector(const char dir[], const char fname[], gsl_vector *v) {
  char *path=malloc(strlen(dir)+64);
  sprintf(path,"%s/%s", dir, fname);
  FILE *f = fopen(path, "rb");
  if (!f) {
    fprintf(stderr, "Error reading data from %s\n", path);
    free(path);
    return(FAILURE);
  }
  int ret = gsl_vector_fscanf(f, v);
  if (ret != 0) {
    fprintf(stderr, "Error reading data from %s.\n",path);
    free(path);
    return(FAILURE);
  }
  fclose(f);
  free(path);
  return(SUCCESS);
}
int Read_Text_Matrix(const char dir[], const char fname[], gsl_matrix *m) {
  char *path=malloc(strlen(dir)+64);
  sprintf(path,"%s/%s", dir, fname);
  FILE *f = fopen(path, "rb");
  if (!f) {
    fprintf(stderr, "Error reading data from %s\n", path);
    free(path);
    return(FAILURE);
  }
  int ret = gsl_matrix_fscanf(f, m);
  if (ret != 0) {
    fprintf(stderr, "Error reading data from %s.\n",path);
    free(path);
    return(FAILURE);
  }
  fclose(f);
  free(path);
  return(SUCCESS);
}
/* Functions to write data to files */
int Write_Vector(const char dir[], const char fname[], gsl_vector *v) {
  char *path=malloc(strlen(dir)+64);
  sprintf(path,"%s/%s", dir, fname);
  FILE *f = fopen(path, "w");
  if (!f) {
    fprintf(stderr, "Error writing data to %s\n", path);
    free(path);
    return(FAILURE);
  }
  int ret = gsl_vector_fwrite(f, v);
  if (ret != 0) {
    fprintf(stderr, "Error writing data to %s\n",path);
    free(path);
    return(FAILURE);
  }
  fclose(f);
  free(path);
  return(SUCCESS);
}
int Write_Matrix(const char dir[], const char fname[], gsl_matrix *m) {
  char *path=malloc(strlen(dir)+64);

  sprintf(path,"%s/%s", dir, fname);
  FILE *f = fopen(path, "w");
  if (!f) {
    fprintf(stderr, "Error writing data to %s\n", path);
    free(path);
    return(FAILURE);
  }
  int ret = gsl_matrix_fwrite(f, m);
  if (ret != 0) {
    fprintf(stderr, "Error writing data to %s\n", path);
    free(path);
    return(FAILURE);
  }
  fclose(f);
  free(path);
  return(SUCCESS);
}
/* Functions to write text data to files */
int Write_Text_Vector(const char dir[], const char fname[], gsl_vector *v) {
  char *path=malloc(strlen(dir)+64);
  sprintf(path,"%s/%s", dir, fname);
  FILE *f = fopen(path, "w");
  if (!f) {
    fprintf(stderr, "Error writing data to %s\n", path);
    free(path);
    return(FAILURE);
  }
  int ret = gsl_vector_fprintf(f, v, "%.16e");
  if (ret != 0) {
    fprintf(stderr, "Error writing data to %s\n",path);
    free(path);
    return(FAILURE);
  }
  fclose(f);
  free(path);
  return(SUCCESS);
}
int Write_Text_Matrix(const char dir[], const char fname[], gsl_matrix *m) {
  char *path=malloc(strlen(dir)+64);
  int ret = 0;

  sprintf(path,"%s/%s", dir, fname);
  FILE *f = fopen(path, "w");
  if (!f) {
    fprintf(stderr, "Error writing data to %s\n",path);
    free(path);
    return(FAILURE);
  }
  int N = (int) m->size1;
  int M = (int) m->size2;
  for(int i=0; i<N; i++){
    for(int j=0; j<M; j++){
      ret |= (fprintf(f, "%.16e ", gsl_matrix_get(m, i, j)) < 0);
    }
    if(i < N-1) ret |= (fprintf(f, "\n") < 0);
  }
  if (ret != 0) {
    fprintf(stderr, "Error writing data to %s\n",path);
    free(path);
    return(FAILURE);
  }
  fclose(f);
  free(path);
  return(SUCCESS);
}

/******** Functions to initialize and clean up CAmpPhaseFrequencySeries structure ********/
void CAmpPhaseFrequencySeries_Init(CAmpPhaseFrequencySeries **freqseries, const int n) {
  if(!freqseries) exit(1);
  /* Create storage for structures */
  if(!*freqseries) *freqseries=malloc(sizeof(CAmpPhaseFrequencySeries));
  else
  {
    CAmpPhaseFrequencySeries_Cleanup(*freqseries);
    *freqseries=malloc(sizeof(CAmpPhaseFrequencySeries));
  }
  gsl_set_error_handler(&Err_Handler);
  (*freqseries)->freq = gsl_vector_alloc(n);
  (*freqseries)->amp_real = gsl_vector_alloc(n);
  (*freqseries)->amp_imag = gsl_vector_alloc(n);
  (*freqseries)->phase = gsl_vector_alloc(n);
}
void CAmpPhaseFrequencySeries_Cleanup(CAmpPhaseFrequencySeries *freqseries) {
  if(freqseries->freq) gsl_vector_free(freqseries->freq);
  if(freqseries->amp_real) gsl_vector_free(freqseries->amp_real);
  if(freqseries->amp_imag) gsl_vector_free(freqseries->amp_imag);
  if(freqseries->phase) gsl_vector_free(freqseries->phase);
  free(freqseries);
}

/******** Functions to initialize and clean up CAmpPhaseSpline structure ********/
void CAmpPhaseSpline_Init(CAmpPhaseSpline **splines, const int n) {
  if(!splines) exit(1);
  /* Create storage for structures */
  if(!*splines) *splines=malloc(sizeof(CAmpPhaseSpline));
  else
  {
    CAmpPhaseSpline_Cleanup(*splines);
  }
  gsl_set_error_handler(&Err_Handler);
  (*splines)->spline_amp_real = gsl_matrix_alloc(n, 5);
  (*splines)->spline_amp_imag = gsl_matrix_alloc(n, 5);
  (*splines)->quadspline_phase = gsl_matrix_alloc(n, 4);
}
void CAmpPhaseSpline_Cleanup(CAmpPhaseSpline *splines) {
  if(splines->spline_amp_real) gsl_matrix_free(splines->spline_amp_real);
  if(splines->spline_amp_imag) gsl_matrix_free(splines->spline_amp_imag);
  if(splines->quadspline_phase) gsl_matrix_free(splines->quadspline_phase);
  free(splines);
}

/******** Functions to initialize and clean up CAmpPhaseGSLSpline structure ********/
void CAmpPhaseGSLSpline_Init(CAmpPhaseGSLSpline **splines, const int n) {
  if(!splines) exit(1);
  /* Create storage for structures */
  if(!*splines) *splines=malloc(sizeof(CAmpPhaseGSLSpline));
  else
  {
    CAmpPhaseGSLSpline_Cleanup(*splines);
  }
  gsl_set_error_handler(&Err_Handler);
  /* Note: for freq we won't copy the vector but simply copy the pointer to the existing one, so we don't allocate anything here */
  (*splines)->spline_amp_real = gsl_spline_alloc(gsl_interp_cspline, n);
  (*splines)->spline_amp_imag = gsl_spline_alloc(gsl_interp_cspline, n);
  (*splines)->spline_phase = gsl_spline_alloc(gsl_interp_cspline, n);
  (*splines)->accel_amp_real = gsl_interp_accel_alloc();
  (*splines)->accel_amp_imag = gsl_interp_accel_alloc();
  (*splines)->accel_phase = gsl_interp_accel_alloc();
}
void CAmpPhaseGSLSpline_Cleanup(CAmpPhaseGSLSpline *splines) {
  if(splines->spline_amp_real) gsl_spline_free(splines->spline_amp_real);
  if(splines->spline_amp_imag) gsl_spline_free(splines->spline_amp_imag);
  if(splines->spline_phase) gsl_spline_free(splines->spline_phase);
  if(splines->accel_amp_real) gsl_interp_accel_free(splines->accel_amp_real);
  if(splines->accel_amp_imag) gsl_interp_accel_free(splines->accel_amp_imag);
  if(splines->accel_phase) gsl_interp_accel_free(splines->accel_phase);
  free(splines);
}

/******** Functions to initialize and clean up ReImFrequencySeries structure ********/
void ReImFrequencySeries_Init(ReImFrequencySeries **freqseries, const int n) {
  if(!freqseries) exit(1);
  /* Create storage for structures */
  if(!*freqseries) *freqseries=malloc(sizeof(ReImFrequencySeries));
  else
  {
    ReImFrequencySeries_Cleanup(*freqseries);
  }
  gsl_set_error_handler(&Err_Handler);
  (*freqseries)->freq = gsl_vector_alloc(n);
  (*freqseries)->h_real = gsl_vector_alloc(n);
  (*freqseries)->h_imag = gsl_vector_alloc(n);
}
void ReImFrequencySeries_Cleanup(ReImFrequencySeries *freqseries) {
  if(freqseries->freq) gsl_vector_free(freqseries->freq);
  if(freqseries->h_real) gsl_vector_free(freqseries->h_real);
  if(freqseries->h_imag) gsl_vector_free(freqseries->h_imag);
  free(freqseries);
}

/******** Functions to initialize and clean up ReImTimeSeries structure ********/
void ReImTimeSeries_Init(ReImTimeSeries **timeseries, const int n) {
  if(!timeseries) exit(1);
  /* Create storage for structures */
  if(!*timeseries) *timeseries=malloc(sizeof(ReImTimeSeries));
  else
  {
    ReImTimeSeries_Cleanup(*timeseries);
  }
  gsl_set_error_handler(&Err_Handler);
  (*timeseries)->times = gsl_vector_alloc(n);
  (*timeseries)->h_real = gsl_vector_alloc(n);
  (*timeseries)->h_imag = gsl_vector_alloc(n);
}
void ReImTimeSeries_Cleanup(ReImTimeSeries *timeseries) {
  if(timeseries->times) gsl_vector_free(timeseries->times);
  if(timeseries->h_real) gsl_vector_free(timeseries->h_real);
  if(timeseries->h_imag) gsl_vector_free(timeseries->h_imag);
  free(timeseries);
}
/******** Functions to initialize and clean up AmpPhaseTimeSeries structure ********/
void AmpPhaseTimeSeries_Init(AmpPhaseTimeSeries **timeseries, const int n) {
  if(!timeseries) exit(1);
  /* Create storage for structures */
  if(!*timeseries) *timeseries=malloc(sizeof(AmpPhaseTimeSeries));
  else
  {
    AmpPhaseTimeSeries_Cleanup(*timeseries);
  }
  gsl_set_error_handler(&Err_Handler);
  (*timeseries)->times = gsl_vector_alloc(n);
  (*timeseries)->h_amp = gsl_vector_alloc(n);
  (*timeseries)->h_phase = gsl_vector_alloc(n);
}
void AmpPhaseTimeSeries_Cleanup(AmpPhaseTimeSeries *timeseries) {
  if(timeseries->times) gsl_vector_free(timeseries->times);
  if(timeseries->h_amp) gsl_vector_free(timeseries->h_amp);
  if(timeseries->h_phase) gsl_vector_free(timeseries->h_phase);
  free(timeseries);
}

/******** Functions to initialize and clean up RealTimeSeries structure ********/
void RealTimeSeries_Init(RealTimeSeries **timeseries, const int n) {
  if(!timeseries) exit(1);
  /* Create storage for structures */
  if(!*timeseries) *timeseries=malloc(sizeof(RealTimeSeries));
  else
  {
    RealTimeSeries_Cleanup(*timeseries);
  }
  gsl_set_error_handler(&Err_Handler);
  (*timeseries)->times = gsl_vector_alloc(n);
  (*timeseries)->h = gsl_vector_alloc(n);
}
void RealTimeSeries_Cleanup(RealTimeSeries *timeseries) {
  if(timeseries->times) gsl_vector_free(timeseries->times);
  if(timeseries->h) gsl_vector_free(timeseries->h);
  free(timeseries);
}

/***************** Functions for the ListmodesCAmpPhaseFrequencySeries structure ****************/
ListmodesCAmpPhaseFrequencySeries* ListmodesCAmpPhaseFrequencySeries_AddModeNoCopy(
	   ListmodesCAmpPhaseFrequencySeries* appended,  /* List structure to prepend to */
	   CAmpPhaseFrequencySeries* freqseries,  /* data to contain */
	   int l, /* major mode number */
	   int m  /* minor mode number */)
{
    ListmodesCAmpPhaseFrequencySeries* list;
    /* Check if the node with this mode already exists */
    list = appended;
    while( list ){
      if( l == list->l && m == list->m ){
	break;
      }
      list = list->next;
    }
    if( list ){ /* We don't allow for the case where the mode already exists in the list*/
      printf("Error: Tried to add an already existing mode to a ListmodesCAmpPhaseFrequencySeries ");
      return(NULL);
    } else { /* In that case, we do NOT COPY the input interpolated data, which therefore can't be
		used anywhere else; this will be acceptable as these operations will only be done
		when interpolating the initialization data */
      list = malloc( sizeof(ListmodesCAmpPhaseFrequencySeries) );
    }
    list->l = l;
    list->m = m;
    if( freqseries ){
      list->freqseries = freqseries;
    } else {
      list->freqseries = NULL;
    }
    if( appended ){
      list->next = appended;
    } else {
        list->next = NULL;
    }
    return list;
}
/* Get the element of a ListmodesCAmpPhaseFrequencySeries with a given index */
ListmodesCAmpPhaseFrequencySeries* ListmodesCAmpPhaseFrequencySeries_GetMode(
	   ListmodesCAmpPhaseFrequencySeries* const list,  /* List structure to get a particular mode from */
	   int l, /*< major mode number */
	   int m  /*< minor mode number */ )
{
    if( !list ) return NULL;

    ListmodesCAmpPhaseFrequencySeries *itr = list;
    while( itr->l != l || itr->m != m ){
        itr = itr->next;
        if( !itr ) return NULL;
    }
    return itr; /* The element returned is itself a pointer to a ListmodesCAmpPhaseFrequencySeries */
}
void ListmodesCAmpPhaseFrequencySeries_Destroy(
	   ListmodesCAmpPhaseFrequencySeries* list  /* List structure to destroy; notice that the data is destroyed too */
)
{
  ListmodesCAmpPhaseFrequencySeries* pop;
  while( (pop = list) ){
    if( pop->freqseries ){ /* Destroying the CAmpPhaseFrequencySeries data */
      CAmpPhaseFrequencySeries_Cleanup( pop->freqseries );
    }
    /* Notice that the mode indices l and m are not freed, like in SphHarmTimeSeries struct indices l and m */
    list = pop->next;
    free( pop );
  }
}

/***************** Functions for the ListmodesCAmpPhaseSpline structure ****************/
ListmodesCAmpPhaseSpline* ListmodesCAmpPhaseSpline_AddModeNoCopy(
	   ListmodesCAmpPhaseSpline* appended,  /* List structure to prepend to */
	   CAmpPhaseSpline* splines,  /* data to contain */
	   int l, /* major mode number */
	   int m  /* minor mode number */)
{
    ListmodesCAmpPhaseSpline* list;
    /* Check if the node with this mode already exists */
    list = appended;
    while( list ){
      if( l == list->l && m == list->m ){
	break;
      }
      list = list->next;
    }
    if( list ){ /* We don't allow for the case where the mode already exists in the list*/
      printf("Error: Tried to add an already existing mode to a ListmodesCAmpPhaseSpline ");
      return(NULL);
    } else { /* In that case, we do NOT COPY the input interpolated data, which therefore can't be
		used anywhere else; this will be acceptable as these operations will only be done
		when interpolating the initialization data */
      list = malloc( sizeof(ListmodesCAmpPhaseSpline) );
    }
    list->l = l;
    list->m = m;
    if( splines ){
      list->splines = splines;
    } else {
      list->splines = NULL;
    }
    if( appended ){
      list->next = appended;
    } else {
        list->next = NULL;
    }
    return list;
}
/* Get the element of a ListmodesCAmpPhaseSpline with a given index */
ListmodesCAmpPhaseSpline* ListmodesCAmpPhaseSpline_GetMode(
	   ListmodesCAmpPhaseSpline* const list,  /* List structure to get a particular mode from */
	   int l, /*< major mode number */
	   int m  /*< minor mode number */ )
{
    if( !list ) return NULL;

    ListmodesCAmpPhaseSpline *itr = list;
    while( itr->l != l || itr->m != m ){
        itr = itr->next;
        if( !itr ) return NULL;
    }
    return itr; /* The element returned is itself a pointer to a ListmodesCAmpPhaseSpline */
}
void ListmodesCAmpPhaseSpline_Destroy(
	   ListmodesCAmpPhaseSpline* list  /* List structure to destroy; notice that the data is destroyed too */
)
{
  ListmodesCAmpPhaseSpline* pop;
  while( (pop = list) ){
    if( pop->splines ){ /* Destroying the CAmpPhaseSpline data */
      CAmpPhaseSpline_Cleanup( pop->splines );
    }
    /* Notice that the mode indices l and m are not freed, like in SphHarmTimeSeries struct indices l and m */
    list = pop->next;
    free( pop );
  }
}

/***********************************************************************/
/**************** I/O functions for internal structures ****************/

/* Read waveform Real time series */
int Read_RealTimeSeries(RealTimeSeries** timeseries, const char dir[], const char file[], const int nblines, const int binary)
{
  /* Initalize and read input */
  int ret;
  gsl_matrix* inmatrix =  gsl_matrix_alloc(nblines, 2);
  if(!binary) ret = Read_Text_Matrix(dir, file, inmatrix);
  else ret = Read_Matrix(dir, file, inmatrix);

  /* Initialize structures */
  RealTimeSeries_Init(timeseries, nblines);

  /* Set values */
  gsl_vector_view timesview = gsl_matrix_column(inmatrix, 0);
  gsl_vector_view hview = gsl_matrix_column(inmatrix, 1);
  gsl_vector_memcpy((*timeseries)->times, &timesview.vector);
  gsl_vector_memcpy((*timeseries)->h, &hview.vector);

  /* Clean up */
  gsl_matrix_free(inmatrix);

  return ret;
}

/* Read waveform Amp/Phase time series */
int Read_AmpPhaseTimeSeries(AmpPhaseTimeSeries** timeseries, const char dir[], const char file[], const int nblines, const int binary)
{
  /* Initalize and read input */
  int ret;
  gsl_matrix* inmatrix =  gsl_matrix_alloc(nblines, 3);
  if(!binary) ret = Read_Text_Matrix(dir, file, inmatrix);
  else ret = Read_Matrix(dir, file, inmatrix);

  /* Initialize structures */
  AmpPhaseTimeSeries_Init(timeseries, nblines);

  /* Set values */
  gsl_vector_view timesview = gsl_matrix_column(inmatrix, 0);
  gsl_vector_view hampview = gsl_matrix_column(inmatrix, 1);
  gsl_vector_view hphaseview = gsl_matrix_column(inmatrix, 2);
  gsl_vector_memcpy((*timeseries)->times, &timesview.vector);
  gsl_vector_memcpy((*timeseries)->h_amp, &hampview.vector);
  gsl_vector_memcpy((*timeseries)->h_phase, &hphaseview.vector);

  /* Clean up */
  gsl_matrix_free(inmatrix);

  return ret;
}

/* Read waveform Re/Im time series */
int Read_ReImTimeSeries(ReImTimeSeries** timeseries, const char dir[], const char file[], const int nblines, const int binary)
{
  /* Initalize and read input */
  int ret;
  gsl_matrix* inmatrix =  gsl_matrix_alloc(nblines, 3);
  if(!binary) ret = Read_Text_Matrix(dir, file, inmatrix);
  else ret = Read_Matrix(dir, file, inmatrix);

  /* Initialize structures */
  ReImTimeSeries_Init(timeseries, nblines);

  /* Set values */
  gsl_vector_view timesview = gsl_matrix_column(inmatrix, 0);
  gsl_vector_view hrealview = gsl_matrix_column(inmatrix, 1);
  gsl_vector_view himagview = gsl_matrix_column(inmatrix, 2);
  gsl_vector_memcpy((*timeseries)->times, &timesview.vector);
  gsl_vector_memcpy((*timeseries)->h_real, &hrealview.vector);
  gsl_vector_memcpy((*timeseries)->h_imag, &himagview.vector);

  /* Clean up */
  gsl_matrix_free(inmatrix);

  return ret;
}

/* Output Re/Im frequency series */
int Write_ReImFrequencySeries(const char dir[], const char file[], ReImFrequencySeries* freqseries, const int binary)
{
  /* Initialize output */
  /* Note: assumes hplus, hcross have same length as expected */
  int nbfreq = freqseries->freq->size;
  gsl_matrix* outmatrix = gsl_matrix_alloc(nbfreq, 3);

  /* Set output matrix */
  gsl_matrix_set_col(outmatrix, 0, freqseries->freq);
  gsl_matrix_set_col(outmatrix, 1, freqseries->h_real);
  gsl_matrix_set_col(outmatrix, 2, freqseries->h_imag);

  /* Output */
  int ret;
  if (!binary) ret = Write_Text_Matrix(dir, file, outmatrix);
  else ret = Write_Matrix(dir, file, outmatrix);

  return ret;
}

/* Output real time series */
int Write_RealTimeSeries(const char dir[], const char file[], RealTimeSeries* timeseries, int binary)
{
  /* Initialize output */
  int nbtimes = timeseries->times->size;
  gsl_matrix* outmatrix = gsl_matrix_alloc(nbtimes, 2);

  /* Set data */
  gsl_matrix_set_col(outmatrix, 0, timeseries->times);
  gsl_matrix_set_col(outmatrix, 1, timeseries->h);

  /* Output */
  int ret;
  if(!binary) ret = Write_Text_Matrix(dir, file, outmatrix);
  else ret = Write_Matrix(dir, file, outmatrix);

  return ret;
}

/* Output Amp/Phase time series */
int Write_AmpPhaseTimeSeries(const char dir[], const char file[], AmpPhaseTimeSeries* timeseries, int binary)
{
  /* Initialize output */
  int nbtimes = timeseries->times->size;
  gsl_matrix* outmatrix = gsl_matrix_alloc(nbtimes, 3);

  /* Set data */
  gsl_matrix_set_col(outmatrix, 0, timeseries->times);
  gsl_matrix_set_col(outmatrix, 1, timeseries->h_amp);
  gsl_matrix_set_col(outmatrix, 2, timeseries->h_phase);

  /* Output */
  int ret;
  if(!binary) ret = Write_Text_Matrix(dir, file, outmatrix);
  else ret = Write_Matrix(dir, file, outmatrix);

  return ret;
}

/* Output Re/Im time series */
int Write_ReImTimeSeries(const char dir[], const char file[], ReImTimeSeries* timeseries, int binary)
{
  /* Initialize output */
  int nbtimes = timeseries->times->size;
  gsl_matrix* outmatrix = gsl_matrix_alloc(nbtimes, 3);

  /* Set data */
  gsl_matrix_set_col(outmatrix, 0, timeseries->times);
  gsl_matrix_set_col(outmatrix, 1, timeseries->h_real);
  gsl_matrix_set_col(outmatrix, 2, timeseries->h_imag);

  /* Output */
  int ret;
  if(!binary) ret = Write_Text_Matrix(dir, file, outmatrix);
  else ret = Write_Matrix(dir, file, outmatrix);

  return ret;
}


/*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
**                                                                                **
** This file forms part of the Underworld geophysics modelling application.         **
**                                                                                **
** For full license and copyright information, please refer to the LICENSE.md file  **
** located at the project root, or contact the authors.                             **
**                                                                                **
**~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*/
#include <petsc.h>
#include <petscmat.h>
#include <petscvec.h>

#include <StGermain/libStGermain/src/StGermain.h>
#include <StgDomain/libStgDomain/src/StgDomain.h>

#include "common-driver-utils.h"
#include "stokes_block_scaling.h"
#include "stokes_Kblock_scaling.h"
#include <StgFEM/libStgFEM/src/StgFEM.h>
#include <PICellerator/libPICellerator/src/PICellerator.h>
#include <Underworld/libUnderworld/src/Underworld.h>
#include "Solvers/SLE/src/SLE.h" /* to give the AugLagStokes_SLE type */
#include "Solvers/KSPSolvers/src/KSPSolvers.h" /* for __KSP_COMMON */

#include "BSSCR.h"

#undef __FUNCT__
#define __FUNCT__ "KSPUnscale_BSSCR"
PetscErrorCode KSPUnscale_BSSCR(KSP ksp)
{
    KSP_BSSCR  *bsscr = (KSP_BSSCR *)ksp->data;
    Mat            Amat,Pmat;
    Vec            B,X;
    Mat            ApproxS;
    MatStokesBlockScaling BA=bsscr->BA;
    PetscTruth     sym;
    MatStructure   pflag;
    PetscErrorCode ierr;

    PetscFunctionBegin;
    X             = ksp->vec_sol;
    B             = ksp->vec_rhs;
    ApproxS       = bsscr->preconditioner->matrix;
    sym           = bsscr->DIsSym;
    ierr=Stg_PCGetOperators(ksp->pc,&Amat,&Pmat,&pflag);CHKERRQ(ierr);
    if( BA->scaling_exists == PETSC_FALSE ){
	PetscPrintf( PETSC_COMM_WORLD, "SCALING does not EXIST in %s\n", __func__);
    }else{
	if( DEFAULT == bsscr->scaletype ){
	    PetscPrintf( PETSC_COMM_WORLD, "Operator scales (pre-Unscaling)\n");
	    BSSCR_MatStokesBlockReportOperatorScales(Amat, sym);
	    BSSCR_MatStokesBlockUnScaleSystem( BA, Amat, B, X, ApproxS, sym);
	    PetscPrintf( PETSC_COMM_WORLD, "Operator scales (post-Unscaling)\n");
	    BSSCR_MatStokesBlockReportOperatorScales(Amat, sym);
	}
	if( KONLY == bsscr->scaletype ){
	    PetscPrintf( PETSC_COMM_WORLD, "Operator scales KONLY (pre-Unscaling)\n");
	    BSSCR_MatStokesKBlockReportOperatorScales(Amat, sym);
	    BSSCR_MatStokesKBlockUnScaleSystem( BA, Amat, B, X, ApproxS, sym);
	    PetscPrintf( PETSC_COMM_WORLD, "Operator scales KONLY (post-Unscaling)\n");
	    BSSCR_MatStokesKBlockReportOperatorScales(Amat, sym);
	}
    }
    bsscr->scaled = PETSC_FALSE;
    PetscFunctionReturn(0);
}
/*
  KSPScale_BSSCR constructs scaling and applies scaling to system.
  Want to do both here when doing nonlinear iterations to keep the
  scaling up to date with each iteration.
 */
#undef __FUNCT__
#define __FUNCT__ "KSPScale_BSSCR"
PetscErrorCode KSPScale_BSSCR(KSP ksp)
{
    KSP_BSSCR  *bsscr = (KSP_BSSCR *)ksp->data;
    Mat            Amat,Pmat;
    Vec            B,X;
    Mat            ApproxS;
    MatStokesBlockScaling BA=bsscr->BA;
    PetscTruth     sym;
    MatStructure   pflag;
    PetscErrorCode ierr;

    PetscFunctionBegin;
    X             = ksp->vec_sol;
    B             = ksp->vec_rhs;
    ApproxS       = bsscr->preconditioner->matrix;
    sym           = bsscr->DIsSym;
    ierr=Stg_PCGetOperators(ksp->pc,&Amat,&Pmat,&pflag);CHKERRQ(ierr);

    if( DEFAULT == bsscr->scaletype ){
	if( BA->scaling_exists ){
	    BSSCR_MatStokesBlockDefaultBuildScaling( BA, Amat);/* rebuild scaling vectors on struct */
	    BA->scalings_have_been_inverted = PETSC_FALSE;
	}else{
	    BSSCR_MatBlock_ConstructScaling( BA, Amat, B, X );/* allocates scaling vectors then calls the above function */
	    BA->scalings_have_been_inverted = PETSC_FALSE;/* above function sets this but I am making it explicit here */
	}
	PetscPrintf( PETSC_COMM_WORLD, "Operator scales (pre-scaling)\n");
	BSSCR_MatStokesBlockReportOperatorScales(Amat, sym);
	BSSCR_MatStokesBlockScaleSystem( BA, Amat, B, X, ApproxS, sym );
	PetscPrintf( PETSC_COMM_WORLD, "Operator scales (post-scaling)\n");
	BSSCR_MatStokesBlockReportOperatorScales(Amat, sym);
	bsscr->scaled = PETSC_TRUE;
    }

    if( KONLY == bsscr->scaletype ){
	if( BA->scaling_exists ){
	    BSSCR_MatStokesKBlockDefaultBuildScaling( BA, Amat, B, X, sym);/* rebuild scaling vectors on struct */
	    BA->scalings_have_been_inverted = PETSC_FALSE;
	}else{
	    BSSCR_MatKBlock_ConstructScaling( BA, Amat, B, X, sym);/* allocates scaling vectors then calls the above function */
	    BA->scalings_have_been_inverted = PETSC_FALSE;/* above function sets this but I am making it explicit here */
	}
	PetscPrintf( PETSC_COMM_WORLD, "Operator scales KONLY (pre-scaling)\n");
	BSSCR_MatStokesKBlockReportOperatorScales(Amat, sym);
	BSSCR_MatStokesKBlockScaleSystem( BA, Amat, B, X, ApproxS, sym );
	PetscPrintf( PETSC_COMM_WORLD, "Operator scales KONLY (post-scaling)\n");
	BSSCR_MatStokesKBlockReportOperatorScales(Amat, sym);
	bsscr->scaled = PETSC_TRUE;
    }
    PetscFunctionReturn(0);
}


/* movstat/gsl_movstat.h
 *
 * Copyright (C) 2018 Patrick Alken
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 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
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#ifndef __GSL_MOVSTAT_H__
#define __GSL_MOVSTAT_H__

#if !defined( GSL_FUN )
#  if !defined( GSL_DLL )
#    define GSL_FUN extern
#  elif defined( BUILD_GSL_DLL )
#    define GSL_FUN extern __declspec(dllexport)
#  else
#    define GSL_FUN extern __declspec(dllimport)
#  endif
#endif

#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

typedef enum
{
  GSL_MOVSTAT_END_PADZERO,
  GSL_MOVSTAT_END_PADVALUE,
  GSL_MOVSTAT_END_TRUNCATE
} gsl_movstat_end_t;

/* accumulator struct
 * size          - return number of bytes needed for accumulator with maximum of n elements
 * init          - initialize accumulator state
 * insert        - insert a single sample into accumulator; if there are already n
 *                 samples in accumulator, oldest sample is overwritten
 * delete_oldest - delete oldest sample from accumulator
 * get           - return accumulated value
 */
typedef struct
{
  size_t (*size) (const size_t n);
  int (*init) (const size_t n, void * vstate);
  int (*insert) (const double x, void * vstate);
  int (*delete_oldest) (void * vstate);
  int (*get) (void * params, double * result, const void * vstate);
} gsl_movstat_accum;

typedef struct
{
  double (* function) (const size_t n, double x[], void * params);
  void * params;
} gsl_movstat_function;

#define GSL_MOVSTAT_FN_EVAL(F,n,x) (*((F)->function))((n),(x),(F)->params)

/* workspace for moving window statistics */

typedef struct
{
  size_t H;          /* number of previous samples in window */
  size_t J;          /* number of after samples in window */
  size_t K;          /* window size K = H + J + 1 */
  double *work;      /* workspace, size K */
  void *state;       /* state workspace for various accumulators */
  size_t state_size; /* bytes allocated for 'state' */
} gsl_movstat_workspace;

/* alloc.c */

GSL_FUN gsl_movstat_workspace *gsl_movstat_alloc(const size_t K);
GSL_FUN gsl_movstat_workspace *gsl_movstat_alloc2(const size_t H, const size_t J);
GSL_FUN gsl_movstat_workspace *gsl_movstat_alloc_with_size(const size_t accum_state_size, const size_t H, const size_t J);
GSL_FUN void gsl_movstat_free(gsl_movstat_workspace * w);

/* apply.c */
GSL_FUN int gsl_movstat_apply_accum(const gsl_movstat_end_t endtype, const gsl_vector * x,
                            const gsl_movstat_accum * accum, void * accum_params,
                            gsl_vector * y, gsl_vector * z,
                            gsl_movstat_workspace * w);
GSL_FUN int gsl_movstat_apply(const gsl_movstat_end_t endtype, const gsl_movstat_function * F,
                      const gsl_vector * x, gsl_vector * y, gsl_movstat_workspace * w);

/* fill.c */
GSL_FUN size_t gsl_movstat_fill(const gsl_movstat_end_t endtype, const gsl_vector * x, const size_t idx,
                        const size_t H, const size_t J, double * window);

GSL_FUN int gsl_movstat_mean(const gsl_movstat_end_t endtype, const gsl_vector * x, gsl_vector * y, gsl_movstat_workspace * w);
GSL_FUN int gsl_movstat_variance(const gsl_movstat_end_t endtype, const gsl_vector * x, gsl_vector * y, gsl_movstat_workspace * w);
GSL_FUN int gsl_movstat_sd(const gsl_movstat_end_t endtype, const gsl_vector * x, gsl_vector * y, gsl_movstat_workspace * w);
GSL_FUN int gsl_movstat_median(const gsl_movstat_end_t endtype, const gsl_vector * x, gsl_vector * y, gsl_movstat_workspace * w);
GSL_FUN int gsl_movstat_min(const gsl_movstat_end_t endtype, const gsl_vector * x, gsl_vector * y, gsl_movstat_workspace * w);
GSL_FUN int gsl_movstat_max(const gsl_movstat_end_t endtype, const gsl_vector * x, gsl_vector * y, gsl_movstat_workspace * w);
GSL_FUN int gsl_movstat_minmax(const gsl_movstat_end_t endtype, const gsl_vector * x, gsl_vector * y_min, gsl_vector * y_max, gsl_movstat_workspace * w);
GSL_FUN int gsl_movstat_mad0(const gsl_movstat_end_t endtype, const gsl_vector * x, gsl_vector * xmedian,
                     gsl_vector * xmad, gsl_movstat_workspace * w);
GSL_FUN int gsl_movstat_mad(const gsl_movstat_end_t endtype, const gsl_vector * x, gsl_vector * xmedian,
                    gsl_vector * xmad, gsl_movstat_workspace * w);
GSL_FUN int gsl_movstat_qqr(const gsl_movstat_end_t endtype, const gsl_vector * x, const double q,
                    gsl_vector * xqqr, gsl_movstat_workspace * w);
GSL_FUN int gsl_movstat_Sn(const gsl_movstat_end_t endtype, const gsl_vector * x,
                   gsl_vector * xscale, gsl_movstat_workspace * w);
GSL_FUN int gsl_movstat_Qn(const gsl_movstat_end_t endtype, const gsl_vector * x,
                   gsl_vector * xscale, gsl_movstat_workspace * w);
GSL_FUN int gsl_movstat_sum(const gsl_movstat_end_t endtype, const gsl_vector * x, gsl_vector * y, gsl_movstat_workspace * w);

/* accumulator variables */

GSL_VAR const gsl_movstat_accum * gsl_movstat_accum_mad;
GSL_VAR const gsl_movstat_accum * gsl_movstat_accum_max;
GSL_VAR const gsl_movstat_accum * gsl_movstat_accum_mean;
GSL_VAR const gsl_movstat_accum * gsl_movstat_accum_median;
GSL_VAR const gsl_movstat_accum * gsl_movstat_accum_min;
GSL_VAR const gsl_movstat_accum * gsl_movstat_accum_minmax;
GSL_VAR const gsl_movstat_accum * gsl_movstat_accum_sd;
GSL_VAR const gsl_movstat_accum * gsl_movstat_accum_Sn;
GSL_VAR const gsl_movstat_accum * gsl_movstat_accum_sum;
GSL_VAR const gsl_movstat_accum * gsl_movstat_accum_Qn;
GSL_VAR const gsl_movstat_accum * gsl_movstat_accum_qqr;
GSL_VAR const gsl_movstat_accum * gsl_movstat_accum_userfunc;
GSL_VAR const gsl_movstat_accum * gsl_movstat_accum_variance;

__END_DECLS

#endif /* __GSL_MOVSTAT_H__ */


#include <mpi.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <sys/stat.h>
#include <sys/types.h>

#include "allvars.h"
#include "proto.h"

#ifdef SUBFIND

#include "fof.h"
#include "subfind.h"

#ifdef PERIODIC
#define NEAREST(x) (((x)>boxhalf)?((x)-boxsize):(((x)<-boxhalf)?((x)+boxsize):(x)))
#else
#define NEAREST(x) (x)
#endif

/*! Structure for communication during the density computation. Holds data that is sent to other processors.
 */
static struct SOdens_in
{
  MyDouble Pos[3];
  double R200;
  int NodeList[NODELISTLENGTH];
}
 *SOdensIn, *SOdensGet;

static struct SOdens_out
{
  double Mass;
#ifdef SO_VEL_DISPERSIONS
  double Vx200, Vy200, Vz200, Disp200;
#endif
}
 *SOdensResult, *SOdensOut;

static double *R200, *M200;

#ifdef SO_VEL_DISPERSIONS
static double *Vx200, *Vy200, *Vz200, *Disp200;
#endif

void subfind_overdensity(void)
{
  long long ntot;
  int i, j, ndone, ndone_flag, npleft, dummy, rep, iter;
  MyFloat *Left, *Right;
  char *Todo;
  int ngrp, sendTask, recvTask, place, nexport, nimport;
  double t0, t1, rguess, overdensity, Deltas[3], rhoback, z, omegaz, x, DeltaMean200, DeltaCrit200,
    DeltaTopHat;

  /* allocate buffers to arrange communication */

  Ngblist = (int *) mymalloc(NumPart * sizeof(int));
  All.BunchSize =
    (int) ((All.BufferSize * 1024 * 1024) / (sizeof(struct data_index) + sizeof(struct data_nodelist) +
					     sizeof(struct SOdens_in) + sizeof(struct SOdens_out) +
					     sizemax(sizeof(struct SOdens_in), sizeof(struct SOdens_out))));
  DataIndexTable = (struct data_index *) mymalloc(All.BunchSize * sizeof(struct data_index));
  DataNodeList = (struct data_nodelist *) mymalloc(All.BunchSize * sizeof(struct data_nodelist));

  Left = (MyFloat *) mymalloc(sizeof(MyFloat) * Ngroups);
  Right = (MyFloat *) mymalloc(sizeof(MyFloat) * Ngroups);
  R200 = (double *) mymalloc(sizeof(double) * Ngroups);
  M200 = (double *) mymalloc(sizeof(double) * Ngroups);
#ifdef SO_VEL_DISPERSIONS
  Vx200 = (double *) mymalloc(sizeof(double) * Ngroups);
  Vy200 = (double *) mymalloc(sizeof(double) * Ngroups);
  Vz200 = (double *) mymalloc(sizeof(double) * Ngroups);
  Disp200 = (double *) mymalloc(sizeof(double) * Ngroups);
#endif

  Todo = mymalloc(sizeof(char) * Ngroups);

  z = 1 / All.Time - 1;

  rhoback = 3 * All.Omega0 * All.Hubble * All.Hubble / (8 * M_PI * All.G);

  omegaz =
    All.Omega0 * pow(1 + z,
		     3) / (All.Omega0 * pow(1 + z, 3) + (1 - All.Omega0 - All.OmegaLambda) * pow(1 + z,
												 2) +
			   All.OmegaLambda);

  DeltaMean200 = 200.0;
  DeltaCrit200 = 200.0 / omegaz;

  x = omegaz - 1;
  DeltaTopHat = 18 * M_PI * M_PI + 82 * x - 39 * x * x;
  DeltaTopHat /= omegaz;

  Deltas[0] = DeltaMean200;	/* standard fixed overdensity with respect to background */
  Deltas[1] = DeltaTopHat;	/* tophat overdensity with respect to background */
  Deltas[2] = DeltaCrit200;	/* overdensity of 200 relative to critical, expressed relative to background density */

  for(rep = 0; rep < 3; rep++)	/* repeat for all three overdensity values */
    {
      for(i = 0; i < Ngroups; i++)
	{
	  if(Group[i].Nsubs > 0)
	    {
	      rguess = pow(All.G * Group[i].Mass / (100 * All.Hubble * All.Hubble), 1.0 / 3);

	      Right[i] = 3 * rguess;
	      Left[i] = 0;

	      Todo[i] = 1;
	    }
	  else
	    {
	      Todo[i] = 0;
	    }
	}

      iter = 0;

      /* we will repeat the whole thing for those groups where we didn't converge to a SO radius yet */
      do
	{
	  t0 = second();

	  i = 0;		/* begin with this index */

	  do
	    {
	      for(j = 0; j < NTask; j++)
		{
		  Send_count[j] = 0;
		  Exportflag[j] = -1;
		}

	      /* do local particles and prepare export list */

	      for(nexport = 0; i < Ngroups; i++)
		{
		  if(Todo[i])
		    {
		      R200[i] = 0.5 * (Left[i] + Right[i]);
		      if(subfind_overdensity_evaluate(i, 0, &nexport, Send_count) < 0)
			break;
		    }
		}

	      qsort(DataIndexTable, nexport, sizeof(struct data_index), data_index_compare);

	      MPI_Allgather(Send_count, NTask, MPI_INT, Sendcount_matrix, NTask, MPI_INT, MPI_COMM_WORLD);

	      for(j = 0, nimport = 0, Recv_offset[0] = 0, Send_offset[0] = 0; j < NTask; j++)
		{
		  Recv_count[j] = Sendcount_matrix[j * NTask + ThisTask];
		  nimport += Recv_count[j];

		  if(j > 0)
		    {
		      Send_offset[j] = Send_offset[j - 1] + Send_count[j - 1];
		      Recv_offset[j] = Recv_offset[j - 1] + Recv_count[j - 1];
		    }
		}

	      SOdensGet = (struct SOdens_in *) mymalloc(nimport * sizeof(struct SOdens_in));
	      SOdensIn = (struct SOdens_in *) mymalloc(nexport * sizeof(struct SOdens_in));

	      /* prepare particle data for export */
	      for(j = 0; j < nexport; j++)
		{
		  place = DataIndexTable[j].Index;

		  SOdensIn[j].Pos[0] = Group[place].Pos[0];
		  SOdensIn[j].Pos[1] = Group[place].Pos[1];
		  SOdensIn[j].Pos[2] = Group[place].Pos[2];
		  SOdensIn[j].R200 = R200[place];

		  memcpy(SOdensIn[j].NodeList,
			 DataNodeList[DataIndexTable[j].IndexGet].NodeList, NODELISTLENGTH * sizeof(int));
		}

	      /* exchange data */
	      for(ngrp = 1; ngrp < (1 << PTask); ngrp++)
		{
		  sendTask = ThisTask;
		  recvTask = ThisTask ^ ngrp;

		  if(recvTask < NTask)
		    {
		      if(Send_count[recvTask] > 0 || Recv_count[recvTask] > 0)
			{
			  /* get the data */
			  MPI_Sendrecv(&SOdensIn[Send_offset[recvTask]],
				       Send_count[recvTask] * sizeof(struct SOdens_in), MPI_BYTE,
				       recvTask, TAG_DENS_A,
				       &SOdensGet[Recv_offset[recvTask]],
				       Recv_count[recvTask] * sizeof(struct SOdens_in), MPI_BYTE,
				       recvTask, TAG_DENS_A, MPI_COMM_WORLD, MPI_STATUS_IGNORE);
			}
		    }
		}

	      myfree(SOdensIn);
	      SOdensResult = (struct SOdens_out *) mymalloc(nimport * sizeof(struct SOdens_out));
	      SOdensOut = (struct SOdens_out *) mymalloc(nexport * sizeof(struct SOdens_out));

	      /* now do the locations that were sent to us */
	      for(j = 0; j < nimport; j++)
		subfind_overdensity_evaluate(j, 1, &dummy, &dummy);

	      if(i >= Ngroups)
		ndone_flag = 1;
	      else
		ndone_flag = 0;

	      MPI_Allreduce(&ndone_flag, &ndone, 1, MPI_INT, MPI_SUM, MPI_COMM_WORLD);

	      /* get the result */
	      for(ngrp = 1; ngrp < (1 << PTask); ngrp++)
		{
		  sendTask = ThisTask;
		  recvTask = ThisTask ^ ngrp;
		  if(recvTask < NTask)
		    {
		      if(Send_count[recvTask] > 0 || Recv_count[recvTask] > 0)
			{
			  /* send the results */
			  MPI_Sendrecv(&SOdensResult[Recv_offset[recvTask]],
				       Recv_count[recvTask] * sizeof(struct SOdens_out),
				       MPI_BYTE, recvTask, TAG_DENS_B,
				       &SOdensOut[Send_offset[recvTask]],
				       Send_count[recvTask] * sizeof(struct SOdens_out),
				       MPI_BYTE, recvTask, TAG_DENS_B, MPI_COMM_WORLD, MPI_STATUS_IGNORE);
			}
		    }
		}

	      /* add the result to the local particles */
	      for(j = 0; j < nexport; j++)
		{
		  place = DataIndexTable[j].Index;

		  M200[place] += SOdensOut[j].Mass;
		}

	      myfree(SOdensOut);
	      myfree(SOdensResult);
	      myfree(SOdensGet);
	    }
	  while(ndone < NTask);

	  /* do final operations on results */
	  for(i = 0, npleft = 0; i < Ngroups; i++)
	    {
	      if(Todo[i])
		{
		  overdensity = M200[i] / (4.0 * M_PI / 3.0 * R200[i] * R200[i] * R200[i]) / rhoback;

		  if((Right[i] - Left[i]) > 1.0e-4 * Left[i])
		    {
		      /* need to redo this group */
		      npleft++;

		      if(overdensity > Deltas[rep])
			Left[i] = R200[i];
		      else
			Right[i] = R200[i];

		      if(iter >= MAXITER - 10)
			{
			  printf
			    ("gr=%d task=%d  R200=%g Left=%g Right=%g Menclosed=%g Right-Left=%g\n   pos=(%g|%g|%g)\n",
			     i, ThisTask, R200[i], Left[i], Right[i],
			     M200[i], Right[i] - Left[i], Group[i].Pos[0], Group[i].Pos[1], Group[i].Pos[2]);
			  fflush(stdout);
			}
		    }
		  else
		    Todo[i] = 0;
		}
	    }

	  sumup_large_ints(1, &npleft, &ntot);

	  t1 = second();

	  if(ntot > 0)
	    {
	      iter++;

	      if(iter > 0 && ThisTask == 0)
		{
		  printf("SO iteration %d: need to repeat for %d%09d particles. (took %g sec)\n", iter,
			 (int) (ntot / 1000000000), (int) (ntot % 1000000000), timediff(t0, t1));
		  fflush(stdout);
		}

	      if(iter > MAXITER)
		{
		  printf("failed to converge in neighbour iteration in density()\n");
		  fflush(stdout);
		  endrun(1155);
		}
	    }
	}
      while(ntot > 0);

#ifdef SO_VEL_DISPERSIONS
      i = 0;			/* begin with this index */
      do
	{
	  for(j = 0; j < NTask; j++)
	    {
	      Send_count[j] = 0;
	      Exportflag[j] = -1;
	    }

	  /* do local particles and prepare export list */

	  for(nexport = 0; i < Ngroups; i++)
	    {
	      if(subfind_overdensity_evaluate_dispersion(i, 0, &nexport, Send_count) < 0)
		break;
	    }

	  qsort(DataIndexTable, nexport, sizeof(struct data_index), data_index_compare);

	  MPI_Allgather(Send_count, NTask, MPI_INT, Sendcount_matrix, NTask, MPI_INT, MPI_COMM_WORLD);

	  for(j = 0, nimport = 0, Recv_offset[0] = 0, Send_offset[0] = 0; j < NTask; j++)
	    {
	      Recv_count[j] = Sendcount_matrix[j * NTask + ThisTask];
	      nimport += Recv_count[j];

	      if(j > 0)
		{
		  Send_offset[j] = Send_offset[j - 1] + Send_count[j - 1];
		  Recv_offset[j] = Recv_offset[j - 1] + Recv_count[j - 1];
		}
	    }

	  SOdensGet = (struct SOdens_in *) mymalloc(nimport * sizeof(struct SOdens_in));
	  SOdensIn = (struct SOdens_in *) mymalloc(nexport * sizeof(struct SOdens_in));

	  /* prepare particle data for export */
	  for(j = 0; j < nexport; j++)
	    {
	      place = DataIndexTable[j].Index;

	      SOdensIn[j].Pos[0] = Group[place].Pos[0];
	      SOdensIn[j].Pos[1] = Group[place].Pos[1];
	      SOdensIn[j].Pos[2] = Group[place].Pos[2];
	      SOdensIn[j].R200 = R200[place];

	      memcpy(SOdensIn[j].NodeList,
		     DataNodeList[DataIndexTable[j].IndexGet].NodeList, NODELISTLENGTH * sizeof(int));
	    }

	  /* exchange data */
	  for(ngrp = 1; ngrp < (1 << PTask); ngrp++)
	    {
	      sendTask = ThisTask;
	      recvTask = ThisTask ^ ngrp;

	      if(recvTask < NTask)
		{
		  if(Send_count[recvTask] > 0 || Recv_count[recvTask] > 0)
		    {
		      /* get the data */
		      MPI_Sendrecv(&SOdensIn[Send_offset[recvTask]],
				   Send_count[recvTask] * sizeof(struct SOdens_in), MPI_BYTE,
				   recvTask, TAG_DENS_A,
				   &SOdensGet[Recv_offset[recvTask]],
				   Recv_count[recvTask] * sizeof(struct SOdens_in), MPI_BYTE,
				   recvTask, TAG_DENS_A, MPI_COMM_WORLD, MPI_STATUS_IGNORE);
		    }
		}
	    }

	  myfree(SOdensIn);
	  SOdensResult = (struct SOdens_out *) mymalloc(nimport * sizeof(struct SOdens_out));
	  SOdensOut = (struct SOdens_out *) mymalloc(nexport * sizeof(struct SOdens_out));

	  /* now do the locations that were sent to us */
	  for(j = 0; j < nimport; j++)
	    subfind_overdensity_evaluate_dispersion(j, 1, &dummy, &dummy);

	  if(i >= Ngroups)
	    ndone_flag = 1;
	  else
	    ndone_flag = 0;

	  MPI_Allreduce(&ndone_flag, &ndone, 1, MPI_INT, MPI_SUM, MPI_COMM_WORLD);

	  /* get the result */
	  for(ngrp = 1; ngrp < (1 << PTask); ngrp++)
	    {
	      sendTask = ThisTask;
	      recvTask = ThisTask ^ ngrp;
	      if(recvTask < NTask)
		{
		  if(Send_count[recvTask] > 0 || Recv_count[recvTask] > 0)
		    {
		      /* send the results */
		      MPI_Sendrecv(&SOdensResult[Recv_offset[recvTask]],
				   Recv_count[recvTask] * sizeof(struct SOdens_out),
				   MPI_BYTE, recvTask, TAG_DENS_B,
				   &SOdensOut[Send_offset[recvTask]],
				   Send_count[recvTask] * sizeof(struct SOdens_out),
				   MPI_BYTE, recvTask, TAG_DENS_B, MPI_COMM_WORLD, MPI_STATUS_IGNORE);
		    }
		}
	    }

	  /* add the result to the local particles */
	  for(j = 0; j < nexport; j++)
	    {
	      place = DataIndexTable[j].Index;

	      Vx200[place] += SOdensOut[j].Vx200;
	      Vy200[place] += SOdensOut[j].Vy200;
	      Vz200[place] += SOdensOut[j].Vz200;
	      Disp200[place] += SOdensOut[j].Disp200;
	    }

	  myfree(SOdensOut);
	  myfree(SOdensResult);
	  myfree(SOdensGet);
	}
      while(ndone < NTask);
#endif

      for(i = 0; i < Ngroups; i++)
	{
	  if(Group[i].Nsubs > 0)
	    {
	      overdensity = M200[i] / (4.0 * M_PI / 3.0 * R200[i] * R200[i] * R200[i]) / rhoback;

	      if((overdensity - Deltas[rep]) > 0.1 * Deltas[rep])
		{
		  R200[i] = M200[i] = 0;
		}
	      else if(M200[i] < 5 * Group[i].Mass / Group[i].Len)
		{
		  R200[i] = M200[i] = 0;
		}
	    }
	  else
	    R200[i] = M200[i] = 0;

	  switch (rep)
	    {
	    case 0:
	      Group[i].M_Mean200 = M200[i];
	      Group[i].R_Mean200 = R200[i];
	      break;
	    case 1:
	      Group[i].M_TopHat200 = M200[i];
	      Group[i].R_TopHat200 = R200[i];
	      break;
	    case 2:
	      Group[i].M_Crit200 = M200[i];
	      Group[i].R_Crit200 = R200[i];
	      break;
	    }

#ifdef SO_VEL_DISPERSIONS
	  if(R200[i] > 0 && M200[i] > 0)
	    {
	      Vx200[i] /= M200[i];
	      Vy200[i] /= M200[i];
	      Vz200[i] /= M200[i];
	      Disp200[i] /= M200[i];
	      Disp200[i] -= (Vx200[i] * Vx200[i] + Vy200[i] * Vy200[i] + Vz200[i] * Vz200[i]);
	      Disp200[i] = sqrt(Disp200[i] / 3);	/* convert to 1D velocity dispersion */
	    }
	  else
	    {
	      Disp200[i] = 0;
	    }

	  switch (rep)
	    {
	    case 0:
	      Group[i].VelDisp_Mean200 = Disp200[i];
	      break;
	    case 1:
	      Group[i].VelDisp_TopHat200 = Disp200[i];
	      break;
	    case 2:
	      Group[i].VelDisp_Crit200 = Disp200[i];
	      break;
	    }
#endif
	}
    }

  myfree(Todo);
#ifdef SO_VEL_DISPERSIONS
  myfree(Disp200);
  myfree(Vz200);
  myfree(Vy200);
  myfree(Vx200);
#endif
  myfree(M200);
  myfree(R200);
  myfree(Right);
  myfree(Left);

  myfree(DataNodeList);
  myfree(DataIndexTable);
  myfree(Ngblist);
}

/*! This function represents the core of the SPH density computation. The
 *  target particle may either be local, or reside in the communication
 *  buffer.
 */
int subfind_overdensity_evaluate(int target, int mode, int *nexport, int *nsend_local)
{
  int startnode, listindex = 0;
  double h, mass, massret;
  MyDouble *pos;

  if(mode == 0)
    {
      pos = Group[target].Pos;
      h = R200[target];
    }
  else
    {
      pos = SOdensGet[target].Pos;
      h = SOdensGet[target].R200;
    }

  if(mode == 0)
    {
      startnode = All.MaxPart;	/* root node */
    }
  else
    {
      startnode = SOdensGet[target].NodeList[0];
      startnode = Nodes[startnode].u.d.nextnode;	/* open it */
    }

  mass = 0;

  while(startnode >= 0)
    {
      while(startnode >= 0)
	{
	  massret = subfind_ovderdens_treefind(pos, h, target, &startnode, mode, nexport, nsend_local);

	  if(massret < 0)
	    return -1;

	  mass += massret;
	}

      if(mode == 1)
	{
	  listindex++;
	  if(listindex < NODELISTLENGTH)
	    {
	      startnode = SOdensGet[target].NodeList[listindex];
	      if(startnode >= 0)
		startnode = Nodes[startnode].u.d.nextnode;	/* open it */
	    }
	}
    }

  if(mode == 0)
    M200[target] = mass;
  else
    SOdensResult[target].Mass = mass;

  return 0;
}

double subfind_ovderdens_treefind(MyDouble searchcenter[3], MyFloat hsml, int target, int *startnode,
				  int mode, int *nexport, int *nsend_local)
{
  int no, p, task, nexport_save;
  struct NODE *current;
  double mass;
  MyDouble dx, dy, dz, dist, r2;

#define FACT2 0.86602540
#ifdef PERIODIC
  MyDouble xtmp;
#endif
  nexport_save = *nexport;

  mass = 0;
  no = *startnode;

  while(no >= 0)
    {
      if(no < All.MaxPart)	/* single particle */
	{
	  p = no;
	  no = Nextnode[no];

	  dist = hsml;
	  dx = NGB_PERIODIC_LONG_X(P[p].Pos[0] - searchcenter[0]);
	  if(dx > dist)
	    continue;
	  dy = NGB_PERIODIC_LONG_Y(P[p].Pos[1] - searchcenter[1]);
	  if(dy > dist)
	    continue;
	  dz = NGB_PERIODIC_LONG_Z(P[p].Pos[2] - searchcenter[2]);
	  if(dz > dist)
	    continue;
	  if(dx * dx + dy * dy + dz * dz > dist * dist)
	    continue;

	  mass += P[p].Mass;
	}
      else
	{
	  if(no >= All.MaxPart + MaxNodes)	/* pseudo particle */
	    {
	      if(mode == 1)
		endrun(12312);

	      if(mode == 0)
		{
		  if(Exportflag[task = DomainTask[no - (All.MaxPart + MaxNodes)]] != target)
		    {
		      Exportflag[task] = target;
		      Exportnodecount[task] = NODELISTLENGTH;
		    }

		  if(Exportnodecount[task] == NODELISTLENGTH)
		    {
		      if(*nexport >= All.BunchSize)
			{
			  *nexport = nexport_save;
			  if(nexport_save == 0)
			    endrun(13005);	/* in this case, the buffer is too small to process even a single particle */
			  for(task = 0; task < NTask; task++)
			    nsend_local[task] = 0;
			  for(no = 0; no < nexport_save; no++)
			    nsend_local[DataIndexTable[no].Task]++;
			  return -1;
			}
		      Exportnodecount[task] = 0;
		      Exportindex[task] = *nexport;
		      DataIndexTable[*nexport].Task = task;
		      DataIndexTable[*nexport].Index = target;
		      DataIndexTable[*nexport].IndexGet = *nexport;
		      *nexport = *nexport + 1;
		      nsend_local[task]++;
		    }

		  DataNodeList[Exportindex[task]].NodeList[Exportnodecount[task]++] =
		    DomainNodeIndex[no - (All.MaxPart + MaxNodes)];

		  if(Exportnodecount[task] < NODELISTLENGTH)
		    DataNodeList[Exportindex[task]].NodeList[Exportnodecount[task]] = -1;
		}

	      no = Nextnode[no - MaxNodes];
	      continue;
	    }

	  current = &Nodes[no];

	  if(mode == 1)
	    {
	      if(current->u.d.bitflags & (1 << BITFLAG_TOPLEVEL))	/* we reached a top-level node again, which means that we are done with the branch */
		{
		  *startnode = -1;
		  return mass;
		}
	    }

	  no = current->u.d.sibling;	/* in case the node can be discarded */
	  dist = hsml + 0.5 * current->len;
	  dx = NGB_PERIODIC_LONG_X(current->center[0] - searchcenter[0]);
	  if(dx > dist)
	    continue;
	  dy = NGB_PERIODIC_LONG_Y(current->center[1] - searchcenter[1]);
	  if(dy > dist)
	    continue;
	  dz = NGB_PERIODIC_LONG_Z(current->center[2] - searchcenter[2]);
	  if(dz > dist)
	    continue;
	  /* now test against the minimal sphere enclosing everything */
	  dist += FACT1 * current->len;
	  if((r2 = (dx * dx + dy * dy + dz * dz)) > dist * dist)
	    continue;

	  if((current->u.d.bitflags & ((1 << BITFLAG_TOPLEVEL) + (1 << BITFLAG_DEPENDS_ON_LOCAL_MASS))) == 0)	/* only use fully local nodes */
	    {
	      /* test whether the node is contained within the sphere */
	      dist = hsml - FACT2 * current->len;
	      if(dist > 0)
		if(r2 < dist * dist)
		  {
		    mass += current->u.d.mass;
		    continue;
		  }
	    }

	  no = current->u.d.nextnode;	/* ok, we need to open the node */
	}
    }

  *startnode = -1;
  return mass;
}

#ifdef SO_VEL_DISPERSIONS

int subfind_overdensity_evaluate_dispersion(int target, int mode, int *nexport, int *nsend_local)
{
  int ngb, n, j, startnode, listindex = 0;
  double h, vx, vy, vz, v2, vvx, vvy, vvz;
  MyDouble *pos;
  double boxsize, boxhalf, vel_to_peculiar, H_of_a;

  boxsize = All.BoxSize;
  boxhalf = 0.5 * All.BoxSize;

  H_of_a = hubble_function(All.Time);
  vel_to_peculiar = 1.0 / All.Time;

  if(mode == 0)
    {
      pos = Group[target].Pos;
      h = R200[target];
    }
  else
    {
      pos = SOdensGet[target].Pos;
      h = SOdensGet[target].R200;
    }

  if(mode == 0)
    {
      startnode = All.MaxPart;	/* root node */
    }
  else
    {
      startnode = SOdensGet[target].NodeList[0];
      startnode = Nodes[startnode].u.d.nextnode;	/* open it */
    }

  vx = vy = vz = v2 = 0;

  while(startnode >= 0)
    {
      while(startnode >= 0)
	{
	  ngb = subfind_ovderdens_treefind_dispersion(pos, h, target, &startnode, mode, nexport, nsend_local);

	  if(ngb < 0)
	    return -1;

	  for(n = 0; n < ngb; n++)
	    {
	      j = Ngblist[n];

	      vvx = vel_to_peculiar * P[j].Vel[0] + H_of_a * All.Time * NEAREST(P[j].Pos[0] - pos[0]);
	      vvy = vel_to_peculiar * P[j].Vel[1] + H_of_a * All.Time * NEAREST(P[j].Pos[1] - pos[1]);
	      vvz = vel_to_peculiar * P[j].Vel[2] + H_of_a * All.Time * NEAREST(P[j].Pos[2] - pos[2]);

	      vx += P[j].Mass * vvx;
	      vy += P[j].Mass * vvy;
	      vz += P[j].Mass * vvz;

	      v2 += P[j].Mass * (vvx * vvx + vvy * vvy + vvz * vvz);
	    }
	}

      if(mode == 1)
	{
	  listindex++;
	  if(listindex < NODELISTLENGTH)
	    {
	      startnode = SOdensGet[target].NodeList[listindex];
	      if(startnode >= 0)
		startnode = Nodes[startnode].u.d.nextnode;	/* open it */
	    }
	}
    }

  if(mode == 0)
    {
      Disp200[target] = v2;
      Vx200[target] = vx;
      Vy200[target] = vy;
      Vz200[target] = vz;
    }
  else
    {
      SOdensResult[target].Disp200 = v2;
      SOdensResult[target].Vx200 = vx;
      SOdensResult[target].Vy200 = vy;
      SOdensResult[target].Vz200 = vz;
    }

  return 0;
}

int subfind_ovderdens_treefind_dispersion(MyDouble searchcenter[3], MyFloat hsml, int target, int *startnode,
					  int mode, int *nexport, int *nsend_local)
{
  int no, p, task, nexport_save, numngb;
  struct NODE *current;
  MyDouble dx, dy, dz, dist, r2;

#define FACT2 0.86602540
#ifdef PERIODIC
  MyDouble xtmp;
#endif
  nexport_save = *nexport;

  numngb = 0;
  no = *startnode;

  while(no >= 0)
    {
      if(no < All.MaxPart)	/* single particle */
	{
	  p = no;
	  no = Nextnode[no];

	  dist = hsml;
	  dx = NGB_PERIODIC_LONG_X(P[p].Pos[0] - searchcenter[0]);
	  if(dx > dist)
	    continue;
	  dy = NGB_PERIODIC_LONG_Y(P[p].Pos[1] - searchcenter[1]);
	  if(dy > dist)
	    continue;
	  dz = NGB_PERIODIC_LONG_Z(P[p].Pos[2] - searchcenter[2]);
	  if(dz > dist)
	    continue;
	  if(dx * dx + dy * dy + dz * dz > dist * dist)
	    continue;

	  Ngblist[numngb++] = p;
	}
      else
	{
	  if(no >= All.MaxPart + MaxNodes)	/* pseudo particle */
	    {
	      if(mode == 1)
		endrun(12312);

	      if(mode == 0)
		{
		  if(Exportflag[task = DomainTask[no - (All.MaxPart + MaxNodes)]] != target)
		    {
		      Exportflag[task] = target;
		      Exportnodecount[task] = NODELISTLENGTH;
		    }

		  if(Exportnodecount[task] == NODELISTLENGTH)
		    {
		      if(*nexport >= All.BunchSize)
			{
			  *nexport = nexport_save;
			  if(nexport_save == 0)
			    endrun(13005);	/* in this case, the buffer is too small to process even a single particle */
			  for(task = 0; task < NTask; task++)
			    nsend_local[task] = 0;
			  for(no = 0; no < nexport_save; no++)
			    nsend_local[DataIndexTable[no].Task]++;
			  return -1;
			}
		      Exportnodecount[task] = 0;
		      Exportindex[task] = *nexport;
		      DataIndexTable[*nexport].Task = task;
		      DataIndexTable[*nexport].Index = target;
		      DataIndexTable[*nexport].IndexGet = *nexport;
		      *nexport = *nexport + 1;
		      nsend_local[task]++;
		    }

		  DataNodeList[Exportindex[task]].NodeList[Exportnodecount[task]++] =
		    DomainNodeIndex[no - (All.MaxPart + MaxNodes)];

		  if(Exportnodecount[task] < NODELISTLENGTH)
		    DataNodeList[Exportindex[task]].NodeList[Exportnodecount[task]] = -1;
		}

	      no = Nextnode[no - MaxNodes];
	      continue;
	    }

	  current = &Nodes[no];

	  if(mode == 1)
	    {
	      if(current->u.d.bitflags & (1 << BITFLAG_TOPLEVEL))	/* we reached a top-level node again, which means that we are done with the branch */
		{
		  *startnode = -1;
		  return numngb;
		}
	    }

	  no = current->u.d.sibling;	/* in case the node can be discarded */
	  dist = hsml + 0.5 * current->len;
	  dx = NGB_PERIODIC_LONG_X(current->center[0] - searchcenter[0]);
	  if(dx > dist)
	    continue;
	  dy = NGB_PERIODIC_LONG_Y(current->center[1] - searchcenter[1]);
	  if(dy > dist)
	    continue;
	  dz = NGB_PERIODIC_LONG_Z(current->center[2] - searchcenter[2]);
	  if(dz > dist)
	    continue;
	  /* now test against the minimal sphere enclosing everything */
	  dist += FACT1 * current->len;
	  if((r2 = (dx * dx + dy * dy + dz * dz)) > dist * dist)
	    continue;

	  no = current->u.d.nextnode;	/* ok, we need to open the node */
	}
    }

  *startnode = -1;
  return numngb;
}

#endif

#endif