#include "Curvilinear.h"
#include "Affine.h"
#include "Vector.h"
#include "mesh/LocalSimplexMesh.h"
#include "mesh/MeshData.h"
#include "tensor/EigenMap.h"
#include "tensor/Managed.h"
#include "tensor/Reshape.h"
#include "util/Math.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU>

#include <algorithm>
#include <cassert>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <iterator>
#include <memory>
#include <stdexcept>

namespace tndm {

template <std::size_t D>
Curvilinear<D>::Curvilinear(LocalSimplexMesh<D> const& mesh, transform_t transform, unsigned degree,
                            NodesFactory<D> const& nodesFactory)
    : N(degree), refElement_(degree, nodesFactory) {
    // Get vertices
    std::size_t vertsPerElement = refElement_.refNodes().size();

    auto storage =
        std::make_shared<mneme::SingleStorage<Verts>>(mesh.numElements() * vertsPerElement);
    vertices.setStorage(storage, 0, mesh.numElements(), vertsPerElement);

    auto vertexData = dynamic_cast<VertexData<D> const*>(mesh.vertices().data());
    if (!vertexData) {
        throw std::runtime_error("Expected vertex data");
    }

    std::size_t vertexNo = 0;
    local_mesh_size_ = 0.0;
    for (auto const& element : mesh.elements()) {
        auto vlids = mesh.template downward<0>(element);
        std::array<std::array<double, D>, D + 1> verts;
        std::size_t localVertexNo = 0;
        for (auto const& vlid : vlids) {
            verts[localVertexNo++] = vertexData->getVertices()[vlid];
        }
        RefPlexToGeneralPlex<D> map(verts);
        for (auto& refNode : refElement_.refNodes()) {
            (*storage)[vertexNo] = transform(map(refNode));
            ++vertexNo;
        }

        for (auto& x : verts) {
            x = transform(x);
        }
        for (auto const& x : verts) {
            for (auto const& y : verts) {
                auto h = norm(x - y);
                local_mesh_size_ = std::max(local_mesh_size_, h);
            }
        }
    }

    Simplex<D> refPlex = Simplex<D>::referenceSimplex();
    f2v = refPlex.downward();

    // Compute reference normals
    std::size_t fsNo = 0;
    for (auto const& f : f2v) {
        std::array<double, D> normal;
        if constexpr (D == 1u) {
            normal[0] = 1.0;
        } else if constexpr (D == 2u) {
            // Compute normal by rotating edge
            normal = refVertices[f[1]] - refVertices[f[0]];
            std::swap(normal[0], normal[1]);
            normal[0] *= -1.0;
        } else {
            // Compute normal by cross product
            auto e1 = refVertices[f[1]] - refVertices[f[0]];
            auto e2 = refVertices[f[2]] - refVertices[f[0]];
            normal = cross(e1, e2);
        }

        refNormals[fsNo] = Eigen::Map<Eigen::Matrix<double, D, 1>>(normal.data());

        std::vector<uint64_t> missingVertex;
        std::set_difference(refPlex.begin(), refPlex.end(), f.begin(), f.end(),
                            std::inserter(missingVertex, missingVertex.begin()));
        assert(missingVertex.size() == 1);

        auto edge = refVertices[missingVertex[0]] - refVertices[f[0]];
        if (dot(normal, edge) > 0) {
            refNormals[fsNo] *= -1.0;
        }
        ++fsNo;
    }
}

template <std::size_t D>
TensorBase<Matrix<double>> Curvilinear<D>::mapResultInfo(std::size_t numPoints) const {
    return TensorBase<Matrix<double>>(D, numPoints);
}

template <std::size_t D>
void Curvilinear<D>::map(std::size_t eleNo, Matrix<double> const& E,
                         Tensor<double, 2u>& result) const {
    assert(eleNo < vertices.size());
    assert(result.shape(0) == D);
    assert(result.shape(1) == E.shape(1));
    assert(E.shape(0) == refElement_.numBasisFunctions());

    auto vertexSpan = vertices[eleNo];
    assert(vertexSpan.size() == refElement_.numBasisFunctions());
    Eigen::Map<Eigen::Matrix<double, D, Eigen::Dynamic>> vertMap(vertexSpan.data()->data(), D,
                                                                 refElement_.numBasisFunctions());
    EigenMap(result) = vertMap * EigenMap(E);
}

template <std::size_t D>
TensorBase<Tensor<double, 3u>> Curvilinear<D>::jacobianResultInfo(std::size_t numPoints) const {
    return TensorBase<Tensor<double, 3u>>(D, D, numPoints);
}

template <std::size_t D>
void Curvilinear<D>::jacobian(std::size_t eleNo, Tensor<double, 3u> const& gradE,
                              Tensor<double, 3u>& result) const {
    assert(eleNo < vertices.size());

    auto vertexSpan = vertices[eleNo];
    assert(vertexSpan.size() == refElement_.numBasisFunctions());
    Eigen::Map<Eigen::Matrix<double, D, Eigen::Dynamic>> vertMap(vertexSpan.data()->data(), D,
                                                                 refElement_.numBasisFunctions());

    assert(gradE.shape(0) == refElement_.numBasisFunctions());
    assert(gradE.shape(1) == D);
    auto gradEMat = reshape(gradE, refElement_.numBasisFunctions(), D * gradE.shape(2));

    assert(result.shape(0) == D);
    assert(result.shape(1) == D);
    assert(result.shape(2) == gradE.shape(2));
    auto resultMat = reshape(result, D, D * gradE.shape(2));

    EigenMap(resultMat) = vertMap * EigenMap(gradEMat);
}

template <std::size_t D>
void Curvilinear<D>::jacobianInv(Tensor<double, 3u> const& jacobian,
                                 Tensor<double, 3u>& result) const {
    for (std::ptrdiff_t i = 0; i < result.shape(2); ++i) {
        auto jAtP = jacobian.subtensor(slice{}, slice{}, i);
        auto resAtP = result.subtensor(slice{}, slice{}, i);
        EigenMap<Matrix<double>, D, D>(resAtP) =
            EigenMap<const Matrix<double>, D, D>(jAtP).inverse();
    }
}

template <std::size_t D>
TensorBase<Vector<double>> Curvilinear<D>::detJResultInfo(std::size_t numPoints) const {
    return TensorBase<Vector<double>>(numPoints);
}

template <std::size_t D>
void Curvilinear<D>::detJ(std::size_t eleNo, Tensor<double, 3u> const& jacobian,
                          Tensor<double, 1u>& result) const {
    for (std::ptrdiff_t i = 0; i < result.shape(0); ++i) {
        auto jAtP = jacobian.subtensor(slice{}, slice{}, i);
        result(i) = EigenMap<const Matrix<double>, D, D>(jAtP).determinant();
    }
}

template <std::size_t D>
void Curvilinear<D>::absDetJ(std::size_t eleNo, Tensor<double, 3u> const& jacobian,
                             Tensor<double, 1u>& result) const {
    for (std::ptrdiff_t i = 0; i < result.shape(0); ++i) {
        auto jAtP = jacobian.subtensor(slice{}, slice{}, i);
        result(i) = std::fabs(EigenMap<const Matrix<double>, D, D>(jAtP).determinant());
    }
}

template <std::size_t D>
TensorBase<Matrix<double>> Curvilinear<D>::normalResultInfo(std::size_t numPoints) const {
    return TensorBase<Matrix<double>>(D, numPoints);
}

template <std::size_t D>
void Curvilinear<D>::normal(std::size_t faceNo, Tensor<double, 1u> const& detJ,
                            Tensor<double, 3u> const& jInv, Tensor<double, 2u>& result) const {
    assert(faceNo < D + 1u);
    // n_{iq} = |J|_q J^{-T}_{ijq} N_j
    for (std::ptrdiff_t i = 0; i < detJ.shape(0); ++i) {
        auto jInvAtP = jInv.subtensor(slice{}, slice{}, i);
        auto res = result.subtensor(slice{}, i);
        EigenMap<Vector<double>, D>(res) =
            std::fabs(detJ(i)) * EigenMap<const Matrix<double>, D, D>(jInvAtP).transpose() *
            refNormals[faceNo];
    }
}

template <std::size_t D> void Curvilinear<D>::normalize(Tensor<double, 2u>& normal) const {
    for (std::ptrdiff_t i = 0; i < normal.shape(1); ++i) {
        auto n = normal.subtensor(slice{}, i);
        EigenMap<Vector<double>, D>(n).normalize();
    }
}

template <std::size_t D>
TensorBase<Tensor<double, 3u>> Curvilinear<D>::facetBasisResultInfo(std::size_t numPoints) const {
    return TensorBase<Tensor<double, 3u>>(D, D, numPoints);
}

template <std::size_t D>
void Curvilinear<D>::facetBasis(std::array<double, D> const& up, Matrix<double> const& normal,
                                Tensor<double, 3u>& result) const {
    assert(result.shape(2) == normal.shape(1));

    constexpr double colinear_tol = 10000.0 * std::numeric_limits<double>::epsilon();

    for (std::ptrdiff_t i = 0; i < result.shape(2); ++i) {
        auto n_in = normal.subtensor(slice{}, i);
        auto n_in_eigen = EigenMap<Vector<double>, D>(n_in);
        auto n = result.subtensor(slice{}, 0, i);
        auto n_eigen = EigenMap<Vector<double>, D>(n);
        n_eigen = n_in_eigen.normalized();

        if constexpr (D == 2u) {
            double s = sgn(up[0] * n(1) - up[1] * n(0));
            if (std::fabs(s) < colinear_tol) {
                throw std::logic_error("Up vector and normal are almost colinear.");
            }
            auto d = result.subtensor(slice{}, 1, i);
            d(0) = -s * n(1);
            d(1) = s * n(0);
        } else if constexpr (D == 3u) {
            auto u = Eigen::Vector3d(up.data());
            auto d = result.subtensor(slice{}, 1, i);
            auto d_eigen = EigenMap<Vector<double>, D>(d);
            auto s = result.subtensor(slice{}, 2, i);
            auto s_eigen = EigenMap<Vector<double>, D>(s);
            s_eigen = u.cross(n_eigen).normalized();
            if (s_eigen.norm() < colinear_tol) {
                throw std::logic_error("Up vector and normal are almost colinear.");
            }
            d_eigen = s_eigen.cross(n_eigen).normalized();
        }
    }
}

template <std::size_t D>
void Curvilinear<D>::facetBasisFromPlexTangents(std::size_t faceNo,
                                                Tensor<double, 3u> const& jacobian,
                                                Matrix<double> const& normal,
                                                Tensor<double, 3u>& result) const {
    assert(result.shape(2) == normal.shape(1));
    assert(result.shape(2) == jacobian.shape(2));

    auto& f = f2v[faceNo];
    auto refTangent = refVertices[f[1]] - refVertices[f[0]];
    for (std::ptrdiff_t i = 0; i < result.shape(2); ++i) {
        auto n = normal.subtensor(slice{}, i);
        auto n_eigen = EigenMap<Vector<double>, D>(n);
        auto n_res = result.subtensor(slice{}, 0, i);
        auto n_res_eigen = EigenMap<Vector<double>, D>(n_res);
        n_res_eigen = n_eigen.normalized();

        if constexpr (D >= 2u) {
            auto jAtP = jacobian.subtensor(slice{}, slice{}, i);
            auto t1_res = result.subtensor(slice{}, 1, i);
            auto t1_res_eigen = EigenMap<Vector<double>, D>(t1_res);
            // first tangent = J * refTangent
            t1_res_eigen = EigenMap<const Matrix<double>, D, D>(jAtP) *
                           Eigen::Map<Eigen::Matrix<double, D, 1>>(refTangent.data());
            t1_res_eigen.normalize();

            if constexpr (D == 3u) {
                auto t2_res = result.subtensor(slice{}, 2, i);
                auto t2_res_eigen = EigenMap<Vector<double>, D>(t2_res);
                t2_res_eigen = n_res_eigen.cross(t1_res_eigen);
                t2_res_eigen.normalize();
            }
        }
    }
}

template <std::size_t D>
std::array<double, D> Curvilinear<D>::facetParam(std::size_t faceNo,
                                                 std::array<double, D - 1> const& chi) const {
    auto& f = f2v[faceNo];
    std::array<double, D> xi;
    double chiSum = 0.0;
    for (std::size_t d = 0; d < chi.size(); ++d) {
        chiSum += chi[d];
    }
    xi = (1.0 - chiSum) * refVertices[f[0]];
    for (std::size_t d = 0; d < chi.size(); ++d) {
        xi = xi + chi[d] * refVertices[f[d + 1]];
    }
    return xi;
}

template <std::size_t D>
std::vector<std::array<double, D>>
Curvilinear<D>::facetParam(std::size_t faceNo,
                           std::vector<std::array<double, D - 1>> const& chis) const {
    std::vector<std::array<double, D>> xis;
    xis.reserve(chis.size());
    for (auto const& chi : chis) {
        xis.emplace_back(facetParam(faceNo, chi));
    }
    return xis;
}

template class Curvilinear<1ul>;
template class Curvilinear<2ul>;
template class Curvilinear<3ul>;

} // namespace tndm


#include "plugins/snake/algo.h"
#include <float.h>
#include <vector>
#include <memory>
#include <utilities/cv.h>
#include <Eigen/Dense>

/*F///////////////////////////////////////////////////////////////////////////////////////
//    Name:      icvSnake8uC1R
//    Purpose:
//    Context:
//    Parameters:
//               img - source image,
//               imgStep - its step in bytes,
//               roi - size of ROI,
//               points - pointer to snake points array
//               n - size of points array,
//               alpha - pointer to coefficient of continuity energy,
//               beta - pointer to coefficient of curvature energy,
//               gamma - pointer to coefficient of image energy,
//               coeffUsage - if CV_VALUE - alpha, beta, gamma point to single value
//                            if CV_MATAY - point to arrays
//               criteria - termination criteria.
//               scheme - image energy scheme
//                         if _CV_SNAKE_IMAGE - image intensity is energy
//                         if _CV_SNAKE_GRAD  - magnitude of gradient is energy
//    Returns:
//F*/

template <class T>
T _max(T a, T b) {
    return (a>b?a:b);
}

template <class T>
T _min(T a, T b) {
    return (a<b?a:b);
}

bool snake_iteration(boost::shared_ptr<std::vector<float> > img,
               int w,
               int h,
               std::vector<Eigen::Vector2i> & points,
               int win_w,
               int win_h,
               float alpha,
               float beta,
               float gamma,
               float delta) {
    int n = points.size();
    int neighbors = win_h * win_w;

    int centerx = win_w >> 1;
    int centery = win_h >> 1;

    int iteration = 0;
    bool converged = false;

    /* check bad arguments */
    assert(points.size() > 2);
    assert((h > 0) && (w > 0));
    assert((win_h > 0) && (win_h & 1));
    assert((win_w > 0) && (win_w & 1));

    //float invn = 1 / ((float) points.size());

    std::vector<float> Econt(neighbors);
    std::vector<float> Ecurv(neighbors);
    std::vector<float> Eimg(neighbors);
    std::vector<float> Esize(neighbors);
    std::vector<float> E(neighbors);

    while( !converged ) {
        float ave_d = 0;
        int moved = 0;

        converged = false;
        iteration++;
        /* compute average distance */
        for(int i = 1; i < n; i++ )
        {
            int diffx = points[i - 1].x() - points[i].x();
            int diffy = points[i - 1].y() - points[i].y();

            ave_d += sqrt( (float) (diffx * diffx + diffy * diffy) );
        }
        ave_d += sqrt( (float) ((points[0].x() - points[n - 1].x()) *
                                  (points[0].x() - points[n - 1].x()) +
                                  (points[0].y() - points[n - 1].y()) * (points[0].y() - points[n - 1].y())));

        ave_d /= n;
        /* average distance computed */
        for(int i = 0; i < n; i++ )
        {
            /* Calculate Econt */
            float maxEcont = 0;
            float maxEcurv = 0;
            float maxEimg = 0;
            float maxEsize = 0;
            float minEcont = FLT_MAX;
            float minEcurv = FLT_MAX;
            float minEimg = FLT_MAX;
            float minEsize = FLT_MAX;
            float Emin = FLT_MAX;

            int offsetx = 0;
            int offsety = 0;
            float tmp;

            /* compute bounds */
            int left = _min( points[i].x(), win_w >> 1 );
            int right = _min( w - 1 - points[i].x(), win_w >> 1 );
            int upper = _min( points[i].y(), win_h >> 1 );
            int bottom = _min( h - 1 - points[i].y(), win_h >> 1 );

            maxEcont = 0;
            minEcont = FLT_MAX;
            for(int j = -upper; j <= bottom; j++ )
            {
                for(int k = -left; k <= right; k++ )
                {
                    int diffx, diffy;

                    if( i == 0 )
                    {
                        diffx = points[n - 1].x() - (points[i].x() + k);
                        diffy = points[n - 1].y() - (points[i].y() + j);
                    }
                    else
                    {
                        diffx = points[i - 1].x() - (points[i].x() + k);
                        diffy = points[i - 1].y() - (points[i].y() + j);
                    }

                    float energy = (float) fabs(ave_d - sqrt( (float) (diffx * diffx + diffy * diffy) ));

                    int idx = (j + centery) * win_w + k + centerx;
                    Econt[idx] = energy;

                    maxEcont = _max( maxEcont, energy );
                    minEcont = _min( minEcont, energy );
                }
            }
            tmp = maxEcont - minEcont;
            tmp = (tmp == 0) ? 0 : (1 / tmp);
            for(int k = 0; k < neighbors; k++ )
            {
                Econt[k] = (Econt[k] - minEcont) * tmp;
            }

            /*  Calculate Ecurv */
            maxEcurv = 0;
            minEcurv = FLT_MAX;
            for(int j = -upper; j <= bottom; j++ )
            {
                for(int k = -left; k <= right; k++ )
                {
                    int tx, ty;
                    float energy;

                    if(i == 0 )
                    {
                        tx = points[n - 1].x() - 2 * (points[i].x() + k) + points[i + 1].x();
                        ty = points[n - 1].y() - 2 * (points[i].y() + j) + points[i + 1].y();
                    }
                    else if( i == n - 1 )
                    {
                        tx = points[i - 1].x() - 2 * (points[i].x() + k) + points[0].x();
                        ty = points[i - 1].y() - 2 * (points[i].y() + j) + points[0].y();
                    }
                    else
                    {
                        tx = points[i - 1].x() - 2 * (points[i].x() + k) + points[i + 1].x();
                        ty = points[i - 1].y() - 2 * (points[i].y() + j) + points[i + 1].y();
                    }
                    Ecurv[(j + centery) * win_w + k + centerx] = energy =
                        (float) (tx * tx + ty * ty);
                    maxEcurv = _max( maxEcurv, energy );
                    minEcurv = _min( minEcurv, energy );
                }
            }
            tmp = maxEcurv - minEcurv;
            tmp = (tmp == 0) ? 0 : (1 / tmp);
            for(int k = 0; k < neighbors; k++ )
            {
                Ecurv[k] = (Ecurv[k] - minEcurv) * tmp;
            }

            /* Calculate Eimg */
            for(int j = -upper; j <= bottom; j++ )
            {
                for(int k = -left; k <= right; k++ )
                {
                    int y = points[i].y();
                    int x = points[i].x();
                    //int idx = (y + j) * w + x + k;
                    int idx = (x + k) * h + h-1 - (y + j);
                    float energy = -(*img)[idx];

                    Eimg[(j + centery) * win_w + k + centerx] = energy;

                    maxEimg = _max( maxEimg, energy );
                    minEimg = _min( minEimg, energy );
                }
            }

            // Test purposes
            /*int y = points[i].y();
            int x = points[i].x();
            int k = 0;
            int j = 0;
            int idx = (x + k) * h + h-1 - (y + j);
            (*img)[idx] = 400;
            */

            tmp = (maxEimg - minEimg);
            tmp = (tmp == 0) ? 0 : (1 / tmp);

            for(int k = 0; k < neighbors; k++ )
            {
                Eimg[k] = (minEimg - Eimg[k]) * tmp;
            }

            /*  Calculate Esize */
            maxEsize = 0;
            minEsize = FLT_MAX;
            for(int j = -upper; j <= bottom; j++ )
            {
                for(int k = -left; k <= right; k++ )
                {
                    Eigen::Vector2i prev;
                    Eigen::Vector2i next;
                    Eigen::Vector2i & curr = points[i];

                    if(i == 0 )
                    {
                        prev = points[n - 1];
                        next = points[i + 1];
                    }
                    else if( i == n - 1 )
                    {
                        prev = points[i - 1];
                        next = points[0];
                    }
                    else
                    {
                        prev = points[i - 1];
                        next = points[i + 1];
                    }

                    float dist1 = pow(curr.x() + k - prev.x(), 2) + pow(curr.y() + j - prev.y(), 2);
                    float dist2 = pow(curr.x() + k - next.x(), 2) + pow(curr.y() + j - next.y(), 2);

                    float energy = (dist1 + dist2) * 0.5f;

                    Esize[(j + centery) * win_w + k + centerx] = energy;
                    maxEsize = _max( maxEsize, energy );
                    minEsize = _min( minEsize, energy );
                }
            }
            tmp = maxEsize - minEsize;
            tmp = (tmp == 0) ? 0 : (1 / tmp);
            for(int k = 0; k < neighbors; k++ )
            {
                Esize[k] = (Esize[k] - minEsize) * tmp;
            }

            /* Find Minimize point in the neighbors */
            for(int k = 0; k < neighbors; k++ )
            {
                E[k] = alpha * Econt[k] + beta * Ecurv[k] + gamma * Eimg[k] + delta * Esize[k];
            }
            Emin = FLT_MAX;
            for(int j = -upper; j <= bottom; j++ )
            {
                for(int k = -left; k <= right; k++ )
                {

                    if( E[(j + centery) * win_w + k + centerx] < Emin )
                    {
                        Emin = E[(j + centery) * win_w + k + centerx];
                        offsetx = k;
                        offsety = j;
                    }
                }
            }

            if( offsetx || offsety )
            {
                //qDebug() << "(" << points[i].x() << points[i].y() << ") + (" << offsetx << offsety << ")" ;
                points[i] = points[i] + Eigen::Vector2i(offsetx, offsety);
                //qDebug() << " = (" << points[i].x() << points[i].y() << ")";
                moved++;
            }
        }
        converged = (moved == 0);
        break;
    }

    return converged;
}


#pragma once
#include <iostream>
#include <vector>
#include <string>
#include <thread>
#include <atomic>
#include <chrono>
#include <time.h>
#include <math.h>
#include <unordered_map>
#include <unordered_set>

#include <opencv2/opencv.hpp>

#include <pcl/kdtree/kdtree_flann.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <octomap/octomap.h>
#include <octomap/ColorOcTree.h>

using namespace std;

class Ray
{
public:
	octomap::OcTreeKey origin;
	octomap::OcTreeKey end;
	octomap::KeyRay* ray_set;
	octomap::KeyRay::iterator start;
	octomap::KeyRay::iterator stop;

	Ray(octomap::OcTreeKey _origin, octomap::OcTreeKey _end, octomap::KeyRay* _ray_set, octomap::KeyRay::iterator _start,octomap::KeyRay::iterator _stop){
		origin = _origin;
		end = _end;
		ray_set = _ray_set;
		start = _start;
		stop = _stop;
	}

	bool operator ==(const Ray& other) const {//用于查询
		return (origin == other.origin) && (end == other.end);
	}
};

class Ray_Hash
{
public:
	size_t operator() (const Ray& ray) const {//利用6个点的double来hash
		return octomap::OcTreeKey::KeyHash()(ray.origin) ^ octomap::OcTreeKey::KeyHash()(ray.end);
	}
};

class Ray_Information
{
public:
	Ray* ray;
	double information_gain;
	double visible;
	double object_visible;
	int voxel_num;
	bool previous_voxel_unknown;

	Ray_Information(Ray* _ray) {
		ray = _ray;
		information_gain = 0;
		visible = 1;
		object_visible = 1;
		previous_voxel_unknown = false;
		voxel_num = 0;
	}

	~Ray_Information() {
		delete ray;
	}

	void clear() {
		information_gain = 0;
		visible = 1;
		object_visible = 1;
		previous_voxel_unknown = false;
		voxel_num = 0;
	}
};

//void ray_graph_thread_process(int ray_id,Ray_Information** rays_info, unordered_map<int, vector<int>>* rays_to_viwes_map, unordered_map<octomap::OcTreeKey, unordered_set<int>, octomap::OcTreeKey::KeyHash>* end_id_map, Voxel_Information* voxel_information);
void information_gain_thread_process(Ray_Information** rays_info, unordered_map<int, vector<int>>* views_to_rays_map, View_Space* view_space, int pos);
void ray_expand_thread_process(int* ray_num, Ray_Information** rays_info, unordered_map<Ray, int, Ray_Hash>* rays_map, unordered_map<int, vector<int>>* views_to_rays_map, unordered_map<int, vector<int>>* rays_to_viwes_map, octomap::ColorOcTree* octo_model, Voxel_Information* voxel_information, View_Space* view_space, rs2_intrinsics* color_intrinsics, pcl::PointCloud<pcl::PointXYZ>::Ptr frontier,int pos);
void ray_cast_thread_process(int* ray_num, Ray_Information** rays_info, unordered_map<Ray, int, Ray_Hash>* rays_map, unordered_map<int, vector<int>>* views_to_rays_map, unordered_map<int, vector<int>>* rays_to_viwes_map, octomap::ColorOcTree* octo_model, Voxel_Information* voxel_information, View_Space* view_space, rs2_intrinsics* color_intrinsics,int pos);
vector<int> get_xmax_xmin_ymax_ymin_in_hull(vector<cv::Point2f>& hull, rs2_intrinsics& color_intrinsics);
bool is_pixel_in_convex(vector<cv::Point2f>& hull, cv::Point2f& pixel);
vector<cv::Point2f> get_convex_on_image(vector<Eigen::Vector4d>& convex_3d, Eigen::Matrix4d& now_camera_pose_world, rs2_intrinsics& color_intrinsics, int& pixel_interval, double& max_range, double& octomap_resolution);
octomap::point3d project_pixel_to_ray_end(int x,int y, rs2_intrinsics& color_intrinsics, Eigen::Matrix4d& now_camera_pose_world, float max_range = 1.0);
double information_function(short& method, double& ray_informaiton, double voxel_information, double& visible, bool& is_unknown, bool& previous_voxel_unknown, bool& is_endpoint, bool& is_occupied, double& object, double& object_visible);
void ray_information_thread_process(int ray_id, Ray_Information** rays_info, unordered_map<Ray, int, Ray_Hash>* rays_map, unordered_map<octomap::OcTreeKey, double, octomap::OcTreeKey::KeyHash>* occupancy_map, unordered_map<octomap::OcTreeKey, double, octomap::OcTreeKey::KeyHash>* object_weight, octomap::ColorOcTree* octo_model, Voxel_Information* voxel_information, View_Space* view_space, short method);
int frontier_check(octomap::point3d node, octomap::ColorOcTree* octo_model, Voxel_Information* voxel_information, double octomap_resolution);
double distance_function(double distance, double alpha);

class Views_Information
{
public:
	double cost_weight;
	Ray_Information** rays_info;
	unordered_map<int, vector<int>>* views_to_rays_map;
	unordered_map<int, vector<int>>* rays_to_viwes_map;
	unordered_map<Ray,int, Ray_Hash>* rays_map;
	unordered_map<octomap::OcTreeKey,double, octomap::OcTreeKey::KeyHash>* occupancy_map;
	unordered_map<octomap::OcTreeKey, double, octomap::OcTreeKey::KeyHash>* object_weight;
	long long max_num_of_rays;
	int ray_num;
	double alpha;
	int K = 6;
	rs2_intrinsics color_intrinsics;
	Voxel_Information* voxel_information;
	octomap::ColorOcTree* octo_model;
	double octomap_resolution;
	int method;
	int pre_edge_cnt;
	int edge_cnt;

	Views_Information(Share_Data* share_data, Voxel_Information* _voxel_information , View_Space* view_space,int iterations)
	{
		//更新内部数据
		voxel_information = _voxel_information;
		cost_weight = share_data->cost_weight;
		color_intrinsics = share_data->color_intrinsics;
		method = share_data->method_of_IG;
		octo_model = share_data->octo_model;
		octomap_resolution = share_data->octomap_resolution;
		voxel_information->octomap_resolution = octomap_resolution;
		alpha = 0.1 / octomap_resolution;
		voxel_information->skip_coefficient = share_data->skip_coefficient;
		//注意视点需要按照id排序来建立映射
		sort(view_space->views.begin(), view_space->views.end(), view_id_compare);
		double now_time = clock();
		views_to_rays_map = new unordered_map<int, vector<int>>();
		rays_to_viwes_map = new unordered_map<int, vector<int>>();
		rays_map = new unordered_map<Ray, int, Ray_Hash>();
		object_weight = new unordered_map<octomap::OcTreeKey, double, octomap::OcTreeKey::KeyHash>();
		occupancy_map = new unordered_map<octomap::OcTreeKey, double, octomap::OcTreeKey::KeyHash>();
		//定义frontier
		vector<octomap::point3d> points;
		pcl::PointCloud<pcl::PointXYZ>::Ptr edge(new pcl::PointCloud<pcl::PointXYZ>);
		double map_size = view_space->predicted_size;
		//查找地图中的edge
		for (octomap::ColorOcTree::leaf_iterator it = octo_model->begin_leafs(), end = octo_model->end_leafs(); it != end; ++it)
		{
			double occupancy = (*it).getOccupancy();
			//记录bbx中key到occ率的映射，用于重复查询
			(*occupancy_map)[it.getKey()] = occupancy;
			if (voxel_information->is_unknown(occupancy)) {
				auto coordinate = it.getCoordinate();
				if (coordinate.x() >= view_space->object_center_world(0) - map_size && coordinate.x() <= view_space->object_center_world(0) + map_size
					&& coordinate.y() >= view_space->object_center_world(1) - map_size && coordinate.y() <= view_space->object_center_world(1) + map_size
					&& coordinate.z() >= view_space->object_center_world(2) - map_size && coordinate.z() <= view_space->object_center_world(2) + map_size)
				{
					points.push_back(coordinate);
					if (frontier_check(coordinate, octo_model, voxel_information, octomap_resolution)==2) edge->points.push_back(pcl::PointXYZ(coordinate.x(), coordinate.y(), coordinate.z()));
				}
			}
		}
		pre_edge_cnt = 0x3f3f3f3f;
		edge_cnt = edge->points.size();
		//根据最邻近frontier，计算地图中该点的是物体表面的可能性
		if (edge->points.size() != 0) {
			pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
			kdtree.setInputCloud(edge);
			std::vector<int> pointIdxNKNSearch(K);
			std::vector<float> pointNKNSquaredDistance(K);
			for (int i = 0; i < points.size(); i++)
			{
				octomap::OcTreeKey key;   bool key_have = octo_model->coordToKeyChecked(points[i], key);
				if (key_have) {
					pcl::PointXYZ searchPoint(points[i].x(), points[i].y(), points[i].z());
					int num = kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance);
					if (num > 0) {
						double p_obj = 1;
						for (int j = 0; j < pointIdxNKNSearch.size(); j++) {
							p_obj *= distance_function(pointNKNSquaredDistance[j], alpha);
						}
						(*object_weight)[key] = p_obj;
					}
				}
			}
		}
		cout << "occupancy_map is " << occupancy_map->size() << endl;
		cout << "edge is " << edge->points.size() << endl;
		cout << "object_map is " << object_weight->size() << endl;
		//根据BBX计算最多有多少射线，射线个数最多为表面积大小*体积，用于分配指针内存
		double pre_line_point = 2.0 * map_size / octomap_resolution;
		long long superficial = ceil(5.0 * pre_line_point * pre_line_point);
		long long volume = ceil(pre_line_point * pre_line_point * pre_line_point);
		max_num_of_rays = superficial* volume;
		rays_info = new Ray_Information * [max_num_of_rays];
		cout << "full rays num is " << max_num_of_rays << endl;
		//计算BBX的八个顶点，用于划定射线范围
		vector<Eigen::Vector4d> convex_3d;
		double x1 = view_space->object_center_world(0) - map_size;
		double x2 = view_space->object_center_world(0) + map_size;
		double y1 = view_space->object_center_world(1) - map_size;
		double y2 = view_space->object_center_world(1) + map_size;
		double z1 = view_space->object_center_world(2) - map_size;
		double z2 = view_space->object_center_world(2) + map_size;
		convex_3d.push_back(Eigen::Vector4d(x1, y1, z1, 1));
		convex_3d.push_back(Eigen::Vector4d(x1, y2, z1, 1));
		convex_3d.push_back(Eigen::Vector4d(x2, y1, z1, 1));
		convex_3d.push_back(Eigen::Vector4d(x2, y2, z1, 1));
		convex_3d.push_back(Eigen::Vector4d(x1, y1, z2, 1));
		convex_3d.push_back(Eigen::Vector4d(x1, y2, z2, 1));
		convex_3d.push_back(Eigen::Vector4d(x2, y1, z2, 1));
		convex_3d.push_back(Eigen::Vector4d(x2, y2, z2, 1));
		voxel_information->convex = convex_3d;
		//分配视点的射线生成器
		thread** ray_caster = new thread *[view_space->views.size()];
		//射线初始下标从0开始
		ray_num = 0;
		for (int i = 0; i < view_space->views.size(); i++) {
			//对该视点分发生成射线的线程
			ray_caster[i] = new thread(ray_cast_thread_process, &ray_num, rays_info, rays_map, views_to_rays_map, rays_to_viwes_map, octo_model, voxel_information, view_space, &color_intrinsics, i);
		}
		//等待每个视点射线生成器计算完成
		for (int i = 0; i < view_space->views.size(); i++) {
			(*ray_caster[i]).join();
		}
		cout << "ray_num is " << ray_num << endl;
		cout << "All views' rays generated with executed time " << clock() - now_time << " ms. Startring compution." << endl;
		//为每条射线分配一个线程
		now_time = clock();
		thread** rays_process = new thread* [ray_num];
		for (int i = 0; i < ray_num; i++) {
			rays_process[i] = new thread(ray_information_thread_process, i, rays_info, rays_map, occupancy_map, object_weight, octo_model, voxel_information, view_space, method);
		}
		//等待射线计算完成
		for (int i = 0; i < ray_num; i++) {
			(*rays_process[i]).join();
		}
		double cost_time = clock() - now_time;
		cout << "All rays' threads over with executed time " << cost_time << " ms." << endl;
		share_data->access_directory(share_data->save_path + "/run_time");
		ofstream fout(share_data->save_path + "/run_time/IG" + to_string(view_space->id) + ".txt");
		fout << cost_time << endl;
		//分配视点的信息统计器
		now_time = clock();
		thread** view_gain = new thread * [view_space->views.size()];
		for (int i = 0; i < view_space->views.size(); i++) {
			//对该视点分发信息统计的线程
			view_gain[i] = new thread(information_gain_thread_process, rays_info, views_to_rays_map, view_space, i);
		}
		//等待每个视点信息统计完成
		for (int i = 0; i < view_space->views.size(); i++) {
			(*view_gain[i]).join();
		}
		cout << "All views' gain threads over with executed time " << clock() - now_time << " ms." << endl;
	}

	void update(Share_Data* share_data, View_Space* view_space,int iterations) {
		//更新内部数据
		double now_time = clock();
		double map_size = view_space->predicted_size;
		//注意视点需要按照id排序来建立映射
		sort(view_space->views.begin(), view_space->views.end(), view_id_compare);
		//重新记录八叉树
		octo_model = share_data->octo_model;
		octomap_resolution = share_data->octomap_resolution;
		alpha = 0.1 / octomap_resolution;
		voxel_information->octomap_resolution = octomap_resolution;
		voxel_information->skip_coefficient = share_data->skip_coefficient;
		//清空视点信息
		for (int i = 0; i < view_space->views.size(); i++) {
			view_space->views[i].information_gain = 0;
			view_space->views[i].voxel_num = 0;
		}
		//避免重复search
		delete occupancy_map;
		occupancy_map = new unordered_map<octomap::OcTreeKey, double, octomap::OcTreeKey::KeyHash>();
		delete object_weight;
		object_weight = new unordered_map<octomap::OcTreeKey, double, octomap::OcTreeKey::KeyHash>();
		//更新frontier
		vector<octomap::point3d> points;
		pcl::PointCloud<pcl::PointXYZ>::Ptr edge(new pcl::PointCloud<pcl::PointXYZ>);
		for (octomap::ColorOcTree::leaf_iterator it = octo_model->begin_leafs(), end = octo_model->end_leafs(); it != end; ++it)
		{
			double occupancy = (*it).getOccupancy();
			(*occupancy_map)[it.getKey()] = occupancy;
			if (voxel_information->is_unknown(occupancy)) {
				auto coordinate = it.getCoordinate();
				if (coordinate.x() >= view_space->object_center_world(0) - map_size && coordinate.x() <= view_space->object_center_world(0) + map_size
					&& coordinate.y() >= view_space->object_center_world(1) - map_size && coordinate.y() <= view_space->object_center_world(1) + map_size
					&& coordinate.z() >= view_space->object_center_world(2) - map_size && coordinate.z() <= view_space->object_center_world(2) + map_size)
				{
					points.push_back(coordinate);
					if (frontier_check(coordinate, octo_model, voxel_information, octomap_resolution)==2) edge->points.push_back(pcl::PointXYZ(coordinate.x(), coordinate.y(), coordinate.z()));
				}
			}
		}
		edge_cnt = edge->points.size();
		if (edge_cnt > pre_edge_cnt) pre_edge_cnt = 0x3f3f3f3f;
		if (edge->points.size() != 0) {
			//计算frontier
			pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
			kdtree.setInputCloud(edge);
			std::vector<int> pointIdxNKNSearch(K);
			std::vector<float> pointNKNSquaredDistance(K);
			for (int i = 0; i < points.size(); i++)
			{
				octomap::OcTreeKey key;   bool key_have = octo_model->coordToKeyChecked(points[i], key);
				if (key_have) {
					pcl::PointXYZ searchPoint(points[i].x(), points[i].y(), points[i].z());
					int num = kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance);
					if (num > 0) {
						double p_obj = 1;
						for (int j = 0; j < pointIdxNKNSearch.size(); j++) {
							p_obj *= distance_function(pointNKNSquaredDistance[j], alpha);
						}
						(*object_weight)[key] = p_obj;
					}
				}
			}
		}
		cout << "edge is " << edge->points.size() << endl;
		cout << "object_map is " << object_weight->size() << endl;
		cout << "occupancy_map is " << occupancy_map->size() << endl;
		cout << "frontier updated with executed time " << clock() - now_time << " ms." << endl;
		//检测是否重生成
		now_time = clock();
		bool regenerate = false;
		if (view_space->object_changed) {
			regenerate = true;
		}
		//如果重生成，则更新数据结构
		if (regenerate) {
			//重计算最大射线数量，从0开始
			double pre_line_point = 2.0 * map_size / octomap_resolution;
			long long superficial = ceil(5.0 * pre_line_point * pre_line_point);
			long long volume = ceil(pre_line_point * pre_line_point * pre_line_point);
			max_num_of_rays = superficial * volume;
			delete[] rays_info;
			rays_info = new Ray_Information * [max_num_of_rays];
			cout << "full rays num is " << max_num_of_rays << endl;
			ray_num = 0;
			delete views_to_rays_map;
			views_to_rays_map = new unordered_map<int, vector<int>>();
			delete rays_to_viwes_map;
			rays_to_viwes_map = new unordered_map<int, vector<int>>();
			delete rays_map;
			rays_map = new unordered_map<Ray, int, Ray_Hash>();

			//计算BBX的八个顶点，用于划定射线范围
			vector<Eigen::Vector4d> convex_3d;
			double x1 = view_space->object_center_world(0) - map_size;
			double x2 = view_space->object_center_world(0) + map_size;
			double y1 = view_space->object_center_world(1) - map_size;
			double y2 = view_space->object_center_world(1) + map_size;
			double z1 = view_space->object_center_world(2) - map_size;
			double z2 = view_space->object_center_world(2) + map_size;
			convex_3d.push_back(Eigen::Vector4d(x1, y1, z1, 1));
			convex_3d.push_back(Eigen::Vector4d(x1, y2, z1, 1));
			convex_3d.push_back(Eigen::Vector4d(x2, y1, z1, 1));
			convex_3d.push_back(Eigen::Vector4d(x2, y2, z1, 1));
			convex_3d.push_back(Eigen::Vector4d(x1, y1, z2, 1));
			convex_3d.push_back(Eigen::Vector4d(x1, y2, z2, 1));
			convex_3d.push_back(Eigen::Vector4d(x2, y1, z2, 1));
			convex_3d.push_back(Eigen::Vector4d(x2, y2, z2, 1));
			voxel_information->convex = convex_3d;
			//分配视点的射线生成器
			thread** ray_caster = new thread * [view_space->views.size()];
			for (int i = 0; i < view_space->views.size(); i++) {
				//对该视点分发生成射线的线程
				ray_caster[i] = new thread(ray_cast_thread_process, &ray_num, rays_info, rays_map, views_to_rays_map, rays_to_viwes_map, octo_model, voxel_information, view_space, &color_intrinsics, i);
			}
			//等待每个视点射线生成器计算完成
			for (int i = 0; i < view_space->views.size(); i++) {
				(*ray_caster[i]).join();
			}
			cout << "ray_num is " << ray_num << endl;
			cout << "All views' rays generated with executed time " << clock() - now_time << " ms. Startring compution." << endl;
		}
		//为每条射线分配一个线程
		now_time = clock();
		thread** rays_process = new thread * [ray_num];
		for (int i = 0; i < ray_num; i++) {
			rays_info[i]->clear();
			rays_process[i] = new thread(ray_information_thread_process, i, rays_info, rays_map, occupancy_map, object_weight, octo_model, voxel_information, view_space, method);
		}
		//等待射线计算完成
		for (int i = 0; i < ray_num; i++) {
			(*rays_process[i]).join();
		}
		double cost_time = clock() - now_time;
		cout << "All rays' threads over with executed time " << cost_time << " ms." << endl;
		share_data->access_directory(share_data->save_path + "/run_time");
		ofstream fout(share_data->save_path + "/run_time/IG" + to_string(view_space->id) + ".txt");
		fout << cost_time << endl;
		now_time = clock();
		thread** view_gain = new thread * [view_space->views.size()];
		for (int i = 0; i < view_space->views.size(); i++) {
			//对该视点分发信息统计的线程
			view_gain[i] = new thread(information_gain_thread_process, rays_info, views_to_rays_map, view_space, i);
		}
		//等待每个视点信息统计完成
		for (int i = 0; i < view_space->views.size(); i++) {
			(*view_gain[i]).join();
		}
		cout << "All views' gain threads over with executed time " << clock() - now_time << " ms." << endl;
	}
};

void information_gain_thread_process(Ray_Information** rays_info, unordered_map<int, vector<int>>* views_to_rays_map, View_Space* view_space, int pos) {
	//视点的每个相关射线信息加入视点
	for (vector<int>::iterator it = (*views_to_rays_map)[pos].begin(); it != (*views_to_rays_map)[pos].end(); it++) {
		view_space->views[pos].information_gain += rays_info[*it]->information_gain;
		view_space->views[pos].voxel_num += rays_info[*it]->voxel_num;
	}
}

void ray_cast_thread_process(int* ray_num, Ray_Information** rays_info, unordered_map<Ray, int, Ray_Hash>* rays_map, unordered_map<int, vector<int>>* views_to_rays_map, unordered_map<int, vector<int>>* rays_to_viwes_map, octomap::ColorOcTree* octo_model, Voxel_Information* voxel_information, View_Space* view_space, rs2_intrinsics* color_intrinsics, int pos) {
	//获取视点位姿
	view_space->views[pos].get_next_camera_pos(view_space->now_camera_pose_world, view_space->object_center_world);
	Eigen::Matrix4d view_pose_world = (view_space->now_camera_pose_world * view_space->views[pos].pose.inverse()).eval();
	//将三维物体BBX根据视点位姿，投影至图片凸包区域
	double skip_coefficient = voxel_information->skip_coefficient;
	//根据能访问的体素来控制射线遍历，注意间隔跳跃参数
	int pixel_interval = color_intrinsics->width;
	double max_range = 6.0 * view_space->predicted_size;
	vector<cv::Point2f> hull;
	hull = get_convex_on_image(voxel_information->convex, view_pose_world, *color_intrinsics, pixel_interval, max_range, voxel_information->octomap_resolution);
	//if (hull.size() != 4 && hull.size() != 5 && hull.size() != 6) cout << "hull wrong with size " << hull.size() << endl;
	//计算凸包的包围盒
	vector<int> boundary;
	boundary = get_xmax_xmin_ymax_ymin_in_hull(hull, *color_intrinsics);
	int xmax = boundary[0];
	int xmin = boundary[1];
	int ymax = boundary[2];
	int ymin = boundary[3];
	//cout << xmax << " " << xmin << " " << ymax << " " << ymin << " ," << pixel_interval <<endl;
	//中间数据结构
	vector<Ray*> rays;
	//int num = 0;
	//检查视点的key
	octomap::OcTreeKey key_origin;
	bool key_origin_have = octo_model->coordToKeyChecked(view_space->views[pos].init_pos(0), view_space->views[pos].init_pos(1), view_space->views[pos].init_pos(2), key_origin);
	if (key_origin_have) {
		octomap::point3d origin = octo_model->keyToCoord(key_origin);
		//遍历包围盒
		//srand(pos);
		//int rr = rand() % 256, gg = rand() % 256, bb = rand() % 256;
		for (int x = xmin; x <= xmax; x += (int)(pixel_interval * skip_coefficient))
			for (int y = ymin; y <= ymax; y += (int)(pixel_interval * skip_coefficient))
			{
				//num++;
				cv::Point2f pixel(x, y);
				//检查是否在凸包区域内部
				if (!is_pixel_in_convex(hull, pixel)) continue;
				//反向投影找到终点
				octomap::point3d end = project_pixel_to_ray_end(x, y, *color_intrinsics, view_pose_world, max_range);
				//显示一下
				//view_space->viewer->addLine<pcl::PointXYZ>(pcl::PointXYZ(origin(0), origin(1), origin(2)), pcl::PointXYZ(end(0), end(1), end(2)), rr, gg, bb, "line" + to_string(pos) + "-" + to_string(x) + "-" + to_string(y));
				octomap::OcTreeKey key_end;
				octomap::point3d direction = end - origin;
				octomap::point3d end_point;
				//越过未知区域，找到终点
				bool found_end_point = octo_model->castRay(origin, direction, end_point, true, max_range);
				if (!found_end_point) {//未找到终点，设置终点为最大距离
					end_point = origin + direction.normalized() * max_range; // use max range instead of stopping at the unknown       found_endpoint = true;
				}
				//检查一下末端是否在地图限制范围内，且命中BBX
				bool key_end_have = octo_model->coordToKeyChecked(end_point, key_end);
				if (key_end_have) {
					//生成射线
					octomap::KeyRay* ray_set = new octomap::KeyRay();
					//获取射线数组，不包含末节点
					bool point_on_ray_getted = octo_model->computeRayKeys(origin, end_point, *ray_set);
					if (!point_on_ray_getted) cout << "Warning. ray cast with wrong max_range." << endl;
					if (ray_set->size() > 950) cout << ray_set->size() << " rewrite the vector size in octreekey.h." << endl;
					//把终点放入射线组
					ray_set->addKey(key_end);
					//第一个非空节点作为射线起点，尾巴开始最后一个非空元素作为射线终点
					octomap::KeyRay::iterator last = ray_set->end();
					last--;
					while (last != ray_set->begin() && (octo_model->search(*last) == NULL)) last--;
					//二分第一个非空元素
					octomap::KeyRay::iterator l = ray_set->begin();
					octomap::KeyRay::iterator r = last;
					octomap::KeyRay::iterator mid = l + (r - l) / 2;
					while (mid != r) {
						if (octo_model->search(*mid) != NULL)
							r = mid;
						else
							l = mid + 1;
						mid = l + (r - l) / 2;
					}
					octomap::KeyRay::iterator first = mid;
					while (first  != ray_set->end() && (octo_model->keyToCoord(*first).x() < view_space->object_center_world(0) - view_space->predicted_size || octo_model->keyToCoord(*first).x() > view_space->object_center_world(0) + view_space->predicted_size
						|| octo_model->keyToCoord(*first).y() < view_space->object_center_world(1) - view_space->predicted_size || octo_model->keyToCoord(*first).y() > view_space->object_center_world(1) + view_space->predicted_size
						|| octo_model->keyToCoord(*first).z() < view_space->object_center_world(2) - view_space->predicted_size || octo_model->keyToCoord(*first).z() > view_space->object_center_world(2) + view_space->predicted_size)) first++;
					//如果没有非空元素，直接丢弃射线
					if (last - first < 0) {
						delete ray_set;
						continue;
					}
					octomap::KeyRay::iterator stop = last;
					stop++;
					//显示一下
					//while (octo_model->keyToCoord(*first).x() < view_space->object_center_world(0) - view_space->predicted_size || octo_model->keyToCoord(*first).x() > view_space->object_center_world(0) + view_space->predicted_size
					//	|| octo_model->keyToCoord(*first).y() < view_space->object_center_world(1) - view_space->predicted_size || octo_model->keyToCoord(*first).y() > view_space->object_center_world(1) + view_space->predicted_size
					//	|| octo_model->keyToCoord(*first).z() < min(view_space->height_of_ground, view_space->object_center_world(2) - view_space->predicted_size) || octo_model->keyToCoord(*first).z() > view_space->object_center_world(2) + view_space->predicted_size) first++;
					//octomap::point3d ss = octo_model->keyToCoord(*first);
					//octomap::point3d ee = octo_model->keyToCoord(*last);
					//view_space->viewer->addLine<pcl::PointXYZ>(pcl::PointXYZ(ss(0), ss(1), ss(2)), pcl::PointXYZ(ee(0), ee(1), ee(2)), rr, gg, bb, "line" + to_string(pos) + "-" + to_string(x) + "-" + to_string(y));
					//将射线加入视点的集合，第一个元素与最后一个元素key+数组+头+尾
					Ray* ray = new Ray(*first, *last, ray_set, first, stop);
					rays.push_back(ray);
				}
			}
	}
	else {
		cout << pos << "th view out of map.check." << endl;
	}
	//cout << "rays " << rays.size() <<" num "<<num<< endl;
	//该视点射线下标的数组
	vector<int> ray_ids;
	ray_ids.resize(rays.size());
	//注意公用的数据结构要加锁
	voxel_information->mutex_rays.lock();
	//获取当前射线的位置
	int ray_id = (*ray_num);
	for (int i = 0; i < rays.size(); i++) {
		//对于这些射线，hash查询一下是否有重复的
		auto hash_this_ray = rays_map->find(*rays[i]);
		//如果没有重复的，就保存该射线
		if (hash_this_ray == rays_map->end()) {
			(*rays_map)[*rays[i]] = ray_id;
			ray_ids[i] = ray_id;
			//创造射线计算类
			rays_info[ray_id] = new Ray_Information(rays[i]);
			vector<int> view_ids;
			view_ids.push_back(pos);
			(*rays_to_viwes_map)[ray_id] = view_ids;
			ray_id++;
		}
		//如果有重复的，说明其他视点也算到了该射线，就把相应的id放入下标数组
		else {
			ray_ids[i] = hash_this_ray->second;
			delete rays[i]->ray_set;
			//其他视点已经记录的射线，把本视点的记录放进去
			vector<int> view_ids = (*rays_to_viwes_map)[ray_ids[i]];
			view_ids.push_back(pos);
			(*rays_to_viwes_map)[ray_ids[i]] = view_ids;
		}
	}
	//更新射线数目
	(*ray_num) = ray_id;
	//更新视点映射的射线数组
	(*views_to_rays_map)[pos] = ray_ids;
	//释放锁
	voxel_information->mutex_rays.unlock();
}

inline vector<int> get_xmax_xmin_ymax_ymin_in_hull(vector<cv::Point2f>& hull, rs2_intrinsics& color_intrinsics) {
	float xmax = 0, xmin = color_intrinsics.width - 1, ymax = 0, ymin = color_intrinsics.height - 1;
	for(int i = 0;i< hull.size();i++){
		xmax = max(xmax, hull[i].x);
		xmin = min(xmin, hull[i].x);
		ymax = max(ymax, hull[i].y);
		ymin = min(ymin, hull[i].y);
	}
	vector<int> boundary;
	boundary.push_back((int)floor(xmax));
	boundary.push_back((int)floor(xmin));
	boundary.push_back((int)floor(ymax));
	boundary.push_back((int)floor(ymin));
	return boundary;
}

inline bool is_pixel_in_convex(vector<cv::Point2f>& hull, cv::Point2f& pixel) {
	double hull_value = pointPolygonTest(hull, pixel, false);
	return hull_value >= 0;
}

inline vector<cv::Point2f> get_convex_on_image(vector<Eigen::Vector4d>& convex_3d, Eigen::Matrix4d& now_camera_pose_world, rs2_intrinsics& color_intrinsics,int& pixel_interval,double& max_range,double& octomap_resolution) {
	//投影立方体顶点至图像坐标系
	double now_range = 0;
	vector<cv::Point2f> contours;
	for (int i = 0; i < convex_3d.size(); i++) {
		Eigen::Vector4d vertex = now_camera_pose_world.inverse() * convex_3d[i];
		float point[3] = { vertex(0), vertex(1),vertex(2) };
		float pixel[2];
		rs2_project_point_to_pixel(pixel, &color_intrinsics, point);
		contours.push_back(cv::Point2f(pixel[0], pixel[1]));
		//cout << pixel[0] << " " << pixel[1] << endl;
		//计算一下最远点离开视点距离
		Eigen::Vector4d view_pos(now_camera_pose_world(0, 3), now_camera_pose_world(1, 3), now_camera_pose_world(2, 3), 1);
		now_range = max(now_range, (view_pos - convex_3d[i]).norm());
	}
	max_range = min(max_range, now_range);
	//计算凸包
	vector<cv::Point2f> hull;
	convexHull(contours, hull, false, true);
	if (!cv::isContourConvex(hull)) {
		cout << "no convex. check BBX." << endl;
		return contours;
	}
	//计算空间最远两点距离，计算像素最远两点距离，根据地图分辨率得到像素偏移
	double pixel_dis = 0;
	double space_dis = 0;
	for (int i = 0; i < hull.size(); i++)
		for (int j = 0; j < hull.size(); j++) if(i!=j){
			Eigen::Vector2d pixel_start(hull[i].x, hull[i].y);
			Eigen::Vector2d pixel_end(hull[j].x, hull[j].y);
			pixel_dis = max(pixel_dis,(pixel_start - pixel_end).norm());
			space_dis = max(space_dis, (convex_3d[i] - convex_3d[j]).norm());
		}
	pixel_interval = (int)(pixel_dis / space_dis * octomap_resolution);
	return hull;
}

inline octomap::point3d project_pixel_to_ray_end(int x,int y, rs2_intrinsics& color_intrinsics, Eigen::Matrix4d& now_camera_pose_world,float max_range) {
	float pixel[2] = { x ,y };
	float point[3];
	rs2_deproject_pixel_to_point(point, &color_intrinsics, pixel, max_range);
	Eigen::Vector4d point_world(point[0], point[1], point[2],1);
	point_world = now_camera_pose_world * point_world;
	return octomap::point3d(point_world(0), point_world(1), point_world(2));
}

void ray_information_thread_process(int ray_id, Ray_Information** rays_info, unordered_map<Ray, int, Ray_Hash>* rays_map, unordered_map<octomap::OcTreeKey, double, octomap::OcTreeKey::KeyHash>* occupancy_map, unordered_map<octomap::OcTreeKey, double, octomap::OcTreeKey::KeyHash>* object_weight, octomap::ColorOcTree* octo_model, Voxel_Information* voxel_information, View_Space* view_space, short method )
{
	//由于检查过，所以第一个节点就是非空节点
	octomap::KeyRay::iterator first = rays_info[ray_id]->ray->start;
	octomap::KeyRay::iterator last = rays_info[ray_id]->ray->stop;
	last--;
	for (octomap::KeyRay::iterator it = rays_info[ray_id]->ray->start; it != rays_info[ray_id]->ray->stop; ++it) {
		//从hash表里查询该key
		auto hash_this_key = (*occupancy_map).find(*it);
		//找不到节点就下一个
		if (hash_this_key== (*occupancy_map).end()) {
			if (method == RSE && it == last) rays_info[ray_id]->information_gain = 0;
			continue;
		}
		//读取节点概率值
		double occupancy = hash_this_key->second;
		//检查一下当前节点是否被占据
		bool voxel_occupied = voxel_information->is_occupied(occupancy);
		//检查一下节点是否未知
		bool voxel_unknown = voxel_information->is_unknown(occupancy);
		//读取一下节点为物体表面率
		double on_object = voxel_information->voxel_object(*it, object_weight);
		//如果被占据，那就是最后一个节点了
		if (voxel_occupied) last = it;
		//如果free，则初始节点要更新
		if (it==first && (!voxel_unknown&&!voxel_occupied)) first = it;
		//判断是否最后一个节点
		bool is_end = (it == last);
		//统计信息熵
		rays_info[ray_id]->information_gain = information_function(method, rays_info[ray_id]->information_gain, voxel_information->entropy(occupancy), rays_info[ray_id]->visible, voxel_unknown, rays_info[ray_id]->previous_voxel_unknown, is_end, voxel_occupied, on_object, rays_info[ray_id]->object_visible);
		rays_info[ray_id]->object_visible *= (1 - on_object);
		if(method == OursIG) rays_info[ray_id]->visible *= voxel_information->get_voxel_visible(occupancy);
		else rays_info[ray_id]->visible *= occupancy;
		rays_info[ray_id]->voxel_num++;
		//如果是最后了就退出
		if (is_end) break;
	}
	while (last - first < -1) first--;
	last++;
	//更新stop为最后一个节点后一个迭代器
	rays_info[ray_id]->ray->stop = last;
	//更新start为第一个迭代器
	rays_info[ray_id]->ray->start = first;
}

inline double information_function(short& method,double& ray_informaiton,double voxel_information,double& visible,bool& is_unknown, bool& previous_voxel_unknown,bool& is_endpoint,bool& is_occupied,double& object,double& object_visible) {
	double final_information = 0;
	switch (method) {
	case OursIG:
		if (is_unknown) {
			final_information = ray_informaiton + object * visible * voxel_information;
		}
		else {
			final_information = ray_informaiton;
		}
		break;
	case OA:
		final_information = ray_informaiton + visible * voxel_information;
		break;
	case UV:
		if(is_unknown) final_information = ray_informaiton + visible * voxel_information;
		else final_information = ray_informaiton;
		break;
	case RSE:
		if (is_endpoint) {
			if (previous_voxel_unknown) {
				if (is_occupied) final_information = ray_informaiton + visible * voxel_information;
				else final_information = 0;
			}
			else final_information = 0;
		}
		else {
			if (is_unknown) {
				previous_voxel_unknown = true;
				final_information = ray_informaiton + visible * voxel_information;
			}
			else {
				previous_voxel_unknown = false;
				final_information = 0;
			}
		}
		break;
	case APORA:
		if (is_unknown) {
			final_information = ray_informaiton + object * object_visible * voxel_information;
		}
		else {
			final_information = ray_informaiton;
		}
		break;
	case Kr:
		if (is_endpoint) {
			if (is_occupied) final_information = ray_informaiton + voxel_information;
			else final_information = 0;
		}
		else final_information = ray_informaiton + voxel_information;
		break;
	}
	return final_information;
}

inline int frontier_check(octomap::point3d node, octomap::ColorOcTree* octo_model, Voxel_Information* voxel_information, double octomap_resolution) {
	int free_cnt = 0;
	int occupied_cnt = 0;
	for (int i = -1; i <= 1; i++)
		for (int j = -1; j <= 1; j++)
			for (int k = -1; k <= 1; k++)
			{
				if (i == 0 && j == 0 && k == 0) continue;
				double x = node.x() + i * octomap_resolution;
				double y = node.y() + j * octomap_resolution;
				double z = node.z() + k * octomap_resolution;
				octomap::point3d neighbour(x, y, z);
				octomap::OcTreeKey neighbour_key;  bool neighbour_key_have = octo_model->coordToKeyChecked(neighbour, neighbour_key);
				if (neighbour_key_have) {
					octomap::ColorOcTreeNode* neighbour_voxel = octo_model->search(neighbour_key);
					if (neighbour_voxel != NULL) {
						free_cnt += voxel_information->voxel_free(neighbour_voxel) == true ? 1 : 0;
						occupied_cnt += voxel_information->voxel_occupied(neighbour_voxel) == true ? 1 : 0;
					}
				}
			}
	//edge
	if (free_cnt >= 1 && occupied_cnt >= 1) return 2;
	//边界
	if (free_cnt >= 1) return 1;
	//啥也不是
	return 0;
}

inline double distance_function(double distance,double alpha) {
	return exp(-pow2(alpha)*distance);
}

//Max Flow
class MCMF {
public:
	struct Edge {
		int from, to, cap, flow;
		double cost;
		Edge(int u, int v, int c, int f, double w)
			: from(u), to(v), cap(c), flow(f), cost(w) {}
	};

	const int INF = 0x3f3f3f3f;
	int n, m;
	vector<Edge> edges;
	vector<vector<int>> G;
	vector<int> inq, p, a;
	vector<double> d;
	double eps = 1e-3;
	bool isZero(const double& val) { return abs(val) < eps; }

	void init(int n) {
		this->n = n;
		G.resize(n);
		d.resize(n);
		a.resize(n);
		p.resize(n);
		inq.resize(n);

		for (int i = 0; i < n; i++) {
			G[i].clear();
			d[i] = a[i] = p[i] = inq[i] = 0;
		}
		edges.clear();
	}

	void AddEdge(int from, int to, int cap, double cost) {
		//cerr << from << ' ' << to << ' ' << cap << ' ' << cost << endl;
		edges.push_back(Edge(from, to, cap, 0, cost));
		edges.push_back(Edge(to, from, 0, 0, -cost));
		m = edges.size();
		G[from].push_back(m - 2);
		G[to].push_back(m - 1);
	}

	bool BellmanFord(int s, int t, int& flow, double& cost) {
		for (int i = 0; i < n; i++) d[i] = INF;
		for (int i = 0; i < n; i++) inq[i] = 0;
		d[s] = 0;
		inq[s] = 1;
		p[s] = 0;
		a[s] = INF;
		queue<int> Q;
		Q.push(s);
		while (!Q.empty()) {
			int u = Q.front();
			Q.pop();
			inq[u] = 0;
			for (int i = 0; i < G[u].size(); i++) {
				Edge& e = edges[G[u][i]];
				if (e.cap > e.flow && d[e.to] > d[u] + e.cost) {
					d[e.to] = d[u] + e.cost;
					p[e.to] = G[u][i];
					a[e.to] = min(a[u], e.cap - e.flow);
					if (!inq[e.to]) {
						Q.push(e.to);
						inq[e.to] = 1;
					}
				}
			}
		}
		if (d[t] == INF) return false;  // 当没有可增广的路时退出
		flow += a[t];
		cost += d[t] * a[t];
		for (int u = t; u != s; u = edges[p[u]].from) {
			edges[p[u]].flow += a[t];
			edges[p[u] ^ 1].flow -= a[t];
		}
		return true;
	}

	vector<int> work(const vector<vector<pair<int, double>>>& vec) {
		int nn = vec.size();
		int S = nn, T = nn + 1;
		init(T + 2);

		vector<bool> vis(nn);
		for (int u = 0; u < nn; u++) {
			for (auto& e : vec[u]) {
				int v = e.first;
				double w = e.second;
				if (!isZero(w)) {
					if (!vis[u]) {
						AddEdge(S, u, 1, 0);
						vis[u] = true;
					}
				}
				else {
					if (!vis[v]) {
						AddEdge(v, T, INF, 0);
						vis[v] = true;
					}
				}
				AddEdge(u, v, INF, -w);
			}
		}
		int flow = 0;
		double cost = 0;
		while (BellmanFord(S, T, flow, cost))
			;

		//cerr << "flow = " << flow << endl;
		//cerr << "cost = " << cost << endl;

		vector<int> ret;
		//cerr << "ret: ";
		for (auto& e : edges)
			if (e.to == T)
				if (e.flow > 0) {
					ret.push_back(e.from);
					//cerr << "(" << e.from << ", " << e.flow << ") ";
				}
		//cerr << endl;

		return ret;
	}
};

/*
solving by Max Flow
*/

void adjacency_list_thread_process(int ray_id, int* ny, int ray_index_shift, int voxel_index_shift, unordered_map<octomap::OcTreeKey, int, octomap::OcTreeKey::KeyHash>* voxel_id_map, vector<vector<pair<int, double>>>* bipartite_list, View_Space* view_space, Views_Information* views_information, Voxel_Information* voxel_information, Share_Data* share_data);

class views_voxels_MF {
public:
	int nx, ny, nz;										//三边的点数，视点数nx，射线数ny,体素数nz
	vector<vector<pair<int, double>>>* bipartite_list;	//邻接表
	View_Space* view_space;
	Views_Information* views_information;
	Voxel_Information* voxel_information;
	Share_Data* share_data;
	unordered_map<octomap::OcTreeKey, int, octomap::OcTreeKey::KeyHash>* voxel_id_map;	//体素下标
	MCMF* mcmf;
	vector<int> view_id_set;

	void solve() {
		double now_time = clock();
		view_id_set = mcmf->work(*bipartite_list);
		double cost_time = clock() - now_time;
		cout << "flow network solved with executed time " << cost_time << " ms." << endl;
		cout << view_id_set.size() << " views getted by max flow." << endl;
		share_data->access_directory(share_data->save_path + "/run_time");
		ofstream fout(share_data->save_path + "/run_time/MF" + to_string(view_space->id) + ".txt");
		fout << cost_time << '\t' << view_id_set.size() << endl;
	}

	vector<int> get_view_id_set() {
		return view_id_set;
	}

	views_voxels_MF(int _nx, View_Space* _view_space, Views_Information* _views_information, Voxel_Information* _voxel_information, Share_Data* _share_data) {
		double now_time = clock();
		view_space = _view_space;
		views_information = _views_information;
		voxel_information = _voxel_information;
		share_data = _share_data;
		//视点按照id排序，并建立三分图邻接表
		sort(view_space->views.begin(), view_space->views.end(), view_id_compare);
		nx = _nx;
		ny = views_information->ray_num;
		bipartite_list = new vector<vector<pair<int, double>>>;
		bipartite_list->resize(nx + ny + share_data->voxels_in_BBX);
		//建立体素的id表
		voxel_id_map = new unordered_map<octomap::OcTreeKey, int, octomap::OcTreeKey::KeyHash>;
		//并行遍历每条射线上的体素累加至对应视点
		nz = 0;
		thread** adjacency_list_process = new thread * [views_information->ray_num];
		for (int i = 0; i < views_information->ray_num; i++) {
			adjacency_list_process[i] = new thread(adjacency_list_thread_process, i, &nz, nx, nx + ny, voxel_id_map, bipartite_list, view_space, views_information, voxel_information, share_data);
		}
		for (int i = 0; i < views_information->ray_num; i++) {
			(*adjacency_list_process[i]).join();
		}
		//输出一下具体的图大小
		if (nz != voxel_id_map->size()) cout << "node_z wrong." << endl;
		int num_of_all_edge = 0;
		int num_of_view_edge = 0;
		for (int i = 0; i < bipartite_list->size(); i++) {
			num_of_all_edge += (*bipartite_list)[i].size();
			if (i > nx && i < nx + ny) num_of_view_edge += (*bipartite_list)[i].size();
		}
		cout << "Full edge is " << num_of_all_edge << ". View edge(in) is " << num_of_view_edge << ". Voexl edge(out) is "<< num_of_all_edge - num_of_view_edge<< "."<< endl;
		cout << "adjacency list with interested voxels num " << ny << " getted with executed time " << clock() - now_time << " ms." << endl;
		mcmf = new MCMF();
	}

	~views_voxels_MF() {
		delete bipartite_list;
		delete voxel_id_map;
		delete mcmf;
	}
};

void adjacency_list_thread_process(int ray_id, int* nz, int ray_index_shift, int voxel_index_shift, unordered_map<octomap::OcTreeKey, int, octomap::OcTreeKey::KeyHash>* voxel_id_map, vector<vector<pair<int, double>>>* bipartite_list, View_Space* view_space, Views_Information* views_information, Voxel_Information* voxel_information, Share_Data* share_data) {
	//该射线被哪些视点看到，加入图中
	vector<int> views_id = (*views_information->rays_to_viwes_map)[ray_id];
	for (int i = 0; i < views_id.size(); i++)
		(*bipartite_list)[ray_id + ray_index_shift].push_back(make_pair(views_id[i], 0.0));
	//仅保留感兴趣体素
	double visible = 1.0;
	octomap::KeyRay::iterator first = views_information->rays_info[ray_id]->ray->start;
	octomap::KeyRay::iterator last = views_information->rays_info[ray_id]->ray->stop;
	for (octomap::KeyRay::iterator it = views_information->rays_info[ray_id]->ray->start; it != views_information->rays_info[ray_id]->ray->stop; ++it) {
		//从hash表里查询该key
		auto hash_this_key = (*views_information->occupancy_map).find(*it);
		//找不到节点就下一个
		if (hash_this_key == (*views_information->occupancy_map).end()) continue;
		//读取节点概率值
		double occupancy = hash_this_key->second;
		//读取一下节点为物体表面率
		double on_object = voxel_information->voxel_object(*it, views_information->object_weight);
		//统计信息熵
		double information_gain = on_object * visible * voxel_information->entropy(occupancy);
		visible *= voxel_information->get_voxel_visible(occupancy);
		if (information_gain > share_data->interesting_threshold) {
			octomap::OcTreeKey node_y = *it;
			int voxel_id;
			voxel_information->mutex_rays.lock();
			auto hash_this_node = voxel_id_map->find(node_y);
			//如果没有记录，就视为新的体素
			if (hash_this_node == voxel_id_map->end()) {
				voxel_id = (*nz) + voxel_index_shift;
				(*voxel_id_map)[node_y] = voxel_id;
				(*nz)++;
			}
			else {
				voxel_id = hash_this_node->second;
			}
			voxel_information->mutex_rays.unlock();
			//对于每个视点，统计该体素的id与价值
			for (int i = 0; i < views_id.size(); i++)
			{
				(*voxel_information->mutex_voxels[voxel_id - voxel_index_shift]).lock();
				(*bipartite_list)[voxel_id].push_back(make_pair(ray_id + ray_index_shift, information_gain));
				(*voxel_information->mutex_voxels[voxel_id - voxel_index_shift]).unlock();
			}
		}
	}
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_LOG_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_LOG_HPP_INCLUDED
#include <boost/simd/function/std.hpp>

#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/function/musl.hpp>
#include <boost/simd/function/std.hpp>
#include <boost/assert.hpp>
#include <boost/config.hpp>
#include <cmath>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/frexp.hpp>
#include <boost/simd/function/horn.hpp>
#include <boost/simd/function/musl.hpp>
#include <boost/simd/function/plain.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/std.hpp>

#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/smallestposval.hpp>
#include <boost/simd/constant/sqrt_2o_2.hpp>

#include <boost/simd/detail/constant/log_2hi.hpp>
#include <boost/simd/detail/constant/log_2lo.hpp>

#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( log_
                          , (typename A0)
                          , bd::cpu_
                          , bs::std_tag
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const std_tag &, A0 a0) const BOOST_NOEXCEPT
    {
      return std::log(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( log_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 a0) const BOOST_NOEXCEPT
    {
      return musl_(log)(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( log_
                          , (typename A0)
                          , bd::cpu_
                          , bs::musl_tag
                          , bd::scalar_< bd::single_<A0> >
                          )
  {
  /* origin: FreeBSD /usr/src/lib/msun/src/e_log2f.c */
  /*
   * ====================================================
   * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
   *
   * Developed at SunPro, a Sun Microsystems, Inc. business.
   * Permission to use, copy, modify, and distribute this
   * software is freely granted, provided that this notice
   * is preserved.
   * ====================================================
   */
     BOOST_FORCEINLINE A0 operator() (const musl_tag &, A0 x) const BOOST_NOEXCEPT
    {
      using uiA0 = bd::as_integer_t<A0, unsigned>;
      using iA0 = bd::as_integer_t<A0,   signed>;
      uiA0 ix = bitwise_cast<uiA0>(x);
      iA0 k = 0;
      if (ix < 0x00800000 || ix>>31)         /* x < 2**-126  */
      {
        if (ix<<1 == 0) return Minf<A0>();  /* log(+-0)=-inf */
        if (ix>>31) return Nan<A0>();       /* log(-#) = NaN */
#ifndef BOOST_SIMD_NO_DENORMALS
        /* subnormal number, scale up x */
        k -= 25;
        x *= 33554432.0f;
        ix = bitwise_cast<iA0>(x);
#endif
      }
      else if (ix >= 0x7f800000)
      {
        return x;
      }
      else if (ix == 0x3f800000)
        return 0;

      /* reduce x into [sqrt(2)/2, sqrt(2)] */
      ix += 0x3f800000 - 0x3f3504f3;
      k += bitwise_cast<iA0>(ix>>23) - 0x7f;
      ix = (ix&0x007fffff) + 0x3f3504f3;
      x =  bitwise_cast<A0>(ix);
      A0 f = dec(x);
      A0 s = f/(2.0f + f);
      A0 z = sqr(s);
      A0 w = sqr(z);
      A0 t1= w*horn<A0, 0x3eccce13, 0x3e789e26>(w);
      A0 t2= z*horn<A0, 0x3f2aaaaa, 0x3e91e9ee>(w);
      A0 R = t2 + t1;
      A0 hfsq = 0.5f*sqr(f);
      A0 dk = k;
      return  fma(dk, Log_2hi<A0>(), ((fma(s, (hfsq+R), dk*Log_2lo<A0>()) - hfsq) + f));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( log_
                          , (typename A0)
                          , bd::cpu_
                          , bs::musl_tag
                          , bd::scalar_< bd::double_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const musl_tag &, A0 x) const BOOST_NOEXCEPT
    {
      using uiA0 = bd::as_integer_t<A0, unsigned>;
      using iA0 = bd::as_integer_t<A0,   signed>;
      uiA0 hx = bitwise_cast<uiA0>(x) >> 32;
      iA0 k = 0;
      if (hx < 0x00100000 || hx>>31)
      {
        if(is_eqz(x)) return Minf<A0>();  /* log(+-0)=-inf */
        if (hx>>31)   return Nan<A0>();   /* log(-#) = NaN */
#ifndef BOOST_SIMD_NO_DENORMALS
        /* subnormal number, scale x up */
        k -= 54;
        x *= 18014398509481984.0;
        hx = bitwise_cast<uiA0>(x) >> 32;
#endif
      }
      else if (hx >= 0x7ff00000)
      {
        return x;
      }
      else if (x == One<A0>())
        return Zero<A0>();

      /* reduce x into [sqrt(2)/2, sqrt(2)] */
      hx += 0x3ff00000 - 0x3fe6a09e;
      k += bitwise_cast<iA0>(hx>>20) - 0x3ff;
      hx = (hx&0x000fffff) + 0x3fe6a09e;
      x = bitwise_cast<A0>( (uint64_t)hx<<32 | (bitwise_and(0xffffffffull, bitwise_cast<uiA0>(x))));

      A0 f = dec(x);
      A0 hfsq = 0.5*sqr(f);
      A0 s = f/(2.0f + f);
      A0 z = sqr(s);
      A0 w = sqr(z);
      A0 t1= w*horn<A0, 0x3fd999999997fa04ll, 0x3fcc71c51d8e78afll, 0x3fc39a09d078c69fll > (w);
      A0 t2= z*horn<A0, 0x3fe5555555555593ll, 0x3fd2492494229359ll
                      , 0x3fc7466496cb03dell, 0x3fc2f112df3e5244ll> (w);
      A0 R = t2 + t1;
      A0 dk = k;
      return  fma(dk, Log_2hi<A0>(), ((fma(s, (hfsq+R), dk*Log_2lo<A0>()) - hfsq) + f));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( log_
                          , (typename A0)
                          , bd::cpu_
                          , bs::plain_tag
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const plain_tag &, A0 x) const BOOST_NOEXCEPT
    {
      return musl_(log)(x); //the "plain" version of the algorithm is never speedier than the "musl" version.
      // the call is here to allow a scalar fallback to simd calls
    }
  };

  }
} }

#endif


//
// Created by cc on 2020/8/5.
//

#include <ros/ros.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
#include <geometry_msgs/PoseStamped.h>
#include <math.h>
#include <Eigen/Eigen>
#include <mavros_msgs/State.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>

using namespace Eigen;

Vector3d current_p;
mavros_msgs::State current_state;
ros::Publisher pva_pub;

void positionCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    /// ENU frame to NWU
    current_p << msg->pose.position.y, -msg->pose.position.x, msg->pose.position.z;
}

void stateCallback(const mavros_msgs::State::ConstPtr &msg)
{
    current_state = *msg;
}

void setPVA(Eigen::Vector3d p, Eigen::Vector3d v, Eigen::Vector3d a, double yaw=0.0)
{
    trajectory_msgs::JointTrajectoryPoint pva_setpoint;

    pva_setpoint.positions.push_back(p(0)); //x
    pva_setpoint.positions.push_back(p(1)); //y
    pva_setpoint.positions.push_back(p(2)); //z
    pva_setpoint.positions.push_back(yaw);

    pva_setpoint.velocities.push_back(v(0));
    pva_setpoint.velocities.push_back(v(1));
    pva_setpoint.velocities.push_back(v(2));

    pva_setpoint.accelerations.push_back(a(0));
    pva_setpoint.accelerations.push_back(a(1));
    pva_setpoint.accelerations.push_back(a(2));

    pva_pub.publish(pva_setpoint);

//    ROS_INFO_THROTTLE(1.0, "P x=%f, y=%f, z=%f", pva_setpoint.positions[0], pva_setpoint.positions[1], pva_setpoint.positions[2]);
//    ROS_INFO_THROTTLE(1.0, "V x=%f, y=%f, z=%f", pva_setpoint.velocities[0], pva_setpoint.velocities[1], pva_setpoint.velocities[2]);
//    ROS_INFO_THROTTLE(1.0, "A x=%f, y=%f, z=%f", pva_setpoint.accelerations[0], pva_setpoint.accelerations[1], pva_setpoint.accelerations[2]);

    ROS_INFO("P x=%f, y=%f, z=%f", pva_setpoint.positions[0], pva_setpoint.positions[1], pva_setpoint.positions[2]);
    ROS_INFO("V x=%f, y=%f, z=%f", pva_setpoint.velocities[0], pva_setpoint.velocities[1], pva_setpoint.velocities[2]);
    ROS_INFO("A x=%f, y=%f, z=%f", pva_setpoint.accelerations[0], pva_setpoint.accelerations[1], pva_setpoint.accelerations[2]);
}

/** This function is to generate state to state trajectory **/
void motion_primitives(Eigen::Vector3d p0, Eigen::Vector3d v0, Eigen::Vector3d a0,
                       Eigen::Vector3d pf, Eigen::Vector3d vf, Eigen::Vector3d af, double v_max, double delt_t,
                       Eigen::MatrixXd &p, Eigen::MatrixXd &v, Eigen::MatrixXd &a, Eigen::VectorXd &t)
{
    // % Choose the time as running in average velocity
    // double decay_parameter = 0.5;
    // double T = 0.2;

    double j_limit = 5;
    double a_limit = 3;
    double v_limit = v_max;

//    double T1 = fabs(af(0)-a0(0))/j_limit > fabs(af(1)-a0(1))/j_limit ? fabs(af(0)-a0(0))/j_limit : fabs(af(1)-a0(1))/j_limit;
//    T1 = T1 > fabs(af(2)-a0(2))/j_limit ? T1 : fabs(af(2)-a0(2))/j_limit;
    double T2 = fabs(vf(0)-v0(0))/a_limit > fabs(vf(1)-v0(1))/a_limit ? fabs(vf(0)-v0(0))/a_limit : fabs(vf(1)-v0(1))/a_limit;
    T2 = T2 > fabs(vf(2)-v0(2))/a_limit ? T2 : fabs(vf(2)-v0(2))/a_limit;
    double T3 = fabs(pf(0)-p0(0))/v_limit > fabs(pf(1)-p0(1))/v_limit ? fabs(pf(0)-p0(0))/v_limit : fabs(pf(1)-p0(1))/v_limit;
    T3 = T3 > fabs(pf(2)-p0(2))/v_limit ? T3 : fabs(pf(2)-p0(2))/v_limit;

//    double T = T1 > T2 ? T1 : T2;
//    T = T > T3 ? T : T3;
    double T = T2;
    T = T > T3 ? T : T3;
    T = T < 0.3 ? 0.3 : T;

//    ROS_INFO_THROTTLE(2, "T=%lf", T);

    int times = T / delt_t;

    p = Eigen::MatrixXd::Zero(times, 3);
    v = Eigen::MatrixXd::Zero(times, 3);
    a = Eigen::MatrixXd::Zero(times, 3);
    t = Eigen::VectorXd::Zero(times);

    // % calculate optimal jerk controls by Mark W. Miller
    for(int ii=0; ii<3; ii++)
    {
        double delt_a = af(ii) - a0(ii);
        double delt_v = vf(ii) - v0(ii) - a0(ii)*T;
        double delt_p = pf(ii) - p0(ii) - v0(ii)*T - 0.5*a0(ii)*T*T;

        //%  if vf is not free
        double alpha = delt_a*60/pow(T,3) - delt_v*360/pow(T,4) + delt_p*720/pow(T,5);
        double beta = -delt_a*24/pow(T,2) + delt_v*168/pow(T,3) - delt_p*360/pow(T,4);
        double gamma = delt_a*3/T - delt_v*24/pow(T,2) + delt_p*60/pow(T,3);

        for(int jj=0; jj<times; jj++)
        {
            double tt = (jj + 1)*delt_t;
            t(jj) = tt;
            p(jj,ii) = alpha/120*pow(tt,5) + beta/24*pow(tt,4) + gamma/6*pow(tt,3) + a0(ii)/2*pow(tt,2) + v0(ii)*tt + p0(ii);
            v(jj,ii) = alpha/24*pow(tt,4) + beta/6*pow(tt,3) + gamma/2*pow(tt,2) + a0(ii)*tt + v0(ii);
            a(jj,ii) = alpha/6*pow(tt,3) + beta/2*pow(tt,2) + gamma*tt + a0(ii);
        }
    }
}

void compute_circular_traj(const double r, const double vel, const Eigen::Vector3d p0, const double t,
                           Eigen::Vector3d &p, Eigen::Vector3d &v, Eigen::Vector3d &a)
//@requires r > 0 && vel > 0 && t >= 0;
{
    const double theta = vel*t/r;

    p(0) = r*cos(theta) + p0(0) - r;
    p(1) = r*sin(theta) + p0(1);
    p(2) = p0(2);

    v(0) = -vel*sin(theta);
    v(1) = vel*cos(theta);
    v(2) = 0;

    a(0) = -vel*vel/r*cos(theta);
    a(1) = -vel*vel/r*sin(theta);
    a(2) = 0;
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "control");
    ros::NodeHandle nh;

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("/mavros/state", 1, stateCallback);
    ros::Subscriber pose_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose", 1, positionCallback);

    pva_pub = nh.advertise<trajectory_msgs::JointTrajectoryPoint>("/pva_setpoint", 1);

    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");

    const int LOOPRATE = 40;
    ros::Rate loop_rate(LOOPRATE);

    // wait for FCU connection
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        loop_rate.sleep();
    }

    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;
    ros::Time last_request = ros::Time::now();

    /// Take off with constant acceleration
    double delt_t = 1.0 / LOOPRATE;
    double yaw_target = M_PI / 2.0;

    // Vector3d recorded_position = current_p;
    // while(ros::ok()){
    //     if(current_state.mode != "OFFBOARD" || !current_state.armed){
    //         setPVA(current_p, Vector3d::Zero(), Vector3d::Zero(), yaw_target);
    //         recorded_position = current_p;
    //     }else{
    //         setPVA(recorded_position, Vector3d::Zero(), Vector3d::Zero(), yaw_target);
    //     }

    //     loop_rate.sleep();
    //     ros::spinOnce();
    // }

    Vector3d circle_p0 = current_p;
    while(ros::ok()){
        if(current_state.mode != "OFFBOARD" || !current_state.armed){
            setPVA(current_p, Vector3d::Zero(), Vector3d::Zero(), yaw_target);
            circle_p0 = current_p;
        }else{
            /** Accelerate period **/
            double circle_speed = 0.8;
            double circle_radius = 0.6;
            double acc_t_total = 2 * circle_radius / circle_speed;
            int acc_times = acc_t_total / delt_t;
            double acc_a_value = circle_speed * circle_speed / 2 / circle_radius;
            circle_p0 = current_p;

            for(int i=0; i<acc_times; i++){
                Eigen::Vector3d p, v ,a;

                p << circle_p0(0), circle_p0(1) + 0.5 * acc_a_value * (i * delt_t) * (i * delt_t), circle_p0(2);
                v << 0.0, acc_a_value * i * delt_t, 0.0;
                a << 0.0, acc_a_value, 0.0;
                setPVA(p, v, a, yaw_target);

                loop_rate.sleep();
                ros::spinOnce();
            }
            ROS_WARN("Accelerate Complete!");

            /** Now draw circle **/
            double init_t = 0.0;
            Vector3d circle_start_point = circle_p0;
            circle_start_point(1) += circle_radius;

            while(ros::ok()){

                Eigen::Vector3d p, v ,a;
                compute_circular_traj(circle_radius, circle_speed, circle_start_point, init_t, p, v, a);
                setPVA(p, v, a, yaw_target);//a_t.row(last_index));
                init_t += delt_t;

                if(current_state.mode != "OFFBOARD" || !current_state.armed){
                    break;
                }

                loop_rate.sleep();
                ros::spinOnce();
            }

        }
        loop_rate.sleep();
        ros::spinOnce();
    }

    // ROS_INFO("Arm and takeoff");
    // while(ros::ok()){
    //     if( current_state.mode != "OFFBOARD" &&
    //         (ros::Time::now() - last_request > ros::Duration(5.0))){
    //         if( set_mode_client.call(offb_set_mode) &&
    //             offb_set_mode.response.mode_sent){
    //             ROS_INFO("Offboard enabled");
    //         }
    //         last_request = ros::Time::now();
    //     } else {
    //         if( !current_state.armed &&
    //             (ros::Time::now() - last_request > ros::Duration(5.0))){
    //             if( arming_client.call(arm_cmd) &&
    //                 arm_cmd.response.success){
    //                 ROS_INFO("Vehicle armed");
    //             }
    //             last_request = ros::Time::now();
    //         }
    //     }

    //     trajectory_msgs::JointTrajectoryPoint pva_setpoint;

    //     if(current_state.mode != "OFFBOARD" || !current_state.armed){
    //         setPVA(current_p, Vector3d::Zero(), Vector3d::Zero(), M_PI);
    //     }else{
    //         counter ++;
    //         double z_sp, vz_sp;
    //         if(counter < take_off_send_times / 2){
    //             z_sp = 0.5*take_off_acc*counter*delt_t*counter*delt_t;
    //             vz_sp = counter*delt_t*take_off_acc;
    //             Vector3d p_sp(recorded_takeoff_position(0), recorded_takeoff_position(1), z_sp);
    //             Vector3d v_sp(0, 0, vz_sp);
    //             setPVA(p_sp, v_sp, Vector3d::Zero(), M_PI);

    //         }else if(counter < take_off_send_times){
    //             double t_this = (counter-take_off_send_times/2)*delt_t;
    //             z_sp = take_off_send_times/2*delt_t*take_off_acc*t_this - 0.5*take_off_acc*t_this*t_this;
    //             vz_sp = take_off_send_times/2*delt_t*take_off_acc - take_off_acc*t_this;

    //             Vector3d p_sp(recorded_takeoff_position(0), recorded_takeoff_position(1), z_sp);
    //             Vector3d v_sp(0, 0, vz_sp);
    //             setPVA(p_sp, v_sp, Vector3d::Zero(), M_PI);

    //         }else{
    //             Vector3d p_sp(recorded_takeoff_position(0), recorded_takeoff_position(1), take_off_height);
    //             setPVA(p_sp, Vector3d::Zero(), Vector3d::Zero(), M_PI);
    //             counter --;
    //         }
    //     }

    //     if(current_p(2) > take_off_height-0.05){
    //         ROS_WARN("Takeoff Complete!");
    //         break;
    //     }

    //     loop_rate.sleep();
    //     ros::spinOnce();
    // }

    // /** Take off complete. Go to a point with minimum jerk trajectory **/
    // double circle_radius = 2;

    // MatrixXd p_t, v_t, a_t;
    // Eigen::VectorXd t_vector;
    // Vector3d v0(0.0, 0.0, 0.0);
    // Vector3d a0(0.0, 0.0, 0.0);

    // Vector3d pf(circle_radius, 0, take_off_height);
    // Vector3d vf(0, 0, 0);
    // Vector3d af(0, 0, 0);

    // motion_primitives(current_p, v0, a0, pf, vf, af, 3.0, delt_t, p_t, v_t, a_t, t_vector);

    // for(int i=0; i<t_vector.size(); i++)
    // {
    //     setPVA(p_t.row(i), v_t.row(i), Vector3d::Zero());// a_t.row(i));
    //     loop_rate.sleep();
    //     ros::spinOnce();
    // }

    // while(ros::ok())
    // {
    //     setPVA(p_t.row(t_vector.size()-1), v_t.row(t_vector.size()-1), Vector3d::Zero());// a_t.row(i));
    //     Vector3d last_sp_p = p_t.row(t_vector.size()-1);
    //     Vector3d delt_p = last_sp_p - current_p;
    //     if(delt_p.norm() < 0.2){
    //         ROS_WARN("Align Complete!");
    //         break;
    //     }

    //     loop_rate.sleep();
    //     ros::spinOnce();
    // }

    // /** Now draw circle **/
    // Vector3d circle_p0 = current_p;
    // double init_t = 0.0;
    // while(ros::ok()){
    //     Eigen::Vector3d p, v ,a;
    //     compute_circular_traj(circle_radius, 5.0, circle_p0, init_t, p, v, a);
    //     setPVA(p, v, a);//a_t.row(last_index));

    //     init_t += delt_t;
    //     loop_rate.sleep();
    //     ros::spinOnce();
    // }

    return 0;
}


//  Copyright John Maddock 2007.
//  Copyright Paul A. Bristow 2010
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

// Note that this file contains quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

#include <iostream>
using std::cout; using std::endl; using std::cerr;

//[policy_eg_9

/*`
The previous example was all well and good, but the custom error handlers
didn't really do much of any use.  In this example we'll implement all
the custom handlers and show how the information provided to them can be
used to generate nice formatted error messages.

Each error handler has the general form:

   template <class T>
   T user_``['error_type]``(
      const char* function,
      const char* message,
      const T& val);

and accepts three arguments:

[variablelist
[[const char* function]
   [The name of the function that raised the error, this string
   contains one or more %1% format specifiers that should be
   replaced by the name of real type T, like float or double.]]
[[const char* message]
   [A message associated with the error, normally this
   contains a %1% format specifier that should be replaced with
   the value of ['value]: however note that overflow and underflow messages
   do not contain this %1% specifier (since the value of ['value] is
   immaterial in these cases).]]
[[const T& value]
   [The value that caused the error: either an argument to the function
   if this is a domain or pole error, the tentative result
   if this is a denorm or evaluation error, or zero or infinity for
   underflow or overflow errors.]]
]

As before we'll include the headers we need first:

*/

#include <boost/math/special_functions.hpp>

/*`
Next we'll implement our own error handlers for each type of error,
starting with domain errors:
*/

namespace boost{ namespace math{
namespace policies
{

template <class T>
T user_domain_error(const char* function, const char* message, const T& val)
{
   /*`
   We'll begin with a bit of defensive programming in case function or message are empty:
   */
   if(function == 0)
       function = "Unknown function with arguments of type %1%";
   if(message == 0)
       message = "Cause unknown with bad argument %1%";
   /*`
   Next we'll format the name of the function with the name of type T, perhaps double:
   */
   std::string msg("Error in function ");
   msg += (boost::format(function) % typeid(T).name()).str();
   /*`
   Then likewise format the error message with the value of parameter /val/,
   making sure we output all the potentially significant digits of /val/:
   */
   msg += ": \n";
   int prec = 2 + (std::numeric_limits<T>::digits * 30103UL) / 100000UL;
   // int prec = std::numeric_limits<T>::max_digits10; //  For C++0X Standard Library
   msg += (boost::format(message) % boost::io::group(std::setprecision(prec), val)).str();
   /*`
   Now we just have to do something with the message, we could throw an
   exception, but for the purposes of this example we'll just dump the message
   to std::cerr:
   */
   std::cerr << msg << std::endl;
   /*`
   Finally the only sensible value we can return from a domain error is a NaN:
   */
   return std::numeric_limits<T>::quiet_NaN();
}

/*`
Pole errors are essentially a special case of domain errors,
so in this example we'll just return the result of a domain error:
*/

template <class T>
T user_pole_error(const char* function, const char* message, const T& val)
{
   return user_domain_error(function, message, val);
}

/*`
Overflow errors are very similar to domain errors, except that there's
no %1% format specifier in the /message/ parameter:
*/
template <class T>
T user_overflow_error(const char* function, const char* message, const T& val)
{
   if(function == 0)
       function = "Unknown function with arguments of type %1%";
   if(message == 0)
       message = "Result of function is too large to represent";

   std::string msg("Error in function ");
   msg += (boost::format(function) % typeid(T).name()).str();

   msg += ": \n";
   msg += message;

   std::cerr << msg << std::endl;

   // Value passed to the function is an infinity, just return it:
   return val;
}

/*`
Underflow errors are much the same as overflow:
*/

template <class T>
T user_underflow_error(const char* function, const char* message, const T& val)
{
   if(function == 0)
       function = "Unknown function with arguments of type %1%";
   if(message == 0)
       message = "Result of function is too small to represent";

   std::string msg("Error in function ");
   msg += (boost::format(function) % typeid(T).name()).str();

   msg += ": \n";
   msg += message;

   std::cerr << msg << std::endl;

   // Value passed to the function is zero, just return it:
   return val;
}

/*`
Denormalised results are much the same as underflow:
*/

template <class T>
T user_denorm_error(const char* function, const char* message, const T& val)
{
   if(function == 0)
       function = "Unknown function with arguments of type %1%";
   if(message == 0)
       message = "Result of function is denormalised";

   std::string msg("Error in function ");
   msg += (boost::format(function) % typeid(T).name()).str();

   msg += ": \n";
   msg += message;

   std::cerr << msg << std::endl;

   // Value passed to the function is denormalised, just return it:
   return val;
}

/*`
Which leaves us with evaluation errors: these occur when an internal
error occurs that prevents the function being fully evaluated.
The parameter /val/ contains the closest approximation to the result
found so far:
*/

template <class T>
T user_evaluation_error(const char* function, const char* message, const T& val)
{
   if(function == 0)
       function = "Unknown function with arguments of type %1%";
   if(message == 0)
       message = "An internal evaluation error occurred with "
                  "the best value calculated so far of %1%";

   std::string msg("Error in function ");
   msg += (boost::format(function) % typeid(T).name()).str();

   msg += ": \n";
   int prec = 2 + (std::numeric_limits<T>::digits * 30103UL) / 100000UL;
   // int prec = std::numeric_limits<T>::max_digits10; // For C++0X Standard Library
   msg += (boost::format(message) % boost::io::group(std::setprecision(prec), val)).str();

   std::cerr << msg << std::endl;

   // What do we return here?  This is generally a fatal error, that should never occur,
   // so we just return a NaN for the purposes of the example:
   return std::numeric_limits<T>::quiet_NaN();
}

} // policies
}} // boost::math

/*`
Now we'll need to define a suitable policy that will call these handlers,
and define some forwarding functions that make use of the policy:
*/

namespace mymath
{ // unnamed.

using namespace boost::math::policies;

typedef policy<
   domain_error<user_error>,
   pole_error<user_error>,
   overflow_error<user_error>,
   underflow_error<user_error>,
   denorm_error<user_error>,
   evaluation_error<user_error>
> user_error_policy;

BOOST_MATH_DECLARE_SPECIAL_FUNCTIONS(user_error_policy)

} // unnamed namespace

/*`
We now have a set of forwarding functions, defined in namespace mymath,
that all look something like this:

``
template <class RealType>
inline typename boost::math::tools::promote_args<RT>::type
   tgamma(RT z)
{
   return boost::math::tgamma(z, user_error_policy());
}
``

So that when we call `mymath::tgamma(z)` we really end up calling
`boost::math::tgamma(z, user_error_policy())`, and any
errors will get directed to our own error handlers:
*/

int main()
{
   // Raise a domain error:
   cout << "Result of erf_inv(-10) is: "
      << mymath::erf_inv(-10) << std::endl << endl;
   // Raise a pole error:
   cout << "Result of tgamma(-10) is: "
      << mymath::tgamma(-10) << std::endl << endl;
   // Raise an overflow error:
   cout << "Result of tgamma(3000) is: "
      << mymath::tgamma(3000) << std::endl << endl;
   // Raise an underflow error:
   cout << "Result of tgamma(-190.5) is: "
      << mymath::tgamma(-190.5) << std::endl << endl;
   // Unfortunately we can't predicably raise a denormalised
   // result, nor can we raise an evaluation error in this example
   // since these should never really occur!
} // int main()

/*`

Which outputs:

[pre
Error in function boost::math::erf_inv<double>(double, double):
Argument outside range \[-1, 1\] in inverse erf function (got p=-10).
Result of erf_inv(-10) is: 1.#QNAN

Error in function boost::math::tgamma<long double>(long double):
Evaluation of tgamma at a negative integer -10.
Result of tgamma(-10) is: 1.#QNAN

Error in function boost::math::tgamma<long double>(long double):
Result of tgamma is too large to represent.
Error in function boost::math::tgamma<double>(double):
Result of function is too large to represent
Result of tgamma(3000) is: 1.#INF

Error in function boost::math::tgamma<long double>(long double):
Result of tgamma is too large to represent.
Error in function boost::math::tgamma<long double>(long double):
Result of tgamma is too small to represent.
Result of tgamma(-190.5) is: 0
]

Notice how some of the calls result in an error handler being called more
than once, or for more than one handler to be called: this is an artefact
of the fact that many functions are implemented in terms of one or more
sub-routines each of which may have it's own error handling.  For example
`tgamma(-190.5)` is implemented in terms of `tgamma(190.5)` - which overflows -
the reflection formula for `tgamma` then notices that it is dividing by
infinity and so underflows.
*/

//] //[/policy_eg_9]