//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_HYPERBOLIC_FUNCTIONS_SCALAR_SINHC_HPP_INCLUDED
#define NT2_HYPERBOLIC_FUNCTIONS_SCALAR_SINHC_HPP_INCLUDED
#include <nt2/hyperbolic/functions/sinhc.hpp>
#include <nt2/hyperbolic/functions/details/sinhc_kernel.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/log_2.hpp>
#include <nt2/include/constants/maxlog.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/functions/scalar/abs.hpp>
#include <nt2/include/functions/scalar/average.hpp>
#include <nt2/include/functions/scalar/exp.hpp>
#include <nt2/include/functions/scalar/if_else.hpp>
#include <nt2/include/functions/scalar/rec.hpp>
#include <nt2/include/functions/scalar/sqr.hpp>
#include <boost/simd/sdk/config.hpp>

#ifndef BOOST_SIMD_NO_INFINITIES
#include <nt2/include/constants/inf.hpp>
#endif

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( sinhc_, tag::cpu_
                            , (A0)
                            , (scalar_< floating_<A0> >)
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL(1)
    {
      //////////////////////////////////////////////////////////////////////////////
      // if x = abs(a0) is less than 1 sinhc is computed using a polynomial(float)
      // respectively rational(double) approx inspired from cephes sinh approx.
      // else according x < Threshold e =  exp(x) or exp(x/2) is respectively
      // computed
      // * in the first case sinh is ((e-rec(e))/2)/x
      // * in the second     sinh is (e/2/x)*e (avoiding undue overflow)
      // Threshold is Maxlog - Log_2 defined in Maxshlog
      //////////////////////////////////////////////////////////////////////////////
      result_type x = nt2::abs(a0);
      #ifndef BOOST_SIMD_NO_INFINITIES
      if (x == Inf<A0>()) return x;
      #endif
      if( x < One<A0>())
      {
        return details::sinhc_kernel<A0>::compute(sqr(x));
      }
      else
      {
        bool test1 = (x >  Maxlog<A0>()-Log_2<A0>());
        A0 fac = if_else(test1, Half<A0>(), One<A0>());
        A0 tmp = exp(x*fac);
        A0 tmp1 = (Half<A0>()*tmp)/x;
        return if_else(test1, tmp1*tmp, average(tmp, -rec(tmp))/x);
      }
     }
  };

} }
#endif


/**********************
规定 type:
type 0: left bottom
type 1: right bottom
type 2: left top
type 3: right top
**********************/

#include "ros/ros.h"
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud.h>

#include <vector>
#include <string.h>
#include <math.h>

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

#include <Eigen/SVD>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include </usr/local/include/g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <cmath>
#include <chrono>

#define start_region_boundary_distance 2.1

std::vector<int> laser_blacklist;

double angle_max;
double angle_min;
double angle_increment;

int corner_type = 0;
//ranges_boundary和angles_boundary是按逆时针顺序排列的边缘点信息
std::vector<double> ranges_boundary;
std::vector<double> angles_boundary;

int corner = 0;

//g2o图优化顶点：位姿
class BoundaryPoseVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    // 重置
    virtual void setToOriginImpl()
    {
        _estimate << 0,0,0;
    }
     // 更新
    virtual void oplusImpl( const double* update )
    {
        _estimate += Eigen::Vector3d(update);
    }
    // 存盘和读盘：留空
    virtual bool read( std::istream& in ) {}
    virtual bool write( std::ostream& out ) const {}
};
// 误差模型 模板参数：观测值维度，类型，连接顶点类型
//_measurement是角度和距离的二维向量
//场地下边的点所构成的图优化边
class BoundaryEdge_Bottom: public g2o::BaseUnaryEdge<2, Eigen::Vector2d, BoundaryPoseVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    BoundaryEdge_Bottom(double point_theta, double point_range):
    BaseUnaryEdge(), _point_theta(point_theta), _point_range(point_range) {}
    // 计算曲线模型误差
    void computeError()
    {
        const BoundaryPoseVertex* pose = static_cast<const BoundaryPoseVertex*> (_vertices[0]);
        const Eigen::Vector3d pose_estimation = pose->estimate();
        _error(0,0) = pose_estimation(1) + _measurement(1) * sin( _measurement(0) + pose_estimation(2) ) ;
    }
    virtual void linearizeOplus()
    {
        const BoundaryPoseVertex* pose = static_cast<const BoundaryPoseVertex*> (_vertices[0]);
        const Eigen::Vector3d pose_estimation = pose->estimate();

        _jacobianOplusXi(0,0) = 0;
        _jacobianOplusXi(0,1) = 1;
        _jacobianOplusXi(0,2) = _point_range * cos(_point_theta + pose_estimation(2));
    }
    virtual bool read( std::istream& in ) {}
    virtual bool write( std::ostream& out ) const {}

public:
	  double _point_range;
		double _point_theta;
		//measurement(0)其实和point_theta是一样的，measurement(1)其实和point_range是一样的
};
//场地左边的点所构成的图优化边
class BoundaryEdge_Left: public g2o::BaseUnaryEdge<2, Eigen::Vector2d, BoundaryPoseVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    BoundaryEdge_Left(double point_theta, double point_range):
    BaseUnaryEdge(), _point_theta(point_theta), _point_range(point_range) {}
    // 计算曲线模型误差
    void computeError()
    {
        const BoundaryPoseVertex* pose = static_cast<const BoundaryPoseVertex*> (_vertices[0]);
        const Eigen::Vector3d pose_estimation = pose->estimate();
        _error(0,0) = pose_estimation(0) + _measurement(1) * cos( _measurement(0) + pose_estimation(2) ) ;
    }
    virtual void linearizeOplus()
    {
        const BoundaryPoseVertex* pose = static_cast<const BoundaryPoseVertex*> (_vertices[0]);
        const Eigen::Vector3d pose_estimation = pose->estimate();

        _jacobianOplusXi(0,0) = 1;
        _jacobianOplusXi(0,1) = 0;
        _jacobianOplusXi(0,2) = -_point_range * sin(_point_theta + pose_estimation(2));
    }
    virtual bool read( std::istream& in ) {}
    virtual bool write( std::ostream& out ) const {}

public:
	  double _point_range;
		double _point_theta;
		//measurement(0)其实和point_theta是一样的，measurement(1)其实和point_range是一样的
};
class BoundaryEdge_Top: public g2o::BaseUnaryEdge<2, Eigen::Vector2d, BoundaryPoseVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    BoundaryEdge_Top(double point_theta, double point_range):
    BaseUnaryEdge(), _point_theta(point_theta), _point_range(point_range) {}
    // 计算曲线模型误差
    void computeError()
    {
        const BoundaryPoseVertex* pose = static_cast<const BoundaryPoseVertex*> (_vertices[0]);
        const Eigen::Vector3d pose_estimation = pose->estimate();
        _error(0,0) = 5.0 - pose_estimation(1) - _measurement(1) * sin( _measurement(0) + pose_estimation(2) ) ;
    }
    virtual void linearizeOplus()
    {
        const BoundaryPoseVertex* pose = static_cast<const BoundaryPoseVertex*> (_vertices[0]);
        const Eigen::Vector3d pose_estimation = pose->estimate();

        _jacobianOplusXi(0,0) = 0;
        _jacobianOplusXi(0,1) = -1;
        _jacobianOplusXi(0,2) = -_point_range * cos(_point_theta + pose_estimation(2));
    }
    virtual bool read( std::istream& in ) {}
    virtual bool write( std::ostream& out ) const {}

public:
	  double _point_range;
		double _point_theta;
		//measurement(0)其实和point_theta是一样的，measurement(1)其实和point_range是一样的
};
class BoundaryEdge_Right: public g2o::BaseUnaryEdge<2, Eigen::Vector2d, BoundaryPoseVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    BoundaryEdge_Right(double point_theta, double point_range):
    BaseUnaryEdge(), _point_theta(point_theta), _point_range(point_range) {}
    // 计算曲线模型误差
    void computeError()
    {
        const BoundaryPoseVertex* pose = static_cast<const BoundaryPoseVertex*> (_vertices[0]);
        const Eigen::Vector3d pose_estimation = pose->estimate();
        _error(0,0) = 8.0 - pose_estimation(0) - _measurement(1) * cos( _measurement(0) + pose_estimation(2) ) ;
    }
    virtual void linearizeOplus()
    {
        const BoundaryPoseVertex* pose = static_cast<const BoundaryPoseVertex*> (_vertices[0]);
        const Eigen::Vector3d pose_estimation = pose->estimate();

        _jacobianOplusXi(0,0) = 0;
        _jacobianOplusXi(0,1) = -1;
        _jacobianOplusXi(0,2) = _point_range * sin(_point_theta + pose_estimation(2));
    }
    virtual bool read( std::istream& in ) {}
    virtual bool write( std::ostream& out ) const {}

public:
	  double _point_range;
		double _point_theta;
		//measurement(0)其实和point_theta是一样的，measurement(1)其实和point_range是一样的
};

//计算提取出来的边界点的曲率的平方的25倍，由于是为了比较大小，不在开方以及乘以倍数，注意不能提取最靠两边的四个点，否则会出现错误
double curve_smoothness_square(int number) {
	double delta_x = 0, delta_y = 0;

	for (int i = -9 ; i < 10; i++) {
			if (i == 0)
				continue;
			double delta_angle = angles_boundary[number + i] - angles_boundary[number];
			delta_x += ranges_boundary[number] - ranges_boundary[number + i] * cos(delta_angle);
			delta_y += ranges_boundary[number + i] * sin(delta_angle);
	}
	double smoothness_square = (delta_x * delta_x + delta_y * delta_y) / (ranges_boundary[number] * ranges_boundary[number]);
	return smoothness_square;
}
//将角度限制在-π到π之间
void angle_standard(double& angle) {
	while (angle > M_PI)
		angle -= 2 * M_PI;
	while (angle < - M_PI)
		angle += 2 * M_PI;
}

//提取边缘点信息到ranges_boundary和angles_boundary
void extract_boundary_information(const std::vector<double> &ranges_raw, const std::vector<int> &increment_raw) {
	int available_number = ranges_raw.size();
	std::vector<bool> distance(available_number, false);

	//找到第一个不符合距离要求的点
	int first_negative = -1;
	for (int i = 0; i < available_number; i++) {
		if (ranges_raw[i] < start_region_boundary_distance)
			distance[i] = true;
		else if (first_negative == -1)
			first_negative = i;
	}

	int continuous_max_last_location = 0;
	int continuous_max_number = 0;
	int continuous_max_first_location = 0;
	int continuous_first_location_temp = 0;
	int continuous_number_now = 0;
	//找到满足距离要求的最大连续点集,以判断两个直角边位置,从第一个不满足距离要求的点开始，循环一圈，防止重复遗漏，以及vector起始终止处连续点集判断错误的情况
	for (int i = first_negative + 1; i < available_number; i++) {
		if (distance[i]) {
			//如果上一个点远，这一个点近，则取当前点作为当前连续点集起始边界，由于是从first_negative+1开始，到available结束的，所以不会判断初始集外的位置
			if (!distance[i - 1])
				continuous_first_location_temp = i;

			continuous_number_now++;
			if (continuous_number_now > continuous_max_number) {
				continuous_max_number = continuous_number_now;
				continuous_max_last_location = i;
				continuous_max_first_location = continuous_first_location_temp;
			}
		} else
			continuous_number_now = 0;
	}
	if (distance[0]) {
		if (!distance[available_number - 1])
			continuous_first_location_temp = 0;

		continuous_number_now++;
		if (continuous_number_now > continuous_max_number) {
			continuous_max_number = continuous_number_now;
			continuous_max_last_location = 0;
			continuous_max_first_location = continuous_first_location_temp;
		}
	} else
		continuous_number_now = 0;
	for (int i = 1; i < first_negative + 1; i++) {
		if (distance[i]) {
			//如果上一个点远，这一个点近，则取当前点作为当前连续点集起始边界，由于是从first_negative+1开始，到available结束的，所以不会判断初始集外的位置
			if (!distance[i - 1])
				continuous_first_location_temp = i;

			continuous_number_now++;
			if (continuous_number_now > continuous_max_number) {
				continuous_max_number = continuous_number_now;
				continuous_max_last_location = i;
				continuous_max_first_location = continuous_first_location_temp;
			}
		} else
			continuous_number_now = 0;
	}

	ranges_boundary.clear();
	angles_boundary.clear();

	if (continuous_max_first_location < continuous_max_last_location) {
		for (int i = continuous_max_first_location; i < continuous_max_last_location + 1; i++) {
			ranges_boundary.push_back(ranges_raw[i]);
			angles_boundary.push_back(angle_min + angle_increment * increment_raw[i]);
		}
	} else {
		for (int i = continuous_max_first_location; i < available_number; i++) {
			ranges_boundary.push_back(ranges_raw[i]);
			angles_boundary.push_back(angle_min + angle_increment * increment_raw[i]);
		}
		for (int i = 0; i <= continuous_max_last_location; i++) {
			ranges_boundary.push_back(ranges_raw[i]);
			angles_boundary.push_back(angle_min + angle_increment * increment_raw[i]);
		}
	}
}

//根据距离算坐标，根据曲率算角点，corner_number是曲率最大点
void find_roughpose_and_corner(Eigen::Vector3d &pose, int &corner_number, const int type) {

	double smoothness_max = 0;
	corner_number = 0;
	for (int i = 9; i < ranges_boundary.size() - 9; i++) {
		double smoothness = curve_smoothness_square(i);
		if (smoothness > smoothness_max) {
			smoothness_max = smoothness;
			corner_number = i;
		}
	}
	std::cout << "Corner: " << corner_number << "\tSmoothness_max1: " << smoothness_max << std::endl;

	double min_range1 = 100, min_range2 = 100;

	for (int i = 0; i < corner_number - 9; i++) {
		if (ranges_boundary[i] < min_range1)
			min_range1 = ranges_boundary[i];
	}
	for (int i = corner_number + 9; i < ranges_boundary.size(); i++) {
		if (ranges_boundary[i] < min_range2) {
			min_range2 = ranges_boundary[i];
		}
	}

	//距离边缘最近的距离分别是lidar位置x,y，alpha是x=0线到lidar的angle_min的基准线的角度 theta是中间变量
	double x, y, alpha, theta;

	if (min_range1 > ranges_boundary[corner_number] && min_range2 > ranges_boundary[corner_number]) {
		std::cout << "Ranges of all boundary points are bigger than range of corner points!" << std::endl
							<< "Fatal error happened! Program is shuting down..." << std::endl;

		exit(0);
	}

	switch(type) {
		case 0:
			alpha = angles_boundary[corner_number];
			x = min_range1;
			y = min_range2;
			if (min_range1 < min_range2)
				theta = acos(min_range1 / ranges_boundary[corner_number]);
			else
				theta = asin(min_range2 / ranges_boundary[corner_number]);
			alpha = M_PI - alpha + theta;
			break;
		case 1:
			alpha = angles_boundary[corner_number];
			x = 8 - min_range2;
			y = min_range1;
			if (min_range1 > min_range2)
				theta = acos(min_range2 / ranges_boundary[corner_number]);
			else
				theta = asin(min_range1 / ranges_boundary[corner_number]);
			alpha = - alpha - theta;
			break;
		case 2:
			alpha = angles_boundary[corner_number];
			x = min_range2;
			y = 5 - min_range1;
			if (min_range1 > min_range2)
				theta = acos(min_range2 / ranges_boundary[corner_number]);
			else
				theta = asin(min_range1 / ranges_boundary[corner_number]);
			alpha = M_PI - theta - alpha;
			break;
		case 3:
			alpha = angles_boundary[corner_number];
			x = 8 - min_range1;
			y = 5 - min_range2;
			if (min_range1 < min_range2)
				theta = acos(min_range1 / ranges_boundary[corner_number]);
			else
				theta = asin(min_range2 / ranges_boundary[corner_number]);
			alpha = -alpha + theta;
			break;
	}

	angle_standard(alpha);
	pose = Eigen::Vector3d(x, y, alpha);
}

Eigen::Vector3d find_pose_precise(const Eigen::Vector3d &initial_pose, const int& corner, const int type) {
	Eigen::Vector3d pose;
	// pose 维度为 3, landmark 维度为 1
	typedef g2o::BlockSolver< g2o::BlockSolverTraits<3, 1> > Block;
	// 线性方程求解器
	Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>();
	// 矩阵块求解器
	Block* solver_ptr = new Block ( linearSolver );
	g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
	g2o::SparseOptimizer optimizer;
	optimizer.setAlgorithm ( solver );

  // vertex
	BoundaryPoseVertex* pose_vertex = new BoundaryPoseVertex(); // camera pose
	pose_vertex->setEstimate(initial_pose);
	pose_vertex->setId(0);
	optimizer.addVertex(pose_vertex);

	int index = 1;

	Eigen::Matrix2d Information_Matrix = Eigen::Matrix2d::Identity() * 1e-4;

	switch(type) {
		case 0:
			for (int i = 0; i < corner - 9; i++) {
				Eigen::Vector2d point(angles_boundary[i], ranges_boundary[i]);
				BoundaryEdge_Left* edge = new BoundaryEdge_Left(angles_boundary[i], ranges_boundary[i]);
				edge->setId(index);
				edge->setVertex(0, pose_vertex);
				// 设置连接的顶点
				edge->setMeasurement(point);
				// 观测数值
				edge->setInformation(Information_Matrix);
				// 信息矩阵：协方差矩阵之逆
				optimizer.addEdge(edge);
				index++;
			}
			for (int i = corner + 9; i < ranges_boundary.size(); i++) {
					Eigen::Vector2d point(angles_boundary[i], ranges_boundary[i]);
					BoundaryEdge_Bottom* edge = new BoundaryEdge_Bottom(angles_boundary[i], ranges_boundary[i]);
					edge->setId(index);
					edge->setVertex(0, pose_vertex);
					// 设置连接的顶点
					edge->setMeasurement(point);
					// 观测数值
					edge->setInformation(Information_Matrix);
					// 信息矩阵：协方差矩阵之逆
					optimizer.addEdge(edge);
					index++;
				}
			break;
		case 1:
			for (int i = 0; i < corner - 9; i++) {
				Eigen::Vector2d point(angles_boundary[i], ranges_boundary[i]);
				BoundaryEdge_Bottom* edge = new BoundaryEdge_Bottom(angles_boundary[i], ranges_boundary[i]);
				edge->setId(index);
				edge->setVertex(0, pose_vertex);
				// 设置连接的顶点
				edge->setMeasurement(point);
				// 观测数值
				edge->setInformation(Information_Matrix);
				// 信息矩阵：协方差矩阵之逆
				optimizer.addEdge(edge);
				index++;
			}
			for (int i = corner + 9; i < ranges_boundary.size(); i++) {
				Eigen::Vector2d point(angles_boundary[i], ranges_boundary[i]);
				BoundaryEdge_Right* edge = new BoundaryEdge_Right(angles_boundary[i], ranges_boundary[i]);
				edge->setId(index);
				edge->setVertex(0, pose_vertex);
				// 设置连接的顶点
				edge->setMeasurement(point);
				// 观测数值
				edge->setInformation(Information_Matrix);
				// 信息矩阵：协方差矩阵之逆
				optimizer.addEdge(edge);
				index++;
			}
			break;
		case 2:
			for (int i = 0; i < corner - 9; i++) {
				Eigen::Vector2d point(angles_boundary[i], ranges_boundary[i]);
				BoundaryEdge_Top* edge = new BoundaryEdge_Top(angles_boundary[i], ranges_boundary[i]);
				edge->setId(index);
				edge->setVertex(0, pose_vertex);
				// 设置连接的顶点
				edge->setMeasurement(point);
				// 观测数值
				edge->setInformation(Information_Matrix);
				// 信息矩阵：协方差矩阵之逆
				optimizer.addEdge(edge);
				index++;
			}
			for (int i = corner + 9; i < ranges_boundary.size(); i++) {
				Eigen::Vector2d point(angles_boundary[i], ranges_boundary[i]);
				BoundaryEdge_Left* edge = new BoundaryEdge_Left(angles_boundary[i], ranges_boundary[i]);
				edge->setId(index);
				edge->setVertex(0, pose_vertex);
				// 设置连接的顶点
				edge->setMeasurement(point);
				// 观测数值
				edge->setInformation(Information_Matrix);
				// 信息矩阵：协方差矩阵之逆
				optimizer.addEdge(edge);
				index++;
			}
			break;
		case 3:
			for (int i = 0; i < corner - 9; i++) {
				Eigen::Vector2d point(angles_boundary[i], ranges_boundary[i]);
				BoundaryEdge_Right* edge = new BoundaryEdge_Right(angles_boundary[i], ranges_boundary[i]);
				edge->setId(index);
				edge->setVertex(0, pose_vertex);
				// 设置连接的顶点
				edge->setMeasurement(point);
				// 观测数值
				edge->setInformation(Information_Matrix);
				// 信息矩阵：协方差矩阵之逆
				optimizer.addEdge(edge);
				index++;
			}
			for (int i = corner + 9; i < ranges_boundary.size(); i++) {
				Eigen::Vector2d point(angles_boundary[i], ranges_boundary[i]);
				BoundaryEdge_Top* edge = new BoundaryEdge_Top(angles_boundary[i], ranges_boundary[i]);
				edge->setId(index);
				edge->setVertex(0, pose_vertex);
				// 设置连接的顶点
				edge->setMeasurement(point);
				// 观测数值
				edge->setInformation(Information_Matrix);
				// 信息矩阵：协方差矩阵之逆
				optimizer.addEdge(edge);
				index++;
			}
			break;
	}
	std::cout << "start optimization" << std::endl;
	optimizer.initializeOptimization();
	optimizer.optimize(100);

	return pose_vertex->estimate();
}

void scanCallback(const sensor_msgs::LaserScanConstPtr msg) {
	angle_max = msg->angle_max;
	angle_min = msg->angle_min;
	angle_increment = msg->angle_increment;
	int angle_number = (int)( (angle_max - angle_min)/angle_increment ) + 1;

	std::vector<double> ranges_raw;
	std::vector<int> increment_raw;

	//去除遮挡点，保留有效雷达点信息，ranges_raw是range信息，increment是有效点顺序信息（用于后续生成角度信息）
	for (int i = 0; i < angle_number; i++) {
		if (msg->ranges[i] != std::numeric_limits<float>::infinity()) {
			ranges_raw.push_back(msg->ranges[i]);
			increment_raw.push_back(i);
		}
	}

	extract_boundary_information(ranges_raw, increment_raw);
	//std::cout << "boundary_size: " << ranges_boundary.size() << std::endl;
	Eigen::Vector3d rough_pose;

	find_roughpose_and_corner(rough_pose, corner, corner_type);

	std::cout << "rough_pose: " << rough_pose.transpose() << std::endl;

	Eigen::Vector3d pose;
	pose = find_pose_precise(rough_pose, corner, corner_type);
	angle_standard(pose(2));

	std::cout << "precise_pose: " << pose.transpose() << std::endl << std::endl;
}

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

  ros::Subscriber scan_sub = n.subscribe("/scan", 1, scanCallback);
	sensor_msgs::LaserScan scan;

	ros::Publisher boundary_pub = n.advertise<sensor_msgs::PointCloud>("boundary", 50);
	ros::Publisher corner_pub = n.advertise<sensor_msgs::PointCloud>("corner", 50);

	ros::Rate loop_rate(10);
	while (ros::ok()) {
		unsigned int num_points = ranges_boundary.size();
		sensor_msgs::PointCloud cloud;
    cloud.header.stamp = ros::Time::now();
    cloud.header.frame_id = "laser";

    cloud.points.resize(num_points);

    //we'll also add an intensity channel to the cloud
    cloud.channels.resize(1);
    cloud.channels[0].name = "intensities";
    cloud.channels[0].values.resize(num_points);

    //generate some fake data for our point cloud
    for(unsigned int i = 0; i < num_points; ++i){
      cloud.points[i].x = ranges_boundary[i] * cos(angles_boundary[i]);
      cloud.points[i].y = ranges_boundary[i] * sin(angles_boundary[i]);
      cloud.points[i].z = 0;
      cloud.channels[0].values[i] = 100;
    }

    sensor_msgs::PointCloud corner_cloud;
		corner_cloud.header.stamp = ros::Time::now();
		corner_cloud.header.frame_id = "laser";
		corner_cloud.points.resize(2);
		corner_cloud.channels.resize(1);
		corner_cloud.channels[0].name = "intensities";
		corner_cloud.channels[0].values.resize(2);
		if (corner != 0) {
			corner_cloud.points[0].x = (ranges_boundary[corner] + 1) * cos(angles_boundary[corner]);
			corner_cloud.points[0].y = (ranges_boundary[corner] + 1) * sin(angles_boundary[corner]);
			corner_cloud.points[0].z = 0;
			corner_cloud.channels[0].values[0] = 200;
		}
		corner_cloud.points[1].x = 1;
		corner_cloud.points[1].y = 0;
		corner_cloud.points[1].z = 0;
		corner_cloud.channels[0].values[1] = 10;
    boundary_pub.publish(cloud);
		corner_pub.publish(corner_cloud);
		ros::spinOnce();
		loop_rate.sleep();
	}
  return 0;
}


//
// Created by Hao Wu on 11/7/16.
//

#include <iostream>
#include "ROWPlus/ROWPlus.h"
#include "lorenz96.h"
#include <boost/numeric/odeint.hpp>

using namespace Eigen;
using namespace ROWPlus;
using namespace boost::numeric::odeint;
using namespace std;

template<typename vec>
void initX(vec& x, size_t N) {
  for (int i = 0; i < N; ++i)
    x[i] = 0.5 * ((double) i - 0.5 * (double) N);
}

int main() {
  const double t1 = 3.0;

  lorenz96 fun;

  std::cout << "Lorenz-96 (N=40)" << std::endl;

  lorenz96::state_type x_ref(40);
  initX(x_ref, 40);
  typedef runge_kutta_cash_karp54<lorenz96::state_type> error_stepper_type;
  integrate_adaptive(make_controlled<error_stepper_type>(1.0e-12, 1.0e-12),
                     fun, x_ref, 0.0, t1, 1e-8 );

  // initialize solution vectors
  lorenz96::state_type x_t1(40);
  VectorXd x_t2(40);

  // creat solver: runge_kutta4_classic
  runge_kutta4_classic< lorenz96::state_type > stepper_rk4;
  // create solver: ROWPlus::rosenbrock4
  ROWPlus::rosenbrock4<lorenz96, double> stepper_grk4t;
  stepper_grk4t.makeConstantStepper(&fun);
  // creat solver: rosenbrock_krylov4 ROK4A
  rosenbrock_krylov4<lorenz96, double> stepper_rok4a(4);
  ODEOptions<double> &_opts = stepper_rok4a.getOptions();
  _opts.TypeScheme = ROK4A;
  stepper_rok4a.makeConstantStepper(&fun);
  // creat solver: rosenbrock_krylov4 ROK4E
  rosenbrock_krylov4<lorenz96, double> stepper_rok4e(4);
  stepper_rok4e.makeConstantStepper(&fun);

  //
  double dt = 0.05;
  while (dt >= 5e-5) {
    ROWPlusSolverSpace::Status ret;
    cout << dt << " ";
    // stepper_rk54
    initX(x_t1, 40);
    integrate_const( stepper_rk4 , fun , x_t1 , 0.0 , t1 , dt );
    Map<VectorXd>(x_t1.data(), 40) -= Map<VectorXd>(x_ref.data(), 40);
    cout << Map<VectorXd>(x_t1.data(), 40).stableNorm() /
            Map<VectorXd>(x_ref.data(), 40).stableNorm() << " ";
    // stepper_grk4t
    initX(x_t2, 40);
    ret = stepper_grk4t.step(x_t2, 0.0, t1, dt);
    if (ret != ROWPlusSolverSpace::ComputeSucessful) {
      cout << "STAT = " << ret << endl;
      return 1;
    }
    x_t2 -= Map<VectorXd>(x_ref.data(), 40);
    cout << x_t2.stableNorm() /
        Map<VectorXd>(x_ref.data(), 40).stableNorm() << " ";
    // stepper_rok4a
    initX(x_t2, 40);
    ret = stepper_rok4a.step(x_t2, 0.0, t1, dt);
    if (ret != ROWPlusSolverSpace::ComputeSucessful) {
      cout << "STAT = " << ret << endl;
      return 1;
    }
    x_t2 -= Map<VectorXd>(x_ref.data(), 40);
    cout << x_t2.stableNorm() /
        Map<VectorXd>(x_ref.data(), 40).stableNorm() << " ";
    // stepper_rok4e
    initX(x_t2, 40);
    ret = stepper_rok4e.step(x_t2, 0.0, t1, dt);
    if (ret != ROWPlusSolverSpace::ComputeSucessful) {
      cout << "STAT = " << ret << endl;
      return 1;
    }
    x_t2 -= Map<VectorXd>(x_ref.data(), 40);
    cout << x_t2.stableNorm() /
        Map<VectorXd>(x_ref.data(), 40).stableNorm() << " ";

    cout << endl;
    dt /= 2.0;
  }
  return 0;
}


/* Copyright (C) 2012-2017 IBM Corp.
 * This program is Licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance
 * with the License. You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. See accompanying LICENSE file.
 */
/* blockMatmul1D.cpp - Data-movement operations on arrays of slots
 */
#include <algorithm>
#include <NTL/BasicThreadPool.h>
#include "matmul.h"

// A class that implements the basic (sparse) 1D matrix-vector functions
template<class type> class blockMatmul1D_impl {
  PA_INJECT(type)
  const MatrixCacheType buildCache;
  std::unique_ptr<CachedzzxMatrix> zCache;
  std::unique_ptr<CachedDCRTMatrix> dCache;

  BlockMatMul<type>& mat;
  const EncryptedArrayDerived<type>& ea;

public:
  blockMatmul1D_impl(MatMulBase& _mat, MatrixCacheType tag, long dim)
    : buildCache(tag), mat(dynamic_cast< BlockMatMul<type>& >(_mat)),
      ea(_mat.getEA().getDerived(type()))
  {
    long n = ea.sizeOfDimension(dim) * ea.getDegree();
    if (buildCache==cachezzX)
      zCache.reset(new CachedzzxMatrix(NTL::INIT_SIZE, n));
    else if (buildCache==cacheDCRT)
      dCache.reset(new CachedDCRTMatrix(NTL::INIT_SIZE, n));
  }

  // Extract one "column" from a matrix that was built with buildLinPolyCoeffs
  bool shiftedColumnInDiag(zzX& zpoly, long f,
                           const std::vector< std::vector<RX> >& diag)
  {
    // extract "column" with index f and store it in cvev
    bool zero = true;
    vector<RX> cvec(ea.size());
    for (long j = 0; j < ea.size(); j++) {
      cvec[j] = diag[j][f];
      if (!NTL::IsZero(cvec[j])) zero = false;
    }
    if (zero) { // all are zeros
      zpoly.kill();
      return true;
    }
    ea.encode(zpoly, cvec);

    if (f>0) {
      long p = ea.getContext().zMStar.getP();
      long m = ea.getContext().zMStar.getM();
      long d = ea.getDegree();
      long exp = PowerMod(mcMod(p, m), d-f, m); // apply inverse automorphism
      const auto& F = ea.getTab().getPhimXMod();
      RX rpoly1, rpoly2;

      convert(rpoly1, zpoly);
      plaintextAutomorph(rpoly2,rpoly1, exp, m, F);
      convert(zpoly, rpoly2);
    }
    return false;
  }

  // Process a single block diagonal with index idx along dimenssion dim,
  // making calls to the get(i,j) method and storing the result in
  // the diag matrix (# of vectors = extenssion-degree).
  bool processDiagonal1(std::vector< std::vector<RX> >& diag,
			long dim, long i, long D, long d)
  {
    bool zDiag = true; // is this a zero diagonal?
    long nzLast = -1;  // index of last non-zero entry

    mat_R entry(INIT_SIZE, d, d);
    std::vector<RX> entry1(d);
    std::vector< std::vector<RX> > tmpDiag(D);

    // Process the entries in this diagonal one at a time
    for (long j = 0; j < D; j++) { // process entry j
      bool zEntry = mat.get(entry, mcMod(j-i, D), j); // entry [j-i mod D, j]
      // get(...) returns true if the entry is empty, false otherwise

      if (!zEntry && IsZero(entry)) zEntry = true;// zero is an empty entry too
      assert(zEntry || (entry.NumRows() == d && entry.NumCols() == d));

      if (!zEntry) {   // not a zero entry
        zDiag = false; // mark diagonal as non-empty

	for (long jj = nzLast+1; jj < j; jj++) {// clear from last nonzero entry
          tmpDiag[jj].assign(d, RX());
        }
        nzLast = j; // current entry is the last nonzero one

        // recode entry as a vector of polynomials
        for (long k = 0; k < d; k++) conv(entry1[k], entry[k]);

        // compute the linearlized polynomial coefficients
	ea.buildLinPolyCoeffs(tmpDiag[j], entry1);
      }
    }
    if (zDiag) return true; // zero diagonal, nothing to do

    // clear trailing zero entries
    for (long jj = nzLast+1; jj < D; jj++) {
      tmpDiag[jj].assign(d, RX());
    }

    if (D==1) diag.assign(ea.size(), tmpDiag[0]); // dimension of size one
    else for (long j = 0; j < ea.size(); j++)
           diag[j] = tmpDiag[ ea.coordinate(dim,j) ];
           // rearrange the indexes based on the current dimension

    return false; // a nonzero diagonal
  }

  // Process a single block diagonal with index idx along dimenssion dim,
  // making calls to the multiGet(i,j,k) method and storing the result in
  // the diag matrix (# of vectors = extenssion-degree).
  bool processDiagonal2(std::vector< std::vector<RX> >& diag,
			long dim, long idx, long D, long d)
  {
    bool zDiag = true; // is this a zero diagonal?
    long nzLast = -1;  // index of last non-zero entry

    mat_R entry(INIT_SIZE, d, d);
    std::vector<RX> entry1(d);

    // Get the slots in this diagonal one at a time
    long blockIdx, rowIdx, colIdx;
    for (long j = 0; j < ea.size(); j++) { // process entry j
      if (dim == ea.dimension()) { // "special" last dimenssion of size 1
	rowIdx = colIdx = 0; blockIdx=j;
      } else {
        std::tie(blockIdx, colIdx)
	  = ea.getContext().zMStar.breakIndexByDim(j, dim);
	rowIdx = mcMod(colIdx-idx,D);
      }
      bool zEntry = mat.multiGet(entry,rowIdx,colIdx,blockIdx);
      // entry [i,j-i mod D] in the block corresponding to blockIdx
      // multiGet(...) returns true if the entry is empty, false otherwise

      if (!zEntry && IsZero(entry)) zEntry=true; // zero is an empty entry too
      assert(zEntry ||
             (entry.NumRows() == d && entry.NumCols() == d));

      if (!zEntry) {    // non-empty entry
	zDiag = false;  // mark diagonal as non-empty

	for (long jj = nzLast+1; jj < j; jj++) {// clear from last nonzero entry
	  for (long k = 0; k < d; k++)
	    clear(diag[jj][k]);
        }
	nzLast = j; // current entry is the last nonzero one

	// recode entry as a vector of polynomials
	for (long k = 0; k < d; k++) conv(entry1[k], entry[k]);

        // compute the linearlized polynomial coefficients
	ea.buildLinPolyCoeffs(diag[j], entry1);
      }
    }
    if (zDiag) return true; // zero diagonal, nothing to do

    // clear trailing zero entries
    for (long jj = nzLast+1; jj < ea.size(); jj++)
      for (long k = 0; k < d; k++)
	clear(diag[jj][k]);

    return false; // a nonzero diagonal
  }

  void multiply(Ctxt* ctxt, long dim, bool oneTransform)
  {
    assert(dim >= 0 && dim <= ea.dimension());
    RBak bak; bak.save(); ea.getTab().restoreContext(); // backup NTL modulus

    long nslots = ea.size();
    long d = ea.getDegree();
    long D = (dim == ea.dimension()) ? 1 : ea.sizeOfDimension(dim);

    const PAlgebra& zMStar = ea.getContext().zMStar;
    long p = zMStar.getP();
    long m = zMStar.getM();

    std::vector< std::vector<RX> > diag(nslots); // scratch space
    for (long j = 0; j < nslots; j++) diag[j].resize(d);

    std::vector<Ctxt> acc;
    std::unique_ptr<Ctxt> shCtxt;
    if (ctxt!=nullptr) { // we need to do an actual multiplication
      ctxt->cleanUp(); // not sure, but this may be a good idea
      acc.assign(d, Ctxt(ZeroCtxtLike, *ctxt));
      shCtxt.reset(new Ctxt(*ctxt));
    }

    // Check if we have the relevant constant in cache
    CachedzzxMatrix* zcp;
    CachedDCRTMatrix* dcp;
    mat.getCache(&zcp, &dcp);

    // Process the diagonals one at a time
    for (long e = 0; e < D; e++) { // process diagonal e
      bool zeroDiag = true;
      // For each diagonal e, we update the d accumulators y_0,..,y_{d-1}
      // with y_f += \sigma^{-f}(\lambda_{e,f}) * \rho^e(x)

      std::vector<zzX> zpoly(d, zzX());
      std::vector<zzX*> zzxPtr(d,nullptr);
      std::vector<DoubleCRT*> dcrtPtr(d,nullptr);

      if (dcp!=nullptr) for (long f=0; f<d; f++) { // DoubleCRT cache exists
          dcrtPtr[f] = (*dcp)[d*e +f].get();
          if (dcrtPtr[f]!=nullptr) zeroDiag = false;
      }
      else if (zcp!=nullptr) for (long f=0; f<d; f++) {// zzX but no DoubleCRT
          zzxPtr[f] = (*zcp)[d*e +f].get();
          if (zzxPtr[f]!=nullptr) zeroDiag = false;
      } else {
       zeroDiag = oneTransform? this->processDiagonal1(diag, dim, e, D, d)
                               : this->processDiagonal2(diag, dim, e, D, d);

        // extract the "columns" from diag and encode them in zpoly
        if (!zeroDiag) for (long f=0; f<d; f++) {
          if (!shiftedColumnInDiag(zpoly[f], f, diag))// returns true on zero
            zzxPtr[f] = &(zpoly[f]);
        }
      }
      if (zeroDiag) continue; // nothing to do for this diagonal
      // done preparing all the zzxPtr, dcrtPtr variables

      // Rotate the ciphertext to position corresponding to diagonal e.
      // The code below uses only rotate-by-one operations if this is
      // a good dimenssion and the matrix is dense, hence it may require
      // fewer key-switching matrices

      if (ctxt!=nullptr) {
	if (e > 0) { // rotate the ciphertext
          *shCtxt = *ctxt;
          ea.rotate1D(*shCtxt, dim, e);
	} // if (e>0)
	// The implementation above incurs an extra mult-by-constant due
	// to the masks in rotate1D when applied in a "bad dimension".
	// These masks can be folded into the constants here, but then
	// we would need two constants for each (e,f) rather than one,
	// namely const*mask and const*(1-mask).
	// We should implement that optimization at some point.

	// Depending on zzxPtr, dcrtPtr, update the accumulated sums
	for (long f=0; f<d; f++) if (dcrtPtr[f]!=nullptr || zzxPtr[f]!=nullptr) {
            Ctxt tmp1(*shCtxt);
            if (dcrtPtr[f] != nullptr) tmp1.multByConstant(*(dcrtPtr[f]));
            else                    tmp1.multByConstant(*(zzxPtr[f]));
            acc[f] += tmp1;
          }
      } // if (ctxt!=nullptr)

      // allocate constants and store in the cache, if needed
     if (buildCache==cachezzX) for (long f=0; f<d; f++) {
          (*zCache)[d*e +f].reset( new zzX(*(zzxPtr[f])) );
      }
      else if (buildCache==cacheDCRT) for (long f=0; f<d; f++) {
          (*dCache)[d*e +f].reset(new DoubleCRT(*(zzxPtr[f]),ea.getContext()));
      }
    } // end of e'th diagonal

    // Finally, compute the result as \sum_{f=0}^{d-1} \sigma_f(y_f)
    if (ctxt!=nullptr) {
      *ctxt = acc[0];
      for (long f = 1; f < d; f++) {
	acc[f].frobeniusAutomorph(f);
	*ctxt += acc[f];
      }
    }
    // "install" the cache (if needed)
    if (buildCache == cachezzX)
      mat.installzzxcache(zCache);
    else if (buildCache == cacheDCRT)
      mat.installDCRTcache(dCache);
  } // end of multiply(...)
};

// Wrapper functions around the implemenmtation class
static void blockMatmul1d(Ctxt* ctxt, MatMulBase& mat, long dim,
		          MatrixCacheType buildCache, bool oneTransform)
{
  MatMulLock locking(mat, buildCache);

  // If locking.getType()!=cacheEmpty then we really do need to
  // build the cache, and we also have the lock for it.

  if (locking.getType()==cacheEmpty && ctxt==nullptr) //  nothing to do
    return;

  switch (mat.getEA().getTag()) {
    case PA_GF2_tag: {
      blockMatmul1D_impl<PA_GF2> M(mat, locking.getType(), dim);
      M.multiply(ctxt, dim, oneTransform);
      break;
    }
    case PA_zz_p_tag: {
      blockMatmul1D_impl<PA_zz_p> M(mat, locking.getType(), dim);
      M.multiply(ctxt, dim, oneTransform);
      break;
    }
    default:
      throw std::logic_error("matmul1d: neither PA_GF2 nor PA_zz_p");
  }
}
void buildCache4BlockMatMul1D(MatMulBase& mat,
			      long dim, MatrixCacheType buildCache)
{ blockMatmul1d(nullptr, mat, dim, buildCache, true); }

void blockMatMul1D(Ctxt& ctxt, MatMulBase& mat, long dim,
               MatrixCacheType buildCache)
{ blockMatmul1d(&ctxt, mat, dim, buildCache, true); }

void buildCache4BlockMatMulti1D(MatMulBase& mat,
				long dim, MatrixCacheType buildCache)
{ blockMatmul1d(nullptr, mat, dim, buildCache, false); }

void blockMatMulti1D(Ctxt& ctxt, MatMulBase& mat, long dim,
		     MatrixCacheType buildCache)
{ blockMatmul1d(&ctxt, mat, dim, buildCache, false); }

// Versions for plaintext rather than ciphertext, useful for debugging
template<class type> class blockMatmul1D_pa_impl {
public:
  PA_INJECT(type)

  static void multiply(NewPlaintextArray& pa, BlockMatMul<type>& mat,
		       long dim, bool oneTrans=false)
  {
    const EncryptedArrayDerived<type>& ea = mat.getEA().getDerived(type());
    const PAlgebra& zMStar = ea.getContext().zMStar;
    RBak bak; bak.save(); ea.getTab().restoreContext();

    long n = ea.size();
    long D = ea.sizeOfDimension(dim);
    long d = ea.getDegree();

    vector< vector<RX> > data1(n/D);
    for (long k = 0; k < n/D; k++)
      data1[k].resize(D);

    // copy the data into a vector of 1D vectors
    vector<RX>& data = pa.getData<type>();
    for (long i = 0; i < n; i++) {
      long k,j;
      std::tie(k,j) = zMStar.breakIndexByDim(i, dim);
      data1[k][j] = data[i];       // k= along dim, j = the rest of i
    }
    for (long k = 0; k < n/D; k++) { // multiply each vector by a matrix
      for (long j = 0; j < D; j++) { // matrix-vector multiplication
	vec_R acc, tmp, tmp1;
	mat_R val;
	acc.SetLength(d);
	for (long i = 0; i < D; i++) {
          bool zero = oneTrans? mat.get(val, i, j)
                              : mat.multiGet(val, i, j, k);
	  if (!zero) { // if non-zero, multiply and add
            VectorCopy(tmp1, data1[k][i], d);
            mul(tmp, tmp1, val);
            add(acc, acc, tmp);
	  }
	}
	long idx = zMStar.assembleIndexByDim(make_pair(k,j), dim);
        conv(data[idx], acc);
      }
    }
  }
};
void blockMatMul1D(NewPlaintextArray& pa, MatMulBase& mat, long dim)
{
  switch (mat.getEA().getTag()) {
    case PA_GF2_tag: {
      BlockMatMul<PA_GF2>& mat1= dynamic_cast< BlockMatMul<PA_GF2>& >(mat);
      blockMatmul1D_pa_impl<PA_GF2>::multiply(pa, mat1, dim, true);
      break;
    }
    case PA_zz_p_tag: {
      BlockMatMul<PA_zz_p>& mat1= dynamic_cast< BlockMatMul<PA_zz_p>& >(mat);
      blockMatmul1D_pa_impl<PA_zz_p>::multiply(pa, mat1, dim, true);
      break;
    }
    default:
      throw std::logic_error("blockMatMul1D: neither PA_GF2 nor PA_zz_p");
  }
}
void blockMatMulti1D(NewPlaintextArray& pa, MatMulBase& mat, long dim)
{
  switch (mat.getEA().getTag()) {
    case PA_GF2_tag: {
      BlockMatMul<PA_GF2>& mat1= dynamic_cast< BlockMatMul<PA_GF2>& >(mat);
      blockMatmul1D_pa_impl<PA_GF2>::multiply(pa, mat1, dim, false);
      break;
    }
    case PA_zz_p_tag: {
      BlockMatMul<PA_zz_p>& mat1= dynamic_cast< BlockMatMul<PA_zz_p>& >(mat);
      blockMatmul1D_pa_impl<PA_zz_p>::multiply(pa, mat1, dim, false);
      break;
    }
    default:
      throw std::logic_error("blockMatMulti1D: neither PA_GF2 nor PA_zz_p");
  }
}


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2001 - 2021 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Authors: Wolfgang Bangerth, Ralf Hartmann, University of Heidelberg, 2001
 */

// 以下第一个include文件现在可能已经众所周知，不需要进一步解释。

#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/convergence_table.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/manifold_lib.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/fe/fe_values.h>

// 这个包含文件是新的。即使我们在本教程中不求解PDE，我们也要使用FE_Nothing类提供的自由度为零的假有限元。

#include <deal.II/fe/fe_nothing.h>

// 下面的头文件也是新的：在其中，我们声明了MappingQ类，我们将使用该类来处理任意阶的多项式映射。

#include <deal.II/fe/mapping_q.h>

// 这又是一个C++的文件。

#include <iostream>
#include <fstream>
#include <cmath>

// 最后一步和以前的程序一样。

namespace Step10
{
  using namespace dealii;

// 现在，由于我们要计算 $\pi$ 的值，我们必须与一些东西进行比较。这些是 $\pi$ 的前几个数字，我们事先定义好，以便以后使用。由于我们想计算两个数字的差值，而这两个数字是相当精确的，计算出的 $\pi$ 的近似值的精度在一个双数变量可以容纳的数字范围内，所以我们宁可将参考值声明为 long double ，并给它增加一些数字。

  const long double pi = 3.141592653589793238462643L;

// 然后，第一个任务将是生成一些输出。由于这个程序非常小，我们在其中没有采用面向对象的技术，也没有声明类（当然，我们使用了库的面向对象的功能）。相反，我们只是将功能打包成独立的函数。我们使这些函数成为空间维数的模板，以符合使用deal.II时的通常做法，尽管我们只对两个空间维数使用这些函数，当试图对任何其他空间维数使用时，会出现异常。

// 这些函数中的第一个只是生成一个圆的三角形（hyperball），并输出 $Q_p$ 的不同值的单元的映射。然后，我们细化一次网格，再做一次。

  template <int dim>
  void gnuplot_output()
  {
    std::cout << "Output of grids into gnuplot files:" << std::endl
              << "===================================" << std::endl;
//因此，
//首先生成一个圆的粗略三角剖分，并将一个合适的边界描述与之关联。默认情况下， GridGenerator::hyper_ball 将SphericalManifold附加到边界上（内部使用FlatManifold），所以我们简单地调用该函数并继续前进。

    Triangulation<dim> triangulation;
    GridGenerator::hyper_ball(triangulation);

// 然后在当前网格上交替生成 $Q_1$ 、 $Q_2$ 和 $Q_3$ 映射的输出，以及（在循环体的末端）对网格进行一次全局细化。

    for (unsigned int refinement = 0; refinement < 2; ++refinement)
      {
        std::cout << "Refinement level: " << refinement << std::endl;

        std::string filename_base = "ball_" + std::to_string(refinement);

        for (unsigned int degree = 1; degree < 4; ++degree)
          {
            std::cout << "Degree = " << degree << std::endl;

// 为此，首先建立一个描述映射的对象。这是用MappingQ类来完成的，该类在构造函数中采用了它应使用的多项式程度作为参数。

            const MappingQ<dim> mapping(degree);

// 顺便提一下，对于一个片状线性映射，你可以给MappingQ的构造函数一个 1 的值，但也有一个MappingQ1类可以达到同样的效果。历史上，它以比MappingQ更简单的方式做了很多事情，但今天只是后者的一个包装。然而，如果你没有明确指定另一个映射，它仍然是库中许多地方隐含使用的类。

// 为了真正用这个映射写出现在的网格，我们设置了一个对象，我们将用它来输出。我们将生成Gnuplot输出，它由一组描述映射的三角图的线条组成。默认情况下，三角剖分的每个面只画一条线，但由于我们想明确地看到映射的效果，所以我们想更详细地了解这些面。这可以通过传递给输出对象一个包含一些标志的结构来实现。在目前的情况下，由于Gnuplot只能画直线，我们在面孔上输出了一些额外的点，这样每个面孔就由30条小线来画，而不是只有一条。这足以让我们看到一条弯曲的线，而不是一组直线的印象。

            GridOut               grid_out;
            GridOutFlags::Gnuplot gnuplot_flags(false, 60);
            grid_out.set_flags(gnuplot_flags);

// 最后，生成一个文件名和一个用于输出的文件。

            std::string filename =
              filename_base + "_mapping_q_" + std::to_string(degree) + ".dat";
            std::ofstream gnuplot_file(filename);

// 然后把三角图写到这个文件里。该函数的最后一个参数是一个指向映射对象的指针。这个参数有一个默认值，如果没有给出值，就会取一个简单的MappingQ1对象，我们在上面简单介绍过。这样就会在输出中产生一个真实边界的片状线性近似。

            grid_out.write_gnuplot(triangulation, gnuplot_file, &mapping);
          }
        std::cout << std::endl;

// 在循环结束时，对网格进行全局细化。

        triangulation.refine_global();
      }
  }

// 现在我们进行代码的主要部分，即 $\pi$ 的近似。圆的面积当然是由 $\pi r^2$ 给出的，所以有一个半径为1的圆，面积代表的只是被搜索的数字。面积的数值计算是通过在整个计算域中积分值为1的常数函数来进行的，即通过计算面积 $\int_K 1 dx=\int_{\hat K} 1
// \ \textrm{det}\ J(\hat x) d\hat x \approx \sum_i \textrm{det}
// \ J(\hat x_i)w(\hat x_i)$ ，其中总和延伸到三角形中所有活动单元上的所有正交点， $w(x_i)$ 是正交点的重量 $x_i$ 。每个单元上的积分都是通过数字正交来逼近的，因此我们唯一需要的额外成分是建立一个FEValues对象，提供每个单元的相应`JxW`值。注意`JxW`是指Jacobian determinant
// times weight的缩写；因为在数字正交中，两个因子总是出现在相同的地方，所以我们只提供合并的数量，而不是两个单独的数量）。我们注意到，在这里我们不会在其最初的目的中使用FEValues对象，即用于计算特定正交点上的特定有限元的基函数值。相反，我们只用它来获得正交点的 "JxW"，而不考虑我们将给FEValues对象的构造者的（假）有限元。给予FEValues对象的实际有限元根本不使用，所以我们可以给任何。

  template <int dim>
  void compute_pi_by_area()
  {
    std::cout << "Computation of Pi by the area:" << std::endl
              << "==============================" << std::endl;

// 对于所有单元的数字正交，我们采用足够高的正交规则。我们选择8阶的QGauss（4点），以确保数字正交引起的误差比由于边界近似的阶数，即所采用的映射的阶数（最大6）要高。请注意，积分，雅各布行列式，不是一个多项式函数（相反，它是一个有理函数），所以我们不使用高斯正交来获得积分的精确值，就像在有限元计算中经常做的那样，但也可以使用任何类似阶数的正交公式来代替。

    const QGauss<dim> quadrature(4);

// 现在开始在多项式映射度=1...4的基础上进行循环。

    for (unsigned int degree = 1; degree < 5; ++degree)
      {
        std::cout << "Degree = " << degree << std::endl;

// 首先生成三角形、边界和映射对象，正如已经看到的那样。

        Triangulation<dim> triangulation;
        GridGenerator::hyper_ball(triangulation);

        const MappingQ<dim> mapping(degree);

// 我们现在创建一个有限元。与其他的例子程序不同，我们实际上不需要用形状函数做任何计算；我们只需要FEValues对象的`JxW`值。因此，我们使用特殊的有限元类FE_Nothing，它的每个单元的自由度正好为零（顾名思义，每个单元的局部基础为空集）。FE_Nothing的一个比较典型的用法见  step-46  。

        const FE_Nothing<dim> fe;

// 同样地，我们需要创建一个DoFHandler对象。我们实际上并没有使用它，但是它将为我们提供`active_cell_iterators'，这是重新初始化三角形的每个单元上的FEValues对象所需要的。

        DoFHandler<dim> dof_handler(triangulation);

// 现在我们设置FEValues对象，向构造函数提供Mapping、假有限元和正交对象，以及要求只在正交点提供`JxW`值的更新标志。这告诉FEValues对象在调用 reinit 函数时不需要计算其他数量，从而节省计算时间。

// 与之前的例子程序相比，FEValues对象的构造最重要的区别是，我们传递了一个映射对象作为第一个参数，它将被用于计算从单元到实数单元的映射。在以前的例子中，这个参数被省略了，结果是隐含地使用了MappingQ1类型的对象。

        FEValues<dim> fe_values(mapping, fe, quadrature, update_JxW_values);

// 我们使用一个ConvergenceTable类的对象来存储所有重要的数据，如 $\pi$ 的近似值和与 $\pi$ 的真实值相比的误差。我们还将使用ConvergenceTable类提供的函数来计算 $\pi$ 的近似值的收敛率。

        ConvergenceTable table;

// 现在我们在三角形的几个细化步骤上循环。

        for (unsigned int refinement = 0; refinement < 6;
             ++refinement, triangulation.refine_global(1))
          {

// 在这个循环中，我们首先将当前三角形的活动单元的数量添加到表格中。这个函数会自动创建一个上标为 "cells "的表格列，以防这个列之前没有被创建。

            table.add_value("cells", triangulation.n_active_cells());

// 然后我们为虚拟有限元分配自由度。严格来说，在我们的特殊情况下，我们不需要这个函数的调用，但我们调用它是为了让DoFHandler高兴 -- 否则它将在下面的 FEValues::reinit 函数中抛出一个断言。

            dof_handler.distribute_dofs(fe);

// 我们将变量面积定义为 "长双"，就像我们之前为 "pi "变量所做的那样。

            long double area = 0;

// 现在我们循环所有的单元格，重新初始化每个单元格的FEValues对象，并将该单元格的所有`JxW`值加到`area`上......

            for (const auto &cell : dof_handler.active_cell_iterators())
              {
                fe_values.reinit(cell);
                for (unsigned int i = 0; i < fe_values.n_quadrature_points; ++i)
                  area += static_cast<long double>(fe_values.JxW(i));
              }

// ...并将得到的区域值和错误存储在表中。我们需要静态转换为双数，因为没有实现add_value(string, long double)函数。请注意，这也涉及到第二个调用，因为 std 命名空间中的 fabs 函数在其参数类型上是重载的，所以存在一个获取并返回 long double 的版本，而全局命名空间中只有一个这样的函数被声明（获取并返回一个双数）。

            table.add_value("eval.pi", static_cast<double>(area));
            table.add_value("error", static_cast<double>(std::fabs(area - pi)));
          }

// 我们想计算`error`列的收敛率。因此我们需要在调用`evaluate_all_convergence_rates`之前，将其他列从收敛率评估中省略。

        table.omit_column_from_convergence_rate_evaluation("cells");
        table.omit_column_from_convergence_rate_evaluation("eval.pi");
        table.evaluate_all_convergence_rates(
                                    ConvergenceTable::reduction_rate_log2);

// 最后我们设置一些量的输出精度和科学模式...

        table.set_precision("eval.pi", 16);
        table.set_scientific("error", true);

// ...并将整个表格写到  std::cout.  。
        table.write_text(std::cout);

        std::cout << std::endl;
      }
  }

// 下面的第二个函数也是计算 $\pi$ 的近似值，但这次是通过域的周长 $2\pi r$ 而不是面积。这个函数只是前一个函数的一个变体。因此，我们主要是给出不同之处的文件。

  template <int dim>
  void compute_pi_by_perimeter()
  {
    std::cout << "Computation of Pi by the perimeter:" << std::endl
              << "===================================" << std::endl;

// 我们采取同样的正交顺序，但这次是`dim-1`维正交，因为我们将在（边界）线上而不是在单元上积分。

    const QGauss<dim - 1> quadrature(4);

// 我们在所有度数上循环，创建三角形、边界、映射、假有限元和DoFHandler对象，如之前所见。

    for (unsigned int degree = 1; degree < 5; ++degree)
      {
        std::cout << "Degree = " << degree << std::endl;
        Triangulation<dim> triangulation;
        GridGenerator::hyper_ball(triangulation);

        const MappingQ<dim>   mapping(degree);
        const FE_Nothing<dim> fe;

        DoFHandler<dim> dof_handler(triangulation);

// 然后我们创建一个FEFaceValues对象，而不是像前一个函数中的FEValues对象。同样，我们传递一个映射作为第一个参数。

        FEFaceValues<dim> fe_face_values(mapping,
                                         fe,
                                         quadrature,
                                         update_JxW_values);
        ConvergenceTable  table;

        for (unsigned int refinement = 0; refinement < 6;
             ++refinement, triangulation.refine_global(1))
          {
            table.add_value("cells", triangulation.n_active_cells());

            dof_handler.distribute_dofs(fe);

// 现在我们在所有单元和每个单元的所有面上运行。只有边界面上的`JxW`值的贡献被添加到长双变量`周长`中。

            long double perimeter = 0;
            for (const auto &cell : dof_handler.active_cell_iterators())
              for (const auto &face : cell->face_iterators())
                if (face->at_boundary())
                  {

// 我们用单元格迭代器和面的编号重新启动FEFaceValues对象。

                    fe_face_values.reinit(cell, face);
                    for (unsigned int i = 0;
                         i < fe_face_values.n_quadrature_points;
                         ++i)
                      perimeter +=
                        static_cast<long double>(fe_face_values.JxW(i));
                  }

// 然后将评估后的数值存储在表中...

            table.add_value("eval.pi", static_cast<double>(perimeter / 2.0L));
            table.add_value(
              "error", static_cast<double>(std::fabs(perimeter / 2.0L - pi)));
          }

// ......然后像前一个函数那样结束这个函数。

        table.omit_column_from_convergence_rate_evaluation("cells");
        table.omit_column_from_convergence_rate_evaluation("eval.pi");
        table.evaluate_all_convergence_rates(
          ConvergenceTable::reduction_rate_log2);

        table.set_precision("eval.pi", 16);
        table.set_scientific("error", true);

        table.write_text(std::cout);

        std::cout << std::endl;
      }
  }
} // namespace Step10

// 下面的主函数只是按照上述函数的出现顺序来调用它们。除此以外，它看起来就像以前的教程程序的主函数一样。

int main()
{
  try
    {
      std::cout.precision(16);

      const unsigned int dim = 2;

      Step10::gnuplot_output<dim>();

      Step10::compute_pi_by_area<dim>();
      Step10::compute_pi_by_perimeter<dim>();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


// Copyright 2004 The Trustees of Indiana University.

// Use, modification and distribution is subject to the Boost Software
// License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#ifndef BOOST_GRAPH_KAMADA_KAWAI_SPRING_LAYOUT_HPP
#define BOOST_GRAPH_KAMADA_KAWAI_SPRING_LAYOUT_HPP

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/johnson_all_pairs_shortest.hpp>
#include <boost/type_traits/is_convertible.hpp>
#include <utility>
#include <iterator>
#include <vector>
#include <boost/limits.hpp>
#include <cmath>

namespace boost {
  namespace detail { namespace graph {
    /**
     * Denotes an edge or display area side length used to scale a
     * Kamada-Kawai drawing.
     */
    template<bool Edge, typename T>
    struct edge_or_side
    {
      explicit edge_or_side(T value) : value(value) {}

      T value;
    };

    /**
     * Compute the edge length from an edge length. This is trivial.
     */
    template<typename Graph, typename DistanceMap, typename IndexMap,
             typename T>
    T compute_edge_length(const Graph&, DistanceMap, IndexMap,
                          edge_or_side<true, T> length)
    { return length.value; }

    /**
     * Compute the edge length based on the display area side
       length. We do this by dividing the side length by the largest
       shortest distance between any two vertices in the graph.
     */
    template<typename Graph, typename DistanceMap, typename IndexMap,
             typename T>
    T
    compute_edge_length(const Graph& g, DistanceMap distance, IndexMap index,
                        edge_or_side<false, T> length)
    {
      T result(0);

      typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator;

      for (vertex_iterator ui = vertices(g).first, end = vertices(g).second;
           ui != end; ++ui) {
        vertex_iterator vi = ui;
        for (++vi; vi != end; ++vi) {
          T dij = distance[get(index, *ui)][get(index, *vi)];
          if (dij > result) result = dij;
        }
      }
      return length.value / result;
    }

    /**
     * Implementation of the Kamada-Kawai spring layout algorithm.
     */
    template<typename Graph, typename PositionMap, typename WeightMap,
             typename EdgeOrSideLength, typename Done,
             typename VertexIndexMap, typename DistanceMatrix,
             typename SpringStrengthMatrix, typename PartialDerivativeMap>
    struct kamada_kawai_spring_layout_impl
    {
      typedef typename property_traits<WeightMap>::value_type weight_type;
      typedef std::pair<weight_type, weight_type> deriv_type;
      typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator;
      typedef typename graph_traits<Graph>::vertex_descriptor
        vertex_descriptor;

      kamada_kawai_spring_layout_impl(
        const Graph& g,
        PositionMap position,
        WeightMap weight,
        EdgeOrSideLength edge_or_side_length,
        Done done,
        weight_type spring_constant,
        VertexIndexMap index,
        DistanceMatrix distance,
        SpringStrengthMatrix spring_strength,
        PartialDerivativeMap partial_derivatives)
        : g(g), position(position), weight(weight),
          edge_or_side_length(edge_or_side_length), done(done),
          spring_constant(spring_constant), index(index), distance(distance),
          spring_strength(spring_strength),
          partial_derivatives(partial_derivatives) {}

      // Compute contribution of vertex i to the first partial
      // derivatives (dE/dx_m, dE/dy_m) (for vertex m)
      deriv_type
      compute_partial_derivative(vertex_descriptor m, vertex_descriptor i)
      {
#ifndef BOOST_NO_STDC_NAMESPACE
        using std::sqrt;
#endif // BOOST_NO_STDC_NAMESPACE

        deriv_type result(0, 0);
        if (i != m) {
          weight_type x_diff = position[m].x - position[i].x;
          weight_type y_diff = position[m].y - position[i].y;
          weight_type dist = sqrt(x_diff * x_diff + y_diff * y_diff);
          result.first = spring_strength[get(index, m)][get(index, i)]
            * (x_diff - distance[get(index, m)][get(index, i)]*x_diff/dist);
          result.second = spring_strength[get(index, m)][get(index, i)]
            * (y_diff - distance[get(index, m)][get(index, i)]*y_diff/dist);
        }

        return result;
      }

      // Compute partial derivatives dE/dx_m and dE/dy_m
      deriv_type
      compute_partial_derivatives(vertex_descriptor m)
      {
#ifndef BOOST_NO_STDC_NAMESPACE
        using std::sqrt;
#endif // BOOST_NO_STDC_NAMESPACE

        deriv_type result(0, 0);

        // TBD: looks like an accumulate to me
        std::pair<vertex_iterator, vertex_iterator> verts = vertices(g);
        for (/* no init */; verts.first != verts.second; ++verts.first) {
          vertex_descriptor i = *verts.first;
          deriv_type deriv = compute_partial_derivative(m, i);
          result.first += deriv.first;
          result.second += deriv.second;
        }

        return result;
      }

      // The actual Kamada-Kawai spring layout algorithm implementation
      bool run()
      {
#ifndef BOOST_NO_STDC_NAMESPACE
        using std::sqrt;
#endif // BOOST_NO_STDC_NAMESPACE

        // Compute d_{ij} and place it in the distance matrix
        if (!johnson_all_pairs_shortest_paths(g, distance, index, weight,
                                              weight_type(0)))
          return false;

        // Compute L based on side length (if needed), or retrieve L
        weight_type edge_length =
          detail::graph::compute_edge_length(g, distance, index,
                                             edge_or_side_length);

        // Compute l_{ij} and k_{ij}
        const weight_type K = spring_constant;
        vertex_iterator ui, end = vertices(g).second;
        for (ui = vertices(g).first; ui != end; ++ui) {
          vertex_iterator vi = ui;
          for (++vi; vi != end; ++vi) {
            weight_type dij = distance[get(index, *ui)][get(index, *vi)];
            if (dij == (std::numeric_limits<weight_type>::max)())
              return false;
            distance[get(index, *ui)][get(index, *vi)] = edge_length * dij;
            distance[get(index, *vi)][get(index, *ui)] = edge_length * dij;
            spring_strength[get(index, *ui)][get(index, *vi)] = K/(dij*dij);
            spring_strength[get(index, *vi)][get(index, *ui)] = K/(dij*dij);
          }
        }

        // Compute Delta_i and find max
        vertex_descriptor p = *vertices(g).first;
        weight_type delta_p(0);

        for (ui = vertices(g).first; ui != end; ++ui) {
          deriv_type deriv = compute_partial_derivatives(*ui);
          put(partial_derivatives, *ui, deriv);

          weight_type delta =
            sqrt(deriv.first*deriv.first + deriv.second*deriv.second);

          if (delta > delta_p) {
            p = *ui;
            delta_p = delta;
          }
        }

        while (!done(delta_p, p, g, true)) {
          // The contribution p makes to the partial derivatives of
          // each vertex. Computing this (at O(n) cost) allows us to
          // update the delta_i values in O(n) time instead of O(n^2)
          // time.
          std::vector<deriv_type> p_partials(num_vertices(g));
          for (ui = vertices(g).first; ui != end; ++ui) {
            vertex_descriptor i = *ui;
            p_partials[get(index, i)] = compute_partial_derivative(i, p);
          }

          do {
            // Compute the 4 elements of the Jacobian
            weight_type dE_dx_dx = 0, dE_dx_dy = 0, dE_dy_dx = 0, dE_dy_dy = 0;
            for (ui = vertices(g).first; ui != end; ++ui) {
              vertex_descriptor i = *ui;
              if (i != p) {
                weight_type x_diff = position[p].x - position[i].x;
                weight_type y_diff = position[p].y - position[i].y;
                weight_type dist = sqrt(x_diff * x_diff + y_diff * y_diff);
                weight_type dist_cubed = dist * dist * dist;
                weight_type k_mi = spring_strength[get(index,p)][get(index,i)];
                weight_type l_mi = distance[get(index, p)][get(index, i)];
                dE_dx_dx += k_mi * (1 - (l_mi * y_diff * y_diff)/dist_cubed);
                dE_dx_dy += k_mi * l_mi * x_diff * y_diff / dist_cubed;
                dE_dy_dx += k_mi * l_mi * x_diff * y_diff / dist_cubed;
                dE_dy_dy += k_mi * (1 - (l_mi * x_diff * x_diff)/dist_cubed);
              }
            }

            // Solve for delta_x and delta_y
            weight_type dE_dx = get(partial_derivatives, p).first;
            weight_type dE_dy = get(partial_derivatives, p).second;

            weight_type delta_x =
              (dE_dx_dy * dE_dy - dE_dy_dy * dE_dx)
              / (dE_dx_dx * dE_dy_dy - dE_dx_dy * dE_dy_dx);

            weight_type delta_y =
              (dE_dx_dx * dE_dy - dE_dy_dx * dE_dx)
              / (dE_dy_dx * dE_dx_dy - dE_dx_dx * dE_dy_dy);

            // Move p by (delta_x, delta_y)
            position[p].x += delta_x;
            position[p].y += delta_y;

            // Recompute partial derivatives and delta_p
            deriv_type deriv = compute_partial_derivatives(p);
            put(partial_derivatives, p, deriv);

            delta_p =
              sqrt(deriv.first*deriv.first + deriv.second*deriv.second);
          } while (!done(delta_p, p, g, false));

          // Select new p by updating each partial derivative and delta
          vertex_descriptor old_p = p;
          for (ui = vertices(g).first; ui != end; ++ui) {
            deriv_type old_deriv_p = p_partials[get(index, *ui)];
            deriv_type old_p_partial =
              compute_partial_derivative(*ui, old_p);
            deriv_type deriv = get(partial_derivatives, *ui);

            deriv.first += old_p_partial.first - old_deriv_p.first;
            deriv.second += old_p_partial.second - old_deriv_p.second;

            put(partial_derivatives, *ui, deriv);
            weight_type delta =
              sqrt(deriv.first*deriv.first + deriv.second*deriv.second);

            if (delta > delta_p) {
              p = *ui;
              delta_p = delta;
            }
          }
        }

        return true;
      }

      const Graph& g;
      PositionMap position;
      WeightMap weight;
      EdgeOrSideLength edge_or_side_length;
      Done done;
      weight_type spring_constant;
      VertexIndexMap index;
      DistanceMatrix distance;
      SpringStrengthMatrix spring_strength;
      PartialDerivativeMap partial_derivatives;
    };
  } } // end namespace detail::graph

  /// States that the given quantity is an edge length.
  template<typename T>
  inline detail::graph::edge_or_side<true, T>
  edge_length(T x)
  { return detail::graph::edge_or_side<true, T>(x); }

  /// States that the given quantity is a display area side length.
  template<typename T>
  inline detail::graph::edge_or_side<false, T>
  side_length(T x)
  { return detail::graph::edge_or_side<false, T>(x); }

  /**
   * \brief Determines when to terminate layout of a particular graph based
   * on a given relative tolerance.
   */
  template<typename T = double>
  struct layout_tolerance
  {
    layout_tolerance(const T& tolerance = T(0.001))
      : tolerance(tolerance), last_energy((std::numeric_limits<T>::max)()),
        last_local_energy((std::numeric_limits<T>::max)()) { }

    template<typename Graph>
    bool
    operator()(T delta_p,
               typename boost::graph_traits<Graph>::vertex_descriptor p,
               const Graph& g,
               bool global)
    {
      if (global) {
        if (last_energy == (std::numeric_limits<T>::max)()) {
          last_energy = delta_p;
          return false;
        }

        T diff = last_energy - delta_p;
        if (diff < T(0)) diff = -diff;
        bool done = (delta_p == T(0) || diff / last_energy < tolerance);
        last_energy = delta_p;
        return done;
      } else {
        if (last_local_energy == (std::numeric_limits<T>::max)()) {
          last_local_energy = delta_p;
          return delta_p == T(0);
        }

        T diff = last_local_energy - delta_p;
        bool done = (delta_p == T(0) || (diff / last_local_energy) < tolerance);
        last_local_energy = delta_p;
        return done;
      }
    }

  private:
    T tolerance;
    T last_energy;
    T last_local_energy;
  };

  /** \brief Kamada-Kawai spring layout for undirected graphs.
   *
   * This algorithm performs graph layout (in two dimensions) for
   * connected, undirected graphs. It operates by relating the layout
   * of graphs to a dynamic spring system and minimizing the energy
   * within that system. The strength of a spring between two vertices
   * is inversely proportional to the square of the shortest distance
   * (in graph terms) between those two vertices. Essentially,
   * vertices that are closer in the graph-theoretic sense (i.e., by
   * following edges) will have stronger springs and will therefore be
   * placed closer together.
   *
   * Prior to invoking this algorithm, it is recommended that the
   * vertices be placed along the vertices of a regular n-sided
   * polygon.
   *
   * \param g (IN) must be a model of Vertex List Graph, Edge List
   * Graph, and Incidence Graph and must be undirected.
   *
   * \param position (OUT) must be a model of Lvalue Property Map,
   * where the value type is a class containing fields @c x and @c y
   * that will be set to the @c x and @c y coordinates of each vertex.
   *
   * \param weight (IN) must be a model of Readable Property Map,
   * which provides the weight of each edge in the graph @p g.
   *
   * \param edge_or_side_length (IN) provides either the unit length
   * @c e of an edge in the layout or the length of a side @c s of the
   * display area, and must be either @c boost::edge_length(e) or @c
   * boost::side_length(s), respectively.
   *
   * \param done (IN) is a 4-argument function object that is passed
   * the current value of delta_p (i.e., the energy of vertex @p p),
   * the vertex @p p, the graph @p g, and a boolean flag indicating
   * whether @p delta_p is the maximum energy in the system (when @c
   * true) or the energy of the vertex being moved. Defaults to @c
   * layout_tolerance instantiated over the value type of the weight
   * map.
   *
   * \param spring_constant (IN) is the constant multiplied by each
   * spring's strength. Larger values create systems with more energy
   * that can take longer to stabilize; smaller values create systems
   * with less energy that stabilize quickly but do not necessarily
   * result in pleasing layouts. The default value is 1.
   *
   * \param index (IN) is a mapping from vertices to index values
   * between 0 and @c num_vertices(g). The default is @c
   * get(vertex_index,g).
   *
   * \param distance (UTIL/OUT) will be used to store the distance
   * from every vertex to every other vertex, which is computed in the
   * first stages of the algorithm. This value's type must be a model
   * of BasicMatrix with value type equal to the value type of the
   * weight map. The default is a a vector of vectors.
   *
   * \param spring_strength (UTIL/OUT) will be used to store the
   * strength of the spring between every pair of vertices. This
   * value's type must be a model of BasicMatrix with value type equal
   * to the value type of the weight map. The default is a a vector of
   * vectors.
   *
   * \param partial_derivatives (UTIL) will be used to store the
   * partial derivates of each vertex with respect to the @c x and @c
   * y coordinates. This must be a Read/Write Property Map whose value
   * type is a pair with both types equivalent to the value type of
   * the weight map. The default is an iterator property map.
   *
   * \returns @c true if layout was successful or @c false if a
   * negative weight cycle was detected.
   */
  template<typename Graph, typename PositionMap, typename WeightMap,
           typename T, bool EdgeOrSideLength, typename Done,
           typename VertexIndexMap, typename DistanceMatrix,
           typename SpringStrengthMatrix, typename PartialDerivativeMap>
  bool
  kamada_kawai_spring_layout(
    const Graph& g,
    PositionMap position,
    WeightMap weight,
    detail::graph::edge_or_side<EdgeOrSideLength, T> edge_or_side_length,
    Done done,
    typename property_traits<WeightMap>::value_type spring_constant,
    VertexIndexMap index,
    DistanceMatrix distance,
    SpringStrengthMatrix spring_strength,
    PartialDerivativeMap partial_derivatives)
  {
    BOOST_STATIC_ASSERT((is_convertible<
                           typename graph_traits<Graph>::directed_category*,
                           undirected_tag*
                         >::value));

    detail::graph::kamada_kawai_spring_layout_impl<
      Graph, PositionMap, WeightMap,
      detail::graph::edge_or_side<EdgeOrSideLength, T>, Done, VertexIndexMap,
      DistanceMatrix, SpringStrengthMatrix, PartialDerivativeMap>
      alg(g, position, weight, edge_or_side_length, done, spring_constant,
          index, distance, spring_strength, partial_derivatives);
    return alg.run();
  }

  /**
   * \overload
   */
  template<typename Graph, typename PositionMap, typename WeightMap,
           typename T, bool EdgeOrSideLength, typename Done,
           typename VertexIndexMap>
  bool
  kamada_kawai_spring_layout(
    const Graph& g,
    PositionMap position,
    WeightMap weight,
    detail::graph::edge_or_side<EdgeOrSideLength, T> edge_or_side_length,
    Done done,
    typename property_traits<WeightMap>::value_type spring_constant,
    VertexIndexMap index)
  {
    typedef typename property_traits<WeightMap>::value_type weight_type;

    typename graph_traits<Graph>::vertices_size_type n = num_vertices(g);
    typedef std::vector<weight_type> weight_vec;

    std::vector<weight_vec> distance(n, weight_vec(n));
    std::vector<weight_vec> spring_strength(n, weight_vec(n));
    std::vector<std::pair<weight_type, weight_type> > partial_derivatives(n);

    return
      kamada_kawai_spring_layout(
        g, position, weight, edge_or_side_length, done, spring_constant, index,
        distance.begin(),
        spring_strength.begin(),
        make_iterator_property_map(partial_derivatives.begin(), index,
                                   std::pair<weight_type, weight_type>()));
  }

  /**
   * \overload
   */
  template<typename Graph, typename PositionMap, typename WeightMap,
           typename T, bool EdgeOrSideLength, typename Done>
  bool
  kamada_kawai_spring_layout(
    const Graph& g,
    PositionMap position,
    WeightMap weight,
    detail::graph::edge_or_side<EdgeOrSideLength, T> edge_or_side_length,
    Done done,
    typename property_traits<WeightMap>::value_type spring_constant)
  {
    return kamada_kawai_spring_layout(g, position, weight, edge_or_side_length,
                                      done, spring_constant,
                                      get(vertex_index, g));
  }

  /**
   * \overload
   */
  template<typename Graph, typename PositionMap, typename WeightMap,
           typename T, bool EdgeOrSideLength, typename Done>
  bool
  kamada_kawai_spring_layout(
    const Graph& g,
    PositionMap position,
    WeightMap weight,
    detail::graph::edge_or_side<EdgeOrSideLength, T> edge_or_side_length,
    Done done)
  {
    typedef typename property_traits<WeightMap>::value_type weight_type;
    return kamada_kawai_spring_layout(g, position, weight, edge_or_side_length,
                                      done, weight_type(1));
  }

  /**
   * \overload
   */
  template<typename Graph, typename PositionMap, typename WeightMap,
           typename T, bool EdgeOrSideLength>
  bool
  kamada_kawai_spring_layout(
    const Graph& g,
    PositionMap position,
    WeightMap weight,
    detail::graph::edge_or_side<EdgeOrSideLength, T> edge_or_side_length)
  {
    typedef typename property_traits<WeightMap>::value_type weight_type;
    return kamada_kawai_spring_layout(g, position, weight, edge_or_side_length,
                                      layout_tolerance<weight_type>(),
                                      weight_type(1.0),
                                      get(vertex_index, g));
  }
} // end namespace boost

#endif // BOOST_GRAPH_KAMADA_KAWAI_SPRING_LAYOUT_HPP


/**
 * \file boost/numeric/ublasx/operation/log10.hpp
 *
 * \brief Apply the \c std::log10 function to each element of a vector or
 *  matrix expression.
 *
 * Copyright (c) 2011, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_LOG10_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_LOG10_HPP

#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/expression/matrix_unary_functor.hpp>
#include <boost/numeric/ublasx/expression/vector_unary_functor.hpp>
#include <cmath>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

namespace detail {

template <typename VectorExprT>
struct vector_log10_functor_traits
{
	typedef VectorExprT input_expression_type;
	typedef typename vector_traits<input_expression_type>::value_type signature_argument_type;
	typedef signature_argument_type signature_result_type;
	typedef vector_unary_functor_traits<
				input_expression_type,
				signature_result_type (signature_argument_type)
			> unary_functor_expression_type;
	typedef typename unary_functor_expression_type::result_type result_type;
	typedef typename unary_functor_expression_type::expression_type expression_type;
};

template <typename MatrixExprT>
struct matrix_log10_functor_traits
{
	typedef MatrixExprT input_expression_type;
	typedef typename matrix_traits<input_expression_type>::value_type signature_argument_type;
	typedef signature_argument_type signature_result_type;
	typedef matrix_unary_functor_traits<
				input_expression_type,
				signature_result_type (signature_argument_type)
			> unary_functor_expression_type;
	typedef typename unary_functor_expression_type::result_type result_type;
	typedef typename unary_functor_expression_type::expression_type expression_type;
};

namespace /*<unnamed>*/ {

/// Auxiliary function used to replace ::std::log2 when that is not available.
template <typename T>
BOOST_UBLAS_INLINE
T log10(T x)
{
	return ::std::log10(x);
}

} // Namespace <unnamed>

} // Namespace detail

/**
 * \brief Applies the \c std::log10 function to each element of a given vector
 *  expression.
 *
 * \tparam VectorExprT The type of the input vector expression.
 *
 * \param ve The input vector expression.
 * \return A vector expression representing the application of \c std::log10 to
 *  each element of \a ve.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename VectorExprT>
BOOST_UBLAS_INLINE
typename detail::vector_log10_functor_traits<VectorExprT>::result_type log10(vector_expression<VectorExprT> const& ve)
{
	typedef typename detail::vector_log10_functor_traits<VectorExprT>::expression_type expression_type;
	typedef typename detail::vector_log10_functor_traits<VectorExprT>::signature_result_type signature_result_type;

	return expression_type(ve(), detail::log10<signature_result_type>);
}

/**
 * \brief Applies the \c std::log10 function to each element of a given matrix
 *  expression.
 *
 * \tparam MatrixExprT The type of the input matrix expression.
 *
 * \param me The input matrix expression.
 * \return A matrix expression representing the application of \c std::log10 to
 *  each element of \a me.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename MatrixExprT>
BOOST_UBLAS_INLINE
typename detail::matrix_log10_functor_traits<MatrixExprT>::result_type log10(matrix_expression<MatrixExprT> const& me)
{
	typedef typename detail::matrix_log10_functor_traits<MatrixExprT>::expression_type expression_type;
	typedef typename detail::matrix_log10_functor_traits<MatrixExprT>::signature_result_type signature_result_type;

	return expression_type(me(), detail::log10<signature_result_type>);
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_LOG10_HPP