// std c
#include <stdio.h>
#include <math.h>
#include <iostream>
#include <fstream>
#include <iostream>
#include <string>
#include <sstream>
#include <ctime>

// opencv pcl
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include "opencv2/imgproc/imgproc.hpp"

// ros
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/PointCloud2.h>

// Eigen
#include <Eigen/Dense>
#include <Eigen/Core>

// ours
#include "detect_3d_cuboid/matrix_utils.h"
#include "detect_3d_cuboid/object_3d_util.h"
#include "tictoc_profiler/profiler.hpp"

using namespace std;
// using namespace cv;
using namespace Eigen;

void detect_3d_cuboid::set_calibration(const Matrix3d &Kalib)
{
	cam_pose.Kalib = Kalib;
	cam_pose.invK = Kalib.inverse();
}

void detect_3d_cuboid::set_cam_pose(const Matrix4d &transToWolrd)
{
	cam_pose.transToWolrd = transToWolrd;
	cam_pose.rotationToWorld = transToWolrd.topLeftCorner<3, 3>();
	Vector3d euler_angles;
	quat_to_euler_zyx(Quaterniond(cam_pose.rotationToWorld), euler_angles(0), euler_angles(1), euler_angles(2));
	cam_pose.euler_angle = euler_angles;
	cam_pose.invR = cam_pose.rotationToWorld.inverse();
	cam_pose.projectionMatrix = cam_pose.Kalib * transToWolrd.inverse().topRows<3>(); // project world coordinate to camera
	cam_pose.KinvR = cam_pose.Kalib * cam_pose.invR;
	cam_pose.camera_yaw = cam_pose.euler_angle(2);
	//TODO relative measure? not good... then need to change transToWolrd.
}

void detect_3d_cuboid::detect_cuboid(const cv::Mat &rgb_img, const Matrix4d &transToWolrd, const MatrixXd &obj_bbox_coors,
									 MatrixXd all_lines_raw, std::vector<ObjectSet> &all_object_cuboids)
{
	set_cam_pose(transToWolrd);
	cam_pose_raw = cam_pose;

	cv::Mat gray_img;
	if (rgb_img.channels() == 3)
		cv::cvtColor(rgb_img, gray_img, cv::COLOR_BGR2GRAY);
	else
		gray_img = rgb_img;

	int img_width = rgb_img.cols;
	int img_height = rgb_img.rows;

	int num_2d_objs = obj_bbox_coors.rows();
	all_object_cuboids.resize(num_2d_objs);

	vector<bool> all_configs;
	all_configs.push_back(consider_config_1);
	all_configs.push_back(consider_config_2);

	// parameters for cuboid generation
	double vp12_edge_angle_thre = 15;
	double vp3_edge_angle_thre = 10;	// 10  10  parameters
	double shorted_edge_thre = 20;		// if box edge are too short. box might be too thin. most possibly wrong.
	bool reweight_edge_distance = true; // if want to compare with all configurations. we need to reweight

	// parameters for proposal scoring
	bool whether_normalize_two_errors = true;
	double weight_vp_angle = 0.8;
	double weight_skew_error = 1.5;
	// if also consider config2, need to weight two erros, in order to compare two configurations

	align_left_right_edges(all_lines_raw); // this should be guaranteed when detecting edges
	if (whether_plot_detail_images)
	{
		cv::Mat output_img;
		plot_image_with_edges(rgb_img, output_img, all_lines_raw, cv::Scalar(255, 0, 0));
		cv::imshow("Raw detected Edges", output_img); //cv::waitKey(0);
	}

	// find ground-wall boundary edges
	Vector4d ground_plane_world(0, 0, 1, 0); // treated as column vector % in my pop-up code, I use [0 0 -1 0]. here I want the normal pointing innerwards, towards the camera to match surface normal prediction
	Vector4d ground_plane_sensor = cam_pose.transToWolrd.transpose() * ground_plane_world;

	//       int object_id=1;
	for (int object_id = 0; object_id < num_2d_objs; object_id++)
	{
		// 	  std::cout<<"object id  "<<object_id<<std::endl;
		ca::Profiler::tictoc("One 3D object total time");
		int left_x_raw = obj_bbox_coors(object_id, 0);
		int top_y_raw = obj_bbox_coors(object_id, 1);
		int obj_width_raw = obj_bbox_coors(object_id, 2);
		int obj_height_raw = obj_bbox_coors(object_id, 3);
		int right_x_raw = left_x_raw + obj_bbox_coors(object_id, 2);
		int down_y_raw = top_y_raw + obj_height_raw;

		std::vector<int> down_expand_sample_all;
		down_expand_sample_all.push_back(0);
		if (whether_sample_bbox_height) // 2D object detection might not be accurate
		{
			int down_expand_sample_ranges = max(min(20, obj_height_raw - 90), 20);
			down_expand_sample_ranges = min(down_expand_sample_ranges, img_height - top_y_raw - obj_height_raw - 1); // should lie inside the image  -1 for c++ index
			if (down_expand_sample_ranges > 10)																		 // if expand large margin, give more samples.
				down_expand_sample_all.push_back(round(down_expand_sample_ranges / 2));
			down_expand_sample_all.push_back(down_expand_sample_ranges);
		}

		// NOTE later if in video, could use previous object yaw..., also reduce search range
		double yaw_init = cam_pose.camera_yaw - 90.0 / 180.0 * M_PI; // yaw init is directly facing the camera, align with camera optical axis
		std::vector<double> obj_yaw_samples;
		linespace<double>(yaw_init - 45.0 / 180.0 * M_PI, yaw_init + 45.0 / 180.0 * M_PI, 6.0 / 180.0 * M_PI, obj_yaw_samples);

		MatrixXd all_configs_errors(400, 9);
		MatrixXd all_box_corners_2ds(800, 8);   // initialize a large eigen matrix
		int valid_config_number_all_height = 0; // all valid objects of all height samples
		ObjectSet raw_obj_proposals;
		raw_obj_proposals.reserve(100);
		// 	    int sample_down_expan_id=1;
		for (int sample_down_expan_id = 0; sample_down_expan_id < down_expand_sample_all.size(); sample_down_expan_id++)
		{
			int down_expand_sample = down_expand_sample_all[sample_down_expan_id];
			int obj_height_expan = obj_height_raw + down_expand_sample;
			int down_y_expan = top_y_raw + obj_height_expan;
			double obj_diaglength_expan = sqrt(obj_width_raw * obj_width_raw + obj_height_expan * obj_height_expan);

			// sample points on the top edges, if edge is too large, give more samples. give at least 10 samples for all edges. for small object, object pose changes lots
			int top_sample_resolution = round(min(20, obj_width_raw / 10)); //  25 pixels
			std::vector<int> top_x_samples;
			linespace<int>(left_x_raw + 5, right_x_raw - 5, top_sample_resolution, top_x_samples);
			MatrixXd sample_top_pts(2, top_x_samples.size());
			for (int ii = 0; ii < top_x_samples.size(); ii++)
			{
				sample_top_pts(0, ii) = top_x_samples[ii];
				sample_top_pts(1, ii) = top_y_raw;
			}

			// expand some small margin for distance map  [10 20]
			int distmap_expand_wid = min(max(min(20, obj_width_raw - 100), 10), max(min(20, obj_height_expan - 100), 10));
			int left_x_expan_distmap = max(0, left_x_raw - distmap_expand_wid);
			int right_x_expan_distmap = min(img_width - 1, right_x_raw + distmap_expand_wid);
			int top_y_expan_distmap = max(0, top_y_raw - distmap_expand_wid);
			int down_y_expan_distmap = min(img_height - 1, down_y_expan + distmap_expand_wid);
			int height_expan_distmap = down_y_expan_distmap - top_y_expan_distmap;
			int width_expan_distmap = right_x_expan_distmap - left_x_expan_distmap;
			Vector2d expan_distmap_lefttop = Vector2d(left_x_expan_distmap, top_y_expan_distmap);
			Vector2d expan_distmap_rightbottom = Vector2d(right_x_expan_distmap, down_y_expan_distmap);

			// find edges inside the object bounding box
			MatrixXd all_lines_inside_object(all_lines_raw.rows(), all_lines_raw.cols()); // first allocate a large matrix, then only use the toprows to avoid copy, alloc
			int inside_obj_edge_num = 0;
			for (int edge_id = 0; edge_id < all_lines_raw.rows(); edge_id++)
				if (check_inside_box(all_lines_raw.row(edge_id).head<2>(), expan_distmap_lefttop, expan_distmap_rightbottom))
					if (check_inside_box(all_lines_raw.row(edge_id).tail<2>(), expan_distmap_lefttop, expan_distmap_rightbottom))
					{
						all_lines_inside_object.row(inside_obj_edge_num) = all_lines_raw.row(edge_id);
						inside_obj_edge_num++;
					}

			// merge edges and remove short lines, after finding object edges.  edge merge in small regions should be faster than all.
			double pre_merge_dist_thre = 20;
			double pre_merge_angle_thre = 5;
			double edge_length_threshold = 30;
			MatrixXd all_lines_merge_inobj;
			merge_break_lines(all_lines_inside_object.topRows(inside_obj_edge_num), all_lines_merge_inobj, pre_merge_dist_thre,
							  pre_merge_angle_thre, edge_length_threshold);

			// compute edge angels and middle points
			VectorXd lines_inobj_angles(all_lines_merge_inobj.rows());
			MatrixXd edge_mid_pts(all_lines_merge_inobj.rows(), 2);
			for (int i = 0; i < all_lines_merge_inobj.rows(); i++)
			{
				lines_inobj_angles(i) = std::atan2(all_lines_merge_inobj(i, 3) - all_lines_merge_inobj(i, 1), all_lines_merge_inobj(i, 2) - all_lines_merge_inobj(i, 0)); // [-pi/2 -pi/2]
				edge_mid_pts.row(i).head<2>() = (all_lines_merge_inobj.row(i).head<2>() + all_lines_merge_inobj.row(i).tail<2>()) / 2;
			}

			// TODO could canny or distance map outside sampling height to speed up!!!!   Then only need to compute canny onces.
			// detect canny edges and compute distance transform  NOTE opencv canny maybe different from matlab. but roughly same
			cv::Rect object_bbox = cv::Rect(left_x_expan_distmap, top_y_expan_distmap, width_expan_distmap, height_expan_distmap); //
			cv::Mat im_canny;
			cv::Canny(gray_img(object_bbox), im_canny, 80, 200); // low thre, high thre    im_canny 0 or 255   [80 200  40 100]
			cv::Mat dist_map;
			cv::distanceTransform(255 - im_canny, dist_map, cv::DIST_L2, 3); // dist_map is float datatype

			if (whether_plot_detail_images)
			{
				cv::imshow("im_canny", im_canny);
				cv::Mat dist_map_img;
				cv::normalize(dist_map, dist_map_img, 0.0, 1.0, cv::NORM_MINMAX);
				cv::imshow("normalized distance map", dist_map_img);
				cv::waitKey();
			}

			// Generate cuboids
			MatrixXd all_configs_error_one_objH(200, 9);
			MatrixXd all_box_corners_2d_one_objH(400, 8);
			int valid_config_number_one_objH = 0;

			std::vector<double> cam_roll_samples;
			std::vector<double> cam_pitch_samples;
			if (whether_sample_cam_roll_pitch)
			{
				linespace<double>(cam_pose_raw.euler_angle(0) - 6.0 / 180.0 * M_PI, cam_pose_raw.euler_angle(0) + 6.0 / 180.0 * M_PI, 3.0 / 180.0 * M_PI, cam_roll_samples);
				linespace<double>(cam_pose_raw.euler_angle(1) - 6.0 / 180.0 * M_PI, cam_pose_raw.euler_angle(1) + 6.0 / 180.0 * M_PI, 3.0 / 180.0 * M_PI, cam_pitch_samples);
			}
			else
			{
				cam_roll_samples.push_back(cam_pose_raw.euler_angle(0));
				cam_pitch_samples.push_back(cam_pose_raw.euler_angle(1));
			}
			// different from matlab. first for loop yaw, then for configurations.
			// 	      int obj_yaw_id=8;
			for (int cam_roll_id = 0; cam_roll_id < cam_roll_samples.size(); cam_roll_id++)
				for (int cam_pitch_id = 0; cam_pitch_id < cam_pitch_samples.size(); cam_pitch_id++)
					for (int obj_yaw_id = 0; obj_yaw_id < obj_yaw_samples.size(); obj_yaw_id++)
					{
						if (whether_sample_cam_roll_pitch)
						{
							Matrix4d transToWolrd_new = transToWolrd;
							transToWolrd_new.topLeftCorner<3, 3>() = euler_zyx_to_rot<double>(cam_roll_samples[cam_roll_id], cam_pitch_samples[cam_pitch_id], cam_pose_raw.euler_angle(2));
							set_cam_pose(transToWolrd_new);
							ground_plane_sensor = cam_pose.transToWolrd.transpose() * ground_plane_world;
						}

						double obj_yaw_esti = obj_yaw_samples[obj_yaw_id];

						Vector2d vp_1, vp_2, vp_3;
						getVanishingPoints(cam_pose.KinvR, obj_yaw_esti, vp_1, vp_2, vp_3); // for object x y z  axis

						MatrixXd all_vps(3, 2);
						all_vps.row(0) = vp_1;
						all_vps.row(1) = vp_2;
						all_vps.row(2) = vp_3;
						// 		  std::cout<<"obj_yaw_esti  "<<obj_yaw_esti<<"  "<<obj_yaw_id<<std::endl;
						MatrixXd all_vp_bound_edge_angles = VP_support_edge_infos(all_vps, edge_mid_pts, lines_inobj_angles,
																				  Vector2d(vp12_edge_angle_thre, vp3_edge_angle_thre));
						// 		  int sample_top_pt_id=15;
						for (int sample_top_pt_id = 0; sample_top_pt_id < sample_top_pts.cols(); sample_top_pt_id++)
						{
							// 		      std::cout<<"sample_top_pt_id "<<sample_top_pt_id<<std::endl;
							Vector2d corner_1_top = sample_top_pts.col(sample_top_pt_id);
							bool config_good = true;
							int vp_1_position = 0; // 0 initial as fail,  1  on left   2 on right
							Vector2d corner_2_top = seg_hit_boundary(vp_1, corner_1_top, Vector4d(right_x_raw, top_y_raw, right_x_raw, down_y_expan));
							if (corner_2_top(0) == -1)
							{ // vp1-corner1 doesn't hit the right boundary. check whether hit left
								corner_2_top = seg_hit_boundary(vp_1, corner_1_top, Vector4d(left_x_raw, top_y_raw, left_x_raw, down_y_expan));
								if (corner_2_top(0) != -1) // vp1-corner1 hit the left boundary   vp1 on the right
									vp_1_position = 2;
							}
							else // vp1-corner1 hit the right boundary   vp1 on the left
								vp_1_position = 1;

							config_good = vp_1_position > 0;
							if (!config_good)
							{
								if (print_details)
									printf("Configuration fails at corner 2, outside segment\n");
								continue;
							}
							if ((corner_1_top - corner_2_top).norm() < shorted_edge_thre)
							{
								if (print_details)
									printf("Configuration fails at edge 1-2, too short\n");
								continue;
							}
							// 		      cout<<"corner_1/2   "<<corner_1_top.transpose()<<"   "<<corner_2_top.transpose()<<endl;
							// 		      int config_ind=0; // have to consider config now.
							for (int config_id = 1; config_id < 3; config_id++) // configuration one or two of matlab version
							{
								if (!all_configs[config_id - 1])
									continue;
								Vector2d corner_3_top, corner_4_top;
								if (config_id == 1)
								{
									if (vp_1_position == 1) // then vp2 hit the left boundary
										corner_4_top = seg_hit_boundary(vp_2, corner_1_top, Vector4d(left_x_raw, top_y_raw, left_x_raw, down_y_expan));
									else // or, then vp2 hit the right boundary
										corner_4_top = seg_hit_boundary(vp_2, corner_1_top, Vector4d(right_x_raw, top_y_raw, right_x_raw, down_y_expan));
									if (corner_4_top(1) == -1)
									{
										config_good = false;
										if (print_details)
											printf("Configuration %d fails at corner 4, outside segment\n", config_id);
										continue;
									}
									if ((corner_1_top - corner_4_top).norm() < shorted_edge_thre)
									{
										if (print_details)
											printf("Configuration %d fails at edge 1-4, too short\n", config_id);
										continue;
									}
									// compute the last point in the top face
									corner_3_top = lineSegmentIntersect(vp_2, corner_2_top, vp_1, corner_4_top, true);
									if (!check_inside_box(corner_3_top, Vector2d(left_x_raw, top_y_raw), Vector2d(right_x_raw, down_y_expan)))
									{ // check inside boundary. otherwise edge visibility might be wrong
										config_good = false;
										if (print_details)
											printf("Configuration %d fails at corner 3, outside box\n", config_id);
										continue;
									}
									if (((corner_3_top - corner_4_top).norm() < shorted_edge_thre) || ((corner_3_top - corner_2_top).norm() < shorted_edge_thre))
									{
										if (print_details)
											printf("Configuration %d fails at edge 3-4/3-2, too short\n", config_id);
										continue;
									}
									// 			      cout<<"corner_3/4   "<<corner_3_top.transpose()<<"   "<<corner_4_top.transpose()<<endl;
								}
								if (config_id == 2)
								{
									if (vp_1_position == 1) // then vp2 hit the left boundary
										corner_3_top = seg_hit_boundary(vp_2, corner_2_top, Vector4d(left_x_raw, top_y_raw, left_x_raw, down_y_expan));
									else // or, then vp2 hit the right boundary
										corner_3_top = seg_hit_boundary(vp_2, corner_2_top, Vector4d(right_x_raw, top_y_raw, right_x_raw, down_y_expan));
									if (corner_3_top(1) == -1)
									{
										config_good = false;
										if (print_details)
											printf("Configuration %d fails at corner 3, outside segment\n", config_id);
										continue;
									}
									if ((corner_2_top - corner_3_top).norm() < shorted_edge_thre)
									{
										if (print_details)
											printf("Configuration %d fails at edge 2-3, too short\n", config_id);
										continue;
									}
									// compute the last point in the top face
									corner_4_top = lineSegmentIntersect(vp_1, corner_3_top, vp_2, corner_1_top, true);
									if (!check_inside_box(corner_4_top, Vector2d(left_x_raw, top_y_expan_distmap), Vector2d(right_x_raw, down_y_expan_distmap)))
									{
										config_good = false;
										if (print_details)
											printf("Configuration %d fails at corner 4, outside box\n", config_id);
										continue;
									}
									if (((corner_3_top - corner_4_top).norm() < shorted_edge_thre) || ((corner_4_top - corner_1_top).norm() < shorted_edge_thre))
									{
										if (print_details)
											printf("Configuration %d fails at edge 3-4/4-1, too short\n", config_id);
										continue;
									}
									// 			      cout<<"corner_3/4   "<<corner_3_top.transpose()<<"   "<<corner_4_top.transpose()<<endl;
								}
								// compute first bottom points    computing bottom points is the same for config 1,2
								Vector2d corner_5_down = seg_hit_boundary(vp_3, corner_3_top, Vector4d(left_x_raw, down_y_expan, right_x_raw, down_y_expan));
								if (corner_5_down(1) == -1)
								{
									config_good = false;
									if (print_details)
										printf("Configuration %d fails at corner 5, outside segment\n", config_id);
									continue;
								}
								if ((corner_3_top - corner_5_down).norm() < shorted_edge_thre)
								{
									if (print_details)
										printf("Configuration %d fails at edge 3-5, too short\n", config_id);
									continue;
								}
								Vector2d corner_6_down = lineSegmentIntersect(vp_2, corner_5_down, vp_3, corner_2_top, true);
								if (!check_inside_box(corner_6_down, expan_distmap_lefttop, expan_distmap_rightbottom))
								{
									config_good = false;
									if (print_details)
										printf("Configuration %d fails at corner 6, outside box\n", config_id);
									continue;
								}
								if (((corner_6_down - corner_2_top).norm() < shorted_edge_thre) || ((corner_6_down - corner_5_down).norm() < shorted_edge_thre))
								{
									if (print_details)
										printf("Configuration %d fails at edge 6-5/6-2, too short\n", config_id);
									continue;
								}
								Vector2d corner_7_down = lineSegmentIntersect(vp_1, corner_6_down, vp_3, corner_1_top, true);
								if (!check_inside_box(corner_7_down, expan_distmap_lefttop, expan_distmap_rightbottom))
								{ // might be slightly different from matlab
									config_good = false;
									if (print_details)
										printf("Configuration %d fails at corner 7, outside box\n", config_id);
									continue;
								}
								if (((corner_7_down - corner_1_top).norm() < shorted_edge_thre) || ((corner_7_down - corner_6_down).norm() < shorted_edge_thre))
								{
									if (print_details)
										printf("Configuration %d fails at edge 7-1/7-6, too short\n", config_id);
									continue;
								}
								Vector2d corner_8_down = lineSegmentIntersect(vp_1, corner_5_down, vp_2, corner_7_down, true);
								if (!check_inside_box(corner_8_down, expan_distmap_lefttop, expan_distmap_rightbottom))
								{
									config_good = false;
									if (print_details)
										printf("Configuration %d fails at corner 8, outside box\n", config_id);
									continue;
								}
								if (((corner_8_down - corner_4_top).norm() < shorted_edge_thre) || ((corner_8_down - corner_5_down).norm() < shorted_edge_thre) || ((corner_8_down - corner_7_down).norm() < shorted_edge_thre))
								{
									if (print_details)
										printf("Configuration %d fails at edge 8-4/8-5/8-7, too short\n", config_id);
									continue;
								}

								MatrixXd box_corners_2d_float(2, 8);
								box_corners_2d_float << corner_1_top, corner_2_top, corner_3_top, corner_4_top, corner_5_down, corner_6_down, corner_7_down, corner_8_down;
								// 			  std::cout<<"box_corners_2d_float \n "<<box_corners_2d_float<<std::endl;
								MatrixXd box_corners_2d_float_shift(2, 8);
								box_corners_2d_float_shift.row(0) = box_corners_2d_float.row(0).array() - left_x_expan_distmap;
								box_corners_2d_float_shift.row(1) = box_corners_2d_float.row(1).array() - top_y_expan_distmap;

								MatrixXi visible_edge_pt_ids, vps_box_edge_pt_ids;
								double sum_dist;
								if (config_id == 1)
								{
									visible_edge_pt_ids.resize(9, 2);
									visible_edge_pt_ids << 1, 2, 2, 3, 3, 4, 4, 1, 2, 6, 3, 5, 4, 8, 5, 8, 5, 6;
									vps_box_edge_pt_ids.resize(3, 4);
									vps_box_edge_pt_ids << 1, 2, 8, 5, 4, 1, 5, 6, 4, 8, 2, 6; // six edges. each row represents two edges [e1_1 e1_2   e2_1 e2_2;...] of one VP
									visible_edge_pt_ids.array() -= 1;
									vps_box_edge_pt_ids.array() -= 1; //change to c++ index
									sum_dist = box_edge_sum_dists(dist_map, box_corners_2d_float_shift, visible_edge_pt_ids);
								}
								else
								{
									visible_edge_pt_ids.resize(7, 2);
									visible_edge_pt_ids << 1, 2, 2, 3, 3, 4, 4, 1, 2, 6, 3, 5, 5, 6;
									vps_box_edge_pt_ids.resize(3, 4);
									vps_box_edge_pt_ids << 1, 2, 3, 4, 4, 1, 5, 6, 3, 5, 2, 6; // six edges. each row represents two edges [e1_1 e1_2   e2_1 e2_2;...] of one VP
									visible_edge_pt_ids.array() -= 1;
									vps_box_edge_pt_ids.array() -= 1;
									sum_dist = box_edge_sum_dists(dist_map, box_corners_2d_float_shift, visible_edge_pt_ids, reweight_edge_distance);
								}
								double total_angle_diff = box_edge_alignment_angle_error(all_vp_bound_edge_angles, vps_box_edge_pt_ids, box_corners_2d_float);
								all_configs_error_one_objH.row(valid_config_number_one_objH).head<4>() = Vector4d(config_id, vp_1_position, obj_yaw_esti, sample_top_pt_id);
								all_configs_error_one_objH.row(valid_config_number_one_objH).segment<3>(4) = Vector3d(sum_dist / obj_diaglength_expan, total_angle_diff, down_expand_sample);
								if (whether_sample_cam_roll_pitch)
									all_configs_error_one_objH.row(valid_config_number_one_objH).segment<2>(7) = Vector2d(cam_roll_samples[cam_roll_id], cam_pitch_samples[cam_pitch_id]);
								else
									all_configs_error_one_objH.row(valid_config_number_one_objH).segment<2>(7) = Vector2d(cam_pose_raw.euler_angle(0), cam_pose_raw.euler_angle(1));
								all_box_corners_2d_one_objH.block(2 * valid_config_number_one_objH, 0, 2, 8) = box_corners_2d_float;
								valid_config_number_one_objH++;
								if (valid_config_number_one_objH >= all_configs_error_one_objH.rows())
								{
									all_configs_error_one_objH.conservativeResize(2 * valid_config_number_one_objH, NoChange);
									all_box_corners_2d_one_objH.conservativeResize(4 * valid_config_number_one_objH, NoChange);
								}
							} //end of config loop
						}	 //end of top id
					}		  //end of yaw

			// 	      std::cout<<"valid_config_number_one_hseight  "<<valid_config_number_one_objH<<std::endl;
			// 	      std::cout<<"all_configs_error_one_objH  \n"<<all_configs_error_one_objH.topRows(valid_config_number_one_objH)<<std::endl;
			// 	      MatrixXd all_corners = all_box_corners_2d_one_objH.topRows(2*valid_config_number_one_objH);
			// 	      std::cout<<"all corners   "<<all_corners<<std::endl;

			VectorXd normalized_score;
			vector<int> good_proposal_ids;
			fuse_normalize_scores_v2(all_configs_error_one_objH.col(4).head(valid_config_number_one_objH), all_configs_error_one_objH.col(5).head(valid_config_number_one_objH),
									 normalized_score, good_proposal_ids, weight_vp_angle, whether_normalize_two_errors);

			for (int box_id = 0; box_id < good_proposal_ids.size(); box_id++)
			{
				int raw_cube_ind = good_proposal_ids[box_id];

				if (whether_sample_cam_roll_pitch)
				{
					Matrix4d transToWolrd_new = transToWolrd;
					transToWolrd_new.topLeftCorner<3, 3>() = euler_zyx_to_rot<double>(all_configs_error_one_objH(raw_cube_ind, 7), all_configs_error_one_objH(raw_cube_ind, 8), cam_pose_raw.euler_angle(2));
					set_cam_pose(transToWolrd_new);
					ground_plane_sensor = cam_pose.transToWolrd.transpose() * ground_plane_world;
				}

				cuboid *sample_obj = new cuboid();
				change_2d_corner_to_3d_object(all_box_corners_2d_one_objH.block(2 * raw_cube_ind, 0, 2, 8), all_configs_error_one_objH.row(raw_cube_ind).head<3>(),
											  ground_plane_sensor, cam_pose.transToWolrd, cam_pose.invK, cam_pose.projectionMatrix, *sample_obj);
				// 		  sample_obj->print_cuboid();
				if ((sample_obj->scale.array() < 0).any())
					continue; // scale should be positive
				sample_obj->rect_detect_2d = Vector4d(left_x_raw, top_y_raw, obj_width_raw, obj_height_raw);
				sample_obj->edge_distance_error = all_configs_error_one_objH(raw_cube_ind, 4); // record the original error
				sample_obj->edge_angle_error = all_configs_error_one_objH(raw_cube_ind, 5);
				sample_obj->normalized_error = normalized_score(box_id);
				double skew_ratio = sample_obj->scale.head(2).maxCoeff() / sample_obj->scale.head(2).minCoeff();
				sample_obj->skew_ratio = skew_ratio;
				sample_obj->down_expand_height = all_configs_error_one_objH(raw_cube_ind, 6);
				if (whether_sample_cam_roll_pitch)
				{
					sample_obj->camera_roll_delta = all_configs_error_one_objH(raw_cube_ind, 7) - cam_pose_raw.euler_angle(0);
					sample_obj->camera_pitch_delta = all_configs_error_one_objH(raw_cube_ind, 8) - cam_pose_raw.euler_angle(1);
				}
				else
				{
					sample_obj->camera_roll_delta = 0;
					sample_obj->camera_pitch_delta = 0;
				}

				raw_obj_proposals.push_back(sample_obj);
			}
		} // end of differnet object height sampling

		// %finally rank all proposals. [normalized_error   skew_error]
		int actual_cuboid_num_small = std::min(max_cuboid_num, (int)raw_obj_proposals.size());
		VectorXd all_combined_score(raw_obj_proposals.size());
		for (int box_id = 0; box_id < raw_obj_proposals.size(); box_id++)
		{
			cuboid *sample_obj = raw_obj_proposals[box_id];
			double skew_error = weight_skew_error * std::max(sample_obj->skew_ratio - nominal_skew_ratio, 0.0);
			if (sample_obj->skew_ratio > max_cut_skew)
				skew_error = 100;
			double new_combined_error = sample_obj->normalized_error + weight_skew_error * skew_error;
			all_combined_score(box_id) = new_combined_error;
		}

		std::vector<int> sort_idx_small(all_combined_score.rows());
		iota(sort_idx_small.begin(), sort_idx_small.end(), 0);
		sort_indexes(all_combined_score, sort_idx_small, actual_cuboid_num_small);
		for (int ii = 0; ii < actual_cuboid_num_small; ii++) // use sorted index
		{
			all_object_cuboids[object_id].push_back(raw_obj_proposals[sort_idx_small[ii]]);
		}

		ca::Profiler::tictoc("One 3D object total time");
	} // end of different objects

	if (whether_plot_final_images || whether_save_final_images)
	{
		cv::Mat frame_all_cubes_img = rgb_img.clone();
		for (int object_id = 0; object_id < all_object_cuboids.size(); object_id++)
			if (all_object_cuboids[object_id].size() > 0)
			{
				plot_image_with_cuboid(frame_all_cubes_img, all_object_cuboids[object_id][0]);
			}
		if (whether_save_final_images)
			cuboids_2d_img = frame_all_cubes_img;
		if (whether_plot_final_images)
		{
			cv::imshow("frame_all_cubes_img", frame_all_cubes_img);
			cv::waitKey(0);
		}
	}
}


//
// Copyright 2010 Scott McMurray.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_HASH_HPP
#define BOOST_HASH_HPP

// Checksums
#include <boost/hash/adler.hpp>
#include <boost/hash/crc.hpp>

// Cryptographic Hashes
#include <boost/hash/cubehash.hpp>
#include <boost/hash/md4.hpp>
#include <boost/hash/md5.hpp>
#include <boost/hash/sha.hpp>
#include <boost/hash/sha1.hpp>
#include <boost/hash/sha2.hpp>

// Infrastructure
#include <boost/hash/compute_digest.hpp>
#include <boost/hash/digest.hpp>
#include <boost/hash/digest_io.hpp>

#endif // BOOST_HASH_HPP


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Master Test Suite
#include <boost/test/unit_test.hpp>

#include "BlindSignature.h"
#include "MoneyOrder.h"
#include "MoneyOrderInfo.h"
#include "Rsa.h"
#include "SecretSplitting.h"
#include "Utilities.h"

#include <gmpxx.h>
#include <string>

BOOST_AUTO_TEST_CASE(money_order_test_1)
{
    MoneyOrder old_mo;

    old_mo.m_amount = 1000;
    old_mo.m_uniqueness = "the uniqueness string";

    for (int i = 0; i < 100; ++i)
    {
        mpz_class left;
        mpz_class right;
        std::tie(left, right) = SecretSplitting::SplitSecret("this is a test");

        old_mo.m_identity_strings.push_back(
               MoneyOrder::IdentityPair(
                   CommitPair( "leftHash_0", "leftR1_0" ),
                   CommitPair( "rightHash_0", "rightR1_0" )));

        old_mo.m_identity_strings.push_back(
               MoneyOrder::IdentityPair(
                   CommitPair( "leftHash_1", "leftR1_1" ),
                   CommitPair( "rightHash_1", "rightR1_1" )));
    }

    std::string serial_str = old_mo.Serialize();

    // Write out the money order to file
    std::stringstream mo_filename;
    mo_filename << "x.bin";
    FILE* mo_output = fopen(mo_filename.str().c_str(), "wb");
    mpz_out_raw(mo_output, Utilities::StringToNumber(serial_str).get_mpz_t());
    fclose(mo_output);

    std::stringstream filename1;
    filename1 << "x.bin";
    FILE* xFile = fopen(filename1.str().c_str(), "rb");
    mpz_class xMpz;
    mpz_inp_raw(xMpz.get_mpz_t(), xFile );
    fclose(xFile);

    MoneyOrder new_mo;
    new_mo.Deserialize(Utilities::NumberToString(xMpz));

    BOOST_CHECK_EQUAL(new_mo.m_amount, old_mo.m_amount);
    BOOST_CHECK_EQUAL(new_mo.m_uniqueness, old_mo.m_uniqueness);
    BOOST_REQUIRE_EQUAL(new_mo.m_identity_strings.size(), 200U);
    BOOST_CHECK_EQUAL(new_mo.m_identity_strings[0].first.first, "leftHash_0");
    BOOST_CHECK_EQUAL(new_mo.m_identity_strings[0].first.second, "leftR1_0");
    BOOST_CHECK_EQUAL(new_mo.m_identity_strings[0].second.first, "rightHash_0");
    BOOST_CHECK_EQUAL(new_mo.m_identity_strings[0].second.second, "rightR1_0");

    BOOST_CHECK_EQUAL(new_mo.m_identity_strings[1].first.first, "leftHash_1");
    BOOST_CHECK_EQUAL(new_mo.m_identity_strings[1].first.second, "leftR1_1");
    BOOST_CHECK_EQUAL(new_mo.m_identity_strings[1].second.first, "rightHash_1");
    BOOST_CHECK_EQUAL(new_mo.m_identity_strings[1].second.second, "rightR1_1");
}

BOOST_AUTO_TEST_CASE(money_order_info_test_1)
{
    std::vector<MoneyOrderInfo> info_vec;
    MoneyOrderInfo info;
    info.m_blinding_factor = "2394262445592642632306707632461079788577338370913227907193177833182985877544234442800057994459481910350241788357623265528469560138808620624954012423617897";

    mpz_class left;
    mpz_class right;
    std::tie(left, right) = SecretSplitting::SplitSecret("this is a test");

    CommitData leftCommitData = GenCommitData(left);
    std::string leftHash = Hash( leftCommitData );

    CommitData rightCommitData = GenCommitData(right);
    std::string rightHash = Hash( rightCommitData );

    info.m_commit_data.push_back(std::pair<CommitData, CommitData>(leftCommitData, rightCommitData));
    info.m_commit_data.push_back(std::pair<CommitData, CommitData>(leftCommitData, rightCommitData));
    info_vec.push_back(info);

    std::string ser = info_vec[0].Serialize();

    MoneyOrderInfo new_info;
    new_info.Deserialize(ser);

    BOOST_CHECK_EQUAL(info.m_blinding_factor, new_info.m_blinding_factor);
    BOOST_CHECK(info.m_commit_data[0].first.b == new_info.m_commit_data[0].first.b);
    BOOST_CHECK(info.m_commit_data[0].second.b == new_info.m_commit_data[0].second.b);
    BOOST_CHECK(info.m_commit_data[1].first.b == new_info.m_commit_data[1].first.b);
    BOOST_CHECK(info.m_commit_data[1].second.b == new_info.m_commit_data[1].second.b);
}


#include <boost/geometry/formulas/sjoberg_intersection.hpp>


///////////////////////////////////////////////////////////////////////////////
// ars::detail::area.hpp                                                     //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_ARS_DETAIL_AREA_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_ARS_DETAIL_AREA_HPP_ER_2009
#include <stdexcept>
#include <boost/ars/detail/data.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace ars{

// Area of the exponential tangent between two
// 2 intersections (one of which is possibly -inf or inf).

template<typename T>
T area_flat_segment(
    const tangent_intersection<T>& a,
    const tangent_intersection<T>& b,
    const T& offset
){
    typedef constant<T> const_;
    T mid = (a.t() + b.t()) / const_::two_;
    return (b.z() - a.z()) * exp(mid - offset);
}

template<typename T>
T area_left_tail(
    const data<T>& b,
    const T& offset
){
    return exp( b.t() - offset) / b.dy();
}
template<typename T>
T area_right_tail(
    const tangent_intersection<T>& a,
    const point<T>& b,
    const T& offset
){
    return - exp( a.t() - offset) / b.dy();
}

template<typename T>
T area_segment(
    const tangent_intersection<T>& a,
    const data<T>& b,
    const T& offset
){
    typedef constant<T> const_;
    // Note that b.dy() (a.z_-b.z_) == a.t_ - b.t_
    T dt  = a.t() - b.t();
    return exp( b.t() - offset) * (const_::one_ - exp(dt)) / b.dy();
}

template<typename T>
T area_segment_safeguarded(
    const tangent_intersection<T>& a,
    const data<T>& b,
    const T& offset
)
{
    typedef constant<T> const_;
    T area;
    if(fabs(b.dy())<const_::lmin_){
        area = area_flat_segment(a,b,offset);
    }else{
        if(a.t()-b.t()>const_::lmax_){
            // exp(a.t_-iter_.t_) = inf,
            // so 1) not computable and 2) b negligible
            // This alternative is computable because (in principle)
            // exp(a.t_- offset) <inf
            area = area_right_tail(a,b,offset);
        }else{
            area = area_segment(a,b,offset);
        }
    }
    return area;
}

}// ars
}// detail
}// statistics
}// boost

#endif


#ifndef PYTHONIC_SCIPY_SPECIAL_JVP_HPP
#define PYTHONIC_SCIPY_SPECIAL_JVP_HPP

#include "pythonic/include/scipy/special/jvp.hpp"

#include "pythonic/types/ndarray.hpp"
#include "pythonic/utils/functor.hpp"
#include "pythonic/utils/numpy_traits.hpp"

#define BOOST_MATH_THREAD_LOCAL thread_local
#include <boost/math/special_functions/bessel_prime.hpp>

PYTHONIC_NS_BEGIN

namespace scipy
{
  namespace special
  {
    namespace details
    {
      template <class T0, class T1>
      double jvp(T0 x, T1 y)
      {
        using namespace boost::math::policies;
        return boost::math::cyl_bessel_j_prime(
            x, y, make_policy(promote_double<true>()));
      }
    }

#define NUMPY_NARY_FUNC_NAME jvp
#define NUMPY_NARY_FUNC_SYM details::jvp
#include "pythonic/types/numpy_nary_expr.hpp"
  }
}
PYTHONIC_NS_END

#endif


// Copyright (c) 2021 Stig Rune Sellevag
//
// This file is distributed under the MIT License. See the accompanying file
// LICENSE.txt or http://www.opensource.org/licenses/mit-license.php for terms
// and conditions.

#ifdef _MSC_VER
#pragma warning(push)
#pragma warning(disable : 4305)
#pragma warning(disable : 5054)
#endif

#include <scilib/mdarray.h>
#include <scilib/linalg.h>
#include <chrono>
#include <iostream>
#include <Eigen/Dense>

using Timer = std::chrono::duration<double, std::milli>;

void print(int n, int m, const Timer& t_eigen, const Timer& t_sci)
{
    std::cout << "Matrix-vector multiplication:\n"
              << "-----------------------------\n"
              << "size =        " << n << " x " << m << '\n'
              << "scilib/eigen = " << t_sci.count() / t_eigen.count() << "\n\n";
}

void benchmark(int n, int m)
{
    Eigen::MatrixXd a1(n, m);
    Eigen::VectorXd a2(m);
    a1.fill(1.0);
    a2.fill(1.0);
    auto t1 = std::chrono::high_resolution_clock::now();
    for (int it = 0; it < 10; ++it) {
        Eigen::VectorXd a3 = a1 * a2;
    }
    auto t2 = std::chrono::high_resolution_clock::now();
    Timer t_eigen = t2 - t1;

    Sci::Matrix<double> b1(n, m);
    b1 = 1.0;
    Sci::Vector<double> b2(m);
    b2 = 1.0;
    t1 = std::chrono::high_resolution_clock::now();
    for (int it = 0; it < 10; ++it) {
        auto b3 = b1 * b2;
    }
    t2 = std::chrono::high_resolution_clock::now();
    Timer t_sci = t2 - t1;

    print(n, m, t_eigen, t_sci);
}

int main()
{
    int n = 10;
    int m = 5;
    benchmark(n, m);

    n = 100;
    m = 50;
    benchmark(n, m);

    n = 1000;
    m = 500;
    benchmark(n, m);

    n = 10000;
    m = 5000;
    benchmark(n, m);
}

#ifdef _MSC_VER
#pragma warning(pop)
#endif


// Boost.Geometry Index
//
// Quickbook Examples
//
// Copyright (c) 2011-2013 Adam Wulkiewicz, Lodz, Poland.
//
// 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)

//[rtree_variants_map

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/ring.hpp>
#include <boost/geometry/geometries/linestring.hpp>

#include <boost/geometry/index/rtree.hpp>

#include <cmath>
#include <vector>
#include <map>
#include <iostream>
#include <boost/foreach.hpp>
#include <boost/variant.hpp>

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

typedef bg::model::point<float, 2, bg::cs::cartesian> point;
typedef bg::model::box<point> box;
typedef bg::model::polygon<point, false, false> polygon; // ccw, open polygon
typedef bg::model::ring<point, false, false> ring; // ccw, open ring
typedef bg::model::linestring<point> linestring;
typedef boost::variant<polygon, ring, linestring> geometry;

typedef std::map<unsigned, geometry> map;
typedef std::pair<box, map::iterator> value;

template <class Container>
void fill(unsigned i, Container & container)
{
    for ( float a = 0 ; a < 6.28316f ; a += 1.04720f )
    {
        float x = i + int(10*::cos(a))*0.1f;
        float y = i + int(10*::sin(a))*0.1f;
        container.push_back(point(x, y));
    }
}

struct print_visitor : public boost::static_visitor<>
{
    void operator()(polygon const& g) const { std::cout << bg::wkt<polygon>(g) << std::endl; }
    void operator()(ring const& g) const { std::cout << bg::wkt<ring>(g) << std::endl; }
    void operator()(linestring const& g) const { std::cout << bg::wkt<linestring>(g) << std::endl; }
};

struct envelope_visitor : public boost::static_visitor<box>
{
    box operator()(polygon const& g) const { return bg::return_envelope<box>(g); }
    box operator()(ring const& g) const { return bg::return_envelope<box>(g); }
    box operator()(linestring const& g) const { return bg::return_envelope<box>(g); }
};

int main()
{
    // geometries container
    map geometries;

    // create some geometries
    for ( unsigned i = 0 ; i < 10 ; ++i )
    {
        unsigned c = rand() % 3;

        if ( 0 == c )
        {
            // create polygon
            polygon p;
            fill(i, p.outer());
            geometries.insert(std::make_pair(i, geometry(p)));
        }
        else if ( 1 == c )
        {
            // create ring
            ring r;
            fill(i, r);
            geometries.insert(std::make_pair(i, geometry(r)));
        }
        else if ( 2 == c )
        {
            // create linestring
            linestring l;
            fill(i, l);
            geometries.insert(std::make_pair(i, geometry(l)));
        }
    }

    // display geometries
    std::cout << "generated geometries:" << std::endl;
    BOOST_FOREACH(map::value_type const& p, geometries)
        boost::apply_visitor(print_visitor(), p.second);

    // create the rtree using default constructor
    bgi::rtree< value, bgi::quadratic<16, 4> > rtree;

    // fill the spatial index
    for ( map::iterator it = geometries.begin() ; it != geometries.end() ; ++it )
    {
        // calculate polygon bounding box
        box b = boost::apply_visitor(envelope_visitor(), it->second);
        // insert new value
        rtree.insert(std::make_pair(b, it));
    }

    // find values intersecting some area defined by a box
    box query_box(point(0, 0), point(5, 5));
    std::vector<value> result_s;
    rtree.query(bgi::intersects(query_box), std::back_inserter(result_s));

    // find 5 nearest values to a point
    std::vector<value> result_n;
    rtree.query(bgi::nearest(point(0, 0), 5), std::back_inserter(result_n));

    // note: in Boost.Geometry the WKT representation of a box is polygon

    // note: the values store the bounding boxes of geometries
    // the geometries aren't used for querying but are printed

    // display results
    std::cout << "spatial query box:" << std::endl;
    std::cout << bg::wkt<box>(query_box) << std::endl;
    std::cout << "spatial query result:" << std::endl;
    BOOST_FOREACH(value const& v, result_s)
        boost::apply_visitor(print_visitor(), v.second->second);

    std::cout << "knn query point:" << std::endl;
    std::cout << bg::wkt<point>(point(0, 0)) << std::endl;
    std::cout << "knn query result:" << std::endl;
    BOOST_FOREACH(value const& v, result_n)
        boost::apply_visitor(print_visitor(), v.second->second);

    return 0;
}

//]


#include <iostream>
#include <sstream>
#include <time.h>
#include <stdio.h>
#include <fstream>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <boost/algorithm/string.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/filesystem.hpp>

using namespace cv;
using namespace std;
using namespace boost::filesystem;

/*
 * 原理为张正友相机标定
 */

int main(int argc, char** argv)
{

    if ( argc != 2 )
    {
        cout<<"Parameter input error"<<endl;
        return 1;
    }
    ofstream fout("/home/xc/caliberation_result.txt");  /* 保存标定结果的文件 */

    // 读取每一幅图像，从中提取出角点，然后对角点进行亚像素精确化
    int image_count = 0;  /* 图像数量 */
    Size image_size;      /* 图像的尺寸 */
    Size board_size = Size(6, 4);             /* 标定板上每行、列的角点数 */
    vector<Point2f> image_points_buf;         /* 缓存每幅图像上检测到的角点 */
    vector<vector<Point2f>> image_points_seq; /* 保存检测到的所有角点 */
    vector<string> filenames;
    //获取图片路径
    path dirPath(argv[1]);
    if(not exists(dirPath) or not is_directory(dirPath))
    {
        cerr<<"不能打开文件路径:"<<argv[1]<<endl;
        return false;
    }

    for(directory_entry& x:directory_iterator(dirPath))
    {
        string extension=x.path().extension().string();
        boost::algorithm::to_lower(extension);
        if(extension==".jpg" or extension==".jpeg" or extension==".png")
        {
            ++image_count;
            cout<<"image_count = "<<image_count<<endl;
            filenames.push_back(x.path().string());
            Mat imageInput=imread(x.path().string());
            if(image_count==1)
            {
                image_size.width=imageInput.cols;
                image_size.height=imageInput.rows;
                cout<<"image_size.width = "<<image_size.width<<endl;
                cout<<"image_size.height = "<<image_size.height<<endl;
            }
            if (0 == findChessboardCorners(imageInput, board_size, image_points_buf))
            {
                cout << "can not find chessboard corners!\n";  // 找不到角点
                exit(1);
            }
            else
            {
                Mat view_gray;
                cvtColor(imageInput, view_gray, CV_RGB2GRAY);  // 转灰度图

                /* 亚像素精确化 */
                // image_points_buf 初始的角点坐标向量，同时作为亚像素坐标位置的输出
                // Size(5,5) 搜索窗口大小
                // （-1，-1）表示没有死区
                // TermCriteria 角点的迭代过程的终止条件, 可以为迭代次数和角点精度两者的组合
                cornerSubPix(view_gray, image_points_buf, Size(5,5), Size(-1,-1), TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));

                image_points_seq.push_back(image_points_buf);  // 保存亚像素角点

                /* 在图像上显示角点位置 */
                drawChessboardCorners(imageInput, board_size, image_points_buf, true); // 用于在图片中标记角点
                namedWindow("Camera Calibration",0);
                imshow("Camera Calibration", imageInput);       // 显示图片
                waitKey(500); //暂停0.5S
            }
        }

    }
    cout<<"end"<<endl;
    int CornerNum = board_size.width * board_size.height;  // 每张图片上总的角点数

    //-------------以下是摄像机标定------------------

    /*棋盘三维信息*/
    Size square_size = Size(10, 10);         /* 实际测量得到的标定板上每个棋盘格的大小 */
    vector<vector<Point3f>> object_points;   /* 保存标定板上角点的三维坐标 */

    /*内外参数*/
    Mat cameraMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0));  /* 摄像机内参数矩阵 */
    vector<int> point_counts;   // 每幅图像中角点的数量
    Mat distCoeffs=Mat(1, 5, CV_32FC1,Scalar::all(0));       /* 摄像机的5个畸变系数：k1,k2,p1,p2,k3 */
    vector<Mat> tvecsMat;      /* 每幅图像的旋转向量 */
    vector<Mat> rvecsMat;      /* 每幅图像的平移向量 */

    /* 初始化标定板上角点的三维坐标 */
    int i, j, t;
    for (t=0; t<image_count; t++)
    {
        vector<Point3f> tempPointSet;
        for (i=0; i<board_size.height; i++)
        {
            for (j=0; j<board_size.width; j++)
            {
                Point3f realPoint;

                /* 假设标定板放在世界坐标系中z=0的平面上 */
                realPoint.x = i * square_size.width;
                realPoint.y = j * square_size.height;
                realPoint.z = 0;
                tempPointSet.push_back(realPoint);
            }
        }
        object_points.push_back(tempPointSet);
    }

    /* 初始化每幅图像中的角点数量，假定每幅图像中都可以看到完整的标定板 */
    for (i=0; i<image_count; i++)
    {
        point_counts.push_back(board_size.width * board_size.height);
    }

    /* 开始标定 */
    // object_points 世界坐标系中的角点的三维坐标
    // image_points_seq 每一个内角点对应的图像坐标点
    // image_size 图像的像素尺寸大小
    // cameraMatrix 输出，内参矩阵
    // distCoeffs 输出，畸变系数
    // rvecsMat 输出，旋转向量
    // tvecsMat 输出，位移向量
    // 0 标定时所采用的算法
    calibrateCamera(object_points, image_points_seq, image_size, cameraMatrix, distCoeffs, rvecsMat, tvecsMat, 0);

    //------------------------标定完成------------------------------------

    // -------------------对标定结果进行评价------------------------------

    double total_err = 0.0;         /* 所有图像的平均误差的总和 */
    double err = 0.0;               /* 每幅图像的平均误差 */
    vector<Point2f> image_points2;  /* 保存重新计算得到的投影点 */
    cerr<<"每幅图像的标定误差：\n";

    for (i=0;i<image_count;i++)
    {
        vector<Point3f> tempPointSet = object_points[i];

        /* 通过得到的摄像机内外参数，对空间的三维点进行重新投影计算，得到新的投影点 */
        projectPoints(tempPointSet, rvecsMat[i], tvecsMat[i], cameraMatrix, distCoeffs, image_points2);

        /* 计算新的投影点和旧的投影点之间的误差*/
        vector<Point2f> tempImagePoint = image_points_seq[i];
        Mat tempImagePointMat = Mat(1, tempImagePoint.size(), CV_32FC2);
        Mat image_points2Mat = Mat(1, image_points2.size(), CV_32FC2);

        for (int j = 0 ; j < tempImagePoint.size(); j++)
        {
            image_points2Mat.at<Vec2f>(0,j) = Vec2f(image_points2[j].x, image_points2[j].y);
            tempImagePointMat.at<Vec2f>(0,j) = Vec2f(tempImagePoint[j].x, tempImagePoint[j].y);
        }
        err = norm(image_points2Mat, tempImagePointMat, NORM_L2);
        total_err += err/= point_counts[i];
        cerr << "第" << i+1 << "幅图像的平均误差：" << err<< "像素" << endl;
    }
    cerr << "总体平均误差：" << total_err/image_count << "像素" <<endl <<endl;

    //-------------------------评价完成---------------------------------------------

    //-----------------------保存定标结果-------------------------------------------
    Mat rotation_matrix = Mat(3,3,CV_32FC1, Scalar::all(0));  /* 保存每幅图像的旋转矩阵 */
    fout << "相机内参数矩阵：" << endl;
    fout << cameraMatrix << endl << endl;
    fout << "畸变系数：\n";
    fout << distCoeffs << endl << endl << endl;
    for (int i=0; i<image_count; i++)
    {
        cerr << "第" << i+1 << "幅图像的旋转向量：" << endl;
        cerr << tvecsMat[i] << endl;

        /* 将旋转向量转换为相对应的旋转矩阵 */
        Rodrigues(tvecsMat[i], rotation_matrix);
        cerr << "第" << i+1 << "幅图像的旋转矩阵：" << endl;
        cerr << rotation_matrix << endl;
        cerr << "第" << i+1 << "幅图像的平移向量：" << endl;
        cerr << rvecsMat[i] << endl << endl;
    }
    cerr<<endl;

    //--------------------标定结果保存结束-------------------------------

    //----------------------显示定标结果--------------------------------

    Mat mapx = Mat(image_size, CV_32FC1);
    Mat mapy = Mat(image_size, CV_32FC1);
    Mat R = Mat::eye(3, 3, CV_32F);
    string imageFileName;
    std::stringstream StrStm;
    for (int i = 0 ; i != image_count ; i++)
    {
        initUndistortRectifyMap(cameraMatrix, distCoeffs, R, cameraMatrix, image_size, CV_32FC1, mapx, mapy);
        Mat imageSource = imread(filenames[i]);
        Mat newimage = imageSource.clone();
        remap(imageSource, newimage, mapx, mapy, INTER_LINEAR);
        StrStm.clear();
        imageFileName.clear();
        StrStm << i+1;
        StrStm >> imageFileName;
        imageFileName += "_d.jpg";
        imwrite(imageFileName, newimage);
        imshow("new",newimage);
    }
    fout.close();
    return 0;
}


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, University of Notre Dame, Notre
// Dame, IN 46556.
//
// Permission to modify the code and to distribute modified code is
// granted, provided the text of this NOTICE is retained, a notice that
// the code was modified is included with the above COPYRIGHT NOTICE and
// with the COPYRIGHT NOTICE in the LICENSE file, and that the LICENSE
// file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================
//
// This example is similar to the one in edge_property.cpp.
// The only difference is that this example uses exterior
// property storage instead of interior (properties).
//
//  Sample output:
//
//    0        --(10, 8)--> 1
//
//    1        --(20, 12)--> 4        --(40, 12)--> 3
//            <--(10,8)-- 0        <--(20,16)-- 2
//    2        --(20, 16)--> 1
//            <--(20,16)-- 5
//    3        --(40, 12)--> 6
//            <--(40,12)-- 1
//    4        --(20, 12)--> 7
//            <--(20,12)-- 1
//    5        --(20, 16)--> 2
//            <--(20,16)-- 6
//    6        --(20, 16)--> 5         --(10, 8)--> 8
//            <--(20,12)-- 7 <--(40,12)-- 3
//    7        --(20, 12)--> 6
//            <--(20,12)-- 4
//    8
//            <--(10,8)-- 6

#include <boost/config.hpp>
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/property_map.hpp>

template <class Graph, class Capacity, class Flow>
void print_network(Graph& G, Capacity capacity, Flow flow)
{
  typedef typename boost::graph_traits<Graph>::vertex_iterator    Viter;
  typedef typename boost::graph_traits<Graph>::out_edge_iterator OutEdgeIter;
  typedef typename boost::graph_traits<Graph>::in_edge_iterator InEdgeIter;

  Viter ui, uiend;
  for (boost::tie(ui, uiend) = boost::vertices(G); ui != uiend; ++ui) {
    OutEdgeIter out, out_end;
    std::cout << *ui << "\t";

    for(boost::tie(out, out_end) = boost::out_edges(*ui, G); out != out_end; ++out)
      std::cout << "--(" << boost::get(capacity, *out) << ", "
           << boost::get(flow, *out) << ")--> " << boost::target(*out,G) << "\t";
    std::cout << std::endl << "\t";

    InEdgeIter in, in_end;
    for(boost::tie(in, in_end) = boost::in_edges(*ui, G); in != in_end; ++in)
      std::cout << "<--(" << boost::get(capacity, *in) << "," << boost::get(flow, *in) << ")-- "
           << boost::source(*in, G) << "\t";
    std::cout << std::endl;
  }
}

int main(int , char* []) {

  typedef boost::adjacency_list<boost::vecS, boost::vecS,
    boost::bidirectionalS, boost::no_property,
    boost::property<boost::edge_index_t, std::size_t> > Graph;

  const int num_vertices = 9;
  Graph G(num_vertices);

  /*          2<----5
             /       ^
            /         \
           V           \
    0 ---->1---->3----->6--->8
           \           ^
            \         /
             V       /
             4----->7
   */

  int capacity[] = { 10, 20, 20, 20, 40, 40, 20, 20, 20, 10 };
  int flow[] = { 8, 12, 12, 12, 12, 12, 16, 16, 16, 8 };

  // insert edges into the graph, and assign each edge an ID number
  // to index into the property arrays
  boost::add_edge(0, 1, 0, G);

  boost::add_edge(1, 4, 1, G);
  boost::add_edge(4, 7, 2, G);
  boost::add_edge(7, 6, 3, G);

  boost::add_edge(1, 3, 4, G);
  boost::add_edge(3, 6, 5, G);

  boost::add_edge(6, 5, 6, G);
  boost::add_edge(5, 2, 7, G);
  boost::add_edge(2, 1, 8, G);

  boost::add_edge(6, 8, 9, G);

  typedef boost::property_map<Graph, boost::edge_index_t>::type EdgeIndexMap;
  EdgeIndexMap edge_id = boost::get(boost::edge_index, G);

  typedef boost::iterator_property_map<int*, EdgeIndexMap, int, int&> IterMap;

  print_network(G, IterMap(capacity, edge_id), IterMap(flow, edge_id));

  return 0;
}


#include<bits/stdc++.h>
using namespace std;
#include <iostream>                  // for std::cout
#include <utility>                   // for std::pair
#include <algorithm>                 // for std::for_each
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/adjacency_matrix.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

using namespace boost;
#include "boost/graph/CND/cnd_function.hpp"

template<typename Graph>
void treesize_vertices(int cur,Graph &graph,bool visited[],int subtree_size[])
{
    visited[cur] = true;
    subtree_size[cur] = 1;
    typedef typename graph_traits<Graph>::adjacency_iterator adj_itr;
    adj_itr ai,ai_end;
    for (boost::tie(ai, ai_end) = adjacent_vertices(cur,graph);ai != ai_end; ++ai)
    {
        if (!visited[*ai])
        {
            treesize_vertices(*ai,graph,visited,subtree_size);
            subtree_size[cur]+=subtree_size[*ai];
        }
    }
}
template<typename Graph>
void check_centroid(int cur,Graph &graph,bool visited[],int subtree_size[],int *check)
{
    visited[cur] = true;
    typedef typename graph_traits<Graph>::adjacency_iterator adj_itr;
    adj_itr ai,ai_end;
    for (boost::tie(ai, ai_end) = adjacent_vertices(cur,graph);ai != ai_end; ++ai)
    {
        if (!visited[*ai])
        {
            if(subtree_size[*ai]>(subtree_size[cur]/2))
            {
                *check=0;
            }
            check_centroid(*ai,graph,visited,subtree_size,check);
        }
    }
}
int main()
{
    int i,n,a,b;
    scanf("%d",&n);
    typedef adjacency_list<vecS, vecS, undirectedS> Graph_list;
    Graph_list graph_list(n);
    for(i=1;i<n;i++)
    {
        scanf("%d %d",&a,&b);
        add_edge(a,b,graph_list);
    }
    pair<Graph_list,int> copy_graph;
    copy_graph=CND(graph_list,0,n);
    int root=copy_graph.second;
    bool visited[1030];
    int subtree_size[1030];
    for(i=0;i<n;i++)
    {
        visited[i]=false;
        subtree_size[i]=0;
    }
    treesize_vertices<Graph_list>(root,copy_graph.first,visited,subtree_size);
    int check=1;
    for(i=0;i<n;i++)
        visited[i]=false;
    check_centroid<Graph_list>(root,copy_graph.first,visited,subtree_size,&check);
    if(check)
    {
        printf("Success\n");
    }

    return 0;

}


//
// Created by Samuel Jackson on 11/03/2016.
//

#include <Eigen/Dense>
#include <set>
#include "CellMatrix.h"

using namespace Eigen;
using namespace Molly;

// offset indicies for neighbour cells for each cell
// we only need to look at half the cells at one time.
// includes 13 neighbours (half the total)
const size_t NUM_OFFSETS = 13;
const int OFFSETS[][3] {
    {1, 0, 0},
    {1, 1, 0},
    {0, 1, 0},
    {-1, 1, 0},
    {0, 0, 1},
    {1, 0, 1},
    {1, 1, 1},
    {0, 1, 1},
    {-1, 1, 1},
    {-1, 0, 1},
    {-1, -1, 1},
    {0, -1, 1},
    {1, -1, 1}
};

CellMatrix::CellMatrix(const double cell_width, size_t d1, size_t d2, size_t d3)
        : cell_width(cell_width) {
    resize(d1, d2, d3);
}

void CellMatrix::resize(size_t d1, size_t d2, size_t d3) {
    data.clear();
    data.resize(d1*d2*d3);

    for (auto iter = data.begin(); iter != data.end(); ++iter) {
        *iter = std::make_shared<Cell>();
    }

    // update the dimensions
    this->d1 = d1;
    this->d2 = d2;
    this->d3 = d3;

    // reinitialise neighbours
    create_neighbours();
}

Cell_ptr CellMatrix::operator()(size_t i, size_t j, size_t k) {
    const size_t index = i*d2*d3 + j*d3 + k;

    if (index >= data.size()) {
        std::stringstream ss;
        ss << "Index is out of points for matrix of size " << d1 << ", " << d2 << ", " << d3;
        throw std::runtime_error(ss.str());
    }

    return data[index];
}

Cell_ptr const CellMatrix::operator()(size_t i, size_t j, size_t k) const {
    const size_t index = i*d2*d3 + j*d3 + k;

    if (index >= data.size()) {
        std::stringstream ss;
        ss << "Index is out of points for matrix of size " << d1 << ", " << d2 << ", " << d3;
        throw std::runtime_error(ss.str());
    }

    return data[index];
}

void CellMatrix::create_neighbours() {
    // loop over each dimension of the cell matrix
    for(size_t i = 0; i < d1; ++i) {
        for(size_t j = 0; j < d2; ++j) {
            for(size_t k = 0; k < d3; ++k) {
                // add pointers to each neighbour of a cell in the cell matrix
                Cell_ptr cell = (*this)(i, j, k);
                for (size_t offset_num = 0; offset_num < NUM_OFFSETS; ++offset_num) {
                    // get offset cell indices
                    const int x_offset = OFFSETS[offset_num][0];
                    const int y_offset = OFFSETS[offset_num][1];
                    const int z_offset = OFFSETS[offset_num][2];

                    // correct the offsets to wrap around
                    size_t x = (i+x_offset) % d1;
                    size_t y = (j+y_offset) % d2;
                    size_t z = (k+z_offset) % d3;

                    //set pointer to neighbour cell
                    Cell_ptr neighbour = (*this)(x, y, z);
                    cell->add_neighbour(neighbour);
                }
            }
        }
    }
}

void CellMatrix::add_molecule(Molecule_ptr mol) {
    Eigen::Vector3i index;
    convert_vector_to_index(mol->r, index);
    (*this)(index(0), index(1), index(2))->add_molecule(mol);
}

void CellMatrix::convert_vector_to_index(const Vector3d& vec, Vector3i& index) const {
    index = (vec / cell_width).cast<int>();
    index(0) = index(0) % d1;
    index(1) = index(1) % d2;
    index(2) = index(2) % d3;
}

void CellMatrix::wrap_cells() {
    Vector3i index;
    int i = 0;

    // loop over every cell in the matrix
    for (auto iter = data.begin(); iter != data.end(); ++iter) {
        auto& molecules = (*iter)->get_molecules();

        // check the molecules in each cell to see if they need to move
        // to a different cell
        for(auto mol = molecules.begin(); mol != molecules.end();) {
            convert_vector_to_index((*mol)->r, index);
            // check if this molecule is in the same cell
            if(i != (index(0)*d2*d3 + index(1)*d3 + index(2))) {
                // if it isn't then move it to the correct cell mark for removal from the current.
                (*this)(index(0), index(1), index(2))->add_molecule(*mol);
                // note: must update iterator after removal
                mol = molecules.erase(mol);
            } else {
                // otherwise just skip to next molecule
                ++mol;
            }
        }
        ++i;
    }
}


/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2014, Texas A&M University
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Texas A&M University nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

#include "Utils/FIRMUtils.h"
#include <boost/math/constants/constants.hpp>
#include <boost/date_time.hpp>
#include <utility>
#include <random>
#include <tinyxml.h>

void FIRMUtils::normalizeAngleToPiRange(double &theta)
{

    while(theta > boost::math::constants::pi<double>())
    {
        theta -= 2*boost::math::constants::pi<double>();
    }

    while(theta < -boost::math::constants::pi<double>())
    {
        theta += 2*boost::math::constants::pi<double>();
    }

}

int FIRMUtils::signum(const double d)
{
        if(d>0)
            return 1;

        if(d<0)
            return -1;
}

int FIRMUtils::generateRandomIntegerInRange(const int floor, const int ceiling)
{
    //std::random_device rd; // obtain a random number from hardware

    //std::mt19937 eng(rd()); // seed the generator

    //std::uniform_int_distribution<> distr(floor, ceiling); // define the range

    //return distr(eng);

    int r = rand()%(ceiling - floor + 1) + floor;

    return r;
}

void FIRMUtils::writeFIRMGraphToXML(const std::vector<std::pair<int,std::pair<arma::colvec,arma::mat> > > nodes, const std::vector<std::pair<std::pair<int,int>,FIRMWeight> > edgeWeights)
{
    TiXmlDocument doc;

 	TiXmlDeclaration* decl = new TiXmlDeclaration( "1.0", "", "" );
	doc.LinkEndChild( decl );

	TiXmlElement * Nodes = new TiXmlElement( "Nodes" );
	doc.LinkEndChild( Nodes );

	for(int i = 0; i < nodes.size(); i++)
	{
        TiXmlElement * node;
        node = new TiXmlElement( "node" );
        Nodes->LinkEndChild( node );

        int nodeID = nodes[i].first; // id of the node in the graph

        arma::colvec xVec = nodes[i].second.first; // x,y,yaw

        arma::mat cov = nodes[i].second.second; // covariance matrix

        node->SetAttribute("id", nodeID);
        node->SetDoubleAttribute("x", xVec(0));
        node->SetDoubleAttribute("y", xVec(1));
        node->SetDoubleAttribute("theta",xVec(2));
        node->SetDoubleAttribute("c11", cov(0,0));
        node->SetDoubleAttribute("c12", cov(0,1));
        node->SetDoubleAttribute("c13", cov(0,2));
        node->SetDoubleAttribute("c21", cov(1,0));
        node->SetDoubleAttribute("c22", cov(1,1));
        node->SetDoubleAttribute("c23", cov(1,2));
        node->SetDoubleAttribute("c31", cov(2,0));
        node->SetDoubleAttribute("c32", cov(2,1));
        node->SetDoubleAttribute("c33", cov(2,2));

   }

    TiXmlElement * Edges = new TiXmlElement( "Edges" );
	doc.LinkEndChild( Edges );

	for(int i = 0; i < edgeWeights.size(); i++)
	{
        TiXmlElement * edge;
        edge = new TiXmlElement( "edge" );
        Edges->LinkEndChild( edge );

        FIRMWeight w = edgeWeights[i].second;

        edge->SetAttribute("startVertexID", edgeWeights[i].first.first);
        edge->SetAttribute("endVertexID", edgeWeights[i].first.second);
        edge->SetDoubleAttribute("successProb", w.getSuccessProbability());
        edge->SetDoubleAttribute("cost", w.getCost());

   }

   // Generate time stamp for saving roadmap
    namespace pt = boost::posix_time;

    pt::ptime now = pt::second_clock::local_time();

    std::string timeStamp(to_iso_string(now)) ;

    std::string roadmapFileName =  "FIRMRoadMap-" + timeStamp + ".xml";

	doc.SaveFile(roadmapFileName);
}

bool FIRMUtils::readFIRMGraphFromXML(const std::string &pathToXML, std::vector<std::pair<int, arma::colvec> > &FIRMNodePosList, std::vector<std::pair<int, arma::mat> > &FIRMNodeCovarianceList, std::vector<std::pair<std::pair<int,int>,FIRMWeight> > &edgeWeights)
{

    TiXmlDocument doc(pathToXML);

    bool loadOkay = doc.LoadFile();

    if ( !loadOkay )
    {
        OMPL_INFORM("FIRMUtils: Could not load Graph from XML . Need to construct graph.");
        return false;
    }

    TiXmlNode* NodeList = 0;

    TiXmlElement* nodeElement = 0;

    TiXmlElement* itemElement = 0;

    NodeList = doc.FirstChild( "Nodes" );

    assert( NodeList );

    nodeElement = NodeList->ToElement(); //convert NodeList to element

    assert( nodeElement  );

    TiXmlNode* child = 0;

    while( (child = nodeElement->IterateChildren(child)))
    {
        assert( child );

        itemElement = child->ToElement();

        assert( itemElement );

        double x = 0, y = 0, theta = 0, c11 = 0, c12 = 0, c13 = 0, c21 = 0, c22 = 0, c23 = 0, c31 = 0, c32 = 0, c33 = 0;
        int id = 0;

        itemElement->QueryIntAttribute("id", &id) ;
        itemElement->QueryDoubleAttribute("x", &x) ;
        itemElement->QueryDoubleAttribute("y", &y) ;
        itemElement->QueryDoubleAttribute("theta", &theta) ;
        itemElement->QueryDoubleAttribute("c11", &c11) ;
        itemElement->QueryDoubleAttribute("c12", &c12) ;
        itemElement->QueryDoubleAttribute("c13", &c13) ;
        itemElement->QueryDoubleAttribute("c21", &c21) ;
        itemElement->QueryDoubleAttribute("c22", &c22) ;
        itemElement->QueryDoubleAttribute("c23", &c23) ;
        itemElement->QueryDoubleAttribute("c31", &c31) ;
        itemElement->QueryDoubleAttribute("c32", &c32) ;
        itemElement->QueryDoubleAttribute("c33", &c33) ;

        arma::colvec xVec(3);
        arma::mat cov(3,3);

        xVec(0) = x;
        xVec(1) = y;
        xVec(2) = theta;

        cov(0,0) = c11;
        cov(0,1) = c12;
        cov(0,2) = c13;
        cov(1,0) = c21;
        cov(1,1) = c22;
        cov(1,2) = c23;
        cov(2,0) = c31;
        cov(2,1) = c32;
        cov(2,2) = c33;

        FIRMNodePosList.push_back(std::make_pair(id,xVec));
        FIRMNodeCovarianceList.push_back(std::make_pair(id,cov));

    }

    //////////////////////
    TiXmlNode* edgeList = 0;

    TiXmlElement* edgeElement = 0;

    TiXmlElement* itemElement2 = 0;

    edgeList = doc.FirstChild( "Edges" );

    assert( edgeList );

    edgeElement = edgeList->ToElement(); //convert NodeList to element

    assert( edgeElement  );

    TiXmlNode* child2 = 0;

    while( (child2 = edgeElement->IterateChildren(child2)))
    {
        assert( child2 );

        itemElement2 = child2->ToElement();

        assert( itemElement2 );

        int startVertexID = 0, endVertexID = 0;
        double successProb = 0, cost = 0;

        itemElement2->QueryIntAttribute("startVertexID", &startVertexID) ;
        itemElement2->QueryIntAttribute("endVertexID", &endVertexID) ;
        itemElement2->QueryDoubleAttribute("successProb", &successProb) ;
        itemElement2->QueryDoubleAttribute("cost", &cost) ;

        FIRMWeight w(cost, successProb);

        edgeWeights.push_back(std::make_pair(std::make_pair(startVertexID, endVertexID),w));

    }

    return true;
}

double FIRMUtils::degree2Radian(double deg)
{
    return boost::math::constants::pi<double>()*deg/180.0;
}

double FIRMUtils::radian2Degree(double rads)
{
    return rads*180.0/boost::math::constants::pi<double>();
}


#include <kr_trackers/traj_gen.h>

#include <Eigen/LU>

TrajectoryGenerator::TrajectoryGenerator(unsigned int continuous_derivative_order, unsigned int minimize_derivative)
    : N_(2 * (continuous_derivative_order + 1)), R_(minimize_derivative)
{
}

void TrajectoryGenerator::setInitialConditions(const Vec3f &pos, const vec_Vec3f &derivatives)
{
  clearWaypoints();
  waypoints_.push_back(pos);

  initial_derivatives_.clear();
  initial_derivatives_.resize(N_ / 2);
  for(size_t i = 0; i < std::min(initial_derivatives_.size(), derivatives.size()); ++i)
  {
    initial_derivatives_[i] = derivatives[i];
  }
}

void TrajectoryGenerator::addWaypoint(const Vec3f &x)
{
  if(waypoints_.empty())
    setInitialConditions(x, vec_Vec3f());
  else
    waypoints_.push_back(x);
}

void TrajectoryGenerator::clearWaypoints(void)
{
  waypoints_.clear();
  coefficients_.clear();
  waypoint_times_.clear();
  initial_derivatives_.clear();
}

std::vector<float> TrajectoryGenerator::computeTimesTrapezoidSpeed(float v_des, float a_des) const
{
  std::vector<float> waypoint_times;
  waypoint_times.reserve(waypoints_.size());

  waypoint_times.push_back(0);  // First waypoint, t = 0

  if(waypoints_.size() < 2)
    return waypoint_times;

  const Vec3f &initial_vel_ = initial_derivatives_[0];
  const int initial_vel_sign = (initial_vel_.dot(waypoints_[1] - waypoints_[0]) >= 0) ? 1 : -1;
  const float v_initial = initial_vel_sign * initial_vel_.norm();

  std::vector<float> accumulated_dist;
  accumulated_dist.reserve(waypoints_.size());
  accumulated_dist.push_back(0);
  for(unsigned int i = 1; i < waypoints_.size(); ++i)
  {
    const float dist = (waypoints_[i] - waypoints_[i - 1]).norm();
    accumulated_dist.push_back(accumulated_dist[i - 1] + dist);
  }
  const float total_dist = accumulated_dist.back();

  float d_accel = std::abs(v_des * v_des - v_initial * v_initial) / 2 / a_des;
  float d_decel = v_des * v_des / 2 / a_des;
  float d_constant = (total_dist - (d_accel + d_decel));
  if(d_accel + d_decel > total_dist)
  {
    d_constant = 0;
    d_accel = total_dist / 2 - v_initial * v_initial / 4 / a_des;
    d_decel = total_dist - d_accel;
  }
  const float t_accel = std::sqrt(v_initial * v_initial + 2 * a_des * d_accel) / a_des;
  const float t_constant = d_constant / v_des;
  const float t_decel = std::sqrt(2 * a_des * d_decel) / a_des;

  for(unsigned int i = 1; i < waypoints_.size(); ++i)
  {
    if(accumulated_dist[i] <= d_accel)  // Accel
    {
      waypoint_times.push_back((std::sqrt(v_initial * v_initial + 2 * a_des * accumulated_dist[i]) - v_initial) /
                               a_des);
    }
    else if(accumulated_dist[i] <= d_accel + d_constant)  // Constant
    {
      waypoint_times.push_back(t_accel + (accumulated_dist[i] - d_accel) / v_des);
    }
    else  // Decel
    {
      waypoint_times.push_back(t_accel + t_constant + t_decel -
                               std::sqrt(2 * (total_dist - accumulated_dist[i]) / a_des));
    }
  }

  return waypoint_times;
}

std::vector<float> TrajectoryGenerator::computeTimesConstantSpeed(float avg_speed) const
{
  std::vector<float> waypoint_times;
  waypoint_times.reserve(waypoints_.size());

  waypoint_times.push_back(0);  // First waypoint, t = 0

  for(unsigned int i = 1; i < waypoints_.size(); ++i)
  {
    waypoint_times.push_back(waypoint_times[i - 1] + std::sqrt((waypoints_[i] - waypoints_[i - 1]).norm()) / avg_speed);
  }
  return waypoint_times;
}

const std::vector<float> &TrajectoryGenerator::getWaypointTimes() const
{
  return waypoint_times_;
}

float TrajectoryGenerator::getTotalTime() const
{
  return waypoint_times_.back();
}

// From https://stackoverflow.com/a/33454406
template <typename T>
T powInt(T x, unsigned int n)
{
  if(n == 0)
    return T{1};

  auto y = T{1};
  while(n > 1)
  {
    if(n % 2 == 1)
      y *= x;
    x *= x;
    n /= 2;
  }
  return x * y;
}

bool TrajectoryGenerator::calculate(const std::vector<float> &waypoint_times)
{
  if(waypoints_.size() < 2)
    return false;

  if(waypoint_times.size() != waypoints_.size())
  {
    printf("waypoint_times.size() != waypoints_.size()\n");
    return false;
  }

  const unsigned int num_waypoints = waypoints_.size();
  const unsigned int num_segments = num_waypoints - 1;
  // printf("num_segments: %d\n", num_segments);

  Eigen::MatrixXf A = Eigen::MatrixXf::Zero(num_segments * N_, num_segments * N_);  // Linear constraints
  Eigen::MatrixXf Q = Eigen::MatrixXf::Zero(num_segments * N_, num_segments * N_);  // Quadratic cost matrix
  for(unsigned int i = 0; i < num_segments; i++)
  {
    float seg_time = waypoint_times[i + 1] - waypoint_times[i];
    for(unsigned int n = 0; n < N_; n++)
    {
      // A_0
      if(n < N_ / 2)
      {
        int val = 1;
        for(unsigned int m = 0; m < n; m++)
          val *= (n - m);
        A(i * N_ + n, i * N_ + n) = val;
      }
      // A_T
      for(unsigned int r = 0; r < N_ / 2; r++)
      {
        if(r <= n)
        {
          int val = 1;
          for(unsigned int m = 0; m < r; m++)
            val *= (n - m);
          A(i * N_ + N_ / 2 + r, i * N_ + n) = val * powInt(seg_time, n - r);
        }
      }
      // Q
      for(unsigned int r = 0; r < N_; r++)
      {
        if(r >= R_ && n >= R_)
        {
          int val = 1;
          for(unsigned int m = 0; m < R_; m++)
            val *= (r - m) * (n - m);
          Q(i * N_ + r, i * N_ + n) = 2 * val * powInt(seg_time, r + n - 2 * R_ + 1) / (r + n - 2 * R_ + 1);
        }
      }
    }
  }
  const unsigned int num_fixed_derivatives = num_waypoints - 2 + N_;
  const unsigned int num_free_derivatives = num_waypoints * N_ / 2 - num_fixed_derivatives;
  // printf("num_fixed_derivatives: %d, num_free_derivatives: %d\n",
  //        num_fixed_derivatives, num_free_derivatives);

  // M
  Eigen::MatrixXf M = Eigen::MatrixXf::Zero(num_segments * N_, num_waypoints * N_ / 2);
  M.block(0, 0, N_ / 2, N_ / 2) = Eigen::MatrixXf::Identity(N_ / 2, N_ / 2);
  M.block(num_segments * N_ - N_ / 2, N_ / 2 + num_waypoints - 2, N_ / 2, N_ / 2) =
      Eigen::MatrixXf::Identity(N_ / 2, N_ / 2);
  for(unsigned int i = 0; i < num_waypoints - 2; i++)
  {
    M((2 * i + 1) * N_ / 2, N_ / 2 + i) = 1;
    M((2 * i + 2) * N_ / 2, N_ / 2 + i) = 1;
    for(unsigned int j = 1; j < N_ / 2; j++)
    {
      M((2 * i + 1) * N_ / 2 + j, num_fixed_derivatives - 1 + i * (N_ / 2 - 1) + j) = 1;
      M((2 * i + 2) * N_ / 2 + j, num_fixed_derivatives - 1 + i * (N_ / 2 - 1) + j) = 1;
    }
  }
  // Eigen::MatrixXf A_inv = A.inverse();
  Eigen::MatrixXf A_inv_M = A.partialPivLu().solve(M);
  Eigen::MatrixXf R = A_inv_M.transpose() * Q * A_inv_M;
#if 0
  //std::cout << "A:\n" << A << std::endl;
  std::cout << "Q:\n" << Q << std::endl;
  //std::cout << "M:\n" << M << std::endl;
  //std::cout << "A_inv_M:\n" << A_inv_M << std::endl;
  std::cout << "R:\n" << R << std::endl;
#endif
  Eigen::MatrixXf Rpp =
      R.block(num_fixed_derivatives, num_fixed_derivatives, num_free_derivatives, num_free_derivatives);
  Eigen::MatrixXf Rpf = R.block(num_fixed_derivatives, 0, num_free_derivatives, num_fixed_derivatives);

  // Fixed derivatives
  Eigen::MatrixX3f Df = Eigen::MatrixX3f(num_fixed_derivatives, 3);
  // First point
  Df.row(0) = waypoints_[0].transpose();
  for(unsigned int i = 1; i < N_ / 2; i++)
  {
    Df.row(i) = initial_derivatives_[i - 1].transpose();
  }
  // Middle waypoints
  for(unsigned int i = 1; i < num_waypoints - 1; i++)
  {
    Df.row((N_ / 2) - 1 + i) = waypoints_[i].transpose();
  }
  // End point
  Df.row(N_ / 2 + (num_waypoints - 2)) = waypoints_[num_waypoints - 1].transpose();
  for(unsigned int i = 1; i < N_ / 2; i++)
  {
    Df.row((N_ / 2) + (num_waypoints - 2) + i) = Vec3f::Zero().transpose();
  }
  // std::cout << "Df:\n" << Df << std::endl;
  Eigen::MatrixX3f D = Eigen::MatrixX3f(num_waypoints * N_ / 2, 3);
  D.topRows(num_fixed_derivatives) = Df;
  if(num_waypoints > 2 && num_free_derivatives > 0)
  {
    Eigen::MatrixX3f Dp = -Rpp.partialPivLu().solve(Rpf * Df);
    // std::cout << "Dp:\n" << Dp << std::endl;
    D.bottomRows(num_free_derivatives) = Dp;
  }
  Eigen::MatrixX3f d = M * D;
  // std::cout << "d:\n" << d << std::endl;
  coefficients_.clear();
  for(unsigned int i = 0; i < num_segments; i++)
  {
    const Eigen::MatrixX3f p = A.block(i * N_, i * N_, N_, N_).partialPivLu().solve(d.block(i * N_, 0, N_, 3));
    // std::cout << "p:\n" << p << std::endl;
    coefficients_.push_back(p);
  }
  waypoint_times_ = waypoint_times;
  return true;
}

bool TrajectoryGenerator::getCommand(const float time, Vec3f &pos, Vec3f &vel, Vec3f &acc, Vec3f &jrk) const
{
  if(time < 0)
    return false;

  int cur_idx = -1;
  for(unsigned int i = 1; i < waypoint_times_.size(); i++)
  {
    if(time <= waypoint_times_[i])
    {
      cur_idx = i - 1;
      break;
    }
  }
  if(cur_idx == -1)
    return false;

  const float t_traj = time - waypoint_times_[cur_idx];
  const Eigen::MatrixX3f &p = coefficients_[cur_idx];
  pos = Vec3f::Zero();
  for(unsigned int i = 0; i < p.rows(); i++)
    pos += p.row(i).transpose() * powInt(t_traj, i);

  vel = Vec3f::Zero();
  for(unsigned int i = 1; i < p.rows(); i++)
    vel += p.row(i).transpose() * (i * powInt(t_traj, i - 1));

  acc = Vec3f::Zero();
  for(unsigned int i = 2; i < p.rows(); i++)
    acc += p.row(i).transpose() * (i * (i - 1) * powInt(t_traj, i - 2));

  jrk = Vec3f::Zero();
  for(unsigned int i = 3; i < p.rows(); i++)
    jrk += p.row(i).transpose() * (i * (i - 1) * (i - 2) * powInt(t_traj, i - 3));

  return true;
}

void TrajectoryGenerator::calcMaxPerSegment(std::vector<float> &max_vel, std::vector<float> &max_acc,
                                            std::vector<float> &max_jrk) const
{
  const unsigned int num_samples_per_seg = 10;
  for(unsigned int seg_idx = 0; seg_idx < waypoint_times_.size() - 1; ++seg_idx)
  {
    const float seg_start_time = waypoint_times_[seg_idx];
    const float seg_end_time = waypoint_times_[seg_idx + 1];
    float seg_duration = seg_end_time - seg_start_time;
    const float dt = seg_duration / num_samples_per_seg;

    float seg_max_vel = 0, seg_max_acc = 0, seg_max_jrk = 0;
    for(unsigned int sample_idx = 0; sample_idx < num_samples_per_seg; ++sample_idx)
    {
      const float t_traj = sample_idx * dt;
      const Eigen::MatrixX3f &p = coefficients_[seg_idx];
      Vec3f vel = Vec3f::Zero();
      for(unsigned int i = 1; i < p.rows(); i++)
        vel += p.row(i).transpose() * (i * powInt(t_traj, i - 1));
      if(vel.norm() > seg_max_vel)
        seg_max_vel = vel.norm();

      Vec3f acc = Vec3f::Zero();
      for(unsigned int i = 2; i < p.rows(); i++)
        acc += p.row(i).transpose() * (i * (i - 1) * powInt(t_traj, i - 2));
      if(acc.norm() > seg_max_acc)
        seg_max_acc = acc.norm();

      Vec3f jrk = Vec3f::Zero();
      for(unsigned int i = 3; i < p.rows(); i++)
        jrk += p.row(i).transpose() * (i * (i - 1) * (i - 2) * powInt(t_traj, i - 3));
      if(jrk.norm() > seg_max_jrk)
        seg_max_jrk = jrk.norm();
    }
    // printf("seg idx: %d\n", seg_idx);
    // printf("max_vel: %f\n", seg_max_vel);
    // printf("max_acc: %f\n", seg_max_acc);
    // printf("max_jrk: %f\n", seg_max_jrk);
    max_vel.push_back(seg_max_vel);
    max_acc.push_back(seg_max_acc);
    max_jrk.push_back(seg_max_jrk);
  }
  return;
}

void TrajectoryGenerator::optimizeWaypointTimes(const float max_vel, const float max_acc, const float max_jrk)
{
  std::vector<float> segment_times;
  for(unsigned int i = 0; i < waypoint_times_.size() - 1; ++i)
    segment_times.push_back(waypoint_times_[i + 1] - waypoint_times_[i]);

  std::vector<float> best_segment_times = segment_times;
  float best_cost = std::numeric_limits<float>::max();

  for(int traj_iter = 0; traj_iter < 20; ++traj_iter)
  {
    // std::cout << std::string(20, '=') << " " << traj_iter << " "
    //           << std::string(20, '=') << "\n";

    for(unsigned int i = 1; i < waypoint_times_.size(); ++i)
    {
      waypoint_times_[i] = waypoint_times_[i - 1] + segment_times[i - 1];
    }
    calculate(waypoint_times_);

    std::vector<float> seg_max_vel, seg_max_acc, seg_max_jrk;
    calcMaxPerSegment(seg_max_vel, seg_max_acc, seg_max_jrk);

    const float vel_cost = 1.0f, acc_cost = 0.1f, jrk_cost = 0.01f;
    float cost = 0;

    const auto rectified_square = [](float x) { return (x > 0) ? x * x : 0; };

    const std::vector<float> prev_segment_times = segment_times;
    bool done = true;
    for(unsigned int i = 0; i < seg_max_vel.size(); ++i)
    {
      cost += vel_cost * rectified_square(seg_max_vel[i] - max_vel) +
              acc_cost * rectified_square(seg_max_acc[i] - max_acc) +
              jrk_cost * rectified_square(seg_max_jrk[i] - max_jrk);

      if(seg_max_vel[i] > max_vel || seg_max_acc[i] > max_acc || seg_max_jrk[i] > max_jrk)
      {
        segment_times[i] *= 1.035f;
        done = false;
      }
      else if(seg_max_vel[i] < 0.9f * max_vel && seg_max_acc[i] < 0.9f * max_acc && seg_max_jrk[i] < 0.9f * max_jrk)
      {
        segment_times[i] *= 0.966f;
        done = false;
      }
    }

    // std::cout << "cost: " << cost << "\n";
    // If no solution found, keep the "best" one (with lowest violations)
    if(cost <= best_cost)
    {
      best_segment_times = prev_segment_times;
      best_cost = cost;
    }
    if(done)
    {
      // std::cout << "Early exit! num iter: " << traj_iter << "\n";
      break;
    }
  }
}


#pragma once
#ifndef BASIS_RADIAL_HPP
#define BASIS_RADIAL_HPP

// c++ libraries
#include <iosfwd>
// eigen
#include <Eigen/Dense>
// symmetry functions
#include "src/nnp/cutoff.hpp"
#include "src/nnp/symm_radial.hpp"
// ann - serialization
#include "src/mem/serialize.hpp"

#ifndef BASIS_RADIAL_PRINT_FUNC
#define BASIS_RADIAL_PRINT_FUNC 0
#endif

struct BasisR{
private:
	PhiRN phiRN_;//type of radial functions
	cutoff::Norm normT_;//normalization scheme
	int nfR_;//number of radial functions
	double rc_;//cutoff radius
	double norm_;//normalization factor
	cutoff::Func* cutoff_;//cutoff function
	PhiR** fR_;//radial functions
	Eigen::VectorXd symm_;//symmetry function
public:
	//==== constructors/destructors ====
	BasisR():norm_(cutoff::Norm::UNKNOWN),phiRN_(PhiRN::UNKNOWN),nfR_(0),fR_(NULL),cutoff_(NULL){}
	BasisR(const BasisR& basisR);
	~BasisR();

	//==== operators ====
	BasisR& operator=(const BasisR& basis);
	friend std::ostream& operator<<(std::ostream& out, const BasisR& basisR);

	//==== initialization ====
	void init_G1(int nR, cutoff::Norm norm, cutoff::Name tcut, double rcut);
	void init_G2(int nR, cutoff::Norm norm, cutoff::Name tcut, double rcut);
	void init_T1(int nR, cutoff::Norm norm, cutoff::Name tcut, double rcut);

	//==== reading/writing ====
	static void write(const char* filename, const BasisR& basis);
	static void read(const char* filename, BasisR& basis);
	static void write(FILE* writer, const BasisR& basis);
	static void read(FILE* reader, BasisR& basis);

	//==== member access ====
	cutoff::Func* cutoff(){return cutoff_;}
	const cutoff::Func* cutoff()const{return cutoff_;}
	double& rc(){return rc_;}
	const double& rc()const{return rc_;}
	const int& nfR()const{return nfR_;}
	PhiRN& phiRN(){return phiRN_;}
	const PhiRN& phiRN()const{return phiRN_;}
	cutoff::Norm& normT(){return normT_;}
	const cutoff::Norm& normT()const{return normT_;}
	PhiR& fR(int i){return *fR_[i];}
	const PhiR& fR(int i)const{return *fR_[i];}
	Eigen::VectorXd& symm(){return symm_;}
	const Eigen::VectorXd& symm()const{return symm_;}

	//==== member functions ====
	void clear();
	void symm(double dr);
	double force(double dr, const double* dEdG)const;

	//==== static functions ====
	static double norm(cutoff::Norm normT, double rc);
};
std::ostream& operator<<(std::ostream& out, const BasisR& basisR);

bool operator==(const BasisR& basis1, const BasisR& basis2);
inline bool operator!=(const BasisR& basis1, const BasisR& basis2){return !(basis1==basis2);}

namespace serialize{

	//**********************************************
	// byte measures
	//**********************************************

	template <> int nbytes(const BasisR& obj);

	//**********************************************
	// packing
	//**********************************************

	template <> int pack(const BasisR& obj, char* arr);

	//**********************************************
	// unpacking
	//**********************************************

	template <> int unpack(BasisR& obj, const char* arr);

}

#endif


#include "A_Star.h"

//#include <queue>
//#include <stdio.h>
//#include <math.h>
//#include <vector>
//#include <cassert>
//#include <typeinfo>
//#include <vector>

#include <boost/math/constants/constants.hpp>
#include <iostream>
#include <vector>

using namespace std;

int A_Star::Get_Shortest_Path(const vector<vector<int>>& array,int Max_Search_Time,bool Debug_Info_Switch){
    //为简单，干脆把把下面数组转为链表结构的数组
    //约定：0是可走的，1表示障碍物不可走，2表示起点，3表示终点
//    vector<vector<int>> array={
//        { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
//        { 0, 0, 0, 1, 1, 0, 0, 0, 0, 0 },
//        { 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 },
//        { 0, 0, 0, 0, 1, 1, 0, 0, 0, 0 },
//        { 0, 0, 0, 0, 0, 1, 3, 0, 0, 0 },
//        { 0, 0, 2, 0, 0, 1, 0, 0, 0, 0 },
//        { 0, 0, 0, 0, 1, 1, 0, 0, 0, 0 },
//        { 0, 0, 0, 0, 1, 1, 0, 0, 0, 0 },
//        { 0, 0, 0, 1, 1, 0, 0, 0, 0, 0 },
//        { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 } };
    debug_info_switch=Debug_Info_Switch;
    int Error_flag=0;
    bool Input_OK=false;
    Input_OK =Input_Verify(array);
    if(!Input_OK) return Error_flag;
    map.Build_Map(array);

    //PNode *Start_node;
//    Start_node=map.Start_node;
    //PNode *End_node;
//    End_node=map.End_node;

    map.Start_node->G=0;
    map.Start_node->H=H_Calculat(map.Start_node,map.End_node);
    map.Start_node->F=map.Start_node->G+map.Start_node->H;
    map.Start_node->open_flag=1;
    open_List.Heap_push(make_pair(map.Start_node->F,map.Start_node));
//   cout<<"hello world!  "<<Open_List->next->G<<" , "<<Open_List->next->H<<endl;

/********** main search ****************/
    Shortest_Path_Long=-1;
    Count=0;
    int Search_end_flag;
    while(1)
    {
        //cout<<"Count=  "<<Count<<endl;
        Search_end_flag=Search();
        if (Search_end_flag==1)
         {
            if(debug_info_switch)  cout<<" Having find the useable path !!! "<<endl;
            Error_flag=0;
            break;
         }
        if (Count>Max_Search_Time)
         {
            cout<<" Path finding timeout !!! "<<endl;
            Error_flag=1;
            break;
         }
        if (Search_end_flag==2)
         {
            if(debug_info_switch)  cout<<" Can not find the useable path !!! "<<endl;
            Error_flag=1;
            break;
         }

        Count++;
 //       cout<<"opt_node->[G,H]  "<<opt_node->G<<" , "<<opt_node->H<<endl;
    }
    if (Error_flag==0)
    {
        PNode *tmp=map.End_node;
        while(tmp)
        {
            //cout<<" ["<<tmp->x<<" , "<<tmp->y<<"] -> ";
            Shortest_Path.push_back({tmp->x,tmp->y});
            tmp=tmp->path_before;
            Shortest_Path_Long++;
        }
        reverse(Shortest_Path.begin(),Shortest_Path.end());  // " use of undlclared identifier 'reverse' ..."  ----it is why?  need #include <boost/math/constants/constants.hpp>
        //for(size_t i=0;i<Shortest_Path.size()/2;i++){
        //    swap(Shortest_Path[i],Shortest_Path[Shortest_Path.size()-1-i]);
        //}

    }

    //map.Clear_Map();

    return Error_flag;
}

bool A_Star::Input_Verify(const vector<vector<int>>& array){
    int Start_Point_Count=0,End_Point_Count=0;
    for(size_t i=0;i<array.size();i++){
        if(array[i].size() != array[0].size()){
            cout<<" Fatal Wrong , The Input Must be a Mutrix !!! "<<endl;
            return false;
        }
        for(size_t j=0;j<array[i].size();j++){
            if (array[i][j]<0 || array[i][j]>3){
                cout<<" Fatal Wrong , The Value in the Input Mutrix must be 0, 1, 2, 3  !!! "<<endl;
                return false;
            }
            if (array[i][j]==2) Start_Point_Count++;
            if (array[i][j]==3) End_Point_Count++;
        }
    }
    if (Start_Point_Count==1 && End_Point_Count==1) return true;
    else{
        if (Start_Point_Count!=1) cout<<" Fatal Wrong , Cannot find the start point or more than one start point !!! "<<endl;
        if (End_Point_Count!=1) cout<<" Fatal Wrong , Cannot find the end point or more than one end point !!! "<<endl;
        return false;
    }
}

float A_Star::H_Calculat(PNode *cur,PNode *end)
{
    float res=float_abs(cur->x-end->x)+float_abs(cur->y-end->y);
    return res;
}

float A_Star::float_abs(float x){
    return x > 0 ? x:-x;
}

int A_Star::Search()
{
     if (open_List.Heap_size()==0){
         if(debug_info_switch)  cout<<" The Open_List_Min_Heap is empty !!! "<<endl;
         return 2;
     }
     PNode *Opt_node;
     Opt_node=open_List.Heap_top().second;
     Opt_node->open_flag=0;
     Opt_node->close_flag=1;
     open_List.Heap_pop();
    for(size_t i=0;i<Opt_node->adjacent_node.size();i++)
    {
        PNode* Tmp_adjacent_next_node=Opt_node->adjacent_node[i];
        if (Tmp_adjacent_next_node->nodetype==EndPoint)       // End point has been found!!!
        {
            Tmp_adjacent_next_node->path_before=Opt_node;
            return 1;
        }

        if (Tmp_adjacent_next_node->open_flag==0 && Tmp_adjacent_next_node->close_flag==0 && Tmp_adjacent_next_node->nodetype !=UnReachable)
        {
            Tmp_adjacent_next_node->path_before=Opt_node;
            Tmp_adjacent_next_node->G=Opt_node->G+1;
            Tmp_adjacent_next_node->H=H_Calculat(Tmp_adjacent_next_node,map.End_node);
            Tmp_adjacent_next_node->F=Tmp_adjacent_next_node->G+Tmp_adjacent_next_node->H;
            Tmp_adjacent_next_node->open_flag=1;    //The open_flag should be modified! --20200312
            open_List.Heap_push(make_pair(Tmp_adjacent_next_node->F,Tmp_adjacent_next_node));
        }

        else if(Tmp_adjacent_next_node->open_flag==1 && Tmp_adjacent_next_node->close_flag==0)
        {
            float tmp_G=Opt_node->G+1;
            float tmp_H=H_Calculat(Tmp_adjacent_next_node,map.End_node);
            float tmp_F=tmp_G+tmp_H;
            if (tmp_F<Tmp_adjacent_next_node->F){
                //update thr f which is in open list!!!!
                //update thr f which is in open list!!!!
                //update thr f which is in open list!!!!
                Tmp_adjacent_next_node->G=tmp_G;
                Tmp_adjacent_next_node->H=tmp_H;
                Tmp_adjacent_next_node->F=tmp_F;
                Tmp_adjacent_next_node->path_before=Opt_node;
                //open_List.Heap_delect(Tmp_adjacent_next_node);
                //open_List.Heap_push(make_pair(Tmp_adjacent_next_node->F,Tmp_adjacent_next_node));
                open_List.Heap_modify(Tmp_adjacent_next_node,Tmp_adjacent_next_node->F);

            }
            else {
                //no action!
            }

        }
        else if(Tmp_adjacent_next_node->open_flag==0 && Tmp_adjacent_next_node->close_flag==1)
        {
            // no action!
        }
        else if((Tmp_adjacent_next_node->open_flag==0 && Tmp_adjacent_next_node->close_flag==0 && Tmp_adjacent_next_node->nodetype ==UnReachable))
        {
            Tmp_adjacent_next_node->close_flag=1;
        }
        else {
            cout<<"Fatel Error!!!"<<endl;
             return 2;
        }

    }

    return 0;
}

void A_Star::clear(){
    Shortest_Path.clear();
    open_List.Heap_clear();
    map.Clear_Map();
}