#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;
}


#ifndef STAN_MATH_PRIM_SCAL_PROB_WEIBULL_RNG_HPP
#define STAN_MATH_PRIM_SCAL_PROB_WEIBULL_RNG_HPP

#include <boost/random/weibull_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/fun/multiply_log.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/meta/length.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>

namespace stan {
  namespace math {

    /**
     * Return a pseudorandom Weibull variate with given shape and scale using
     * the specified random number generator.
     *
     * @tparam RNG class of random number generator
     * @param alpha shape parameter
     * @param sigma scale parameter
     * @param rng random number generator
     * @return Weibull random variate
     * @throw std::domain_error if alpha or sigma is nonpositive
     */
    template <class RNG>
    inline double
    weibull_rng(double alpha,
                double sigma,
                RNG& rng) {
      using boost::variate_generator;
      using boost::random::weibull_distribution;

      static const char* function("weibull_rng");

      check_positive_finite(function, "Shape parameter", alpha);
      check_positive_finite(function, "Scale parameter", sigma);

      variate_generator<RNG&, weibull_distribution<> >
        weibull_rng(rng, weibull_distribution<>(alpha, sigma));
      return weibull_rng();
    }

  }
}
#endif


#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();
			}
		}
	}
}


#include <cmath>
#include <iostream>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

#include <cedar/perf/vcycle_model.h>

using namespace cedar;

vcycle_model::vcycle_model(short nd, int v1, int v2) : nd(nd), v1(v1), v2(v2), isleaf(true)
{
	if (nd == 2) {
		nc = 4;
		ns = 9;
	} else {
		nc = 8;
		ns = 27;
	}

	for (auto i : range(nd)) {
		nb[i] = 1;
	}
}

int & vcycle_model::nblocks(int dim)
{
	return this->nb[dim];
}

int vcycle_model::nblocks(int dim) const
{
	return this->nb[dim];
}

void vcycle_model::add_level(topo_ptr grid)
{
	grids.push_back(grid);
}

grid_topo & vcycle_model::grid(int i)
{
	int ind;
	if (i < 0) ind = (i + 1)*-1;
	else ind = grids.size() - i - 1;
	#ifdef DEBUG
	return *grids.at(ind);
	#else
	return *grids[ind];
	#endif
}

const grid_topo & vcycle_model::grid(int i) const
{
	int ind;
	if (i < 0) ind = (i + 1)*-1;
	else ind = grids.size() - i - 1;
	#ifdef DEBUG
	return *grids.at(ind);
	#else
	return *grids[ind];
	#endif
}

topo_ptr vcycle_model::grid_ptr(int i)
{
	int ind;
	if (i < 0) ind = (i + 1)*-1;
	else ind = grids.size() - i - 1;
	#ifdef DEBUG
	return grids.at(ind);
	#else
	return grids[ind];
	#endif
}

float vcycle_model::tsmooth(int lvl) const
{
	float time = 0;
	float prod = 1;
	float sigma = 0;
	for (int i = 0; i < nd; i++) {
		prod *= grid(lvl).nlocal(i);
		sigma += grid(lvl).nlocal(i);
	}
	time += 2 * ns * prod * (v1+v2) * tc;
	time += (v1 + v2)*(2*nd*ts + 2*sigma*tw)*nc;

	return time;
}

float vcycle_model::tresidual(int lvl) const
{
	float time = 0;
	float prod = 1;
	float sigma = 0;
	for (int i = 0; i < nd; i++) {
		prod *= grid(lvl).nlocal(i);
		sigma += grid(lvl).nlocal(i);
	}

	time += 2*ns*prod*tc;
	time += 2*nd*ts + 2*sigma*tw;

	return time;
}

float vcycle_model::trestrict(int lvl) const
{
	float time = 0;
	float prod = 1;
	for (int i = 0; i < nd; i++) {
		prod *= grid(lvl).nlocal(i);
	}

	time += 2*ns*prod*tc;

	return time;
}

float vcycle_model::tinterp(int lvl) const
{
	float time = 0;
	float tc_interp =  2.7828045360610174*tc;

	if (nd == 2) {
		time += grid(lvl).nlocal(0)*grid(lvl).nlocal(1) * tc_interp;
		time += 20 * grid(lvl-1).nlocal(0)*grid(lvl-1).nlocal(1) * tc_interp;
		time += 6 * (grid(lvl-1).nlocal(0) + grid(lvl-1).nlocal(1)) * tc_interp;

		time += 4 * ts;
		time += 2 * (grid(lvl).nlocal(0) + grid(lvl).nlocal(1)) * tw;
	} else {
		float prodf = 1;
		float prodc = 1;
		float sigmaf = 0;
		for (int i = 0; i < nd; i++) {
			prodf *= grid(lvl).nlocal(i);
			prodc *= grid(lvl-1).nlocal(i);
			sigmaf += grid(lvl).nlocal(i);
		}
		time += (prodf + 60 * prodc + 15 * grid(lvl-1).nlocal(0) * grid(lvl-1).nlocal(3)) * tc_interp;
		time += (6*grid(lvl-1).nlocal(1)*grid(lvl-1).nlocal(2) + grid(lvl-1).nlocal(2)) * tc_interp;
		time += 6*ts + 2*sigmaf * tw;
	}

	return time;
}

float vcycle_model::tcgsolve() const
{
	float time = 0;

	if (!isleaf) {
		// int p = grid(0).nproc();
		int gather_size = 1;
		len_t cg_size = 1;
		for (auto i : range(nd)) {
			gather_size *= std::ceil(static_cast<double>(grid(0).nproc(i)) / static_cast<double>(nb[i]));
			cg_size *= std::ceil(static_cast<double>(grid(0).nglobal(i))/static_cast<double>(nb[i]));
		}

		// allgather
		time += cg_perf->time();
		time += std::ceil(std::log2(gather_size))*ts;
		time += cg_size*(1 + std::ceil(std::log2(gather_size)))*tw;
		// time += cg_size*(gather_size-1)/gather_size*tw;

		// gather scatter
		// time += 2*std::ceil(std::log2(gather_size))*ts;
		// time += cg_size*tw;

		// time += 2*std::ceil(std::log2(gather_size))*ts;
		// time += cg_size*tw;
	}

	return time;
}

float vcycle_model::agglom() const
{
	float time = 0;

	if (!isleaf) {
		int p = grid(0).nproc();
		int gather_size = 1;
		len_t cg_size = 1;
		for (auto i : range(nd)) {
			gather_size *= std::ceil(static_cast<double>(grid(0).nproc(i)) / static_cast<double>(nb[i]));
			cg_size *= std::ceil(static_cast<double>(grid(0).nglobal(i))/static_cast<double>(nb[i]));
		}

		// allgather
		time += std::ceil(std::log2(gather_size))*ts;
		time += cg_size*(1 + std::ceil(std::log2(gather_size)))*tw;
		// time += cg_size*(gather_size-1)/gather_size*tw;

		// gather scatter
		// time += 2*std::ceil(std::log2(gather_size))*ts;
		// time += cg_size*tw;

		// time += 2*std::ceil(std::log2(gather_size))*ts;
		// time += cg_size*tw;
	}

	return time;
}

float vcycle_model::time() const
{
	float time = 0;

	time += tcgsolve();
	for (int i = 1; i < ngrids(); i++) {
		time += tsmooth(i);
		time += tresidual(i);
		time += trestrict(i);
		time += tinterp(i);
	}

	return time;
}

void vcycle_model::set_cgperf(std::shared_ptr<perf_model> mod)
{
	cg_perf = mod;
	isleaf = false;
}

std::shared_ptr<perf_model> vcycle_model::get_cgperf()
{
	return cg_perf;
}

int vcycle_model::nproc() const
{
	return grid(0).nproc();
}

void vcycle_model::save_levels()
{
	std::ofstream smooth_file;
	std::ofstream residual_file;
	std::ofstream interp_file;
	std::ofstream restrict_file;
	smooth_file.open("smooth.txt", std::ios::out | std::ios::trunc | std::ios::binary);
	residual_file.open("residual.txt", std::ios::out | std::ios::trunc | std::ios::binary);
	interp_file.open("interp.txt", std::ios::out | std::ios::trunc | std::ios::binary);
	restrict_file.open("restrict.txt", std::ios::out | std::ios::trunc | std::ios::binary);
	for (auto i = 1; i < ngrids(); i++) {
		smooth_file << i << " " << tsmooth(i) << '\n';
		residual_file << i << " " << tresidual(i) << '\n';
		interp_file << i << " " << tinterp(i) << '\n';
		restrict_file << i << " " << trestrict(i) << '\n';
	}
	smooth_file.close();
	residual_file.close();
	interp_file.close();
	restrict_file.close();
}

void vcycle_model::rep(std::ostream & os) const
{
	os << "======== vcycle model ========" << '\n';
	os << "nproc:       " << grid(-1).nproc(0) << " " << grid(-1).nproc(1) << '\n';
	os << "nblock:      " << nblocks(0) << " " << nblocks(1) << '\n';
	os << "local size:  " << grid(-1).nlocal(0) << " x " << grid(-1).nlocal(1) << '\n';
	os << "global size: " << grid(-1).nglobal(0) << " x " << grid(-1).nglobal(1) << '\n';
	os << "coarse size: " << grid(0).nglobal(0) << " x " << grid(0).nglobal(1) << '\n';
	os << "nlevel:      " << ngrids() << '\n';
	os << "cgtime:      " << tcgsolve() << '\n';
	os << "time:        " << time() << '\n';
	os << '\n';
	if (!isleaf) {
		os << *cg_perf;
	}
}

void vcycle_model::recur_times(boost::property_tree::ptree & children) const
{
	using namespace boost::property_tree;

	ptree child;

	float smooth_time = 0;
	float residual_time = 0;
	float interp_time = 0;
	float restrict_time = 0;
	for (int i=1; i < ngrids(); i++) {
		smooth_time += tsmooth(i);
		residual_time += tresidual(i);
		interp_time += tinterp(i);
		restrict_time += trestrict(i);
	}

	child.put("relaxation", smooth_time*10);
	child.put("residual", residual_time*10);
	child.put("interp-add", interp_time*10);
	child.put("solve", time()*10);
	child.put("restrict", restrict_time*10);
	child.put("agglomerate", agglom()*10);

	if (!isleaf) {
		(*cg_perf).recur_times(children);
	}

	children.push_back(std::make_pair("",child));
}

void vcycle_model::save_times(std::string iname, std::string oname) const
{
	using namespace boost::property_tree;

	ptree pt, children;

	json_parser::read_json(iname, pt);

	recur_times(children);

	pt.add_child("model", children);
	json_parser::write_json(oname, pt);
}


#define BOOST_TEST_MODULE "test_bond_length_gocontact_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <test/util/utility.hpp>

#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/forcefield/local/BondLengthGoContactInteraction.hpp>
#include <mjolnir/math/constants.hpp>
#include <mjolnir/util/make_unique.hpp>
#include <random>

BOOST_AUTO_TEST_CASE(BondLengthGoContact_numerical_difference)
{
    namespace test = mjolnir::test;

    using traits_type      = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type        = traits_type::real_type;
    using coord_type       = traits_type::coordinate_type;
    using boundary_type    = traits_type::boundary_type;
    using system_type      = mjolnir::System<traits_type>;
    using potential_type   = mjolnir::GoContactPotential<real_type>;
    using interaction_type = mjolnir::BondLengthInteraction<traits_type, potential_type>;

    const real_type k(1.0);
    const real_type native(std::sqrt(3.0));

    potential_type   potential(k, native);
    interaction_type interaction("none", {{ {{0,1}}, potential}});

    std::mt19937 mt(123456789);

    for(std::size_t i = 0; i < 1000; ++i)
    {
        system_type sys(2, boundary_type{});
        test::clear_everything(sys);

        sys.position(0) = coord_type( 0.0, 0.0, 0.0);
        sys.position(1) = coord_type( 1.0, 1.0, 1.0);

        test::apply_random_rotation(sys, mt);
        test::apply_random_perturbation(sys, mt, 0.01);

        constexpr real_type tol = 2e-4;
        constexpr real_type dr  = 1e-5;

        test::check_force(sys, interaction, tol, dr);
        test::check_virial(sys, interaction, tol);
        test::check_force_and_virial(sys, interaction, tol);
        test::check_force_and_energy(sys, interaction, tol);
        test::check_force_energy_virial(sys, interaction, tol);
    }
}

BOOST_AUTO_TEST_CASE(BondLengthGoContactInteraction)
{
    using traits_type      = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type        = traits_type::real_type;
    using coord_type       = traits_type::coordinate_type;
    using boundary_type    = traits_type::boundary_type;
    using system_type      = mjolnir::System<traits_type>;
    using potential_type   = mjolnir::GoContactPotential<real_type>;
    using interaction_type = mjolnir::BondLengthInteraction<traits_type, potential_type>;

    constexpr real_type tol = 1e-7;

    const coord_type pos1(1.0, 0.0, 0.0);
    const coord_type pos2(0.0, 0.0, 0.0);
    system_type sys(2, boundary_type{});

    sys.mass(0)     = 1.0;
    sys.mass(1)     = 1.0;
    sys.rmass(0)    = 1.0;
    sys.rmass(1)    = 1.0;
    sys.position(0) = pos1;
    sys.position(1) = pos2;
    sys.velocity(0) = coord_type(0,0,0);
    sys.velocity(1) = coord_type(0,0,0);
    sys.force(0)    = coord_type(0,0,0);
    sys.force(1)    = coord_type(0,0,0);
    sys.name(0)     = "X";
    sys.name(1)     = "X";
    sys.group(0)    = "NONE";
    sys.group(1)    = "NONE";

    const real_type k(1.0);
    const real_type native(1.0);

    potential_type   potential(k, native);
    interaction_type interaction("none", {{ {{0,1}}, potential}});

    const int N = 10000;
    const real_type dx = 0.001;
    for(int i = 1; i < N; ++i)
    {
        const auto dist = 0.6 + i * dx;
        sys.position(0) = coord_type(0.0,  0.0, 0.0);
        sys.position(1) = coord_type(dist, 0.0, 0.0);
        sys.force(0)    = coord_type(0.0, 0.0, 0.0);
        sys.force(1)    = coord_type(0.0, 0.0, 0.0);

        const real_type deriv = potential.derivative(dist);

        interaction.calc_force(sys);

        const real_type force_strength1 = mjolnir::math::length(sys.force(0));
        const real_type force_strength2 = mjolnir::math::length(sys.force(1));

        BOOST_TEST(force_strength1 == std::abs(deriv), boost::test_tools::tolerance(tol));
        BOOST_TEST(force_strength2 == std::abs(deriv), boost::test_tools::tolerance(tol));

        const auto E_pot = potential.potential(dist);
        const auto E_int = interaction.calc_energy(sys);

        BOOST_TEST(E_pot == E_int, boost::test_tools::tolerance(tol));
    }
}


#pragma once

#include "pulsar/math/EigenHash.hpp"
#include "pulsar/math/BlockByIrrepSpin.hpp"
#include "pulsar/math/TensorImpl.hpp"
#include <bphash/types/memory.hpp>

#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>
namespace pulsar {

/** \brief Specialization of TensorImpl to Eigen's Matrix
 *
 *  \todo Should save/load use the fact that you can grab the double* from an
 *        Eigen matrix?
 *  \todo A lot of duplicate code b/w this and EigenVectorImpl combine?
 */
class EigenMatrixImpl : public MatrixDImpl
{
    public:
        using matrix_type=Eigen::MatrixXd;
        using shared_matrix=std::shared_ptr<matrix_type>;
        /*! \brief For serialization only
         *
         * \warning NOT FOR USE OUTSIDE OF SERIALIZATION
         * \todo Replace if cereal fixes this
         */
        EigenMatrixImpl() = default;

        ///Aliases this Eigen matrix to \p mat
        EigenMatrixImpl(const shared_matrix& mat)
            : mat_(mat) { }

        ///Copies Eigen matrix from \p mat
        EigenMatrixImpl(const matrix_type & mat)
            : mat_(std::make_shared<matrix_type>(mat)) { }

        ///Moves Eigen matrix from \p mat
        EigenMatrixImpl(matrix_type && mat)
            : mat_(std::make_shared<matrix_type>(std::move(mat))) { }

        ///True if the underlying Eigen matrices are the same
        bool operator==(const EigenMatrixImpl& rhs)const
        {
            return *mat_==*rhs.mat_;
        }

        ///True if the underlying Eigen matrices are not the same
        bool operator!=(const EigenMatrixImpl& rhs)const
        {
            return !((*this)==rhs);
        }

        /*! \brief Obtain a hash of the data
         *
         * Details depend on what kind of data is stored.
         * See the hashing functions of the stored type
         * for details.
         */
        bphash::HashValue my_hash(void) const
        {
            return bphash::make_hash(bphash::HashType::Hash128, *this);
        }

        ///\copydoc TensorImpl::sizes
        virtual std::array<size_t, 2> sizes(void) const
        {
            return {static_cast<size_t>((*mat_).rows()),
                    static_cast<size_t>((*mat_).cols())};
        }

        ///\copydoc TensorImpl::get_value
        virtual double get_value(std::array<size_t, 2> idx) const
        {
            return (*mat_)(idx[0], idx[1]);
        }

        ///\copydoc TensorImpl::set_Value
        virtual void set_value(std::array<size_t, 2> idx, double val)
        {
            (*mat_)(idx[0], idx[1]) = val;
        }

        ///Allows you to get the actual matrix (in constant form)
        std::shared_ptr<const matrix_type> get_matrix(void) const
        {
            return mat_;
        }

    private:
        shared_matrix mat_;///<The actual matrix

        DECLARE_SERIALIZATION_FRIENDS
        BPHASH_DECLARE_HASHING_FRIENDS
        template<class Archive>
        void save(Archive & archive) const
        {
            archive(cereal::base_class<MatrixDImpl>(this));
            unsigned int nrow = (unsigned int) mat_->rows();
            unsigned int ncol = (unsigned int) mat_->cols();

            archive(nrow, ncol);

            //! \todo slow
            for(unsigned int i = 0; i < nrow; i++)
            for(unsigned int j = 0; j < ncol; j++)
                archive((*mat_)(i,j));
        }

        template<class Archive>
        void load(Archive & archive)
        {
            archive(cereal::base_class<MatrixDImpl>(this));

            unsigned int nrow, ncol;
            archive(nrow, ncol);

            mat_ = std::make_shared<Eigen::MatrixXd>(nrow, ncol);

            //! \todo slow
            for(unsigned int i = 0; i < nrow; i++)
            for(unsigned int j = 0; j < ncol; j++)
                archive((*mat_)(i,j));
        }

        void hash(bphash::Hasher & h) const
        {
            h(*mat_);
        }
};

template<size_t rank>
class EigenTensorImpl : public TensorImpl<rank,double>
{
    public:
        using tensor_type=Eigen::Tensor<double,rank>;
        using shared_tensor=std::shared_ptr<tensor_type>;
        /*! \brief For serialization only
         *
         * \warning NOT FOR USE OUTSIDE OF SERIALIZATION
         * \todo Replace if cereal fixes this
         */
        EigenTensorImpl() = default;

        ///Aliases this Eigen Tensor to \p mat
        EigenTensorImpl(const shared_tensor& mat)
            : mat_(mat) { }

        ///Copies Eigen Tensor from \p mat
        EigenTensorImpl(const tensor_type & mat)
            : mat_(std::make_shared<tensor_type>(mat)) { }

        ///Moves Eigen Tensor from \p mat
        EigenTensorImpl(tensor_type && mat)
            : mat_(std::make_shared<tensor_type>(std::move(mat))) { }

        ///True if the underlying Eigen matrices are the same
        bool operator==(const EigenTensorImpl<rank>& rhs)const
        {
            return mat_==rhs.mat_;
        }

        ///True if the underlying Eigen matrices are not the same
        bool operator!=(const EigenTensorImpl& rhs)const
        {
            return !((*this)==rhs);
        }

        /*! \brief Obtain a hash of the data
         *
         * Details depend on what kind of data is stored.
         * See the hashing functions of the stored type
         * for details.
         */
        bphash::HashValue my_hash(void) const
        {
            return bphash::make_hash(bphash::HashType::Hash128, *this);
        }

        ///\copydoc TensorImpl::sizes
        virtual std::array<size_t, rank> sizes(void) const
        {
            auto dim=mat_->dimensions();
            std::array<size_t,rank> temp;
            for(size_t i=0;i<rank;++i)temp[i]=dim[i];
            return temp;
        }

        ///\copydoc TensorImpl::get_value
        virtual double get_value(std::array<size_t, rank> idx) const
        {
            return (*mat_)(idx);
        }

        ///\copydoc TensorImpl::set_Value
        virtual void set_value(std::array<size_t, rank> idx, double val)
        {
            (*mat_)(idx) = val;
        }

        ///Allows you to get the actual Tensor (in constant form)
        std::shared_ptr<const tensor_type> get_matrix(void) const
        {
            return mat_;
        }

    private:
        shared_tensor mat_;///<The actual Tensor

        DECLARE_SERIALIZATION_FRIENDS
        BPHASH_DECLARE_HASHING_FRIENDS
        template<class Archive>
        void save(Archive & archive) const
        {
            archive(cereal::base_class<TensorImpl<rank,double>>(this));
            //archive(nrow, ncol);
        }

        template<class Archive>
        void load(Archive & archive)
        {
            archive(cereal::base_class<TensorImpl<rank,double>>(this));
        }

        void hash(bphash::Hasher &) const
        {
           // h(*mat_);
        }
};

///Same as EigenMatrix except for vectors
class EigenVectorImpl : public VectorDImpl
{
    public:
        using vector_type=Eigen::VectorXd;///<Type of Eigen's vector class
        using shared_vector=std::shared_ptr<vector_type>;///<Type of ptr to vec

        /*! \brief For serialization only
         *
         * \warning NOT FOR USE OUTSIDE OF SERIALIZATION
         * \todo Replace if cereal fixes this
         */
        EigenVectorImpl() = default;

        ///Aliases this vector to the one in \p mat
        EigenVectorImpl(const shared_vector & mat)
            : mat_(mat) { }

        ///Deep copies the vector \p mat
        EigenVectorImpl(const vector_type & mat)
            : mat_(std::make_shared<vector_type>(mat)) { }

        ///Moves \p mat into this vector
        EigenVectorImpl(vector_type && mat)
            : mat_(std::make_shared<vector_type>(std::move(mat))) { }

                ///True if the underlying Eigen vectors are the same
        bool operator==(const EigenVectorImpl& rhs)const
        {
            return *mat_==*rhs.mat_;
        }

        ///True if the underlying Eigen vectors are not the same
        bool operator!=(const EigenVectorImpl& rhs)const
        {
            return !((*this)==rhs);
        }

        ///\copydoc TensorImpl::sizes()
        virtual std::array<size_t, 1> sizes(void) const
        {
            return {static_cast<size_t>((*mat_).size())};
        }

        ///\copydoc TensorImpl::get_value
        virtual double get_value(std::array<size_t, 1> idx) const
        {
            return (*mat_)(idx[0]);
        }

        ///\copydoc TensorImpl::set_value
        virtual void set_value(std::array<size_t, 1> idx, double val)
        {
            (*mat_)(idx[0]) = val;
        }

        ///Returns the vector wrapped in this class
        std::shared_ptr<const vector_type> get_matrix(void) const
        {
            return mat_;
        }

        ///Returns the hash of this object
        bphash::HashValue my_hash()const
        {
            return bphash::make_hash(bphash::HashType::Hash128, *this);
        }

    private:
        shared_vector mat_;///<The actual vector

        DECLARE_SERIALIZATION_FRIENDS
        BPHASH_DECLARE_HASHING_FRIENDS

        template<class Archive>
        void save(Archive & archive) const
        {
            archive(cereal::base_class<VectorDImpl>(this));
            unsigned int nrow = (unsigned int) mat_->rows();
            unsigned int ncol = (unsigned int) mat_->cols();

            archive(nrow, ncol);

            //! \todo slow
            for(unsigned int i = 0; i < nrow; i++)
            for(unsigned int j = 0; j < ncol; j++)
                archive((*mat_)(i,j));
        }

        template<class Archive>
        void load(Archive & archive)
        {
            archive(cereal::base_class<VectorDImpl>(this));

            unsigned int nrow, ncol;
            archive(nrow, ncol);

            mat_ = std::make_shared<Eigen::VectorXd>(nrow, ncol);

            //! \todo slow
            for(unsigned int i = 0; i < nrow; i++)
            for(unsigned int j = 0; j < ncol; j++)
                archive((*mat_)(i,j));
        }

        void hash(bphash::Hasher & h) const
        {
            h(*mat_);
        }
};

///Converts a pulsar matrix to an Eigen matrix
std::shared_ptr<const Eigen::MatrixXd>
convert_to_eigen(const MatrixDImpl & ten);

///Converts a pulsar vector to an Eigen vector
std::shared_ptr<const Eigen::VectorXd>
convert_to_eigen(const VectorDImpl & ten);

///Converts a pulsar tensor to an Eigen tensor
template<size_t rank>
std::shared_ptr<const Eigen::Tensor<double,rank>>
convert_to_eigen(const TensorImpl<rank,double>& ten)
{
    auto test = dynamic_cast<const EigenTensorImpl<rank>*>(&ten);
    if(test)
        return test->get_matrix();
    else
        throw pulsar::PulsarException("Not an Eigen Tensor and conversion not coded");
}

///Eigen Matrix suitable for use with symmetry and spin
typedef BlockByIrrepSpin<std::shared_ptr<EigenMatrixImpl>> BlockedEigenMatrix;

///Eigen vector suitable for use with symmetry and spin
typedef BlockByIrrepSpin<std::shared_ptr<EigenVectorImpl>> BlockedEigenVector;

} // close namespace pulsar


// Copyright (c) 2014-2018 The Dash Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "chainparams.h"
#include "validation.h"

#include "test/test_zeroone.h"

#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(subsidy_tests, TestingSetup)

BOOST_AUTO_TEST_CASE(block_subsidy_test)
{
    const Consensus::Params& consensusParams = Params(CBaseChainParams::MAIN).GetConsensus();

    uint32_t nPrevBits;
    int32_t nPrevHeight;
    CAmount nSubsidy;

    nPrevBits = 0x1c4a47c4;
    nPrevHeight = 262770;
    nSubsidy = GetBlockSubsidy(nPrevBits, nPrevHeight, consensusParams, false);
    BOOST_CHECK_EQUAL(nSubsidy, 2383333334ULL);

    nPrevBits = 0x1c4a47c4;
    nPrevHeight = 262801;
    nSubsidy = GetBlockSubsidy(nPrevBits, nPrevHeight, consensusParams, false);
    BOOST_CHECK_EQUAL(nSubsidy, 2383333334ULL);

    nPrevBits = 0x1c29ec00;
    nPrevHeight = 525599;
    nSubsidy = GetBlockSubsidy(nPrevBits, nPrevHeight, consensusParams, false);
    BOOST_CHECK_EQUAL(nSubsidy, 2184722223ULL);

    nPrevBits = 0x1c29ec00;
    nPrevHeight = 525601;
    nSubsidy = GetBlockSubsidy(nPrevBits, nPrevHeight, consensusParams, false);
    BOOST_CHECK_EQUAL(nSubsidy, 2184722223ULL);
}

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/*!
  @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