#include <algorithm>
#include <iostream>
#include <queue>
#include <vector>
#include <set>
#include <map>
#include <random>
#include <boost/pending/disjoint_sets.hpp>

namespace sublevel {
typedef std::vector<std::vector<int>> Graph;

struct Ans {
    Ans(int e, int d, int es, int ds)
        : eat(e), death(d), eat_size(es), death_size(ds),
        total_height(0.0) {
    }
    int eat;
    int death;
    int eat_size;
    int death_size;
    float total_height;
};

class RootedForest {
public:
    Graph tree_;
    std::vector<int> pred_;
    std::vector<int> height_;

    explicit RootedForest(Graph tree) : tree_(tree) {
    }

    void Initilize() {
        pred_.resize(tree_.size(), -1);
        height_.resize(tree_.size(), -1);
        std::queue<int> que;
        size_t counter = 0;
        while (counter < tree_.size()) {
            if (que.empty()) {
                int next_start = 0;
                while (height_[next_start] != -1) {
                    ++next_start;
                }
                que.push(next_start);
                ++counter;
                height_[next_start] = 0;
            }
            while (!que.empty()) {
                int current = que.front();
                que.pop();
                for (auto neu : tree_[current]) {
                    if (height_[neu] == -1) {
                        height_[neu] = height_[current] + 1;
                        pred_[neu] = current;
                        que.push(neu);
                        ++counter;
                    }
                }
            }
        }
    }

    bool GetWay (int lhs, int rhs, std::vector<int>& answer) {
        std::vector<int> left_way;
        std::vector<int> right_way;
        while (height_[lhs] > height_[rhs]) {
            left_way.push_back(lhs);
            lhs = pred_[lhs];
        }
        while (height_[lhs] < height_[rhs]) {
            right_way.push_back(rhs);
            rhs = pred_[rhs];
        }
        while ((lhs != -1 && rhs != -1) && lhs != rhs) {
            left_way.push_back(lhs);
            lhs = pred_[lhs];
            right_way.push_back(rhs);
            rhs = pred_[rhs];
        }
        if (lhs != -1 && rhs != -1) {
            for (size_t step = 0; step < left_way.size(); ++step) {
                answer.push_back(left_way[step]);
            }
            answer.push_back(lhs);
            for (size_t step = right_way.size(); step > 0; --step) {
                answer.push_back(right_way[step - 1]);
            }
            return true;
        }
        return false;
    }
};

class VirtualCloud {
public:
    float* values_;
    size_t cloud_size_;
    std::vector<int> order_;
    std::set<int> trash_;
    Graph graph_;
    Graph minima_graph_;
    VirtualCloud(float* val, size_t size) : values_(val), cloud_size_(size) {
    }
    virtual ~VirtualCloud() = default;
    void GetOrder() {
        order_.reserve(cloud_size_);
        for (int index = 0; index < static_cast<int>(cloud_size_); ++index) {
            order_.push_back(index);
        }
        std::sort(order_.begin(), order_.end(),
                  [&](int lhs, int rhs) { return (*(values_ + lhs) < *(values_ + rhs) ||
                                                  (*(values_ + lhs) == *(values_ + rhs) && lhs < rhs)); } );
    }

    virtual void GetGraph() {
    }

    virtual void SetGraph(Graph&& gr) {
        graph_ = gr;
    }

    std::map<int, Ans> SublevelHomology() {
        GetGraph();
        minima_graph_.resize(cloud_size_);
        std::map<int, Ans> answer;
        int lenght = graph_.size();
        std::vector<int> back_ord(lenght, 0);
        for (int id = 0; id < lenght; ++id) {
            back_ord[order_[id]] = id;
        }
        std::vector<int> siz(lenght + 1, 0);
        std::vector<int> ord(lenght + 1, 0);
        boost::disjoint_sets<int*,int*> ds(&ord[0], &siz[0]);
        auto comp = [&](int lhs, int rhs){ return back_ord[lhs - 1] < back_ord[rhs - 1]; };
        for (int ind = 0; ind < lenght; ++ind) {
            int vertex = order_[ind];
            if (trash_.find(vertex) != trash_.end()) {
                continue;
            }
            std::map<int, int, decltype(comp)> clusters(comp);
            for (auto neubour : graph_[vertex]) {
                if (trash_.find(neubour) == trash_.end()) {
                    int next_cluster = ds.find_set(neubour + 1);
                    if (clusters.find(next_cluster) == clusters.end()) {
                        clusters.emplace(next_cluster, neubour);
                    } else if (back_ord[neubour] < back_ord[clusters[next_cluster]]) {
                        clusters[next_cluster] = neubour;
                    }
                }
            }
            for (auto& item : clusters) {
                minima_graph_[vertex].push_back(item.second);
                minima_graph_[item.second].push_back(vertex);
            }
            if (clusters.empty()) {
                ds.make_set(vertex + 1);
                answer.emplace(vertex, Ans(-1, -1, -1, 1));
                continue;
            }
            int start = clusters.begin()->first;
            ds.make_set(vertex + 1);
            ds.link(vertex + 1, start);
            ++answer.at(start - 1).death_size;
            answer.at(start - 1).total_height += *(values_ + vertex) - *(values_ + start - 1);
            for (auto it = next(clusters.begin()); it != clusters.end(); ++it) {
                ord[start] = std::max(ord[start], ord[it->first]);
                ds.link(it->first, start);
                answer.at(it->first - 1).eat = start - 1;
                answer.at(it->first - 1).death = vertex;
                answer.at(it->first - 1).eat_size = answer.at(start - 1).death_size;
                answer.at(start - 1).death_size += answer.at(it->first - 1).death_size;
                answer.at(start - 1).total_height += answer.at(it->first - 1).total_height +
                                                     (*(values_ + it->first - 1) - *(values_ + start - 1)) *
                                                     answer.at(it->first - 1).death_size;
            }
        }
        return answer;
    }
};

class GridCloud : public VirtualCloud {
private:
    std::vector<size_t> shape_;
    std::vector<size_t> shift_;

    int GetIndex(int point, int dim) {
        return (point % shift_[dim + 1]) / shift_[dim];
    }

public:
    GridCloud(float* val, size_t size, std::vector<size_t> sh)
        : VirtualCloud(val, size), shape_(sh) {
        shift_.resize(shape_.size() + 1, 1);
        for (size_t id = 1; id < shape_.size() + 1; ++id) {
            shift_[id] = shift_[id - 1] * shape_[id - 1];
        }
    }

    void GetGraph() {
        graph_.resize(cloud_size_);
        for (size_t index = 0; index < cloud_size_; ++index) {
            for (size_t dim = 0; dim < shape_.size(); ++dim) {
                size_t down = index - shift_[dim];
                if (GetIndex(index, dim) != 0 && *(values_ + index) >= *(values_ + down)) {
                    graph_[index].push_back(down);
                }
                size_t up = index + shift_[dim];
                if (GetIndex(up, dim) != 0 && *(values_ + index) > *(values_ + up)) {
                    graph_[index].push_back(up);
                }
            }
        }
    }
};

}


// ---------------------------------------------------------------------
//
// Copyright (c) 2019 - 2021 by the IBAMR developers
// All rights reserved.
//
// This file is part of IBAMR.
//
// IBAMR is free software and is distributed under the 3-clause BSD
// license. The full text of the license can be found in the file
// COPYRIGHT at the top level directory of IBAMR.
//
// ---------------------------------------------------------------------

#include <ibtk/config.h>

#include "InterpolationUtilities.h"
#include "tbox/MathUtilities.h"
#include "tbox/Pointer.h"

IBTK_DISABLE_EXTRA_WARNINGS
#include <Eigen/Core>
#include <Eigen/QR>
IBTK_ENABLE_EXTRA_WARNINGS

#include <HierarchyCellDataOpsReal.h>
#include <Patch.h>
#include <PatchLevel.h>

#include <algorithm>

using Eigen::Matrix3d;
using Eigen::Vector3d;

namespace IBTK
{
double
InterpolationUtilities::interpolate(const vector<double>& X,
                                    const int data_idx,
                                    Pointer<CellVariable<NDIM, double> > Q_var,
                                    Pointer<PatchHierarchy<NDIM> > patch_hierarchy,
                                    const std::vector<RobinBcCoefStrategy<NDIM>*>& bc_coefs,
                                    const double data_time,
                                    const int depth)
{
    double q_val = 0.0;
    VariableDatabase<NDIM>* var_db = VariableDatabase<NDIM>::getDatabase();
    const int data_idx_temp =
        var_db->registerVariableAndContext(Q_var, var_db->getContext("Interpolation"), IntVector<NDIM>(3));
    for (int ln = 0; ln <= patch_hierarchy->getFinestLevelNumber(); ++ln)
    {
        Pointer<PatchLevel<NDIM> > level = patch_hierarchy->getPatchLevel(ln);
        level->allocatePatchData(data_idx_temp);
    }
    HierarchyCellDataOpsReal<NDIM, double> hier_cc_data_ops(patch_hierarchy);
    hier_cc_data_ops.copyData(data_idx_temp, data_idx);
    typedef HierarchyGhostCellInterpolation::InterpolationTransactionComponent InterpolationTransactionComponent;
    std::vector<InterpolationTransactionComponent> ghost_cell_components(1);
    ghost_cell_components[0] = InterpolationTransactionComponent(
        data_idx_temp, "CONSERVATIVE_LINEAR_REFINE", false, "CONSERVATIVE_COARSEN", "LINEAR", false, bc_coefs, NULL);
    HierarchyGhostCellInterpolation ghost_fill_op;
    ghost_fill_op.initializeOperatorState(ghost_cell_components, patch_hierarchy);
    ghost_fill_op.fillData(data_time);
    bool done = false;
    for (int ln = patch_hierarchy->getFinestLevelNumber(); ln >= 0 && !done; --ln)
    {
        // Start at the finest level...
        Pointer<PatchLevel<NDIM> > level = patch_hierarchy->getPatchLevel(ln);
        CellIndex<NDIM> idx = IndexUtilities::getCellIndex(X, level->getGridGeometry(), level->getRatio());
        for (PatchLevel<NDIM>::Iterator p(level); p && !done; p++)
        {
            Pointer<Patch<NDIM> > patch = level->getPatch(p());
            const Pointer<CartesianPatchGeometry<NDIM> > p_geom = patch->getPatchGeometry();
            const double* const dx = p_geom->getDx();
            const double* const x_lower = p_geom->getXLower();
            const Box<NDIM>& patch_box = patch->getBox();
            Pointer<CellData<NDIM, double> > S_data = patch->getPatchData(data_idx_temp);
            if (patch_box.contains(idx))
            {
                // Great. The patch is currently on this level
                // Let's create a box that contains this data
                Box<NDIM> box(idx, idx);
                // Grow it by some number of grid cells
                box.grow(IntVector<NDIM>(3));
                // Loop through the box, make sure the point is located
                // OUTSIDE the disk
                std::vector<double> x(NDIM);
                CellData<NDIM, int> i_data(box, 1, IntVector<NDIM>(0));
                CellData<NDIM, double> si_data(box, NDIM + 1, IntVector<NDIM>(0));
                si_data.fillAll(std::numeric_limits<double>::signaling_NaN());
                const CellIndex<NDIM> ci_l = patch_box.lower();
                int num = 0;
                for (CellIterator<NDIM> i(box); i; i++)
                {
                    CellIndex<NDIM> ci = i();
                    for (int d = 0; d < NDIM; ++d) x[d] = x_lower[d] + dx[d] * (ci(d) - ci_l(d) + 0.5);
                    double r = sqrt(x[0] * x[0] + x[1] * x[1]);
                    if (r > 1.0)
                    {
                        i_data(ci) = 1;
                        si_data(ci, NDIM) = (*S_data)(ci, depth);
                        num++;
                    }
                    else
                    {
                        i_data(ci) = -1;
                    }
                }
                // We have a box containing the point and data the says if we are inside or outside disk.
                // Find directions and interpolate. First in x, then in y.
                std::vector<int> completed_dims;
                q_val = InterpolationUtilities::interpolate_in_boxes(
                    idx, X, i_data, si_data, p_geom, patch_box, 0, 0, completed_dims);
                done = true;
            }
        }
    }
    for (int ln = 0; ln <= patch_hierarchy->getFinestLevelNumber(); ++ln)
    {
        Pointer<PatchLevel<NDIM> > level = patch_hierarchy->getPatchLevel(ln);
        level->deallocatePatchData(data_idx_temp);
    }
    q_val = IBTK_MPI::sumReduction(q_val);
    return q_val;
}

double
InterpolationUtilities::interpolateL2(const std::vector<double>& X,
                                      const int data_idx,
                                      Pointer<CellVariable<NDIM, double> > Q_var,
                                      SAMRAI::tbox::Pointer<SAMRAI::hier::PatchHierarchy<NDIM> > patch_hierarchy,
                                      const std::vector<SAMRAI::solv::RobinBcCoefStrategy<NDIM>*>& bc_coefs,
                                      const double data_time,
                                      const int depth)
{
    double q_val = 0.0;
    VariableDatabase<NDIM>* var_db = VariableDatabase<NDIM>::getDatabase();
    const int data_idx_temp =
        var_db->registerVariableAndContext(Q_var, var_db->getContext("Interpolation"), IntVector<NDIM>(3));
    for (int ln = 0; ln <= patch_hierarchy->getFinestLevelNumber(); ++ln)
    {
        Pointer<PatchLevel<NDIM> > level = patch_hierarchy->getPatchLevel(ln);
        level->allocatePatchData(data_idx_temp);
    }
    HierarchyCellDataOpsReal<NDIM, double> hier_cc_data_ops(patch_hierarchy);
    hier_cc_data_ops.copyData(data_idx_temp, data_idx);
    typedef HierarchyGhostCellInterpolation::InterpolationTransactionComponent InterpolationTransactionComponent;
    std::vector<InterpolationTransactionComponent> ghost_cell_components(1);
    ghost_cell_components[0] = InterpolationTransactionComponent(
        data_idx_temp, "CONSERVATIVE_LINEAR_REFINE", false, "CONSERVATIVE_COARSEN", "LINEAR", false, bc_coefs, NULL);
    HierarchyGhostCellInterpolation ghost_fill_op;
    ghost_fill_op.initializeOperatorState(ghost_cell_components, patch_hierarchy);
    ghost_fill_op.fillData(data_time);
    bool done = false;
    for (int ln = patch_hierarchy->getFinestLevelNumber(); ln >= 0 && !done; --ln)
    {
        // Start at the finest level...
        Pointer<PatchLevel<NDIM> > level = patch_hierarchy->getPatchLevel(ln);
        CellIndex<NDIM> idx = IndexUtilities::getCellIndex(X, level->getGridGeometry(), level->getRatio());
        for (PatchLevel<NDIM>::Iterator p(level); p && !done; p++)
        {
            Pointer<Patch<NDIM> > patch = level->getPatch(p());
            const Pointer<CartesianPatchGeometry<NDIM> > p_geom = patch->getPatchGeometry();
            const double* const dx = p_geom->getDx();
            const double* const x_lower = p_geom->getXLower();
            const Box<NDIM>& patch_box = patch->getBox();
            Pointer<CellData<NDIM, double> > S_data = patch->getPatchData(data_idx_temp);
            if (patch_box.contains(idx))
            {
                // Great. The patch is currently on this level
                // Let's create a box that contains this data
                Box<NDIM> box(idx, idx);
                // Grow it by some number of grid cells
                box.grow(2);
                // Loop through the box, make sure the point is located
                // OUTSIDE the disk
                std::vector<double> x(NDIM);
                CellData<NDIM, int> i_data(box, 1, IntVector<NDIM>(0));
                const CellIndex<NDIM> ci_l = patch_box.lower();
                int num = 0;
                for (CellIterator<NDIM> i(box); i; i++)
                {
                    CellIndex<NDIM> ci = i();
                    for (int d = 0; d < NDIM; ++d) x[d] = x_lower[d] + dx[d] * (ci(d) - ci_l(d) + 0.5);
                    double r = sqrt(x[0] * x[0] + x[1] * x[1]);
                    if (r > 1.0)
                    {
                        i_data(ci) = 1;
                        num++;
                    }
                }
                // Moving least squares
                VectorXd rhs = VectorXd::Zero(6);
                VectorXd soln = VectorXd::Zero(6);
                MatrixXd mat = MatrixXd::Zero(6, 6);
                for (CellIterator<NDIM> i(box); i; i++)
                {
                    CellIndex<NDIM> ci = i();
                    if (i_data(ci) == 1)
                    {
                        for (int d = 0; d < NDIM; ++d) x[d] = x_lower[d] + dx[d] * (ci(d) - ci_l(d) + 0.5);
                        // Fill in RHS and MATRIX values
                        double w = weight_fcn(X, x);
                        double f = (*S_data)(ci, depth);
                        rhs(0) += w * f;
                        rhs(1) += w * f * x[0];
                        rhs(2) += w * f * x[1];
                        rhs(3) += w * f * x[0] * x[0];
                        rhs(4) += w * f * x[1] * x[1];
                        rhs(5) += w * f * x[0] * x[1];
                        mat(0, 0) += w; /*mat(0,1) += x[0]*w; mat(0,2) += x[1]*w; mat(0,3) += x[0]*x[0]*w; mat(0,4) +=
                                           x[1]*x[1]*w; mat(0,5) += x[0]*x[1]*w;*/
                        mat(1, 0) += x[0] * w;
                        mat(1, 1) += x[0] * x[0] * w; /* mat(1,2) += x[0]*x[1]*w; mat(2,3) += x[0]*x[0]*x[0]*w; mat(2,4)
                                                         += x[0]*x[1]*x[1]*w; mat(3,5) += x[0]*x[0]*x[1]*w;*/
                        mat(2, 0) += x[1] * w;
                        mat(2, 1) += x[1] * x[0] * w;
                        mat(2, 2) += x[1] * x[1] * w;
                        mat(3, 0) += x[0] * x[0] * w;
                        mat(3, 1) += x[0] * x[0] * x[0] * w;
                        mat(3, 2) += x[0] * x[0] * x[1] * w;
                        mat(3, 3) += x[0] * x[0] * x[0] * x[0] * w;
                        mat(4, 0) += x[1] * x[1] * w;
                        mat(4, 1) += x[1] * x[1] * x[0] * w;
                        mat(4, 2) += x[1] * x[1] * x[1] * w;
                        mat(4, 3) += x[1] * x[1] * x[0] * x[0] * w;
                        mat(4, 4) += x[1] * x[1] * x[1] * x[1] * w;
                        mat(5, 0) += x[0] * x[1] * w;
                        mat(5, 1) += x[0] * x[1] * x[0] * w;
                        mat(5, 2) += x[0] * x[1] * x[1] * w;
                        mat(5, 3) += x[0] * x[1] * x[0] * x[0] * w;
                        mat(5, 4) += x[0] * x[1] * x[1] * x[1] * w;
                        mat(5, 5) += x[0] * x[1] * x[0] * x[1] * w;
                    }
                }
                //                mat(0,1) = mat(1,0); mat(0,2) = mat(2,0); mat(0,3) = mat(3,0); mat(0,4) = mat(4,0);
                //                mat(0,5) = mat(5,0); mat(1,2) = mat(2,1); mat(1,3) = mat(3,1); mat(1,4) = mat(4,1);
                //                mat(1,5) = mat(5,1); mat(2,3) = mat(3,2); mat(2,4) = mat(4,2); mat(2,5) = mat(5,2);
                //                mat(3,4) = mat(4,3); mat(3,5) = mat(5,3);
                //                mat(4,5) = mat(5,4);
                soln = mat.ldlt().solve(rhs);
                q_val = soln(0) + soln(1) * X[0] + soln(2) * X[1] + soln(3) * X[0] * X[0] + soln(4) * X[1] * X[1] +
                        soln(5) * X[0] * X[1];
                done = true;
            }
        }
    }
    q_val = IBTK_MPI::sumReduction(q_val);

    for (int ln = 0; ln <= patch_hierarchy->getFinestLevelNumber(); ++ln)
    {
        Pointer<PatchLevel<NDIM> > level = patch_hierarchy->getPatchLevel(ln);
        level->deallocatePatchData(data_idx_temp);
    }
    return q_val;
}

double
InterpolationUtilities::weight_fcn(const std::vector<double>& x, const std::vector<double>& x_j)
{
    double r = sqrt((x[0] - x_j[0]) * (x[0] - x_j[0]) + (x[1] - x_j[1]) * (x[1] - x_j[1]));
    double w = exp(-10.0 * r * r);
    w = 1.0;
    return w;
}

double
InterpolationUtilities::interpolate(const double& l, const std::vector<int>& i, const std::vector<double>& y)
{
    double val = 0.0;
    TBOX_ASSERT(i.size() == 3);
    TBOX_ASSERT(y.size() == i.size());
    val = y[0] + (y[1] - y[0]) * l / static_cast<double>(i[1] - i[0]) +
          (y[2] * static_cast<double>(i[0] - i[1]) + y[0] * static_cast<double>(i[1] - i[2]) +
           y[1] * static_cast<double>(i[2] - i[0])) *
              l * (l - 1.0) / (static_cast<double>((i[0] - i[1]) * (i[0] - i[2]) * (i[1] - i[2])));
    return val;
}

double
InterpolationUtilities::interpolate_in_boxes(const CellIndex<NDIM>& idx,
                                             const std::vector<double>& X,
                                             CellData<NDIM, int>& r_data,
                                             CellData<NDIM, double>& q_data,
                                             Pointer<CartesianPatchGeometry<NDIM> > pgeom,
                                             const Box<NDIM>& pbox,
                                             int dim,
                                             int cycle,
                                             std::vector<int>& completed_dims)
{
    double q_val = 0.0;
    // form list of indices.
    std::vector<CellIndex<NDIM> > idx_list;
    std::vector<int> i_list;
    bool done = false;
    while (!done)
    {
        idx_list.clear();
        i_list.clear();
        dim = dim % NDIM;
        if (r_data(idx) == 1)
        {
            idx_list.push_back(idx);
            i_list.push_back(0);
        }
        int s = 1;
        while (idx_list.size() < 3)
        {
            IntVector<NDIM> si(0);
            si(dim) = s;
            if (r_data(idx + si) == 1)
            {
                idx_list.push_back(idx + si);
                i_list.push_back(s);
            }
            if (r_data(idx - si) == 1)
            {
                idx_list.push_back(idx - si);
                i_list.push_back(-s);
            }
            s++;
        }
        int min_val = *std::min_element(i_list.begin(), i_list.end());
        int max_val = *std::max_element(i_list.begin(), i_list.end());
        if (std::max(std::abs(min_val), std::abs(max_val)) > 3)
        {
            dim++;
            if (std::find(completed_dims.begin(), completed_dims.end(), dim) != completed_dims.end() || dim > NDIM)
            {
                TBOX_ERROR("already completed dimension, or dimension too high");
            }
        }
        else
        {
            done = true;
        }
    }
    while (idx_list.size() > 3)
    {
        idx_list.pop_back();
    }
    completed_dims.push_back(dim);
    // We start at dim = 0. Each call generates a new box to interpolate inside of, and increases the dim by 1.
    if (cycle < NDIM - 1)
    {
        std::vector<int> i_list;
        std::vector<double> y_data;
        for (std::vector<CellIndex<NDIM> >::const_iterator cit = idx_list.begin(); cit != idx_list.end(); ++cit)
        {
            const CellIndex<NDIM>& cidx = *cit;
            q_data(cidx, cycle + 1) = InterpolationUtilities::interpolate_in_boxes(
                cidx, X, r_data, q_data, pgeom, pbox, dim + 1, cycle + 1, completed_dims);
            i_list.push_back(cidx(dim));
            y_data.push_back(q_data(cidx, cycle + 1));
        }
        const double* dx = pgeom->getDx();
        const double* xlow = pgeom->getXLower();
        const CellIndex<NDIM>& idxl = pbox.lower();
        double xx = xlow[dim] + dx[dim] * (idx_list[0](dim) - idxl(dim) + 0.5);
        q_val = InterpolationUtilities::interpolate((X[dim] - xx) / dx[dim], i_list, y_data);
    }
    else if (cycle == NDIM - 1)
    {
        std::vector<int> i_list;
        std::vector<double> y_data;
        for (std::vector<CellIndex<NDIM> >::const_iterator cit = idx_list.begin(); cit != idx_list.end(); ++cit)
        {
            const CellIndex<NDIM>& cidx = *cit;
            i_list.push_back(cidx(dim));
            y_data.push_back(q_data(cidx, cycle + 1));
        }
        const double* dx = pgeom->getDx();
        const double* xlow = pgeom->getXLower();
        const CellIndex<NDIM>& idxl = pbox.lower();
        double xx = xlow[dim] + dx[dim] * (idx_list[0](dim) - idxl(dim) + 0.5);
        q_val = InterpolationUtilities::interpolate((X[dim] - xx) / dx[dim], i_list, y_data);
    }
    else
    {
        // Shouldn't get here.
        TBOX_ERROR("Invalid dimension.\n");
    }
    return q_val;
}
} // namespace IBTK


#include <boost/version.hpp>
#if BOOST_VERSION < 106300 && BOOST_VERSION >= 106200
// Boost 1.62 is missing an iostream include...
#include <iostream>
#endif
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/pointing_segment.hpp>
#include <boost/geometry/index/rtree.hpp>

#include "lanelet2_core/geometry/LineString.h"
#include "lanelet2_core/geometry/Polygon.h"

namespace lanelet {
namespace geometry {
namespace {
using V3d = BasicPoint3d;

struct LineParams {
  double sN;
  double sD;
  double tN;
  double tD;
};

constexpr double SmallNum = 1.e-10;

inline LineParams calculateLineParams(double a, double b, double c, double d, double e, double den) {
  // compute the line parameters of the two closest points
  if (den < SmallNum) {  // the lines are almost parallel
    // force using point P0 on segment S1
    // to prevent possible division by 0.0 later
    return {0.0, 1.0, e, c};
  }
  LineParams lp{};
  lp.sD = den;
  lp.tD = den;
  // get the closest points on the infinite lines
  lp.sN = (b * e - c * d);
  lp.tN = (a * e - b * d);
  if (lp.sN < 0.0) {  // sc < 0 => the s=0 edge is visible
    lp.sN = 0.0;
    lp.tN = e;
    lp.tD = c;
  } else if (lp.sN > lp.sD) {  // sc > 1  => the s=1 edge is visible
    lp.sN = lp.sD;
    lp.tN = e + b;
    lp.tD = c;
  }

  return lp;
}

std::pair<BasicPoint3d, BasicPoint3d> projectedPoint3d(const BasicPoint3d& p1, const BasicPoint3d& p2,
                                                       const BasicPoint3d& q1, const BasicPoint3d& q2) {
  // see http://geomalgorithms.com/a07-_distance.html
  V3d w = p1 - q1;
  V3d u = p2 - p1;
  V3d v = q2 - q1;
  double a = u.dot(u);
  double b = u.dot(v);
  double c = v.dot(v);
  double d = u.dot(w);
  double e = v.dot(w);
  auto den = a * c - b * b;

  auto lp = calculateLineParams(a, b, c, d, e, den);

  if (lp.tN < 0.0) {  // tc < 0 => the t=0 edge is visible
    lp.tN = 0.0;
    // recompute sc for this edge
    if (-d < 0.0) {
      lp.sN = 0.0;
    } else if (-d > a) {
      lp.sN = lp.sD;
    } else {
      lp.sN = -d;
      lp.sD = a;
    }
  } else if (lp.tN > lp.tD) {  // tc > 1  => the t=1 edge is visible
    lp.tN = lp.tD;
    // recompute sc for this edge
    if ((-d + b) < 0.0) {
      lp.sN = 0;
    } else if ((-d + b) > a) {
      lp.sN = lp.sD;
    } else {
      lp.sN = (-d + b);
      lp.sD = a;
    }
  }
  // finally do the division to get sc and tc
  double sc = (std::abs(lp.sN) < SmallNum ? 0.0 : lp.sN / lp.sD);
  double tc = (std::abs(lp.tN) < SmallNum ? 0.0 : lp.tN / lp.tD);

  return {p1 + (sc * u), q1 + (tc * v)};  // return the closest distance
}

namespace bg = boost::geometry;
namespace bgi = bg::index;
namespace bgm = boost::geometry::model;
using BasicSegment = bgm::pointing_segment<const BasicPoint3d>;
using Box = bgm::box<bgm::point<double, 3, boost::geometry::cs::cartesian>>;
using Node = std::pair<Box, BasicSegment>;
using RTree = bgi::rtree<Node, bgi::linear<8>>;

template <typename LineString1T, typename LineString2T>
std::pair<BasicPoint3d, BasicPoint3d> projectedPoint3dOrdered(const LineString1T& smallerRange,
                                                              const LineString2T& greaterRange) {
  // catch some degerated cases
  ConstHybridLineString3d duplicate;
  if (smallerRange.size() == 1 && greaterRange.size() == 1) {
    std::pair<BasicPoint3d, BasicPoint3d> ret(greaterRange.front(), smallerRange.front());
    return ret;
  }
  auto values =
      utils::transform(bg::segments_begin(greaterRange), bg::segments_end(greaterRange), [](const auto& segm) {
        Box box;
        boost::geometry::envelope(segm, box);
        return Node(box, segm);
      });
  RTree tree(values.begin(), values.end());

  bool first = true;
  double dMin{};
  std::pair<BasicPoint3d, BasicPoint3d> closestPair;
  for (auto it = bg::segments_begin(smallerRange); it != bg::segments_end(smallerRange); ++it) {
    Box queryBox;
    bg::envelope(*it, queryBox);
    for (auto qIt = tree.qbegin(bgi::nearest(queryBox, unsigned(greaterRange.size()))); qIt != tree.qend();
         ++qIt, first = false) {
      const auto& nearest = *qIt;
      auto dBox = boost::geometry::distance(nearest.first, queryBox);
      if (!first && dBox > dMin) {
        break;
      }
      auto projPair = projectedPoint3d(*nearest.second.first, *nearest.second.second, *it->first, *it->second);
      auto d = (projPair.first - projPair.second).norm();
      if (first || d < dMin) {
        closestPair = projPair;
        dMin = d;
      }
    }
  }
  return closestPair;
}

template <typename LineString1T, typename LineString2T>
std::pair<BasicPoint3d, BasicPoint3d> projectedPoint3dImpl(const LineString1T& l1, const LineString2T& l2) {
  if (l1.size() < l2.size()) {
    return projectedPoint3dOrdered(l1, l2);
  }
  auto res = projectedPoint3dOrdered(l2, l1);
  return {res.second, res.first};
}
}  // namespace

namespace internal {
std::pair<BasicPoint3d, BasicPoint3d> projectedPoint3d(const CompoundHybridLineString3d& l1,
                                                       const CompoundHybridLineString3d& l2) {
  return projectedPoint3dImpl(l1, l2);
}

std::pair<BasicPoint3d, BasicPoint3d> projectedPoint3d(const ConstHybridLineString3d& l1,
                                                       const CompoundHybridLineString3d& l2) {
  return projectedPoint3dImpl(l1, l2);
}

std::pair<BasicPoint3d, BasicPoint3d> projectedPoint3d(const CompoundHybridLineString3d& l1,
                                                       const ConstHybridLineString3d& l2) {
  return projectedPoint3dImpl(l1, l2);
}

std::pair<BasicPoint3d, BasicPoint3d> projectedPoint3d(const ConstHybridLineString3d& l1,
                                                       const ConstHybridLineString3d& l2) {
  return projectedPoint3dImpl(l1, l2);
}

std::pair<BasicPoint3d, BasicPoint3d> projectedPoint3d(const ConstHybridLineString3d& l1, const BasicLineString3d& l2) {
  return projectedPoint3dImpl(l1, l2);
}

std::pair<BasicPoint3d, BasicPoint3d> projectedPoint3d(const BasicLineString3d& l1, const ConstHybridLineString3d& l2) {
  return projectedPoint3dImpl(l1, l2);
}

std::pair<BasicPoint3d, BasicPoint3d> projectedPoint3d(const BasicLineString3d& l1, const BasicLineString3d& l2) {
  return projectedPoint3dImpl(l1, l2);
}

std::pair<BasicPoint3d, BasicPoint3d> projectedBorderPoint3d(const ConstHybridPolygon3d& l1,
                                                             const ConstHybridPolygon3d& l2) {
  return projectedPoint3dImpl(l1, l2);
}

std::pair<BasicPoint3d, BasicPoint3d> projectedBorderPoint3d(const CompoundHybridPolygon3d& l1,
                                                             const CompoundHybridPolygon3d& l2) {
  return projectedPoint3dImpl(l1, l2);  // NOLINT
}

}  // namespace internal

Segment<BasicPoint2d> closestSegment(const BasicLineString2d& lineString, const BasicPoint2d& pointToProject) {
  helper::ProjectedPoint<BasicPoint2d> projectedPoint;
  distance(lineString, pointToProject, projectedPoint);
  return Segment<BasicPoint2d>(projectedPoint.result->segmentPoint1, projectedPoint.result->segmentPoint2);
}
Segment<BasicPoint3d> closestSegment(const BasicLineString3d& lineString, const BasicPoint3d& pointToProject) {
  helper::ProjectedPoint<BasicPoint3d> projectedPoint;
  distance(lineString, pointToProject, projectedPoint);
  return Segment<BasicPoint3d>(projectedPoint.result->segmentPoint1, projectedPoint.result->segmentPoint2);
}
}  // namespace geometry
}  // namespace lanelet


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.
// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// 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)

#ifndef BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_GEOMETRIES_QUATERNION_HPP
#define BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_GEOMETRIES_QUATERNION_HPP

#include <cstddef>

#include <boost/geometry/extensions/algebra/core/tags.hpp>
#include <boost/geometry/extensions/algebra/geometries/concepts/quaternion_concept.hpp>

// WARNING!
// It is probable that the sequence of coordinate will change in the future
// at the beginning there would be xyz, w would become the last coordinate

namespace boost { namespace geometry
{

namespace model
{

template <typename T>
class quaternion
{
    BOOST_CONCEPT_ASSERT( (concepts::Quaternion<quaternion>) );

public:

    /// @brief Default constructor, no initialization
    inline quaternion()
    {}

    /// @brief Constructor to set components
    inline quaternion(T const& w, T const& x, T const& y, T const& z)
    {
        m_values[0] = w;
        m_values[1] = x;
        m_values[2] = y;
        m_values[3] = z;
    }

    /// @brief Get a coordinate
    /// @tparam K coordinate to get
    /// @return the coordinate
    template <std::size_t K>
    inline T const& get() const
    {
        BOOST_STATIC_ASSERT(K < 4);
        return m_values[K];
    }

    /// @brief Set a coordinate
    /// @tparam K coordinate to set
    /// @param value value to set
    template <std::size_t K>
    inline void set(T const& value)
    {
        BOOST_STATIC_ASSERT(K < 4);
        m_values[K] = value;
    }

private:

    T m_values[4];
};

} // namespace model

#ifndef DOXYGEN_NO_TRAITS_SPECIALIZATIONS
namespace traits
{

template <typename CoordinateType>
struct tag<model::quaternion<CoordinateType> >
{
    typedef quaternion_tag type;
};

template <typename CoordinateType>
struct coordinate_type<model::quaternion<CoordinateType> >
{
    typedef CoordinateType type;
};

//template <typename CoordinateType>
//struct coordinate_system<model::quaternion<CoordinateType> >
//{
//    typedef cs::cartesian type;
//};

template <typename CoordinateType>
struct dimension<model::quaternion<CoordinateType> >
    : boost::integral_constant<std::size_t, 4>
{};

template<typename CoordinateType, std::size_t Dimension>
struct access<model::quaternion<CoordinateType>, Dimension>
{
    static inline CoordinateType get(
        model::quaternion<CoordinateType> const& v)
    {
        return v.template get<Dimension>();
    }

    static inline void set(
        model::quaternion<CoordinateType> & v,
        CoordinateType const& value)
    {
        v.template set<Dimension>(value);
    }
};

} // namespace traits
#endif // DOXYGEN_NO_TRAITS_SPECIALIZATIONS

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_GEOMETRIES_QUATERNION_HPP


#include <iostream>
#include <vector>
#include <cassert>
#include <cstring>
#include <limits>
#include <mpi.h>
//#include <Eigen/Eigenvalues>

enum KernelType {CubicSpline = 0, WendlandC2 = 1, WendlandC4 = 2};

#include "particle_simulator.hpp"
#include "hdr_time.hpp"
#include "hdr_run.hpp"
#ifdef USE_INTRINSICS
#include "vector_x86.hpp"
#endif
#include "hdr_dimension.hpp"
#include "hdr_kernel.hpp"
#include "hdr_sph.hpp"
#include "hdr_hgas.hpp"
#include "hdr_bhns.hpp"

class SPHAnalysis : public HelmholtzGas {
public:
    SPHAnalysis() {
        this->id    = 0;
        this->istar = 0;
        this->mass  = 0.;
        this->pos   = 0.;
        this->vel   = 0.;
        this->uene  = 0.;
        this->alph  = 0.;
        this->alphu = 0.;
        this->ksr   = 0.;
        for(PS::S32 k = 0; k < NR::NumberOfNucleon; k++) {
            this->cmps[k] = 0.;
        }
    }

    PS::F64 calcRadialVelocity() {
        PS::F64 r2 = this->pos * this->pos;
        PS::F64 rv = this->pos * this->vel;
        return (rv / sqrt(r2));
    }

    void readAscii(FILE * fp) {
        fscanf(fp, "%lld%lld%lf", &this->id, &this->istar, &this->mass);      //  3
        fscanf(fp, "%lf%lf%lf", &this->pos[0], &this->pos[1], &this->pos[2]); //  6
        fscanf(fp, "%lf%lf%lf", &this->vel[0], &this->vel[1], &this->vel[2]); //  9
        fscanf(fp, "%lf%lf%lf", &this->acc[0], &this->acc[1], &this->acc[2]); // 12
        fscanf(fp, "%lf%lf%lf", &this->uene, &this->alph, &this->alphu);      // 15
        fscanf(fp, "%lf%lf%6d", &this->dens, &this->ksr,  &this->np);         // 18
        fscanf(fp, "%lf%lf%lf", &this->vsnd, &this->pres, &this->temp);       // 21
        fscanf(fp, "%lf%lf%lf", &this->divv, &this->rotv, &this->bswt);       // 24
        fscanf(fp, "%lf%lf%lf", &this->pot,  &this->abar, &this->zbar);       // 27
        fscanf(fp, "%lf",       &this->enuc);                                 // 28
        fscanf(fp, "%lf%lf%lf", &this->vsmx, &this->udot, &this->dnuc);       // 31
        for(PS::S32 k = 0; k < NuclearReaction::NumberOfNucleon; k++) {       // 32 -- 44
            fscanf(fp, "%lf", &this->cmps[k]);
        }
        fscanf(fp, "%lf", &this->pot3);
        fscanf(fp, "%lf%lf%lf", &this->tempmax[0], &this->tempmax[1], &this->tempmax[2]);
        fscanf(fp, "%lf", &this->entr);
    }

};

class Shell {
public:
    PS::S64 nump; // The number of particles in the shell
    PS::F64 mass; // The total mass in the shell
    PS::F64 ener; // The total internal energy in the shell
    PS::F64 rvel; // Radial velocity in the shell
    NR::Nucleon mele;

    Shell() {
        this->nump = 0;
        this->mass = 0.;
        this->ener = 0.;
        this->rvel = 0.;
        for(PS::S64 k = 0; k < NR::NumberOfNucleon; k++) {
            this->mele[k] = 0.;
        }
    }

    void increment(SPHAnalysis & sph) {
        this->nump += 1;
        this->mass += sph.mass;
        this->ener += sph.mass * sph.uene;
        this->rvel += sph.calcRadialVelocity();
        for(PS::S64 k = 0; k < NR::NumberOfNucleon; k++) {
            this->mele[k] += sph.mass * sph.cmps[k];
        }
    }

    void reduceAddition(Shell sloc) {
        this->nump = PS::Comm::getSum(sloc.nump);
        this->mass = PS::Comm::getSum(sloc.mass);
        this->ener = PS::Comm::getSum(sloc.ener);
        this->rvel = PS::Comm::getSum(sloc.rvel);
        for(PS::S64 k = 0; k < NR::NumberOfNucleon; k++) {
            this->mele[k] = PS::Comm::getSum(sloc.mele[k]);
        }
    }

};

template <class Tsph>
void sphericalizeWhiteDwarf(PS::F64 rmax,
                            PS::F64 drad,
                            char * ofile,
                            Tsph & sph) {

    PS::F64vec cntr_loc = 0.;
    PS::F64vec cvel_loc = 0.;
    PS::F64    dens_loc = 0.;
    for(PS::S64 i = 0; i < sph.getNumberOfParticleLocal(); i++) {
        dens_loc += sph[i].dens;
        cntr_loc += sph[i].dens * sph[i].pos;
        cvel_loc += sph[i].dens * sph[i].vel;
    }
    PS::F64vec cntr_glb = PS::Comm::getSum(cntr_loc);
    PS::F64vec cvel_glb = PS::Comm::getSum(cvel_loc);
    PS::F64    dens_glb = PS::Comm::getSum(dens_loc);
    cntr_glb = (1. / dens_glb) * cntr_glb;
    cvel_glb = (1. / dens_glb) * cvel_glb;

    PS::S64 nbin = (PS::S64)(rmax / drad) + 1;
    Shell * sloc = (Shell *)malloc(sizeof(Shell) * nbin);

    for(PS::S64 i = 0; i < sph.getNumberOfParticleLocal(); i++) {
        sph[i].pos = sph[i].pos - cntr_glb;
        sph[i].vel = sph[i].vel - cvel_glb;
        PS::F64 rad1  = sqrt(sph[i].pos * sph[i].pos);
        PS::S64 ibin = (PS::S64)(rad1 / drad);
        if(ibin >= nbin) {
            continue;
        }
        ///////////// Ad hoc method //////////////
        if(sph[i].istar == 0 && sph[i].cmps[1] > 0.2 && rad1 < 3e10) {
            for(PS::S64 k = 0; k < NR::NumberOfNucleon; k++) {
                if(k != 12) {
                    sph[i].cmps[k] = 0.;
                } else {
                    sph[i].cmps[k] = 1.;
                }
            }
            //continue;
        }
        //////////////////////////////////////////
        sloc[ibin].increment(sph[i]);
    }

    Shell * sglb = (Shell *)malloc(sizeof(Shell) * nbin);
    for(PS::S64 ibin = 0; ibin < nbin; ibin++) {
        sglb[ibin].reduceAddition(sloc[ibin]);
    }

    if(PS::Comm::getRank() == 0) {
        PS::S64 nshl = 0;
        PS::F64 mass = 0.;
        PS::F64 ener = 0.;
        PS::F64 rvel = 0.;
        PS::F64 rad0 = 0.;
        PS::F64 menc = 0.;
        NR::Nucleon mele;
        FILE * fp = fopen(ofile, "w");
        for(PS::S64 ibin = 0; ibin < nbin; ibin++) {
            nshl += sglb[ibin].nump;
            mass += sglb[ibin].mass;
            ener += sglb[ibin].ener;
            rvel += sglb[ibin].rvel;
            for(PS::S64 k = 0; k < NR::NumberOfNucleon; k++) {
                mele[k] += sglb[ibin].mele[k];
            }
            if(nshl < 10000 && ibin != nbin - 1) {
            //if(nshl < 1000 && ibin != nbin - 1) {
                continue;
            }

            PS::F64 rad1   = drad * (ibin + 1);
            PS::F64 radius = ((rad0 != 0.) ? sqrt(rad0 * rad1) : 0.5 * rad1);
            PS::F64 volume = 4. * M_PI / 3. * (rad1 * rad1 * rad1 - rad0 * rad0 * rad0);
            PS::F64 dens   = mass / volume;
            PS::F64 uene   = ener / mass;
            rvel  = rvel / (PS::S64)nshl;
            NR::Nucleon cmps;
            for(PS::S64 k = 0; k < NR::NumberOfNucleon; k++) {
                cmps[k] = mele[k] / mass;
            }
            menc += mass;

            PS::F64 tin = 1e9;
            PS::F64 pout, cout, tout, sout, uout;
            flash_helmholtz_(&dens, &uene, &tin, cmps.getPointer(), &pout, &cout, &tout, &sout);
            tout = (tout < 1e7) ? 1e7 : tout;
            flash_helmholtz_e_(&dens, &tout, cmps.getPointer(), &uout);
            flash_helmholtz_(&dens, &uout, &tout, cmps.getPointer(), &pout, &cout, &tout, &sout);
            fprintf(fp, "%+e %+e %+e %+e %+e %+e %+e",
                    rad1, dens, pout, tout, uout, menc/CodeUnit::SolarMass, rvel);
            for(PS::S64 k = 0; k < NR::NumberOfNucleon; k++) {
                fprintf(fp, " %+e", cmps[k]);
            }
            fprintf(fp, " %+e", sout);
            fprintf(fp, "\n");

            nshl = 0;
            mass = 0.;
            ener = 0.;
            rvel = 0.;
            for(PS::S64 k = 0; k < NR::NumberOfNucleon; k++) {
                mele[k] = 0.;
            }
            rad0 = rad1;
        }
        fclose(fp);
    }

    free(sloc);
    free(sglb);

}

int main(int argc, char ** argv) {
    PS::Initialize(argc, argv);

    PS::ParticleSystem<SPHAnalysis> sph;
    sph.initialize();
    sph.createParticle(0);
    sph.setNumberOfParticleLocal(0);

    char idir[1024], odir[1024];
    PS::F64 rmax, drad;
    PS::S64 ibgn, iend;
    FILE * fp = fopen(argv[1], "r");
    fscanf(fp, "%s", idir);
    fscanf(fp, "%s", odir);
    fscanf(fp, "%lf%lf", &rmax, &drad);
    fscanf(fp, "%lld%lld", &ibgn, &iend);
    fclose(fp);

    init_flash_helmholtz_(&CodeUnit::FractionOfCoulombCorrection);

    for(PS::S64 itime = ibgn; itime <= iend; itime++) {
        char tfile[1024];
        FILE *fp = NULL;
        PS::S64 tdir = 0;
        for(PS::S64 iidir = 0; iidir < 100; iidir++) {
            sprintf(tfile, "%s/t%02d/sph_t%04d_p%06d_i%06d.dat", idir, iidir, itime,
                    PS::Comm::getNumberOfProc(), 0);
            fp = fopen(tfile, "r");
            if(fp != NULL) {
                tdir = iidir;
                break;
            }
        }
        if(fp == NULL) {
            fprintf(stderr, "%s is not found.\n", tfile);
            continue;
        }
        fclose(fp);

        char sfile[1024];
        sprintf(sfile, "%s/t%02d/sph_t%04d", idir, tdir, itime);
        sph.readParticleAscii(sfile, "%s_p%06d_i%06d.dat");

        char ofile[1024];
        sprintf(ofile, "%s/poly.dat", odir);
        sphericalizeWhiteDwarf(rmax, drad, ofile, sph);

    }

    PS::Finalize();

    return 0;
}


/*
// Copyright 2018 University of Liege
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Authors:
// - Adrien Crovato
*/

//// Field derivatives mapping
// Identify cells for which spatial derivatives should be prevented so that surface panel crossing does not occur
// Results are stored in boolean matrices: row(f) = [dx-, dx+, dy-, dy+, dz-, dz+]
//
// I/O:
// - sGrid: temporary dynamic array containing body panel vertices
// - numC: list of numerical parameters (structure)
// - bPan: body panels (structure)
// - bPan: wake panels (structure)
// - fPan: field panels (structure)

#include <iostream>
#include <Eigen/Dense>
#include "map_derivatives.h"
#include "cast_ray_pip.h"

using namespace std;
using namespace Eigen;

void map_derivatives(MatrixX3d &sGrid, Numerical_CST &numC, Network &bPan, Network &wPan, Field &fPan) {

    //// Begin
    // Variable definition
    int allow = 0;
    double minX, minY, maxY, minZ, maxZ; // minimal bounding box holding the geometry
    Vector3d u, w;
    // Display check
    cout << "Mapping field derivatives... " << flush;
    // Resize field and network matrices
    fPan.fbdMap.resize(fPan.nF,6);
    fPan.fwdMap.resize(fPan.nF,6);
    // Define box enclosing the body
    minX = sGrid.col(0).minCoeff() - numC.TOLB;
    minY = sGrid.col(1).minCoeff() - numC.TOLB;
    maxY = sGrid.col(1).maxCoeff() + numC.TOLB;
    minZ = sGrid.col(2).minCoeff() - numC.TOLB;
    maxZ = sGrid.col(2).maxCoeff() + numC.TOLB;

    //// Map field derivatives
    int f = 0;
    for (int j = 0; j < fPan.nY; j++) {
        for (int k = 0; k < fPan.nZ; k++) {
            for (int i = 0; i < fPan.nX; i++) {

                // If cell is inside the body, derivatives are prevented
                if (!fPan.fMap(f)) {
                    fPan.fbdMap(f, 0) = 0;
                    fPan.fbdMap(f, 1) = 0;
                    fPan.fbdMap(f, 2) = 0;
                    fPan.fbdMap(f, 3) = 0;
                    fPan.fbdMap(f, 4) = 0;
                    fPan.fbdMap(f, 5) = 0;
                    fPan.fwdMap(f, 0) = 0;
                    fPan.fwdMap(f, 1) = 0;
                    fPan.fwdMap(f, 2) = 0;
                    fPan.fwdMap(f, 3) = 0;
                    fPan.fwdMap(f, 4) = 0;
                    fPan.fwdMap(f, 5) = 0;
                }
                else {
                    /// 1. X-backward
                    // Check if cell is not on the domain border
                    if (i == 0) {
                        fPan.fbdMap(f, 0) = 0;
                        fPan.fwdMap(f, 0) = 0;
                    }
                    // Create a box enclosing the geometry, based on cell dimensions to restrict the algorithm complexity
                    else if ((fPan.CG(f,0) < minX)
                             || (fPan.CG(f,1) < minY || fPan.CG(f,1) > maxY)
                             || (fPan.CG(f,2) < minZ || fPan.CG(f,2) > maxZ)) {
                        fPan.fbdMap(f, 0) = 1;
                        fPan.fwdMap(f, 0) = 1;
                    }
                    // Check if previous cell is not an internal cell
                    else if (!fPan.fMap(f-1)) {
                        fPan.fbdMap(f, 0) = 0;
                        fPan.fwdMap(f, 0) = 0;
                    }
                    // Create a vector joining the center of two adjacent cells and check if that vector crosses a panel
                    else {
                        u = fPan.CG.row(f-1).transpose() - fPan.CG.row(f).transpose();
                        // Body panel
                        for (int p = 0; p < bPan.nP; p++) {
                            w = fPan.CG.row(f).transpose() - bPan.CG.row(p).transpose();

                            allow = cast_ray_pip(u(0),u(1),u(2),w(0),w(1),w(2),fPan.CG(f,0),fPan.CG(f,1),fPan.CG(f,2),
                                                 bPan.n(p,0),bPan.n(p,1),bPan.n(p,2),bPan.v0(p,0),bPan.v0(p,1),bPan.v0(p,2),
                                                 bPan.v1(p,0),bPan.v1(p,1),bPan.v1(p,2),bPan.v2(p,0),bPan.v2(p,1),bPan.v2(p,2),
                                                 bPan.v3(p,0),bPan.v3(p,1),bPan.v3(p,2));
                            if (!allow)
                                break;
                        }
                        fPan.fbdMap(f, 0) = allow; // Store the result in a boolean matrix
                        // Wake panel
                        for (int p = 0; p < wPan.nP; p++) {
                            w = fPan.CG.row(f).transpose() - wPan.CG.row(p).transpose();

                            allow = cast_ray_pip(u(0),u(1),u(2),w(0),w(1),w(2),fPan.CG(f,0),fPan.CG(f,1),fPan.CG(f,2),
                                                 wPan.n(p,0),wPan.n(p,1),wPan.n(p,2),wPan.v0(p,0),wPan.v0(p,1),wPan.v0(p,2),
                                                 wPan.v1(p,0),wPan.v1(p,1),wPan.v1(p,2),wPan.v2(p,0),wPan.v2(p,1),wPan.v2(p,2),
                                                 wPan.v3(p,0),wPan.v3(p,1),wPan.v3(p,2));
                            if (!allow)
                                break;
                        }
                        fPan.fwdMap(f, 0) = allow; // Store the result in a boolean matrix
                    }

                    /// 2. X-forward
                    // Check if cell is not on the domain border
                    if (i == fPan.nX - 1) {
                        fPan.fbdMap(f, 1) = 0;
                        fPan.fwdMap(f, 1) = 0;
                    }
                    // Create a box enclosing the geometry, based on cell dimensions to restrict the algorithm complexity
                    else if (((fPan.CG(f,0)+fPan.deltaX) < minX)
                             || (fPan.CG(f,1) < minY || fPan.CG(f,1) > maxY)
                             || (fPan.CG(f,2) < minZ || fPan.CG(f,2) > maxZ)) {
                        fPan.fbdMap(f, 1) = 1;
                        fPan.fwdMap(f, 1) = 1;
                    }
                    // Check if next cell is not an internal cell
                    else if (!fPan.fMap(f+1)) {
                        fPan.fbdMap(f, 1) = 0;
                        fPan.fwdMap(f, 1) = 0;
                    }
                    // Create a vector joining the center of two adjacent cells and check if that vector crosses a panel
                    else {
                        u = fPan.CG.row(f+1).transpose() - fPan.CG.row(f).transpose();
                        // Body panel
                        for (int p = 0; p < bPan.nP; p++) {
                            w = fPan.CG.row(f).transpose() - bPan.CG.row(p).transpose();

                            allow = cast_ray_pip(u(0),u(1),u(2),w(0),w(1),w(2),fPan.CG(f,0),fPan.CG(f,1),fPan.CG(f,2),
                                                 bPan.n(p,0),bPan.n(p,1),bPan.n(p,2),bPan.v0(p,0),bPan.v0(p,1),bPan.v0(p,2),
                                                 bPan.v1(p,0),bPan.v1(p,1),bPan.v1(p,2),bPan.v2(p,0),bPan.v2(p,1),bPan.v2(p,2),
                                                 bPan.v3(p,0),bPan.v3(p,1),bPan.v3(p,2));
                            if (!allow)
                                break;
                        }
                        fPan.fbdMap(f, 1) = allow; // Store the result in a boolean matrix
                        // Wake panel
                        for (int p = 0; p < wPan.nP; p++) {
                            w = fPan.CG.row(f).transpose() - wPan.CG.row(p).transpose();

                            allow = cast_ray_pip(u(0),u(1),u(2),w(0),w(1),w(2),fPan.CG(f,0),fPan.CG(f,1),fPan.CG(f,2),
                                                 wPan.n(p,0),wPan.n(p,1),wPan.n(p,2),wPan.v0(p,0),wPan.v0(p,1),wPan.v0(p,2),
                                                 wPan.v1(p,0),wPan.v1(p,1),wPan.v1(p,2),wPan.v2(p,0),wPan.v2(p,1),wPan.v2(p,2),
                                                 wPan.v3(p,0),wPan.v3(p,1),wPan.v3(p,2));
                            if (!allow)
                                break;
                        }
                        fPan.fwdMap(f, 1) = allow; // Store the result in a boolean matrix
                    }

                    /// 3. Y-backward
                    // Check if cell is not on the domain border
                    if (j == 0) {
                        fPan.fbdMap(f, 2) = 0;
                        fPan.fwdMap(f, 2) = 0;
                    }
                    // Create a box enclosing the geometry, based on cell dimensions to restrict the algorithm complexity
                    else if ((fPan.CG(f,0) < minX)
                             || (fPan.CG(f,1) < minY || (fPan.CG(f,1)-fPan.deltaY) > maxY)
                             || (fPan.CG(f,2) < minZ || fPan.CG(f,2) > maxZ)) {
                        fPan.fbdMap(f, 2) = 1;
                        fPan.fwdMap(f, 2) = 1;
                    }
                    // Check if previous cell is not an internal cell
                    else if (!fPan.fMap(f-fPan.nX*fPan.nZ)) {
                        fPan.fbdMap(f, 2) = 0;
                        fPan.fwdMap(f, 2) = 0;
                    }
                    // Create a vector joining the center of two adjacent cells and check if that vector crosses a panel
                    else {
                        u = fPan.CG.row(f-fPan.nX*fPan.nZ).transpose() - fPan.CG.row(f).transpose();
                        // Body panel
                        for (int p = 0; p < bPan.nP; p++) {
                            w = fPan.CG.row(f).transpose() - bPan.CG.row(p).transpose();

                            allow = cast_ray_pip(u(0),u(1),u(2),w(0),w(1),w(2),fPan.CG(f,0),fPan.CG(f,1),fPan.CG(f,2),
                                                 bPan.n(p,0),bPan.n(p,1),bPan.n(p,2),bPan.v0(p,0),bPan.v0(p,1),bPan.v0(p,2),
                                                 bPan.v1(p,0),bPan.v1(p,1),bPan.v1(p,2),bPan.v2(p,0),bPan.v2(p,1),bPan.v2(p,2),
                                                 bPan.v3(p,0),bPan.v3(p,1),bPan.v3(p,2));
                            if (!allow)
                                break;
                        }
                        fPan.fbdMap(f, 2) = allow; // Store the result in a boolean matrix
                        // Wake panel
                        for (int p = 0; p < wPan.nP; p++) {
                            w = fPan.CG.row(f).transpose() - wPan.CG.row(p).transpose();

                            allow = cast_ray_pip(u(0),u(1),u(2),w(0),w(1),w(2),fPan.CG(f,0),fPan.CG(f,1),fPan.CG(f,2),
                                                 wPan.n(p,0),wPan.n(p,1),wPan.n(p,2),wPan.v0(p,0),wPan.v0(p,1),wPan.v0(p,2),
                                                 wPan.v1(p,0),wPan.v1(p,1),wPan.v1(p,2),wPan.v2(p,0),wPan.v2(p,1),wPan.v2(p,2),
                                                 wPan.v3(p,0),wPan.v3(p,1),wPan.v3(p,2));
                            if (!allow)
                                break;
                        }
                        fPan.fwdMap(f, 2) = allow; // Store the result in a boolean matrix
                    }

                    /// 4. Y-forward
                    // Check if cell is not on the domain border
                    if (j == fPan.nY - 1) {
                        fPan.fbdMap(f, 3) = 0;
                        fPan.fwdMap(f, 3) = 0;
                    }
                    // Create a box enclosing the geometry, based on cell dimensions to restrict the algorithm complexity
                    else if ((fPan.CG(f,0) < minX)
                             || ((fPan.CG(f,1)+fPan.deltaY) < minY || fPan.CG(f,1) > maxY)
                             || (fPan.CG(f,2) < minZ || fPan.CG(f,2) > maxZ)) {
                        fPan.fbdMap(f, 3) = 1;
                        fPan.fwdMap(f, 3) = 1;
                    }
                    // Check if next cell is not an internal cell
                    else if (!fPan.fMap(f+fPan.nX*fPan.nZ)) {
                        fPan.fbdMap(f, 3) = 0;
                        fPan.fwdMap(f, 3) = 0;
                    }
                    // Create a vector joining the center of two adjacent cells and check if that vector crosses a panel
                    else {
                        u = fPan.CG.row(f+fPan.nX*fPan.nZ).transpose() - fPan.CG.row(f).transpose();
                        // Body panel
                        for (int p = 0; p < bPan.nP; p++) {
                            w = fPan.CG.row(f).transpose() - bPan.CG.row(p).transpose();

                            allow = cast_ray_pip(u(0),u(1),u(2),w(0),w(1),w(2),fPan.CG(f,0),fPan.CG(f,1),fPan.CG(f,2),
                                                 bPan.n(p,0),bPan.n(p,1),bPan.n(p,2),bPan.v0(p,0),bPan.v0(p,1),bPan.v0(p,2),
                                                 bPan.v1(p,0),bPan.v1(p,1),bPan.v1(p,2),bPan.v2(p,0),bPan.v2(p,1),bPan.v2(p,2),
                                                 bPan.v3(p,0),bPan.v3(p,1),bPan.v3(p,2));
                            if (!allow)
                                break;
                        }
                        fPan.fbdMap(f, 3) = allow; // Store the result in a boolean matrix
                        // Wake panel
                        for (int p = 0; p < wPan.nP; p++) {
                            w = fPan.CG.row(f).transpose() - wPan.CG.row(p).transpose();

                            allow = cast_ray_pip(u(0),u(1),u(2),w(0),w(1),w(2),fPan.CG(f,0),fPan.CG(f,1),fPan.CG(f,2),
                                                 wPan.n(p,0),wPan.n(p,1),wPan.n(p,2),wPan.v0(p,0),wPan.v0(p,1),wPan.v0(p,2),
                                                 wPan.v1(p,0),wPan.v1(p,1),wPan.v1(p,2),wPan.v2(p,0),wPan.v2(p,1),wPan.v2(p,2),
                                                 wPan.v3(p,0),wPan.v3(p,1),wPan.v3(p,2));
                            if (!allow)
                                break;
                        }
                        fPan.fwdMap(f, 3) = allow; // Store the result in a boolean matrix
                    }

                    /// 5. Z-backward
                    // Check if cell is not on the domain border
                    if (k == 0) {
                        fPan.fbdMap(f, 4) = 0;
                        fPan.fwdMap(f, 4) = 0;
                    }
                    // Create a box enclosing the geometry, based on cell dimensions to restrict the algorithm complexity
                    else if ((fPan.CG(f,0) < minX)
                             || (fPan.CG(f,1) < minY || fPan.CG(f,1) > maxY)
                             || (fPan.CG(f,2) < minZ || (fPan.CG(f,2)-fPan.deltaZ) > maxZ)) {
                        fPan.fbdMap(f, 4) = 1;
                        fPan.fwdMap(f, 4) = 1;
                    }
                    // Check if previous cell is not an internal cell
                    else if (!fPan.fMap(f-fPan.nX)) {
                        fPan.fbdMap(f, 4) = 0;
                        fPan.fwdMap(f, 4) = 0;
                    }
                    // Create a vector joining the center of two adjacent cells and check if that vector crosses a panel
                    else {
                        u = fPan.CG.row(f-fPan.nX).transpose() - fPan.CG.row(f).transpose();
                        // Body panel
                        for (int p = 0; p < bPan.nP; p++) {
                            w = fPan.CG.row(f).transpose() - bPan.CG.row(p).transpose();

                            allow = cast_ray_pip(u(0),u(1),u(2),w(0),w(1),w(2),fPan.CG(f,0),fPan.CG(f,1),fPan.CG(f,2),
                                                 bPan.n(p,0),bPan.n(p,1),bPan.n(p,2),bPan.v0(p,0),bPan.v0(p,1),bPan.v0(p,2),
                                                 bPan.v1(p,0),bPan.v1(p,1),bPan.v1(p,2),bPan.v2(p,0),bPan.v2(p,1),bPan.v2(p,2),
                                                 bPan.v3(p,0),bPan.v3(p,1),bPan.v3(p,2));
                            if (!allow)
                                break;
                        }
                        fPan.fbdMap(f, 4) = allow; // Store the result in a boolean matrix
                        // Wake panel
                        for (int p = 0; p < wPan.nP; p++) {
                            w = fPan.CG.row(f).transpose() - wPan.CG.row(p).transpose();

                            allow = cast_ray_pip(u(0),u(1),u(2),w(0),w(1),w(2),fPan.CG(f,0),fPan.CG(f,1),fPan.CG(f,2),
                                                 wPan.n(p,0),wPan.n(p,1),wPan.n(p,2),wPan.v0(p,0),wPan.v0(p,1),wPan.v0(p,2),
                                                 wPan.v1(p,0),wPan.v1(p,1),wPan.v1(p,2),wPan.v2(p,0),wPan.v2(p,1),wPan.v2(p,2),
                                                 wPan.v3(p,0),wPan.v3(p,1),wPan.v3(p,2));
                            if (!allow)
                                break;
                        }
                        fPan.fwdMap(f, 4) = allow; // Store the result in a boolean matrix
                    }

                    /// 6. Z-forward
                    // Check if cell is not on the domain border
                    if (k == fPan.nZ - 1) {
                        fPan.fbdMap(f, 5) = 0;
                        fPan.fwdMap(f, 5) = 0;
                    }
                    // Create a box enclosing the geometry, based on cell dimensions to restrict the algorithm complexity
                    else if ((fPan.CG(f,0) < minX)
                             || (fPan.CG(f,1) < minY || fPan.CG(f,1) > maxY)
                             || ((fPan.CG(f,2)+fPan.deltaZ) < minZ || fPan.CG(f,2) > maxZ)) {
                        fPan.fbdMap(f, 5) = 1;
                        fPan.fwdMap(f, 5) = 1;
                    }
                    // Check if next cell is not an internal cell
                    else if (!fPan.fMap(f+fPan.nX)) {
                        fPan.fbdMap(f, 5) = 0;
                        fPan.fwdMap(f, 5) = 0;
                    }
                    // Create a vector joining the center of two adjacent cells and check if that vector crosses a panel
                    else {
                        u = fPan.CG.row(f+fPan.nX).transpose() - fPan.CG.row(f).transpose();
                        // Body panel
                        for (int p = 0; p < bPan.nP; p++) {
                            w = fPan.CG.row(f).transpose() - bPan.CG.row(p).transpose();

                            allow = cast_ray_pip(u(0),u(1),u(2),w(0),w(1),w(2),fPan.CG(f,0),fPan.CG(f,1),fPan.CG(f,2),
                                                 bPan.n(p,0),bPan.n(p,1),bPan.n(p,2),bPan.v0(p,0),bPan.v0(p,1),bPan.v0(p,2),
                                                 bPan.v1(p,0),bPan.v1(p,1),bPan.v1(p,2),bPan.v2(p,0),bPan.v2(p,1),bPan.v2(p,2),
                                                 bPan.v3(p,0),bPan.v3(p,1),bPan.v3(p,2));
                            if (!allow)
                                break;
                        }
                        fPan.fbdMap(f, 5) = allow; // Store the result in a boolean matrix
                        // Wake panel
                        for (int p = 0; p < wPan.nP; p++) {
                            w = fPan.CG.row(f).transpose() - wPan.CG.row(p).transpose();

                            allow = cast_ray_pip(u(0),u(1),u(2),w(0),w(1),w(2),fPan.CG(f,0),fPan.CG(f,1),fPan.CG(f,2),
                                                 wPan.n(p,0),wPan.n(p,1),wPan.n(p,2),wPan.v0(p,0),wPan.v0(p,1),wPan.v0(p,2),
                                                 wPan.v1(p,0),wPan.v1(p,1),wPan.v1(p,2),wPan.v2(p,0),wPan.v2(p,1),wPan.v2(p,2),
                                                 wPan.v3(p,0),wPan.v3(p,1),wPan.v3(p,2));
                            if (!allow)
                                break;
                        }
                        fPan.fwdMap(f, 5) = allow; // Store the result in a boolean matrix
                    }
                }
                f++;
            }
        }
    }

    //// Control display
    cout << "Done!" << endl;
    #ifdef VERBOSE
        cout << "Allowed derivatives (body) map: " << fPan.fbdMap.rows() << 'X' << fPan.fbdMap.cols() << endl;
        for (int i = 0; i < fPan.nF; i++)
            cout << fPan.fbdMap.row(i) << endl;
        cout << endl;
        cout << "Allowed derivatives (wake) map: " << fPan.fwdMap.rows() << 'X' << fPan.fwdMap.cols() << endl;
        for (int i = 0; i < fPan.nF; i++)
            cout << fPan.fwdMap.row(i) << endl;
    #endif
    cout << endl;
}


//  (C) Copyright Nick Thompson 2020.
//  Use, modification and distribution are 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)
#ifndef BOOST_MATH_TOOLS_ULP_PLOT_HPP
#define BOOST_MATH_TOOLS_ULP_PLOT_HPP
#include <algorithm>
#include <iostream>
#include <iomanip>
#include <cassert>
#include <vector>
#include <utility>
#include <fstream>
#include <string>
#include <list>
#include <random>
#include <stdexcept>
#include <boost/math/tools/condition_numbers.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/algorithm/string/predicate.hpp>

// Design of this function comes from:
// https://blogs.mathworks.com/cleve/2017/01/23/ulps-plots-reveal-math-function-accurary/

// The envelope is the maximum of 1/2 and half the condition number of function evaluation.

namespace boost::math::tools {

namespace detail {
template<class F1, class F2, class CoarseReal, class PreciseReal>
void write_gridlines(std::ostream& fs, int horizontal_lines, int vertical_lines,
                     F1 x_scale, F2 y_scale, CoarseReal min_x, CoarseReal max_x, PreciseReal min_y, PreciseReal max_y,
                     int graph_width, int graph_height, int margin_left, std::string const & font_color)
{
  // Make a grid:
  for (int i = 1; i <= horizontal_lines; ++i) {
      PreciseReal y_cord_dataspace = min_y +  ((max_y - min_y)*i)/horizontal_lines;
      auto y = y_scale(y_cord_dataspace);
      fs << "<line x1='0' y1='" << y << "' x2='" << graph_width
         << "' y2='" << y
         << "' stroke='gray' stroke-width='1' opacity='0.5' stroke-dasharray='4' />\n";

      fs << "<text x='" <<  -margin_left/4 + 5 << "' y='" << y - 3
         << "' font-family='times' font-size='10' fill='" << font_color << "' transform='rotate(-90 "
         << -margin_left/4 + 8 << " " << y + 5 << ")'>"
         << std::setprecision(4) << y_cord_dataspace << "</text>\n";
   }

    for (int i = 1; i <= vertical_lines; ++i) {
        CoarseReal x_cord_dataspace = min_x +  ((max_x - min_x)*i)/vertical_lines;
        CoarseReal x = x_scale(x_cord_dataspace);
        fs << "<line x1='" << x << "' y1='0' x2='" << x
           << "' y2='" << graph_height
           << "' stroke='gray' stroke-width='1' opacity='0.5' stroke-dasharray='4' />\n";

        fs << "<text x='" <<  x - 10  << "' y='" << graph_height + 10
           << "' font-family='times' font-size='10' fill='" << font_color << "'>"
           << std::setprecision(4) << x_cord_dataspace << "</text>\n";
    }
}
}

template<class F, typename PreciseReal, typename CoarseReal>
class ulps_plot {
public:
    ulps_plot(F hi_acc_impl, CoarseReal a, CoarseReal b,
             size_t samples = 1000, bool perturb_abscissas = false, int random_seed = -1);

    ulps_plot& clip(PreciseReal clip);

    ulps_plot& width(int width);

    ulps_plot& envelope_color(std::string const & color);

    ulps_plot& title(std::string const & title);

    ulps_plot& background_color(std::string const & background_color);

    ulps_plot& font_color(std::string const & font_color);

    ulps_plot& crop_color(std::string const & color);

    ulps_plot& nan_color(std::string const & color);

    ulps_plot& ulp_envelope(bool write_ulp);

    template<class G>
    ulps_plot& add_fn(G g, std::string const & color = "steelblue");

    ulps_plot& horizontal_lines(int horizontal_lines);

    ulps_plot& vertical_lines(int vertical_lines);

    void write(std::string const & filename) const;

    friend std::ostream& operator<<(std::ostream& fs, ulps_plot const & plot)
    {
        using std::abs;
        using std::floor;
        using std::isnan;
        if (plot.ulp_list_.size() == 0)
        {
            throw std::domain_error("No functions added for comparison.");
        }
        if (plot.width_ <= 1)
        {
            throw std::domain_error("Width = " + std::to_string(plot.width_) + ", which is too small.");
        }

        PreciseReal worst_ulp_distance = 0;
        PreciseReal min_y = std::numeric_limits<PreciseReal>::max();
        PreciseReal max_y = std::numeric_limits<PreciseReal>::lowest();
        for (auto const & ulp_vec : plot.ulp_list_)
        {
            for (auto const & ulp : ulp_vec)
            {
                if (static_cast<PreciseReal>(abs(ulp)) > worst_ulp_distance)
                {
                    worst_ulp_distance = static_cast<PreciseReal>(abs(ulp));
                }
                if (static_cast<PreciseReal>(ulp) < min_y)
                {
                    min_y = static_cast<PreciseReal>(ulp);
                }
                if (static_cast<PreciseReal>(ulp) > max_y)
                {
                    max_y = static_cast<PreciseReal>(ulp);
                }
            }
        }

        // half-ulp accuracy is the best that can be expected; sometimes we can get less, but barely less.
        // then the axes don't show up; painful!
        if (max_y < 0.5) {
            max_y = 0.5;
        }
        if (min_y > -0.5) {
            min_y = -0.5;
        }

        if (plot.clip_ > 0)
        {
            if (max_y > plot.clip_)
            {
                max_y = plot.clip_;
            }
            if (min_y < -plot.clip_)
            {
                min_y = -plot.clip_;
            }
        }

        int height = static_cast<int>(floor(double(plot.width_)/1.61803));
        int margin_top = 40;
        int margin_left = 25;
        if (plot.title_.size() == 0)
        {
            margin_top = 10;
            margin_left = 15;
        }
        int margin_bottom = 20;
        int margin_right = 20;
        int graph_height = height - margin_bottom - margin_top;
        int graph_width = plot.width_ - margin_left - margin_right;

        // Maps [a,b] to [0, graph_width]
        auto x_scale = [&](CoarseReal x)->CoarseReal
        {
            return ((x-plot.a_)/(plot.b_ - plot.a_))*static_cast<CoarseReal>(graph_width);
        };

        auto y_scale = [&](PreciseReal y)->PreciseReal
        {
            return ((max_y - y)/(max_y - min_y) )*static_cast<PreciseReal>(graph_height);
        };

        fs << "<?xml version=\"1.0\" encoding='UTF-8' ?>\n"
           << "<svg xmlns='http://www.w3.org/2000/svg' width='"
           << plot.width_ << "' height='"
           << height << "'>\n"
           << "<style>\nsvg { background-color:" << plot.background_color_ << "; }\n"
           << "</style>\n";
        if (plot.title_.size() > 0)
        {
            fs << "<text x='" << floor(plot.width_/2)
               << "' y='" << floor(margin_top/2)
               << "' font-family='Palatino' font-size='25' fill='"
               << plot.font_color_  << "'  alignment-baseline='middle' text-anchor='middle'>"
               << plot.title_
               << "</text>\n";
        }

        // Construct SVG group to simplify the calculations slightly:
        fs << "<g transform='translate(" << margin_left << ", " << margin_top << ")'>\n";
            // y-axis:
        fs  << "<line x1='0' y1='0' x2='0' y2='" << graph_height
            << "' stroke='gray' stroke-width='1'/>\n";
        PreciseReal x_axis_loc = y_scale(static_cast<PreciseReal>(0));
        fs << "<line x1='0' y1='" << x_axis_loc
            << "' x2='" << graph_width << "' y2='" << x_axis_loc
            << "' stroke='gray' stroke-width='1'/>\n";

        if (worst_ulp_distance > 3)
        {
            detail::write_gridlines(fs, plot.horizontal_lines_, plot.vertical_lines_, x_scale, y_scale, plot.a_, plot.b_,
                                    min_y, max_y, graph_width, graph_height, margin_left, plot.font_color_);
        }
        else
        {
            std::vector<double> ys{-3.0, -2.5, -2.0, -1.5, -1.0, -0.5, 0.5, 1.0, 1.5, 2.0, 2.5, 3.0};
            for (size_t i = 0; i < ys.size(); ++i)
            {
                if (min_y <= ys[i] && ys[i] <= max_y)
                {
                    PreciseReal y_cord_dataspace = ys[i];
                    PreciseReal y = y_scale(y_cord_dataspace);
                    fs << "<line x1='0' y1='" << y << "' x2='" << graph_width
                       << "' y2='" << y
                       << "' stroke='gray' stroke-width='1' opacity='0.5' stroke-dasharray='4' />\n";

                    fs << "<text x='" <<  -margin_left/2 << "' y='" << y - 3
                       << "' font-family='times' font-size='10' fill='" << plot.font_color_ << "' transform='rotate(-90 "
                       << -margin_left/2 + 7 << " " << y << ")'>"
                       <<  std::setprecision(4) << y_cord_dataspace << "</text>\n";
                }
            }
            for (int i = 1; i <= plot.vertical_lines_; ++i)
            {
                CoarseReal x_cord_dataspace = plot.a_ +  ((plot.b_ - plot.a_)*i)/plot.vertical_lines_;
                CoarseReal x = x_scale(x_cord_dataspace);
                fs << "<line x1='" << x << "' y1='0' x2='" << x
                   << "' y2='" << graph_height
                   << "' stroke='gray' stroke-width='1' opacity='0.5' stroke-dasharray='4' />\n";

                fs << "<text x='" <<  x - 10  << "' y='" << graph_height + 10
                   << "' font-family='times' font-size='10' fill='" << plot.font_color_ << "'>"
                   << std::setprecision(4) << x_cord_dataspace << "</text>\n";
            }
        }

        int color_idx = 0;
        for (auto const & ulp : plot.ulp_list_)
        {
            std::string color = plot.colors_[color_idx++];
            for (size_t j = 0; j < ulp.size(); ++j)
            {
                if (isnan(ulp[j]))
                {
                    if(plot.nan_color_ == "")
                        continue;
                    CoarseReal x = x_scale(plot.coarse_abscissas_[j]);
                    PreciseReal y = y_scale(static_cast<PreciseReal>(plot.clip_));
                    fs << "<circle cx='" << x << "' cy='" << y << "' r='1' fill='" << plot.nan_color_ << "'/>\n";
                    y = y_scale(static_cast<PreciseReal>(-plot.clip_));
                    fs << "<circle cx='" << x << "' cy='" << y << "' r='1' fill='" << plot.nan_color_ << "'/>\n";
                }
                if (plot.clip_ > 0 && static_cast<PreciseReal>(abs(ulp[j])) > plot.clip_)
                {
                   if (plot.crop_color_ == "")
                      continue;
                   CoarseReal x = x_scale(plot.coarse_abscissas_[j]);
                   PreciseReal y = y_scale(static_cast<PreciseReal>(ulp[j] < 0 ? -plot.clip_ : plot.clip_));
                   fs << "<circle cx='" << x << "' cy='" << y << "' r='1' fill='" << plot.crop_color_ << "'/>\n";
                }
                else
                {
                   CoarseReal x = x_scale(plot.coarse_abscissas_[j]);
                   PreciseReal y = y_scale(static_cast<PreciseReal>(ulp[j]));
                   fs << "<circle cx='" << x << "' cy='" << y << "' r='1' fill='" << color << "'/>\n";
                }
            }
        }

        if (plot.ulp_envelope_)
        {
            std::string close_path = "' stroke='"  + plot.envelope_color_ + "' stroke-width='1' fill='none'></path>\n";
            size_t jstart = 0;
            while (plot.cond_[jstart] > max_y)
            {
                ++jstart;
                if (jstart >= plot.cond_.size())
                {
                    goto done;
                }
            }

            size_t jmin = jstart;
        new_top_path:
            if (jmin >= plot.cond_.size())
            {
                goto start_bottom_paths;
            }
            fs << "<path d='M" << x_scale(plot.coarse_abscissas_[jmin]) << " " << y_scale(plot.cond_[jmin]);

            for (size_t j = jmin + 1; j < plot.coarse_abscissas_.size(); ++j)
            {
                bool bad = isnan(plot.cond_[j]) || (plot.cond_[j] > max_y);
                if (bad)
                {
                    ++j;
                    while ( (j < plot.coarse_abscissas_.size() - 2) && bad)
                    {
                        bad = isnan(plot.cond_[j]) || (plot.cond_[j] > max_y);
                        ++j;
                    }
                    jmin = j;
                    fs << close_path;
                    goto new_top_path;
                }

                CoarseReal t = x_scale(plot.coarse_abscissas_[j]);
                PreciseReal y = y_scale(plot.cond_[j]);
                fs << " L" << t << " " << y;
            }
            fs << close_path;
        start_bottom_paths:
            jmin = jstart;
        new_bottom_path:
            if (jmin >= plot.cond_.size())
            {
                goto done;
            }
            fs << "<path d='M" << x_scale(plot.coarse_abscissas_[jmin]) << " " << y_scale(-plot.cond_[jmin]);

            for (size_t j = jmin + 1; j < plot.coarse_abscissas_.size(); ++j)
            {
                bool bad = isnan(plot.cond_[j]) || (-plot.cond_[j] < min_y);
                if (bad)
                {
                    ++j;
                    while ( (j < plot.coarse_abscissas_.size() - 2) && bad)
                    {
                        bad = isnan(plot.cond_[j]) || (-plot.cond_[j] < min_y);
                        ++j;
                    }
                    jmin = j;
                    fs << close_path;
                    goto new_bottom_path;
                }
                CoarseReal t = x_scale(plot.coarse_abscissas_[j]);
                PreciseReal y = y_scale(-plot.cond_[j]);
                fs << " L" << t << " " << y;
            }
            fs << close_path;
        }
    done:
        fs << "</g>\n"
           << "</svg>\n";
        return fs;
    }

private:
    std::vector<PreciseReal> precise_abscissas_;
    std::vector<CoarseReal> coarse_abscissas_;
    std::vector<PreciseReal> precise_ordinates_;
    std::vector<PreciseReal> cond_;
    std::list<std::vector<CoarseReal>> ulp_list_;
    std::vector<std::string> colors_;
    CoarseReal a_;
    CoarseReal b_;
    PreciseReal clip_;
    int width_;
    std::string envelope_color_;
    bool ulp_envelope_;
    int horizontal_lines_;
    int vertical_lines_;
    std::string title_;
    std::string background_color_;
    std::string font_color_;
    std::string crop_color_;
    std::string nan_color_;
};

template<class F, typename PreciseReal, typename CoarseReal>
ulps_plot<F, PreciseReal, CoarseReal>& ulps_plot<F, PreciseReal, CoarseReal>::envelope_color(std::string const & color)
{
    envelope_color_ = color;
    return *this;
}

template<class F, typename PreciseReal, typename CoarseReal>
ulps_plot<F, PreciseReal, CoarseReal>& ulps_plot<F, PreciseReal, CoarseReal>::clip(PreciseReal clip)
{
    clip_ = clip;
    return *this;
}

template<class F, typename PreciseReal, typename CoarseReal>
ulps_plot<F, PreciseReal, CoarseReal>& ulps_plot<F, PreciseReal, CoarseReal>::width(int width)
{
    width_ = width;
    return *this;
}

template<class F, typename PreciseReal, typename CoarseReal>
ulps_plot<F, PreciseReal, CoarseReal>& ulps_plot<F, PreciseReal, CoarseReal>::horizontal_lines(int horizontal_lines)
{
    horizontal_lines_ = horizontal_lines;
    return *this;
}

template<class F, typename PreciseReal, typename CoarseReal>
ulps_plot<F, PreciseReal, CoarseReal>& ulps_plot<F, PreciseReal, CoarseReal>::vertical_lines(int vertical_lines)
{
    vertical_lines_ = vertical_lines;
    return *this;
}

template<class F, typename PreciseReal, typename CoarseReal>
ulps_plot<F, PreciseReal, CoarseReal>& ulps_plot<F, PreciseReal, CoarseReal>::title(std::string const & title)
{
    title_ = title;
    return *this;
}

template<class F, typename PreciseReal, typename CoarseReal>
ulps_plot<F, PreciseReal, CoarseReal>& ulps_plot<F, PreciseReal, CoarseReal>::background_color(std::string const & background_color)
{
    background_color_ = background_color;
    return *this;
}

template<class F, typename PreciseReal, typename CoarseReal>
ulps_plot<F, PreciseReal, CoarseReal>& ulps_plot<F, PreciseReal, CoarseReal>::font_color(std::string const & font_color)
{
    font_color_ = font_color;
    return *this;
}

template<class F, typename PreciseReal, typename CoarseReal>
ulps_plot<F, PreciseReal, CoarseReal>& ulps_plot<F, PreciseReal, CoarseReal>::crop_color(std::string const & color)
{
    crop_color_ = color;
    return *this;
}

template<class F, typename PreciseReal, typename CoarseReal>
ulps_plot<F, PreciseReal, CoarseReal>& ulps_plot<F, PreciseReal, CoarseReal>::nan_color(std::string const & color)
{
    nan_color_ = color;
    return *this;
}

template<class F, typename PreciseReal, typename CoarseReal>
ulps_plot<F, PreciseReal, CoarseReal>& ulps_plot<F, PreciseReal, CoarseReal>::ulp_envelope(bool write_ulp_envelope)
{
    ulp_envelope_ = write_ulp_envelope;
    return *this;
}

template<class F, typename PreciseReal, typename CoarseReal>
void ulps_plot<F, PreciseReal, CoarseReal>::write(std::string const & filename) const
{
    if (!boost::algorithm::ends_with(filename, ".svg"))
    {
        throw std::logic_error("Only svg files are supported at this time.");
    }
    std::ofstream fs(filename);
    fs << *this;
    fs.close();
}

template<class F, typename PreciseReal, typename CoarseReal>
ulps_plot<F, PreciseReal, CoarseReal>::ulps_plot(F hi_acc_impl, CoarseReal a, CoarseReal b,
             size_t samples, bool perturb_abscissas, int random_seed) : crop_color_("red")
{
    // Use digits10 for this comparison in case the two types have differeing radixes:
    static_assert(std::numeric_limits<PreciseReal>::digits10 >= std::numeric_limits<CoarseReal>::digits10, "PreciseReal must have higher precision that CoarseReal");
    if (samples < 10)
    {
        throw std::domain_error("Must have at least 10 samples, samples = " + std::to_string(samples));
    }
    if (b <= a)
    {
        throw std::domain_error("On interval [a,b], b > a is required.");
    }
    a_ = a;
    b_ = b;

    std::mt19937_64 gen;
    if (random_seed == -1)
    {
        std::random_device rd;
        gen.seed(rd());
    }
    // Boost's uniform_real_distribution can generate quad and multiprecision random numbers; std's cannot:
    boost::random::uniform_real_distribution<PreciseReal> dis(static_cast<PreciseReal>(a), static_cast<PreciseReal>(b));
    precise_abscissas_.resize(samples);
    coarse_abscissas_.resize(samples);

    if (perturb_abscissas)
    {
        for(size_t i = 0; i < samples; ++i)
        {
            precise_abscissas_[i] = dis(gen);
        }
        std::sort(precise_abscissas_.begin(), precise_abscissas_.end());
        for (size_t i = 0; i < samples; ++i)
        {
            coarse_abscissas_[i] = static_cast<CoarseReal>(precise_abscissas_[i]);
        }
    }
    else
    {
        for(size_t i = 0; i < samples; ++i)
        {
            coarse_abscissas_[i] = static_cast<CoarseReal>(dis(gen));
        }
        std::sort(coarse_abscissas_.begin(), coarse_abscissas_.end());
        for (size_t i = 0; i < samples; ++i)
        {
            precise_abscissas_[i] = static_cast<PreciseReal>(coarse_abscissas_[i]);
        }
    }

    precise_ordinates_.resize(samples);
    for (size_t i = 0; i < samples; ++i)
    {
        precise_ordinates_[i] = hi_acc_impl(precise_abscissas_[i]);
    }

    cond_.resize(samples, std::numeric_limits<PreciseReal>::quiet_NaN());
    for (size_t i = 0 ; i < samples; ++i)
    {
        PreciseReal y = precise_ordinates_[i];
        if (y != 0)
        {
            // Maybe cond_ is badly names; should it be half_cond_?
            cond_[i] = boost::math::tools::evaluation_condition_number(hi_acc_impl, precise_abscissas_[i])/2;
            // Half-ULP accuracy is the correctly rounded result, so make sure the envelop doesn't go below this:
            if (cond_[i] < 0.5)
            {
                cond_[i] = 0.5;
            }
        }
        // else leave it as nan.
    }
    clip_ = -1;
    width_ = 1100;
    envelope_color_ = "chartreuse";
    ulp_envelope_ = true;
    horizontal_lines_ = 8;
    vertical_lines_ = 10;
    title_ = "";
    background_color_ = "black";
    font_color_ = "white";
}

template<class F, typename PreciseReal, typename CoarseReal>
template<class G>
ulps_plot<F, PreciseReal, CoarseReal>& ulps_plot<F, PreciseReal, CoarseReal>::add_fn(G g, std::string const & color)
{
    using std::abs;
    size_t samples = precise_abscissas_.size();
    std::vector<CoarseReal> ulps(samples);
    for (size_t i = 0; i < samples; ++i)
    {
        PreciseReal y_hi_acc = precise_ordinates_[i];
        PreciseReal y_lo_acc = static_cast<PreciseReal>(g(coarse_abscissas_[i]));
        PreciseReal absy = abs(y_hi_acc);
        PreciseReal dist = static_cast<PreciseReal>(nextafter(static_cast<CoarseReal>(absy), std::numeric_limits<CoarseReal>::max()) - static_cast<CoarseReal>(absy));
        ulps[i] = static_cast<CoarseReal>((y_lo_acc - y_hi_acc)/dist);
    }
    ulp_list_.emplace_back(ulps);
    colors_.emplace_back(color);
    return *this;
}

} // namespace boost::math::tools
#endif