// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018 www.open3d.org
//
// 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.
// ----------------------------------------------------------------------------

#include "open3d/geometry/PointCloud.h"
#include "open3d/geometry/TriangleMesh.h"
#include "open3d/utility/Console.h"

#include <Eigen/Dense>
#include <cfloat>
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <iostream>
#include <list>

// clang-format off
#include "PoissonRecon/Src/PreProcessor.h"
#include "PoissonRecon/Src/MyMiscellany.h"
#include "PoissonRecon/Src/CmdLineParser.h"
#include "PoissonRecon/Src/FEMTree.h"
#include "PoissonRecon/Src/PPolynomial.h"
#include "PoissonRecon/Src/PointStreamData.h"
// clang-format on

namespace open3d {
namespace geometry {
namespace poisson {

// The order of the B-Spline used to splat in data for color interpolation
static const int DATA_DEGREE = 0;
// The order of the B-Spline used to splat in the weights for density estimation
static const int WEIGHT_DEGREE = 2;
// The order of the B-Spline used to splat in the normals for constructing the
// Laplacian constraints
static const int NORMAL_DEGREE = 2;
// The default finite-element degree
static const int DEFAULT_FEM_DEGREE = 1;
// The default finite-element boundary type
static const BoundaryType DEFAULT_FEM_BOUNDARY = BOUNDARY_NEUMANN;
// The dimension of the system
static const int DIMENSION = 3;

class Open3DData {
public:
    Open3DData() : normal_(0, 0, 0), color_(0, 0, 0) {}
    Open3DData(const Eigen::Vector3d& normal, const Eigen::Vector3d& color)
        : normal_(normal), color_(color) {}

    Open3DData operator*(double s) const {
        return Open3DData(s * normal_, s * color_);
    }
    Open3DData operator/(double s) const {
        return Open3DData(normal_ / s, (1 / s) * color_);
    }
    Open3DData& operator+=(const Open3DData& d) {
        normal_ += d.normal_;
        color_ += d.color_;
        return *this;
    }
    Open3DData& operator*=(double s) {
        normal_ *= s;
        color_ *= s;
        return *this;
    }

public:
    Eigen::Vector3d normal_;
    Eigen::Vector3d color_;
};

template <typename Real>
class Open3DPointStream
    : public InputPointStreamWithData<Real, DIMENSION, Open3DData> {
public:
    Open3DPointStream(const open3d::geometry::PointCloud* pcd)
        : pcd_(pcd), xform_(nullptr), current_(0) {}
    void reset(void) { current_ = 0; }
    bool nextPoint(Point<Real, 3>& p, Open3DData& d) {
        if (current_ >= pcd_->points_.size()) {
            return false;
        }
        p.coords[0] = static_cast<Real>(pcd_->points_[current_](0));
        p.coords[1] = static_cast<Real>(pcd_->points_[current_](1));
        p.coords[2] = static_cast<Real>(pcd_->points_[current_](2));

        if (xform_ != nullptr) {
            p = (*xform_) * p;
        }

        if (pcd_->HasNormals()) {
            d.normal_ = pcd_->normals_[current_];
        } else {
            d.normal_ = Eigen::Vector3d(0, 0, 0);
        }

        if (pcd_->HasColors()) {
            d.color_ = pcd_->colors_[current_];
        } else {
            d.color_ = Eigen::Vector3d(0, 0, 0);
        }

        current_++;
        return true;
    }

public:
    const open3d::geometry::PointCloud* pcd_;
    XForm<Real, 4>* xform_;
    size_t current_;
};

template <typename _Real>
class Open3DVertex {
public:
    typedef _Real Real;

    Open3DVertex() : Open3DVertex(Point<Real, 3>(0, 0, 0)) {}
    Open3DVertex(Point<Real, 3> point)
        : point(point), normal_(0, 0, 0), color_(0, 0, 0), w_(0) {}

    Open3DVertex& operator*=(Real s) {
        point *= s;
        normal_ *= s;
        color_ *= s;
        w_ *= s;
        return *this;
    }

    Open3DVertex& operator+=(const Open3DVertex& p) {
        point += p.point;
        normal_ += p.normal_;
        color_ += p.color_;
        w_ += p.w_;
        return *this;
    }

    Open3DVertex& operator/=(Real s) {
        point /= s;
        normal_ /= s;
        color_ /= s;
        w_ /= s;
        return *this;
    }

public:
    // point can not have trailing _, because template methods assume that it is
    // named this way
    Point<Real, 3> point;
    Eigen::Vector3d normal_;
    Eigen::Vector3d color_;
    double w_;
};

template <unsigned int Dim, class Real>
struct FEMTreeProfiler {
    FEMTree<Dim, Real>& tree;
    double t;

    FEMTreeProfiler(FEMTree<Dim, Real>& t) : tree(t) {}
    void start(void) {
        t = Time(), FEMTree<Dim, Real>::ResetLocalMemoryUsage();
    }
    void dumpOutput(const char* header) const {
        FEMTree<Dim, Real>::MemoryUsage();
        if (header) {
            utility::LogDebug("{} {} (s), {} (MB) / {} (MB) / {} (MB)", header,
                              Time() - t,
                              FEMTree<Dim, Real>::LocalMemoryUsage(),
                              FEMTree<Dim, Real>::MaxMemoryUsage(),
                              MemoryInfo::PeakMemoryUsageMB());
        } else {
            utility::LogDebug("{} (s), {} (MB) / {} (MB) / {} (MB)", Time() - t,
                              FEMTree<Dim, Real>::LocalMemoryUsage(),
                              FEMTree<Dim, Real>::MaxMemoryUsage(),
                              MemoryInfo::PeakMemoryUsageMB());
        }
    }
};

template <class Real, unsigned int Dim>
XForm<Real, Dim + 1> GetBoundingBoxXForm(Point<Real, Dim> min,
                                         Point<Real, Dim> max,
                                         Real scaleFactor) {
    Point<Real, Dim> center = (max + min) / 2;
    Real scale = max[0] - min[0];
    for (unsigned int d = 1; d < Dim; d++) {
        scale = std::max<Real>(scale, max[d] - min[d]);
    }
    scale *= scaleFactor;
    for (unsigned int i = 0; i < Dim; i++) {
        center[i] -= scale / 2;
    }
    XForm<Real, Dim + 1> tXForm = XForm<Real, Dim + 1>::Identity(),
                         sXForm = XForm<Real, Dim + 1>::Identity();
    for (unsigned int i = 0; i < Dim; i++) {
        sXForm(i, i) = (Real)(1. / scale), tXForm(Dim, i) = -center[i];
    }
    return sXForm * tXForm;
}

template <class Real, unsigned int Dim>
XForm<Real, Dim + 1> GetBoundingBoxXForm(Point<Real, Dim> min,
                                         Point<Real, Dim> max,
                                         Real width,
                                         Real scaleFactor,
                                         int& depth) {
    // Get the target resolution (along the largest dimension)
    Real resolution = (max[0] - min[0]) / width;
    for (unsigned int d = 1; d < Dim; d++) {
        resolution = std::max<Real>(resolution, (max[d] - min[d]) / width);
    }
    resolution *= scaleFactor;
    depth = 0;
    while ((1 << depth) < resolution) {
        depth++;
    }

    Point<Real, Dim> center = (max + min) / 2;
    Real scale = (1 << depth) * width;

    for (unsigned int i = 0; i < Dim; i++) {
        center[i] -= scale / 2;
    }
    XForm<Real, Dim + 1> tXForm = XForm<Real, Dim + 1>::Identity(),
                         sXForm = XForm<Real, Dim + 1>::Identity();
    for (unsigned int i = 0; i < Dim; i++) {
        sXForm(i, i) = (Real)(1. / scale), tXForm(Dim, i) = -center[i];
    }
    return sXForm * tXForm;
}

template <class Real, unsigned int Dim>
XForm<Real, Dim + 1> GetPointXForm(InputPointStream<Real, Dim>& stream,
                                   Real width,
                                   Real scaleFactor,
                                   int& depth) {
    Point<Real, Dim> min, max;
    stream.boundingBox(min, max);
    return GetBoundingBoxXForm(min, max, width, scaleFactor, depth);
}

template <class Real, unsigned int Dim>
XForm<Real, Dim + 1> GetPointXForm(InputPointStream<Real, Dim>& stream,
                                   Real scaleFactor) {
    Point<Real, Dim> min, max;
    stream.boundingBox(min, max);
    return GetBoundingBoxXForm(min, max, scaleFactor);
}

template <unsigned int Dim, typename Real>
struct ConstraintDual {
    Real target, weight;
    ConstraintDual(Real t, Real w) : target(t), weight(w) {}
    CumulativeDerivativeValues<Real, Dim, 0> operator()(
            const Point<Real, Dim>& p) const {
        return CumulativeDerivativeValues<Real, Dim, 0>(target * weight);
    };
};

template <unsigned int Dim, typename Real>
struct SystemDual {
    Real weight;
    SystemDual(Real w) : weight(w) {}
    CumulativeDerivativeValues<Real, Dim, 0> operator()(
            const Point<Real, Dim>& p,
            const CumulativeDerivativeValues<Real, Dim, 0>& dValues) const {
        return dValues * weight;
    };
    CumulativeDerivativeValues<double, Dim, 0> operator()(
            const Point<Real, Dim>& p,
            const CumulativeDerivativeValues<double, Dim, 0>& dValues) const {
        return dValues * weight;
    };
};

template <unsigned int Dim>
struct SystemDual<Dim, double> {
    typedef double Real;
    Real weight;
    SystemDual(Real w) : weight(w) {}
    CumulativeDerivativeValues<Real, Dim, 0> operator()(
            const Point<Real, Dim>& p,
            const CumulativeDerivativeValues<Real, Dim, 0>& dValues) const {
        return dValues * weight;
    };
};

template <typename Vertex,
          typename Real,
          typename SetVertexFunction,
          unsigned int... FEMSigs,
          typename... SampleData>
void ExtractMesh(
        float datax,
        bool linear_fit,
        UIntPack<FEMSigs...>,
        std::tuple<SampleData...>,
        FEMTree<sizeof...(FEMSigs), Real>& tree,
        const DenseNodeData<Real, UIntPack<FEMSigs...>>& solution,
        Real isoValue,
        const std::vector<typename FEMTree<sizeof...(FEMSigs),
                                           Real>::PointSample>* samples,
        std::vector<Open3DData>* sampleData,
        const typename FEMTree<sizeof...(FEMSigs),
                               Real>::template DensityEstimator<WEIGHT_DEGREE>*
                density,
        const SetVertexFunction& SetVertex,
        XForm<Real, sizeof...(FEMSigs) + 1> iXForm,
        std::shared_ptr<open3d::geometry::TriangleMesh>& out_mesh,
        std::vector<double>& out_densities) {
    static const int Dim = sizeof...(FEMSigs);
    typedef UIntPack<FEMSigs...> Sigs;
    static const unsigned int DataSig =
            FEMDegreeAndBType<DATA_DEGREE, BOUNDARY_FREE>::Signature;
    typedef typename FEMTree<Dim,
                             Real>::template DensityEstimator<WEIGHT_DEGREE>
            DensityEstimator;

    FEMTreeProfiler<Dim, Real> profiler(tree);

    CoredMeshData<Vertex, node_index_type>* mesh;
    mesh = new CoredVectorMeshData<Vertex, node_index_type>();

    bool non_manifold = true;
    bool polygon_mesh = false;

    profiler.start();
    typename IsoSurfaceExtractor<Dim, Real, Vertex>::IsoStats isoStats;
    if (sampleData) {
        SparseNodeData<ProjectiveData<Open3DData, Real>,
                       IsotropicUIntPack<Dim, DataSig>>
                _sampleData =
                        tree.template setMultiDepthDataField<DataSig, false>(
                                *samples, *sampleData, (DensityEstimator*)NULL);
        for (const RegularTreeNode<Dim, FEMTreeNodeData, depth_and_offset_type>*
                     n = tree.tree().nextNode();
             n; n = tree.tree().nextNode(n)) {
            ProjectiveData<Open3DData, Real>* clr = _sampleData(n);
            if (clr) (*clr) *= (Real)pow(datax, tree.depth(n));
        }
        isoStats = IsoSurfaceExtractor<Dim, Real, Vertex>::template Extract<
                Open3DData>(Sigs(), UIntPack<WEIGHT_DEGREE>(),
                            UIntPack<DataSig>(), tree, density, &_sampleData,
                            solution, isoValue, *mesh, SetVertex, !linear_fit,
                            !non_manifold, polygon_mesh, false);
    } else {
        isoStats = IsoSurfaceExtractor<Dim, Real, Vertex>::template Extract<
                Open3DData>(Sigs(), UIntPack<WEIGHT_DEGREE>(),
                            UIntPack<DataSig>(), tree, density, NULL, solution,
                            isoValue, *mesh, SetVertex, !linear_fit,
                            !non_manifold, polygon_mesh, false);
    }

    mesh->resetIterator();
    out_densities.clear();
    for (size_t vidx = 0; vidx < mesh->outOfCorePointCount(); ++vidx) {
        Vertex v;
        mesh->nextOutOfCorePoint(v);
        v.point = iXForm * v.point;
        out_mesh->vertices_.push_back(
                Eigen::Vector3d(v.point[0], v.point[1], v.point[2]));
        out_mesh->vertex_normals_.push_back(v.normal_);
        out_mesh->vertex_colors_.push_back(v.color_);
        out_densities.push_back(v.w_);
    }
    for (size_t tidx = 0; tidx < mesh->polygonCount(); ++tidx) {
        std::vector<CoredVertexIndex<node_index_type>> triangle;
        mesh->nextPolygon(triangle);
        if (triangle.size() != 3) {
            open3d::utility::LogError("got polygon");
        } else {
            out_mesh->triangles_.push_back(Eigen::Vector3i(
                    triangle[0].idx, triangle[1].idx, triangle[2].idx));
        }
    }

    delete mesh;
}

template <class Real, typename... SampleData, unsigned int... FEMSigs>
void Execute(const open3d::geometry::PointCloud& pcd,
             std::shared_ptr<open3d::geometry::TriangleMesh>& out_mesh,
             std::vector<double>& out_densities,
             int depth,
             size_t width,
             float scale,
             bool linear_fit,
             UIntPack<FEMSigs...>) {
    static const int Dim = sizeof...(FEMSigs);
    typedef UIntPack<FEMSigs...> Sigs;
    typedef UIntPack<FEMSignature<FEMSigs>::Degree...> Degrees;
    typedef UIntPack<FEMDegreeAndBType<
            NORMAL_DEGREE, DerivativeBoundary<FEMSignature<FEMSigs>::BType,
                                              1>::BType>::Signature...>
            NormalSigs;
    typedef typename FEMTree<Dim,
                             Real>::template DensityEstimator<WEIGHT_DEGREE>
            DensityEstimator;
    typedef typename FEMTree<Dim, Real>::template InterpolationInfo<Real, 0>
            InterpolationInfo;

    XForm<Real, Dim + 1> xForm, iXForm;
    xForm = XForm<Real, Dim + 1>::Identity();

    float datax = 32.f;
    int base_depth = 0;
    int base_v_cycles = 1;
    float confidence = 0.f;
    float point_weight = 2.f * DEFAULT_FEM_DEGREE;
    float confidence_bias = 0.f;
    float samples_per_node = 1.5f;
    float cg_solver_accuracy = 1e-3f;
    int full_depth = 5;
    int iters = 8;
    bool exact_interpolation = false;

    double startTime = Time();
    Real isoValue = 0;

    FEMTree<Dim, Real> tree(MEMORY_ALLOCATOR_BLOCK_SIZE);
    FEMTreeProfiler<Dim, Real> profiler(tree);

    size_t pointCount;

    Real pointWeightSum;
    std::vector<typename FEMTree<Dim, Real>::PointSample> samples;
    std::vector<Open3DData> sampleData;
    DensityEstimator* density = NULL;
    SparseNodeData<Point<Real, Dim>, NormalSigs>* normalInfo = NULL;
    Real targetValue = (Real)0.5;

    // Read in the samples (and color data)
    {
        Open3DPointStream<Real> pointStream(&pcd);

        if (width > 0) {
            xForm = GetPointXForm<Real, Dim>(pointStream, (Real)width,
                                             (Real)(scale > 0 ? scale : 1.),
                                             depth) *
                    xForm;
        } else {
            xForm = scale > 0 ? GetPointXForm<Real, Dim>(pointStream,
                                                         (Real)scale) *
                                        xForm
                              : xForm;
        }

        pointStream.xform_ = &xForm;

        {
            auto ProcessDataWithConfidence = [&](const Point<Real, Dim>& p,
                                                 Open3DData& d) {
                Real l = (Real)d.normal_.norm();
                if (!l || l != l) return (Real)-1.;
                return (Real)pow(l, confidence);
            };
            auto ProcessData = [](const Point<Real, Dim>& p, Open3DData& d) {
                Real l = (Real)d.normal_.norm();
                if (!l || l != l) return (Real)-1.;
                d.normal_ /= l;
                return (Real)1.;
            };
            if (confidence > 0) {
                pointCount = FEMTreeInitializer<Dim, Real>::template Initialize<
                        Open3DData>(tree.spaceRoot(), pointStream, depth,
                                    samples, sampleData, true,
                                    tree.nodeAllocators[0], tree.initializer(),
                                    ProcessDataWithConfidence);
            } else {
                pointCount = FEMTreeInitializer<Dim, Real>::template Initialize<
                        Open3DData>(tree.spaceRoot(), pointStream, depth,
                                    samples, sampleData, true,
                                    tree.nodeAllocators[0], tree.initializer(),
                                    ProcessData);
            }
        }
        iXForm = xForm.inverse();

        utility::LogDebug("Input Points / Samples: {} / {}", pointCount,
                          samples.size());
    }

    int kernelDepth = depth - 2;
    if (kernelDepth < 0) {
        utility::LogError(
                "[CreateFromPointCloudPoisson] depth (={}) has to be >= 2",
                depth);
    }

    DenseNodeData<Real, Sigs> solution;
    {
        DenseNodeData<Real, Sigs> constraints;
        InterpolationInfo* iInfo = NULL;
        int solveDepth = depth;

        tree.resetNodeIndices();

        // Get the kernel density estimator
        {
            profiler.start();
            density = tree.template setDensityEstimator<WEIGHT_DEGREE>(
                    samples, kernelDepth, samples_per_node, 1);
            profiler.dumpOutput("#   Got kernel density:");
        }

        // Transform the Hermite samples into a vector field
        {
            profiler.start();
            normalInfo = new SparseNodeData<Point<Real, Dim>, NormalSigs>();
            std::function<bool(Open3DData, Point<Real, Dim>&)>
                    ConversionFunction =
                            [](Open3DData in, Point<Real, Dim>& out) {
                                // Point<Real, Dim> n = in.template data<0>();
                                Point<Real, Dim> n(in.normal_(0), in.normal_(1),
                                                   in.normal_(2));
                                Real l = (Real)Length(n);
                                // It is possible that the samples have non-zero
                                // normals but there are two co-located samples
                                // with negative normals...
                                if (!l) return false;
                                out = n / l;
                                return true;
                            };
            std::function<bool(Open3DData, Point<Real, Dim>&, Real&)>
                    ConversionAndBiasFunction = [&](Open3DData in,
                                                    Point<Real, Dim>& out,
                                                    Real& bias) {
                        // Point<Real, Dim> n = in.template data<0>();
                        Point<Real, Dim> n(in.normal_(0), in.normal_(1),
                                           in.normal_(2));
                        Real l = (Real)Length(n);
                        // It is possible that the samples have non-zero normals
                        // but there are two co-located samples with negative
                        // normals...
                        if (!l) return false;
                        out = n / l;
                        bias = (Real)(log(l) * confidence_bias /
                                      log(1 << (Dim - 1)));
                        return true;
                    };
            if (confidence_bias > 0) {
                *normalInfo = tree.setDataField(
                        NormalSigs(), samples, sampleData, density,
                        pointWeightSum, ConversionAndBiasFunction);
            } else {
                *normalInfo = tree.setDataField(
                        NormalSigs(), samples, sampleData, density,
                        pointWeightSum, ConversionFunction);
            }
            ThreadPool::Parallel_for(0, normalInfo->size(),
                                     [&](unsigned int, size_t i) {
                                         (*normalInfo)[i] *= (Real)-1.;
                                     });
            profiler.dumpOutput("#     Got normal field:");
            utility::LogDebug("Point weight / Estimated Area: {:e} / {:e}",
                              pointWeightSum, pointCount * pointWeightSum);
        }

        // Trim the tree and prepare for multigrid
        {
            profiler.start();
            constexpr int MAX_DEGREE = NORMAL_DEGREE > Degrees::Max()
                                               ? NORMAL_DEGREE
                                               : Degrees::Max();
            tree.template finalizeForMultigrid<MAX_DEGREE>(
                    full_depth,
                    typename FEMTree<Dim, Real>::template HasNormalDataFunctor<
                            NormalSigs>(*normalInfo),
                    normalInfo, density);
            profiler.dumpOutput("#       Finalized tree:");
        }

        // Add the FEM constraints
        {
            profiler.start();
            constraints = tree.initDenseNodeData(Sigs());
            typename FEMIntegrator::template Constraint<
                    Sigs, IsotropicUIntPack<Dim, 1>, NormalSigs,
                    IsotropicUIntPack<Dim, 0>, Dim>
                    F;
            unsigned int derivatives2[Dim];
            for (unsigned int d = 0; d < Dim; d++) derivatives2[d] = 0;
            typedef IsotropicUIntPack<Dim, 1> Derivatives1;
            typedef IsotropicUIntPack<Dim, 0> Derivatives2;
            for (unsigned int d = 0; d < Dim; d++) {
                unsigned int derivatives1[Dim];
                for (unsigned int dd = 0; dd < Dim; dd++)
                    derivatives1[dd] = dd == d ? 1 : 0;
                F.weights[d]
                         [TensorDerivatives<Derivatives1>::Index(derivatives1)]
                         [TensorDerivatives<Derivatives2>::Index(
                                 derivatives2)] = 1;
            }
            tree.addFEMConstraints(F, *normalInfo, constraints, solveDepth);
            profiler.dumpOutput("#  Set FEM constraints:");
        }

        // Free up the normal info
        delete normalInfo, normalInfo = NULL;

        // Add the interpolation constraints
        if (point_weight > 0) {
            profiler.start();
            if (exact_interpolation) {
                iInfo = FEMTree<Dim, Real>::
                        template InitializeExactPointInterpolationInfo<Real, 0>(
                                tree, samples,
                                ConstraintDual<Dim, Real>(
                                        targetValue,
                                        (Real)point_weight * pointWeightSum),
                                SystemDual<Dim, Real>((Real)point_weight *
                                                      pointWeightSum),
                                true, false);
            } else {
                iInfo = FEMTree<Dim, Real>::
                        template InitializeApproximatePointInterpolationInfo<
                                Real, 0>(
                                tree, samples,
                                ConstraintDual<Dim, Real>(
                                        targetValue,
                                        (Real)point_weight * pointWeightSum),
                                SystemDual<Dim, Real>((Real)point_weight *
                                                      pointWeightSum),
                                true, 1);
            }
            tree.addInterpolationConstraints(constraints, solveDepth, *iInfo);
            profiler.dumpOutput("#Set point constraints:");
        }

        utility::LogDebug(
                "Leaf Nodes / Active Nodes / Ghost Nodes: {} / {} / {}",
                tree.leaves(), tree.nodes(), tree.ghostNodes());
        utility::LogDebug("Memory Usage: {:.3f} MB",
                          float(MemoryInfo::Usage()) / (1 << 20));

        // Solve the linear system
        {
            profiler.start();
            typename FEMTree<Dim, Real>::SolverInfo sInfo;
            sInfo.cgDepth = 0, sInfo.cascadic = true, sInfo.vCycles = 1,
            sInfo.iters = iters, sInfo.cgAccuracy = cg_solver_accuracy,
            sInfo.verbose = utility::Logger::i().verbosity_level_ ==
                            utility::VerbosityLevel::Debug,
            sInfo.showResidual = utility::Logger::i().verbosity_level_ ==
                                 utility::VerbosityLevel::Debug,
            sInfo.showGlobalResidual = SHOW_GLOBAL_RESIDUAL_NONE,
            sInfo.sliceBlockSize = 1;
            sInfo.baseDepth = base_depth, sInfo.baseVCycles = base_v_cycles;
            typename FEMIntegrator::template System<Sigs,
                                                    IsotropicUIntPack<Dim, 1>>
                    F({0., 1.});
            solution = tree.solveSystem(Sigs(), F, constraints, solveDepth,
                                        sInfo, iInfo);
            profiler.dumpOutput("# Linear system solved:");
            if (iInfo) delete iInfo, iInfo = NULL;
        }
    }

    {
        profiler.start();
        double valueSum = 0, weightSum = 0;
        typename FEMTree<Dim, Real>::template MultiThreadedEvaluator<Sigs, 0>
                evaluator(&tree, solution);
        std::vector<double> valueSums(ThreadPool::NumThreads(), 0),
                weightSums(ThreadPool::NumThreads(), 0);
        ThreadPool::Parallel_for(
                0, samples.size(), [&](unsigned int thread, size_t j) {
                    ProjectiveData<Point<Real, Dim>, Real>& sample =
                            samples[j].sample;
                    Real w = sample.weight;
                    if (w > 0)
                        weightSums[thread] += w,
                                valueSums[thread] +=
                                evaluator.values(sample.data / sample.weight,
                                                 thread, samples[j].node)[0] *
                                w;
                });
        for (size_t t = 0; t < valueSums.size(); t++)
            valueSum += valueSums[t], weightSum += weightSums[t];
        isoValue = (Real)(valueSum / weightSum);
        profiler.dumpOutput("Got average:");
        utility::LogDebug("Iso-Value: {:e} = {:e} / {:e}", isoValue, valueSum,
                          weightSum);
    }

    auto SetVertex = [](Open3DVertex<Real>& v, Point<Real, Dim> p, Real w,
                        Open3DData d) {
        v.point = p;
        v.normal_ = d.normal_;
        v.color_ = d.color_;
        v.w_ = w;
    };
    ExtractMesh<Open3DVertex<Real>, Real>(
            datax, linear_fit, UIntPack<FEMSigs...>(),
            std::tuple<SampleData...>(), tree, solution, isoValue, &samples,
            &sampleData, density, SetVertex, iXForm, out_mesh, out_densities);

    if (density) delete density, density = NULL;
    utility::LogDebug("#          Total Solve: {:9.1f} (s), {:9.1f} (MB)",
                      Time() - startTime, FEMTree<Dim, Real>::MaxMemoryUsage());
}

}  // namespace poisson

std::tuple<std::shared_ptr<TriangleMesh>, std::vector<double>>
TriangleMesh::CreateFromPointCloudPoisson(const PointCloud& pcd,
                                          size_t depth,
                                          size_t width,
                                          float scale,
                                          bool linear_fit) {
    static const BoundaryType BType = poisson::DEFAULT_FEM_BOUNDARY;
    typedef IsotropicUIntPack<
            poisson::DIMENSION,
            FEMDegreeAndBType</* Degree */ 1, BType>::Signature>
            FEMSigs;

    if (!pcd.HasNormals()) {
        utility::LogError("[CreateFromPointCloudPoisson] pcd has no normals");
    }

#ifdef _OPENMP
    ThreadPool::Init((ThreadPool::ParallelType)(int)ThreadPool::OPEN_MP,
                     std::thread::hardware_concurrency());
#else
    ThreadPool::Init((ThreadPool::ParallelType)(int)ThreadPool::THREAD_POOL,
                     std::thread::hardware_concurrency());
#endif

    auto mesh = std::make_shared<TriangleMesh>();
    std::vector<double> densities;
    poisson::Execute<float>(pcd, mesh, densities, static_cast<int>(depth),
                            width, scale, linear_fit, FEMSigs());

    ThreadPool::Terminate();

    return std::make_tuple(mesh, densities);
}

}  // namespace geometry
}  // namespace open3d


///////////////////////////////////////////////////////////////////////////////
// distribution::survival::record::random_generator_v0.hpp                   //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under 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_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_RECORD_GENERATOR_V0_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_RECORD_GENERATOR_V0_HPP_ER_2009
#include <boost/function.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/lambda/bind.hpp>

#include <boost/fusion/include/at_key.hpp>
#include <boost/fusion/include/make_map.hpp>

#include <boost/statistics/detail/distribution/survival/record/key/include.hpp>
#include <boost/statistics/detail/distribution/model/key/include.hpp>
#include <boost/statistics/detail/distribution/survival/failure_time/meta/include.hpp>

#include <boost/statistics/detail/distribution/survival/record/random/generator.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace survival{
namespace record{
namespace meta{

    template<typename T>
    struct random_generator<0,T>
    {
        // Generates failure times given a model, covariate and a unif
        // random generation

        typedef tag::failure_time tag_ft_;
        typedef typename
                boost::fusion::result_of::make_map<tag_ft_,T>::type ft_;
        typedef ft_(*fp_)(const T&);
        typedef boost::function<T()> 	gen_val_;
        typedef boost::function<ft_()> 	type;

        template<typename X,typename M,typename U>
        static type call(const X& x,const M& m,U& urng)
        {
            return type(
                boost::lambda::bind<ft_>(
                    fp_(boost::fusion::make_map<tag_ft_>),
                        boost::lambda::bind<T>(
                        gen_val_(
                            failure_time::make_random_generator(
                                x,
                                m,
                                urng
                            )
                        )
                    )
                )
            );
        }

        template<typename Map,typename U>
        static type call(const Map& map,U& urng)
        {
            typedef model::tag::covariate 	tag_x_;
            typedef model::tag::model 		tag_m_;
            return call(
                boost::fusion::at_key<tag_x_>(map),
                boost::fusion::at_key<tag_m_>(map),
                urng
            );
        }
    };

}// meta
}// record
}// survival
}// distribution
}// detail
}// statistics
}// boost

#endif


//
// Libor Novak
// 03/12/2017
//
// Detects objects by using a single scale detector on an image pyramid
//

#include <caffe/caffe.hpp>
#include "caffe/util/benchmark.hpp"
#include "caffe/util/pgp.hpp"

// This code only works with OpenCV!
#ifdef USE_OPENCV

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <algorithm>
#include <iosfwd>
#include <memory>
#include <string>
#include <utility>
#include <vector>

#include <boost/program_options.hpp>
namespace po = boost::program_options;

/**
 * @brief Wraps the input layer into a vector of cv::Mat so we could assign data to it more easily
 * @param input_layer Pointer to the net input layer blob
 * @param input_channels Vector of cv::Mat, which will be assigned
 */
void wrapInputLayer (caffe::Blob<float>* input_layer, std::vector<cv::Mat> &out_input_channels)
{
    out_input_channels.clear();

    int height = input_layer->shape(2);
    int width  = input_layer->shape(3);

    float* input_data = input_layer->mutable_cpu_data();

    for (int i = 0; i < input_layer->shape(1); ++i)
    {
        cv::Mat channel(height, width, CV_32FC1, input_data);
        out_input_channels.push_back(channel);
        input_data += width * height;
    }
}

void runPyramidDetection (const std::string &path_prototxt, const std::string &path_caffemodel,
                          const std::string &path_image_list, const std::string &path_pgp)
{
#ifdef CPU_ONLY
    caffe::Caffe::set_mode(caffe::Caffe::CPU);
#else
    caffe::Caffe::set_mode(caffe::Caffe::GPU);
#endif

    caffe::CPUTimer timer;

//    const std::vector<double> scales = { 2.25, 1.5, 1.0, 0.66, 0.44, 0.29 };
    const std::vector<double> scales = { 1.0 };

    // Create network and load trained weights from caffemodel file
    auto net = std::make_shared<caffe::Net<float>>(path_prototxt, caffe::TEST);
    net->CopyTrainedLayersFrom(path_caffemodel);

    caffe::Blob<float>* input_layer  = net->input_blobs()[0];
    LOG(INFO) << "We have " << net->output_blobs().size() << " accumulators on the output";

    CHECK_EQ(net->num_inputs(), 1) << "Network should have exactly one input.";
    CHECK_EQ(input_layer->shape(1), 3) << "Input layer must have 3 channels.";

    // Load the P matrices and ground planes
    std::map<std::string, PGP> pgps;
    if (path_pgp != "") pgps = PGP::readPGPFile(path_pgp);

    // Prepare the input channels
    std::vector<cv::Mat> input_channels;
    wrapInputLayer(input_layer, input_channels);

    std::ifstream infile(path_image_list.c_str());
    CHECK(infile) << "Unable to open image list TXT file '" << path_image_list << "'!";
    std::string line; // int i = 0;
    while (std::getline(infile, line))
    {
        LOG(INFO) << line;
        CHECK(boost::filesystem::exists(line)) << "Image '" << line << "' not found!";

        // Load the image
        timer.Start();
        cv::Mat image = cv::imread(line, CV_LOAD_IMAGE_COLOR);
        cv::Mat imagef; image.convertTo(imagef, CV_32FC3);

        // Convert to zero mean and unit variance
        imagef -= cv::Scalar(128.0f, 128.0f, 128.0f);
        imagef *= 1.0f/128.0f;

        timer.Stop();
        std::cout << "Time to read image: " << timer.MilliSeconds() << " ms" << std::endl;

        const double GCW = 800;
        const double GCH = 1200;
        const double GCS = 10;
        cv::Mat ground_canvas(GCH, GCW, CV_8UC3, cv::Scalar(255,255,255));
        cv::line(ground_canvas, cv::Point(GCW/2, 0), cv::Point(GCW/2, GCH), cv::Scalar(200, 200, 200), 4);

        // Build the image pyramid and run detection on each scale of the pyramid
        timer.Start();
        for (double s: scales)
        {
            cv::Mat imagef_scaled; cv::resize(imagef, imagef_scaled, cv::Size(), s, s);
            std::cout << "Current size: " << imagef_scaled.size() << " (" << s << ")" << std::endl;

            if (imagef_scaled.rows != input_layer->shape(2) || imagef_scaled.cols != input_layer->shape(3))
            {
                // Reshape the network
                input_layer->Reshape(1, input_layer->shape(1), imagef_scaled.rows, imagef_scaled.cols);
                net->Reshape();

                wrapInputLayer(input_layer, input_channels);
            }

            // Copy the image to the input layer of the network
            cv::split(imagef_scaled, input_channels);

            net->Forward();

            // Show the result
            int ai = 0;
            for (caffe::Blob<float>* output: net->output_blobs())
            {
//                caffe::Blob<float>* output = net->output_blobs()[0];
                float *data_output = output->mutable_cpu_data();

                cv::Mat acc_prob(output->shape(2), output->shape(3), CV_32FC1, data_output+output->offset(0, 0));

                if (output->shape(1) == 5)
                {
                    // 2D bounding box
                    cv::Mat acc_xmin(output->shape(2), output->shape(3), CV_32FC1, data_output+output->offset(0, 1));
                    cv::Mat acc_ymin(output->shape(2), output->shape(3), CV_32FC1, data_output+output->offset(0, 2));
                    cv::Mat acc_xmax(output->shape(2), output->shape(3), CV_32FC1, data_output+output->offset(0, 3));
                    cv::Mat acc_ymax(output->shape(2), output->shape(3), CV_32FC1, data_output+output->offset(0, 4));

                    double mx;
                    cv::minMaxLoc(acc_prob, 0, &mx);
                    std::cout << mx << std::endl;

                    cv::imshow("Accumulator " + net->blob_names()[net->output_blob_indices()[ai++]] + " (" + std::to_string(s) + ")", acc_prob);

                    // Draw detected boxes
                    for (int i = 0; i < acc_prob.rows; ++i)
                    {
                        for (int j = 0; j < acc_prob.cols; ++j)
                        {
                            float conf = acc_prob.at<float>(i, j);
                            if (conf >= 0.5)
                            {
                                // Check if it is a local maximum
                                if (i > 0)
                                {
                                    if (j > 0 && acc_prob.at<float>(i-1, j-1) > conf) continue;
                                    if (acc_prob.at<float>(i-1, j) > conf) continue;
                                    if (j < acc_prob.cols-1 && acc_prob.at<float>(i-1, j+1) > conf) continue;
                                }
                                if (j > 0 && acc_prob.at<float>(i, j-1) > conf) continue;
                                if (j < acc_prob.cols-1 && acc_prob.at<float>(i, j+1) > conf) continue;
                                if (i < acc_prob.rows-1)
                                {
                                    if (j > 0 && acc_prob.at<float>(i+1, j-1) > conf) continue;
                                    if (acc_prob.at<float>(i+1, j) > conf) continue;
                                    if (j < acc_prob.cols-1 && acc_prob.at<float>(i+1, j+1) > conf) continue;
                                }

                                // Ok, this is a local maximum
                                int xmin = acc_xmin.at<float>(i, j) / s;
                                int ymin = acc_ymin.at<float>(i, j) / s;
                                int xmax = acc_xmax.at<float>(i, j) / s;
                                int ymax = acc_ymax.at<float>(i, j) / s;

                                std::cout << acc_xmin.at<float>(i, j) << " " << acc_ymin.at<float>(i, j) << " " << acc_xmax.at<float>(i, j) << " " << acc_ymax.at<float>(i, j) << std::endl;

                                if (xmin >= xmax || ymin >= ymax)
                                {
                                    // This does not make sense
                                    std::cout << "WARNING: Coordinates do not make sense! [" << xmin << "," << ymin << "," << xmax << "," << ymax << "]" << std::endl;
                                    continue;
                                }

                                cv::rectangle(image, cv::Rect(xmin, ymin, xmax-xmin, ymax-ymin), cv::Scalar(0,0,255));
//                                cv::circle(image, cv::Point(4*j/s, 4*i/s), 2, cv::Scalar(0,255,0), -1);
                            }
                        }
                    }
                }
                else if (output->shape(1) == 8)
                {
                    // 3D bounding box
                    // fblx, fbly, fbrx, fbry, rblx, rbly, ftly
                    cv::Mat acc_fblx(output->shape(2), output->shape(3), CV_32FC1, data_output+output->offset(0, 1));
                    cv::Mat acc_fbly(output->shape(2), output->shape(3), CV_32FC1, data_output+output->offset(0, 2));
                    cv::Mat acc_fbrx(output->shape(2), output->shape(3), CV_32FC1, data_output+output->offset(0, 3));
                    cv::Mat acc_fbry(output->shape(2), output->shape(3), CV_32FC1, data_output+output->offset(0, 4));
                    cv::Mat acc_rblx(output->shape(2), output->shape(3), CV_32FC1, data_output+output->offset(0, 5));
                    cv::Mat acc_rbly(output->shape(2), output->shape(3), CV_32FC1, data_output+output->offset(0, 6));
                    cv::Mat acc_ftly(output->shape(2), output->shape(3), CV_32FC1, data_output+output->offset(0, 7));

                    double mx;
                    cv::minMaxLoc(acc_prob, 0, &mx);
                    std::cout << mx << std::endl;

                    cv::imshow("Accumulator " + net->blob_names()[net->output_blob_indices()[ai++]] + " (" + std::to_string(s) + ")", acc_prob);

                    // Draw detected 3D boxes
                    for (int i = 0; i < acc_prob.rows; ++i)
                    {
                        for (int j = 0; j < acc_prob.cols; ++j)
                        {
                            float conf = acc_prob.at<float>(i, j);
                            if (conf >= 0.5)
                            {
                                // Check if it is a local maximum
                                if (i > 0)
                                {
                                    if (j > 0 && acc_prob.at<float>(i-1, j-1) > conf) continue;
                                    if (acc_prob.at<float>(i-1, j) > conf) continue;
                                    if (j < acc_prob.cols-1 && acc_prob.at<float>(i-1, j+1) > conf) continue;
                                }
                                if (j > 0 && acc_prob.at<float>(i, j-1) > conf) continue;
                                if (j < acc_prob.cols-1 && acc_prob.at<float>(i, j+1) > conf) continue;
                                if (i < acc_prob.rows-1)
                                {
                                    if (j > 0 && acc_prob.at<float>(i+1, j-1) > conf) continue;
                                    if (acc_prob.at<float>(i+1, j) > conf) continue;
                                    if (j < acc_prob.cols-1 && acc_prob.at<float>(i+1, j+1) > conf) continue;
                                }

                                // Ok, this is a local maximum
                                int fblx = acc_fblx.at<float>(i, j) / s;
                                int fbly = acc_fbly.at<float>(i, j) / s;
                                int fbrx = acc_fbrx.at<float>(i, j) / s;
                                int fbry = acc_fbry.at<float>(i, j) / s;
                                int rblx = acc_rblx.at<float>(i, j) / s;
                                int rbly = acc_rbly.at<float>(i, j) / s;
                                int ftly = acc_ftly.at<float>(i, j) / s;

                                if (path_pgp != "")
                                {
                                    auto pgpi = pgps.find(line);
                                    if (pgpi != pgps.end())
                                    {
                                        PGP pgp = (*pgpi).second;
                                        BB3D bb3d("", 1, conf, fblx, fbly, fbrx, fbry, rblx, rbly, ftly);

                                        // Reconstruct the 3D coordinates
                                        cv::Mat X_3x8 = pgp.reconstructAndFixBB3D(bb3d);

                                        cv::line(ground_canvas, cv::Point(GCS*X_3x8.at<double>(0,0)+GCW/2, GCH-GCS*X_3x8.at<double>(2,0)), cv::Point(GCS*X_3x8.at<double>(0,1)+GCW/2, GCH-GCS*X_3x8.at<double>(2,1)), cv::Scalar(0,255,0), 2);
                                        cv::line(ground_canvas, cv::Point(GCS*X_3x8.at<double>(0,0)+GCW/2, GCH-GCS*X_3x8.at<double>(2,0)), cv::Point(GCS*X_3x8.at<double>(0,3)+GCW/2, GCH-GCS*X_3x8.at<double>(2,3)), cv::Scalar(255,0,0), 2);
                                        cv::line(ground_canvas, cv::Point(GCS*X_3x8.at<double>(0,1)+GCW/2, GCH-GCS*X_3x8.at<double>(2,1)), cv::Point(GCS*X_3x8.at<double>(0,2)+GCW/2, GCH-GCS*X_3x8.at<double>(2,2)), cv::Scalar(255,0,0), 2);
                                        cv::line(ground_canvas, cv::Point(GCS*X_3x8.at<double>(0,2)+GCW/2, GCH-GCS*X_3x8.at<double>(2,2)), cv::Point(GCS*X_3x8.at<double>(0,3)+GCW/2, GCH-GCS*X_3x8.at<double>(2,3)), cv::Scalar(0,0,255), 2);

                                        // Project them back to image
                                        cv::Mat x_2x8 = pgp.projectXtox(X_3x8);

                                        // Front side
                                        cv::line(image, cv::Point(x_2x8.at<double>(0,4), x_2x8.at<double>(1,4)), cv::Point(x_2x8.at<double>(0,5), x_2x8.at<double>(1,5)), cv::Scalar(0,255,0));
                                        cv::line(image, cv::Point(x_2x8.at<double>(0,5), x_2x8.at<double>(1,5)), cv::Point(x_2x8.at<double>(0,1), x_2x8.at<double>(1,1)), cv::Scalar(0,255,0));
                                        // Rear side
                                        cv::line(image, cv::Point(x_2x8.at<double>(0,2), x_2x8.at<double>(1,2)), cv::Point(x_2x8.at<double>(0,3), x_2x8.at<double>(1,3)), cv::Scalar(0,0,255));
                                        cv::line(image, cv::Point(x_2x8.at<double>(0,7), x_2x8.at<double>(1,7)), cv::Point(x_2x8.at<double>(0,3), x_2x8.at<double>(1,3)), cv::Scalar(0,0,255));
                                        cv::line(image, cv::Point(x_2x8.at<double>(0,7), x_2x8.at<double>(1,7)), cv::Point(x_2x8.at<double>(0,6), x_2x8.at<double>(1,6)), cv::Scalar(0,0,255));
                                        cv::line(image, cv::Point(x_2x8.at<double>(0,6), x_2x8.at<double>(1,6)), cv::Point(x_2x8.at<double>(0,2), x_2x8.at<double>(1,2)), cv::Scalar(0,0,255));
                                        // Connections
                                        cv::line(image, cv::Point(x_2x8.at<double>(0,4), x_2x8.at<double>(1,4)), cv::Point(x_2x8.at<double>(0,7), x_2x8.at<double>(1,7)), cv::Scalar(255,0,0));
                                        cv::line(image, cv::Point(x_2x8.at<double>(0,5), x_2x8.at<double>(1,5)), cv::Point(x_2x8.at<double>(0,6), x_2x8.at<double>(1,6)), cv::Scalar(255,0,0));
                                        cv::line(image, cv::Point(x_2x8.at<double>(0,1), x_2x8.at<double>(1,1)), cv::Point(x_2x8.at<double>(0,2), x_2x8.at<double>(1,2)), cv::Scalar(255,0,0));
                                    }
                                    else
                                    {
                                        std::cout << "WARNING: PGP entry not found for " << line << std::endl;
                                    }
                                }

                                cv::line(image, cv::Point(fblx, fbly), cv::Point(fbrx, fbry), cv::Scalar(0, 255, 0));
                                cv::line(image, cv::Point(fblx, fbly), cv::Point(fblx, ftly), cv::Scalar(0, 255, 0));
                                cv::line(image, cv::Point(fblx, fbly), cv::Point(rblx, rbly), cv::Scalar(255, 0, 0));

//                                cv::circle(image, cv::Point(4*j/s, 4*i/s), 2, cv::Scalar(0,255,0), -1);
                            }
                        }
                    }
                }
            }
        }

        timer.Stop();
        std::cout << "Time to detection: " << timer.MilliSeconds() << " ms" << std::endl;

        cv::imshow("Image", image);
        if (path_pgp != "") cv::imshow("XZ plane", ground_canvas);
        cv::waitKey(0);
    }
}

// -----------------------------------------------  MAIN  ------------------------------------------------ //

struct ProgramArguments
{
    std::string path_prototxt;
    std::string path_caffemodel;
    std::string path_image_list;
    std::string path_pgp;
};

/**
 * @brief Parses arguments of the program
 */
void parseArguments (int argc, char** argv, ProgramArguments &pa)
{
    try {
        po::options_description desc("Arguments");
        desc.add_options()
            ("help", "Print help")
            ("prototxt", po::value<std::string>(&pa.path_prototxt)->required(),
             "Model file of the network (*.prototxt)")
            ("caffemodel", po::value<std::string>(&pa.path_caffemodel)->required(),
             "Weight file of the network (*.caffemodel)")
            ("image_list", po::value<std::string>(&pa.path_image_list)->required(),
             "Path to a TXT file with paths to the images to be tested")
            ("pgp", po::value<std::string>(&pa.path_pgp)->default_value(""),
             "Path to a PGP file with calibration matrices and ground planes")
        ;

        po::positional_options_description positional;
        positional.add("prototxt", 1);
        positional.add("caffemodel", 1);
        positional.add("image_list", 1);
        positional.add("pgp", 1);

        // Parse the input arguments
        po::variables_map vm;
        po::store(po::command_line_parser(argc, argv).options(desc).positional(positional).run(), vm);

        if (vm.count("help")) {
            std::cout << "Usage: ./detect_pyramid path/f.prototxt path/f.caffemodel path/image_list.txt (path/calib.pgp)\n";
            std::cout << desc;
            exit(EXIT_SUCCESS);
        }

        po::notify(vm);

        if (!boost::filesystem::exists(pa.path_prototxt))
        {
            std::cerr << "ERROR: File '" << pa.path_prototxt << "' does not exist!" << std::endl;
            exit(EXIT_FAILURE);
        }
        if (!boost::filesystem::exists(pa.path_caffemodel))
        {
            std::cerr << "ERROR: File '" << pa.path_caffemodel << "' does not exist!" << std::endl;
            exit(EXIT_FAILURE);
        }
        if (!boost::filesystem::exists(pa.path_image_list))
        {
            std::cerr << "ERROR: File '" << pa.path_image_list << "' does not exist!" << std::endl;
            exit(EXIT_FAILURE);
        }
        if (pa.path_pgp != "" && !boost::filesystem::exists(pa.path_pgp))
        {
            std::cerr << "ERROR: File '" << pa.path_pgp << "' does not exist!" << std::endl;
            exit(EXIT_FAILURE);
        }
    }
    catch(std::exception& e)
    {
        std::cerr << e.what() << "\n";
        exit(EXIT_FAILURE);
    }
}

int main (int argc, char** argv)
{
    ::google::InitGoogleLogging(argv[0]);

    ProgramArguments pa;
    parseArguments(argc, argv, pa);

    runPyramidDetection(pa.path_prototxt, pa.path_caffemodel, pa.path_image_list, pa.path_pgp);

    return EXIT_SUCCESS;
}

#else
int main(int argc, char** argv) {
    LOG(FATAL) << "This example requires OpenCV; compile with USE_OPENCV.";
}
#endif  // USE_OPENCV


/*******************************************************************************
*
*  Filename    : ComputeGenWeightUnc.cc
*  Description : Main function for looping through EDM files to compute GenWeight
*  Author      : Yi-Mu "Enoch" Chen [ ensc@hep1.phys.ntu.edu.tw ]
*
*******************************************************************************/
#include "DataFormats/FWLite/interface/Event.h"
#include "DataFormats/FWLite/interface/Handle.h"
#include "SimDataFormats/GeneratorProducts/interface/LHEEventProduct.h"

#include "ManagerUtils/PlotUtils/interface/Common.hpp"
#include "TstarAnalysis/Common/interface/GetEventWeight.hpp"

#include <boost/format.hpp>
#include <boost/program_options.hpp>
#include <iostream>
#include <map>

#include "TFile.h"

using namespace std;
namespace opt = boost::program_options;

int
main( int argc, char const* argv[] )
{
  opt::variables_map args;
  opt::options_description desc( "Options for ComputeGenWeightUnc" );
  desc.add_options()
    ( "inputfiles,i",  opt::value<vector<string> >()->required(), "Which EDM files to uses as input" )
    ( "title,t",         opt::value<string>()->required(), "title of sample" )
    ( "outputfile,o",    opt::value<string>()->required(), "prefix to plot files" )
  ;

  opt::store( opt::parse_command_line( argc, argv, desc ), args );
  opt::notify( args );

  const auto filelist = args["inputfiles"].as<vector<string> >();
  const auto idgroup  = GetPdfIdGrouping( filelist.front() );

  std::map<unsigned, double> pdfweightsum;
  std::map<unsigned, double> qcdweightsum;
  unsigned counter = 0;

  for( const auto& file : filelist ){
    fwlite::Event ev( TFile::Open( file.c_str() ) );

    for( ev.toBegin(); !ev.atEnd(); ++ev, ++counter ){
      cout << "\rEvent[" << counter<<  "]..." << flush;

      fwlite::Handle<LHEEventProduct> evthandle;
      evthandle.getByLabel( ev, "externalLHEProducer" );

      const auto pdfidlist = ( idgroup.front().front() == 1 ) ?
                             GetMadGraphIdList( ev, idgroup ) :
                             GetPowHegIdList( ev, idgroup );
      const auto qcdidlist = ( idgroup.front().front() == 1 ) ?
                             GetMadGraphScaleIdList( ev ) :
                             GetPowHegScaleIdList( ev );
      const double centralweight = ( idgroup.front().front() == 1 ) ?
                                   evthandle.ref().weights().front().wgt :
                                   evthandle.ref().originalXWGTUP();

      const double otherweights = GetEventWeight( ev );

      for( const auto& weightinfo : evthandle.ref().weights() ){
        const unsigned thisid   = stoi( weightinfo.id );
        const double thisweight = weightinfo.wgt / centralweight;
        if( find( pdfidlist.begin(), pdfidlist.end(), thisid ) != pdfidlist.end() ){
          pdfweightsum[thisid] += thisweight * otherweights;
        }
        if( find( qcdidlist.begin(), qcdidlist.end(), thisid ) != qcdidlist.end() ){
          qcdweightsum[thisid] += thisweight * otherweights;
        }
      }
    }
  }

  {
    double sumsq = 0;
    double sum   = 0;

    for( const auto weightsum : pdfweightsum ){
      cout << weightsum.first << ": " << weightsum.second << endl;
      sumsq += weightsum.second * weightsum.second;
      sum   += weightsum.second;
    }

    boost::format pdffmt( "PDF>> first:%lf, mean:%lf, rms:%lf, relerr:%lf" );
    const double size  = pdfweightsum.size();
    const double first = pdfweightsum.begin()->second;
    const double mean  = sum / size;
    const double rms   = sqrt( ( sumsq - ( ( sum*sum )/( size ) ) )/( size-1 ) );
    cout << pdffmt % first % mean % rms % ( rms/mean )
         << endl;

    TH1D* pdfplot = new TH1D( "", ";PDF variation (arbitray index);Sum of weights (normalized to mean)", pdfweightsum.size(), 0, pdfweightsum.size() );
    size_t index  = 0;

    for( const auto weightsum : pdfweightsum ){
      pdfplot->SetBinContent( index+1, weightsum.second/mean );
      index++;
    }

    TCanvas* c = mgr::NewCanvas();
    pdfplot->Draw( "HIST" );
    pdfplot->SetMaximum( std::max( pdfplot->GetMaximum(), 2.0 )*1.1 );
    pdfplot->SetMinimum( std::min( pdfplot->GetMinimum(), 0.0 )*0.9 );
    pdfplot->SetStats( 0 );

    mgr::SetAxis( pdfplot );
    mgr::SetSinglePad( c );
    mgr::DrawCMSLabel( SIMULATION );

    mgr::LatexMgr latex;
    latex.SetOrigin( PLOT_X_TEXT_MIN, PLOT_Y_TEXT_MAX, TOP_LEFT )
    .WriteLine( args["title"].as<string>() )
    .WriteLine( str( boost::format( "standard devaition (normalized):%lf" )%( rms/mean ) ) );

    mgr::SaveToPDF( c, args["outputfile"].as<string>() + "_pdfunc.pdf" );
  }
  {
    double minval = 1e20;
    double maxval = 0;
    double sum    = 0;
    double sumsq  = 0;

    for( const auto weightsum : qcdweightsum ){
      cout << weightsum.first << ": " << weightsum.second << endl;
      minval = std::min( minval, weightsum.second );
      maxval = std::max( maxval, weightsum.second );
      sum   += weightsum.second;
      sumsq += weightsum.second * weightsum.second;
    }

    const double size = qcdweightsum.size();
    // const double mean = sum / qcdweightsum.size();
    const double rms  = sqrt( ( sumsq - ( ( sum*sum )/( size ) ) )/( size-1 ) );

    boost::format qcdfmt( "QCD>> first:%lf, envelope:%lf/%lf, relenv:%lf/%lf(%lf)" );
    const double first = qcdweightsum.begin()->second;
    cout << qcdfmt % first
      % maxval % minval
      % ( ( maxval/first )-1 )
      % ( 1-( minval/first ) )
      %  ( ( ( maxval/first )-1 )/2 +  ( 1-( minval/first ) )/2 )
         << endl;

    TH1D* qcdplot = new TH1D( "", ";Scale variation (arbitray index);Sum of weights (normalized to mean)", qcdweightsum.size(), 0, qcdweightsum.size() );
    size_t index  = 0;

    for( const auto weightsum : qcdweightsum ){
      qcdplot->SetBinContent( index+1, weightsum.second/first );
      const string label =
        ( weightsum.first == 1 || weightsum.first == 1001 ) ? "#mu_{R}=1 #mu_{F}=1" :
        ( weightsum.first == 2 || weightsum.first == 1002 ) ? "#mu_{R}=1 #mu_{F}=2" :
        ( weightsum.first == 3 || weightsum.first == 1003 ) ? "#mu_{R}=1 #mu_{F}=0.5" :
        ( weightsum.first == 4 || weightsum.first == 1004 ) ? "#mu_{R}=2 #mu_{F}=1" :
        ( weightsum.first == 5 || weightsum.first == 1005 ) ? "#mu_{R}=2 #mu_{F}=2" :
        ( weightsum.first == 6 || weightsum.first == 1006 ) ? "#mu_{R}=2 #mu_{F}=0.5" :
        ( weightsum.first == 7 || weightsum.first == 1007 ) ? "#mu_{R}=0.5 #mu_{F}=1" :
        ( weightsum.first == 8 || weightsum.first == 1008 ) ? "#mu_{R}=0.5 #mu_{F}=2" :
        ( weightsum.first == 9 || weightsum.first == 1009 ) ? "#mu_{R}=0.5 #mu_{F}=0.5" :
        "";
      qcdplot->GetXaxis()->SetBinLabel( index+1, label.c_str() );
      index++;
    }

    TCanvas* c = mgr::NewCanvas();
    qcdplot->Draw( "HIST" );
    qcdplot->SetMaximum( std::max( qcdplot->GetMaximum(), 2.0 )*1.1 );
    qcdplot->SetMinimum( std::min( qcdplot->GetMinimum(), 0.0 )*0.9 );
    qcdplot->SetStats( 0 );

    mgr::SetAxis( qcdplot );
    mgr::SetSinglePad( c );
    c->SetBottomMargin(0.2);
    c->SetRightMargin(0.1);
    qcdplot->GetXaxis()->SetTitleOffset(1.5);
    mgr::DrawCMSLabel( SIMULATION );

    mgr::LatexMgr latex;
    latex.SetOrigin( PLOT_X_TEXT_MIN, PLOT_Y_TEXT_MAX, TOP_LEFT )
    .WriteLine( args["title"].as<string>() )
    .WriteLine( str( boost::format( "Max (normalized):%.3lf" )%( maxval/first ) ) )
    .WriteLine( str( boost::format( "Min (normalized):%.3lf" )%( minval/first ) ) )
    .WriteLine( str( boost::format( "S.D. (normalized):%.3lf" )%( rms/first ) ) );

    mgr::SaveToPDF( c, args["outputfile"].as<string>() + "_qcdunc.pdf" );
  }
  return 0;
}


#include <fstream>
#include <sstream>
#include <iostream>
#include <string>
#include <iomanip>
#include <optional>
#include <cctype>
#include <map>
#include <algorithm>
#include <vector>
#include <range/v3/view.hpp>
#include <range/v3/algorithm.hpp>
#include <boost/dynamic_bitset.hpp>
#include <complex>
using namespace std::literals::complex_literals;
#include "../aoc2020/matrix.h"
#include "../aoc2020/mypair.h"

namespace views = ranges::views;
enum {
  Clean = 0,
  Weakened,
  Infected,
  Flagged
};

class Game
{
  using Position = std::complex<int>;
  constexpr static inline int N = 20001;
  int origin = N / 2;

  FixedMatrix<char, N> m;
  Mypair cur_;
  bool valid() const noexcept
  {
    return (0 <= cur_.x && cur_.x < N && 0 <= cur_.y && cur_.y < N);
  }
  Position startdir_;
  static constexpr Position dir_[4] = { { 0, 1 }, { 1, 0 }, { 0, -1 }, { -1, 0 } };
  int cnt_ = 0;

public:
  Game() : cur_(origin, origin), startdir_(0, 1)
  {
  }
  int forward(int n)
  {
    for (int i = 0; (i < n) && valid(); ++i) {
      cnt_ += (m(cur_.x, cur_.y) == Weakened);
      turn(m(cur_.x, cur_.y));
      m(cur_.x, cur_.y) = (m(cur_.x, cur_.y) + 1) % 4;
      cur_.x -= startdir_.imag();
      cur_.y += startdir_.real();
    }
    if (!valid()) {
      std::cerr << "invalid index:" << cur_.x << cur_.y << std::endl;
    }
    return cnt_;
  }
  void turn(char state)
  {
    startdir_ *= dir_[state];
  }

  friend std::istream &operator>>(std::istream &is, Game &g)
  {
    std::string s;
    int N = 25;
    for (int i = g.origin - N / 2; std::getline(is, s); ++i) {
      std::vector<int> v(s.size());
      int N = v.size();

      std::transform(s.begin(), s.end(), v.begin(), [](char c) { return (c == '#') * 2; });
      for (int j = g.origin - N / 2; j < g.origin + N / 2 + 1; ++j) {
        g.m(i, j) = v[j - (g.origin - N / 2)];
      }
    }
    return is;
  }
};

int main(int argc, char **argv)
{
  if (argc > 1) {
    Game g;
    std::ifstream iss(argv[1]);
    iss >> g;
    std::cout << g.forward(10000000) << std::endl;
  }
}


#include <cstdio>
#include <limits>

#include <sstream>
#include <iomanip>

#include <boost/lexical_cast.hpp>

#include "../asserts.hpp"
#include "as3_value.hpp"
#include "../swf_player.hpp"

namespace avm2
{
	const char*	as3_value::to_string() const
	{
		switch(type_) {
			case UNDEFINED: return "undefined";
			case BOOLEAN: return b_ ? "true" : "false";
			case NUMERIC: {
				if(d_ == std::numeric_limits<double>::quiet_NaN()
					|| d_ == std::numeric_limits<double>::signaling_NaN()) {
					return "NaN";
				} else {
					std::stringstream ss_fixed;
					ss_fixed << std::setprecision(14) << std::ios::fixed << d_;
					std::string s_fix = ss_fixed.str();
					std::stringstream ss_sci;
					ss_fixed << std::setprecision(14) << std::ios::scientific << d_;
					std::string s_sci = ss_sci.str();
					if(s_sci.length() < s_fix.length()) {
						return s_sci.c_str();
					}
					return s_fix.c_str();
				}
			}
			case STRING: return s_.c_str();
			case OBJECT: return o_ == NULL ? "null" : o_->to_string();
			case PROPERTY:
				/*
				convert property p_ to a string.
				*/
			default: ASSERT_LOG(false, "to_string() bad type: " << type_);
		}
		return "undefined";
	}

	std::string as3_value::to_std_string()
	{
		return to_string();
	}

	as3_value operator+(const as3_value& v1, const as3_value& v2)
	{
		auto p1 = v1.to_primitive();
		auto p2 = v2.to_primitive();
		if(p1.type_ == as3_value::STRING || p2.type_ == as3_value::STRING) {
			return as3_value(p1.to_string() + p2.to_std_string());
		}
		return as3_value(p1.to_number() + p2.to_number());
	}

	as3_value as3_value::to_primitive(HintType hint) const
	{
		if(type_ == OBJECT && o_ != NULL) {
			return o_->default_value(hint);
		} else if(type_ == PROPERTY) {
			ASSERT_LOG(false, "XXX todo PROPERTY::to_primitive");
		}
		return *this;
	}

	int32_t as3_value::to_int32()
	{
		double num = to_number();
		if(num == std::numeric_limits<double>::quiet_NaN()
			|| num == std::numeric_limits<double>::signaling_NaN()
			|| abs(num) == 0
			|| abs(num) == std::numeric_limits<double>::infinity()) {
			return 0;
		}
		double pos_int = (num > 0 ? 1 : -1) * floor(abs(num));
		return int32_t(fmod(pos_int, 4294967296));
	}

	double as3_value::to_number()
	{
		switch(type_) {
			case UNDEFINED:		return std::numeric_limits<double>::infinity();
			case BOOLEAN:		return b_ ? 1 : 0;
			case NUMERIC:		return d_;
			case OBJECT:		return o_ == NULL ? 0 : o_->to_number();
			case PROPERTY: {
				ASSERT_LOG(false, "XXX todo PROPERTY::to_number");
			}
		}
		ASSERT_LOG(type_ != STRING, "FATAL: unknown type_ value: " << type_);
		// String case -- we do thing the lazy way, not the compliant way.
		double num = std::numeric_limits<double>::infinity();
		try {
			num = boost::lexical_cast<double>(s_);
		} catch(boost::bad_lexical_cast&) {
			std::cerr << "Caught a bad floating point cast from " << s_ << " assuming infinity" << std::endl;
		}
		return num;
	}
}


#ifndef LEVELTREE_H
#define LEVELTREE_H

#include <assert.h>                            // for assert
#include <boost/container/detail/std_fwd.hpp>  // for pair
#include <boost/icl/discrete_interval.hpp>     // for discrete_interval
#include <boost/icl/interval_map.hpp>          // for interval_map
#include <boost/icl/interval_set.hpp>          // for interval_set
#include <ostream>                             // for basic_ostream
#include <tuple>                               // for tuple
#include <vector>                              // for vector
#include "../instance/job.hpp"  // for Job, Job::JobId
#include "generated_config.hpp"                // for ENABLE_ASSERTIONS
#include "../util/log.hpp"                        // for Log
#include "jobset.hpp"

namespace level_tree {

class LinearLevelAssigner {
public:
  LinearLevelAssigner(double min, double max, unsigned int levels);
  unsigned int get_level(double amount) const;

  bool operator==(const LinearLevelAssigner & rhs) const;

  unsigned int level_count() const;

private:
  double min_val;
  double level_size;
  unsigned int levels;
};

} // namespace level_tree

template<class Key, class LevelAssigner>
class LevelTree {
public:
  using JobId = Job::JobId;
  using Interval = typename boost::icl::discrete_interval<Key>::type;
  using LevelProfile = std::vector<std::tuple<const Interval, unsigned int, double>>;
  using ContentData = std::vector<std::pair<const Interval, const JobSet>>;

  LevelTree(LevelAssigner level_assigner, Interval horizon);

  class Intervals {
  public:
    // TODO move them into a single template, only one operator is different!
    Intervals operator-(const Intervals &rhs) const;
    Intervals operator+(const Intervals &rhs) const;
    Intervals operator&(const Intervals &rhs) const;
    Intervals operator!() const;

    Intervals extend(Key extend_left, Key extend_right) const;
    Intervals move_left(Key amount) const;

    void push_back(Interval interval);
    const std::vector<Interval> & get() const;

    Key coverage() const;
    bool empty() const;

	  bool overlaps(const Interval & interval) const;

    // String output
    // Needs to be *defined* here :(
    template<class _CharT, class _Traits>
    friend std::basic_ostream<_CharT, _Traits>& operator<<(std::basic_ostream<_CharT, _Traits>& stream, const typename LevelTree<Key, LevelAssigner>::Intervals & intervals) {
      stream << "[ ";

      bool first = true;
      for (const auto & interval : intervals.get()) {
        if (!first) {
          stream << ", ";
        }
        stream << interval;
        first = false;
      }

      stream << " ]";

      return stream;
    }

  private:
    std::vector<Interval> content; // assumed to be ordered!

    template<class binop>
    Intervals operator_base(binop op, const Intervals & rhs) const;
  };

  static Interval make_interval(Key start, Key end)
  {
#ifdef ENABLE_ASSERTIONS
    assert(end > start);
#endif
    return Interval(start, end);
  }

  // TODO make this a move insert
  void insert(const Interval &interval, JobSet payload);
  void remove(const Interval &interval, const JobSet &payload);

  void insert(const Interval &interval, JobId job, double amount);
  void remove(const Interval &interval, JobId job, double amount);

  //void move(const Interval &old_interval, const Interval &new_interval, JobId job, double amount);

  unsigned int level_at(Key point);

  // TODO this requires copying…
  Intervals get_level(unsigned int level);
  LevelProfile get_profile(const Interval &query);
  LevelProfile get_profile();
  ContentData get_content();

  // operators
  bool operator==(const LevelTree<Key, LevelAssigner> & rhs);

  void dbg_verify();

  // String output
  // Needs to be *defined* here :(
  template<class _CharT, class _Traits>
  friend std::basic_ostream<_CharT, _Traits>& operator<<(std::basic_ostream<_CharT, _Traits>& stream, const typename LevelTree<Key, LevelAssigner>::Interval & interval);

private:
  void update_levels(const LevelProfile &oldp, const LevelProfile &newp);

  std::vector<boost::icl::interval_set<Key>> levels;
  boost::icl::interval_map<Key, JobSet> content;

  LevelAssigner level_assigner;
  // TODO should be const…
  Interval horizon;

  // Interval comparison
  bool strictly_before(const Interval &lhs, const Interval &rhs);
  bool strictly_after(const Interval &lhs, const Interval &rhs);
  bool contains(const Interval &lhs, const Interval &rhs);

  // TODO FIXME move to unit-test?
  void dbg_verify_level_containment();
  void dbg_verify_level_assignment();

  Log l;
};

#endif


#ifndef GRAPHLAB_SAMPLING_TREE_HPP
#define GRAPHLAB_SAMPLING_TREE_HPP

#include <vector>

#include <boost/integer.hpp>
#include <boost/random.hpp>
#include <boost/random/linear_congruential.hpp>

#include <graphlab/parallel/pthread_tools.hpp>
#include <graphlab/parallel/atomic.hpp>

#include <graphlab/util/synchronized_queue.hpp>

namespace graphlab {

  template <typename Data, typename Priority>
  class sampling_tree{
  private:

    typedef boost::lagged_fibonacci607 random_source;
    //typedef boost::minstd_rand0 random_source;
    typedef boost::uniform_01<random_source> distribution_type;
    std::vector<random_source> rngs;
    std::vector<distribution_type> distributions;

    // each element in the tree is the sum of the elements in the children
    std::vector<Priority> tree;

    // here we store all the leaves.
    std::vector< synchronized_queue<std::pair<Data, Priority> > > leaves;
    std::vector<spinlock> locks;
    atomic<size_t> num_elements;

    size_t max_key_value;

    size_t tree_interior_size;

    Priority minpriority;

    size_t nextpow2(size_t val) {
      --val;
      val = val | (val >> 1);
      val = val | (val >> 2);
      val = val | (val >> 4);
      val = val | (val >> 8);
      val = val | (val >> 16);
#ifdef _LP64
      val = val | (val >> 32);
#endif
      return val + 1;
    }

    /// Returns the index of a leaf sampled proportionate to its priority.
    /// Returns false on failure
    bool sample_tree(size_t &leaf, size_t cpuid) {
      size_t loc = 0;
      while (loc < tree_interior_size) {
        // get the left and right priorities
        Priority left = tree[loc * 2 + 1];
        Priority right = tree[loc * 2 + 2];
        // if both are zero, the sample has failed. Return
        if (left + right == 0) return false;
        else if (right == 0) {
          // if right is zero, move left
          loc = loc * 2 + 1;
        } else if (left == 0) {
          // if left is zero move right
          loc = loc * 2 + 2;
        } else {
          // pick from a bernoulli trial
          Priority childsum = left + right;

          double rndnumber = distributions[cpuid]() ;
          // double rndnumber = rand();
          Priority sel = rndnumber * childsum;
          loc = loc * 2 + 1 + (sel > left);
        }
      }
      leaf = loc - tree_interior_size;
      return true;
    }

    /// Pops an entry on a particular leaf. Returns false on failure
    bool pop_leaf(size_t leaf, std::pair<Data, Priority> &entry) {
      locks[leaf].lock();
      if (leaves[leaf].safepop(&entry)) {

        if (leaves[leaf].size() > 0) {
          tree[leaf + tree_interior_size] -= entry.second;
          if (tree[leaf + tree_interior_size] < 0)
            tree[leaf + tree_interior_size] = 0;
        } else {
          tree[leaf + tree_interior_size] = 0;
        }

        locks[leaf].unlock();
        return true;
      }
      locks[leaf].unlock();
      return false;
    }

    /// Propagates a cumulative sum update up the tree.
    void propagate_up(size_t leaf) {
      leaf += tree_interior_size;
      while (leaf != 0) {
        volatile Priority *sibling1 = &(tree[leaf]);
        // the binary here is equivalent to (+1) if leaf is odd and (-1) if
        // leaf is even
        volatile Priority *sibling2 = &(tree[leaf + (leaf & 1)*2 - 1]);

        leaf = (leaf - 1) >> 1;
        volatile Priority *parent = &(tree[leaf]);
        // write to the parent. Use a concurrent write mechanism
        do {
          (*parent) = (*sibling1) + (*sibling2);
        } while((*parent) != (*sibling1) + (*sibling2));
      }
    }

  public:

    sampling_tree(size_t maxkeyval,
                  Priority minimumpriority = 1E-10,
                  size_t ncpus = 16) : rngs(ncpus){
      for(size_t i = 0; i < rngs.size(); ++i) {
        rngs[i].seed(rand());
        distributions.push_back(distribution_type(rngs[i]));

      }

      max_key_value = maxkeyval;
      minpriority = minimumpriority;
      // create the leaves
      //leaves.resize(maxkeyval, synchronized_queue<std::pair<Data, Priority> >(1));
      leaves.resize(maxkeyval);
      locks.resize(maxkeyval);
      // create the tree. (full binary tree
      tree_interior_size = nextpow2(maxkeyval) - 1;
      size_t treesize = 2 * nextpow2(maxkeyval) - 1;
      tree.resize(treesize, Priority(0));
      num_elements.value = 0;
    }

    ~sampling_tree(){}

    size_t size() {
      return num_elements.value;
    }

    bool pop(size_t &key , Data &ret, Priority &pr, size_t cpuid = 0) {
      if (size() == 0) return false;
      std::pair<Data, Priority> dppair;
      while (true) {
        if (sample_tree(key, cpuid)) {
          if (pop_leaf(key, dppair)) {
            propagate_up(key);
            break;
          }
        }
        if (size() == 0) {
          return false;
        }
      }
      ret = dppair.first;
      pr = dppair.second;
      num_elements.dec();
      return true;
    }

    bool sample(size_t &k, size_t cpuid = 0) {
      if (size() == 0) return false;
      while (true) {
        if (sample_tree(k, cpuid)) {
          return true;
        } else if (size() == 0) {
          return false;
        }
      }
    }

    void push(const size_t& key, const Data& item, const Priority &pr) {
      num_elements.inc();

      locks[key].lock();
      leaves[key].push(std::make_pair(item, std::max(pr, minpriority)));
      tree[key + tree_interior_size] += pr;
      locks[key].unlock();

      propagate_up(key);
    }

    void increase_priority(const size_t& key, const Priority &pr) {
      // try to add my priority into the leaf. use CAS to make sure the
      // add gets in
      locks[key].lock();
      tree[key + tree_interior_size] += pr;
      locks[key].unlock();

      propagate_up(key);
    }

    void print_tree() {
      for (size_t i = 0;i < std::min(tree.size(),size_t(1000)); ++i) {
        std::cout << tree[i] << " ";
      }
    }
  };

}
#endif


// SPDX-FileCopyrightText: 2020 Nils Funk, Imperial College London
// SPDX-License-Identifier: BSD-3-Clause

#ifndef __COMMON_HPP
#define __COMMON_HPP

#include <Eigen/Dense>
#include "se/octree.hpp"
#include "se/node.hpp"

/**
 * \brief Finds the first valid point along a ray starting from (ray_origin_M + t * ray_dir_M). Returns false if no
 * valid point can be found before the maximum travelled distance is reached.
 *
 * \param[in]     map                Reference to the octree
 * \param[in]     select_node_value  lambda function selecting the node value to be interpolated
 * \param[in]     select_voxel_value lambda function selecting the voxel value to be interpolated
 * \param[in]     ray_origin_M       Origin of the ray in map frame [m]
 * \param[in]     ray_dir_M          Direction of the ray in map frame [m]
 * \param[in]     step_size          Size of a step per iteration [m]
 * \param[in]     t_max              Maximum travelled distance along the ray [m]
 * \param[in,out] t                  Travelled distance along the ray [m]
 * \param[out]    f                  Interpolated value at ray_origin_M + t * ray_dir_M
 * \param[out]    p                  First valid point along the ray starting from
 *                                   the input t (ray_origin_M + t * ray_dir_M)
 * \return        is_valid           True if valid point could be found before reaching t_max. False otherwise.
 */
template <typename FieldType, template <typename FieldT> class OctreeT,
          typename NodeValueSelector,
          typename VoxelValueSelector>
bool find_valid_point(const OctreeT<FieldType>& map,
                      NodeValueSelector         select_node_value,
                      VoxelValueSelector        select_voxel_value,
                      const Eigen::Vector3f&    ray_origin_M,
                      const Eigen::Vector3f&    ray_dir_M,
                      const float               step_size,
                      const float               t_max,
                      float&                    t,
                      float&                    value,
                      Eigen::Vector3f&          point_M) {
  bool is_valid = false;
  value = 0;
  typename FieldType::VoxelData peek_data;

  Eigen::Vector3f ray_pos_M = ray_origin_M + t * ray_dir_M;
  if (map.containsPoint(ray_pos_M)) {
    map.getAtPoint(ray_pos_M, peek_data, 0);
    if (peek_data.y > 0) {
      value = map.interpAtPoint(ray_pos_M, select_node_value, select_voxel_value, 0, is_valid).first;
    }
  }

  while (!is_valid) {
    t += step_size;
    if (t > t_max) {
      return false;
    }

    ray_pos_M = ray_origin_M + t * ray_dir_M;
    if (map.containsPoint(ray_pos_M)) {
      map.getAtPoint(ray_pos_M, peek_data, 0);
      if (peek_data.y > 0) {
        value = map.interpAtPoint(ray_pos_M, select_node_value, select_voxel_value, 0, is_valid).first;
      }
    }
  }
  point_M = ray_pos_M;
  return true;
}

/**
 * \brief Finds the first valid point along a ray starting from (ray_origin_M + t * ray_dir_M). Returns false if no
 * valid point can be found before the maximum travelled distance is reached.
 *
 * \param[in]     map                Reference to the octree
 * \param[in]     select_value       lambda function selecting the node and voxel value to be interpolated
 * \param[in]     ray_origin_M       Origin of the ray in map frame [m]
 * \param[in]     ray_dir_M          Direction of the ray in map frame [m]
 * \param[in]     step_size          Size of a step per iteration [m]
 * \param[in]     t_max              Maximum travelled distance along the ray [m]
 * \param[in,out] t                  Travelled distance along the ray [m]
 * \param[out]    f                  Interpolated value at ray_origin_M + t * ray_dir_M
 * \param[out]    p                  First valid point along the ray starting from
 *                                   the input t (ray_origin_M + t * ray_dir_M)
 * \return        is_valid           True if valid point could be found before reaching t_max. False otherwise.
 */
template <typename FieldType, template <typename FieldT> class OctreeT,
          typename ValueSelector>
bool find_valid_point(const OctreeT<FieldType>& map,
                      ValueSelector             select_value,
                      const Eigen::Vector3f&    ray_origin_M,
                      const Eigen::Vector3f&    ray_dir_M,
                      const float               step_size,
                      const float               t_max,
                      float&                    t,
                      float&                    value,
                      Eigen::Vector3f&          point_M) {

  return find_valid_point(map, select_value, select_value, ray_origin_M, ray_dir_M, step_size, t_max, t, value, point_M);
}

#endif //__COMMON_HPP


#pragma once

#include <string>
#include <limits>
#include <utility>
#include <map>
#include <stack>
#include <cmath>

#include <boost/lexical_cast.hpp>
#include "fmt/format.h"
#include <boost/math/constants/constants.hpp>

#include "GNAObject.hh"
#include "parameters/parameter.hh"

/* #define COVARIANCE_DEBUG */

template <typename T>
class ParameterWrapper {
public:
  ParameterWrapper(const std::string &name)
    : m_var(name.c_str()) { }
  ParameterWrapper(parameter<void> &pvar)
    : m_var(pvar) { }

  T value() const noexcept { return m_var; }
  void set(T value) { m_var = value; }

  const variable<T> &getVariable() const noexcept { return m_var; }
protected:
  parameter<T> m_var;
};

template <typename T>
class Variable: public GNASingleObjectT<T,T>,
                public TransformationBind<Variable<T>,T,T>
{
public:
  Variable(const std::string &name, bool allocate=false)
    : m_varhandle(variable_(&m_var, name, static_cast<size_t>(allocate))), m_name(name)
  {}

  Variable(const std::string &name, variable<void> var)
    : Variable(name)
  {
    m_varhandle.bind(variable<T>(var));
    this->transformations.front().updateTypes();
  }

  ~Variable() override = default;

  virtual T value() const noexcept { return m_var.value(); }
  const variable<T> &getVariable() const noexcept { return m_var; }

  std::string name() const noexcept { return m_name; }

  std::string inNamespace() const noexcept { return m_namespace; }
  void setNamespace(const std::string& ns_name) { m_namespace = ns_name; };
  std::string qualifiedName() const { return m_namespace + "." + m_name; };

  std::string label() const noexcept { return m_label; }
  void setLabel(const std::string& label) { m_label=label; }
  void setLabel(std::string&& label) { m_label = label; }

  size_t hash() const {return reinterpret_cast<size_t>(this);}

protected:
  variable<T> m_var{};
  ParametrizedTypes::VariableHandle<T> m_varhandle;
  std::string m_name;
  std::string m_label;
  std::string m_namespace;
};

template <>
inline Variable<double>::Variable(const std::string &name, bool allocate)
  : m_varhandle(variable_(&m_var, name, static_cast<size_t>(allocate))), m_name(name)
{
  transformation_("value")
    .output(name)
    .types([](Variable<double> *obj, TypesFunctionArgs& fargs) {
        fargs.rets[0] = DataType().points().shape(obj->m_var.size());
      })
    .func([](Variable<double> *obj, FunctionArgs& fargs) {
        obj->m_var.values(fargs.rets[0].buffer);
      })
    .finalize();
}

#ifdef PROVIDE_SINGLE_PRECISION
template <>
inline Variable<float>::Variable(const std::string &name, bool allocate)
  : m_varhandle(variable_(&m_var, name, static_cast<size_t>(allocate))), m_name(name)
{
  transformation_("value")
    .output(name)
    .types([](Variable<float> *obj, TypesFunctionArgs& fargs) {
        fargs.rets[0] = DataType().points().shape(obj->m_var.size());
      })
    .func([](Variable<float> *obj, FunctionArgs& fargs) {
        obj->m_var.values(fargs.rets[0].buffer);
      })
    .finalize();
}
#endif

template <typename T>
class Parameter: public Variable<T> {
public:
  Parameter(const std::string &name)
    : Variable<T>(name, true/*allocate*/), m_par(this->m_varhandle.claim())
    {
      this->transformations.front().updateTypes();
    }

  virtual void set(T value)
    { m_par = value; }

  T push() {
    m_stack.push(this->value());
    return this->value();
  }

  T push(T value) {
    m_stack.push(this->value());
    this->set(value);
    return value;
  }

  T pop() {
    if(!m_stack.empty()){
      this->set(m_stack.top());
      m_stack.pop();
    }
    return this->value();
  }

  virtual T cast(const std::string& v) const
    { return boost::lexical_cast<T>(v); }

  virtual T cast(const T& v) const noexcept
    { return v; }

  T central() const noexcept { return m_central; }
  void setCentral(T value) noexcept { m_central = value; }
  void reset() { set(this->central()); }

  T step() const noexcept { return m_step; }
  void setStep(T step) noexcept { m_step = step; }

  T relativeValue(T diff) const noexcept
    { return this->value() + diff*this->m_step; }
  void setRelativeValue(T diff)
    { set(relativeValue(diff)); }

  void addLimits(T min, T max)
    { m_limits.push_back(std::make_pair(min, max)); }

  const std::vector<std::pair<T, T>>& limits() const
    { return m_limits; }

  bool influences(SingleOutput &out) const {
    return out.single().depends(this->getVariable());
  }

  bool isFixed() const noexcept { return this->m_fixed; }
  void setFixed() noexcept { this->m_fixed = true; }

  bool isFree() const noexcept { return this->m_free; }
  void setFree(bool free=true) noexcept { this->m_free = free; }

  const parameter<T>& getParameter() const noexcept { return m_par; }

protected:
  T m_central;
  T m_step;
  std::vector<std::pair<T, T>> m_limits;
  parameter<T> m_par;
  bool m_fixed = false;
  bool m_free = false;

  std::stack<T> m_stack;
};

template <typename T>
class GaussianParameter: public Parameter<T> {
public:
  GaussianParameter(const std::string &name)
    : Parameter<T>(name) { }
  std::vector<GaussianParameter<T>*> m_cov_pars{};

  T sigma() const noexcept { return m_sigma; }
  void setSigma(T sigma) noexcept {
    this->m_sigma=sigma;
    if(std::isinf(sigma)){
      this->setFree();
    }else{
      this->setFree(false);
      this->setStep(sigma*0.1);
    }
  }

   void setRelSigma(T relsigma) {
     auto central = this->central();
     if(central==static_cast<T>(0)){
       throw std::runtime_error("May not set relative uncertainty to the parameter with central=0");
     }
     this->setSigma(central*relsigma);
   }

  bool isCorrelated(const GaussianParameter<T>& other) const noexcept {
    auto it = this->m_covariances.find(&other);
    if (it == this->m_covariances.end() and (&other != this)) {
      return false;
    } else {
      return true;
    }
  }

  bool isCorrelated() const noexcept {
    return !m_covariances.empty();
  }

  std::vector<GaussianParameter<T>*>  getAllCorrelatedWith() const {
    std::vector<GaussianParameter<T>*> tmp;
    for (const auto& item: this->m_covariances){
      tmp.push_back(const_cast<GaussianParameter<T>*>(item.first));
    }
    return tmp;
  }

  void setCovariance(GaussianParameter<T>& other, T cov) {
#ifdef COVARIANCE_DEBUG
    fmt::print("Covariance of parameters {0} and {1} is set to {2}",
               this->name(), other.name(), cov);
#endif
    if (&other != this) {
      this->m_covariances[&other] = cov;
      other.updateCovariance(*this, cov);
    } else {
      this->setSigma(std::sqrt(cov));
    }
  }

  void updateCovariance(GaussianParameter<T>& other, T cov) {
#ifdef COVARIANCE_DEBUG
    fmt::print("Covariance of parameters {0} and {1} is updated "
               "to {2} after setting in {0}", this->name(), other.name(), cov);
#endif
    this->m_covariances[&other] = cov;
  }

  T getCovariance(const GaussianParameter<T>& other) const noexcept {
    if (this == &other) {return this->sigma()*this->sigma();}
    auto search = m_covariances.find(&other);
    if (search != m_covariances.end()) {
      return search->second;
    } else  {
#ifdef COVARIANCE_DEBUG
      fmt::print("Parameters {0} and {1} are not covariated", this->name(), other.name());
#endif
      return static_cast<T>(0.);
    }
  }

  T getCorrelation(const GaussianParameter<T>& other) const noexcept {
    if (this == &other) {return static_cast<T>(1);}
    auto search = m_covariances.find(&other);
    if (search != m_covariances.end()) {
      return search->second / (this->sigma() * other.sigma());
    } else  {
#ifdef COVARIANCE_DEBUG
      fmt::print("Parameters {0} and {1} are not covariated", this->name(), other.name());
#endif
      return static_cast<T>(0.);
    }
  }

  T normalValue(T reldiff) const noexcept
  { return this->central() + reldiff*this->m_sigma; }

  void setNormalValue(T reldiff)
  { this->set(this->normalValue(reldiff)); }
protected:
  using CovStorage = std::map<const GaussianParameter<T>*, T>;
  T m_sigma;
  CovStorage m_covariances;
};

template <typename T>
class UniformAngleParameter: public Parameter<T> {
public:
  UniformAngleParameter(const std::string &name)
    : Parameter<T>(name)
    { this->setStep(0.017453292519943295); /*1 degree*/ }

  void set(T value) override;

  T cast(const std::string &v) const override;
  T cast(const T &v) const noexcept override { return Parameter<T>::cast(v); }
};

template <typename T>
inline void UniformAngleParameter<T>::set(T value) {
  const double pi = boost::math::constants::pi<T>();

  T v = value - static_cast<int>(value/pi/2)*2*pi;
  if (v >= pi) {
    v -= 2*pi;
  }
  else if (v<-pi){
    v += 2*pi;
  }
  Parameter<T>::set(v);
}

template <typename T>
inline T UniformAngleParameter<T>::cast(const std::string &v) const {
  try {
    return boost::lexical_cast<T>(v);
  } catch (const boost::bad_lexical_cast&) {
  }
  const double pi = boost::math::constants::pi<T>();
  size_t pipos = v.find("pi");
  if (pipos == std::string::npos) {
    throw std::runtime_error("invalid uniform angle value");
  }
  T a = boost::lexical_cast<T>(v.substr(0, pipos));
  T b = boost::lexical_cast<T>(v.substr(pipos+2));
  return a*pi/b;
}


#pragma once
#include <vector>
#include <algorithm>
#include <memory>
#include <random>
#include <chrono>
#include <iostream>
#include <cstring>
#include <Eigen/Core>
#include "agent.hpp"

using std::vector;

constexpr int world_size = 128;

enum Block {
  Empty = 0,
  Wall,
  Food,
};

struct Pixel {
  uint8_t block : 4;
  uint8_t pheromone_1 : 1;
  uint8_t pheromone_2 : 1;
  uint8_t reserved_1 : 1;
  uint8_t reserved_2 : 1;
};

struct Agent {
  int x, y;
  float state[agent_num_states];
};

using PixelMatrix = Eigen::Matrix<uint8_t, world_size, world_size>;

class World {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  int total_score_ = 0;
  Pixel pixels_[world_size*world_size];
  vector<Agent> agents_;

  explicit World(unsigned int seed) {
    rng_.seed(seed);
  }

  void init_map(PixelMatrix walls, PixelMatrix food) {
    std::memset(pixels_, 0, sizeof(pixels_));
    for (int y=0; y<world_size; y++) {
      for (int x=0; x<world_size; x++) {
        Block b = Empty;
        if (food(y, x) != 0) b = Food;
        if (walls(y, x) != 0) b = Wall;
        pixels_[y*world_size+x].block = b;
      }
    }
  }

  void init_agents(const AgentController &ac, int agent_count, bool easy_start, float walk_through_wall_prob) {
    walk_through_wall_prob_ = walk_through_wall_prob;
    agentController_ = ac;
    constexpr int N = world_size;
    int spread = easy_start ? N/4 : N/12;
    std::uniform_int_distribution<int> pos_dist(N/2 - spread, N/2 + spread);
    std::normal_distribution<float> state_dist(0.0, 1.0);

    std::default_random_engine rng_fixed;
    rng_fixed.seed(1);
    std::default_random_engine &rng_init = easy_start ? rng_fixed : rng_;

    for (int i=0; i<agent_count; i++) {
      Agent a;
      do {
        a.x = pos_dist(rng_init);
        a.y = pos_dist(rng_init);
      } while (easy_start && pixel_at(a.x, a.y).block != Empty);
      for (auto &v : a.state) {
        v = state_dist(rng_init);
      }
      agents_.push_back(a);
    }
  }

  void tick() {
    tick_agents();
    tick_pheromones();
  }

 private:
  AgentController agentController_;
  std::default_random_engine rng_;
  float walk_through_wall_prob_;

  inline Pixel& pixel_at(int x, int y) {
    unsigned int x_ = static_cast<unsigned>(x) % world_size;
    unsigned int y_ = static_cast<unsigned>(y) % world_size;
    return pixels_[y_*world_size+x_];
  }

  void tick_agents() {
    constexpr int N = world_size;
    std::normal_distribution<float> normal_dist(0.0f, 1.0f);
    std::bernoulli_distribution walk_through_wall(static_cast<float>(walk_through_wall_prob_));
    for (auto &a: agents_) {
      pixels_[a.y*N + a.x].pheromone_1 = 1;

      Eigen::Matrix<float, agent_num_inputs, 1> inputs;
      int idx = 0;
      // see Wall
      inputs(idx++) = pixel_at(a.x+0, a.y+0).block == Wall ? 1 : 0;
      inputs(idx++) = pixel_at(a.x+1, a.y+0).block == Wall ? 1 : 0;
      inputs(idx++) = pixel_at(a.x-1, a.y+0).block == Wall ? 1 : 0;
      inputs(idx++) = pixel_at(a.x+0, a.y+1).block == Wall ? 1 : 0;
      inputs(idx++) = pixel_at(a.x+0, a.y-1).block == Wall ? 1 : 0;
      // see Food
      // inputs(idx++) = pixel_at(a.x+0, a.y+0).block == Food ? 1 : 0;  // always 0
      inputs(idx++) = pixel_at(a.x+1, a.y+0).block == Food ? 1 : 0;
      inputs(idx++) = pixel_at(a.x-1, a.y+0).block == Food ? 1 : 0;
      inputs(idx++) = pixel_at(a.x+0, a.y+1).block == Food ? 1 : 0;
      inputs(idx++) = pixel_at(a.x+0, a.y-1).block == Food ? 1 : 0;
      // smell Pheromone
      int count_pheremone_1 = 0;
      // count_pheremone_1 += pixel_at(a.x+0, a.y+0).pheromone_1;  // always 1
      count_pheremone_1 += pixel_at(a.x+1, a.y+0).pheromone_1;
      count_pheremone_1 += pixel_at(a.x-1, a.y+0).pheromone_1;
      count_pheremone_1 += pixel_at(a.x+0, a.y+1).pheromone_1;
      count_pheremone_1 += pixel_at(a.x+0, a.y-1).pheromone_1;
      inputs(idx++) = count_pheremone_1 * (1.0f / 5.0f);
      // know previous state
      for (int i=0; i<agent_num_states; i++) inputs[idx++] = a.state[i];
      // one random input
      inputs(idx++) = normal_dist(rng_);

      // assert that cannot be turned off (but probably will be optimized away)
      if (idx != agent_num_inputs) {
        std::cout << "produced " << idx << " inputs, expected " << agent_num_inputs << std::endl;
        exit(1);
      }

      AgentAction action = agentController_.calc(inputs, a.state, rng_);

      int dx = 0;
      int dy = 0;
      switch (action) {
        case AgentAction::Right: dx = +1; break;
        case AgentAction::Down:  dy = +1; break;
        case AgentAction::Left:  dx = -1; break;
        case AgentAction::Up:    dy = -1; break;
      }

      if (pixel_at(a.x + dx, a.y + dy).block != Wall || walk_through_wall(rng_)) {
        a.x = static_cast<unsigned>(a.x + dx) % world_size;
        a.y = static_cast<unsigned>(a.y + dy) % world_size;
      }

      pixels_[a.y*N + a.x].pheromone_1 = 1;
      if (pixels_[a.y*N + a.x].block == Food) {
        pixels_[a.y*N + a.x].block = Empty;
        total_score_++;
      }
    }
  }

  void tick_pheromones() {
    constexpr int N = world_size;
    std::geometric_distribution<> dist(1.0f/128);
    int idx = dist(rng_);
    while (idx < N*N) {
      pixels_[idx].pheromone_1 = 0;
      idx += dist(rng_) + 1;
    }
    // std::random_shuffle(agents_.begin(), agents_.end());
  }
};