#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/xfeatures2d.hpp>
#include <opencv2/xfeatures2d/nonfree.hpp>
#include <opencv2/opencv.hpp>

#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <sophus/se3.hpp>
#include <Eigen/Core>

#include <chrono>
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <cmath>
#include<math.h>
using namespace std;
using namespace cv;
//AVERAGE
double Average(vector<double> v)
{      double sum=0;
       for(int i=0;i<v.size();i++)
               sum+=v[i];
       return sum/v.size();
}
//DEVIATION
double Deviation(vector<double> v, double ave)
{
       double E=0;
       for(int i=0;i<v.size();i++){
               E+=(v[i] - ave)*(v[i] - ave);
       }
       return sqrt(E/v.size());
}
void writeResults( const string& filename, const vector<string>& timestamps, const vector<Mat>& Rt )
{
    CV_Assert( timestamps.size() == Rt.size() );

    ofstream file( filename.c_str() );
    if( !file.is_open() )
        return;

    cout.precision(4);
    for( size_t i = 0; i < Rt.size(); i++ )
    {
        const Mat& Rt_curr = Rt[i];
        if( Rt_curr.empty() )
            continue;

        CV_Assert( Rt_curr.type() == CV_64FC1 );

        Mat R = Rt_curr(Rect(0,0,3,3)), rvec;
        Rodrigues(R, rvec);
        double alpha = norm( rvec );
        if(alpha > DBL_MIN)
            rvec = rvec / alpha;

        double cos_alpha2 = std::cos(0.5 * alpha);
        double sin_alpha2 = std::sin(0.5 * alpha);

        rvec *= sin_alpha2;

        CV_Assert( rvec.type() == CV_64FC1 );
        // timestamp tx ty tz qx qy qz qw
        file << timestamps[i] << " " << fixed
             << Rt_curr.at<double>(0,3) << " " << Rt_curr.at<double>(1,3) << " " << Rt_curr.at<double>(2,3) << " "
             << rvec.at<double>(0) << " " << rvec.at<double>(1) << " " << rvec.at<double>(2) << " " << cos_alpha2 << endl;

    }
    file.close();
}

void find_feature_matches(
 const Mat &img_1, const Mat &img_2, std::vector<KeyPoint> &keypoints_1,vector<KeyPoint> &keypoints_2,std::vector<DMatch> &matches, const Mat &img_3);
 void find_feature_matches_another(
 const Mat &img_1, const Mat &img_2, std::vector<KeyPoint> &keypoints_1,vector<KeyPoint> &keypoints_2,std::vector<DMatch> &matches, const Mat &img_3);

// // 像素坐标转相机归一化坐标
 Point2d pixel2cam(const Point2d &p, const Mat &K);

// BA by g2o
typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVector3d;

Mat bundleAdjustmentG2O(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose,
  const string& filename,
  const vector<string>& timestamps
);

// BA by gauss-newton
Mat bundleAdjustmentGaussNewton(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose,
  int mode
);
double calc_residual(
   const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  Sophus::SE3d &pose,
  const Mat &K,
  vector<double>& residuals
);
int main(int argc, char **argv) {
	if(argc != 4){
        cout << "Format: file_with_rgb_depth_pairs trajectory_file odometry_name [Rgbd or ICP or RgbdICP or FastICP]" << endl;
        return -1;
   }

   vector<string> timestamps;
   vector<Mat> Rts, Rts_ba;
   Rts.push_back(Mat::eye(4,4,CV_64FC1));
   Rts_ba.push_back(Mat::eye(4,4,CV_64FC1));
   const string filename = argv[1];
   ifstream file( filename.c_str() );
   if( !file.is_open() )
      return -1;
   char dlmrt = '/';
   size_t pos = filename.rfind(dlmrt);
   string dirname = pos == string::npos ? "" : filename.substr(0, pos) + dlmrt;

   const int timestampLength = 17;
   const int rgbPathLehgth = 17+8;
   const int depthPathLehgth = 17+10;

   float fx = 517.3f, // default
         fy = 516.5f,
         cx = 318.6f,
         cy = 255.3f;
   // string value;
   // string inFileStr;
   // string myArray[10000];
   // int j = 0;

   // int arrSize = sizeof(myArray)/sizeof(myArray[0]);
   // cout << arrSize << endl;

   // cout << "first " << myArray[0] << endl;

   string datas[793];
   string str1;
   std::getline(file, str1);
	datas[0] = str1;
   string timestap3 = str1.substr(0, timestampLength);
   timestamps.push_back(timestap3);
   for(int i = 1; !file.eof(); i++)
   {
      string str;
      std::getline(file, str);
      datas[i] = str;
      if(str.empty()) break;
      if(str.at(0) == '#') continue; /* comment */
      cout << " previous image: " << datas[i-1] << "\n" << " current image "<< str << endl;
      Mat image, depth, image1, depth1, image2, depth2;
      if(i > 2) {
         string rgbFilename2 = datas[i-2].substr(timestampLength + 1, rgbPathLehgth );
         string timestap2 = datas[i-2].substr(0, timestampLength);
         string depthFilename2 = datas[i-2].substr(2*timestampLength + rgbPathLehgth + 3, depthPathLehgth );

         image2 = imread(dirname + rgbFilename2);
         depth2 = imread(dirname + depthFilename2, -1);
      }

	   string rgbFilename1 = datas[i-1].substr(timestampLength + 1, rgbPathLehgth );
      string timestap1 = datas[i-1].substr(0, timestampLength);
      string depthFilename1 = datas[i-1].substr(2*timestampLength + rgbPathLehgth + 3, depthPathLehgth );
      image1 = imread(dirname + rgbFilename1);
      depth1 = imread(dirname + depthFilename1, -1);

	   string rgbFilename = str.substr(timestampLength + 1, rgbPathLehgth );
      string timestap = str.substr(0, timestampLength);
      string depthFilename = str.substr(2*timestampLength + rgbPathLehgth + 3, depthPathLehgth );
      image = imread(dirname + rgbFilename);
      depth = imread(dirname + depthFilename, -1);
      // cout << "height " << image.rows << " width " << image.cols << endl; // 480 * 640
      // cout << "prev prev " << datas[i-2] << " previous image: " << datas[i-1] << " current image "<< str << endl;
      CV_Assert(!image.empty());
      CV_Assert(!depth.empty());
      CV_Assert(!image1.empty());
      CV_Assert(!depth1.empty());
      CV_Assert(depth.type() == CV_16UC1);
      CV_Assert(depth1.type() == CV_16UC1);

      if(i > 2){
         CV_Assert(!image2.empty());
         CV_Assert(!depth2.empty());
         CV_Assert(depth2.type() == CV_16UC1);
      }
      //   //-- 读取图像
      //   Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
      //   Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);
      //   assert(img_1.data && img_2.data && "Can not load images!");

      std::vector<KeyPoint> keypoints_1, keypoints_2, key1, key2;
      vector<DMatch> matches;
      Ptr<FeatureDetector> detector = ORB::create();
      detector->detect(image1, key1);
      detector->detect(image, key2);
      // if(key1.size() == 0 || key2.size() == 0){
      //    find_feature_matches_another(image1, image, keypoints_1, keypoints_2, matches, image2);
      //    cout << "第二個: " <<  "一共找到了" << matches.size() << "组匹配点" << endl;
      // }
      // else{
         find_feature_matches(image1, image, keypoints_1, keypoints_2, matches, image2);
         // find_feature_matches(image1, image, keypoints_1, keypoints_2, matches, image2);
         cout << "第一個: " <<"一共找到了" << matches.size() << "组匹配点" << endl;
      // }

      // 建立3D点
      //Mat d1 = imread(depth1, IMREAD_UNCHANGED);       // 深度图为16位无符号数，单通道图像
      Mat K = (Mat_<double>(3, 3) << 517.3f, 0, 318.6f, 0, 516.5f, 255.3f, 0, 0, 1);
      vector<Point3f> pts_3d;
      vector<Point2f> pts_2d;
      std::vector<KeyPoint> keys1, keys2;
      std::vector<cv::Point2f> points1, points2;
      int index = 1;
      for (DMatch m:matches) {
         // if (index <keypoints_2.size()){
            ushort d = depth1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
            // cout << "depth " << d << endl;
            if (d == 0){   // bad depth
               continue;
               // d = 1;
            }
            // cout << "this is matches: " <<  index << endl;
            float dd = d / 5000.0;

            // cout << "keypoints_1[m.queryIdx].pt " << keypoints_1[m.queryIdx].pt << " keypoints_2[m.trainIdx].pt " << keypoints_2[m.trainIdx].pt << " depth " << d << endl;
            Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
            keys1.push_back(keypoints_1[m.queryIdx]);
            keys2.push_back(keypoints_2[m.trainIdx]);
            points1.push_back(keypoints_1[m.queryIdx].pt);
            points2.push_back(keypoints_2[m.trainIdx].pt);
            // cout << "p1.x " << p1.x << " p1.y " << p1.y << " depth " << dd << endl;

            pts_3d.push_back(Point3f(p1.x * dd, p1.y * dd, dd));
            pts_2d.push_back(keypoints_2[m.trainIdx].pt);
            index += 1;
         // }
      }
      bool b = false;
      // if(i ==373) {b = true;}
      cout << "3d-2d pairs: " << pts_3d.size() << " " << pts_2d.size() << " "  << i <<  endl;
      keypoints_1.clear();
      keypoints_1 = keys1;
      keypoints_2.clear();
      keypoints_2 = keys2;
      chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
      Mat r, t, inliers;
      solvePnPRansac(pts_3d, pts_2d, K, Mat(), r, t, b, 1000, 6.0, 0.99, inliers, SOLVEPNP_ITERATIVE);
      // cout << inliers << inliers.size()  << inliers.at<int>(1,0)  << inliers.at<int>(3,0)<< endl;
      for (int i=0; i<inliers.rows; i++){
         cout << "inliers -> keypoints_1: " << keypoints_1[inliers.at<int>(i,0)].pt << "inliers -> keypoints_2: " << keypoints_2[inliers.at<int>(i,0)].pt << endl;
      }
      // cout << "inliers "<< inliers << endl;
      cout<<"pnp OK = "<<b<<", inliers point num = "<<inliers.rows<<endl;
      vector<Scalar> colors;
      RNG rng;
      for(int j = 0; j < 100; j++){
         int r = rng.uniform(0, 256);
         int g = rng.uniform(0, 256);
         int b = rng.uniform(0, 256);
         colors.push_back(Scalar(r,g,b));
      }
      for(int j=0; j<inliers.rows; j++){
         circle(image1, keys1[inliers.at<int>(j,0)].pt, 5, colors[j], -1);
         circle(image, keys2[inliers.at<int>(j,0)].pt, 5, colors[j], -1);
      }

      Mat combined_img;
      hconcat(image1,image,combined_img);
      imwrite("./images/" + to_string(i) + ".jpg", combined_img);
      // imwrite("./images/" + to_string(i) + ".jpg", image);
      Mat R;
      cv::Rodrigues(r, R); // r为旋转向量形式，用Rodrigues公式转换为矩阵
      chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
      chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
      cout << "solve pnp in opencv cost time: " << time_used.count() << " seconds." << endl;
      cout << "R=" << endl << R << endl;
      cout << "t=" << endl << t << endl;
      timestamps.push_back( timestap );
      Mat output;
      hconcat(R, t, output);

      Mat Rt = Mat::eye(4,4,CV_64FC1);
      Rt.at<double>(0,0) = R.at<double>(0,0);
      Rt.at<double>(0,1) = R.at<double>(0,1);
      Rt.at<double>(0,2) = R.at<double>(0,2);
      Rt.at<double>(1,0) = R.at<double>(1,0);
      Rt.at<double>(1,1) = R.at<double>(1,1);
      Rt.at<double>(1,2) = R.at<double>(1,2);
      Rt.at<double>(2,0) = R.at<double>(2,0);
      Rt.at<double>(2,1) = R.at<double>(2,1);
      Rt.at<double>(2,2) = R.at<double>(2,2);
      Rt.at<double>(0,3) = t.at<double>(0,0);
      Rt.at<double>(1,3) = t.at<double>(0,1);
      Rt.at<double>(2,3) = t.at<double>(0,2);

      Mat& prevRt = *Rts.rbegin();
      cout << "prevRt " << prevRt << endl;
      cout << "Rt " << Rt << endl;

      // for (int l=0; l<2; l++){
      //    cout << pts_3d[l].x << endl;
      // }
      // for (int i=0; i<inliers.rows; i++){
      //    cout << "inliers -> keypoints_1: " << keypoints_1[inliers.at<int>(i,0)].pt << "inliers -> keypoints_2: " << keypoints_2[inliers.at<int>(i,0)].pt << endl;
      // }
      for (int i=0; i<inliers.rows; i++){
         Mat m( 4,1, CV_64FC1);
         m.at<double>(0,0) = keypoints_1[inliers.at<int>(i,0)].pt.x;
         m.at<double>(1,0) = keypoints_1[inliers.at<int>(i,0)].pt.y;
         m.at<double>(2,0) = pts_3d[i].z;
         m.at<double>(3,0) = 1;
         // cout << "original " << m.t() << " projected " << (prevRt * Rt * m).t() << "  " << "actual " << keypoints_2[inliers.at<int>(i,0)].pt<< endl;
      }
      Rts.push_back(prevRt * Rt);
      Mat Rt_ba;
      VecVector3d pts_3d_eigen;
      VecVector2d pts_2d_eigen;
      for (size_t i = 0; i < pts_3d.size(); ++i) {
         // cout << "vector3d " << Eigen::Vector3d(pts_3d[i].x, pts_3d[i].y, pts_3d[i].z) << "vector2d " << Eigen::Vector2d(pts_2d[i].x, pts_2d[i].y) << endl;
         pts_3d_eigen.push_back(Eigen::Vector3d(pts_3d[i].x, pts_3d[i].y, pts_3d[i].z));
         pts_2d_eigen.push_back(Eigen::Vector2d(pts_2d[i].x, pts_2d[i].y));
      }
      // for (size_t i = 0; i < pts_3d.size(); ++i) {
      //    for (size_t j = 0; j < 3; ++j) {
      //       cout << " eigen3d " << pts_3d_eigen[i][j] << cout << " eigen2d "  <<*pts_2d_eigen[i][j];
      //    }
      // }
      // cout << "calling bundle adjustment by g2o" << endl;
      // Sophus::SE3d pose_g2o;
      // t1 = chrono::steady_clock::now();
      // Rt_ba = bundleAdjustmentG2O(pts_3d_eigen, pts_2d_eigen, K, pose_g2o, argv[2], timestamps);
      // Mat& prevRtba = *Rts_ba.rbegin();
      // Rts_ba.push_back(prevRtba * Rt_ba);
      // t2 = chrono::steady_clock::now();
      // time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
      // cout << "solve pnp by g2o cost time: " << time_used.count() << " seconds." << endl;
     cout << "calling bundle adjustment by gauss newton" << endl;
     Sophus::SE3d pose_gn;
     t1 = chrono::steady_clock::now();
     int mode = 0; // 0=huber
     Mat Rt_baGauss = bundleAdjustmentGaussNewton(pts_3d_eigen, pts_2d_eigen, K, pose_gn, mode);
      Mat& prevRtbaGauss = *Rts_ba.rbegin();
      cout << "prevRt " << prevRtbaGauss << endl;
      cout << "Rt " << RtGauss << endl;
      Rts_ba.push_back(prevRtbaGauss * Rt_baGauss);
     t2 = chrono::steady_clock::now();
     time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
     cout << "solve pnp by gauss newton cost time: " << time_used.count() << " seconds." << endl;

   }
   // writeResults(argv[2], timestamps, Rts);
   writeResults(argv[2], timestamps, Rts_ba);

  return 0;
}
void find_feature_matches_another(const Mat &img_1, const Mat &img_2,
                           std::vector<KeyPoint> &keypoints_1,
			                  std::vector<KeyPoint> &keypoints_2,
                           std::vector<DMatch> &matches,
                           const Mat &img_3) {
   Mat descriptors_1, descriptors_2, descriptors_3, descriptors_4;
   Ptr<FeatureDetector> detector = AgastFeatureDetector::create();
   Ptr<DescriptorExtractor> descriptor = AgastFeatureDetector::create();
   Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
   detector->detect(img_1, keypoints_1);
   detector->detect(img_2, keypoints_2);
   cout << "keypoints_1.size() "<< keypoints_1.size() << " keypoints_2.size() " << keypoints_2.size() << endl;
   // if(keypoints_1.size() != 0 && keypoints_2.size() != 0){
      for (int i=0; i< keypoints_1.size(); i++){
         // cout << "keypoint1 " << keypoints_1[i].pt << "keypoint2 " << keypoints_2[i].pt << endl;
      }
      descriptor->compute(img_1, keypoints_1, descriptors_1);
      descriptor->compute(img_2, keypoints_2, descriptors_2);
   // }
   int eee = descriptors_1.empty();
   int ddd = descriptors_2.empty();

   vector<DMatch> match;
   // Mat de1, de2;
   // if(ddd == true || eee == true){
   //    de1 = descriptors_3;
   //    de2 = descriptors_4;
   // }else {
   //    de1 = descriptors_1;
   //    de2 = descriptors_2;
   // }
   // matcher->match(de1, de2, match);
   matcher->match(descriptors_1, descriptors_2, match);
   double min_dist = 10000, max_dist = 0;
     for (int i = 0; i < descriptors_1.rows; i++) {
    double dist = match[i].distance;
     if (dist < min_dist) min_dist = dist;
     if (dist > max_dist) max_dist = dist;
   }
   printf("-- Max dist : %f \n", max_dist);
   printf("-- Min dist : %f \n", min_dist);
   for (int i = 0; i < descriptors_1.rows; i++) {
    if (match[i].distance <= max(2 * min_dist, 10.0)) {
       matches.push_back(match[i]);
     }
   }
 }
 void find_feature_matches(const Mat &img_1, const Mat &img_2,
                           std::vector<KeyPoint> &keypoints_1,
			                  std::vector<KeyPoint> &keypoints_2,
                           std::vector<DMatch> &matches,
                           const Mat &img_3) {

   Mat descriptors_1, descriptors_2, descriptors_3, descriptors_4;
   Ptr<FeatureDetector> detector = ORB::create();
   Ptr<DescriptorExtractor> descriptor = ORB::create();
   // Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
   Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("FlannBased");
   // cv::FlannBasedMatcher matcher = cv::FlannBasedMatcher(cv::makePtr<cv::flann::LshIndexParams>(12, 20, 2));
   detector->detect(img_1, keypoints_1);
   detector->detect(img_2, keypoints_2);
   cout << "keypoints_1.size() "<< keypoints_1.size() << " keypoints_2.size() " << keypoints_2.size() << endl;
   // if(keypoints_1.size() != 0 && keypoints_2.size() != 0){

      descriptor->compute(img_1, keypoints_1, descriptors_1);
      descriptor->compute(img_2, keypoints_2, descriptors_2);
   // }
   // cout << "des1 " << descriptors_1  << "des2 " << descriptors_2 << endl;
   int eee = descriptors_1.empty();
   int ddd = descriptors_2.empty();
   // for (int i=0; i< keypoints_1.size(); i++){
   //    cout << "keypoint1 " << keypoints_1[i].pt << "keypoint2 " << keypoints_2[i].pt << endl;
   // }
   // cout << "ddd " << ddd << " eee " << eee << endl;
   // if(ddd == true || eee == true){
   //    keypoints_1.clear();
   //    keypoints_2.clear();
   //    detector->detect( img_3, keypoints_1 );
   //    detector->detect( img_2, keypoints_2 );
   //    cout << "keypoints_3.size() "<< keypoints_1.size() << " keypoints_3.size() " << keypoints_2.size() << endl;
   //    for (int i=0; i< keypoints_1.size(); i++){
   //       cout << "keypoint3 " << keypoints_1[i].pt << "keypoint4 " << keypoints_2[i].pt << endl;
   //    }
   //    descriptor->compute( img_3, keypoints_1, descriptors_3 );
   //    descriptor->compute( img_2, keypoints_2, descriptors_4 );
   //    cout << "des3 " << descriptors_3  << "des4 " << descriptors_4 << endl;

   // }
   vector<DMatch> match;
   // BFMatcher matcher ( NORM_HAMMING );
   // Mat de1, de2;

   // if(ddd == true || eee == true){
   //    de1 = descriptors_3;
   //    de2 = descriptors_4;
   // }else {
   //    de1 = descriptors_1;
   //    de2 = descriptors_2;
   // }
   // matcher->match(de1, de2, match);

   // matcher->match(descriptors_1, descriptors_2, match);
   vector<vector<DMatch>> knn_matches;
   descriptors_1.convertTo(descriptors_1, CV_32F);
   descriptors_2.convertTo(descriptors_2, CV_32F);
   matcher->knnMatch( descriptors_1, descriptors_2, knn_matches, 2);
   // descriptors_1.convertTo(descriptors_1, CV_8UC1);
   // descriptors_2.convertTo(descriptors_2, CV_8UC1);
   // matcher.knnMatch( descriptors_1, descriptors_2, knn_matches, 2);
   // for (size_t k = 0; k < knn_matches.size(); k++)
   //  {
   //      for (size_t i = 0; i < knn_matches[k].size(); i++)
   //      {
   //          // const cv::DMatch& match = knn_matches[k][i];
   //          cout << knn_matches[i][k].trainIdx << " ";
   //      }
   //      cout << endl;
   //  }
    const float ratio_thresh = 0.7f;
   //  std::vector<DMatch> good_matches;
    for (size_t i = 0; i < knn_matches.size(); i++)
    {
        if (knn_matches[i][0].distance < ratio_thresh * knn_matches[i][1].distance)
        {
            matches.push_back(knn_matches[i][0]);
        }
    }
   // double min_dist = 10000, max_dist = 0;
   // for (int i = 0; i < descriptors_1.rows; i++) {
   //    double dist = match[i].distance;
   //    if (dist < min_dist) min_dist = dist;
   //    if (dist > max_dist) max_dist = dist;
   // }
   // cout << match.size() << endl;
   // printf("-- Max dist : %f \n", max_dist);
   // printf("-- Min dist : %f \n", min_dist);
   // for (int i = 0; i < match.size(); i++) {
   //  if (match[i].distance <= max(2 * min_dist, 10.0)) {
   //    // if (matches[i].distance < 2 * min_dist) {
   //       matches.push_back(match[i]);
   //   }
   // }

 }

 Point2d pixel2cam(const Point2d &p, const Mat &K) {
   return Point2d
     (
       (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
       (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
     );
 }
double calc_residual(
   const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  Sophus::SE3d &pose,
  const Mat &K,
  vector<double>& residuals
){
double fx = K.at<double>(0, 0);
  double fy = K.at<double>(1, 1);
  double cx = K.at<double>(0, 2);
  double cy = K.at<double>(1, 2);
  vector<double> res_std;
  for (int i=0; i<points_3d.size(); i++){
     Eigen::Vector3d pc = pose * points_3d[i];
     Eigen::Vector2d proj(fx * pc[0] / pc[2] + cx, fy * pc[1] / pc[2] + cy);
     Eigen::Vector2d error = points_2d[i] - proj;
     residuals.push_back(error.squaredNorm());
      if (isnan(pc[2]) == false) {
         res_std.push_back(error.squaredNorm());
      }
  }
  double avg = Average(res_std);
  double std = Deviation(res_std,avg);
  return std;
}
Mat bundleAdjustmentGaussNewton(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose,
  int mode) {
  typedef Eigen::Matrix<double, 6, 1> Vector6d;
  const int iterations = 100;
  double cost = 0, lastCost = 0;
  double fx = K.at<double>(0, 0);
  double fy = K.at<double>(1, 1);
  double cx = K.at<double>(0, 2);
  double cy = K.at<double>(1, 2);
   vector<double> residuals;
   double res_std = calc_residual(points_3d, points_2d, pose, K, residuals);
   // for(int i=0; i < residuals.size(); i++) {
   //    cout << residuals.at(i) << endl;
   // }
   cout << "deviation: " << res_std << endl;
   double huber_k = 1.345 * res_std;
   vector<double> weight;
   if(mode == 0){
      for (int j=0; j<residuals.size(); j++){
         if(residuals[j] <= huber_k){
            weight.push_back(1.0);
         }else {
            weight.push_back(huber_k/residuals[j]);
         }
      }
   }else {
      for (int j=0; j<residuals.size(); j++){
         weight.push_back(0.0);
      }
   }

  for (int iter = 0; iter < iterations; iter++) {
    Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
    Vector6d b = Vector6d::Zero();

    cost = 0;
    // compute cost
    for (int i = 0; i < points_3d.size(); i++) {
      Eigen::Vector3d pc = pose * points_3d[i];
      double inv_z = 1.0 / pc[2];
      double inv_z2 = inv_z * inv_z;
      Eigen::Vector2d proj(fx * pc[0] / pc[2] + cx, fy * pc[1] / pc[2] + cy);

      Eigen::Vector2d e = points_2d[i] - proj;

      cost += e.squaredNorm();
      Eigen::Matrix<double, 2, 6> J;
      J << -fx * inv_z,
        0,
        fx * pc[0] * inv_z2,
        fx * pc[0] * pc[1] * inv_z2,
        -fx - fx * pc[0] * pc[0] * inv_z2,
        fx * pc[1] * inv_z,
        0,
        -fy * inv_z,
        fy * pc[1] * inv_z2,
        fy + fy * pc[1] * pc[1] * inv_z2,
        -fy * pc[0] * pc[1] * inv_z2,
        -fy * pc[0] * inv_z;

      H += J.transpose() * (J * weight[i]);
      b += -J.transpose() * (e * weight[i]);
    }

    Vector6d dx;
    dx = H.ldlt().solve(b);

    if (isnan(dx[0])) {
      cout << "result is nan!" << endl;
      break;
    }

    if (iter > 0 && cost >= lastCost) {
      // cost increase, update is not good
      cout << "cost: " << cost << ", last cost: " << lastCost << endl;
      break;
    }

    // update your estimation
    pose = Sophus::SE3d::exp(dx) * pose;
    lastCost = cost;

    cout << "iteration " << iter << " cost=" << std::setprecision(12) << cost << endl;
    if (dx.norm() < 1e-6) {
      // converge
      break;
    }

  }
   Mat Rt = Mat::eye(4,4,CV_64FC1);
      Rt.at<double>(0,0) = pose.matrix()(0);
      Rt.at<double>(1,0) = pose.matrix()(1);
      Rt.at<double>(2,0) = pose.matrix()(2);
      Rt.at<double>(3,0) = pose.matrix()(3);
      Rt.at<double>(0,1) = pose.matrix()(4);
      Rt.at<double>(1,1) = pose.matrix()(5);
      Rt.at<double>(2,1) = pose.matrix()(6);
      Rt.at<double>(3,1) = pose.matrix()(7);
      Rt.at<double>(0,2) = pose.matrix()(8);
      Rt.at<double>(1,2) = pose.matrix()(9);
      Rt.at<double>(2,2) = pose.matrix()(10);
      Rt.at<double>(3,2) = pose.matrix()(11);
      Rt.at<double>(0,3) = pose.matrix()(12);
      Rt.at<double>(1,3) = pose.matrix()(13);
      Rt.at<double>(2,3) = pose.matrix()(14);
      Rt.at<double>(3,3) = pose.matrix()(15);

      cout << "pose by g-n: \n" << pose.matrix() << endl;

      return Rt;
}

/// vertex and edges used in g2o ba
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> {
   public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

   virtual void setToOriginImpl() override {
      _estimate = Sophus::SE3d();
   }

   /// left multiplication on SE3
   virtual void oplusImpl(const double *update) override {
      Eigen::Matrix<double, 6, 1> update_eigen;
      update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
      _estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
   }

   virtual bool read(istream &in) override {}

   virtual bool write(ostream &out) const override {}
};

class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose> {
   public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

      EdgeProjection(const Eigen::Vector3d &pos, const Eigen::Matrix3d &K) : _pos3d(pos), _K(K) {}

   virtual void computeError() override {
      const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
      Sophus::SE3d T = v->estimate();
      Eigen::Vector3d pos_pixel = _K * (T * _pos3d);
      pos_pixel /= pos_pixel[2];
      _error = _measurement - pos_pixel.head<2>();
   }

   virtual void linearizeOplus() override {
      const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
      Sophus::SE3d T = v->estimate();
      Eigen::Vector3d pos_cam = T * _pos3d;
      double fx = _K(0, 0);
      double fy = _K(1, 1);
      double cx = _K(0, 2);
      double cy = _K(1, 2);
      double X = pos_cam[0];
      double Y = pos_cam[1];
      double Z = pos_cam[2];
      double Z2 = Z * Z;
      _jacobianOplusXi
         << -fx / Z, 0, fx * X / Z2, fx * X * Y / Z2, -fx - fx * X * X / Z2, fx * Y / Z,
         0, -fy / Z, fy * Y / (Z * Z), fy + fy * Y * Y / Z2, -fy * X * Y / Z2, -fy * X / Z;
   }

   virtual bool read(istream &in) override {}

   virtual bool write(ostream &out) const override {}

   private:
      Eigen::Vector3d _pos3d;
      Eigen::Matrix3d _K;
};

Mat bundleAdjustmentG2O(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose,
  const string& filename,
  const vector<string>& timestamps) {

  // 构建图优化，先设定g2o
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType;  // pose is 6, landmark is 3
  typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
  // 梯度下降方法，可以从GN, LM, DogLeg 中选
  auto solver = new g2o::OptimizationAlgorithmGaussNewton(
  g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
  g2o::SparseOptimizer optimizer;     // 图模型
  optimizer.setAlgorithm(solver);   // 设置求解器
  optimizer.setVerbose(true);       // 打开调试输出

  // vertex
  VertexPose *vertex_pose = new VertexPose(); // camera vertex_pose
  vertex_pose->setId(0);
  vertex_pose->setEstimate(Sophus::SE3d());
  optimizer.addVertex(vertex_pose);

  // K
  Eigen::Matrix3d K_eigen;
  K_eigen <<
          K.at<double>(0, 0), K.at<double>(0, 1), K.at<double>(0, 2),
    K.at<double>(1, 0), K.at<double>(1, 1), K.at<double>(1, 2),
    K.at<double>(2, 0), K.at<double>(2, 1), K.at<double>(2, 2);

  // edges
  int index = 1;
  for (size_t i = 0; i < points_2d.size(); ++i) {
    auto p2d = points_2d[i];
    auto p3d = points_3d[i];
    EdgeProjection *edge = new EdgeProjection(p3d, K_eigen);
    edge->setId(index);
    edge->setVertex(0, vertex_pose);
    edge->setMeasurement(p2d);
    edge->setInformation(Eigen::Matrix2d::Identity());
    optimizer.addEdge(edge);
    index++;
  }

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  optimizer.setVerbose(true);
  optimizer.initializeOptimization();
  optimizer.optimize(10);
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "optimization costs time: " << time_used.count() << " seconds." << endl;
  cout << "pose estimated by g2o =\n" << vertex_pose->estimate().matrix() << endl;
//   cout << "pose matrix "  << vertex_pose->estimate().matrix()(0) << vertex_pose->estimate().matrix()(1) << vertex_pose->estimate().matrix()(4) << endl;

  Mat Rt = Mat::eye(4,4,CV_64FC1);
  Rt.at<double>(0,0) = vertex_pose->estimate().matrix()(0);
  Rt.at<double>(1,0) = vertex_pose->estimate().matrix()(1);
  Rt.at<double>(2,0) = vertex_pose->estimate().matrix()(2);
  Rt.at<double>(3,0) = vertex_pose->estimate().matrix()(3);
  Rt.at<double>(0,1) = vertex_pose->estimate().matrix()(4);
  Rt.at<double>(1,1) = vertex_pose->estimate().matrix()(5);
  Rt.at<double>(2,1) = vertex_pose->estimate().matrix()(6);
  Rt.at<double>(3,1) = vertex_pose->estimate().matrix()(7);
  Rt.at<double>(0,2) = vertex_pose->estimate().matrix()(8);
  Rt.at<double>(1,2) = vertex_pose->estimate().matrix()(9);
  Rt.at<double>(2,2) = vertex_pose->estimate().matrix()(10);
  Rt.at<double>(3,2) = vertex_pose->estimate().matrix()(11);
  Rt.at<double>(0,3) = vertex_pose->estimate().matrix()(12);
  Rt.at<double>(1,3) = vertex_pose->estimate().matrix()(13);
  Rt.at<double>(2,3) = vertex_pose->estimate().matrix()(14);
  Rt.at<double>(3,3) = vertex_pose->estimate().matrix()(15);

  pose = vertex_pose->estimate();
  return Rt;
}


// Compute and store ridgelet coefficients in linearized form
// Input: fsolution*h5 file(s) computed with bte_omp_ftcg executable

#include <hdf5.h>
#include <omp.h>
#include <yaml-cpp/yaml.h>
#include <Eigen/Dense>
#include <algorithm>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <cmath>
#include <cstdio>
#include <iostream>
#include <memory>
#include <regex>
#include <stdexcept>
#include <string>
#include <tuple>

#include "base/eigen2hdf.hpp"
#include "base/exceptions.hpp"
#include "brt_config.h"
#include "fft/fft2_r2c.hpp"
#include "ridgelet/rc_linearize.hpp"
#include "ridgelet/ridgelet_cell_array.hpp"
#include "ridgelet/ridgelet_frame.hpp"
#include "ridgelet/rt.hpp"
#include "spectral/quadrature/qhermitew.hpp"

typedef FFTr2c<PlannerR2C> fft_t;
// typedef FFTr2c<PlannerR2COD> fft_t;

unsigned int PLANNER_STRATEGY = FFTW_MEASURE;

typedef RT<double, RidgeletFrame, fft_t> RT_t;
typedef RT_t::rt_coeff_t rt_coeff_t;
typedef RidgeletCellArray<rt_coeff_t> rca_t;

namespace po = boost::program_options;

std::string out_fname = "rt_coeffs.h5";

int get_frame(const std::string& fname)
{
  std::regex my_regex(".*solution_vector([0-9]*)");
  std::smatch match;
  bool found = std::regex_search(fname, match, my_regex);
  if (found) {
    return atoi(match[1].str().c_str());
  } else {
    throw std::runtime_error("could not find frame number");
  }
}

unsigned int get_ncoeffs(const RidgeletFrame& rf)
{
  unsigned int nc = 0;
  for (auto& lam : rf.lambdas()) {
    auto t = tgrid_dim(lam, rf);
    unsigned int TX = std::get<0>(t) * std::get<1>(t);
    nc += TX;
  }
  return nc;
}

bool is_power_of_two(int x)
{
  return (x>0) && !(x & (x-1));
}

int main(int argc, char* argv[])
{
  std::cout << "SOURCE::INFO::GIT_BRANCHNAME " << GIT_BNAME << "\n";
  std::cout << "SOURCE::INFO::GIT_SHA1       " << GIT_SHA1 << "\n";

  // disable buffering of printf
  std::setbuf(stdout, NULL);

  po::options_description options("options");
  options.add_options()
      ("help", "produce help message")
      ("dst", po::value<std::string>(), "write output to dst");

  po::variables_map vm;
  try {
    po::store(po::parse_command_line(argc, argv, options), vm);
    po::notify(vm);
  } catch (std::exception& e) {
    if (vm.count("help"))
      std::cout << options << "\n";
    else
      std::cout << e.what() << "\n";
    return 1;
  }
  if (vm.count("help")) {
    std::cout << options << "\n";
    return 0;
  }

  if (vm.count("dst")) {
    out_fname = vm["dst"].as<std::string>();
  }
  std::cerr << "writing output to " << out_fname << std::endl;

  typedef Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> array_t;
  // read input files from stdin and check if they exist
  std::string input_line;
  std::vector<std::string> fnames;
  while (std::cin) {
    std::getline(std::cin, input_line);
    // check for empty (e.g. terminating line)
    std::regex my_regex("^([[:space:]]*|)$");
    std::smatch my_match;
    std::regex_search(input_line, my_match, my_regex);
    // cout << "match size: " << my_match.size() << "\n";
    if (my_match.size() > 0) {
      break;
    } else {
      if (boost::filesystem::is_regular_file(input_line) ||
          boost::filesystem::is_symlink(input_line))
        fnames.push_back(input_line);
      else
        throw std::runtime_error("File " + input_line + " does not exist. Exiting.");
    }
  };

  std::cout << "Found " << fnames.size() << " input files\n";

  // load files
  std::shared_ptr<RidgeletFrame> rf_ptr;
  std::shared_ptr<RCLinearize> rcl_ptr;

  hid_t h5f_out = H5Fcreate(out_fname.c_str(), H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);

  for (auto& fname : fnames) {
    int frame = get_frame(fname);
    array_t coeffs;
    hid_t h5f = H5Fopen(fname.c_str(), H5F_ACC_RDONLY, H5P_DEFAULT);
    eigen2hdf::load(h5f, "C", coeffs);
    int N = coeffs.rows();
    int L = coeffs.cols();
    int K = std::sqrt(N);
    ASSERT(K * K == N);
    int Lx = std::sqrt(L);
    std::cout << "Lx: " << Lx << "\n";
    ASSERT(Lx * Lx == L);
    ASSERT(is_power_of_two(Lx));
    H5Fclose(h5f);

    if (!rf_ptr) {
      int J = std::log2(2 * Lx) - 2;
      std::cout << "J: "  << J << "\n";
      rf_ptr = std::make_shared<RidgeletFrame>(J, J, 1, 1);
      rcl_ptr = std::make_shared<RCLinearize>(*rf_ptr);
      fft_t fft;
      init_fftw(fft, PLANNER_STRATEGY, *rf_ptr);
    }
    fft_t fft;
    RT_t rt(*rf_ptr);

    typedef RT_t::complex_array_t complex_array_t;

    // store Ridgelet coefficients for each direction in the following array:
    typedef Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> out_array_t;

    int ncoeffs = get_ncoeffs(*rf_ptr);
    out_array_t RTCOEFFS(K * K, ncoeffs);

#pragma omp parallel
    {
      complex_array_t Fhh(Lx, Lx);
      complex_array_t Fh(2 * Lx, 2 * Lx);
      rca_t f_rc(*rf_ptr);

#pragma omp for schedule(dynamic) collapse(2)
      for (int j1 = 0; j1 < K; j1++) {
        for (int j2 = 0; j2 < K; j2++) {
          // read F..
          int idv = j2 * K + j1;
          Eigen::Map<array_t> F(coeffs.data() + L * idv, Lx, Lx);
          fft.ft(Fhh, F, false);
          Fh.setZero();
          ftcut(Fh, Lx, Lx) = Fhh;
          hf_zero(Fh);
          rt.rt(f_rc.coeffs(), Fh);

          std::vector<double> rclin = rcl_ptr->linearize(f_rc.coeffs());

          // valgrind complains about the following
          // std::copy(rclin.begin(), rclin.end(), RTCOEFFS.row(idv).data());
          Eigen::Map<Eigen::VectorXd> vrclin(rclin.data(), rclin.size());
          RTCOEFFS.row(idv) = vrclin;
        }  // end for j2
      }    // end for j1
    }      // end omp parallel

    // save to hdf
    eigen2hdf::save(h5f_out, std::to_string(frame), RTCOEFFS);

  }  // end for files
  H5Fclose(h5f_out);

  return 0;
}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Surface_mesh.h>

#include <CGAL/Polygon_mesh_processing/connected_components.h>
#include <CGAL/Polygon_mesh_processing/IO/polygon_mesh_io.h>

#include <boost/iterator/function_output_iterator.hpp>
#include <boost/property_map/property_map.hpp>

#include <iostream>
#include <fstream>
#include <map>

typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;
typedef Kernel::Point_3                                     Point;
typedef Kernel::Compare_dihedral_angle_3                    Compare_dihedral_angle_3;

typedef CGAL::Surface_mesh<Point>                           Mesh;

namespace PMP = CGAL::Polygon_mesh_processing;

template <typename G>
struct Constraint : public boost::put_get_helper<bool,Constraint<G> >
{
  typedef typename boost::graph_traits<G>::edge_descriptor edge_descriptor;
  typedef boost::readable_property_map_tag      category;
  typedef bool                                  value_type;
  typedef bool                                  reference;
  typedef edge_descriptor                       key_type;

  Constraint()
    :g_(NULL)
  {}

  Constraint(G& g, double bound)
    : g_(&g), bound_(bound)
  {}

  bool operator[](edge_descriptor e) const
  {
    const G& g = *g_;
    return compare_(g.point(source(e, g)),
                    g.point(target(e, g)),
                    g.point(target(next(halfedge(e, g), g), g)),
                    g.point(target(next(opposite(halfedge(e, g), g), g), g)),
                   bound_) == CGAL::SMALLER;
  }

  const G* g_;
  Compare_dihedral_angle_3 compare_;
  double bound_;
};

template <typename PM>
struct Put_true
{
  Put_true(const PM pm)
    :pm(pm)
  {}

  template <typename T>
  void operator()(const T& t)
  {
    put(pm, t, true);
  }

  PM pm;
};

int main(int argc, char* argv[])
{
  const std::string filename = (argc > 1) ? argv[1] : CGAL::data_file_path("meshes/blobby_3cc.off");

  Mesh mesh;
  if(!PMP::IO::read_polygon_mesh(filename, mesh))
  {
    std::cerr << "Invalid input." << std::endl;
    return 1;
  }

  typedef boost::graph_traits<Mesh>::face_descriptor face_descriptor;
  const double bound = std::cos(0.75 * CGAL_PI);

  std::vector<face_descriptor> cc;
  face_descriptor fd = *faces(mesh).first;
  PMP::connected_component(fd,
      mesh,
      std::back_inserter(cc));

  std::cerr << "Connected components without edge constraints" << std::endl;
  std::cerr << cc.size() << " faces in the CC of " << fd << std::endl;

  // Instead of writing the faces into a container, you can set a face property to true
  typedef Mesh::Property_map<face_descriptor, bool> F_select_map;
  F_select_map fselect_map =
    mesh.add_property_map<face_descriptor, bool>("f:select", false).first;
  PMP::connected_component(fd,
      mesh,
      boost::make_function_output_iterator(Put_true<F_select_map>(fselect_map)));

  std::cerr << "\nConnected components with edge constraints (dihedral angle < 3/4 pi)" << std::endl;
  Mesh::Property_map<face_descriptor, std::size_t> fccmap =
    mesh.add_property_map<face_descriptor, std::size_t>("f:CC").first;
  std::size_t num = PMP::connected_components(mesh,
      fccmap,
      PMP::parameters::edge_is_constrained_map(Constraint<Mesh>(mesh, bound)));

  std::cerr << "- The graph has " << num << " connected components (face connectivity)" << std::endl;
  typedef std::map<std::size_t/*index of CC*/, unsigned int/*nb*/> Components_size;
  Components_size nb_per_cc;
  for(face_descriptor f : faces(mesh)){
    nb_per_cc[ fccmap[f] ]++;
  }
  for(const Components_size::value_type& cc : nb_per_cc){
    std::cout << "\t CC #" << cc.first
              << " is made of " << cc.second << " faces" << std::endl;
  }

  std::cerr << "- We keep only components which have at least 4 faces" << std::endl;
  PMP::keep_large_connected_components(mesh,
      4,
      PMP::parameters::edge_is_constrained_map(Constraint<Mesh>(mesh, bound)));

  std::cerr << "- We keep the two largest components" << std::endl;
  PMP::keep_largest_connected_components(mesh,
      2,
      PMP::parameters::edge_is_constrained_map(Constraint<Mesh>(mesh, bound)));

  return 0;
}


#pragma once

#include <Eigen/Dense>

/**
 * Abstract class that should be inherited from to create specific
 * kalman filters
 *
 * Must initialize:
 *  x_k1_k1, x_k_k1, x_k_k,
 *  P_k1_k1, P_k_k1, P_k_k,
 *  F_k, B_k, H_k,
 *  Q_k, R_k
 *
 * Every predict, must update:
 *  u_k
 *
 * Every PredictWithUpdate, must update:
 *  u_k, z_k
 *
 * The most recently updated values
 *  x_k_k, P_k_k
 *
 * Taken from https://en.wikipedia.org/wiki/Kalman_filter
 *
 * Conversion between code notation and wiki notation is...
 * x_k1_k1 is X_(k-1, k-1)
 * x_k_k is X_(k, k)
 * etc
 */
class KalmanFilter {
public:
    /**
     * Creates a general kalman filter with the given sizes
     * Use a child class to setup the specific state matricies
     * Assumes 1 input
     *
     * @param stateSize The size of the state vector
     * @param observationSize The size of the observation vector
     */
    KalmanFilter(unsigned int stateSize, unsigned int observationSize) :
        x_k1_k1(stateSize), x_k_k1(stateSize), x_k_k(stateSize),
        u_k(1), z_k(observationSize),
        y_k_k1(observationSize), y_k_k(observationSize),
        P_k1_k1(stateSize, stateSize), P_k_k1(stateSize, stateSize), P_k_k(stateSize, stateSize),
        S_k(observationSize, observationSize), K_k(stateSize, observationSize),
        F_k(stateSize, stateSize), B_k(stateSize, 1), H_k(observationSize, stateSize),
        Q_k(stateSize, stateSize), R_k(observationSize, observationSize),
        I(Eigen::MatrixXd::Identity(stateSize, stateSize)) {}

    /**
     * Predicts without update
     */
    void predict();

    /**
     * Predicts with update
     * z_k must be set with the observation
     */
    void predictWithUpdate();

protected:
    Eigen::VectorXd x_k1_k1;
    Eigen::VectorXd x_k_k1;
    Eigen::VectorXd x_k_k;

    Eigen::VectorXd u_k;
    Eigen::VectorXd z_k;

    Eigen::VectorXd y_k_k1;
    Eigen::VectorXd y_k_k;

    Eigen::MatrixXd P_k1_k1;
    Eigen::MatrixXd P_k_k1;
    Eigen::MatrixXd P_k_k;

    Eigen::MatrixXd S_k;
    Eigen::MatrixXd K_k;

    Eigen::MatrixXd F_k;
    Eigen::MatrixXd B_k;
    Eigen::MatrixXd H_k;

    Eigen::MatrixXd Q_k;
    Eigen::MatrixXd R_k;

    Eigen::MatrixXd I;
};


#pragma once

//TODO to be removed...

#include <cmath>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

#include <pcl/filters/uniform_sampling.h>

#include <pcl/common/centroid.h>

#include <pcl/kdtree/kdtree_flann.h>

#include <Eigen/Geometry>
#include <Eigen/Dense>

#include "types.h"

namespace PoseEstimation
{
    /**********************************************************
     *     Useful helper methods (unused at this moment)      *
     **********************************************************/

    const double PI = 3.14159265358979323846264338327950288419716939937510582;
    const double DEG2RAD = PI / 180.0;
    const double RAD2DEG = 180.0 / PI;

    /**
     * @brief Finds the minimum element in an array.
     * @details Returns the index of the minimum element in an array. -1 if array is empty / null.
     *
     * @param array The input array.
     * @return The index of the minimum element.
     */
    template<class T>
    int min_element(const T *array)
    {
        if (!array)
            return -1;
        int length = sizeof(array) / sizeof(T);
        if (length <= 0)
            return -1;
        int m = 0;
        for (int i = 1; i < length; ++i)
        {
            if (array[i] < array[m])
                m = i;
        }
        return m;
    }

    /**
     * @brief Calculates the squared distance between two points.
     * @details Calculates the squared Euclidean distance between the two points.
     *
     * @param a First point with xyz-coordinates.
     * @param b Second point with xyz-coordinates.
     *
     * @return Squared distance between a and b.
     */
    float sqr_distance(const PointType &a, const PointType &b)
    {
        return pow(a.x-b.x, 2) + pow(a.y-b.y, 2) + pow(a.z-b.z, 2);
    }

    /**
     * @brief Calculates the squared distance between two points.
     * @details Calculates the squared Euclidean distance between the two points.
     *
     * @param a First point with xyz-coordinates.
     * @param b Second point with xyz-coordinates.
     *
     * @return Squared distance between a and b.
     */
    float sqr_distance(const Eigen::Vector3f &a, const Eigen::Vector3f &b)
    {
        return pow(a[0]-b[0], 2) + pow(a[1]-b[1], 2) + pow(a[2]-b[2], 2);
    }

    /**
     * @brief Converts a PCL point to an Eigen vector.
     * @details Converts a PCL point to an Eigen 3d vector by using the xyz-coordinates.
     *
     * @param point The PCL point.
     * @return The Eigen vector.
     */
    Eigen::Vector3f point2vec(const PointType &point)
    {
        return Eigen::Vector3f(point.x, point.y, point.z);
    }

    /**
     * @brief Converts an Eigen vector to a PCL point.
     * @details Converts an Eigen 3d vector to a PCL point by using the xyz-coordinates.
     *
     * @param point The PCL point.
     * @return The Eigen vector.
     */
    PointType vec2point(const Eigen::Vector3f &point)
    {
        PointType p;
        p.x = point[0];
        p.y = point[1];
        p.z = point[2];
        return p;
    }

    /**
     * @brief Generates a skew-symmetric matrix from a vector.
     * @details The skew-symmetric matrix S from a vector u allows to calculate the
     * cross-product u x v by using only the matrix multiplication S * v.
     *
     * @param v The input vector.
     * @return The skew-symmetric matrix of v.
     */
    template <typename T>
    inline Eigen::Matrix<T, 3, 3> makeSkewSymmetric(const Eigen::Matrix<T, 3, 1> &v)
    {
        Eigen::Matrix<T, 3, 3> out;
        out <<	   0, -v[2],  v[1],
                v[2],     0, -v[0],
               -v[1],  v[0],     0;

        return out;
    }
}


// g2o - General Graph Optimization
// Copyright (C) 2012 R. Kümmerle
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <Eigen/Core>
#include <iostream>

#include "g2o/stuff/sampler.h"
#include "g2o/stuff/command_args.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/solver.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/base_vertex.h"
#include "g2o/core/base_unary_edge.h"
#include "g2o/solvers/dense/linear_solver_dense.h"

using namespace std;

/**
 * \brief the params, a, b, and lambda for a * exp(-lambda * t) + b
 */
class VertexParams : public g2o::BaseVertex<3, Eigen::Vector3d>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    VertexParams()
    {
    }

    virtual bool read(std::istream& /*is*/)
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }

    virtual bool write(std::ostream& /*os*/) const
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }

    virtual void setToOriginImpl()
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
    }

    virtual void oplusImpl(const double* update)
    {
      Eigen::Vector3d::ConstMapType v(update);
      _estimate += v;
    }
};

/**
 * \brief measurement for a point on the curve
 *
 * Here the measurement is the point which is lies on the curve.
 * The error function computes the difference between the curve
 * and the point.
 */
class EdgePointOnCurve : public g2o::BaseUnaryEdge<1, Eigen::Vector2d, VertexParams>
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    EdgePointOnCurve()
    {
    }
    virtual bool read(std::istream& /*is*/)
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }
    virtual bool write(std::ostream& /*os*/) const
    {
      cerr << __PRETTY_FUNCTION__ << " not implemented yet" << endl;
      return false;
    }

    void computeError()
    {
      const VertexParams* params = static_cast<const VertexParams*>(vertex(0));
      const double& a = params->estimate()(0);
      const double& b = params->estimate()(1);
      const double& lambda = params->estimate()(2);
      double fval = a * exp(-lambda * measurement()(0)) + b;
      _error(0) = fval - measurement()(1);
    }
};

int main(int argc, char** argv)
{
  int numPoints;
  int maxIterations;
  bool verbose;
  std::vector<int> gaugeList;
  string dumpFilename;
  g2o::CommandArgs arg;
  arg.param("dump", dumpFilename, "", "dump the points into a file");
  arg.param("numPoints", numPoints, 50, "number of points sampled from the curve");
  arg.param("i", maxIterations, 10, "perform n iterations");
  arg.param("v", verbose, false, "verbose output of the optimization process");

  arg.parseArgs(argc, argv);

  // generate random data
  double a = 2.;
  double b = 0.4;
  double lambda = 0.2;
  Eigen::Vector2d* points = new Eigen::Vector2d[numPoints];
  for (int i = 0; i < numPoints; ++i) {
    double x = g2o::Sampler::uniformRand(0, 10);
    double y = a * exp(-lambda * x) + b;
    // add Gaussian noise
    y += g2o::Sampler::gaussRand(0, 0.02);
    points[i].x() = x;
    points[i].y() = y;
  }

  if (dumpFilename.size() > 0) {
    ofstream fout(dumpFilename.c_str());
    for (int i = 0; i < numPoints; ++i)
      fout << points[i].transpose() << endl;
  }

  // some handy typedefs
  typedef g2o::BlockSolver< g2o::BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic> >  MyBlockSolver;
  typedef g2o::LinearSolverDense<MyBlockSolver::PoseMatrixType> MyLinearSolver;

  // setup the solver
  g2o::SparseOptimizer optimizer;
  optimizer.setVerbose(false);

  g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(
    g2o::make_unique<MyBlockSolver>(g2o::make_unique<MyLinearSolver>()));

  optimizer.setAlgorithm(solver);

  // build the optimization problem given the points
  // 1. add the parameter vertex
  VertexParams* params = new VertexParams();
  params->setId(0);
  params->setEstimate(Eigen::Vector3d(1,1,1)); // some initial value for the params
  optimizer.addVertex(params);
  // 2. add the points we measured to be on the curve
  for (int i = 0; i < numPoints; ++i) {
    EdgePointOnCurve* e = new EdgePointOnCurve;
    e->setInformation(Eigen::Matrix<double, 1, 1>::Identity());
    e->setVertex(0, params);
    e->setMeasurement(points[i]);
    optimizer.addEdge(e);
  }

  // perform the optimization
  optimizer.initializeOptimization();
  optimizer.setVerbose(verbose);
  optimizer.optimize(maxIterations);

  if (verbose)
    cout << endl;

  // print out the result
  cout << "Target curve" << endl;
  cout << "a * exp(-lambda * x) + b" << endl;
  cout << "Iterative least squares solution" << endl;
  cout << "a      = " << params->estimate()(0) << endl;
  cout << "b      = " << params->estimate()(1) << endl;
  cout << "lambda = " << params->estimate()(2) << endl;
  cout << endl;

  // clean up
  delete[] points;

  return 0;
}


//==================================================================================================
/*!
  @file
  @copyright 2016 NumScale SAS

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

#include <boost/config.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/arch/common/detail/generic/stirling_kernel.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/real.hpp>
#include <boost/simd/constant/sqrt_2pi.hpp>
#include <boost/simd/function/exp.hpp>
#include <boost/simd/function/if_else_nan.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/is_gez.hpp>
#include <boost/simd/function/pow_abs.hpp>
#include <boost/simd/function/rec.hpp>
#ifndef BOOST_SIMD_NO_INVALIDS
#include <boost/simd/function/is_nan.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD_IF ( stirling_
                             , (typename A0, typename X)
                             , (detail::is_native<X>)
                             , bd::cpu_
                             , bs::pack_< bd::floating_<A0>, X>
                             )
  {
    BOOST_FORCEINLINE A0 operator() (const A0& a00) const BOOST_NOEXCEPT
    {
      const A0 Stirlingsplitlim = Real<A0, 0X4061E083BA3443D4ULL, 0X41D628F6UL>();// 143.01608, 26.77f
      const A0 Stirlinglargelim = Real<A0, 0x4065800000000000ULL, 0X420C28F3UL>();// 172, 35.0399895f
      A0 a0 = if_else_nan(is_gez(a00), a00);
      A0 w = bs::rec(a0);
      w = fma(w,detail::stirling_kernel<A0>::stirling1(w), bs::One<A0>());
      A0 y = bs::exp(-a0);
      auto test = is_less(a0, Stirlingsplitlim);
      A0 z =  a0 - bs::Half<A0>();
      z =  if_else(test, z, Half<A0>()*z);
      A0 v =  bs::pow_abs(a0,z);
      y *= v;
      y = if_else(test,y, y*v); /* Avoid overflow in pow() */
      y *= bs::Sqrt_2pi<A0>()*w;
      #ifndef BOOST_SIMD_NO_INFINITIES
      y = if_else(is_equal(a0, Inf<A0>()), a0, y);
      #endif
      return if_else(a0 > Stirlinglargelim, Inf<A0>(), y);
    }
  };
} } }

#endif


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

//
// As detailed in https://github.com/boostorg/math/issues/544
// our original non-central T test data wasn't accurate past about
// 20 decimal places.  This data generator takes the original input
// values and generates new CDF and CDF-complement values via
// tanh_sinh integration - an option that wasn't available when
// the original values were generated back in 2008.
//
// Note that this code will take SEVERAL DAYS to run on typical
// 2021 hardware.
//

#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/beta.hpp>
#include <boost/math/special_functions/hypergeometric_1F1.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/math/quadrature/exp_sinh.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/mpfr.hpp>
#include <fstream>

#include <libs/math/test/table_type.hpp>

using namespace boost::math::tools;
using namespace boost::math;
using namespace std;
using namespace boost::multiprecision;

//using big_t = number<cpp_bin_float<200, digit_base_10, void, long long>>;
using big_t = number<mpfr_float_backend<200>>;

big_t nct_A(big_t v, big_t x, big_t nu)
{
   big_t result = boost::math::hypergeometric_1F1(v / 2 + 1, big_t(3) / 2, nu * nu * x * x / (2 * (v + x * x)));
   result *= boost::math::constants::root_two<big_t>() * nu * x;
   result /= (v + x * x) * boost::math::tgamma((v + 1) / 2);
   return result;
}
big_t nct_B(big_t v, big_t x, big_t nu)
{
   big_t result = boost::math::hypergeometric_1F1((v + 1) / 2, big_t(1) / 2, nu * nu * x * x / (2 * (x * x + v)));
   result /= sqrt(v + x * x) * boost::math::tgamma(v / 2 + 1);
   return result;
}
big_t nct_PDF(big_t v, big_t nu, big_t x)
{
   big_t result = nct_A(v, x, nu) + nct_B(v, x, nu);
   result *= pow(v, v / 2) * boost::math::tgamma(v + 1);
   result /= pow(big_t(2), v) * exp(nu * nu / 2) * pow(v + x * x, v / 2) * boost::math::tgamma(v / 2);
   return result;
}

big_t nc_t_F(big_t v, big_t nc, big_t t)
{
   unsigned j = 0;
   big_t sum = 0;
   big_t tol = std::numeric_limits<big_t>::epsilon();
   do
   {
      big_t x = t * t / (t * t + v);
      big_t p = exp(-nc * nc / 2) * pow(nc * nc / 2, j) / (2 * boost::math::factorial<big_t>(j));
      big_t q = (nc / 2) * exp(-nc * nc / 2) * pow(nc * nc / 2, j) / (boost::math::constants::root_two<big_t>() * boost::math::tgamma(j + big_t(3) / 2));
      big_t term = p * boost::math::ibeta(big_t(j) + 0.5, v / 2, x) + q * boost::math::ibeta(big_t(j + 1), v / 2, x);
      ++j;

      sum += term;

      if (fabs(sum * tol) > fabs(term))
         break;
   } while (true);

   return sum + boost::math::constants::half<big_t>() * (1 + boost::math::erf(-nc / boost::math::constants::root_two<big_t>()));
}
big_t nc_t(big_t v, big_t nc, big_t t)
{
   return t < 0 ? 1 - nc_t_F(v, -nc, t) : nc_t_F(v, nc, t);
}

#define SC_(x) BOOST_JOIN(x, f)

int main(int, char* [])
{
   mpfr_set_emax(mpfr_get_emax_max());
   mpfr_set_emin(mpfr_get_emin_min());

   boost::math::quadrature::exp_sinh<big_t> integrator(10);
   using T = float;

#include <libs/math/test/nct.ipp>

   for (unsigned i = 0; i < nct.size(); ++i)
   {
      big_t error1, error2;
      big_t v(nct[i][0]), nc(nct[i][1]), x(nct[i][2]);
      big_t cdf, ccdf;
      try{
         cdf = integrator.integrate([&](big_t y) { return nct_PDF(v, nc, y); }, -std::numeric_limits<big_t>::infinity(), x, big_t(1e-36), &error1);
         ccdf = integrator.integrate([&](big_t y) { return nct_PDF(v, nc, y); }, x, std::numeric_limits<big_t>::infinity(), big_t(1e-36), &error2);
      }
      catch(const std::exception& e)
      {
         std::cout << "// " << e.what() << " reverting to ibeta method" << std::endl;
         error1 = error2 = 0;
         cdf = nc_t(v, nc, x);
         ccdf = 1 - cdf;
      }

      if (error1 > 1e-35)
      {
         std::cout << "// Accuracy for cdf was " << error1 << " reverting to ibeta method" << std::endl;
         cdf = nc_t(v, nc, x);
      }
      if (error2 > 1e-35)
      {
         std::cout << "// Accuracy for complement cdf was " << error2 << " reverting to ibeta method" << std::endl;
         ccdf = 1 - nc_t(v, nc, x);
      }

      std::cout << std::setprecision(40);
      std::cout << "{{ SC_(" << nct[i][0] << "), SC_(" << nct[i][1] << "), SC_(" << nct[i][2] << "), SC_(";
      std::cout << cdf << "), SC_(" << ccdf << ") }}," << std::endl;
   }

#include <libs/math/test/nct_small_delta.ipp>
   for (unsigned i = 0; i < nct_small_delta.size(); ++i)
   {
      big_t error1, error2;
      big_t v(nct_small_delta[i][0]), nc(nct_small_delta[i][1]), x(nct_small_delta[i][2]);
      big_t cdf, ccdf;
      try {
         cdf = integrator.integrate([&](big_t y) { return nct_PDF(v, nc, y); }, -std::numeric_limits<big_t>::infinity(), x, big_t(1e-36), &error1);
         ccdf = integrator.integrate([&](big_t y) { return nct_PDF(v, nc, y); }, x, std::numeric_limits<big_t>::infinity(), big_t(1e-36), &error2);
      }
      catch (const std::exception& e)
      {
         std::cout << "// " << e.what() << " reverting to ibeta method" << std::endl;
         error1 = error2 = 0;
         cdf = nc_t(v, nc, x);
         ccdf = 1 - cdf;
      }

      if (error1 > 1e-35)
      {
         std::cout << "// Accuracy for cdf was " << error1 << " reverting to ibeta method" << std::endl;
         cdf = nc_t(v, nc, x);
      }
      if (error2 > 1e-35)
      {
         std::cout << "// Accuracy for complement cdf was " << error2 << " reverting to ibeta method" << std::endl;
         ccdf = 1 - nc_t(v, nc, x);
      }

      std::cout << std::setprecision(40);
      std::cout << "{{ SC_(" << v << "), SC_(" << nc << "), SC_(" << x << "), SC_(";
      std::cout << cdf << "), SC_(" << ccdf << ") }}," << std::endl;
   }

#include "../test/nct_asym.ipp"
   for (unsigned i = 0; i < nct_asym.size(); ++i)
   {
      big_t error1, error2;
      big_t v(nct_asym[i][0]), nc(nct_asym[i][1]), x(nct_asym[i][2]);
      big_t cdf, ccdf;
      try {
         cdf = integrator.integrate([&](big_t y) { return nct_PDF(v, nc, y); }, -std::numeric_limits<big_t>::infinity(), x, big_t(1e-36), &error1);
         ccdf = integrator.integrate([&](big_t y) { return nct_PDF(v, nc, y); }, x, std::numeric_limits<big_t>::infinity(), big_t(1e-36), &error2);
      }
      catch (const std::exception& e)
      {
         std::cout << "// " << e.what() << " reverting to ibeta method" << std::endl;
         error1 = error2 = 0;
         cdf = nc_t(v, nc, x);
         ccdf = 1 - cdf;
      }

      if (error1 > 1e-35)
      {
         std::cout << "// Accuracy for cdf was " << error1 << " reverting to ibeta method" << std::endl;
         cdf = nc_t(v, nc, x);
      }
      if (error2 > 1e-35)
      {
         std::cout << "// Accuracy for complement cdf was " << error2 << " reverting to ibeta method" << std::endl;
         ccdf = 1 - nc_t(v, nc, x);
      }

      std::cout << std::setprecision(40);
      std::cout << "{{ SC_(" << v << "), SC_(" << nc << "), SC_(" << x << "), SC_(";
      std::cout << cdf << "), SC_(" << ccdf << ") }}," << std::endl;
   }

   return 0;
}


// Composite trajectory
// Author: Max Schwarz <max.schwarz@uni-bonn.de>

#include "compositetrajectory.h"

#include <math.h>
#include <boost/concept_check.hpp>

const double STEP_PATTERN_LENGTH = 6.0 + 5.0 * 0.9;
const double NUM_STEP_PATTERNS = 2.0;

const double TIME_WAIT = 1.0;

const double KICK_DRAWBACK_HIP = 50.0 * M_PI/180.0;
const double KICK_DRAWBACK_KNEE = 45.0 * M_PI/180.0;
const double KICK_DEST_HIP = -60.0 * M_PI/180.0;
const double KICK_DEST_KNEE = 0;
const double KICK_LENGTH = 0.9;
const double KICK_STOP_LENGTH = 0.4;
const double KICK_DRAWBACK_LEN = 2.0;

const double NUM_KICK_PATTERNS = 5.0;

const int ID_KNEE = 14;
const int ID_HIP = 12;

double sinFromTo(double from, double to, double freq, double time)
{
	const double AMPLITUDE = to-from;

	return 0.5 * AMPLITUDE * sin(freq * 2.0 * M_PI * time - M_PI/2.0) + 0.5*AMPLITUDE + from;
}

double CompositeTrajectory::endTime() const
{
	return NUM_STEP_PATTERNS * STEP_PATTERN_LENGTH + TIME_WAIT
		+ NUM_KICK_PATTERNS * (KICK_LENGTH + KICK_STOP_LENGTH + KICK_DRAWBACK_LEN);
}

double stepPattern(int id, double time)
{
	if(time < 6.0)
	{
		const double ANGLE = 60.0 * M_PI / 180.0;
		const double FREQ = 0.5;

		switch(id)
		{
			case ID_HIP: return sinFromTo(0, -ANGLE, FREQ, time);
			case ID_KNEE: return sinFromTo(0, 2.0*ANGLE, FREQ, time);
		}
	}
	else
	{
		const double ANGLE = 30.0 * M_PI / 180.0;
		const double FREQ = 1.0 / 0.9;
		time -= 6.0;
		switch(id)
		{
			case ID_HIP: return sinFromTo(0, -ANGLE, FREQ, time);
			case ID_KNEE: return sinFromTo(0, 2.0*ANGLE, FREQ, time);
		}
	}

	return 0.0;
}

double kickPattern(int id, double time)
{
	double FREQ = 1.0 / (KICK_LENGTH - 0.25) / 2.0;
	double hip_freq = (time / KICK_LENGTH) * FREQ;

	double amplitudeFactor = 0.5 * (KICK_DRAWBACK_HIP / (KICK_LENGTH/2));
	double amplitude = amplitudeFactor * time * time;

	if(time <= KICK_LENGTH)
	{
		switch(id)
		{
			case ID_HIP: return amplitude * sin(2.0 * M_PI * hip_freq * time);
			case ID_KNEE:
			{
				double hipZeroTime = sqrt(0.5 / FREQ * KICK_LENGTH);

				if(time > hipZeroTime)
					return 0;

				return sinFromTo(KICK_DEST_KNEE, KICK_DRAWBACK_KNEE, 1.0 / hipZeroTime, time);
			}
		}
	}
	time -= KICK_LENGTH;

	double endPos = kickPattern(ID_HIP, KICK_LENGTH);
	double endVel = 2.0 * amplitudeFactor * KICK_LENGTH * sin(2.0 * M_PI * FREQ * KICK_LENGTH)
		+ 4.0 * M_PI * FREQ * amplitudeFactor * KICK_LENGTH * KICK_LENGTH * cos(2.0 * M_PI * FREQ * KICK_LENGTH);

	double stopPos = endPos + 0.5 * endVel * KICK_STOP_LENGTH;

	if(time <= KICK_STOP_LENGTH)
	{
		switch(id)
		{
			case ID_HIP:
			{
				return endPos + endVel * time - 0.5 * endVel / KICK_STOP_LENGTH * time*time;
			}
			case ID_KNEE:
				return 0;
		}
	}
	time -= KICK_STOP_LENGTH;

	double k = stopPos;
	switch(id)
	{
		case ID_HIP: return k - k/KICK_DRAWBACK_LEN * time;
		case ID_KNEE: return 0;
	}

	return 0;
}

double CompositeTrajectory::position(int id, double time) const
{
	if(id != ID_HIP && id != ID_KNEE)
		return 0.0;

	if(time < NUM_STEP_PATTERNS * STEP_PATTERN_LENGTH)
	{
		double ptime = time;
		while(ptime >= STEP_PATTERN_LENGTH)
			ptime -= STEP_PATTERN_LENGTH;

		double pos = stepPattern(id, ptime);

		if(id == ID_HIP)
		{
			const double ANGLE = 70.0 * M_PI / 180.0;
			const double FREQ = 1.0 / (NUM_STEP_PATTERNS * STEP_PATTERN_LENGTH);
			pos += 0.5 * ANGLE * sin(FREQ * 2.0 * M_PI * time + M_PI/2.0) - 0.5*ANGLE;
		}

		return pos;
	}
	time -= NUM_STEP_PATTERNS*STEP_PATTERN_LENGTH;

	if(time < TIME_WAIT)
		return 0;
	time -= TIME_WAIT;

	double ptime = time;
	while(ptime >= KICK_LENGTH + KICK_STOP_LENGTH + KICK_DRAWBACK_LEN)
		ptime -= KICK_LENGTH + KICK_STOP_LENGTH + KICK_DRAWBACK_LEN;
	return kickPattern(id, ptime);
}


#include <algorithm>
#include <iterator>
#include <stdexcept>
#include <vector>
#include <boost/iterator/counting_iterator.hpp>
#include <CGAL/Orthogonal_k_neighbor_search.h>
#include <CGAL/Search_traits_3.h>
#include <CGAL/Search_traits_adapter.h>
#include <Eigen/SparseCore>
#include <Eigen/SparseLU>
#include <Euclid/Geometry/TriMeshGeometry.h>

namespace Euclid
{

namespace _impl
{

// Construct linear system for mesh
template<typename Mesh, typename FT>
void construct_equation(const Mesh& mesh,
                        const std::vector<int>& ids,
                        Eigen::SparseMatrix<FT>& A,
                        Eigen::SparseMatrix<FT>& B)
{
    using SpMat = Eigen::SparseMatrix<FT>;
    const auto inv_sigma = static_cast<FT>(1.0); // parameter

    auto fimap = get(boost::face_index, mesh);
    auto vpmap = get(boost::vertex_point, mesh);
    auto m = B.cols();
    auto n = B.rows();
    auto sum = 0.0;
    auto n_faces = num_faces(mesh);
    std::vector<int> rows;
    std::vector<int> cols;
    std::vector<FT> edge_len;
    std::vector<FT> ds;
    rows.reserve(3 * n_faces);
    cols.reserve(3 * n_faces);
    edge_len.reserve(3 * n_faces);
    ds.reserve(3 * n_faces);

    // Compute the ingredients of the laplacian matrix
    for (auto fa : faces(mesh)) {
        auto fa_id = ids[get(fimap, fa)];
        auto na = Euclid::face_normal(fa, mesh);

        for (auto ha : CGAL::halfedges_around_face(halfedge(fa, mesh), mesh)) {
            auto hb = opposite(ha, mesh);
            if (!is_border(hb, mesh)) { // Non-boundary

                // Determine whether the incident edge is concave or convex
                auto pa = get(vpmap, target(next(ha, mesh), mesh));
                auto pb = get(vpmap, target(next(hb, mesh), mesh));
                auto p = pb - pa;
                auto eta = p * na <= 0.0 ? 0.2 : 1.0;

                auto fb = face(hb, mesh);
                auto fb_id = ids[get(fimap, fb)];
                auto nb = Euclid::face_normal(fb, mesh);
                auto diff = 0.5 * eta * (na - nb).squared_length();

                cols.push_back(fa_id);
                rows.push_back(fb_id);
                ds.push_back(diff);
                edge_len.push_back(Euclid::edge_length(ha, mesh));
                sum += diff;
            }
            else { // Placeholder
                ds.push_back(-1.0);
            }
        }
    }

    // Normalize
    std::vector<Eigen::Triplet<FT>> values;
    values.reserve(cols.size());
    auto inv_avg = edge_len.size() / (sum * 0.5);
    auto d_iter = ds.begin();
    auto e_iter = edge_len.begin();
    auto c_iter = cols.begin();
    auto r_iter = rows.begin();
    while (d_iter != ds.end()) {
        auto prob_sum = 0.0;
        for (auto j = 0; j < 3; ++j) {
            if (*d_iter != -1.0) {
                auto prob =
                    std::exp(-(*d_iter * inv_avg) * inv_sigma) * *e_iter++;
                *d_iter++ = -prob;
                prob_sum += prob;
            }
            else {
                ++d_iter;
            }
        }
        prob_sum = 1.0 / prob_sum;
        d_iter -= 3;
        for (auto j = 0; j < 3; ++j) {
            if (*d_iter != -1.0) {
                values.emplace_back(*c_iter++, *r_iter++, *d_iter++ * prob_sum);
            }
            else {
                ++d_iter;
            }
        }
    }

    for (unsigned i = 0; i < num_faces(mesh); ++i) {
        values.emplace_back(i, i, static_cast<FT>(1.0));
    }

    // Fill in the matrix
    SpMat L(m + n, m + n);
    L.setFromTriplets(values.begin(), values.end());
    A = L.bottomRightCorner(n, n);
    B = -L.bottomLeftCorner(n, m);
    A.makeCompressed();
    B.makeCompressed();
}

// // Construct linear system for point cloud
// template<typename ForwardIterator,
//          typename Index,
//          typename PPMap,
//          typename NPMap,
//          typename FT>
// void construct_equation(ForwardIterator first,
//                         ForwardIterator beyond,
//                         PPMap point_pmap,
//                         NPMap normal_pmap,
//                         const std::vector<std::vector<int>>& neighbors,
//                         const std::vector<Index>& indices,
//                         const std::vector<int>& ids,
//                         Eigen::SparseMatrix<FT>& A,
//                         Eigen::SparseMatrix<FT>& B)
// {
//     using SpMat = Eigen::SparseMatrix<FT>;

//     const auto inv_sigma1 = 1.0;
//     const auto inv_sigma2 = 1.0;
//     auto m = B.cols();
//     auto n = B.rows();
//     auto n_points = m + n;
//     auto n_neighbors = neighbors[0].size();
//     std::vector<int> rows;
//     std::vector<int> cols;
//     std::vector<FT> d1s;
//     std::vector<FT> d2s;
//     rows.reserve(n_points * n_neighbors);
//     cols.reserve(n_points * n_neighbors);
//     d1s.reserve(n_points * n_neighbors);
//     d2s.reserve(n_points * n_neighbors);
//     auto d2_sum = 0.0;

//     // Compute the ingredients of the laplacian matrix
//     auto i = 0;
//     for (auto iter = first; iter != beyond; ++iter, ++i) {
//         auto pi = get(point_pmap, *iter);
//         auto ni = get(normal_pmap, *iter);
//         auto d1_sum = 0.0;

//         for (auto j = 0; j < n_neighbors; ++j) {
//             auto neighbor = neighbors[i][j];
//             cols.push_back(ids[i]);
//             rows.push_back(ids[neighbor]);

//             auto pj = get(point_pmap, indices[neighbor]);
//             auto nj = get(normal_pmap, indices[neighbor]);
//             auto eta = ((pj - pi) - ((pj - pi) * ni) * ni) * nj >= 0.0
//                            ? 0.2
//                            : 1.0; // convex : concave

//             auto d1 = (pi - pj).squared_length();
//             d1s.push_back(d1);
//             d1_sum += d1;
//             auto d2 = 0.5 * eta * (ni - nj).squared_length();
//             d2s.push_back(d2);
//             d2_sum += d2;
//         }

//         auto d1_inv_avg = n_neighbors / d1_sum;
//         for (auto j = 0; j < n_neighbors; ++j) {
//             auto idx = i * n_neighbors + j;
//             d1s[idx] = std::exp(-d1s[idx] * d1_inv_avg * inv_sigma1);
//         }
//     }

//     auto d2_inv_avg = (n_points * n_neighbors) / d2_sum;
//     for (auto& d2 : d2s) {
//         d2 = std::exp(-d2 * d2_inv_avg * inv_sigma2);
//     }

//     // Normalize
//     std::vector<Eigen::Triplet<FT>> values;
//     values.reserve(n_points * (n_neighbors + 1));
//     for (auto i = 0; i < n_points; ++i) {
//         std::vector<FT> ds(n_neighbors);
//         FT sum = 0.0;
//         for (auto j = 0; j < n_neighbors; ++j) {
//             auto idx = i * n_neighbors + j;
//             ds[j] = d1s[idx] * d2s[idx];
//             sum += ds[j];
//         }
//         FT inv_sum = 1.0 / sum;
//         for (auto j = 0; j < n_neighbors; ++j) {
//             auto idx = i * n_neighbors + j;
//             values.emplace_back(cols[idx], rows[idx], -ds[j] * inv_sum);
//         }
//         values.emplace_back(i, i, static_cast<FT>(1.0));
//     }

//     // Fill in the matrix
//     SpMat L(m + n, m + n);
//     L.setFromTriplets(values.begin(), values.end());
//     A = L.bottomRightCorner(n, n);
//     B = -L.bottomLeftCorner(n, m);
//     A.makeCompressed();
//     B.makeCompressed();
// }

// // Adapt PointPropertyMap to take in integer as key type
// template<typename PPMap>
// class IPMapAdaptor
// {
//     using PointKey = typename boost::property_traits<PPMap>::key_type;
//     using Point_3 = typename boost::property_traits<PPMap>::value_type;

// public:
//     using value_type = Point_3;
//     using reference = const value_type&;
//     using key_type = int;
//     using category = boost::lvalue_property_map_tag;

//     IPMapAdaptor(PPMap ppmap, const std::vector<PointKey>& pks)
//         : _ppmap(ppmap), _pks(pks)
//     {}

//     reference operator[](key_type key) const { return _ppmap[_pks[key]]; }

//     friend reference get(IPMapAdaptor ipmap, key_type key)
//     {
//         return ipmap[key];
//     }

// private:
//     PPMap _ppmap;
//     const std::vector<PointKey>& _pks;
// };

} // namespace _impl

template<typename Mesh>
void random_walk_segmentation(const Mesh& mesh,
                              const std::vector<unsigned>& seeds,
                              std::vector<unsigned>& segments)
{
    using VertexPointMap =
        typename boost::property_map<Mesh, boost::vertex_point_t>::type;
    using Point_3 = typename boost::property_traits<VertexPointMap>::value_type;
    using FT = typename CGAL::Kernel_traits<Point_3>::Kernel::FT;
    using SpMat = Eigen::SparseMatrix<FT>;

    // Construct the linear equation
    auto seed_indices = seeds;
    auto m = static_cast<int>(seed_indices.size()); // Number of seeded
    auto n = static_cast<int>(num_faces(mesh)) - m; // Number of unseeded
    std::sort(seed_indices.begin(), seed_indices.end());
    std::vector<int> ids(m + n, -1); // ids[FacetID] -> MatrixID
    size_t inc = 0;
    while (inc < seed_indices.size()) {
        ids[seed_indices[inc]] = inc;
        ++inc;
    }
    for (auto& id : ids) {
        if (id == -1) {
            id = inc++;
        }
    }
    SpMat A(n, n);
    SpMat B(n, m);
    _impl::construct_equation(mesh, ids, A, B);

    std::vector<int> inv_ids(m + n);
    for (auto i = 0; i < m + n; ++i) {
        inv_ids[ids[i]] = i;
    }

    // Solve the equation to segment
    segments.resize(n + m, 0);
    for (auto s : seed_indices) {
        segments[s] = s;
    }
    std::vector<FT> max_probabilities(n, static_cast<FT>(-1.0));
    Eigen::SparseLU<SpMat> solver;
    solver.compute(A);
    if (solver.info() != Eigen::Success) {
        throw std::runtime_error(solver.lastErrorMessage());
    }
    for (auto i = 0; i < m; ++i) {
        Eigen::Matrix<FT, Eigen::Dynamic, 1> b = B.col(i);
        Eigen::Matrix<FT, Eigen::Dynamic, 1> x = solver.solve(b);
        for (auto j = 0; j < n; ++j) {
            if (x(j, 0) > max_probabilities[j]) {
                max_probabilities[j] = x(j, 0);
                segments[inv_ids[j + m]] = seed_indices[i];
            }
        }
    }
}

// template<typename ForwardIterator, typename PPMap, typename NPMap>
// void random_walk_segmentation(ForwardIterator first,
//                               ForwardIterator beyond,
//                               PPMap point_pmap,
//                               NPMap normal_pmap,
//                               const std::vector<unsigned>& seeds,
//                               std::vector<unsigned>& segments)
// {

//     using Index = std::iterator_traits<ForwardIterator>::value_type;
//     using Point_3 = boost::property_traits<PPMap>::value_type;
//     using Vector_3 = boost::property_traits<NPMap>::value_type;
//     using Kernel = CGAL::Kernel_traits<Point_3>::Kernel;
//     using IPMap = _impl::IPMapAdaptor<PPMap>;
//     using BaseTraits = CGAL::Search_traits_3<Kernel>;
//     using KdTreeTraits = CGAL::Search_traits_adapter<int, IPMap, BaseTraits>;
//     using KNN = CGAL::Orthogonal_k_neighbor_search<KdTreeTraits>;
//     using KdTree = typename KNN::Tree;
//     using Splitter = KdTree::Splitter;
//     using Distance = KNN::Distance;
//     using FT = Kernel::FT;
//     using SpMat = Eigen::SparseMatrix<FT>;
//     const int k = 6; // Use 6 neighbors according to Euler formula

//     // Construct the IPMap
//     std::vector<Index> indices;
//     int n = 0;
//     for (auto iter = first; iter != beyond; ++iter) {
//         indices.push_back(*iter);
//         ++n;
//     }
//     IPMap ipmap(point_pmap, indices);

//     // Construct the k-d tree
//     KdTree tree(boost::counting_iterator<int>(0),
//                 boost::counting_iterator<int>(n),
//                 Splitter(),
//                 KdTreeTraits(ipmap));
//     Distance dist(ipmap);

//     // Query neighbors for all points and store the neighbors' indices
//     std::vector<std::vector<int>> neighbors;
//     for (auto iter = first; iter < beyond; ++iter) {
//         std::vector<int> neighbors_i;
//         neighbors_i.reserve(k);

//         auto p = point_pmap[*iter];
//         KNN knn(tree, p, k + 1, static_cast<FT>(0.00001), true, dist);

//         auto it = knn.begin();
//         ++it; // The first will always return the identical point
//         for (it; it != knn.end(); ++it) {
//             neighbors_i.push_back(it->first);
//         }

//         neighbors.push_back(neighbors_i);
//     }

//     // Construct the linear equation
//     auto seed_indices = seeds;
//     auto m = static_cast<int>(seed_indices.size()); // Number of seeded
//     n -= m;                                         // Number of unseeded
//     std::sort(seed_indices.begin(), seed_indices.end());
//     SpMat A(n, n);
//     SpMat B(n, m);
//     std::vector<int> ids(m + n, -1); // ids[PointID] -> MatrixID
//     int inc = 0;
//     while (inc < seed_indices.size()) {
//         ids[seed_indices[inc]] = inc;
//         ++inc;
//     }
//     for (auto& id : ids) {
//         if (id == -1) { id = inc++; }
//     }
//     _impl::construct_equation(
//         first, beyond, point_pmap, normal_pmap, neighbors, indices, ids, A,
//         B);

//     std::vector<int> inv_ids(m + n);
//     for (auto i = 0; i < m + n; ++i) {
//         inv_ids[ids[i]] = i;
//     }

//     // Solve the equation to segment
//     segments.resize(n + m, 0);
//     for (auto s : seed_indices) {
//         segments[s] = s;
//     }
//     std::vector<FT> max_probabilities(n, static_cast<FT>(-1.0));
//     Eigen::SparseLU<SpMat> solver;
//     solver.compute(A);
//     if (solver.info() != Eigen::Success) {
//         std::cerr << solver.lastErrorMessage() << std::endl;
//         return;
//     }
//     for (auto i = 0; i < m; ++i) {
//         Eigen::Matrix<FT, Eigen::Dynamic, 1> b = B.col(i);
//         Eigen::Matrix<FT, Eigen::Dynamic, 1> x = solver.solve(b);
//         for (auto j = 0; j < n; ++j) {
//             if (x(j, 0) > max_probabilities[j]) {
//                 max_probabilities[j] = x(j, 0);
//                 segments[inv_ids[j + m]] = seed_indices[i];
//             }
//         }
//     }
// }

} // namespace Euclid


/* compile_time_power.hpp
 *
*/

#ifndef _COMPILE_TIME_POWER_HPP_
#define _COMPILE_TIME_POWER_HPP_

#include <type_traits>

#include <boost/multiprecision/cpp_int.hpp>

namespace alex::utils::math
{
    namespace concepts
    {
    #if __cplusplus > 201703L && __cpp_concepts >= 201907L
        template<typename T>
        concept NumberType = std::is_integral_v<T> ||
                            std::is_floating_point_v<T> ||
                            boost::multiprecision::is_number<T>::value;
    #else
        // Legacy SFINAE
        template<typename T> struct IsNumberType
        { static constexpr bool value{std::is_integral<T>::value || std::is_floating_point<T>::value}; };
    #endif
    } // namespace alex::utils::math::concepts

#if 0
    template<
#if __cplusplus > 201703L && __cpp_concepts >= 201907L
        concepts::NumberType number_type,
        number_type number,
        size_t pow
#else
        typename number_type,
        number_type number,
        size_t pow,
        typename = std::enable_if_t<concepts::IsNumberType<number_type>::value>
#endif
    >
    struct ct_power
    { static constexpr number_type value{number * ct_power<number_type, number, pow - 1>::value}; };

    template<
#if __cplusplus > 201703L && __cpp_concepts >= 201907L
        concepts::NumberType number_type,
        number_type number
#else
        typename number_type,
        number_type number
#endif
    > struct ct_power<number_type, number, 1> { static constexpr number_type value{number}; };

    template<
#if __cplusplus > 201703L && __cpp_concepts >= 201907L
        concepts::NumberType number_type,
        number_type number
#else
        typename number_type,
        number_type number
#endif
    > struct ct_power<number_type, number, 0> { static constexpr number_type value{static_cast<number_type>(1)}; };
#else
    template<
#if __cplusplus > 201703L && __cpp_concepts >= 201907L
        concepts::NumberType number_type
#else
        typename number_type, typename = std::enable_if_t<concepts::IsNumberType<number_type>::value>
#endif
    >
    constexpr number_type ct_power(number_type number, size_t pow)
    {
        if (pow == 0) return 1;
        if (pow == 1) return number;
        return number * ct_power(number, pow - 1);
    }
#endif
} // namespace alex::utils::math

#endif // _COMPILE_TIME_POWER_HPP_


/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file dcs/fog/confidence_intervals.hpp
 *
 * \brief Confidence interval estimation
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2017 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef DCS_FOG_CONFIDENCE_INTERVALS_HPP
#define DCS_FOG_CONFIDENCE_INTERVALS_HPP

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/students_t.hpp>
#include <dcs/assert.hpp>
#include <dcs/debug.hpp>
#include <dcs/logging.hpp>
#include <dcs/macro.hpp>
#include <dcs/math/function/iszero.hpp>
#include <dcs/math/function/sqr.hpp>
#include <dcs/math/traits/float.hpp>
#include <cstddef>
#include <limits>
#include <stdexcept>
#include <string>
#include <vector>

namespace dcs { namespace fog {

template <typename RealT>
class ci_mean_estimator
{
private:
    typedef boost::accumulators::accumulator_set<RealT,
                                                 boost::accumulators::stats<boost::accumulators::tag::count,
                                                                            boost::accumulators::tag::mean,
                                                                            boost::accumulators::tag::variance>> accumulator_type;

public:
    static const RealT default_ci_level;
    static const RealT default_relative_precision;
    static const std::size_t default_min_sample_size;
    static const std::size_t default_max_sample_size;

    explicit ci_mean_estimator(RealT confidence_level = default_ci_level,
                                       RealT relative_precision = default_relative_precision,
                                       std::size_t min_sample_size = default_min_sample_size,
                                       std::size_t max_sample_size = default_max_sample_size)
    : ci_level_(confidence_level),
      target_rel_prec_(relative_precision),
      n_min_(min_sample_size),
      n_max_(max_sample_size),
      name_("Unnamed"),
      n_target_(std::numeric_limits< std::size_t >::max()),
      n_detected_(false),
      n_aborted_(false),
      n_first_call_(true),
      unstable_(false),
      done_(false)
    {
        // pre: min sample size >= 2
        DCS_ASSERT(n_min_ >= 2,
                   DCS_EXCEPTION_THROW(std::invalid_argument,
                                       "Min sample size must be >= 2"));
        // pre: min sample size <= max sample size
        DCS_ASSERT(n_min_ <= n_max_,
                   DCS_EXCEPTION_THROW(std::invalid_argument,
                                       "Min sample size must be <= max sample size"));
    }

    void name(std::string const& s)
    {
        name_ = s;
    }

    std::string name() const
    {
        return name_;
    }

    std::size_t size() const
    {
        return boost::accumulators::count(stat_);
    }

    std::size_t target_size() const
    {
        return n_target_;
    }

    RealT estimate() const
    {
        return boost::accumulators::mean(stat_);
    }

    RealT variance() const
    {
        //FIXME: Boost.Accumulators variance computes the biased sample variance
        const std::size_t n = this->size();
        return (n/static_cast<RealT>(n-1))*boost::accumulators::variance(stat_);
    }

    RealT standard_deviation() const
    {
        return std::sqrt(this->variance());
    }

    RealT half_width() const
    {
        const std::size_t n = this->size();

        if (n > 1)
        {
            boost::math::students_t_distribution<RealT> t_dist(n-1);
            const RealT t = boost::math::quantile(t_dist, (1+ci_level_)*0.5);

            return t*(this->standard_deviation()/std::sqrt(n));
        }

        return std::numeric_limits<RealT>::infinity();
    }

    RealT target_relative_precision() const
    {
        return target_rel_prec_;
    }

    RealT relative_precision() const
    {
        if (!::dcs::math::iszero(this->estimate()) && this->size() > 1)
        {
            return this->half_width() / std::abs(this->estimate());
        }

        return std::numeric_limits<RealT>::infinity();
    }

    RealT lower() const
    {
        return this->estimate() - this->half_width();
    }

    RealT upper() const
    {
        return this->estimate() + this->half_width();
    }

    bool done() const
    {
        return done_;
    }

    bool unstable() const
    {
        return unstable_;
    }

    void collect(RealT obs)
    {
        if (n_aborted_)
        {
            return;
        }

        stat_(obs);

        //this->check_precision();
        this->check_precision_alt();
DCS_DEBUG_TRACE("(" << name_ << ") Statistic Info: estimate: " << this->estimate() << ", s.d.: " << this->standard_deviation() << ", size: " << this->size() << ", n_target_: " << n_target_ << ", n_min_: " << n_min_ << ", n_max_: " << n_max_ << ", rel.prec.: " << this->relative_precision() << ", n_detected_: " << std::boolalpha << n_detected_ << ", n_aborted_: " << n_aborted_ << ", unstable: " << unstable_ << ", done: " << done_ << ")");//XXX
    }

    void reset()
    {
        stat_ = accumulator_type();
        n_aborted_ = n_detected_
                   = false;
        n_first_call_ = true;
        unstable_ = false;
        done_ = false;
        n_target_ = std::numeric_limits< std::size_t >::max();
    }

private:
    void check_precision()
    {
        const std::size_t n = this->size();

        if (n_detected_ && n >= n_target_)
        {
            if (std::isinf(target_rel_prec_))
            {
                done_ = true;
            }
            else
            {
                done_ = ::dcs::math::float_traits<RealT>::definitely_less_equal(this->relative_precision(), target_rel_prec_);
            }
        }
        if (!n_detected_ || (n >= n_target_ && !done_))
        {
            // Sample size is still to be detected...
            // ... or needs to be redetected since, after having performed the
            //        detection, the precision has not been reached yet.

            DCS_DEBUG_TRACE("(" << name_ << ") Detecting sample size...");

            this->detect();

            if (n_detected_)
            {
                if (n >= n_target_)
                {
                    if (done_)
                    {
                        // Ooops! The new detected number of replications is the
                        // same of or greater than the one previously detected
                        // AND we have already performed this number of replications
                        // AND precision has not been reached yet.
                        // This means that we are unable to reach the target
                        // precision. So disable this statistic.

                        ::dcs::log_warn(DCS_LOGGING_AT, "Statistic '" + name_ + "' will be disabled: unable to reach the wanted precision.");

                        unstable_ = true;
                    }

                    done_ = true;
                }

                DCS_DEBUG_TRACE("(" << name_ << ") Sample size detected: " << n_target_ << " (already collected: " << n << ")");
            }
        }

#ifdef DCS_DEBUG
        if (done_)
        {
            DCS_DEBUG_TRACE("(" << name_ << ") [Sample #" << n << "] Detected precision: mean = " << this->estimate() << " - reached precision = " << this->relative_precision() << " - target precision: " << target_rel_prec_);
        }
        else if (n_detected_)
        {
            //DCS_DEBUG_TRACE("(" << name_ << ") Sample size detected: " << n_target_ << " (already collected: " << n << ")");
            if (n >= n_target_)
            {
                DCS_DEBUG_TRACE("(" << name_ << ") [Sample #" << n << "] Failed to detect precision: mean = " << this->estimate() << " - reached precision = " << this->relative_precision() << " - target precision: " << target_rel_prec_);
            }
            else
            {
                DCS_DEBUG_TRACE("(" << name_ << ") [Sample #" << n << "] Precision not yet reached: not enough replications (done: " << n << " - needed: " << n_target_ << ")");
            }
        }
#endif
    }

    bool detect()
    {
        std::size_t n = this->size();

        if (n < n_min_)
        {
            n_detected_ = false;
            return false;
        }
        if (n >= n_max_)
        {
            n_aborted_ = true;
            return false;
        }
        if (std::isinf(target_rel_prec_) && !n_detected_)
        {
            n_target_ = n;
            n_detected_ = true;
            return true;
        }

        // Use the procedure described in [1], chapter 11.
        //
        // References
        // 1. J. Banks et al.
        //    "Discrete-Event System Simulations,"
        //    4th Edition, Prentice-Hall, 2005
        //

        const RealT mean = this->estimate();
        const RealT sd = this->standard_deviation();

        if (sd < 0 || std::isinf(sd))
        {
            ::dcs::log_warn(DCS_LOGGING_AT, "Standard deviation is negative or infinite");
            n_detected_ = false;
            return false;
        }

        const RealT half_alpha = (1-ci_level_)*0.5;

        // Compute an initial estimate of sample size
        if (n_first_call_)
        {
            n_first_call_ = false;

            boost::math::normal_distribution<RealT> norm;
            const RealT z = boost::math::quantile(norm, half_alpha);
            n =  static_cast< std::size_t >(::dcs::math::sqr(z*sd/(target_rel_prec_*mean)));

            if (n < n_min_)
            {
                n = n_min_;
            }
        }

        RealT n_want = 0;

        // Compute the real estimate of sample size
        do
        {
            boost::math::students_t_distribution<RealT> student_t(n-1);
            const RealT t = boost::math::quantile(student_t, half_alpha);
            n_want = ::dcs::math::sqr(t*sd/(target_rel_prec_*mean));

            if (n < n_want)
            {
                ++n;
            }
        }
        while (n < n_want && n < n_max_);

        if (n <= n_max_)
        {
            if (n_detected_ && n >= n_target_ && !done_)
            {
                // Ooops! The new detected sample size is the
                // same of or greater than the one previously detected
                // AND we have already collected this number of samples
                // AND precision has not been reached yet.
                // This means that we are unable to reach the target
                // precision. So disable this statistic.

                ::dcs::log_warn(DCS_LOGGING_AT, "Statistic '" + name_ + "' will be disabled: unable to reach the wanted precision.");

                unstable_ = true;
            }

            n_target_ = n;
            n_detected_ = true;
            //done_ = true;
        }
        else
        {
            n_target_ = n_max_;
            n_detected_ = false;
            n_aborted_ = true;
        }

DCS_DEBUG_TRACE("(" << name_ << ") Detecting Sample Size --> " << std::boolalpha << n_detected_ << " (n_target_: " << n_target_ << " - n_want: " << n_want << " - n_max_: " << n_max_ << " - n_aborted_: " << n_aborted_ << " - unstable: " << unstable_ << " - done: " << done_ << ")");//XXX

        return n_detected_;
    }

    bool check_precision_alt()
    {
        std::size_t n = this->size();

        if (n < n_min_)
        {
            n_detected_ = false;
            return false;
        }
        if (n >= n_max_)
        {
            n_aborted_ = true;
            return false;
        }
        if (std::isinf(target_rel_prec_))
        {
            n_target_ = n;
            n_detected_ = true;
            done_ = true;
            return true;
        }

        // Use the procedure described in [1], chapter 11.
        //
        // References
        // 1. J. Banks et al.
        //    "Discrete-Event System Simulations,"
        //    4th Edition, Prentice-Hall, 2005
        //

        const RealT mean = this->estimate();
        const RealT sd = this->standard_deviation();

        if (sd < 0 || std::isinf(sd))
        {
            ::dcs::log_warn(DCS_LOGGING_AT, "Standard deviation is negative or infinite");
            n_detected_ = false;
            return false;
        }

        const RealT half_alpha = (1-ci_level_)*0.5;

        // Compute an initial estimate of sample size
        if (n_first_call_)
        {
            n_first_call_ = false;

            boost::math::normal_distribution<RealT> norm;
            const RealT z = boost::math::quantile(norm, half_alpha);
            n =  static_cast< std::size_t >(::dcs::math::sqr(z*sd/(target_rel_prec_*mean)));

            if (n < n_min_)
            {
                n = n_min_;
            }
        }

        RealT n_want = 0;

        // Compute the real estimate of sample size
        do
        {
            boost::math::students_t_distribution<RealT> student_t(n-1);
            const RealT t = boost::math::quantile(student_t, half_alpha);
            n_want = ::dcs::math::sqr(t*sd/(target_rel_prec_*mean));

            if (n < n_want)
            {
                ++n;
            }
        }
        while (n < n_want && n < n_max_);

        if (n <= n_max_)
        {
//          if (n_detected_ && n >= n_target_ && !done_)
//          {
//              // Ooops! The new detected sample size is the
//              // same of or greater than the one previously detected
//              // AND we have already collected this number of samples
//              // AND precision has not been reached yet.
//              // This means that we are unable to reach the target
//              // precision. So disable this statistic.
//
//              ::dcs::log_warn(DCS_LOGGING_AT, "Statistic '" + name_ + "' will be disabled: unable to reach the wanted precision.");
//
//              unstable_ = true;
//          }

            if (n <= this->size())
            {
//              n_target_ = this->size();
                done_ = true;
            }
//          else
//          {
//              n_target_ = n;
//          }
            n_target_ = n;
            n_detected_ = true;
            //done_ = true;
        }
        else
        {
            n_target_ = n_max_;
            n_detected_ = false;
            n_aborted_ = true;
        }

DCS_DEBUG_TRACE("(" << name_ << ") Detecting Sample Size --> " << std::boolalpha << n_detected_ << " (n_target_: " << n_target_ << " - n_want: " << n_want << " - n_max_: " << n_max_ << " - n_aborted_: " << n_aborted_ << " - unstable: " << unstable_ << " - done: " << done_ << ")");//XXX

        return n_detected_;
    }

private:
    RealT ci_level_;
    RealT target_rel_prec_;
    std::size_t n_min_;
    std::size_t n_max_;
    std::string name_;
    accumulator_type stat_; ///< The accumulated statistics
    std::size_t n_target_; ///< The sample size needed to reach the target relative precision
    bool n_detected_; ///< Tells if the sample size needed to reach the target relative precision has been achieved
    bool n_aborted_; ///< Tells if the sample size detection process has been aborted
    bool n_first_call_; ///< Tells if this is the first invocation of the sample size detection process
    bool unstable_; ///< Tells if this statistics has shown an unstable behavior
    bool done_; ///< Tells if this statistics has reached the target precision
}; // ci_mean_estimator

template <typename RT>
const RT ci_mean_estimator<RT>::default_ci_level = 0.95;

template <typename RT>
const RT ci_mean_estimator<RT>::default_relative_precision = 0.04;

template <typename RT>
const std::size_t ci_mean_estimator<RT>::default_min_sample_size = 2;

template <typename RT>
const std::size_t ci_mean_estimator<RT>::default_max_sample_size = std::numeric_limits< std::size_t >::max();

}} // Namespace dcs::fog

#endif // DCS_FOG_CONFIDENCE_INTERVALS_HPP