// std c
#include <stdio.h>
#include <math.h>
#include <iostream>
#include <fstream>
#include <iostream>
#include <string>
#include <sstream>
#include <ctime>
#include<numeric>
// opencv pcl
#include <opencv/cv.h>
#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_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 (C) 2021 Christian Brommer, Control of Networked Systems, University of Klagenfurt, Austria.
//
// All rights reserved.
//
// This software is licensed under the terms of the BSD-2-Clause-License with
// no commercial use allowed, the full terms of which are made available
// in the LICENSE file. No license in patents is granted.
//
// You can contact the author at <christian.brommer@ieee.org>

#include <mars/ekf.h>
#include <mars/general_functions/utils.h>
#include <Eigen/Dense>

namespace mars
{
Eigen::MatrixXd Ekf::CalculateCorrection()
{
  // Calculate innovation
  S_ = H_ * P_ * H_.transpose() + R_;
  S_ = Utils::EnforceMatrixSymmetry(S_);

  // Calculate Klamen Gain
  K_ = P_ * H_.transpose() * S_.inverse();

  // Calculate Correction
  Eigen::MatrixXd correction = K_ * res_;

  return correction;
}

Eigen::MatrixXd Ekf::CalculateCovUpdate()
{
  // Calculate ErrorState Covariance
  int64_t state_size = H_.cols();

  Eigen::MatrixXd I_state = Eigen::MatrixXd::Identity(state_size, state_size);

  Eigen::MatrixXd KH = I_state - K_ * H_;
  Eigen::MatrixXd updated_P = KH * P_ * KH.transpose() + K_ * R_ * K_.transpose();

  return updated_P;
}
}


/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center.

All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

#include "mitkConnectomicsStatisticsCalculator.h"
#include "mitkConnectomicsNetworkConverter.h"

#include <numeric>

#include <boost/graph/clustering_coefficient.hpp>

#ifdef _MSC_VER
# pragma warning(push)
# pragma warning(disable: 4172)
#endif
#include <boost/graph/connected_components.hpp>
#include <boost/graph/betweenness_centrality.hpp>

#ifdef _MSC_VER
# pragma warning(pop)
#endif

#include <boost/graph/visitors.hpp>

#include "vnl/algo/vnl_symmetric_eigensystem.h"

template<typename GraphType>
class all_pairs_shortest_recorder : public boost::default_bfs_visitor
{
public:
  typedef typename boost::graph_traits<GraphType>::vertex_descriptor VertexType;
  typedef typename boost::graph_traits<GraphType>::edge_descriptor EdgeType;

  all_pairs_shortest_recorder(int* dist, int &max, unsigned int &size)
  {
    d = dist;
    ecc = &max;
    component_size = &size;
  }

  void tree_edge(EdgeType edge, const GraphType& graph) {
    VertexType u = boost::source(edge, graph);
    VertexType v = boost::target(edge, graph);
    d[v] = d[u] + 1;
    *ecc = d[v];
    *component_size = *component_size + 1;
  }
private:
  int* d;
  int* ecc;
  unsigned int* component_size;
};

mitk::ConnectomicsStatisticsCalculator::ConnectomicsStatisticsCalculator()
  : m_Network( nullptr )
  , m_NumberOfVertices( 0 )
  , m_NumberOfEdges( 0 )
  , m_AverageDegree( 0.0 )
  , m_ConnectionDensity( 0.0 )
  , m_NumberOfConnectedComponents( 0 )
  , m_AverageComponentSize( 0.0 )
  , m_Components(0)
  , m_LargestComponentSize( 0 )
  , m_HopPlotExponent( 0.0 )
  , m_EffectiveHopDiameter( 0.0 )
  , m_VectorOfClusteringCoefficientsC( 0 )
  , m_VectorOfClusteringCoefficientsD( 0 )
  , m_VectorOfClusteringCoefficientsE( 0 )
  , m_AverageClusteringCoefficientsC( 0.0 )
  , m_AverageClusteringCoefficientsD( 0.0 )
  , m_AverageClusteringCoefficientsE( 0.0 )
  , m_VectorOfVertexBetweennessCentralities( 0 )
  , m_PropertyMapOfVertexBetweennessCentralities( )
  , m_AverageVertexBetweennessCentrality( 0.0 )
  , m_VectorOfEdgeBetweennessCentralities( 0 )
  , m_PropertyMapOfEdgeBetweennessCentralities( )
  , m_AverageEdgeBetweennessCentrality( 0.0 )
  , m_NumberOfIsolatedPoints( 0 )
  , m_RatioOfIsolatedPoints( 0.0 )
  , m_NumberOfEndPoints( 0 )
  , m_RatioOfEndPoints( 0.0 )
  , m_VectorOfEccentrities( 0 )
  , m_VectorOfEccentrities90( 0 )
  , m_VectorOfAveragePathLengths( 0.0 )
  , m_Diameter( 0 )
  , m_Diameter90( 0 )
  , m_Radius( 0 )
  , m_Radius90( 0 )
  , m_AverageEccentricity( 0.0 )
  , m_AverageEccentricity90( 0.0 )
  , m_AveragePathLength( 0.0 )
  , m_NumberOfCentralPoints( 0 )
  , m_RatioOfCentralPoints( 0.0 )
  , m_VectorOfSortedEigenValues( 0 )
  , m_SpectralRadius( 0.0 )
  , m_SecondLargestEigenValue( 0.0 )
  , m_AdjacencyTrace( 0.0 )
  , m_AdjacencyEnergy( 0.0 )
  , m_VectorOfSortedLaplacianEigenValues( 0 )
  , m_LaplacianTrace( 0.0 )
  , m_LaplacianEnergy( 0.0 )
  , m_LaplacianSpectralGap( 0.0 )
  , m_VectorOfSortedNormalizedLaplacianEigenValues( 0 )
  , m_NormalizedLaplacianTrace( 0.0 )
  , m_NormalizedLaplacianEnergy( 0.0 )
  , m_NormalizedLaplacianNumberOf2s( 0 )
  , m_NormalizedLaplacianNumberOf1s( 0 )
  , m_NormalizedLaplacianNumberOf0s( 0 )
  , m_NormalizedLaplacianLowerSlope( 0.0 )
  , m_NormalizedLaplacianUpperSlope( 0.0 )
  , m_SmallWorldness( 0.0 )
{
}

mitk::ConnectomicsStatisticsCalculator::~ConnectomicsStatisticsCalculator()
{
}

void mitk::ConnectomicsStatisticsCalculator::Update()
{
  CalculateNumberOfVertices();
  CalculateNumberOfEdges();
  CalculateAverageDegree();
  CalculateConnectionDensity();
  CalculateNumberOfConnectedComponents();
  CalculateAverageComponentSize();
  CalculateLargestComponentSize();
  CalculateRatioOfNodesInLargestComponent();
  CalculateHopPlotValues();
  CalculateClusteringCoefficients();
  CalculateBetweennessCentrality();
  CalculateIsolatedAndEndPoints();
  CalculateShortestPathMetrics();
  CalculateSpectralMetrics();
  CalculateLaplacianMetrics();
  CalculateNormalizedLaplacianMetrics();
  CalculateSmallWorldness();
}

void mitk::ConnectomicsStatisticsCalculator::CalculateNumberOfVertices()
{
  m_NumberOfVertices = boost::num_vertices( *(m_Network->GetBoostGraph()) );
}

void mitk::ConnectomicsStatisticsCalculator::CalculateNumberOfEdges()
{
  m_NumberOfEdges = boost::num_edges(  *(m_Network->GetBoostGraph()) );
}

void  mitk::ConnectomicsStatisticsCalculator::CalculateAverageDegree()
{
  m_AverageDegree = ( ( (double) m_NumberOfEdges * 2.0 ) / (double) m_NumberOfVertices );
}

void mitk::ConnectomicsStatisticsCalculator::CalculateConnectionDensity()
{
  double numberOfPossibleEdges = (double) m_NumberOfVertices * ( (double) m_NumberOfVertices - 1 ) / 2;

  m_ConnectionDensity = (double) m_NumberOfEdges / numberOfPossibleEdges;
}

void mitk::ConnectomicsStatisticsCalculator::CalculateNumberOfConnectedComponents()
{
  m_Components.resize( m_NumberOfVertices );
  m_NumberOfConnectedComponents = boost::connected_components( *(m_Network->GetBoostGraph()), &m_Components[0] );
}

void mitk::ConnectomicsStatisticsCalculator::CalculateAverageComponentSize()
{
  m_AverageComponentSize = (double) m_NumberOfVertices / (double) m_NumberOfConnectedComponents ;
}

void mitk::ConnectomicsStatisticsCalculator::CalculateLargestComponentSize()
{
  m_LargestComponentSize = 0;
  std::vector<unsigned int> bins( m_NumberOfConnectedComponents );

  for(unsigned int i=0; i < m_NumberOfVertices; i++)
  {
    bins[ m_Components[i] ]++;
  }

  for(unsigned int i=0; i < m_NumberOfConnectedComponents; i++)
  {
    if (bins[i] > m_LargestComponentSize )
    {
      m_LargestComponentSize = bins[i];
    }
  }
}

void mitk::ConnectomicsStatisticsCalculator::CalculateRatioOfNodesInLargestComponent()
{
  m_RatioOfNodesInLargestComponent = (double) m_LargestComponentSize / (double) m_NumberOfVertices ;
}

void mitk::ConnectomicsStatisticsCalculator::CalculateHopPlotValues()
{
  std::vector<int> bins( m_NumberOfVertices );

  VertexIteratorType vi, vi_end;
  unsigned int index( 0 );

  for( boost::tie( vi, vi_end ) = boost::vertices( *(m_Network->GetBoostGraph()) ); vi != vi_end; ++vi)
  {
    std::vector<int> distances(m_NumberOfVertices, 0);
    int max_distance = 0;
    VertexDescriptorType src = *vi;
    distances[src] = 0;
    unsigned int size = 0;

    boost::breadth_first_search(*(m_Network->GetBoostGraph()), src,
      visitor(all_pairs_shortest_recorder< NetworkType >
      (&distances[0], max_distance, size)));

    for(index=0; index < distances.size(); index++)
    {
      if(distances[index] > 0)
      {
        bins[distances[index]]++;
      }
    }
  }

  bins[0] = m_NumberOfVertices;
  for(index=1; index < bins.size(); index++)
  {
    bins[index] = bins[index] + bins[index-1];
  }

  int counter = 0;
  double C=0, D=0, E=0, F=0;

  for (unsigned int i=1; i<bins.size()-1; i++)
  {
    counter ++;
    if(fabs(log(double(bins[i+1])) - log(double(bins[i]))) < 0.0000001)
    {
      break;
    }
  }

  for (int i=1; i<=counter; i++)
  {
    double x = log(double(i));
    double y = log(double(bins[i]));
    C += x;
    D += y;
    E += x * y;
    F += x * x;
  }
  double b = (D*F - C*E)/(F*counter - C*C);
  m_HopPlotExponent = (E - b*C)/F;

  m_EffectiveHopDiameter =
    std::pow( ( m_NumberOfVertices * m_NumberOfVertices )
    / ( m_NumberOfVertices + 2 * m_NumberOfEdges ), 1.0 / m_HopPlotExponent );
}

void mitk::ConnectomicsStatisticsCalculator::CalculateClusteringCoefficients()
{
  VertexIteratorType vi, vi_end;
  std::vector<double> m_VectorOfClusteringCoefficientsC;
  std::vector<double> m_VectorOfClusteringCoefficientsD;
  std::vector<double> m_VectorOfClusteringCoefficientsE;
  typedef std::set<VertexDescriptorType> NeighborSetType;
  typedef NetworkType::out_edge_iterator OutEdgeIterType;

  for(boost::tie(vi,vi_end) = boost::vertices( *(m_Network->GetBoostGraph()) ); vi!=vi_end; ++vi)
  {
    // Get the list of vertices which are in the neighborhood of vi.
    std::pair<AdjacencyIteratorType, AdjacencyIteratorType> adjacent =
      boost::adjacent_vertices(*vi, *(m_Network->GetBoostGraph()) );

    //Populate a set with the neighbors of vi
    NeighborSetType neighbors;
    for(; adjacent.first!=adjacent.second; ++adjacent.first)
    {
      neighbors.insert(*adjacent.first);
    }

    // Now, count the edges between vertices in the neighborhood.
    unsigned int neighborhood_edge_count = 0;
    if(neighbors.size() > 0)
    {
      NeighborSetType::iterator iter;
      for(iter = neighbors.begin(); iter != neighbors.end(); ++iter)
      {
        std::pair<OutEdgeIterType, OutEdgeIterType> oe = out_edges(*iter, *(m_Network->GetBoostGraph()) );
        for(; oe.first != oe.second; ++oe.first)
        {
          if(neighbors.find(target(*oe.first, *(m_Network->GetBoostGraph()) )) != neighbors.end())
          {
            ++neighborhood_edge_count;
          }
        }
      }
      neighborhood_edge_count /= 2;
    }
    //Clustering Coefficienct C,E
    if(neighbors.size() > 1)
    {
      double num   = neighborhood_edge_count;
      double denum = neighbors.size() * (neighbors.size()-1)/2;
      m_VectorOfClusteringCoefficientsC.push_back( num / denum);
      m_VectorOfClusteringCoefficientsE.push_back( num / denum);
    }
    else
    {
      m_VectorOfClusteringCoefficientsC.push_back(0.0);
    }

    //Clustering Coefficienct D
    if(neighbors.size() > 0)
    {
      double num   = neighbors.size() + neighborhood_edge_count;
      double denum = ( (neighbors.size()+1) * neighbors.size()) / 2;
      m_VectorOfClusteringCoefficientsD.push_back( num / denum);
    }
    else
    {
      m_VectorOfClusteringCoefficientsD.push_back(0.0);
    }
  }

  // Average Clustering coefficienies:
  m_AverageClusteringCoefficientsC = std::accumulate(m_VectorOfClusteringCoefficientsC.begin(),
    m_VectorOfClusteringCoefficientsC.end(),
    0.0) / m_NumberOfVertices;

  m_AverageClusteringCoefficientsD = std::accumulate(m_VectorOfClusteringCoefficientsD.begin(),
    m_VectorOfClusteringCoefficientsD.end(),
    0.0) / m_NumberOfVertices;

  m_AverageClusteringCoefficientsE = std::accumulate(m_VectorOfClusteringCoefficientsE.begin(),
    m_VectorOfClusteringCoefficientsE.end(),
    0.0) / m_VectorOfClusteringCoefficientsE.size();
}

void mitk::ConnectomicsStatisticsCalculator::CalculateBetweennessCentrality()
{
  // std::map used for convenient initialization
  EdgeIndexStdMapType stdEdgeIndex;
  // associative property map needed for iterator property map-wrapper
  EdgeIndexMapType edgeIndex(stdEdgeIndex);

  EdgeIteratorType iterator, end;

  // sets iterator to start end end to end
  boost::tie(iterator, end) = boost::edges( *(m_Network->GetBoostGraph()) );

  int i(0);
  for ( ; iterator != end; ++iterator, ++i)
  {
    stdEdgeIndex.insert(std::pair< EdgeDescriptorType, int >( *iterator, i));
  }

  // Define EdgeCentralityMap
  m_VectorOfEdgeBetweennessCentralities.resize( m_NumberOfEdges, 0.0);
  // Create the external property map
  m_PropertyMapOfEdgeBetweennessCentralities = EdgeIteratorPropertyMapType(m_VectorOfEdgeBetweennessCentralities.begin(), edgeIndex);

  // Define VertexCentralityMap
  VertexIndexMapType vertexIndex = get(boost::vertex_index, *(m_Network->GetBoostGraph()) );
  m_VectorOfVertexBetweennessCentralities.resize( m_NumberOfVertices, 0.0);
  // Create the external property map
  m_PropertyMapOfVertexBetweennessCentralities = VertexIteratorPropertyMapType(m_VectorOfVertexBetweennessCentralities.begin(), vertexIndex);

  boost::brandes_betweenness_centrality( *(m_Network->GetBoostGraph()),
    m_PropertyMapOfVertexBetweennessCentralities, m_PropertyMapOfEdgeBetweennessCentralities );

  m_AverageVertexBetweennessCentrality = std::accumulate(m_VectorOfVertexBetweennessCentralities.begin(),
    m_VectorOfVertexBetweennessCentralities.end(),
    0.0) / (double) m_NumberOfVertices;

  m_AverageEdgeBetweennessCentrality = std::accumulate(m_VectorOfEdgeBetweennessCentralities.begin(),
    m_VectorOfEdgeBetweennessCentralities.end(),
    0.0) / (double) m_NumberOfEdges;
}

void mitk::ConnectomicsStatisticsCalculator::CalculateIsolatedAndEndPoints()
{
  m_NumberOfIsolatedPoints = 0;
  m_NumberOfEndPoints = 0;

  VertexIteratorType vi, vi_end;
  for( boost::tie(vi,vi_end) = boost::vertices( *(m_Network->GetBoostGraph()) ); vi!=vi_end; ++vi)
  {
    int degree = boost::out_degree(*vi, *(m_Network->GetBoostGraph()) );
    if(degree == 0)
    {
      m_NumberOfIsolatedPoints++;
    }
    else if (degree == 1)
    {
      m_NumberOfEndPoints++;
    }
  }

  m_RatioOfEndPoints = (double) m_NumberOfEndPoints / (double) m_NumberOfVertices;
  m_RatioOfIsolatedPoints = (double) m_NumberOfIsolatedPoints / (double) m_NumberOfVertices;
}

/**
* Calculates Shortest Path Related metrics of the graph.  The
* function runs a BFS from each node to find out the shortest
* distances to other nodes in the graph. The maximum of this distance
* is called the eccentricity of that node. The maximum eccentricity
* in the graph is called diameter and the minimum eccentricity is
* called the radius of the graph.  Central points are those nodes
* having eccentricity equals to radius.
*/
void mitk::ConnectomicsStatisticsCalculator::CalculateShortestPathMetrics()
{
  //for all vertices:
  VertexIteratorType vi, vi_end;

  //store the eccentricities in a vector.
  m_VectorOfEccentrities.resize( m_NumberOfVertices );
  m_VectorOfEccentrities90.resize( m_NumberOfVertices );
  m_VectorOfAveragePathLengths.resize( m_NumberOfVertices );

  //assign diameter and radius while iterating over the ecccencirities.
  m_Diameter              = 0;
  m_Diameter90            = 0;
  m_Radius                = std::numeric_limits<unsigned int>::max();
  m_Radius90              = std::numeric_limits<unsigned int>::max();
  m_AverageEccentricity   = 0.0;
  m_AverageEccentricity90 = 0.0;
  m_AveragePathLength     = 0.0;

  //The size of the giant connected component so far.
  unsigned int giant_component_size = 0;
  VertexDescriptorType radius_src(0);

  //Loop over the vertices
  for( boost::tie(vi, vi_end) = boost::vertices( *(m_Network->GetBoostGraph()) ); vi!=vi_end; ++vi)
  {
    //We are going to start a BFS, initialize the neccessary
    //structures for that.  Store the distances of nodes from the
    //source in distance vector. The maximum distance is stored in
    //max. The BFS will start from the node that vi is pointing, that
    //is the src is *vi. We also init the distance of the src node to
    //itself to 0. size gives the number of nodes discovered during
    //this BFS.
    std::vector<int> distances( m_NumberOfVertices );
    int max_distance = 0;
    VertexDescriptorType src = *vi;
    distances[src] = 0;
    unsigned int size = 0;

    breadth_first_search(*(m_Network->GetBoostGraph()), src,
      visitor(all_pairs_shortest_recorder<NetworkType>
      (&distances[0], max_distance, size)));
    // vertex vi has eccentricity equal to max_distance
    m_VectorOfEccentrities[src] = max_distance;

    //check whether there is any change in the diameter or the radius.
    //note that the diameter we are calculating here is also the
    //diameter of the giant connected component!
    if(m_VectorOfEccentrities[src] > m_Diameter)
    {
      m_Diameter = m_VectorOfEccentrities[src];
    }

    //The radius should be calculated on the largest connected
    //component, otherwise it is very likely that radius will be 1.
    //We change the value of the radius only if this is the giant
    //connected component so far. After all the eccentricities are
    //found we should loop over this connected component and find the
    //minimum eccentricity which is the radius. So we keep the src
    //node, so that we can find the connected component later on.
    if(size > giant_component_size)
    {
      giant_component_size = size;
      radius_src = src;
    }

    //Calculate in how many hops we can reach 90 percent of the
    //nodes. We store the number of hops we can reach in h hops in the
    //bucket vector. That is bucket[h] gives the number of nodes
    //reachable in exactly h hops. sum of bucket[i<h] gives the number
    //of nodes that are reachable in less than h hops. We also
    //calculate sum of the distances from this node to every single
    //other node in the graph.
    int reachable90 = std::ceil((double)size * 0.9);
    std::vector <int> bucket (max_distance+1);
    int counter = 0;
    for(unsigned int i=0; i<distances.size(); i++)
    {
      if(distances[i]>0)
      {
        bucket[distances[i]]++;
        m_VectorOfAveragePathLengths[src] += distances[i];
        counter ++;
      }
    }
    if(counter > 0)
    {
      m_VectorOfAveragePathLengths[src] = m_VectorOfAveragePathLengths[src] / counter;
    }

    int eccentricity90 = 0;
    while(reachable90 > 0)
    {
      eccentricity90 ++;
      reachable90 = reachable90 - bucket[eccentricity90];
    }
    // vertex vi has eccentricity90 equal to eccentricity90
    m_VectorOfEccentrities90[src] = eccentricity90;
    if(m_VectorOfEccentrities90[src] > m_Diameter90)
    {
      m_Diameter90 = m_VectorOfEccentrities90[src];
    }
  }

  //We are going to calculate the radius now. We stored the src node
  //that when we start a BFS gives the giant connected component, and
  //we have the eccentricities calculated. Iterate over the nodes of
  //this giant component and find the minimum eccentricity.
  std::vector<int> component( m_NumberOfVertices );
  boost::connected_components( *(m_Network->GetBoostGraph()), &component[0]);
  for (unsigned int i=0; i<component.size(); i++)
  {
    //If we are in the same component and the radius is not the
    //minimum so far store the eccentricity as the radius.
    if( component[i] == component[radius_src])
    {
      if(m_Radius > m_VectorOfEccentrities[i])
      {
        m_Radius = m_VectorOfEccentrities[i];
      }
      if(m_Radius90 > m_VectorOfEccentrities90[i])
      {
        m_Radius90 = m_VectorOfEccentrities90[i];
      }
    }
  }

  m_AverageEccentricity = std::accumulate(m_VectorOfEccentrities.begin(),
    m_VectorOfEccentrities.end(), 0.0) / m_NumberOfVertices;

  m_AverageEccentricity90 = std::accumulate(m_VectorOfEccentrities90.begin(),
    m_VectorOfEccentrities90.end(), 0.0) / m_NumberOfVertices;

  m_AveragePathLength = std::accumulate(m_VectorOfAveragePathLengths.begin(),
    m_VectorOfAveragePathLengths.end(), 0.0) / m_NumberOfVertices;

  //calculate Number of Central Points, nodes having eccentricity = radius.
  m_NumberOfCentralPoints = 0;
  for (boost::tie(vi, vi_end) = boost::vertices( *(m_Network->GetBoostGraph()) ); vi != vi_end; ++vi)
  {
    if(m_VectorOfEccentrities[*vi] == m_Radius)
    {
      m_NumberOfCentralPoints++;
    }
  }
  m_RatioOfCentralPoints = (double)m_NumberOfCentralPoints / m_NumberOfVertices;
}

void mitk::ConnectomicsStatisticsCalculator::CalculateSpectralMetrics()
{
  mitk::ConnectomicsNetworkConverter::Pointer converter = mitk::ConnectomicsNetworkConverter::New();
  converter->SetNetwork( m_Network );
  vnl_matrix<double> adjacencyMatrix = converter->GetNetworkAsVNLAdjacencyMatrix();

  vnl_symmetric_eigensystem<double> eigenSystem(adjacencyMatrix);

  m_AdjacencyTrace = 0;
  m_AdjacencyEnergy = 0;
  m_VectorOfSortedEigenValues.clear();

  for(unsigned int i=0; i < m_NumberOfVertices; ++i)
  {
    double value = std::fabs(eigenSystem.get_eigenvalue(i));
    m_VectorOfSortedEigenValues.push_back(value);
    m_AdjacencyTrace += value;
    m_AdjacencyEnergy += value * value;
  }

  std::sort(m_VectorOfSortedEigenValues.begin(), m_VectorOfSortedEigenValues.end());

  m_SpectralRadius = m_VectorOfSortedEigenValues[ m_NumberOfVertices - 1];
  m_SecondLargestEigenValue  = m_VectorOfSortedEigenValues[ m_NumberOfVertices - 2];
}

void mitk::ConnectomicsStatisticsCalculator::CalculateLaplacianMetrics()
{
  mitk::ConnectomicsNetworkConverter::Pointer converter = mitk::ConnectomicsNetworkConverter::New();
  converter->SetNetwork( m_Network );
  vnl_matrix<double> adjacencyMatrix = converter->GetNetworkAsVNLAdjacencyMatrix();
  vnl_matrix<double> laplacianMatrix ( m_NumberOfVertices, m_NumberOfVertices, 0);
  vnl_matrix<double> degreeMatrix = converter->GetNetworkAsVNLDegreeMatrix();

  m_VectorOfSortedLaplacianEigenValues.clear();
  laplacianMatrix = degreeMatrix - adjacencyMatrix;
  int numberOfConnectedComponents = 0;
  vnl_symmetric_eigensystem <double> laplacianEigenSystem( laplacianMatrix );
  m_LaplacianEnergy = 0;
  m_LaplacianTrace  = 0;
  for(unsigned int i(0); i < m_NumberOfVertices; ++i)
  {
    double value = std::fabs( laplacianEigenSystem.get_eigenvalue(i) );
    m_VectorOfSortedLaplacianEigenValues.push_back( value );
    m_LaplacianTrace += value;
    m_LaplacianEnergy += value * value;
    if ( std::fabs( value ) < mitk::eps )
    {
      numberOfConnectedComponents++;
    }
  }

  std::sort(m_VectorOfSortedLaplacianEigenValues.begin(), m_VectorOfSortedLaplacianEigenValues.end());
  for(unsigned int i(0); i < m_VectorOfSortedLaplacianEigenValues.size(); ++i)
  {
    if(m_VectorOfSortedLaplacianEigenValues[i] > mitk::eps )
    {
      m_LaplacianSpectralGap = m_VectorOfSortedLaplacianEigenValues[i];
      break;
    }
  }
}

void  mitk::ConnectomicsStatisticsCalculator::CalculateNormalizedLaplacianMetrics()
{
  vnl_matrix<double> normalizedLaplacianMatrix(m_NumberOfVertices, m_NumberOfVertices, 0);
  EdgeIteratorType ei, ei_end;
  VertexDescriptorType sourceVertex, destinationVertex;
  int sourceIndex, destinationIndex;
  VertexIndexMapType vertexIndexMap = boost::get(boost::vertex_index, *(m_Network->GetBoostGraph()) );
  m_VectorOfSortedNormalizedLaplacianEigenValues.clear();

  // Normalized laplacian matrix
  for( boost::tie(ei, ei_end) = boost::edges( *(m_Network->GetBoostGraph()) ); ei != ei_end; ++ei)
  {
    sourceVertex = boost::source(*ei, *(m_Network->GetBoostGraph()) );
    sourceIndex = vertexIndexMap[sourceVertex];

    destinationVertex = boost::target(*ei, *(m_Network->GetBoostGraph()) );
    destinationIndex = vertexIndexMap[destinationVertex];
    int sourceDegree = boost::out_degree(sourceVertex, *(m_Network->GetBoostGraph()) );
    int destinationDegree = boost::out_degree(destinationVertex, *(m_Network->GetBoostGraph()) );

    normalizedLaplacianMatrix.put(
      sourceIndex, destinationIndex, -1 / (sqrt(double(sourceDegree * destinationDegree))));
    normalizedLaplacianMatrix.put(
      destinationIndex, sourceIndex, -1 / (sqrt(double(sourceDegree * destinationDegree))));
  }

  VertexIteratorType vi, vi_end;
  for(boost::tie(vi, vi_end)=boost::vertices( *(m_Network->GetBoostGraph()) ); vi!=vi_end; ++vi)
  {
    if(boost::out_degree(*vi, *(m_Network->GetBoostGraph()) ) > 0)
    {
      normalizedLaplacianMatrix.put(vertexIndexMap[*vi], vertexIndexMap[*vi], 1);
    }
  }
  //End of normalized laplacian matrix definition

  vnl_symmetric_eigensystem <double>
    normalizedLaplacianEigensystem(normalizedLaplacianMatrix);

  double N1=0, C1=0, D1=0, E1=0, F1=0, b1=0;
  double N2=0, C2=0, D2=0, E2=0, F2=0, b2=0;
  m_NormalizedLaplacianNumberOf2s = 0;
  m_NormalizedLaplacianNumberOf1s = 0;
  m_NormalizedLaplacianNumberOf0s = 0;
  m_NormalizedLaplacianTrace = 0;
  m_NormalizedLaplacianEnergy = 0;

  for(unsigned int i(0); i< m_NumberOfVertices; ++i)
  {
    double eigenValue = std::fabs(normalizedLaplacianEigensystem.get_eigenvalue(i));
    m_VectorOfSortedNormalizedLaplacianEigenValues.push_back(eigenValue);
    m_NormalizedLaplacianTrace  += eigenValue;
    m_NormalizedLaplacianEnergy += eigenValue * eigenValue;

    //0
    if(eigenValue < mitk::eps)
    {
      m_NormalizedLaplacianNumberOf0s++;
    }

    //Between 0 and 1.
    else if(eigenValue > mitk::eps && eigenValue< 1 - mitk::eps)
    {
      C1 += i;
      D1 += eigenValue;
      E1 += i * eigenValue;
      F1 += i * i;
      N1 ++;
    }

    //1
    else if(std::fabs( std::fabs(eigenValue) - 1) < mitk::eps)
    {
      m_NormalizedLaplacianNumberOf1s++;
    }

    //Between 1 and 2
    else if(std::fabs(eigenValue) > 1+mitk::eps && std::fabs(eigenValue)< 2 - mitk::eps)
    {
      C2 += i;
      D2 += eigenValue;
      E2 += i * eigenValue;
      F2 += i * i;
      N2 ++;
    }

    //2
    else if(std::fabs( std::fabs(eigenValue) - 2) < mitk::eps)
    {
      m_NormalizedLaplacianNumberOf2s++;
    }
  }

  b1 = (D1*F1 - C1*E1)/(F1*N1 - C1*C1);
  m_NormalizedLaplacianLowerSlope = (E1 - b1*C1)/F1;

  b2 = (D2*F2 - C2*E2)/(F2*N2 - C2*C2);
  m_NormalizedLaplacianUpperSlope = (E2 - b2*C2)/F2;
}

void mitk::ConnectomicsStatisticsCalculator::CalculateSmallWorldness()
{
  double k( this->GetAverageDegree() );
  double N( this->GetNumberOfVertices() );
  // The clustering coefficient of an Erdos-Reny network is equivalent to
  // the likelihood two random nodes are connected
  double gamma = this->GetAverageClusteringCoefficientsC() / ( k / N );
  //The mean path length of an Erdos-Reny network is approximately
  // ln( #vertices ) / ln( average degree )
  double lambda = this->GetAveragePathLength() / ( std::log( N ) / std::log( k ) );

  m_SmallWorldness = gamma / lambda;
}


//          Copyright (C) 2012, Michele Caini.
// 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)

//          Two Graphs Common Spanning Trees Algorithm
//      Based on academic article of Mint, Read and Tarjan
//     Efficient Algorithm for Common Spanning Tree Problem
// Electron. Lett., 28 April 1983, Volume 19, Issue 9, p.346-347

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/two_graphs_common_spanning_trees.hpp>
#include <exception>
#include <vector>

using namespace std;

typedef
boost::adjacency_list
  <
    boost::vecS,         // OutEdgeList
    boost::vecS,         // VertexList
    boost::undirectedS,  // Directed
    boost::no_property,  // VertexProperties
    boost::no_property,  // EdgeProperties
    boost::no_property,  // GraphProperties
    boost::listS         // EdgeList
  >
Graph
;

typedef
boost::graph_traits<Graph>::vertex_descriptor
vertex_descriptor;

typedef
boost::graph_traits<Graph>::edge_descriptor
edge_descriptor;

typedef
boost::graph_traits<Graph>::vertex_iterator
vertex_iterator;

typedef
boost::graph_traits<Graph>::edge_iterator
edge_iterator;

int main(int argc, char **argv)
{
  Graph iG, vG;
  vector< edge_descriptor > iG_o;
  vector< edge_descriptor > vG_o;

  iG_o.push_back(boost::add_edge(0, 1, iG).first);
  iG_o.push_back(boost::add_edge(0, 2, iG).first);
  iG_o.push_back(boost::add_edge(0, 3, iG).first);
  iG_o.push_back(boost::add_edge(0, 4, iG).first);
  iG_o.push_back(boost::add_edge(1, 2, iG).first);
  iG_o.push_back(boost::add_edge(3, 4, iG).first);

  vG_o.push_back(boost::add_edge(1, 2, vG).first);
  vG_o.push_back(boost::add_edge(2, 0, vG).first);
  vG_o.push_back(boost::add_edge(2, 3, vG).first);
  vG_o.push_back(boost::add_edge(4, 3, vG).first);
  vG_o.push_back(boost::add_edge(0, 3, vG).first);
  vG_o.push_back(boost::add_edge(0, 4, vG).first);

  vector<bool> inL(iG_o.size(), false);

  std::vector< std::vector<bool> > coll;
  boost::tree_collector<
      std::vector< std::vector<bool> >,
      std::vector<bool>
    > tree_collector(coll);
  boost::two_graphs_common_spanning_trees
    (
      iG,
      iG_o,
      vG,
      vG_o,
      tree_collector,
      inL
    );

  std::vector< std::vector<bool> >::iterator it;
  for(it = coll.begin(); it != coll.end(); ++it) {
    // Here you can play with the trees that the algorithm has found.
  }

  return 0;
}


#include "pybind11/pybind11.h"

#include "xtensor/xmath.hpp"
#include "xtensor/xarray.hpp"
#include <boost/pending/disjoint_sets.hpp>

#define FORCE_IMPORT_ARRAY
#include "xtensor-python/pytensor.hpp"
#include "xtensor-python/pyarray.hpp"

#include <iostream>
#include <numeric>
#include <cmath>

namespace py = pybind11;

xt::pyarray<uint64_t> connected_components(const xt::pytensor<bool, 2> & image)
{
    typedef typename xt::pytensor<bool, 2>::shape_type IndexType;
    const auto & shape = image.shape();
    const std::size_t n_nodes = shape[0] * shape[1];

    // make union find and map seeds to reperesentatives
    std::vector<uint64_t> ranks(n_nodes);
    std::vector<uint64_t> parents(n_nodes);
    boost::disjoint_sets<uint64_t*, uint64_t*> ufd(&ranks[0], &parents[0]);
    for(uint64_t node = 0; node < n_nodes; ++node) {
        ufd.make_set(node);
    }

    // flatten the image
    auto flat_image = xt::flatten(image);

    const int n_ngbs = 4;
    std::vector<int> shifts_x = {-1, 1, 0, 0};
    std::vector<int> shifts_y = {0, 0, -1, 1};

    // run connected components
    for(uint64_t u = 0; u < n_nodes; ++u) {
        if(flat_image[u] == 0) {
            continue;
        }

        // get representative
        const uint64_t ru = ufd.find_set(u);

        const auto coordinate = xt::unravel_index(u, shape);
        std::vector<IndexType> neighbor_coords;

        // make the neighbors
        for(unsigned ngb = 0; ngb < n_ngbs; ++ngb) {
            const int sx = shifts_x[ngb];
            const int sy = shifts_y[ngb];
            const int64_t x = coordinate[0] + sx;
            const int64_t y = coordinate[1] + sy;

            // bounds check
            if(x < 0 || x >= shape[0]) {
                continue;
            }
            if(y < 0 || y >= shape[1]) {
                continue;
            }

            neighbor_coords.emplace_back(IndexType({x, y}));
        }

        const auto neighbors = xt::ravel_indices(neighbor_coords, shape);
        // iterate over the neighbors
        for(const uint64_t v: neighbors) {
            if(flat_image[v] == 0) {
                continue;
            }

            const uint64_t rv = ufd.find_set(v);
            if(ru == rv) {
                continue;
            }

            ufd.link(ru, rv);
        }
    }

    xt::pyarray<uint64_t> seg = xt::zeros<uint64_t>({n_nodes});
    for(uint64_t u = 0; u < n_nodes; ++u) {
        seg[u] = ufd.find_set(u);
    }
    seg.reshape(shape);
    return seg;
}

PYBIND11_MODULE(ccxt, m)
{
    xt::import_numpy();

    m.doc() = R"pbdoc(
        ccxt

        .. currentmodule:: ccxt

        .. autosummary::
           :toctree: _generate

           connected_components
    )pbdoc";
    m.def("connected_components", connected_components, "compute connected_components");
}


/*
 * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT)
 *
 * This software may be modified and distributed under the terms of the
 * BSD 3-Clause license. See the accompanying LICENSE file for details.
 */

#include <BayesFilters/LinearStateModel.h>

#include <Eigen/Dense>

using namespace bfl;
using namespace Eigen;

void LinearStateModel::propagate(const Eigen::Ref<const Eigen::MatrixXd>& cur_states, Eigen::Ref<Eigen::MatrixXd> prop_states)
{
    if (is_skipping() && (have_exogenous_model() && exogenous_model().is_skipping()))
    {
        prop_states = cur_states;
    }
    else if (!is_skipping() && (have_exogenous_model() && !exogenous_model().is_skipping()))
    {
        MatrixXd exogenous_state(cur_states.rows(), cur_states.cols());

        exogenous_model().propagate(cur_states, exogenous_state);

        prop_states = getStateTransitionMatrix() * cur_states + exogenous_state;
    }
    else if (!is_skipping())
    {
        prop_states = getStateTransitionMatrix() * cur_states;
    }
    else if (have_exogenous_model() && !exogenous_model().is_skipping())
    {
        exogenous_model().propagate(cur_states, prop_states);
    }
}


#include "adjust_orientation_LBFGS.h"
#include <Eigen/Dense>
#include <Eigen/Sparse>
using Eigen::Matrix3d;
using Eigen::Vector3d;
void evalfunc_ad_LBFGS(int N, double* x, double *prev_x, double* f, double* g, void* user_pointer)
{
	double alpha = x[0]; double beta = x[1]; double gamma = x[2];
	double cos_alpha = std::cos(alpha); double sin_alpha = std::sin(alpha);
	double cos_beta = std::cos(beta); double sin_beta = std::sin(beta);
	double cos_gamma = std::cos(gamma); double sin_gamma = std::sin(gamma);
	double r00 = cos_alpha*cos_gamma - cos_beta*sin_alpha*sin_gamma;
	double r01 = sin_alpha*cos_gamma + cos_beta*cos_alpha*sin_gamma;
	double r02 = sin_beta*sin_gamma;
	double r10 = -cos_alpha*sin_gamma - cos_beta*sin_alpha*cos_gamma;
	double r11 = -sin_alpha*sin_gamma + cos_beta*cos_alpha*cos_gamma;
	double r12 = sin_beta*cos_gamma;
	double r20 = sin_beta*sin_alpha;
	double r21 = -sin_beta*cos_alpha;
	double r22 = cos_beta;

	user_pointer_ad* up = (user_pointer_ad*)(user_pointer);
	int nbf = (int)up->bfn.size(); *f = 0.0;
	g[0] = 0; g[1] = 0; g[2] = 0;
	for (int i = 0; i < nbf; ++i)
	{
		OpenVolumeMesh::Geometry::Vec3d& n = up->bfn[i];
		//OpenVolumeMesh::Geometry::Vec3d n(4.0, 5.0, 6.0);
		double nx = r00 * n[0] + r01 * n[1] + r02 * n[2];
		double ny = r10 * n[0] + r11 * n[1] + r12 * n[2];
		double nz = r20 * n[0] + r21 * n[1] + r22 * n[2];
		*f += (nx*ny)*(nx*ny) + (ny*nz)*(ny*nz) + (nz*nx)*(nz*nx);
		double d_nx_alpha = (-sin_alpha*cos_gamma - cos_beta*cos_alpha*sin_gamma) * n[0] + (cos_alpha*cos_gamma - cos_beta*sin_alpha*sin_gamma) * n[1] + 0 * n[2];
		double d_ny_alpha = (sin_alpha*sin_gamma - cos_beta*cos_alpha*cos_gamma) * n[0] + (-cos_alpha*sin_gamma - cos_beta*sin_alpha*cos_gamma) * n[1] + 0 * n[2];
		double d_nz_alpha = sin_beta*cos_alpha * n[0] + sin_beta*sin_alpha * n[1] + 0 * n[2];
		double d_nx_beta = (sin_beta*sin_alpha*sin_gamma) * n[0] + (-sin_beta*cos_alpha*sin_gamma) * n[1] + cos_beta*sin_gamma * n[2];
		double d_ny_beta = (sin_beta*sin_alpha*cos_gamma) * n[0] + (-sin_beta*cos_alpha*cos_gamma) * n[1] + cos_beta*cos_gamma * n[2];
		double d_nz_beta = cos_beta*sin_alpha *n[0] - cos_beta*cos_alpha * n[1] - sin_beta * n[2];
		double d_nx_gamma = (-cos_alpha*sin_gamma - cos_beta*sin_alpha*cos_gamma) * n[0] + (-sin_alpha*sin_gamma + cos_beta*cos_alpha*cos_gamma) * n[1] + sin_beta*cos_gamma * n[2];
		double d_ny_gamma = (-cos_alpha*cos_gamma + cos_beta*sin_alpha*sin_gamma) * n[0] + (-sin_alpha*cos_gamma - cos_beta*cos_alpha*sin_gamma) * n[1] + (-sin_beta*sin_gamma) * n[2];
		double d_nz_gamma = 0.0;
		g[0] += 2 * (nx*ny)*(d_nx_alpha*ny + nx*d_ny_alpha) + 2 * (ny*nz)*(d_ny_alpha*nz + ny*d_nz_alpha) + 2 * (nz*nx)*(d_nz_alpha*nx + nz*d_nx_alpha);
		g[1] += 2 * (nx*ny)*(d_nx_beta*ny + nx*d_ny_beta) + 2 * (ny*nz)*(d_ny_beta*nz + ny*d_nz_beta) + 2 * (nz*nx)*(d_nz_beta*nx + nz*d_nx_beta);
		g[2] += 2 * (nx*ny)*(d_nx_gamma*ny + nx*d_ny_gamma) + 2 * (ny*nz)*(d_ny_gamma*nz + ny*d_nz_gamma) + 2 * (nz*nx)*(d_nz_gamma*nx + nz*d_nx_gamma);
		std::cout << "g: " << g[0] << " " << g[1] << " " << g[2] << std::endl;
	}
}
void evalfunc_ad_LBFGS_xyz(int N, double* x, double *prev_x, double* f, double* g, void* user_pointer)
{
	//x y z axis rotation
	double cos_phi = std::cos(x[0]), sin_phi = std::sin(x[0]);
	double cos_theta = std::cos(x[1]), sin_theta = std::sin(x[1]);
	double cos_xi = std::cos(x[2]), sin_xi = std::sin(x[2]);
	Matrix3d Mtheta, Mphi, Msi, DMtheta, DMphi, DMsi;
	Mtheta.setZero(); Mphi.setZero(); Msi.setZero();
	Mphi(0,0) = 1; Mphi(1,1) = Mphi(2,2) = cos_phi, Mphi(1, 2) = sin_phi, Mphi(2, 1) = -sin_phi;
	Mtheta(1, 1) = 1; Mtheta(0,0) = Mtheta(2,2) = cos_theta, Mtheta(2,0) = sin_theta, Mtheta(0, 2) = -sin_theta;
	Msi(2, 2) = 1; Msi(0, 0) = Msi(1, 1) = cos_xi, Msi(0, 1) = sin_xi, Msi(1, 0) = -sin_xi;
	DMtheta.setZero(); DMphi.setZero(); DMsi.setZero();
	DMphi(1, 1) = DMphi(2, 2) = -sin_phi, DMphi(1, 2) = cos_phi, DMphi(2, 1) = -cos_phi;
	DMtheta(0, 0) = DMtheta(2, 2) = -sin_theta, DMtheta(2, 0) = cos_theta, DMtheta(0, 2) = -cos_theta;
	DMsi(0, 0) = DMsi(1, 1) = -sin_xi, DMsi(0, 1) = cos_xi, DMsi(1,0) = -cos_xi;
	Matrix3d M = Msi * Mphi * Mtheta, dMsi = DMsi * Mphi * Mtheta, dMphi = Msi * DMphi * Mtheta, dMtheta = Msi * Mphi * DMtheta;
	Vector3d DN, diffN_theta, diffN_phi, diffN_si;
	double xy, yz, zx;
	user_pointer_ad* up = (user_pointer_ad*)(user_pointer);
	int nbf = (int)up->bfn.size(); *f = 0.0;
	g[0] = 0; g[1] = 0; g[2] = 0;
	for (int i = 0; i < nbf; ++i)
	{
		OpenVolumeMesh::Geometry::Vec3d& n = up->bfn[i];
		Vector3d N(n[0], n[1], n[2]);
		//Vector3d  N(1, 2, 3);
		DN = M * N;
		diffN_theta = dMtheta * N;
		diffN_phi = dMphi * N;
		diffN_si = dMsi * N;
		xy = DN[0] * DN[1];
		yz = DN[1] * DN[2];
		zx = DN[2] * DN[0];
		*f += (xy * xy + yz * yz + zx * zx) / 2;
		g[0] += xy * (diffN_phi[0] * DN[1] + DN[0] * diffN_phi[1]) +
			yz * (diffN_phi[1] * DN[2] + DN[1] * diffN_phi[2]) +
			zx * (diffN_phi[2] * DN[0] + DN[2] * diffN_phi[0]);
		g[1] += xy * (diffN_theta[0] * DN[1] + DN[0] * diffN_theta[1]) +
			yz * (diffN_theta[1] * DN[2] + DN[1] * diffN_theta[2]) +
			zx * (diffN_theta[2] * DN[0] + DN[2] * diffN_theta[0]);
		g[2] += xy * (diffN_si[0] * DN[1] + DN[0] * diffN_si[1]) +
			yz * (diffN_si[1] * DN[2] + DN[1] * diffN_si[2]) +
			zx * (diffN_si[2] * DN[0] + DN[2] * diffN_si[0]);
		//std::cout << "f: " << *f << " g: " << g[0] << " " << g[1] << " " << g[2] << std::endl;
	}

	//double alpha = x[0]; double beta = x[1]; double gamma = x[2];
	//double cos_alpha = std::cos(alpha); double sin_alpha = std::sin(alpha);
	//double cos_beta = std::cos(beta); double sin_beta = std::sin(beta);
	//double cos_gamma = std::cos(gamma); double sin_gamma = std::sin(gamma);
	//double r00 = cos_alpha * cos_gamma - cos_beta * sin_alpha*sin_gamma;
	//double r01 = sin_alpha * cos_gamma + cos_beta * cos_alpha*sin_gamma;
	//double r02 = sin_beta * sin_gamma;
	//double r10 = -cos_alpha * sin_gamma - cos_beta * sin_alpha*cos_gamma;
	//double r11 = -sin_alpha * sin_gamma + cos_beta * cos_alpha*cos_gamma;
	//double r12 = sin_beta * cos_gamma;
	//double r20 = sin_beta * sin_alpha;
	//double r21 = -sin_beta * cos_alpha;
	//double r22 = cos_beta;
	//user_pointer_ad* up = (user_pointer_ad*)(user_pointer);
	//int nbf = up->bfn.size(); *f = 0.0;
	//g[0] = 0; g[1] = 0; g[2] = 0;
	//for (int i = 0; i < nbf; ++i)
	//{
	//	OpenVolumeMesh::Geometry::Vec3d& n = up->bfn[i];
	//	double nx = r00 * n[0] + r01 * n[1] + r02 * n[2];
	//	double ny = r10 * n[0] + r11 * n[1] + r12 * n[2];
	//	double nz = r20 * n[0] + r21 * n[1] + r22 * n[2];
	//	//*f += (nx*ny)*(nx*ny) + (ny*nz)*(ny*nz) + (nz*nx)*(nz*nx);
	//	*f += nx * nx * nx * nx + ny * ny * ny * ny + nz * nz * nz * nz;
	//	double d_nx_alpha = (-sin_alpha * cos_gamma - cos_beta * cos_alpha*sin_gamma) * n[0] + (cos_alpha*cos_gamma - cos_beta * sin_alpha*sin_gamma) * n[1] + 0 * n[2];
	//	double d_ny_alpha = (sin_alpha*sin_gamma - cos_beta * cos_alpha*cos_gamma) * n[0] + (-cos_alpha * sin_gamma - cos_beta * sin_alpha*cos_gamma) * n[1] + 0 * n[2];
	//	double d_nz_alpha = sin_beta * cos_alpha * n[0] + sin_beta * sin_alpha * n[1] + 0 * n[2];
	//	double d_nx_beta = (sin_beta*sin_alpha*sin_gamma) * n[0] + (-sin_beta * cos_alpha*sin_gamma) * n[1] + cos_beta * sin_gamma * n[2];
	//	double d_ny_beta = (sin_beta*sin_alpha*cos_gamma) * n[0] + (-sin_beta * cos_alpha*cos_gamma) * n[1] + cos_beta * cos_gamma * n[2];
	//	double d_nz_beta = cos_beta * sin_alpha *n[0] - cos_beta * cos_alpha * n[1] - sin_beta * n[2];
	//	double d_nx_gamma = (-cos_alpha * sin_gamma - cos_beta * sin_alpha*cos_gamma) * n[0] + (-sin_alpha * sin_gamma + cos_beta * cos_alpha*cos_gamma) * n[1] + sin_beta * cos_gamma * n[2];
	//	double d_ny_gamma = (-cos_alpha * cos_gamma + cos_beta * sin_alpha*sin_gamma) * n[0] + (-sin_alpha * cos_gamma - cos_beta * cos_alpha*sin_gamma) * n[1] + (-sin_beta * sin_gamma) * n[2];
	//	double d_nz_gamma = 0.0;
	//	/*g[0] += 2 * (nx*ny)*(d_nx_alpha*ny + nx * d_ny_alpha) + 2 * (ny*nz)*(d_ny_alpha*nz + ny * d_nz_alpha) + 2 * (nz*nx)*(d_nz_alpha*nx + nz * d_nx_alpha);
	//	g[1] += 2 * (nx*ny)*(d_nx_beta*ny + nx * d_ny_beta) + 2 * (ny*nz)*(d_ny_beta*nz + ny * d_nz_beta) + 2 * (nz*nx)*(d_nz_beta*nx + nz * d_nx_beta);
	//	g[2] += 2 * (nx*ny)*(d_nx_gamma*ny + nx * d_ny_gamma) + 2 * (ny*nz)*(d_ny_gamma*nz + ny * d_nz_gamma) + 2 * (nz*nx)*(d_nz_gamma*nx + nz * d_nx_gamma);*/
	//	g[0] += 4 * nx * nx * nx * (d_nx_alpha) + 4 * ny * ny * ny * (d_ny_alpha) + 4 * nz * nz * nz * (d_nz_alpha);
	//	g[1] += 4 * nx * nx * nx * (d_nx_beta) + 4 * ny * ny * ny * (d_ny_beta) + 4 * nz * nz * nz * (d_nz_beta);
	//	g[2] += 4 * nx * nx * nx * (d_nx_gamma) + 4 * ny * ny * ny * (d_ny_gamma) + 4 * nz * nz * nz * (d_nz_gamma);
	//}
}
void newiteration_ad_LBFGS(int iter, int call_iter, double *x, double* f, double *g, double* gnorm, void* user_pointer)
{
	printf("%d %d; %4.3e, %4.3e\n", iter, call_iter, f[0], gnorm[0]);
}
bool ad_LBFGS(std::vector<OpenVolumeMesh::Geometry::Vec3d>& bfn, std::vector<double>& X)
{
	user_pointer_ad up;
	up.bfn = bfn;
	//LBFGS
	double parameter[20];
	int info[20];
	//initialize
	INIT_HLBFGS(parameter, info);
	parameter[2] = 0.9;
	info[4] = 100; //number of iteration
	info[6] = 0;
	info[7] = 0; //if with hessian 1, without 0
	info[10] = 0;
	int N = 3; int M = 7;
	//m is the number of history value
	//n is the number of variables
	printf("-------------------------------\n");
	printf("start LBFGS\n");
	//function change to nx^4 + ny^4 + nz^ 4?
	//HLBFGS(N, M, &X[0], evalfunc_ad_LBFGS, 0, HLBFGS_UPDATE_Hessian, newiteration_ad_LBFGS, parameter, info, &up);
	HLBFGS(N, M, &X[0], evalfunc_ad_LBFGS_xyz, 0, HLBFGS_UPDATE_Hessian, newiteration_ad_LBFGS, parameter, info, &up);
	return true;
}