/*
 * Copyright Shichao Yang,2016, Carnegie Mellon University
 * Email: shichaoy@andrew.cmu.edu
 */

#include "Mapping.h"
#include "Frame.h"
#include "Map_plane.h"
#include "isam_plane3d.h"

#include "pop_up_wall/matrix_utils.h"
#include "pop_up_wall/popup_plane.h"

#include <Eigen/Dense>
#include "ros/ros.h"

using namespace std;
using namespace isam;
using namespace Eigen;

const bool useRelative = false;

double tictoc_t0;
void _tic() {
  tictoc_t0 = pcl::getTime();
}
void _toc(const string& s) {
  cout << "Time for " << s << ": " << pcl::getTime() - tictoc_t0 << endl;
}

Mapper_mono::Mapper_mono():nn( "~" ) {
    Properties prop = _slam.properties();
     prop.method = LEVENBERG_MARQUARDT;  //defalut is gauss-newton
  //  prop.method = DOG_LEG;
    prop.mod_batch = 1;
    prop.quiet=true;  // print optimization
    prop.epsilon2*=0.1;
    prop.epsilon_abs*=0.1;  // more iterations
    prop.epsilon_rel*=0.1;
    /** Termination criterion:  stop whenever the /difference/ in absolute
    * sum-of-squares errors between two estimates falls below this fraction
    * of the /total/ absolute sum-of-squares errors. */
    _slam.set_properties(prop);

    nn.param ("/planar_test/package_path", package_path, package_path);
    nn.param ("/planar_test/edge_asso_2ddist", edge_asso_2ddist, edge_asso_2ddist);
    nn.param ("/planar_test/edge_asso_proj", edge_asso_proj, edge_asso_proj);
    nn.param ("/planar_test/edge_asso_angle", edge_asso_angle, edge_asso_angle);
    nn.param ("/planar_test/edge_asso_planedist", edge_asso_planedist, edge_asso_planedist);
    nn.param ("/planar_test/assoc_near_frames", assoc_near_frames, assoc_near_frames);
    nn.param ("/planar_test/print_assoc_detail", print_assoc_detail, print_assoc_detail);

    nn.param ("/planar_test/pose_sigma_x", pose_sigma_x, pose_sigma_x);
    nn.param ("/planar_test/pose_sigma_y", pose_sigma_y, pose_sigma_y);
    nn.param ("/planar_test/pose_sigma_z", pose_sigma_z, pose_sigma_z);
    nn.param ("/planar_test/pose_sigma_yaw", pose_sigma_yaw, pose_sigma_yaw);
    nn.param ("/planar_test/pose_sigma_pitch", pose_sigma_pitch, pose_sigma_pitch);
    nn.param ("/planar_test/pose_sigma_roll", pose_sigma_roll, pose_sigma_roll);
    nn.param ("/planar_test/plane_sigma_x", plane_sigma_x, plane_sigma_x);
    nn.param ("/planar_test/plane_sigma_y", plane_sigma_y, plane_sigma_y);
    nn.param ("/planar_test/plane_sigma_z", plane_sigma_z, plane_sigma_z);
    nn.param ("/planar_test/plane_sigma_dist_mul", plane_sigma_dist_mul, plane_sigma_dist_mul);

    poseSigmas<<pose_sigma_x,pose_sigma_y,pose_sigma_z,pose_sigma_yaw,pose_sigma_pitch,pose_sigma_roll;
    planeSigmas<<plane_sigma_x,plane_sigma_y,plane_sigma_z;
    poseCov = boost::shared_ptr<Covariance> ( new Covariance(poseSigmas.cwiseProduct(poseSigmas).asDiagonal()));
    planeCov = boost::shared_ptr<Covariance> ( new Covariance(planeSigmas.cwiseProduct(planeSigmas).asDiagonal()));

    nn.param ("/planar_test/use_loop_close", use_loop_close, use_loop_close);
}

Mapper_mono::~Mapper_mono()
{
    if (mpORBextractor!=nullptr)
      delete mpORBextractor;
    if (mpORBVocabulary!=nullptr)
      delete mpORBVocabulary;
}

void Mapper_mono::initialize_orb_matching()
{
    if (use_loop_close){
	string strVocFile=package_path+"/params/ORBvoc.bin";
	cout << endl << "Loading ORB Vocabulary. This could take a while." << endl;
	mpORBVocabulary=new ORB_SLAM::ORBVocabulary();

	bool bVocLoad = mpORBVocabulary->loadFromBinaryFile(strVocFile);

	if (bVocLoad)
	    cout << endl << "Finish Load ORB Vocabulary." << endl;
	else
	    ROS_ERROR("Cannod Load ORB Vocabulary.");
	assert(bVocLoad);
	string strSettingPath=package_path+"/params/ORB_Settings.yaml";
	cv::FileStorage fSettings(strSettingPath.c_str(), cv::FileStorage::READ);
	if(!fSettings.isOpened())
	    ROS_ERROR("Wrong path to settings. Path must be absolut or relative to ORB_SLAM package directory.");

	assert(fSettings.isOpened());
	int nFeatures = fSettings["ORBextractor.nFeatures"];
	float fScaleFactor = fSettings["ORBextractor.scaleFactor"];
	int nLevels = fSettings["ORBextractor.nLevels"];
	int fastTh = fSettings["ORBextractor.fastTh"];
	int Score = fSettings["ORBextractor.nScoreType"];
	assert(Score==1 || Score==0);
	mpORBextractor = new ORB_SLAM::ORBextractor(nFeatures,fScaleFactor,nLevels,Score,fastTh);
    }
}

float point_proj_to_lineseg(const Vector2f &begin_pt, const Vector2f &end_pt, const Vector2f &query_pt)
{  // v w p
      // Return minimum distance between line segment vw and point p
    float length = (end_pt-begin_pt).norm();
    if (length < 0.001) return (query_pt-begin_pt).norm();   // v == w case
    // Consider the line extending the segment, parameterized as v + t (w - v).
    // We find projection of point p onto the line.
    // It falls where t = [(p-v) . (w-v)] / |w-v|^2
    float t = ((query_pt-begin_pt).dot(end_pt-begin_pt))/length/length;
    if (t>1.0)
      return 1.0;
    if (t<0.0)
      return 0.0;
    return t;
}

vector<tracking_frame*> Mapper_mono::DetectLoopCandidates(tracking_frame* pKF)
{
      list<pair<float,tracking_frame*> > lScoreAndMatch;
      int nscores=0;
      // Compute similarity score. Retain the matches whose score is higher than minScore
      float minScore=0.01;  // initial ORB slam determines this number based on covisity graph?
      float bestAccScore=minScore;
      for (int i=0;i<50;i++)  // HACK. only search begin 50 frames?? refer to orb slam for better loop closure
      {
	  tracking_frame* pKFi=all_frames[i];
	  float si = mpORBVocabulary->score(pKF->mBowVec,pKFi->mBowVec);
	  if(si>=minScore){
	       lScoreAndMatch.push_back(make_pair(si,pKFi));
	       if(si>bestAccScore)
                   bestAccScore=si;  // update the best score
	  }
      }
      if(lScoreAndMatch.empty())
	  return vector<tracking_frame*>();

      list<pair<float,tracking_frame*> > lAccScoreAndMatch;
      // Return all those keyframes with a score higher than 0.75*bestScore
      float minScoreToRetain = 0.75f*bestAccScore;

      set<tracking_frame*> spAlreadyAddedKF;
      vector<tracking_frame*> vpLoopCandidates;
      vpLoopCandidates.reserve(lAccScoreAndMatch.size());

      for(list<pair<float,tracking_frame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)
      {
	  if(it->first>minScoreToRetain)
	  {
	      tracking_frame* pKFi = it->second;
	      if(!spAlreadyAddedKF.count(pKFi))
	      {
		  vpLoopCandidates.push_back(pKFi);
		  spAlreadyAddedKF.insert(pKFi);
	      }
	  }
      }
      return vpLoopCandidates;
}

// find the loop planes from the loop frame
std::tuple<int, double> Mapper_mono::findLoopPlane(const Pose3d& curr_esti_pose, const Map_plane* check_plane, int old_loop_frame_ind) {
      int numMatches = 0;
      int bestMatch = -1;
      double bestReprojError = -1;

      const Plane3d curr_plane_local=check_plane->plane_vertex->value();
      Plane3d curr_plane_world = curr_plane_local.transform_from(curr_esti_pose.oTw());  //  transform plane to global coordinate
      Eigen::Matrix4d curr_plane_pose_mat= curr_esti_pose.wTo();
      Eigen::MatrixXf curr_bounding_2d_poly=check_plane->plane_bound_close_2D_polys;

      int start_frame_id=std::max(old_loop_frame_ind-0,  0);   // search neighbour frames' planes
      int end_frame_id=std::min(old_loop_frame_ind+1,  (int)all_frames.size());   // better method is covisity graph to find connected frames
      bool plane_found=false;
      for (int frame_id = start_frame_id; frame_id < end_frame_id; frame_id++) {     // could speed up, only use recent frames
	   tracking_frame* potential_frame=all_frames[frame_id];
	   for (int plane_id=0;plane_id<potential_frame->observed_planes.size();plane_id++){
		int loop_cand_global_id=potential_frame->observed_planes[plane_id]->landmark_index;
		if (all_landmarks[loop_cand_global_id]->deteted_by_merge)  // if it is a dead node, being merged node, skip this
		    continue;
		if ( check_plane->frame_plane_indice==0 && all_landmarks[loop_cand_global_id]->frame_plane_indice==0 ){  // if both are ground, associate them immediately.
		      numMatches++;
		      bestMatch = loop_cand_global_id;
		      plane_found=true;
		      break;
		}
		if ( ( check_plane->frame_plane_indice==0 && all_landmarks[loop_cand_global_id]->frame_plane_indice>=1 )  ||    // if one is ground and the other is wall, must not be associated
		      ( check_plane->frame_plane_indice>=1 && all_landmarks[loop_cand_global_id]->frame_plane_indice==0 ) )
		      continue;   // skip this plane, check further plane

		if (all_landmarks[loop_cand_global_id]->being_tracked_times<10)
		     continue;

// 		Plane3d old_plane_world = _planeNodes[loop_cand_global_id]->value();
		cout<<"to test global id "<<all_landmarks[loop_cand_global_id]->landmark_index<<"  at frame "<<all_landmarks[loop_cand_global_id]->frame_seq_id<<" indice "<<all_landmarks[loop_cand_global_id]->good_plane_indice<<endl;
		Vector4f old_plane_2d_edge=potential_frame->ground_seg2d_lines.row( potential_frame->good_plane_indices(plane_id)-1);
		float poly_2d_dist=0;   //for loop closing, I think we can only use image based difference, bow or pixel difference
		for (int i=0;i<2;i++){
		      Vector2d distance_all;
		      for (int j=0;j<2;j++)
			  distance_all(j)=(curr_bounding_2d_poly.col(i)-old_plane_2d_edge.segment<2>(j*2)).norm();
		      poly_2d_dist+=distance_all.minCoeff();
		}
		poly_2d_dist=poly_2d_dist/2;
		float poly_2d_dist_cross=0;
		for (int i=0;i<2;i++){
		      Vector2f distance_all;
		      for (int j=0;j<2;j++)
			  distance_all(j)=(curr_bounding_2d_poly.col(j)-old_plane_2d_edge.segment<2>(i*2)).norm();
		      poly_2d_dist_cross+=distance_all.minCoeff();
		}
		poly_2d_dist_cross=poly_2d_dist_cross/2;
		float plane_2d_img_dis=poly_2d_dist+poly_2d_dist_cross;

		numMatches++;
		float total_error=plane_2d_img_dis;
//      	if (print_assoc_detail)
		    cout<<"total error "<<total_error<<endl;
		if (numMatches==1 || total_error<bestReprojError ){ // the smaller, the better  //different frame might say the same plane node, it doesn't matter.
		    bestMatch = loop_cand_global_id;
		    bestReprojError = total_error;   // weight combination of angle and 3d projection overlapping
		}
		continue;
	   }
	   if (plane_found)
	     break;
      }
//       if (numMatches > 1) {
// 	  cout << "Warning: multiple potential matches for one plane." << endl;
//       }
      if (0)
      {
	  if (bestMatch<0){
	      ROS_WARN_STREAM("Not finding matched planes, add as new landmark.");
	  }else{
	      ROS_WARN_STREAM("Final matches global at  "<<bestMatch<<"  in frame  "<< all_landmarks[bestMatch]->frame_seq_id <<"  at indice  "<<all_landmarks[bestMatch]->good_plane_indice);
	  }
      }
      return make_tuple(bestMatch, bestReprojError);
}

// returns the <id, matcherror> of the best matching plane, or -1 if no match found.  naive association based on geometric distance.
std::tuple<int, double> Mapper_mono::findClosestPlane(const Pose3d& curr_esti_pose, const Map_plane* check_plane) {
      int numMatches = 0;
      int bestMatch = -1;
      double bestDist = -1;
      double bestReprojError = -1;

      const Plane3d curr_plane_local=check_plane->temp_value;

      Plane3d curr_plane_world = curr_plane_local.transform_from(curr_esti_pose.oTw());  //  transform plane to global coordinate
      Eigen::Matrix4d curr_plane_pose_mat= curr_esti_pose.wTo();
      Eigen::MatrixXf curr_bounding_2d_poly=check_plane->plane_bound_close_2D_polys;
      Eigen::MatrixXf curr_bounding_3d_poly=check_plane->plane_bound_close_3D_polys;
//       cout<<"curr 1 "<<curr_bounding_3d_poly.col(0).head(2).transpose()<<endl;
//       cout<<"curr 2 "<<curr_bounding_3d_poly.col(1).head(2).transpose()<<endl;
//       cout<<"curr normal "<<curr_plane_world.normal().transpose()<<endl;
      int start_plane_id = 0; //std::max((int)_planeNodes.size() - 20,0);
      int end_plane_id = all_landmarks.size();

      for (int c = start_plane_id; c < end_plane_id; c++) {     // could speed up, only use recent frames
	    if (all_landmarks[c]->deteted_by_merge)  // if it is a dead node, being merged node, skip this
		continue;
	    if ( check_plane->frame_plane_indice==0 && all_landmarks[c]->frame_plane_indice==0 ){  // if both are ground, associate them immediately.
		  numMatches++;
		  bestMatch = c;
		  break;
	    }
	    if ( ( check_plane->frame_plane_indice==0 && all_landmarks[c]->frame_plane_indice>=1 )  ||    // if one is ground and the other is wall, must not be associated
		  ( check_plane->frame_plane_indice>=1 && all_landmarks[c]->frame_plane_indice==0 ) )
		  continue;   // skip this plane, check further plane

	    Plane3d old_plane_world = all_landmarks[c]->plane_vertex->value();
	    Plane3d old_plane_local;
	    if (useRelative) {
		old_plane_world = old_plane_world.transform_from(all_landmarks[c]->plane_vertex->base()->value().oTw());
	    }
	    old_plane_local = old_plane_world.transform_to(curr_esti_pose.wTo());

	    Eigen::MatrixXf old_bounding_3d_poly=all_landmarks[c]->plane_bound_close_3D_polys;

	    if (check_plane->frame_seq_id-all_landmarks[c]->frame_seq_id>assoc_near_frames)  // only recent several frames
		continue;
	    // first check their plane angle distance, NOTE  that this actually different left wall and right wall, and front wall
	    double angle_diff_normal=acos(curr_plane_world.normal().dot(old_plane_world.normal()))*180.0/PI; // 0~pi
	    if (print_assoc_detail){
		cout<<"To compare frame  "<< all_landmarks[c]->frame_seq_id <<"  plane indice  "<<all_landmarks[c]->good_plane_indice<<endl;
		cout<<"angle_diff_normal "<<angle_diff_normal<<endl;
	    }

	    if (angle_diff_normal>edge_asso_angle)  // angle too big difference, don't associate
		continue;

	    double actual_2d_error_thre=edge_asso_2ddist;
	    double actual_covering_thre=edge_asso_proj;
	    if (angle_diff_normal<25.0){   // if angle very similiar, reduce the threshold of 2d image distance
		actual_covering_thre=edge_asso_proj/3;
		actual_2d_error_thre=edge_asso_2ddist*1.5;
		if (angle_diff_normal<=10.0){
		  actual_covering_thre=edge_asso_proj/2;
		}
	    }

	    // calculate distance to plane; this assumes that normals are similar
	    double plane_dist = curr_plane_local.distance(old_plane_local.point0());
	    if (print_assoc_detail){
// 		  cout<<"curr_plane_world pram "<<curr_plane_world<<" old_world_plane_pram "<<old_plane_world<<endl;
// 		  cout<<"curr_plane_local pram "<<curr_plane_local<<" old_local_plane_pram "<<old_plane_local<<endl;
		cout<<"plane dist "<<plane_dist<<endl;
	    }
	    if (plane_dist > edge_asso_planedist)
		  continue;
	    if (plane_dist < 1.5)
		actual_covering_thre=actual_covering_thre/2;

	    // check edges 2d coordinates difference TODO this only works in recent frames as plane_bound_close_2D_polys is always the latest
	    float poly_2d_dist=0;
	    for (int i=0;i<2;i++){  //(double)curr_bounding_2d_poly.cols()  // only need to evaluate ground edges instead of 2d polygons
		  Vector2f distance_all;
		  for (int j=0;j<2;j++)  		// all_landmarks[c]->.plane_bound_close_2D_polys.cols()  could be replaced by reproj_bounding_2d_poly
		      distance_all(j)=(curr_bounding_2d_poly.col(i)- all_landmarks[c]->plane_bound_close_2D_polys.col(j)).norm();  //if we use _plane_boundaries directly, plane matching only works for short period
		  poly_2d_dist+=distance_all.minCoeff();
	    }
	    poly_2d_dist=poly_2d_dist/2;  //(double)curr_bounding_2d_poly.cols()
	    float poly_2d_dist_cross=0;
	    for (int i=0;i<2;i++){
		  Vector2f distance_all;
		  for (int j=0;j<2;j++)  		// could be replaced by reproj_bounding_2d_poly
		      distance_all(j)=(curr_bounding_2d_poly.col(j)- all_landmarks[c]->plane_bound_close_2D_polys.col(i)).norm();  //if we use _plane_boundaries directly, plane matching only works for short period
		  poly_2d_dist_cross+=distance_all.minCoeff();
	    }
	    poly_2d_dist_cross=poly_2d_dist_cross/2; // (double)curr_bounding_2d_poly.cols()

	    if (print_assoc_detail)
		  cout<<"mean 2d error  "<<poly_2d_dist<<"  "<<poly_2d_dist_cross<<"   thre "<<actual_2d_error_thre<<endl;
	    if (poly_2d_dist>actual_2d_error_thre || poly_2d_dist_cross> actual_2d_error_thre)
		  continue;

// 	      projection overlap I only need to compute line projection overlap namely ground segments
// 	      TODO this also works for recent frames as the the bounding poly  could use history...namely all previous polys
// 	      is always the latest  // more complicated method is to compute polygon overlapping: http://doc.cgal.org/latest/Boolean_set_operations_2/index.html#Chapter_2D_Regularized_Boolean_Set-Operations
	    float old_bg_proj_new=point_proj_to_lineseg(curr_bounding_3d_poly.col(0).head(2),curr_bounding_3d_poly.col(1).head(2),old_bounding_3d_poly.col(0).head(2));
	    float old_ed_proj_new=point_proj_to_lineseg(curr_bounding_3d_poly.col(0).head(2),curr_bounding_3d_poly.col(1).head(2),old_bounding_3d_poly.col(1).head(2));
	    float covering_old_to_new=fabs(old_bg_proj_new-old_ed_proj_new);
	    float new_bg_proj_old=point_proj_to_lineseg(old_bounding_3d_poly.col(0).head(2),old_bounding_3d_poly.col(1).head(2),curr_bounding_3d_poly.col(0).head(2));
	    float new_ed_proj_old=point_proj_to_lineseg(old_bounding_3d_poly.col(0).head(2),old_bounding_3d_poly.col(1).head(2),curr_bounding_3d_poly.col(1).head(2));
	    float covering_new_to_old=fabs(new_bg_proj_old-new_ed_proj_old);

	    if (print_assoc_detail)
		cout<<"covering_old_to_new "<<covering_old_to_new<<"  "<<covering_new_to_old<<"  thre "<<actual_covering_thre<<endl;
	    if (covering_old_to_new<actual_covering_thre || covering_new_to_old<actual_covering_thre)  // if very small overlapping, don't associate
		continue;

	    numMatches++;
	    double total_error=angle_diff_normal/edge_asso_angle*3+(1-covering_old_to_new)+(1-covering_new_to_old);
	    total_error+=std::max(poly_2d_dist,poly_2d_dist_cross)/actual_2d_error_thre+plane_dist/4;
// 	    double total_error=std::max(poly_2d_dist,poly_2d_dist_cross)/actual_2d_error_thre;

	    if (print_assoc_detail)
		  cout<<"total score "<<total_error<<endl;
	    if (numMatches==1 || total_error<bestReprojError ){  // the smaller, the better
		bestMatch = c;
		bestReprojError = total_error;   // weight combination of angle and 3d projection overlapping
	    }
	    if (print_assoc_detail)
		cout<<"add as potential match"<<endl;
        }

// 	if (numMatches > 1) {
// 	    cout << "Warning: multiple potential matches for one plane." << endl;
// // 	    cout<<"Warning: multiple potential matches for one plane."<<endl;
// 	}

	if (0)
	{
	    if (bestMatch<0){
		ROS_WARN_STREAM("Not finding matched planes, add as new landmark.");
	    }else{
		ROS_WARN_STREAM("Final matches global at  "<<bestMatch<<"  in frame  "<< all_landmarks[bestMatch]->frame_seq_id <<"  at indice  "<<all_landmarks[bestMatch]->good_plane_indice);
	    }
	}
	return make_tuple(bestMatch, bestReprojError);
//         return bestMatch;
}

// only one thread, add frame, associate, change graph, then optimize
void Mapper_mono::processFrame(vector<Map_plane> &new_planes, tracking_frame* newframe)
{
      vector<Map_plane> new_planes_mod;
      for (int jj=0;jj<new_planes.size();jj++)  // start from 1, if want to skip ground
	  new_planes_mod.push_back(new_planes[jj]);
      //NOTE In current code, if don't use ground, frame still stores ground plane polygons, but won't have plane node. so attention when using them...
      //eg: in  pub_all_hist_frame_planes_poly()  update_plane_measurement()  reproj_to_newplane() ...
      //for (int plane_id=1;....)   plane_id-1;

//       cout<<"process frame plane size   "<<new_planes_mod.size()<<endl;
      _tic();
      // data association
      Pose3d estimate_pose;  //get the latest isam estimates of pose, assumed to be the place where planes measurements come from. if isam is fast, assumed right.
      if (all_frames.size() > 0) {
	  estimate_pose = (all_frames.back()->pose_vertex->value()).oplus(newframe->temp_pose);
      }
      int numOldPlanes = all_landmarks.size();
      int numNewPlanes = 0;
      vector<int> assoc(new_planes_mod.size());   // find the association id.
      vector<double> assoc_error(new_planes_mod.size());
      int id=-1;
      double error=-1;
      for (int i = 0; i < new_planes_mod.size(); i++) {
// 	  cout<<"To match plane---------------------------  "<<i<<"  at  frame "<<new_planes_mod[i].frame_seq_id<<endl;
	  std::tie(id, error) = findClosestPlane(estimate_pose, &new_planes_mod[i]);

	  if (id > -1) {  //if found match, check some conditions
	      bool conflict=false;  // force one-to-one match, not allow two-to-one match
	      for (int j=0;j<i;j++){
		  if (assoc[j]==id){
		      double raw_length=(new_planes_mod[j].plane_bound_close_2D_polys.col(0)-new_planes_mod[j].plane_bound_close_2D_polys.col(1)).norm();
		      double new_length=(new_planes_mod[i].plane_bound_close_2D_polys.col(0)-new_planes_mod[i].plane_bound_close_2D_polys.col(1)).norm();
		      if (new_length>raw_length){  // new is better, replace old, set as new plane    error<assoc_error[j]
			  assoc[j]=numOldPlanes + numNewPlanes;
			  numNewPlanes++;
			  assoc_error[j]=-1;
			  assoc[i]=id;
			  assoc_error[i]=error;
			  ROS_WARN_STREAM("Dual match, add new landmark  "<<j<<"  at frame  "<<new_planes_mod[j].frame_seq_id);
		      }
		      else{   // Old is better, add new plane as new landmark
			  assoc[i] = numOldPlanes + numNewPlanes;
			  numNewPlanes++;
			  assoc_error[i] = 0.0;
			  ROS_WARN_STREAM("Dual match, add new landmark  "<<i<<"  at frame  "<<new_planes_mod[i].frame_seq_id);
		      }
		      conflict=true;
		      break;
		    }
	      }
	      if (conflict == false) {  // single-match
		  assoc[i] = id;
		  assoc_error[i] = error;
	      }
	  }
	  else {
	    assoc[i] = numOldPlanes + numNewPlanes; // no match, later create new plane, add to the map
	    numNewPlanes++;
	  }
      }
      _toc("data association");

      // add new pose node
      Pose3d_Node* poseNode = new Pose3d_Node();
      _slam.add_node(poseNode);
      newframe->pose_vertex=poseNode;
      all_frames.push_back(newframe);

      // add pose constraints
      if (all_frames.size() == 1) {       // if this is the first node, set as origin
	    Pose3d origin=newframe->temp_pose;   // prior on first pose only   first odometry (I set in main.cpp) is initial pose
	    Pose3d_Factor* pose_prior = new Pose3d_Factor(poseNode, origin, *poseCov);
	    _slam.add_factor(pose_prior);
      } else {   // else, add odometry relative measurements
	    poseNode->init(estimate_pose);   // initialize pose, not as important as odometry constraints
	    // could use some hacks to change the posecov, see previous nsh2 commits
	    Pose3d_Pose3d_Factor* constraint = new Pose3d_Pose3d_Factor(all_frames[all_frames.size()-2]->pose_vertex, poseNode, newframe->temp_pose,*poseCov);   //pose1, pose2, measure (odometry), noise
	    _slam.add_factor(constraint);
      }

      // add new planes node
      for (int i = 0; i < numNewPlanes; i++) {
	    Plane3d_Node* planeNode = new Plane3d_Node();
	    _slam.add_node(planeNode);

	    Map_plane* new_landmark= new Map_plane();
	    new_landmark->plane_vertex=planeNode;
	    all_landmarks.push_back(new_landmark);
	    new_landmark->landmark_index=all_landmarks.size()-1;
      }

      // add plane-pose constraints
      for (int i = 0; i < assoc.size(); i++){  // for all the planes observation in this frame
	  isam::Plane3d_Node* planeNode = all_landmarks[assoc[i]]->plane_vertex;
	  isam::Plane3d measure = new_planes_mod[i].temp_value;    // plane measurements come from current frame's segmentation.
	  if (assoc[i]>numOldPlanes-1){  // if it is added as a new plane, initialize the plane's pose
		Plane3d p = measure.transform_from(estimate_pose.oTw());  // transform to global frame
  // 	      p.plane_type=new_planes_mod[i].frame_plane_indice; // want to reduce plane dof, should not use it. will make the information matrix invertible, not positive definite.
		planeNode->init(p);   //initialize plane position
		if (new_planes_mod[i].frame_plane_indice==0){  //if ground change to anchor node, not optimized
		    Plane3d origin(Vector4d(0,0,-1,0));    // gronud plane is ideally (0,0,-1,0)
		    Plane3d_Factor *ground_prior=new Plane3d_Factor(planeNode, origin, *planeCov); // add a prior for ground plane
		    _slam.add_factor(ground_prior);
		}
	  }

	  double plane_dist=new_planes_mod[i].plane_dist_cam; //TODO variance is related to depth, could also be related to wide. the wider, the longer edges, the less covariance
	  plane_dist=plane_dist<3?3:plane_dist;
	  plane_dist=plane_dist>8?8:plane_dist;  // cut into [1,6]m
	  planeSigmas<<(plane_dist-1)*plane_sigma_dist_mul+5,(plane_dist-1)*plane_sigma_dist_mul+5,(plane_dist-1)*plane_sigma_dist_mul+5;  // nsh 3, tum 2

	  Covariance planeCov_new(planeSigmas.cwiseProduct(planeSigmas).asDiagonal());
	  Pose3d_Plane3d_Factor* fac = new Pose3d_Plane3d_Factor(poseNode,planeNode, measure, planeCov_new, useRelative);

	  // NOTE if want to pop up plane from lines in each iteration!!!  to use it: comment some parts in isam_plane3d.h and don't use ground planes
	  // but it is sensitive if there is bad object association.... not good on this dataset.
	  // results: it is not good on this dataset.
// 	  Eigen::MatrixXf plane_ground_2d_edge(1,4);
// 	  plane_ground_2d_edge.row(0).head<2>() =  new_planes_mod[i].plane_bound_close_2D_polys.col(0).head<2>();
// 	  plane_ground_2d_edge.row(0).tail<2>() =  new_planes_mod[i].plane_bound_close_2D_polys.col(1).head<2>();
// 	  fac->precompute_edge_ray(inv_calib,plane_ground_2d_edge);

	  _slam.add_factor(fac);

	  // update using the latest plane property, not landmark_index
	  copy_plane(all_landmarks[assoc[i]],&(new_planes_mod[i]));

	  newframe->observed_planes.push_back(all_landmarks[assoc[i]]);
	  newframe->pose_plane_facs.push_back(fac);
      }

       // detect loop closing
      bool loop_success=false;
      if (use_loop_close){
	  newframe->compute_frame_BoW(mpORBVocabulary, mpORBextractor); // compute Bag of words
	  newframe->bow_score_to_origin = mpORBVocabulary->score(newframe->mBowVec,all_frames[0]->mBowVec);  // the higher, the more similiar,
	  //TODO now I only detect loop with first frame. actually we should compare with all previous frames. see orbslam for more details.
	  // successive KF image might by 0.1~0.15  quite different image is around 0.001.  a little similar image is 0.01-0.09
	  cout<<"orb similarity score---------------- "<<newframe->bow_score_to_origin<<endl;
	  float loop_closing_thre=0.01;  // here I detect three successive frames, more clever method is to follow orbslam, to use covisity graph.
	  if (newframe->frame_seq_id>100 && newframe->bow_score_to_origin>0.01 && all_frames[all_frames.size()-2]->bow_score_to_origin>0.01 &&all_frames[all_frames.size()-3]->bow_score_to_origin>0.01 ){
	      ROS_ERROR("begin loop closing !!!!!!!!!!!");
	      loop_success=loopclose_merge(all_frames.size()-1);  // if detect close loop, then re-research the global candidate, merge them
	  }
      }

      cout<<"BA num of Poses/planes/nodes/factors:   "<<all_frames.size()<<"  "<<all_landmarks.size()<<"   "<<_slam.get_nodes().size()<<"   "
	  <<"  "<<_slam.get_factors().size()<<endl;

      _tic();
      if ( (new_planes_mod[0].frame_seq_id % 5==0) || loop_success)
	  int iterations=_slam.batch_optimization();
      else
	  _slam.update();  //incremental update

      // draw information matrix pattern
//       if (all_frames.size()==300)
//       {
// 	    ROS_ERROR("save eps");
//
// 	    SparseSystem  fat_mat= _slam.get_R();
// 	    string pattern_img=package_path+"/pattern_with_g.eps";
// 	    fat_mat.save_pattern_eps(pattern_img);
// 	    fat_mat.print_stats();
//
// 	    _slam.remove_node(all_landmarks[0]->plane_vertex);
// 	    cout << "New Number of nodes: " << _slam.get_nodes().size() << endl;
// 	    cout << "New Number of factors: " << _slam.get_factors().size() << endl;
//
// 	    int iterations=_slam.batch_optimization();
//
// 	    SparseSystem  fat_mat2= _slam.get_R();
// 	    string pattern_img2=package_path+"/pattern_no_g.eps";
// 	    fat_mat2.save_pattern_eps(pattern_img2);
// 	    fat_mat2.print_stats();
//       }

	_toc("SLAM");

//       if (last_frame_process){
	reproj_to_newplane();

	cout<<"mapping: finish processing"<<endl;
}

//   TODO if want to optimize pose (especially orientation), need to reproject planes. pose x,y  doesn't change plane measurements
//      update measurements using the latest pose. to reproject again, computation is low, because I didn't change cloud here. otherwise it might by heavy
void Mapper_mono::update_plane_measurement()
{
//      return;

// 	ca::Profiler::tictoc("update plane measurements");
	MatrixX4f all_planes_sensor_new;
	isam::Plane3d plane_measurement_new;
	for (const tracking_frame* one_frame: all_frames){
	    Matrix4d latest_pose = one_frame->pose_vertex->value().wTo();  // reproject to get plane measurements
	    Matrix4f pose=latest_pose.cast<float>();
	    popup_plane::update_plane_equation_from_seg(one_frame->ground_seg2d_lines,inv_calib,pose, all_planes_sensor_new);
	    for (int plane_id=0; plane_id<one_frame->good_plane_indices.rows();plane_id++) {
		plane_measurement_new=isam::Plane3d(all_planes_sensor_new.row(one_frame->good_plane_indices(plane_id)).cast<double>());
		one_frame->pose_plane_facs[plane_id]->set_measurement(plane_measurement_new);  // update measurements in slam factor
	    }
	}
// 	ca::Profiler::tictoc("update plane measurements");
}

void Mapper_mono::reproj_to_newplane()   //TODO now I reproject everything, later I could reproject only recent frames to save time
{
      for (int i=0;i<all_landmarks.size();i++){
	  // TODO reproject all the plane vertex, now I forget previous slam projected bouding polygon, only use latest descriptors,
	  // project a better way is to merge the polygons
	  if (all_landmarks[i]->deteted_by_merge)  // if it is a dead node, being merged node, skip this
	      continue;
	  for (int pt=0;pt<all_landmarks[i]->plane_bound_close_3D_polys.cols();pt++){
	      Vector3d updated_polyvertex=all_landmarks[i]->plane_vertex->value().project_to_plane(all_landmarks[i]->plane_bound_close_3D_polys.col(pt).cast<double>());
	      all_landmarks[i]->plane_bound_close_3D_polys.col(pt)=updated_polyvertex.cast<float>();
	  }
      }
      // could skip this step to speed up, just for visualization
      for (int frame_ind=0;frame_ind<all_frames.size();frame_ind++) {
	  //?skip the first groud plane, not used in slam anymore
	  for (int plane_ind=0;plane_ind<all_frames[frame_ind]->all_3d_bound_polygons_good_world.size();plane_ind++){
	       Plane3d curr_plane=all_frames[frame_ind]->observed_planes[plane_ind]->plane_vertex->value();
	      for (int pt=0;pt<all_frames[frame_ind]->all_3d_bound_polygons_good_world[plane_ind].cols();pt++){
		    Vector3d updated_polyvertex=curr_plane.project_to_plane(all_frames[frame_ind]->all_3d_bound_polygons_world_raw[plane_ind].col(pt).cast<double>());
		    all_frames[frame_ind]->all_3d_bound_polygons_good_world[plane_ind].col(pt)=updated_polyvertex.cast<float>();
	      }
	  }
      }
}

// loop close for one frame, merge all the planes in that frame, NOTE!!! I didn't delete the node, I just set the descriptor's state as deleted
// so this doesn't change the landmark index
bool Mapper_mono::loopclose_merge(int to_merge_frame_ind)
{
      bool really_merged=false;
      tracking_frame* to_merge_frame=all_frames[to_merge_frame_ind];
      int global_asso_id = -1;
      double error = -1.0;
      vector<int> removed_idxs;
      vector<int> assoc_closing_idxs;
      vector<Pose3d_Plane3d_Factor*> to_delete_facs;
      vector<Pose3d_Plane3d_Factor*> to_add_facs;

      // containing  frame_id  curr_plane_ind, to_plane_id, error;  initialize all -1
//       MatrixXd loop_cond_matrix=-1* (MatrixXd::Ones(to_merge_frame->observed_planes.size(),4));

      for (int i = 0; i < to_merge_frame->observed_planes.size(); ++i) {
	  int raw_plane_index_all=to_merge_frame->observed_planes[i]->landmark_index;  // this is current association id, I need to shift to actual loop closing ones.
	  cout<<"Might search loop close for plane----------------  "<<i<<"  at  frame "<<to_merge_frame->frame_seq_id<<endl;
	  if (raw_plane_index_all<30)  // HACK in nsh2 if it is already associated to beginning loop node, don't need to merge
	      continue;

	  cout<<"Really to find global matches"<<endl;
	  // HACK  now always associate to first frame... later, we could search the similar frame id using DetectLoopCandidates, see orb slam for more details
	  std::tie(global_asso_id, error) = findLoopPlane(to_merge_frame->pose_vertex->value(), all_landmarks[raw_plane_index_all], 0);

	  // loop close with first frame
	  if (global_asso_id >= 0 && global_asso_id!=raw_plane_index_all) {   // if not the same plane

	      list<Factor*> move_facs = all_landmarks[raw_plane_index_all]->plane_vertex->factors();
	      for (auto it : move_facs) {
		    Pose3d_Plane3d_Factor* old_fac = dynamic_cast<Pose3d_Plane3d_Factor*>(it);
		    Pose3d_Node* pose_node=dynamic_cast<Pose3d_Node*>(old_fac->nodes()[0]);  // cast the base class pointer to the inherited class
		    Pose3d_Plane3d_Factor* new_fac = new Pose3d_Plane3d_Factor(pose_node, all_landmarks[global_asso_id]->plane_vertex, old_fac->measurement(), old_fac->noise(), useRelative);
		    to_delete_facs.push_back(old_fac);
		    to_add_facs.push_back(new_fac);

		    _slam.add_factor(new_fac);
		    _slam.remove_factor(old_fac);  // the factor is still there, not deallocated
	      }
	      removed_idxs.push_back(raw_plane_index_all);
	      assoc_closing_idxs.push_back(global_asso_id);
	      really_merged=true;
	      ROS_WARN_STREAM("loop close merge " << raw_plane_index_all << " into " << global_asso_id);
	  }
      }

      if (removed_idxs.size()>0)  // change all_frames's factors information, and frame index, need this to update factors (update measurements)
	  for (int i=0;i<removed_idxs.size();i++)
	      for (tracking_frame* & one_frame: all_landmarks[removed_idxs[i]]->observer_frames)
		  for (int plane_id=0;plane_id<one_frame->observed_planes.size();plane_id++)
		  {
		      for (int fac_id=0;fac_id<to_delete_facs.size();fac_id++)
			  if (one_frame->pose_plane_facs[plane_id]==to_delete_facs[fac_id])
			      one_frame->pose_plane_facs[plane_id]==to_add_facs[fac_id];

		      if (one_frame->observed_planes[plane_id]->landmark_index==removed_idxs[i])
			  one_frame->observed_planes[plane_id]=all_landmarks[assoc_closing_idxs[i]];
		  }

      if (removed_idxs.size()>0)   // Replace old descriptor with the new descriptor, delete node
	  for (int i=0;i<removed_idxs.size();i++){
		copy_plane(all_landmarks[assoc_closing_idxs[i]],all_landmarks[removed_idxs[i]]);  // will replace timestamps..
		all_landmarks[removed_idxs[i]]->deteted_by_merge=true;
		_slam.remove_node(all_landmarks[removed_idxs[i]]->plane_vertex);
		delete all_landmarks[removed_idxs[i]]->plane_vertex;     // note this is deallocation, not delete vector element, didn't change vector size
	  }

      return really_merged;
}


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

  @copyright 2012-2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_LOG_2LO_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_LOG_2LO_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-constant

    Generates constant Log_2lo.This constant is coupled with Log2_hi and is
    used in the float logarithms computations
    We have double(Log_2lo<float>())+double(Log2_hi<float>()) == Log_2<double>()

    @par Semantic:

    @code
    T r = log_2lo<T>();
    @endcode

**/
  template<typename T> T Log_2lo();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant

      Generates constant Log_2lo.This constant is coupled with Log2_hi and is
      used in the float logarithms computations
      We have double(Log_2lo<float>())+double(Log2_hi<float>()) == Log_2<double>()

      Generate the  constant log_2lo.

      @return The Log_2lo constant for the proper type
    **/
    const boost::dispatch::functor<tag::log_2lo_> log_2lo = {};
  }
} }
#endif

#include <boost/simd/constant/definition/log_2lo.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


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

//
// Unit tests for canonical signatures
//

//#include "script.h"
#include "util.h"
#include "data/sig_noncanonical.json.h"
#include "data/sig_canonical.json.h"

#include <boost/foreach.hpp>
#include <boost/test/unit_test.hpp>
#include "json/json_spirit_writer_template.h"
#include <openssl/ecdsa.h>

using namespace std;
using namespace json_spirit;

// In script_tests.cpp
extern Array read_json(const std::string& jsondata);

BOOST_AUTO_TEST_SUITE(canonical_tests)

// OpenSSL-based test for canonical signature (without test for hashtype byte)
bool static IsCanonicalSignature_OpenSSL_inner(const std::vector<unsigned char>& vuchSig) {
	if (vuchSig.size() == 0) {
		return false;
	}
	const unsigned char *puchInput = &vuchSig[0];
	ECDSA_SIG *pSig = NULL;
	d2i_ECDSA_SIG(&pSig, &puchInput, vuchSig.size());
	if (pSig == NULL) {
		return false;
	}
	unsigned char arruchBuf[256];
	unsigned char *puchBuf = arruchBuf;
	unsigned int unLen = i2d_ECDSA_SIG(pSig, NULL);
	if (unLen != vuchSig.size()) {
		ECDSA_SIG_free(pSig);
		return false;
	}
	unLen = i2d_ECDSA_SIG(pSig, &puchBuf);
	ECDSA_SIG_free(pSig);
	return (memcmp(&vuchSig[0], &arruchBuf[0], unLen) == 0);
}

// OpenSSL-based test for canonical signature
bool static IsCanonicalSignature_OpenSSL(const std::vector<unsigned char> &vuchSignature) {
	if (vuchSignature.size() < 1) {
		return false;
	}
	if (vuchSignature.size() > 127) {
		return false;
	}
	if (vuchSignature[vuchSignature.size() - 1] & 0x7C) {
		return false;
	}

	std::vector<unsigned char> vuchSig(vuchSignature);
	vuchSig.pop_back();
	if (!IsCanonicalSignature_OpenSSL_inner(vuchSig)) {
		return false;
	}
	return true;
}

//BOOST_AUTO_TEST_CASE(script_canon)
//{
//    Array tests = read_json(std::string(json_tests::sig_canonical, json_tests::sig_canonical + sizeof(json_tests::sig_canonical)));
//
//    for (auto &tv : tests) {
//        string test = tv.get_str();
//        if (IsHex(test)) {
//            std::vector<unsigned char> sig = ParseHex(test);
//            BOOST_CHECK_MESSAGE(IsCanonicalSignature(sig, SCRIPT_VERIFY_STRICTENC), test);
//            BOOST_CHECK_MESSAGE(IsCanonicalSignature_OpenSSL(sig), test);
//        }
//    }
//}

//BOOST_AUTO_TEST_CASE(script_noncanon)
//{
//    Array tests = read_json(std::string(json_tests::sig_noncanonical, json_tests::sig_noncanonical + sizeof(json_tests::sig_noncanonical)));
//
//    for (auto &tv : tests) {
//        string test = tv.get_str();
//        if (IsHex(test)) {
//            std::vector<unsigned char> sig = ParseHex(test);
//            BOOST_CHECK_MESSAGE(!IsCanonicalSignature(sig, SCRIPT_VERIFY_STRICTENC), test);
//            BOOST_CHECK_MESSAGE(!IsCanonicalSignature_OpenSSL(sig), test);
//        }
//    }
//}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#ifdef POLYSOLVE_WITH_HYPRE

////////////////////////////////////////////////////////////////////////////////
#include <polysolve/LinearSolver.hpp>
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <vector>

#include <HYPRE_utilities.h>
#include <HYPRE.h>
#include <HYPRE_parcsr_ls.h>
#include <HYPRE_parcsr_mv.h>
#include <IJ_matrix.h>

////////////////////////////////////////////////////////////////////////////////
//
// https://computation.llnl.gov/sites/default/files/public/hypre-2.11.2_usr_manual.pdf
// https://github.com/LLNL/hypre/blob/v2.14.0/docs/HYPRE_usr_manual.pdf
//

namespace polysolve
{

    class LinearSolverHypre : public LinearSolver
    {

    public:
        LinearSolverHypre();
        ~LinearSolverHypre();

    private:
        POLYSOLVE_DELETE_MOVE_COPY(LinearSolverHypre)

    public:
        //////////////////////
        // Public interface //
        //////////////////////

        // Set solver parameters
        virtual void setParameters(const json &params) override;

        // Retrieve memory information from Pardiso
        virtual void getInfo(json &params) const override;

        // Analyze sparsity pattern
        virtual void analyzePattern(const StiffnessMatrix &A, const int precond_num) override { precond_num_ = precond_num; }

        // Factorize system matrix
        virtual void factorize(const StiffnessMatrix &A) override;

        // Solve the linear system Ax = b
        virtual void solve(const Ref<const VectorXd> b, Ref<VectorXd> x) override;

        // Name of the solver type (for debugging purposes)
        virtual std::string name() const override { return "Hypre"; }

    protected:
        int dimension_ = 1; // 1 = scalar (Laplace), 2 or 3 = vector (Elasticity)
        int max_iter_ = 1000;
        int pre_max_iter_ = 1;
        double conv_tol_ = 1e-10;

        HYPRE_Int num_iterations;
        HYPRE_Complex final_res_norm;

    private:
        bool has_matrix_ = false;
        int precond_num_;

        HYPRE_IJMatrix A;
        HYPRE_ParCSRMatrix parcsr_A;
    };

} // namespace polysolve

#endif


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

#include "amount.h"
#include "test/test_bitcoin.h"

#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(amount_tests, BasicTestingSetup)

BOOST_AUTO_TEST_CASE(GetFeeTest)
{
    CFeeRate feeRate;

    feeRate = CFeeRate(0);
    // Must always return 0
    BOOST_CHECK_EQUAL(feeRate.GetFee(0), 0);
    BOOST_CHECK_EQUAL(feeRate.GetFee(1e5), 0);

    feeRate = CFeeRate(1000);
    // Must always just return the arg
    BOOST_CHECK_EQUAL(feeRate.GetFee(0), 0);
    BOOST_CHECK_EQUAL(feeRate.GetFee(1), 1);
    BOOST_CHECK_EQUAL(feeRate.GetFee(121), 121);
    BOOST_CHECK_EQUAL(feeRate.GetFee(999), 999);
    BOOST_CHECK_EQUAL(feeRate.GetFee(1e3), 1e3);
    BOOST_CHECK_EQUAL(feeRate.GetFee(9e3), 9e3);

    feeRate = CFeeRate(123);
    // Truncates the result, if not integer
    BOOST_CHECK_EQUAL(feeRate.GetFee(0), 0);
    BOOST_CHECK_EQUAL(feeRate.GetFee(8), 1); // Special case: returns 1 instead of 0
    BOOST_CHECK_EQUAL(feeRate.GetFee(9), 1);
    BOOST_CHECK_EQUAL(feeRate.GetFee(121), 14);
    BOOST_CHECK_EQUAL(feeRate.GetFee(122), 15);
    BOOST_CHECK_EQUAL(feeRate.GetFee(999), 122);
    BOOST_CHECK_EQUAL(feeRate.GetFee(1e3), 123);
    BOOST_CHECK_EQUAL(feeRate.GetFee(9e3), 1107);
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef __SNP_COUNT_H__
#define __SNP_COUNT_H__

#include <armadillo>

#include <plink/snp_row.hpp>

/**
 * Counts the number of cases and controls with each genotype. The
 * counts are based on the weight, so an individual with weight 0.5
 * will be counted as 0.5 instead of 1.0.
 *
 * @param row1 The first snp.
 * @param row2 The second snp.
 * @param phenotype The phenotype 0.0 or 1.0.
 * @param weight The weight of each individual.
 *
 * @return Counts for each genotype. They are represented as a 9x2 matrix,
 *         so that each row is a cell ordered from left to right and top to bottom,
 *         and the first cell in each row is for controls and the second for cases.
 */
arma::mat joint_count(const snp_row &row1, const snp_row &row2, const arma::vec &phenotype, const arma::vec &weight);

/**
 * Aggregates the phenotype for each genotype. The
 * counts are based on the weight, so an individual with weight 0.5 will
 * get 0.5 * phenotype.
 *
 * @param row1 The first snp.
 * @param row2 The second snp.
 * @param phenotype The phenotype 0.0 or 1.0.
 * @param weight The weight of each individual.
 *
 * @return Counts for each genotype. They are represented as a 9x3 matrix,
 *         so that each row is a cell ordered from left to right and top to bottom,
 *         and the columns are sum of phenotypes, number of individuals and sum of squared phenotypes.
 */
arma::mat joint_count_cont(const snp_row &row1, const snp_row &row2, const arma::vec &phenotype, const arma::vec &weight);

/**
 * Counts the number of individuals with each genotype.
 *
 * @param row1 The first snp.
 * @param row2 The second snp.
 *
 * @return Counts for each genotype. They are represented as a 9 element vector,
 *         so that each cell is ordered from left to right and top to bottom.
 */
arma::vec joint_count(const snp_row &row1, const snp_row &row2);

/**
 * Counts the number of cases and controls with each phenotype. The
 * counts are based on the weight, so an individual with weight 0.5
 * will be counted as 0.5 instead of 1.0.
 *
 * @param row1 The first snp.
 * @param row2 The second snp.
 * @param phenotype The phenotype 0.0 or 1.0.
 * @param weight The weight of each individual.
 *
 * @return Counts for each phenotype. The are represented as a 2 element vector,
 *         so that the first element is the number of controls and the second the
 *         number of cases. Individuals with the first or second snp missing is
 *         ignored.
 */
arma::vec pheno_count(const snp_row &row1, const snp_row &row2, const arma::vec &phenotype, const arma::vec &weight);

/**
 * Counts the number of cases and controls with each genotype for one
 * of the snps. The counts are based on the weight, so an individual with weight 0.5
 * will be counted as 0.5 instead of 1.0.
 *
 * @param row1 The first snp.
 * @param row2 The second snp.
 * @param phenotype The phenotype 0.0 or 1.0.
 * @param weight The weight of each individual.
 *
 * @return Counts for each genotype for one snp. They are represented as a 3x2 matrix,
 *         so that each row is a allele (0, 1, 2) and the first cell in each row is
 *         for controls and the second for cases. Individuals with any snp missing is
 *         ignored.
 */
arma::mat single_count(const snp_row &row1, const snp_row &row2, const arma::vec &phenotype, const arma::vec &weight);

/**
 * Estimates the probabilities for 0, 1 and 2 for a snp.
 *
 * @param row A snp.
 *
 * @return A 3-element vector that contains the probability
 *         of each allele, i.e. it sums to 1.0.
 */
arma::vec compute_maf(const snp_row &row);

/**
 * Estimates the minor allele frequency.
 *
 * @param row A snp.
 *
 * @return The frequency of the minor allele, i.e. the allele
 *         that has the lowest frequency in this population.
 */
float compute_real_maf(const snp_row &row);

/**
 * Find the minimum but respect that the values
 * can be na, in that case return the non-na value.
 *
 * @param a Any value.
 * @param b Any value.
 * @param na The value that should be interpreted as 'na'.
 *
 * @return The minimum value of a and b, if a is missing then
 *         b is returned, if b is missing then a is returned,
 *         if both are missing na is returned.
 */
double min_na(double a, double b, double na = -9);

#endif /* End of __SNP_COUNT_H__ */


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2014 Benoit Steiner <benoit.steiner.goog@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"

#include <Eigen/CXX11/Tensor>

using Eigen::Tensor;
using Eigen::array;

template <int DataLayout>
static void test_simple_shuffling()
{
  Tensor<float, 4, DataLayout> tensor(2,3,5,7);
  tensor.setRandom();
  array<ptrdiff_t, 4> shuffles;
  shuffles[0] = 0;
  shuffles[1] = 1;
  shuffles[2] = 2;
  shuffles[3] = 3;

  Tensor<float, 4, DataLayout> no_shuffle;
  no_shuffle = tensor.shuffle(shuffles);

  VERIFY_IS_EQUAL(no_shuffle.dimension(0), 2);
  VERIFY_IS_EQUAL(no_shuffle.dimension(1), 3);
  VERIFY_IS_EQUAL(no_shuffle.dimension(2), 5);
  VERIFY_IS_EQUAL(no_shuffle.dimension(3), 7);

  for (int i = 0; i < 2; ++i) {
    for (int j = 0; j < 3; ++j) {
      for (int k = 0; k < 5; ++k) {
        for (int l = 0; l < 7; ++l) {
          VERIFY_IS_EQUAL(tensor(i,j,k,l), no_shuffle(i,j,k,l));
        }
      }
    }
  }

  shuffles[0] = 2;
  shuffles[1] = 3;
  shuffles[2] = 1;
  shuffles[3] = 0;
  Tensor<float, 4, DataLayout> shuffle;
  shuffle = tensor.shuffle(shuffles);

  VERIFY_IS_EQUAL(shuffle.dimension(0), 5);
  VERIFY_IS_EQUAL(shuffle.dimension(1), 7);
  VERIFY_IS_EQUAL(shuffle.dimension(2), 3);
  VERIFY_IS_EQUAL(shuffle.dimension(3), 2);

  for (int i = 0; i < 2; ++i) {
    for (int j = 0; j < 3; ++j) {
      for (int k = 0; k < 5; ++k) {
        for (int l = 0; l < 7; ++l) {
          VERIFY_IS_EQUAL(tensor(i,j,k,l), shuffle(k,l,j,i));
        }
      }
    }
  }
}

template <int DataLayout>
static void test_expr_shuffling()
{
  Tensor<float, 4, DataLayout> tensor(2,3,5,7);
  tensor.setRandom();

  array<ptrdiff_t, 4> shuffles;
  shuffles[0] = 2;
  shuffles[1] = 3;
  shuffles[2] = 1;
  shuffles[3] = 0;
  Tensor<float, 4, DataLayout> expected;
  expected = tensor.shuffle(shuffles);

  Tensor<float, 4, DataLayout> result(5,7,3,2);

  array<int, 4> src_slice_dim{{2,3,1,7}};
  array<int, 4> src_slice_start{{0,0,0,0}};
  array<int, 4> dst_slice_dim{{1,7,3,2}};
  array<int, 4> dst_slice_start{{0,0,0,0}};

  for (int i = 0; i < 5; ++i) {
    result.slice(dst_slice_start, dst_slice_dim) =
        tensor.slice(src_slice_start, src_slice_dim).shuffle(shuffles);
    src_slice_start[2] += 1;
    dst_slice_start[0] += 1;
  }

  VERIFY_IS_EQUAL(result.dimension(0), 5);
  VERIFY_IS_EQUAL(result.dimension(1), 7);
  VERIFY_IS_EQUAL(result.dimension(2), 3);
  VERIFY_IS_EQUAL(result.dimension(3), 2);

  for (int i = 0; i < expected.dimension(0); ++i) {
    for (int j = 0; j < expected.dimension(1); ++j) {
      for (int k = 0; k < expected.dimension(2); ++k) {
        for (int l = 0; l < expected.dimension(3); ++l) {
          VERIFY_IS_EQUAL(result(i,j,k,l), expected(i,j,k,l));
        }
      }
    }
  }

  dst_slice_start[0] = 0;
  result.setRandom();
  for (int i = 0; i < 5; ++i) {
    result.slice(dst_slice_start, dst_slice_dim) =
        tensor.shuffle(shuffles).slice(dst_slice_start, dst_slice_dim);
    dst_slice_start[0] += 1;
  }

  for (int i = 0; i < expected.dimension(0); ++i) {
    for (int j = 0; j < expected.dimension(1); ++j) {
      for (int k = 0; k < expected.dimension(2); ++k) {
        for (int l = 0; l < expected.dimension(3); ++l) {
          VERIFY_IS_EQUAL(result(i,j,k,l), expected(i,j,k,l));
        }
      }
    }
  }
}

template <int DataLayout>
static void test_shuffling_as_value()
{
  Tensor<float, 4, DataLayout> tensor(2,3,5,7);
  tensor.setRandom();
  array<ptrdiff_t, 4> shuffles;
  shuffles[2] = 0;
  shuffles[3] = 1;
  shuffles[1] = 2;
  shuffles[0] = 3;
  Tensor<float, 4, DataLayout> shuffle(5,7,3,2);
  shuffle.shuffle(shuffles) = tensor;

  VERIFY_IS_EQUAL(shuffle.dimension(0), 5);
  VERIFY_IS_EQUAL(shuffle.dimension(1), 7);
  VERIFY_IS_EQUAL(shuffle.dimension(2), 3);
  VERIFY_IS_EQUAL(shuffle.dimension(3), 2);

  for (int i = 0; i < 2; ++i) {
    for (int j = 0; j < 3; ++j) {
      for (int k = 0; k < 5; ++k) {
        for (int l = 0; l < 7; ++l) {
          VERIFY_IS_EQUAL(tensor(i,j,k,l), shuffle(k,l,j,i));
        }
      }
    }
  }

  array<ptrdiff_t, 4> no_shuffle;
  no_shuffle[0] = 0;
  no_shuffle[1] = 1;
  no_shuffle[2] = 2;
  no_shuffle[3] = 3;
  Tensor<float, 4, DataLayout> shuffle2(5,7,3,2);
  shuffle2.shuffle(shuffles) = tensor.shuffle(no_shuffle);
  for (int i = 0; i < 5; ++i) {
    for (int j = 0; j < 7; ++j) {
      for (int k = 0; k < 3; ++k) {
        for (int l = 0; l < 2; ++l) {
          VERIFY_IS_EQUAL(shuffle2(i,j,k,l), shuffle(i,j,k,l));
        }
      }
    }
  }
}

template <int DataLayout>
static void test_shuffle_unshuffle()
{
  Tensor<float, 4, DataLayout> tensor(2,3,5,7);
  tensor.setRandom();

  // Choose a random permutation.
  array<ptrdiff_t, 4> shuffles;
  for (int i = 0; i < 4; ++i) {
    shuffles[i] = i;
  }
  array<ptrdiff_t, 4> shuffles_inverse;
  for (int i = 0; i < 4; ++i) {
    const ptrdiff_t index = internal::random<ptrdiff_t>(i, 3);
    shuffles_inverse[shuffles[index]] = i;
    std::swap(shuffles[i], shuffles[index]);
  }

  Tensor<float, 4, DataLayout> shuffle;
  shuffle = tensor.shuffle(shuffles).shuffle(shuffles_inverse);

  VERIFY_IS_EQUAL(shuffle.dimension(0), 2);
  VERIFY_IS_EQUAL(shuffle.dimension(1), 3);
  VERIFY_IS_EQUAL(shuffle.dimension(2), 5);
  VERIFY_IS_EQUAL(shuffle.dimension(3), 7);

  for (int i = 0; i < 2; ++i) {
    for (int j = 0; j < 3; ++j) {
      for (int k = 0; k < 5; ++k) {
        for (int l = 0; l < 7; ++l) {
          VERIFY_IS_EQUAL(tensor(i,j,k,l), shuffle(i,j,k,l));
        }
      }
    }
  }
}

EIGEN_DECLARE_TEST(cxx11_tensor_shuffling)
{
  CALL_SUBTEST(test_simple_shuffling<ColMajor>());
  CALL_SUBTEST(test_simple_shuffling<RowMajor>());
  CALL_SUBTEST(test_expr_shuffling<ColMajor>());
  CALL_SUBTEST(test_expr_shuffling<RowMajor>());
  CALL_SUBTEST(test_shuffling_as_value<ColMajor>());
  CALL_SUBTEST(test_shuffling_as_value<RowMajor>());
  CALL_SUBTEST(test_shuffle_unshuffle<ColMajor>());
  CALL_SUBTEST(test_shuffle_unshuffle<RowMajor>());
}


#ifndef CPP_TENSOR_PRODUCT_BEZIER_TRIANGLE_HH
#define CPP_TENSOR_PRODUCT_BEZIER_TRIANGLE_HH

#include "utils.hh"

#include <Eigen/Core>

#include <type_traits>
#include <utility>
#include <stdexcept>

namespace tl
{
template <std::size_t D, typename T, typename C, std::size_t... Degrees>
struct TensorProductDerivativeType;

template <std::size_t D, typename T, typename C, std::size_t... Degrees>
using TensorProductDerivativeType_t =
        typename TensorProductDerivativeType<D, T, C, Degrees...>::type;

template <std::size_t D, typename T, typename C, std::size_t... Degrees>
struct TensorProductDerivative;

/**
 * Traits class for accessing typedefs from specializations of
 * TensorProductBezierTriangle.
 *
 * Must declare two `static constexpr std::size_t` members `NCoords` and
 * `NCoeffs` that specify the number of coordinates and coefficients of the
 * polynomial. E.g.:
 *
 * ```cpp
 * template <typename T, typename C>
 * struct TensorProductTraits<TensorProductBezierTriangle<T, C, 3>>
 * {
 *     static constexpr std::size_t NCoords = 3;
 *     static constexpr std::size_t NCoeffs = 3;
 * };
 * ```
 *
 * @tparam Derived Specialization of TensorProductBezierTriangle
 */
template <typename Derived>
struct TensorProductTraits;

/**
 * @brief Base class for specializations of TensorProductBezierTriangle.
 * @details Base class holding the public interface and common functionality for
 *      specializations of TensorProductBezierTriangle.
 *
 *      Specializations of TensorProductBezierTriangle that derive from this
 *      class need to define a Traits class (see TensorProductTraits) as well a
 *      number of static member functions. It also needs to declare the base
 *      class as a friend and inherit its constructors. A minimal example for a
 *      specialization looks as follows:
 *
 *      ```cpp
 *      template <typename T, typename C>
 *      struct TensorProductTraits<TensorProductBezierTriangle<T, C, 1>>
 *      {
 *          static constexpr std::size_t NCoords = 3;
 *          static constexpr std::size_t NCoeffs = 3;
 *      };
 *
 *      template <typename T, typename C>
 *      class TensorProductBezierTriangle<T, C, 1>
 *              : public TensorProductBezierTriangleBase<
 *                      TensorProductBezierTriangle<T, C, 1>, T, C, 1>
 *      {
 *      public:
 *          using Base = TensorProductBezierTriangleBase<
 *                          TensorProductBezierTriangle<T, C, 1>, T, C, 1>;
 *          using Coords = typename Base::Coords;
 *          using Coeffs = typename Base::Coeffs;
 *
 *          friend Base;
 *
 *          using Base::Base;
 *
 *      private:
 *          using Self = TensorProductBezierTriangle<T, C, 1>;
 *          using Traits = TensorProductTraits<Self>;
 *          using Basis = Eigen::Matrix<C, NCoeffs, 1>;
 *          using DomainPoints = Eigen::Matrix<C, NCoeffs, NCoords>;
 *
 *          // Coordinates of the control points
 *          static const DomainPoints& domainPoints();
 *
 *          // Convert coordinates into polynomial basis
 *          static Basis makeBasis(const Coords& pos);
 *
 *          // Compute coefficients for one of four subdivision triangles when
 *          // splitting in the given dimension D
 *          template<int I, int D=0>
 *          static Coeffs splitCoeffs(const Coeffs& in);
 *
 *          // Compute coefficients of an interpolating polynomial given sampled
 *          // values at the control points.
 *          static Coeffs computeCoeffs(const Coeffs& samples);
 *      };
 *
 *      ```
 *
 * @tparam Derived The specialization of TensorProductBezierTriangle that
 *     inherits from this class.
 * @tparam T Type of the coefficients
 * @tparam C Type of the coordinates (must be scalar)
 * @tparam Degrees Degrees of the polynomials participating in the tensor
 *     product
 */
template <typename Derived, typename T, typename C, std::size_t... Degrees>
class TensorProductBezierTriangleBase
{
public:
    constexpr static int NCoords = sizeof...(Degrees) * 3;
    constexpr static std::size_t NCoeffs = ((((Degrees+1)*(Degrees+2))/2) * ...);
    using Coords = Eigen::Matrix<C, NCoords, 1>;
    using Coeffs = std::array<T, NCoeffs>;

    TensorProductBezierTriangleBase() = default;

    /**
     * Create polynomial with the given coefficients.
     */
    explicit TensorProductBezierTriangleBase(const Coeffs& coefficients)
            : _coeffs(coefficients)
    {
    }

    // Check for type that is callable with Coords and returns something convertible to T
    template <typename Function>
    using is_legal_init_function =
            std::is_convertible<std::result_of_t<Function(Coords)>, T>;

    /**
     * Create polynomial interpolating the given function at the control points.
     *
     * @tparam Function Callable that takes Coords and returns a value
     *      convertible to T.
     */
    template<
        typename Function,
        typename std::enable_if<
            is_legal_init_function<Function>::value,
            int
        >::type = 0>
    explicit TensorProductBezierTriangleBase(Function func)
    {
        auto rhs = Coeffs{};
        const auto& dp = Derived::domainPoints();
        for(auto i: cpp_utils::range(NCoeffs))
        {
            rhs[i] = func(dp.row(i));
        }
        _coeffs = Derived::computeCoeffs(rhs);
    }

    /**
     * Evaluate the polynomial at the given coordinates.
     */
    T operator()(const Coords& pos) const
    {
        auto base = Derived::makeBasis(pos);
        auto result = T{base[0] * _coeffs[0]};
        for(auto i: cpp_utils::range(1, NCoeffs))
        {
            result += base[i] * _coeffs[i];
        }
        return result;
    }

    /**
     * Access the i-th coefficient.
     */
    T& operator[](std::size_t i)
    {
        return _coeffs[i];
    }

    /**
     * Access the i-th coefficient.
     */
    const T& operator[](std::size_t i) const
    {
        return _coeffs[i];
    }

    /**
     * Access the coefficients directly
     */
    Coeffs& coefficients()
    {
        return _coeffs;
    }

    /**
     * Access the coefficients directly
     */
    const Coeffs& coefficients() const
    {
        return _coeffs;
    }

    /**
     * @brief Split the triangle into four new ones in the given space.
     * @details The triangle A-B-C is split into four new ones numbered
     *      according to the following diagram:
     * ```
     *          C
     *         / \
     *        / 2 \
     *       o-----o
     *      / \ 3 / \
     *     / 0 \ / 1 \
     *    A-----o-----B
     * ```
     * The corners of the new ones are ordered starting with the bottom left
     * (bottom for the middle one) and counting in counter-clockwise direction.
     *
     * @tparam D Space in which to split
     * @return Array of four new polynomials representing the split parts.
     */
    template <std::size_t D=0>
    std::array<Derived, 4> split() const
    {
        return {split<D>(0), split<D>(1), split<D>(2), split<D>(3)};
    }

    /**
     * Equivalent to `split<D>()[part]`
     *
     * @param part index of the sub-triangle of the split (0-3)
     * @tparam std::D=0 Space in which to split
     * @return One new polynomial representing one of the four sub-triangles
     */
    template <std::size_t D=0>
    Derived split(std::size_t part) const
    {
        if(D >= sizeof...(Degrees))
            throw(std::invalid_argument(
                    "D must be smaller than the number of Degrees"));
        switch(part)
        {
            case 0: return Derived{Derived::template splitCoeffs<0, D>(_coeffs)};
            case 1: return Derived{Derived::template splitCoeffs<1, D>(_coeffs)};
            case 2: return Derived{Derived::template splitCoeffs<2, D>(_coeffs)};
            case 3: return Derived{Derived::template splitCoeffs<3, D>(_coeffs)};
            default: throw std::invalid_argument("Part must be smaller than 4!");
        }
    }

    template <std::size_t D>
    TensorProductDerivativeType_t<D, T, C, Degrees...>
    derivative(std::size_t i) const
    {
        using Derivative =
                TensorProductDerivativeType_t<D, T, C, Degrees...>;
        using OpType = TensorProductDerivative<D, T, C, Degrees...>;

        assert(i < 3);

        return Derivative{OpType::deriv_op(_coeffs, i)};
    }

    void operator+=(const Derived& other)
    {
        for(auto i: cpp_utils::range(_coeffs.size()))
        {
            _coeffs[i] += other[i];
        }
    }

    void operator-=(const Derived& other)
    {
        for(auto i: cpp_utils::range(_coeffs.size()))
        {
            _coeffs[i] -= other[i];
        }
    }

    template <typename Scalar>
    void operator*=(const Scalar& scalar)
    {
        for(auto& c: _coeffs)
        {
            c *= scalar;
        }
    }

    template <typename Scalar>
    void operator/=(const Scalar& scalar)
    {
        for(auto& c: _coeffs)
        {
            c /= scalar;
        }
    }

    Derived operator-() const
    {
        auto r_coeffs = Coeffs{};
        for(auto i: cpp_utils::range(r_coeffs.size()))
        {
            r_coeffs[i] = -this->coefficients()[i];
        }
        return Derived{r_coeffs};
    }

    Derived operator+(const Derived& other) const
    {
        auto result = Derived{static_cast<Derived const &>(*this)};
        result += other;
        return result;
    }

    Derived operator-(const Derived& other) const
    {
        return *this + (-other);
    }

    template <typename Scalar>
    Derived operator*(const Scalar& scalar) const
    {
        auto result = Derived{static_cast<Derived const &>(*this)};
        result *= scalar;
        return result;
    }

    template <typename Scalar>
    Derived operator/(const Scalar& scalar) const
    {
        auto result = Derived{static_cast<Derived const &>(*this)};
        result /= scalar;
        return result;
    }

    template <typename Scalar>
    friend Derived operator*(const Scalar& scalar, const Derived& poly)
    {
        return poly * scalar;
    }

    friend bool operator==(const Derived& o1, const Derived& o2)
    {
        return o1.coefficients() == o2.coefficients();
    }
    friend bool operator!=(const Derived& o1, const Derived& o2)
    {
        return !(o1 == o2);
    }
private:

    // Coefficients of the Bezier Triangle
    Coeffs _coeffs = {};
};

/**
 * Unspecialized class template for a tensor product of Bernstein-Bézier
 * triangles.
 *
 * A tensor product of Bernstein-Bézier triangles has the form
 *
 * \f{align*}{
 *     f(\alpha_1, \beta_1, \gamma_1, \dots, \alpha_n, \beta_n, \gamma_n) &=
 *     \sum_{i_1+j_1+k_1=d_1} \frac{d_1!}{i_1!j_1!k_1!} \; b_{i_1j_1k_1} \;
 *          \alpha_1^{i_1} \beta_1^{j_1} \gamma_1^{k_1} \cdots
 *     \sum_{i_n+j_n+k_n=d_n} \frac{d_n!}{i_n!j_n!k_n!} \; b_{i_nj_nk_n} \;
 *          \alpha_n^{i_n} \beta_n^{j_n} \gamma_n^{k_n}\\
 *     &= \sum_{i_1+j_1+k_1=d_1} \cdots \sum_{i_n+j_n+k_n=d_n}
 *          \frac{d_1!}{i_1!j_1!k_1!} \cdots \frac{d_n!}{i_n!j_n!k_n!} \;
 *          b_{i_1j_1k_1\dots\i_nj_nk_n} \;
 *          \alpha_1^{i_1} \beta_1^{j_1} \gamma_1^{k_1} \cdots
 *          \alpha_n^{i_n} \beta_n^{j_n} \gamma_n^{k_n}\\
 *     &\text{with}\; \alpha_i + \beta_i + \gamma_i = 1
 * \f}
 *
 * Where \f$\alpha_i, \beta_i, \gamma_i\f$ are the barycentric coordinates of
 * the i-th Bernstein-Bézier triangle, \f$d_i\f$ is the degree of the i-th
 * Bernstein-Bézier triangle, and \f$b_{\dots}\f$ are the coefficients.
 *
 * @tparam T Type of the coefficients
 * @tparam C Type of the coordinates (must be scalar)
 * @tparam Degrees Degrees of the polynomials
 */
template <typename T, typename C, std::size_t... Degrees>
class TensorProductBezierTriangle
{
};

} // namespace tl

#endif


#ifndef ASLAM_GRID_CALIBRATION_TARGET_BASE_HPP
#define ASLAM_GRID_CALIBRATION_TARGET_BASE_HPP

#include <vector>
#include <utility>
#include <eigen3/Eigen/Core>
#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/split_member.hpp>
#include <boost/serialization/base_object.hpp>
#include <boost/serialization/export.hpp>
#include <sm/eigen/serialization.hpp>
#include <sm/boost/serialization.hpp>
#include <sm/assert_macros.hpp>

namespace aslam {
namespace cameras {

/**
 * \class GridCalibrationTarget
 * \brief a class that represents a calibration target with known geometry
 *
 * The class is a little limiting:
 * The target is supposed to be square such that each row has the same number of points.
 * Points along a row are supposed to be nominally colinear.
 * Points along a column are supposed to be nominally colinear.
 * Missing points are not allowed.
 *
 */
class GridCalibrationTargetBase {
 public:
  /// \brief initialize base class
  GridCalibrationTargetBase(size_t rows, size_t cols);
  virtual ~GridCalibrationTargetBase() {
  };

 public:
  SM_DEFINE_EXCEPTION(Exception, std::runtime_error);

  typedef boost::shared_ptr<GridCalibrationTargetBase> Ptr;
  typedef boost::shared_ptr<const GridCalibrationTargetBase> ConstPtr;

  /// \brief get the number of points of the full grid
  size_t size() const {
    return _rows * _cols;
  };

  /// \brief the number of rows in the calibration target
  size_t rows() const {
    return _rows;
  };

  /// \brief the number of columns in the calibration target
  size_t cols() const {
    return _cols;
  };

  /// \brief get a point from the target expressed in the target frame
  Eigen::Vector3d point(size_t i) const;

  /// \brief get all points from the target expressed in the target frame
  Eigen::MatrixXd points() const;

  /// \brief get the grid coordinates for a point
  std::pair<size_t, size_t> pointToGridCoordinates(size_t i) const;

  /// \brief get the point index from the grid coordinates
  size_t gridCoordinatesToPoint(size_t r, size_t c) const;

  /// \brief get a point from the target expressed in the target frame
  ///        by row and column
  Eigen::Vector3d gridPoint(size_t r, size_t c) const;

  /// \brief extract the calibration target points from an image
  ///        outCornerObserved flags wheter the corresponding point
  ///        in outImagePoints was observed
  virtual bool computeObservation(const cv::Mat & /*image*/,
                                  Eigen::MatrixXd & /*outImagePoints*/,
                                  std::vector<bool> & /*outCornerObserved*/) const
  {
    SM_ASSERT_TRUE(Exception, true, "you need to implement this method for each target!");
    return false;
  };

  /// \brief return pointer to the i-th grid point in target frame
  double * getPointDataPointer(size_t i);

 protected:
  /// \brief grid points stored in row-major order (idx = cols * r + c)
  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> _points;

  /// \brief the number of point rows in the calibration target
  size_t _rows;

  /// \brief the number of point columns in the calibration target
  size_t _cols;

  ///////////////////////////////////////////////////
  // Serialization support
  ///////////////////////////////////////////////////
 public:
  enum {CLASS_SERIALIZATION_VERSION = 1};
  BOOST_SERIALIZATION_SPLIT_MEMBER()

  //serialization ctor
  GridCalibrationTargetBase() {};

 protected:
  friend class boost::serialization::access;

  template<class Archive>
  void save(Archive & ar, const unsigned int /* version */) const {
    ar << BOOST_SERIALIZATION_NVP(_points);
    ar << BOOST_SERIALIZATION_NVP(_rows);
    ar << BOOST_SERIALIZATION_NVP(_cols);
  }
  template<class Archive>
  void load(Archive & ar, const unsigned int /* version */) {
    ar >> BOOST_SERIALIZATION_NVP(_points);
    ar >> BOOST_SERIALIZATION_NVP(_rows);
    ar >> BOOST_SERIALIZATION_NVP(_cols);
  }

}; //class GridCalibrationTargetBase

} // namespace cameras
} // namespace aslam

SM_BOOST_CLASS_VERSION(aslam::cameras::GridCalibrationTargetBase);
BOOST_CLASS_EXPORT_KEY(aslam::cameras::GridCalibrationTargetBase);

#endif /* ASLAM_GRID_CALIBRATION_TARGET_BASE_HPP */


//
//! Copyright © 2019
//! Brandon Kohn
//
//  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)
//
#pragma once

#include <geometrix/algorithm/intersection/polygon_polygon_border_intersection.hpp>
#include <geometrix/algorithm/doubly_connected_edge_list.hpp>
#include <geometrix/algorithm/intersection/segment_polygon_border_intersection.hpp>
#include <geometrix/primitive/polygon.hpp>
#include <geometrix/algorithm/point_sequence/area.hpp>

#include <boost/graph/adjacency_list.hpp>
#include <boost/container/flat_map.hpp>

#include <bitset>

namespace geometrix{

	namespace detail {

		template <typename Point>
		class loop_graph
		{
			using point_type = Point;

			enum class vertex_type : std::uint8_t
			{
			    node = 1
			  , intersection = 2
			  , start = 4
			};

			struct vertex_properties
			{
				vertex_properties() = default;

				vertex_properties(const point_type& p, vertex_type type = vertex_type::node)
					: point(p)
					, flags(type)
				{}

				bool is_start() const
				{
					return flags & vertex_type::start;
				}

				void set_start()
				{
					flags |= vertex_type::start;
				}

				bool is_node() const
				{
					return flags & vertex_type::node;
				}

				void set_node()
				{
					flags |= vertex_type::node;
				}

				bool is_intersection() const
				{
					return flags & vertex_type::intersection;
				}

				void set_intersection()
				{
					flags |= vertex_type::intersection;
				}

				point_type     point;

				//! bits: <intersection, start, node>
				std::bitset<3> flags;
			};

			struct edge_properties
			{
				edge_properties(std::size_t marker = (std::numeric_limits<std::size_t>::max)())
					: marker(marker)
				{}

				std::size_t marker;
			};

			using graph = boost::adjacency_list
			<
			    boost::vecS
			  , boost::vecS
			  , boost::directedS
			  , vertex_properties
			  , edge_properties
			>;

			using vertex = typename graph::vertex_descriptor;
			using edge = typename graph::edge_descriptor;
			using point_vertex_map = boost::container::flat_map<point_type, vertex, geometrix::lexicographical_comparer<geometrix::absolute_tolerance_comparison_policy<units::length>>>;

		public:

			loop_graph() = default;

			vertex add_start_vertex(const point_type& p)
			{
				auto it = m_pointVertexMap.lower_bound(p);
				if (it != m_pointVertexMap.end() && !m_pointVertexMap.key_comp()(p, *it))
				{
					auto v = it->second;
					m_graph[v].set_start();
					return v;
				}

				auto v = boost::add_vertex(m_graph, vertex_properties{ p, vertex_type::start });
				m_pointVertexMap.emplace(it, p, v);
				return v;
			}

			vertex add_intersection_vertex(const point_type& p)
			{
				auto it = m_pointVertexMap.lower_bound(p);
				if (it != m_pointVertexMap.end() && !m_pointVertexMap.key_comp()(p, *it))
				{
					auto v = it->second;
					m_graph[v].set_intersection();
					return v;
				}

				auto v = boost::add_vertex(m_graph, vertex_properties{ p, vertex_type::intersection });
				m_pointVertexMap.emplace(it, p, v);
				return v;
			}

			vertex add_node_vertex(const point_type& p)
			{
				auto it = m_pointVertexMap.lower_bound(p);
				if (it != m_pointVertexMap.end() && !m_pointVertexMap.key_comp()(p, *it))
				{
					auto v = it->second;
					m_graph[v].set_node();
					return v;
				}

				auto v = boost::add_vertex(m_graph, vertex_properties{ p, vertex_type::node });
				m_pointVertexMap.emplace(it, p, v);
				return v;
			}

			void add_edge()
			{

			}

		private:

			mutable graph            m_graph;
			mutable point_vertex_map m_pointVertexMap;

		};
	}

	template <typename Polygon, typename NumberComparisonPolicy>
	inline std::vector<polygon<typename geometric_traits<Polygon>::point_type>> split_self_intersections( const Polygon& poly, const NumberComparisonPolicy& cmp )
	{
		using geometrix::detail::loop_graph;
		typedef point_sequence_traits<Polygon> access;
		typedef typename access::point_type point_type;
		std::size_t size = access::size( poly );
		GEOMETRIX_ASSERT( size > 2 );

		std::vector<polygon<point_type>> results = { polygon<point_type>{ } };
		std::size_t iCurr = 0;

		auto next = [size]( std::size_t i ){ return (i + 1) % size; };
		auto adjacent = [&next](std::size_t i, std::size_t j) { return next(i) == j || next(j) == i; };
		auto process = [&]( std::size_t i, std::size_t j ) -> void
		{
			point_type x[2];
			auto iType = segment_segment_intersection(access::get_point(poly, i), access::get_point(poly, next(i)), access::get_point(poly, j), access::get_point(poly, next(j)), x, cmp);
			if (iType == e_non_crossing)
			{
				return;
			}

			if(iType == e_endpoint)
			{
				if (adjacent(i, j))
				{
					return;
				}
			}

			if (iType == e_crossing)
			{
				results[iCurr++].push_back(x[0]);
				results.emplace_back(1, x[0]);
			}

			if(iType == e_overlapping)
			{

			}
		};

		for (std::size_t i = 0; i < size - 2; ++i)
		{
			results[iCurr].push_back(access::get_point(poly, i));
			for (std::size_t j = i + 2; j < size; ++j)
			{
				process(i, j);
			}
		}

		return true;
	}

}//! namespace geometrix;


#include "swift_vio/PointLandmarkSimulation.hpp"

#include <random>
#include <Eigen/Core>

namespace simul {
void PointLandmarkSimulation::projectLandmarksToNFrame(
    const std::vector<Eigen::Vector4d,
                      Eigen::aligned_allocator<Eigen::Vector4d>>&
        homogeneousPoints,
    okvis::kinematics::Transformation& T_WS_ref,
    std::shared_ptr<const okvis::cameras::NCameraSystem> cameraSystemRef,
    std::shared_ptr<okvis::MultiFrame> nframes,
    std::vector<std::vector<size_t>>* frameLandmarkIndices,
    std::vector<std::vector<int>>* keypointIndices,
    const double* imageNoiseMag) {
  size_t numFrames = nframes->numFrames();
  std::vector<std::vector<cv::KeyPoint>> frame_keypoints;
  // project landmarks onto frames of nframes
  for (size_t i = 0; i < numFrames; ++i) {
    std::vector<size_t> lmk_indices;
    std::vector<cv::KeyPoint> keypoints;
    std::vector<int> frameKeypointIndices(homogeneousPoints.size(), -1);
    for (size_t j = 0; j < homogeneousPoints.size(); ++j) {
      Eigen::Vector2d projection;
      Eigen::Vector4d point_C = cameraSystemRef->T_SC(i)->inverse() *
                                T_WS_ref.inverse() * homogeneousPoints[j];
      okvis::cameras::CameraBase::ProjectionStatus status =
          cameraSystemRef->cameraGeometry(i)->projectHomogeneous(point_C,
                                                                 &projection);
      if (status == okvis::cameras::CameraBase::ProjectionStatus::Successful) {
        Eigen::Vector2d measurement(projection);
        if (imageNoiseMag) {
          std::random_device rd{};
          std::mt19937 gen{rd()};
          std::normal_distribution<> d{0, *imageNoiseMag};
          measurement[0] += d(gen);
          measurement[1] += d(gen);
        }
        frameKeypointIndices[j] = keypoints.size();
        keypoints.emplace_back(measurement[0], measurement[1], 8.0);
        lmk_indices.emplace_back(j);
      }
    }
    frameLandmarkIndices->emplace_back(lmk_indices);
    frame_keypoints.emplace_back(keypoints);
    nframes->resetKeypoints(i, keypoints);
    keypointIndices->emplace_back(frameKeypointIndices);
  }
}
}  // namespace simul


#define BOOST_TEST_MODULE test_tmc_paper_weighted_dataset
#include <boost/test/included/unit_test.hpp>

//#define TIMERS

#include "../lib/evaluator.h"
#include "../lib/border_system_creator.h"
#include "../lib/layer_partitioning_creator.h"
#include "../lib/classifier_tmc.h"

BOOST_AUTO_TEST_CASE( tmc_paper_int1 ) {
    const std::string names_file("data/tmc_paper_int1/tmc_paper.names");
    const std::string data_file("data/tmc_paper_int1/tmc_paper.data");
    const std::string eval_file("data/tmc_paper_int1/tmc_paper.test");
    BOOST_TEST_MESSAGE("Creating train sample from file: " << data_file);
    BOOST_TEST_MESSAGE("Creating evaluation sample from file: " << eval_file);

    std::shared_ptr<FeatureNames> pFN = std::make_shared<FeatureNames>(names_file);

    SampleCreator sample_creator;
    sample_creator.set_feature_names(pFN);

    std::shared_ptr<Sample> pSample = sample_creator.from_file(data_file);
    std::shared_ptr<Sample> pSampleEval = sample_creator.from_file(eval_file);

    BOOST_TEST_MESSAGE("Resulting sample size: " << pSample->size());
    BOOST_CHECK_EQUAL(pSample->dim(), 2);
    BOOST_CHECK_EQUAL(pSample->size(), 992);
    BOOST_CHECK_EQUAL(pSampleEval->size(), 992);
    std::shared_ptr<LayerPartitioningCreator> pLayerPartitioningCreator = std::make_shared<LayerPartitioningCreator>();
    pLayerPartitioningCreator->push_back(pSample);
    auto pEvaluator = std::make_shared<Evaluator>();
    std::pair<std::pair<double, double>, std::pair<double, double>> confusion_matrix_train, confusion_matrix_eval;
    double roc_err_train, roc_err_eval;
    size_t counter = 0;
    do {
        const auto pLP = pLayerPartitioningCreator->get_layer_partitioning();
        const auto pBSC = std::make_shared<BorderSystemCreator>();
        const auto pBS = pBSC->from_layer_partitioning(pLP);
        (*pEvaluator).set_classifier(std::make_shared<ClassifierTmc>(pBS));

        (*pEvaluator).set_sample(pSample);
        confusion_matrix_train = pEvaluator->get_confusion_matrix();
        roc_err_train = pEvaluator->get_roc_error();

        (*pEvaluator).set_sample(pSampleEval);
        confusion_matrix_eval = pEvaluator->get_confusion_matrix();
        roc_err_eval = pEvaluator->get_roc_error();

        BOOST_TEST_MESSAGE("");
        BOOST_TEST_MESSAGE("############      Iteration " << counter++ << "       ############");
        BOOST_TEST_MESSAGE("Train conf matr ### Eval conf matr");
        BOOST_TEST_MESSAGE(confusion_matrix_train.first.first << "  " << confusion_matrix_train.first.second << "    ###    "
        << confusion_matrix_eval.first.first << "   " << confusion_matrix_eval.first.second);
        BOOST_TEST_MESSAGE(confusion_matrix_train.second.first << "  " << confusion_matrix_train.second.second << "    ###    "
        << confusion_matrix_eval.second.first << "   " << confusion_matrix_eval.second.second);
        BOOST_TEST_MESSAGE("------------------------------------------------");
        BOOST_TEST_MESSAGE("Train rank err ### Eval rank err");
        BOOST_TEST_MESSAGE( roc_err_train << "    ###    " << roc_err_eval);
        BOOST_TEST_MESSAGE("################################################");

    }
    while (pLayerPartitioningCreator->do_one_step());
    BOOST_CHECK_EQUAL(roc_err_train, 0.070182427159209151);
    BOOST_CHECK_EQUAL(roc_err_eval, 0.11113911290322581);
    std::pair<std::pair<double, double>, std::pair<double, double>> confusion_matrix_eval_({{391,79.5},{105,416.5}});
    BOOST_CHECK(confusion_matrix_eval == confusion_matrix_eval_);
}

BOOST_AUTO_TEST_CASE( tmc_paper_int2 ) {
    const std::string names_file("data/tmc_paper_int2/tmc_paper.names");
    const std::string data_file("data/tmc_paper_int2/tmc_paper.data");
    const std::string eval_file("data/tmc_paper_int2/tmc_paper.test");
    BOOST_TEST_MESSAGE("Creating train sample from file: " << data_file);
    BOOST_TEST_MESSAGE("Creating evaluation sample from file: " << eval_file);

    std::shared_ptr<FeatureNames> pFN = std::make_shared<FeatureNames>(names_file);

    SampleCreator sample_creator;
    sample_creator.set_feature_names(pFN);

    std::shared_ptr<Sample> pSample = sample_creator.from_file(data_file);
    std::shared_ptr<Sample> pSampleEval = sample_creator.from_file(eval_file);

    BOOST_TEST_MESSAGE("Resulting sample size: " << pSample->size());
    BOOST_CHECK_EQUAL(pSample->dim(), 2);
    BOOST_CHECK_EQUAL(pSample->size(), 992);
    BOOST_CHECK_EQUAL(pSampleEval->size(), 992);
    std::shared_ptr<LayerPartitioningCreator> pLayerPartitioningCreator = std::make_shared<LayerPartitioningCreator>();
    pLayerPartitioningCreator->push_back(pSample);
    auto pEvaluator = std::make_shared<Evaluator>();
    std::pair<std::pair<double, double>, std::pair<double, double>> confusion_matrix_train, confusion_matrix_eval;
    double roc_err_train, roc_err_eval;
    size_t counter = 0;
    do {
        const auto pLP = pLayerPartitioningCreator->get_layer_partitioning();
        const auto pBSC = std::make_shared<BorderSystemCreator>();
        const auto pBS = pBSC->from_layer_partitioning(pLP);
        (*pEvaluator).set_classifier(std::make_shared<ClassifierTmc>(pBS));

        (*pEvaluator).set_sample(pSample);
        confusion_matrix_train = pEvaluator->get_confusion_matrix();
        roc_err_train = pEvaluator->get_roc_error();

        (*pEvaluator).set_sample(pSampleEval);
        confusion_matrix_eval = pEvaluator->get_confusion_matrix();
        roc_err_eval = pEvaluator->get_roc_error();

        BOOST_TEST_MESSAGE("");
        BOOST_TEST_MESSAGE("############      Iteration " << counter++ << "       ############");
        BOOST_TEST_MESSAGE("Train conf matr ### Eval conf matr");
        BOOST_TEST_MESSAGE(confusion_matrix_train.first.first << "  " << confusion_matrix_train.first.second << "    ###    "
                                                              << confusion_matrix_eval.first.first << "   " << confusion_matrix_eval.first.second);
        BOOST_TEST_MESSAGE(confusion_matrix_train.second.first << "  " << confusion_matrix_train.second.second << "    ###    "
                                                               << confusion_matrix_eval.second.first << "   " << confusion_matrix_eval.second.second);
        BOOST_TEST_MESSAGE("------------------------------------------------");
        BOOST_TEST_MESSAGE("Train rank err ### Eval rank err");
        BOOST_TEST_MESSAGE( roc_err_train << "    ###    " << roc_err_eval);
        BOOST_TEST_MESSAGE("################################################");

    }
    while (pLayerPartitioningCreator->do_one_step());
    BOOST_CHECK_EQUAL(roc_err_train, 0.06986423440821446);
    BOOST_CHECK_EQUAL(roc_err_eval, 0.109978619276795);
    std::pair<std::pair<double, double>, std::pair<double, double>> confusion_matrix_train_({{205289659,31186637},{41529598,211977223}});
    BOOST_CHECK(confusion_matrix_train == confusion_matrix_train_);
    std::pair<std::pair<double, double>, std::pair<double, double>> confusion_matrix_eval_({{389,87},{107,409}});
    BOOST_CHECK(confusion_matrix_eval == confusion_matrix_eval_);
}

BOOST_AUTO_TEST_CASE( tmc_paper_float1 ) {
    const std::string names_file("data/tmc_paper_float1/tmc_paper.names");
    const std::string data_file("data/tmc_paper_float1/tmc_paper.data");
    const std::string eval_file("data/tmc_paper_float1/tmc_paper.test");
    BOOST_TEST_MESSAGE("Creating train sample from file: " << data_file);
    BOOST_TEST_MESSAGE("Creating evaluation sample from file: " << eval_file);

    std::shared_ptr<FeatureNames> pFN = std::make_shared<FeatureNames>(names_file);

    SampleCreator sample_creator;
    sample_creator.set_feature_names(pFN);

    std::shared_ptr<Sample> pSample = sample_creator.from_file(data_file);
    std::shared_ptr<Sample> pSampleEval = sample_creator.from_file(eval_file);

    BOOST_TEST_MESSAGE("Resulting sample size: " << pSample->size());
    BOOST_CHECK_EQUAL(pSample->dim(), 2);
    BOOST_CHECK_EQUAL(pSample->size(), 992);
    BOOST_CHECK_EQUAL(pSampleEval->size(), 992);
    std::shared_ptr<LayerPartitioningCreator> pLayerPartitioningCreator = std::make_shared<LayerPartitioningCreator>();
    pLayerPartitioningCreator->push_back(pSample);
    auto pEvaluator = std::make_shared<Evaluator>();
    std::pair<std::pair<double, double>, std::pair<double, double>> confusion_matrix_train, confusion_matrix_eval;
    double roc_err_train, roc_err_eval, err_rate_train, err_rate_eval;
    size_t counter = 0;
    do {
        const auto pLP = pLayerPartitioningCreator->get_layer_partitioning();
        const auto pBSC = std::make_shared<BorderSystemCreator>();
        const auto pBS = pBSC->from_layer_partitioning(pLP);
        (*pEvaluator).set_classifier(std::make_shared<ClassifierTmc>(pBS));

        (*pEvaluator).set_sample(pSample);
        confusion_matrix_train = pEvaluator->get_confusion_matrix();
        roc_err_train = pEvaluator->get_roc_error();
        err_rate_train = pEvaluator->get_error_rate();

        (*pEvaluator).set_sample(pSampleEval);
        confusion_matrix_eval = pEvaluator->get_confusion_matrix();
        roc_err_eval = pEvaluator->get_roc_error();
        err_rate_eval = pEvaluator->get_error_rate();

        BOOST_TEST_MESSAGE("");
        BOOST_TEST_MESSAGE("############      Iteration " << counter++ << "       ############");
        BOOST_TEST_MESSAGE("Train conf matr ### Eval conf matr");
        BOOST_TEST_MESSAGE(confusion_matrix_train.first.first << "  " << confusion_matrix_train.first.second << "    ###    "
                                                              << confusion_matrix_eval.first.first << "   " << confusion_matrix_eval.first.second);
        BOOST_TEST_MESSAGE(confusion_matrix_train.second.first << "  " << confusion_matrix_train.second.second << "    ###    "
                                                               << confusion_matrix_eval.second.first << "   " << confusion_matrix_eval.second.second);
        BOOST_TEST_MESSAGE("------------------------------------------------");
        BOOST_TEST_MESSAGE("Train rank err ### Eval rank err");
        BOOST_TEST_MESSAGE( roc_err_train << "    ###    " << roc_err_eval);
        BOOST_TEST_MESSAGE("------------------------------------------------");
        BOOST_TEST_MESSAGE("Train err rate ### Eval err rate");
        BOOST_TEST_MESSAGE( err_rate_train << "    ###    " << err_rate_eval);
        BOOST_TEST_MESSAGE("################################################");

    }
    while (pLayerPartitioningCreator->do_one_step());
    BOOST_CHECK_EQUAL(roc_err_train, 0.070182427159209151);
    BOOST_CHECK_EQUAL(roc_err_eval, 0.11113911290322581);
    std::pair<std::pair<double, double>, std::pair<double, double>> confusion_matrix_eval_({{391,79.5},{105,416.5}});
    BOOST_CHECK(confusion_matrix_eval == confusion_matrix_eval_);
}


#include <iostream>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/search/kdtree.h>

#include <Eigen/Eigen>
#include <math.h>
#include <random>

//ROS
#include <ros/ros.h>
#include <ros/console.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Point.h>
#include <std_msgs/Float64MultiArray.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>

//Octomap
#include <octomap_msgs/Octomap.h>
#include <octomap_msgs/conversions.h>
#include <octomap/OcTree.h>
#include <dynamicEDT3D/dynamicEDTOctomap.h>

#include <mission.hpp>

using namespace std;

pcl::search::KdTree<pcl::PointXYZ> kdtreeLocalMap;

random_device rd;
default_random_engine eng(rd());
uniform_real_distribution<double>  rand_x;
uniform_real_distribution<double>  rand_y;
uniform_real_distribution<double>  rand_w;
uniform_real_distribution<double>  rand_h;

ros::Publisher all_map_pub;

vector<double> _state;

int obs_num;
double margin;
double x_min, y_min, z_min, x_max, y_max, z_max;
double r_min, r_max, h_min, h_max, resolution;

sensor_msgs::PointCloud2 globalMap_pcd;
pcl::PointCloud<pcl::PointXYZ> cloudMap;

void RandomMapGenerate(const SwarmPlanning::Mission& mission)
{
    double numel_e = 0.00001;
    pcl::PointXYZ pt_random;

    rand_x = uniform_real_distribution<double>(x_min, x_max);
    rand_y = uniform_real_distribution<double>(y_min, y_max);
    rand_w = uniform_real_distribution<double>(r_min, r_max);
    rand_h = uniform_real_distribution<double>(h_min, h_max);

    int obs_iter = 0;
    while(obs_iter < obs_num)
    {
        double x, y, w, h;
        x    = rand_x(eng);
        y    = rand_y(eng);
        w    = rand_w(eng);

        bool quadInObs = false;
        for (int qi = 0; qi < mission.qn; qi++) {
            if (sqrt(pow(x - mission.startState[qi][0], 2) + pow(y - mission.startState[qi][1], 2)) < mission.quad_size[qi] + w + margin ||
                sqrt(pow(x - mission.goalState[qi][0], 2) + pow(y - mission.goalState[qi][1], 2)) < mission.quad_size[qi] + w  + margin){
                quadInObs = true;
                break;
            }
        }
        if(quadInObs){
            continue;
        }
        x = floor(x/resolution) * resolution + resolution / 2.0;
        y = floor(y/resolution) * resolution + resolution / 2.0;

        int widNum = ceil(w/resolution);

        for(int r = -widNum/2.0; r < widNum/2.0; r ++ ) {
            for (int s = -widNum / 2.0; s < widNum / 2.0; s++) {
                h = rand_h(eng);
                int heiNum = ceil(h / resolution);
                for (int t = 0; t < heiNum; t++) {
                    pt_random.x = x + (r + 0.5) * resolution + numel_e;
                    pt_random.y = y + (s + 0.5) * resolution + numel_e;
                    pt_random.z = (t + 0.5) * resolution + numel_e;
                    cloudMap.points.push_back(pt_random);
                }
            }
        }

        obs_iter++;
    }

    cloudMap.width = cloudMap.points.size();
    cloudMap.height = 1;
    cloudMap.is_dense = true;

    ROS_WARN("Finished generate random map ");

    kdtreeLocalMap.setInputCloud( cloudMap.makeShared() );
}

void pubSensedPoints()
{
    pcl::toROSMsg(cloudMap, globalMap_pcd);
    globalMap_pcd.header.frame_id = "world";
    all_map_pub.publish(globalMap_pcd);
}

int main (int argc, char** argv) {
    ros::init (argc, argv, "random_map_generator");
    ros::NodeHandle n( "~" );
    ros::V_string args;
    ros::removeROSArgs(argc, argv, args);

    all_map_pub   = n.advertise<sensor_msgs::PointCloud2>("all_map", 1);

    n.param<double>("world/x_min", x_min, -5);
    n.param<double>("world/y_min", y_min, -5);
    n.param<double>("world/z_min", z_min, 0);
    n.param<double>("world/x_max", x_max, 5);
    n.param<double>("world/y_max", y_max, 5);
    n.param<double>("world/z_max", z_max, 2.5);
    n.param<double>("world/margin", margin, 1.5);

    n.param<int>("world/obs_num", obs_num,  6);
    n.param<double>("world/resolution",  resolution, 0.1);
    n.param<double>("world/r_min", r_min,   0.3);
    n.param<double>("world/r_max", r_max,   0.8);
    n.param<double>("world/h_min", h_min,   1.0);
    n.param<double>("world/h_max", h_max,   2.5);

    SwarmPlanning::Mission mission;
    if(!mission.setMission(n)){
        return -1;
    }

    // generate map msg
    RandomMapGenerate(mission);

    ros::Rate rate(10);
    int count = 0;
    while (ros::ok())
    {
        if(count < 100) {
            pubSensedPoints();
            count++;
        }
        ros::spinOnce();
        rate.sleep();
    }
}


///////////////////////////////////////////////////////////////////////////////
// kernel::functional::rp_visitor.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_KERNEL_ESTIMATION_DENSITY_VISITOR_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_KERNEL_ESTIMATION_DENSITY_VISITOR_HPP_ER_2009
#include <algorithm>
#include <boost/concept/assert.hpp>
#include <boost/concept_check.hpp>
#include <boost/call_traits.hpp>
#include <boost/call_traits.hpp>
#include <boost/type_traits/is_reference.hpp>
#include  <boost/type_traits/is_convertible.hpp>
#include <boost/mpl/nested_type.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/statistics/detail/kernel/estimation/detail/mean_accumulator.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace kernel{

    // A type that models Trainer and Predictor
    // See https://svn.boost.org/svn/boost/sandbox/statistics/cross_validation/boost/statistics/detail/cross_validation/estimator/concept/
    //
    // R:   range of a data range
    // V:   visitor
    // K:   kernel
    // A:   accumulator
    //
    // V                                      ->     data range of type R
    // rp_visitor                             ->     {x[i]:i=1,...,n}
    // meta_nw_visitor_unary<F1,F2>::apply    ->     {(x,y)[i]:i=1,...,n}
    //
    // It is recommended that R derives from iterator_range because it is cheap
    // to copy.
    template<
        typename R,
        template<typename,typename,typename> class V,
        typename K,
        typename A = typename
            detail::mean_accumulator<typename K::result_type>::type
    >
    class estimator{

        public:
        typedef R training_dataset_type;
        typedef K kernel_type;
        typedef A accumulator_type;

        template<typename X>
        struct visitor
        {
            typedef V<K,const X&,A> arg_;
            typedef typename mpl::nested_type<arg_>::type type;
        };

        template<typename X>
        struct result_of_predict
        {
            typedef typename visitor<X>::type v_;
            typedef typename v_::result_type type;
        };

        // Constructor
        estimator(){}
        estimator(K k)
            :k_(k){}
        estimator(K k,const A& a)
            :k_(k),a_(a){}
        estimator(const estimator& that)
            :td_(that.td_),k_(that.k_),a_(that.a_){}
        estimator& operator=(const estimator& that){
            if(&that = this){
                this->td_ = that.td_;
                this->k_ = that.k_;
                this->a_ = that.a_;
            }
            return *this;
        }

        // Access
        const R& training_dataset()const{
            return this->td_;
        }
        const K& kernel()const{
            return this->k_;
        }
        const A& accumulator()const{
            return this->a_;
        }

        void train(const R& training_dataset){
            BOOST_CONCEPT_ASSERT((
                boost::Assignable<R>
            ));
            this->td_ = training_dataset;
        }

        template<typename X>
        typename visitor<X>::type
        visit(const X& x)const{
            typedef estimator this_type;
            typedef typename visitor<X>::type v_;

            return std::for_each(
                boost::begin( this->training_dataset() ),
                boost::end( this->training_dataset() ),
                v_( this->kernel(), x, this->a_)
            );
        }

        template<typename X>
        typename result_of_predict<X>::type
        predict(const X& x)const{
            return visit(x).estimate();
        }

    protected:
        R td_;
        K k_;
        A a_; // Initialized accumulator
    };

}// kernel
}// detail
}// statistics
}// boost

#endif