#include <iostream>
#include <vector>
#include <chrono>
#include <thread>
#include <mutex>

#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <pangolin/pangolin.h>

#include <Eigen/Core>

#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <g2o/types/sba/types_sba.h>

using namespace std;
using namespace cv;
using namespace Eigen;
using namespace g2o;

mutex mtx;

class CocoPoint
{
public:
  int id;
  vector<Point2f> coords;
  Point3d loc;
  vector<int> frames_id;

  CocoPoint(int point_id, int frame_id, Point2f coord) {
    id = point_id;
    frames_id.push_back(frame_id);
    coords.push_back(coord);
  }
};

class Frame
{
public:
  int id;
  Mat image;
  vector<KeyPoint> kps;
  Mat dps;
  vector<int> pts_id;
  Mat pose;

  Frame(int frame_id,
      Mat input_image,
      vector<KeyPoint> frame_kps,
      Mat frame_dps)
  {
    id = frame_id;
    image = input_image;
    kps = frame_kps;
    dps = frame_dps;
  }
};

//class VertexPoint : public g2o::BaseVertex<3, Vector3d> {
//  public:
//  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
//
//  VertexPoint() {}
//
//  virtual void setToOriginImpl() override {
//  _estimate = Vector3d(0, 0, 0);
//  }
//
//  virtual void oplusImpl(const double *update) override {
//  _estimate += Vector3d(update[0], update[1], update[2]);
//  }
//
//  virtual bool read(istream &in) {}
//  virtual bool write (ostream &out) const {}
//};

class Extractor
{
public:
  int MAX_FEATURES = 3000;
  double scale = 1.0; // must be integers..... also this affects the time needed for each frame drastically. how can i improve?
  double thresh = 0.75;
  double dist_thresh = 0.1;

  Extractor (int MAX_FEATURES = 3000,
            double scale = 1.0,
            double thresh = 0.75,
            double dist_thresh = 0.1)
  {
    MAX_FEATURES = MAX_FEATURES;
    scale = scale;
    thresh = thresh;
    dist_thresh = dist_thresh;
  }

  void getFeatures(Mat image, vector<KeyPoint> &frame_kps, Mat &frame_dps)
  {
    Ptr<ORB> orb = ORB::create();
    vector<KeyPoint> keypoints;
    Mat descriptors;
    vector<Point2f> corners;

    int frame_width = image.cols * scale;
    int frame_height = image.rows * scale;

    Mat gray;
    cvtColor(image, gray, CV_BGR2GRAY);

    for(size_t y = 0; y < 1/scale; y++)
    {
      for(size_t x = 0; x < 1/scale; x++)
      {
        vector<Point2f> curr_corners;
        Rect roi = Rect(x*frame_width, y*frame_height, frame_width, frame_height);
        Mat mask = Mat::zeros(image.size(), CV_8UC1);
        mask(roi).setTo(Scalar::all(255));
        goodFeaturesToTrack(gray, curr_corners, MAX_FEATURES*scale*scale, 0.1, 8, mask, 3, false, 0.04);
        corners.insert(corners.end(), curr_corners.begin(), curr_corners.end());
      }
    }

    cout << "Detected corners : " << corners.size() << "\n";
    // Find keypoints and Compute descriptors
    for(size_t i = 0; i < corners.size(); i++)
    {
      keypoints.push_back(KeyPoint(corners[i], 20));
    }

    orb->compute(image, keypoints, descriptors);
    frame_kps = keypoints;
    frame_dps = descriptors;
  }

  void matchFeatures(vector<Frame> &frames, vector<CocoPoint> &points)
  {
    Frame &prev_frame = frames[frames.size()-2];
    Frame &curr_frame = frames[frames.size()-1];

    // Match keypoints based on descriptors
    Ptr<BFMatcher> matcher = BFMatcher::create(NORM_HAMMING2);
    vector<vector<DMatch>> knn_matches;
    matcher->knnMatch(curr_frame.dps, prev_frame.dps, knn_matches, 2);

    // Filter invalid matches
    vector<DMatch> good_matches;
    for(size_t i = 0; i < knn_matches.size(); i++)
    {
      if(knn_matches[i][0].distance < thresh * knn_matches[i][1].distance)
      {
        if(sqrt((curr_frame.kps[knn_matches[i][0].queryIdx].pt.x - prev_frame.kps[knn_matches[i][0].trainIdx].pt.x)*(curr_frame.kps[knn_matches[i][0].queryIdx].pt.x - prev_frame.kps[knn_matches[i][0].trainIdx].pt.x) + (curr_frame.kps[knn_matches[i][0].queryIdx].pt.y - prev_frame.kps[knn_matches[i][0].trainIdx].pt.y)*(curr_frame.kps[knn_matches[i][0].queryIdx].pt.y - prev_frame.kps[knn_matches[i][0].trainIdx].pt.y)) < dist_thresh*sqrt(curr_frame.image.cols*curr_frame.image.cols+curr_frame.image.rows*curr_frame.image.rows))
        {
          good_matches.push_back(knn_matches[i][0]);
        }
      }
    }

    cout << "Good Matches     : " << good_matches.size() << "\n";

    // Put good matches into each frame
    int addNew = 0;
    int addOld = 0;
    for(size_t i = 0; i < good_matches.size(); i++)
    {
      if(prev_frame.pts_id[good_matches[i].trainIdx] == -1)
      {
        addNew++;
        CocoPoint point = CocoPoint(points.size(), frames.size()-2, prev_frame.kps[good_matches[i].trainIdx].pt);
        points.push_back(point);
        points[points.size()-1].frames_id.push_back(curr_frame.id);
        points[points.size()-1].coords.push_back(curr_frame.kps[good_matches[i].queryIdx].pt);
        curr_frame.pts_id[good_matches[i].queryIdx] = points.size()-1;
      }
      else
      {
        addOld++;
        points[prev_frame.pts_id[good_matches[i].trainIdx]].frames_id.push_back(curr_frame.id);
        points[prev_frame.pts_id[good_matches[i].trainIdx]].coords.push_back(curr_frame.kps[good_matches[i].queryIdx].pt);
        curr_frame.pts_id[good_matches[i].queryIdx] = prev_frame.pts_id[good_matches[i].trainIdx];
      }
    }

    cout << "New Points       : " << addNew << "\n";
    cout << "Old Points       : " << addOld << "\n";
  }
};

class PointMap
{
public:
  vector<Frame> frames;
  vector<CocoPoint> points;
  Extractor extractor = Extractor();
  Mat K = (Mat_<double>(3, 3) << 9.842439e+02, 0.000000e+00, 6.900000e+02, 0.000000e+00, 9.808141e+02, 2.331966e+02, 0.000000e+00, 0.000000e+00, 1.000000e+00);
  Mat dist = (Mat_<double>(1, 5) << -3.728755e-01, 2.037299e-01, 2.219027e-03, 1.383707e-03, -7.233722e-02);

  void initFirstFrame(Mat &first_frame)
  {
    points.reserve(1000000);

    vector<KeyPoint> keypoints;
    Mat descriptors;
    Mat pose = Mat::eye(4, 4, CV_64F);

    // Find and draw features
    extractor.getFeatures(first_frame, keypoints, descriptors);

    // Put first frame into frames
    Frame frame = Frame(frames.size(), first_frame, keypoints, descriptors);
    frame.pose = pose;
    frames.push_back(frame);

    for(size_t i = 0; i < frames[0].kps.size(); i++)
    {
      CocoPoint point  = CocoPoint(points.size(), 0, frames[0].kps[i].pt);
      points.push_back(point);
      frames[0].pts_id.push_back(i);
    }
  }

  void extractAndMatch(Mat &curr_frame)
  {
    vector<KeyPoint> keypoints;
    Mat descriptors;

    // Find the features evenly by doing the grid
    extractor.getFeatures(curr_frame, keypoints, descriptors);

    // Put current frame into frames
    Frame frame = Frame(frames.size(), curr_frame, keypoints, descriptors);
    frames.push_back(frame);

    for(size_t i = 0; i < frames[frames.size()-1].kps.size(); i++)
    {
      frames[frames.size()-1].pts_id.push_back(-1);
    }
    // Match features with previous frame and directly add into Map
    extractor.matchFeatures(frames, points);
  }

  void estimatePose()
  {

    vector<Point2f> curr_points;
    vector<Point2f> prev_points;
    for(size_t i = 0; i < frames[frames.size()-1].pts_id.size(); i++)
    {
      if(frames[frames.size()-1].pts_id[i] != -1)
      {
        for(size_t j = 0; j < points[frames[frames.size()-1].pts_id[i]].frames_id.size(); j++)
        {
          if(points[frames[frames.size()-1].pts_id[i]].frames_id[j] == frames.size()-2)
          {
            curr_points.push_back(points[frames[frames.size()-1].pts_id[i]].coords[points[frames[frames.size()-1].pts_id[i]].coords.size()-1]);
            prev_points.push_back(points[frames[frames.size()-1].pts_id[i]].coords[points[frames[frames.size()-1].pts_id[i]].coords.size()-2]);
          }
        }
      }
    }

    Mat fundamental_matrix;
    fundamental_matrix = findFundamentalMat(curr_points, prev_points, CV_FM_8POINT);

    Point2d principal_point(K.at<double>(0, 2), K.at<double>(1, 2));
    double focal_length = K.at<double>(1, 1);
    //Point2d principal_point(9.842439e+02, 2.331966e+02);
    //double focal_length = 9.808141e+02;
    Mat essential_matrix;

    essential_matrix = findEssentialMat(prev_points, curr_points, focal_length, principal_point);
    //essential_matrix = findEssentialMat(curr_points, prev_points, focal_length, principal_point);

    Mat R;
    Mat t;
    recoverPose(essential_matrix, prev_points, curr_points, R, t, focal_length, principal_point);
    //recoverPose(essential_matrix, curr_points, prev_points, R, t, focal_length, principal_point);

    Mat T;
    Mat btm = (Mat_<double>(1, 4) << 0.000000e+00, 0.000000e+00, 0.000000e+00, 1.000000e+00);
    hconcat(R, t, T);
    vconcat(T, btm, T);
    T.convertTo(T, CV_64F);

    //Mat t_x = (Mat_<double>(3, 3) << 0, -t.at<double>(2, 0), t.at<double>(1, 0),
    //    t.at<double>(2, 0), 0, -t.at<double>(0, 0),
    //    -t.at<double>(1, 0), t.at<double>(0, 0), 0);
    //cout << "t^R=" << endl << t_x * R << endl;

    //for(size_t i = 0; i < frames[frames.size()-1].pts_id.size(); i++)
    //{
    //  if(frames[frames.size()-1].pts_id[i] != -1)
    //  {
    //    for(size_t j = 0; j < points[frames[frames.size()-1].pts_id[i]].frames_id.size(); j++)
    //    {
    //      if(points[frames[frames.size()-1].pts_id[i]].frames_id[j] == frames.size()-2)
    //      {
    //        Point2d pt1 = points[frames[frames.size()-1].pts_id[i]].coords[points[frames[frames.size()-1].pts_id[i]].coords.size()-1];
    //        Mat y1 = (Mat_<double>(3, 1) << pt1.x, pt1.y, 1);
    //        Point2d pt2 = points[frames[frames.size()-1].pts_id[i]].coords[points[frames[frames.size()-1].pts_id[i]].coords.size()-2];
    //        Mat y2 = (Mat_<double>(3, 1) << pt2.x, pt2.y, 1);
    //        Mat d = y2.t() * t_x * R * y1;
    //        cout << "epipolar constraint" << d << endl;
    //      }
    //    }
    //  }
    //}

    //frames[frames.size()-1].pose = frames[frames.size()-2].pose*T;
    frames[frames.size()-1].pose = T*frames[frames.size()-2].pose;
    //lowest_z = min(lowest_z, -frames[frames.size()-1].pose.at<double>(2,3));
  }

  void displayVideo()
  {

    Mat image;
    image = frames[frames.size()-1].image.clone();
    for(auto keypoint : frames[frames.size()-1].kps)
    {
      circle(image, keypoint.pt, 4, Scalar(0, 255, 0));
    }

    imshow("Videos", image);
    cout << "Pose             : \n" << frames[frames.size()-1].pose << "\n";
    cout << "Total Frames     : " << frames.size() << "\n";
    cout << "Total Points     : " << points.size() << "\n";
  }

  void triangulate()
  {
    Frame prev_frame = frames[frames.size()-2];
    Frame curr_frame = frames[frames.size()-1];

    Mat T1 = prev_frame.pose.rowRange(0, 3).clone();
    Mat T2 = curr_frame.pose.rowRange(0, 3).clone();

    vector<Point2f> pts_1, pts_2;
    vector<int> ids;
    for(size_t i = 0; i < curr_frame.kps.size(); i++)
    {
      if(curr_frame.pts_id[i] != -1)
      {
        ids.push_back(curr_frame.pts_id[i]);
        pts_1.push_back(points[curr_frame.pts_id[i]].coords[points[curr_frame.pts_id[i]].coords.size()-2]);
        pts_2.push_back(points[curr_frame.pts_id[i]].coords[points[curr_frame.pts_id[i]].coords.size()-1]);
      }
    }
    undistortPoints(pts_1, pts_1, K, dist);
    undistortPoints(pts_2, pts_2, K, dist);

    Mat pts_4d;
    triangulatePoints(T1, T2, pts_1, pts_2, pts_4d);
    vector<Point3d> toBeAddedPoints;

    for(size_t i = 0; i < pts_4d.cols; i++)
    {
      Mat temp = pts_4d.col(i);
      temp = temp / temp.at<float>(3,0);
      Point3d p(
          temp.at<float>(0,0),
          temp.at<float>(1,0),
          temp.at<float>(2,0)
      );
      points[ids[i]].loc = p;
    }
  }
};

void graphOptimize(PointMap &world)
{
  typedef BlockSolver_6_3  SlamBlockSolver;
  typedef LinearSolverEigen<SlamBlockSolver::PoseMatrixType> SlamLinearSolver;

  // allocating the optimizer
  SparseOptimizer optimizer;

  auto linearSolver = g2o::make_unique<SlamLinearSolver>();
  linearSolver->setBlockOrdering(false);
  OptimizationAlgorithmLevenberg* solver = new OptimizationAlgorithmLevenberg(
     g2o::make_unique<SlamBlockSolver>(std::move(linearSolver)));

  optimizer.setAlgorithm(solver);
  optimizer.setVerbose(true);

  // Add pose vertices
  for(size_t i = 0; i < world.frames.size(); i++)
  {
    const Frame& frame = world.frames[i];
    VertexSE3Expmap* vPose = new VertexSE3Expmap;

    Eigen::Matrix<double,3,3> R;
    R << frame.pose.at<float>(0,0), frame.pose.at<float>(0,1), frame.pose.at<float>(0,2),
       frame.pose.at<float>(1,0), frame.pose.at<float>(1,1), frame.pose.at<float>(1,2),
       frame.pose.at<float>(2,0), frame.pose.at<float>(2,1), frame.pose.at<float>(2,2);
    Eigen::Matrix<double,3,1> t(frame.pose.at<float>(0,3), frame.pose.at<float>(1,3), frame.pose.at<float>(2,3));
    vPose->setId(i*2);
    vPose->setEstimate(g2o::SE3Quat(R, t));
    if(i == 0) vPose->setFixed(true);
    optimizer.addVertex(vPose);
    cout << "Adding pose vertex " << i << endl;
  }

  // Add point vertices
  for(size_t i = 0; i < world.points.size(); i++)
  {
    if( world.points[i].loc.x != 0 && world.points[i].loc.y != 0 && world.points[i].loc.z != 0)
    {
      VertexPointXYZ* vPoint = new VertexPointXYZ();

      Matrix<double, 3, 1> v;
      v << world.points[i].loc.x, world.points[i].loc.y, world.points[i].loc.z;

      vPoint->setEstimate(v);
      vPoint->setId(i*2+1);
      vPoint->setMarginalized(true);
      optimizer.addVertex(vPoint);
      cout << "Adding point vertex " << i << endl;

      for(size_t j = 0; j < world.points[i].frames_id.size(); j++)
      {
        Matrix<double, 2, 1> obs;
        obs << world.points[i].coords[j].x, world.points[i].coords[j].y;

        EdgeSE3ProjectXYZ* e = new EdgeSE3ProjectXYZ();

        e->setVertex(0, dynamic_cast<OptimizableGraph::Vertex*>(optimizer.vertex(i*2+1)));
        e->setVertex(1, dynamic_cast<OptimizableGraph::Vertex*>(optimizer.vertex(world.points[i].frames_id[j]*2)));
        e->setVertex(0, vPoint);
        e->setMeasurement(obs);
        e->setInformation(Matrix2d::Identity());

        RobustKernelHuber* rk = new g2o::RobustKernelHuber;
        e->setRobustKernel(rk);
        rk->setDelta(sqrt(5.99));

        e->fx = 9.842439e+02;
        e->fy = 9.808141e+02;
        e->cx = 6.900000e+02;
        e->cy = 2.331966e+02;

        optimizer.addEdge(e);
        cout << "Connecting point " << i*2+1 << " with pose " << world.points[i].frames_id[j] << endl;
      }
    }
  }

  cout << "Done" << endl;

  optimizer.initializeOptimization();
  optimizer.optimize(5);

  for(size_t i = 0; i < world.frames.size(); i++)
  {
    VertexSE3Expmap* vPose = static_cast<VertexSE3Expmap*>(optimizer.vertex(i*2));

    SE3Quat SE3quat = vPose->estimate();
    Matrix<double,4,4> eigMat = SE3quat.to_homogeneous_matrix();
    Mat cvMat(4,4,CV_64F);
    for(int i=0;i<4;i++)
        for(int j=0; j<4; j++)
            cvMat.at<double>(i,j) = eigMat(i,j);
    world.frames[i].pose = cvMat;
  }
}

void display3D(PointMap &world)
{
  pangolin::BindToContext("3d View");
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_BLEND);

  // Define Projection and initial ModelView matrix
  pangolin::OpenGlRenderState s_cam(
    pangolin::ProjectionMatrix(1024,768,420,420,512,389,0.1,1000),
    pangolin::ModelViewLookAt(1, -20, -60, 0,0,0, pangolin::AxisNegY)
  );

  // Create Interactive View in window
  pangolin::Handler3D handler(s_cam);
  pangolin::View& d_cam = pangolin::CreateDisplay()
          .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f/768.0f)
          .SetHandler(&handler);

  while( !pangolin::ShouldQuit() )
  {
    // Lock the data while in use
    mtx.lock();

    // Clear screen and activate view to render into
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    d_cam.Activate(s_cam);

    double w=1.0;
    double h_ratio=0.75;
    double z_ratio=0.6;
    double h = w * h_ratio;
    double z = w * z_ratio;

    // Draw camera movements
    for (size_t i = 0; i < world.frames.size(); i++)
    {
      Mat pose;
      if(!world.frames[i].pose.empty())
      {
        pose = world.frames[i].pose.clone();
      }
      else
      {
        break;
      }
      pangolin::OpenGlMatrix Twc;
      Twc.SetIdentity();

      Mat Rwc(3, 3, CV_64F);
      Mat twc(3, 1, CV_64F);

      Rwc = pose.rowRange(0, 3).colRange(0, 3).t();
      twc = Rwc*-pose.rowRange(0, 3).col(3);

      Twc.m[0] = Rwc.at<double>(0,0);
      Twc.m[1] = Rwc.at<double>(1,0);
      Twc.m[2] = Rwc.at<double>(2,0);
      Twc.m[3]  = 0.0;

      Twc.m[4] = Rwc.at<double>(0,1);
      Twc.m[5] = Rwc.at<double>(1,1);
      Twc.m[6] = Rwc.at<double>(2,1);
      Twc.m[7]  = 0.0;

      Twc.m[8] = Rwc.at<double>(0,2);
      Twc.m[9] = Rwc.at<double>(1,2);
      Twc.m[10] = Rwc.at<double>(2,2);
      Twc.m[11]  = 0.0;

      Twc.m[12] = twc.at<double>(0);
      Twc.m[13] = twc.at<double>(1);
      Twc.m[14] = twc.at<double>(2);
      Twc.m[15]  = 1.0;

      s_cam.Follow(Twc);

      glPushMatrix();
      glMultMatrixd(Twc.m);

      glBegin(GL_LINES);
      glColor3f(0.0,0.0,1.0);
      glVertex3f(0,0,0);
      glVertex3f(w,h,z);
      glVertex3f(0,0,0);
      glVertex3f(w,-h,z);
      glVertex3f(0,0,0);
      glVertex3f(-w,-h,z);
      glVertex3f(0,0,0);
      glVertex3f(-w,h,z);

      glVertex3f(w,h,z);
      glVertex3f(w,-h,z);

      glVertex3f(-w,h,z);
      glVertex3f(-w,-h,z);

      glVertex3f(-w,h,z);
      glVertex3f(w,h,z);

      glVertex3f(-w,-h,z);
      glVertex3f(w,-h,z);
      glEnd();

      glPopMatrix();
    }
    for (size_t i = 0; i < world.points.size(); i++)
    {

      pangolin::OpenGlMatrix Twc;
      Twc.SetIdentity();

      glPushMatrix();
      //glMultMatrixd(Twc.m);

      glPointSize(1.5);
      glBegin(GL_POINTS);
      glColor3f(0.5,1.0,1.0);
      for(size_t j = 0; j < world.points[i].frames_id.size(); j++)
      {
        if(world.points[i].frames_id[j] == world.frames.size()-1)
        {
          glColor3f(1.0,0.2,0.0);
        }
      }
      double x = world.points[i].loc.x;
      double y = world.points[i].loc.y;
      double z = world.points[i].loc.z;
      if(isnan(x) or isnan(y) or isnan(z)) continue;
      glVertex3d(x, y, z);
      glEnd();

      glPopMatrix();
    }
  pangolin::glDrawAxis(3);

  // Swap frames and Process Events
  pangolin::FinishFrame();

  // Unlock the data after use
  mtx.unlock();
  }

  pangolin::GetBoundWindow()->RemoveCurrent();
}

int main(int argc, char **argv)
{
  // Check input
  if(argv[1] == nullptr)
  {
    cerr << "./slam <Video.mp4>\n";
    return -1;
  }

  // Check video type
  VideoCapture cap(argv[1]);
  if(!cap.isOpened())
  {
    cerr << "Error opening video stream or file.\n";
    return -1;
  }

  PointMap World = PointMap();

  pangolin::CreateWindowAndBind("3d View", 1024, 768);
  glEnable(GL_DEPTH_TEST);
  pangolin::GetBoundWindow()->RemoveCurrent();

  thread render(display3D, ref(World));

  while(1)
  {
    // Load frame from video
    Mat frame;
    cap >> frame;
    if(frame.empty()) break;
    cout << "****************** Frame " << cap.get(CAP_PROP_POS_FRAMES) << " *****************\n";

    // Process image
    chrono::steady_clock::time_point start = chrono::steady_clock::now();

    if(cap.get(CAP_PROP_POS_FRAMES) > 1)
    {
      World.extractAndMatch(frame);
      World.estimatePose();
      World.triangulate();
    }
    else
    {
      World.initFirstFrame(frame);
    }

    // Display
    World.displayVideo();

    //char c = (char)waitKey(1000/cap.get(CAP_PROP_FPS)); // display according to original fps
    char c = (char)waitKey(1);
    if(c==27) break; // Esc

    chrono::steady_clock::time_point end = chrono::steady_clock::now();
    chrono::duration<double> interval = chrono::duration_cast<chrono::duration<double>>(end - start);
    cout << "Time used        : " << interval.count() << "\n";
  }

  vector<int>::size_type max_obs = 2;
  int arr[10000] = {0};
  int good_sum = 0;
  for(size_t i = 0;  i < World.points.size(); i++)
  {
    max_obs = max(World.points[i].frames_id.size(), max_obs);
    if(World.points[i].frames_id.size() >= 3)
    {
      good_sum += 1;
    }
  }

  cout << "The max appearance of a point is "<< max_obs << endl;
  cout << "The proportion of good matched points: " << (double)good_sum/(double)World.points.size() << endl;
  render.join();

  cap.release();
  destroyAllWindows();
  return 0;
}


/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2014, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */

#include <Eigen/Dense>
#include <shmfw/variable.h>

int main() {
    ShmFw::HandlerPtr shmHdl = ShmFw::Handler::create ( ShmFw::DEFAULT_SEGMENT_NAME(), ShmFw::DEFAULT_SEGMENT_SIZE() );
    ShmFw::Var<Eigen::Vector3d> a ( "Eigen::Vector3d_a",shmHdl );
    a = Eigen::Vector3d::Random();
    ShmFw::Var<Eigen::Matrix3d> M ( "Eigen::Matrix3d_M",shmHdl );
    M = Eigen::Matrix3d::Random();
    ShmFw::Var<Eigen::Vector3d> b ( "Eigen::Vector3d_b",shmHdl );
    std::cout << M << std::endl;
}


//=======================================================================
// Copyright 2007 Aaron Windsor
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/ref.hpp>
#include <vector>

#include <boost/graph/boyer_myrvold_planar_test.hpp>
#include <boost/graph/is_kuratowski_subgraph.hpp>

using namespace boost;

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

    typedef adjacency_list< vecS, vecS, undirectedS,
        property< vertex_index_t, int >, property< edge_index_t, int > >
        graph;

    // Create a K_6 (complete graph on 6 vertices), which
    // contains both Kuratowski subgraphs as minors.
    graph g(6);
    add_edge(0, 1, g);
    add_edge(0, 2, g);
    add_edge(0, 3, g);
    add_edge(0, 4, g);
    add_edge(0, 5, g);
    add_edge(1, 2, g);
    add_edge(1, 3, g);
    add_edge(1, 4, g);
    add_edge(1, 5, g);
    add_edge(2, 3, g);
    add_edge(2, 4, g);
    add_edge(2, 5, g);
    add_edge(3, 4, g);
    add_edge(3, 5, g);
    add_edge(4, 5, g);

    // Initialize the interior edge index
    property_map< graph, edge_index_t >::type e_index = get(edge_index, g);
    graph_traits< graph >::edges_size_type edge_count = 0;
    graph_traits< graph >::edge_iterator ei, ei_end;
    for (boost::tie(ei, ei_end) = edges(g); ei != ei_end; ++ei)
        put(e_index, *ei, edge_count++);

    // Test for planarity - we know it is not planar, we just want to
    // compute the kuratowski subgraph as a side-effect
    typedef std::vector< graph_traits< graph >::edge_descriptor >
        kuratowski_edges_t;
    kuratowski_edges_t kuratowski_edges;
    if (boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
            boyer_myrvold_params::kuratowski_subgraph
            = std::back_inserter(kuratowski_edges)))
        std::cout << "Input graph is planar" << std::endl;
    else
    {
        std::cout << "Input graph is not planar" << std::endl;

        std::cout << "Edges in the Kuratowski subgraph: ";
        kuratowski_edges_t::iterator ki, ki_end;
        ki_end = kuratowski_edges.end();
        for (ki = kuratowski_edges.begin(); ki != ki_end; ++ki)
        {
            std::cout << *ki << " ";
        }
        std::cout << std::endl;

        std::cout << "Is a kuratowski subgraph? ";
        if (is_kuratowski_subgraph(
                g, kuratowski_edges.begin(), kuratowski_edges.end()))
            std::cout << "Yes." << std::endl;
        else
            std::cout << "No." << std::endl;
    }

    return 0;
}


#include <boost/icl/rational.hpp>


#pragma once

#ifdef POLYSOLVE_WITH_PARDISO

////////////////////////////////////////////////////////////////////////////////
#include <polysolve/LinearSolver.hpp>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Sparse>
////////////////////////////////////////////////////////////////////////////////
//
// http://pardiso-project.org/manual/manual.pdf
//
// See page 29 for instruction on installing and running Pardiso
// The following environment variables must be set:
// - OMP_NUM_THREADS: number of threads
// - PARDISO_LIC_PATH: path to the folder containing the license file

namespace polysolve
{

    class LinearSolverPardiso : public LinearSolver
    {

    public:
        LinearSolverPardiso();
        ~LinearSolverPardiso();

    private:
        POLYSOLVE_DELETE_MOVE_COPY(LinearSolverPardiso)

    protected:
        void setType(int _mtype);
        void init();
        void freeNumericalFactorizationMemory();

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

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

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

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

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

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

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

    protected:
        Eigen::VectorXi ia, ja;
        VectorXd a;

    protected:
        int numRows;

        ///////////////////
        // Pardiso stuff //
        ///////////////////

        // |-------|-----------------------------------------|
        // | mtype | matrix type                             |
        // |-------|-----------------------------------------|
        // |    1  | real and structurally symmetric         |
        // |    2  | real and symmetric positive definite    |
        // |   -2  | real and symmetric indefinite           |
        // |    3  | complex and structurally symmetric      |
        // |    4  | complex and Hermitian positive definite |
        // |   -4  | complex and Hermitian indefinite        |
        // |    6  | complex and symmetric                   |
        // |   11  | real and nonsymmetric                   |
        // |   13  | complex and nonsymmetric                |
        // |-------|-----------------------------------------|

        bool isSymmetric()
        {
            switch (mtype)
            {
            case 2:
            case -2:
            case 4:
            case -4:
            case 6:
                return true;
            default:
                return false;
            }
        }

        int mtype;    // Matrix type
        int nrhs = 1; // Number of right hand sides.

        // Internal solver memory pointer pt,
        // 32-bit: int pt[64]; 64-bit: long int pt[64]
        // or void *pt[64] should be OK on both architectures
        void *pt[64];

        // Pardiso control parameters.
        int iparm[64];
        double dparm[64];
        int maxfct, mnum, phase, error, msglvl, solver = 0;

        // Auxiliary variables.
        double ddum; // Double dummy
        int idum;    // Integer dummy

        int numUniqueElements;
    };

} // namespace polysolve

#endif


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * testTriangulationFactor.cpp
 *
 *  Created on: July 30th, 2013
 *      Author: cbeall3
 */

#include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/nonlinear/Expression.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>

#include <boost/assign.hpp>
#include <boost/assign/std/vector.hpp>

using namespace std;
using namespace gtsam;
using namespace boost::assign;

// Some common constants
static const boost::shared_ptr<Cal3_S2> sharedCal = //
    boost::make_shared<Cal3_S2>(1500, 1200, 0, 640, 480);

// Looking along X-axis, 1 meter above ground plane (x-y)
static const Rot3 upright = Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2);
static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1));
SimpleCamera camera1(pose1, *sharedCal);

// landmark ~5 meters infront of camera
static const Point3 landmark(5, 0.5, 1.2);

// 1. Project two landmarks into two cameras and triangulate
Point2 z1 = camera1.project(landmark);

//******************************************************************************
TEST( triangulation, TriangulationFactor ) {

  Key pointKey(1);
  SharedNoiseModel model;
  typedef TriangulationFactor<SimpleCamera> Factor;
  Factor factor(camera1, z1, model, pointKey);

  // Use the factor to calculate the Jacobians
  Matrix HActual;
  factor.evaluateError(landmark, HActual);

  Matrix HExpected = numericalDerivative11<Vector,Point3>(
      boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark);

  // Verify the Jacobians are correct
  CHECK(assert_equal(HExpected, HActual, 1e-3));
}

//******************************************************************************
TEST( triangulation, TriangulationFactorStereo ) {

  Key pointKey(1);
  SharedNoiseModel model=noiseModel::Isotropic::Sigma(3,0.5);
  Cal3_S2Stereo::shared_ptr stereoCal(new Cal3_S2Stereo(1500, 1200, 0, 640, 480, 0.5));
  StereoCamera camera2(pose1, stereoCal);

  StereoPoint2 z2 = camera2.project(landmark);

  typedef TriangulationFactor<StereoCamera> Factor;
  Factor factor(camera2, z2, model, pointKey);

  // Use the factor to calculate the Jacobians
  Matrix HActual;
  factor.evaluateError(landmark, HActual);

  Matrix HExpected = numericalDerivative11<Vector,Point3>(
      boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark);

  // Verify the Jacobians are correct
  CHECK(assert_equal(HExpected, HActual, 1e-3));

  // compare same problem against expression factor
  Expression<StereoPoint2>::UnaryFunction<Point3>::type f = boost::bind(&StereoCamera::project2, camera2, _1, boost::none, _2);
  Expression<Point3> point_(pointKey);
  Expression<StereoPoint2> project2_(f, point_);

  ExpressionFactor<StereoPoint2> eFactor(model, z2, project2_);

  Values values;
  values.insert(pointKey, landmark);

  vector<Matrix> HActual1(1), HActual2(1);
  Vector error1 = factor.unwhitenedError(values, HActual1);
  Vector error2 = eFactor.unwhitenedError(values, HActual2);
  EXPECT(assert_equal(error1, error2));
  EXPECT(assert_equal(HActual1[0], HActual2[0]));
}

//******************************************************************************
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
//******************************************************************************


// *****************************************************************************
//
// Copyright (c) 2014, Southwest Research Institute® (SwRI®)
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of Southwest Research Institute® (SwRI®) nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> 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 <swri_image_util/geometry_util.h>

#include <cmath>
#include <limits>
#include <vector>

#include <opencv2/imgproc/imgproc.hpp>

#include <Eigen/Dense>
#include <Eigen/SVD>

#include <swri_geometry_util/intersection.h>
#include <swri_math_util/constants.h>

namespace swri_image_util
{
  bool Intersects(const BoundingBox& box1, const BoundingBox& box2)
  {
    return (box1 & box2).area() > 0;
  }

  double GetOverlappingArea(const cv::Rect& rect, const cv::Mat& rigid_transform)
  {
    // List of points corresponding to the input rectangle.
    std::vector<cv::Vec2d> points;

    // List of points correspondng to the transformed rectangle.
    std::vector<cv::Vec2d> points_t;

    // Create a point for each corner of the input rectangle.
    points.push_back(cv::Vec2d(rect.x, rect.y));
    points.push_back(cv::Vec2d(rect.x + rect.width, rect.y));
    points.push_back(cv::Vec2d(rect.x + rect.width, rect.y + rect.height));
    points.push_back(cv::Vec2d(rect.x, rect.y + rect.height));

    // Transform the input points to the transformed points using the rigid
    // transform.
    cv::transform(cv::InputArray(points), cv::OutputArray(points_t), rigid_transform);

    // Use swri_geometry_util to get the overlapping area
    return swri_geometry_util::PolygonIntersectionArea(points, points_t);
  }

  cv::Mat ProjectEllipsoid(const cv::Mat& ellipsoid)
  {
    cv::Mat ellipse;

    if (ellipsoid.rows == 3 && ellipsoid.cols == 3 && ellipsoid.type() == CV_32FC1)
    {
      if (ellipsoid.at<float>(2, 2) >= std::numeric_limits<double>::max() * 0.5)
      {
        ellipse.create(2, 2, CV_32FC1);
        ellipse.at<float>(0, 0) = ellipsoid.at<float>(0, 0);
        ellipse.at<float>(0, 1) = ellipsoid.at<float>(0, 1);
        ellipse.at<float>(1, 0) = ellipsoid.at<float>(1, 0);
        ellipse.at<float>(1, 1) = ellipsoid.at<float>(1, 1);

        return ellipse;
      }

      Eigen::Matrix3d A;
      for (size_t r = 0; r < 3; r++)
      {
        for (size_t c = 0; c < 3; c++)
        {
          A(r, c) = ellipsoid.at<float>(r, c);
        }
      }

      // Specify the main vector directions (x and y)
      Eigen::Vector3d ax1;
      Eigen::Vector3d ax2;
      ax1 << 1, 0, 0;
      ax2 << 0, 1, 0;

      // Initialize the normal vector (here the normal vector, in the state
      // space is in the theta direction).
      Eigen::Vector3d n_ax;
      n_ax << 0, 0, 1;

      ///////////////////////
      // Calculate A prime //
      ///////////////////////

      Eigen::Matrix3d A_sym_temp = A.transpose() + A;

      // N_ax_temp = (n_ax*n_ax')
      Eigen::Matrix3d N_ax_temp = n_ax * n_ax.transpose();

      // A_prime_1 = A_sym_temp*N_ax_temp
      //           = (A+A')*(n_ax*n_ax')
      Eigen::Matrix3d A_prime_1 = A_sym_temp * N_ax_temp;

      // A_prime_2 = A_prime_1*A_sym_temp
      //           = (A+A')*(n_ax*n_ax')*(A+A')
      Eigen::Matrix3d A_prime_2 = A_prime_1 * A_sym_temp;

      // scale_1 = n_ax'*A
      Eigen::RowVector3d scale_1 = n_ax.transpose() * A;

      // scale_2 = (scale_1*n_ax)*-4
      //         = (n_ax'*A*n_ax)*-4
      double scale = (scale_1 * n_ax)(0,0) * -4.0;

      // A_temp = scale*A_temp
      //        = scale_2*A = -4*(n_ax'*A*n_ax)*A
      Eigen::Matrix3d A_temp = A * scale;

      // Aprime = A_prime_2 + A_temp
      //        = (A+A')*(n_ax*n_ax')*(A+A') - 4*(n_ax'*A*n_ax)*A
      Eigen::Matrix3d Aprime = A_prime_2 + A_temp;

      ///////////////////////
      // Calculate C prime //
      ///////////////////////

      // C_temp = n_ax'*A
      Eigen::RowVector3d C_temp = n_ax.transpose() * A;

      // Cprime = -4.0*C_temp*n_ax
      //        = -4.0*n_ax'*A*n_ax
      double cp = (-4.0 * C_temp) * n_ax;

      // Bprime = Aprime/Cprime;
      Eigen::Matrix3d Bprime = Aprime / cp;

      // Jp = axes_def;
      //    = [ax1(:),ax2(:)] = [1,0;0,1;0,0];
      Eigen::Matrix<double, 3, 2> Jp;
      Jp << 1, 0,
            0, 1,
            0, 0;

      ///////////////////////
      // Calculate D prime //
      ///////////////////////

      // Dprime_temp = Jp'*Bprime
      Eigen::Matrix<double, 2, 3> Dprime_temp = Jp.transpose() * Bprime;

      // Dprime = Dprime_temp * Jp
      //        = Jp'*Bprime*Jp
      Eigen::Matrix2d Dprime = Dprime_temp * Jp;

      for (size_t r = 0; r < 2; r++)
      {
        for (size_t c = 0; c < 2; c++)
        {
          if (Dprime(r, c) != Dprime(r, c))
          {
            return ellipse;
          }
        }
      }

      ellipse.create(2, 2, CV_32FC1);
      ellipse.at<float>(0, 0) = Dprime(0, 0);
      ellipse.at<float>(0, 1) = Dprime(0, 1);
      ellipse.at<float>(1, 0) = Dprime(1, 0);
      ellipse.at<float>(1, 1) = Dprime(1, 1);
    }

    return ellipse;
  }

  std::vector<tf::Vector3> GetEllipsePoints(
      const cv::Mat& ellipse,
      const tf::Vector3& center,
      double scale,
      int32_t num_points)
  {
    std::vector<tf::Vector3> perimeter;

    if (ellipse.rows == 2 && ellipse.cols == 2 && ellipse.type() == CV_32FC1 &&
        num_points > 2)
    {
      Eigen::Matrix2d Dprime;
      Dprime(0, 0) = ellipse.at<float>(0, 0);
      Dprime(0, 1) = ellipse.at<float>(0, 1);
      Dprime(1, 0) = ellipse.at<float>(1, 0);
      Dprime(1, 1) = ellipse.at<float>(1, 1);

      Eigen::JacobiSVD<Eigen::Matrix2d> svd(Dprime, Eigen::ComputeFullV);

      Eigen::Vector2d Sigma = svd.singularValues();
      Eigen::Matrix2d Vt = svd.matrixV().transpose();

      double xprime_scale = std::sqrt(Sigma(0));
      double yprime_scale = std::sqrt(Sigma(1));

      if (xprime_scale <= 0 || yprime_scale <= 0)
      {
        return perimeter;
      }

      Eigen::MatrixX2d Xp1(num_points, 2);
      for (int32_t i = 0; i < num_points; i++)
      {
        double phi =
            (static_cast<double>(i) / static_cast<double>(num_points))
            * swri_math_util::_2pi;

        Xp1(i, 0) = xprime_scale * std::cos(phi) * scale;
        Xp1(i, 1) = yprime_scale * std::sin(phi) * scale;
      }

      // Xell = Xp1*(V')'
      Eigen::MatrixX2d Xell = Xp1 * Vt;

      perimeter.resize(num_points);
      for (int32_t i = 0; i < num_points; i++)
      {
        perimeter[i].setX(Xell(i, 0) + center.x());
        perimeter[i].setY(Xell(i, 1) + center.y());
      }
    }

    return perimeter;
  }
}


// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2019 www.open3d.org
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
// ----------------------------------------------------------------------------

#include "Open3D/Geometry/TriangleMesh.h"

#include <Eigen/Dense>
#include <queue>
#include <tuple>

#include "Open3D/Utility/Console.h"

namespace open3d {
namespace geometry {

std::shared_ptr<TriangleMesh> SubdivideMidpoint(const TriangleMesh& input,
                                                int number_of_iterations) {
    auto mesh = std::make_shared<TriangleMesh>();
    mesh->vertices_ = input.vertices_;
    mesh->vertex_colors_ = input.vertex_colors_;
    mesh->vertex_normals_ = input.vertex_normals_;
    mesh->triangles_ = input.triangles_;

    bool has_vert_normal = input.HasVertexNormals();
    bool has_vert_color = input.HasVertexColors();
    bool has_tria_normal = input.HasTriangleNormals();
    // Compute and return midpoint.
    // Also adds edge - new vertex refrence to new_verts map.
    auto SubdivideEdge =
            [&](std::unordered_map<Edge, int, utility::hash_tuple::hash<Edge>>&
                        new_verts,
                int vidx0, int vidx1) {
                int min = std::min(vidx0, vidx1);
                int max = std::max(vidx0, vidx1);
                Edge edge(min, max);
                if (new_verts.count(edge) == 0) {
                    mesh->vertices_.push_back(0.5 * (mesh->vertices_[min] +
                                                     mesh->vertices_[max]));
                    if (has_vert_normal) {
                        mesh->vertex_normals_.push_back(
                                0.5 * (mesh->vertex_normals_[min] +
                                       mesh->vertex_normals_[max]));
                    }
                    if (has_vert_color) {
                        mesh->vertex_colors_.push_back(
                                0.5 * (mesh->vertex_colors_[min] +
                                       mesh->vertex_colors_[max]));
                    }
                    int v01idx = mesh->vertices_.size() - 1;
                    new_verts[edge] = v01idx;
                    return v01idx;
                } else {
                    return new_verts[edge];
                }
            };
    for (int iter = 0; iter < number_of_iterations; ++iter) {
        std::unordered_map<Edge, int, utility::hash_tuple::hash<Edge>>
                new_verts;
        std::vector<Eigen::Vector3i> new_triangles(4 * mesh->triangles_.size());
        for (size_t tidx = 0; tidx < mesh->triangles_.size(); ++tidx) {
            const auto& triangle = mesh->triangles_[tidx];
            int vidx0 = triangle(0);
            int vidx1 = triangle(1);
            int vidx2 = triangle(2);
            int vidx01 = SubdivideEdge(new_verts, vidx0, vidx1);
            int vidx12 = SubdivideEdge(new_verts, vidx1, vidx2);
            int vidx20 = SubdivideEdge(new_verts, vidx2, vidx0);
            new_triangles[tidx * 4 + 0] =
                    Eigen::Vector3i(vidx0, vidx01, vidx20);
            new_triangles[tidx * 4 + 1] =
                    Eigen::Vector3i(vidx01, vidx1, vidx12);
            new_triangles[tidx * 4 + 2] =
                    Eigen::Vector3i(vidx12, vidx2, vidx20);
            new_triangles[tidx * 4 + 3] =
                    Eigen::Vector3i(vidx01, vidx12, vidx20);
        }
        mesh->triangles_ = new_triangles;
    }

    if (input.HasTriangleNormals()) {
        mesh->ComputeTriangleNormals();
    }

    return mesh;
}

}  // namespace geometry
}  // namespace open3d


/**
 * Copyright 2021 Scott Brauer
 *
 * 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  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.
 */

/**
 * @file   Candlestick.hpp
 * @author sbrauer
 * @date   Nov 22, 2021
 */

#ifndef OFFCENTER_TRADING_DATATYPES_CALCULATION_CANDLESTICK_HPP_
#define OFFCENTER_TRADING_DATATYPES_CALCULATION_CANDLESTICK_HPP_

#include <boost/multiprecision/mpfr.hpp>

#include "offcenter/common/Types.hpp"
#include "offcenter/trading/datatypes/common/Candlestick.hpp"

namespace offcenter {
namespace trading {
namespace datatypes {
namespace calculation {

//using CandlestickPrice = boost::multiprecision::mpfr_float;
using CandlestickPrice = offcenter::common::MPF;

using BidAskCandlestick = offcenter::trading::datatypes::common::BidAskCandlestick<CandlestickPrice>;

} /* namespace calculation */

namespace common {

/**
 * Calculate max value of two CandlestickPrice values.
 *
 * @param one First value
 * @param two Second value
 */
template<>
const offcenter::trading::datatypes::calculation::CandlestickPrice max<offcenter::trading::datatypes::calculation::CandlestickPrice>(
		const offcenter::trading::datatypes::calculation::CandlestickPrice& one,
		const offcenter::trading::datatypes::calculation::CandlestickPrice& two);
/**
 * Calculate min value of two CandlestickPrice values.
 *
 * @param one First value
 * @param two Second value
 */
template<>
const offcenter::trading::datatypes::calculation::CandlestickPrice min<offcenter::trading::datatypes::calculation::CandlestickPrice>(
		const offcenter::trading::datatypes::calculation::CandlestickPrice& one,
		const offcenter::trading::datatypes::calculation::CandlestickPrice& two);

/**
 * Calculate sin of a CandlestickPrice value.
 *
 * @param value Value to calc sin of
 */
template<>
const offcenter::trading::datatypes::calculation::CandlestickPrice sine(
		const offcenter::trading::datatypes::calculation::CandlestickPrice& value);

} /* namespace common */
} /* namespace datatypes */
} /* namespace trading */
} /* namespace offcenter */

#endif /* OFFCENTER_TRADING_DATATYPES_CALCULATION_CANDLESTICK_HPP_ */


#ifdef USE_MKL
#include <mkl_cblas.h>
#include <mkl_lapacke.h>
#else
#include <cblas.h>
#include <lapacke.h>
#endif
#include <Eigen/Core> // after MKL
#include <Eigen/Dense>
#include <fstream>
#include <array>
#include <random>
#include <mutex>
#include <iostream>
#include <map>
#include <tuple>

#include <mpi.h>

#include "tasktorrent/tasktorrent.hpp"
#include "util_shared.hpp"

using namespace std;
using namespace Eigen;
using namespace ttor;

typedef array<int, 2> int2;
typedef array<int, 3> int3;

struct denseQR {

    /** Usual stuff */
    const int rank;
    const int nranks;
    const int n_threads;
    Communicator* comm;
    Threadpool* tp;

    /** Task flows needed **/
    Taskflow<int> dgeqrt_tf;  // A[k,k] = QR
    Taskflow<int2> dtsqrt_tf;
    Taskflow<int2> dlarfb_tf;
    Taskflow<int3> dssrfb_tf;
    Taskflow<int2> gather_tf;
    Taskflow<int> computeQ_tf;

    /** Taskflows obtained from parent function **/
    Taskflow<int>* notify_tf;

    /** Active messages between tasks **/
    ActiveMsg<view<double>,view<double>,view<int>,int>* am_dgeqrt_2_dlarfb;
    ActiveMsg<view<double>,int>* am_dgeqrt_2_dtsqrt;
    ActiveMsg<view<double>,int, int>* am_dtsqrt_2_dtsqrt;
    ActiveMsg<view<double>,view<double>,int,view<int>,int>* am_dtsqrt_2_dssrfb;
    ActiveMsg<view<double>,int, int, int>* am_dlarfb_2_dssrfb;
    ActiveMsg<view<double>,int, int, int>* am_dssrfb_2_dssrfb;
    ActiveMsg<view<double>,view<double>,view<double>,int,int>* am_gather_0;
    ActiveMsg<view<double>,view<double>,int,int>* am_gather_1;
    ActiveMsg<view<double>,int,int>* am_gather_2;

    /** Matrix A (input and output) **/
    MatrixXd* A;
    MatrixXd* Tmat;
    MatrixXd* Q;
    int p; // No. of ranks in x dim
    int q; // No. of ranks in y dim
    int n; // block size
    int M; // no. of blocks in y dim
    int N; // no. of blocks in x dim

    /** Workspace **/
    map<int2, MatrixXd> Mat;
    map<int2, MatrixXd> T;

    /* Constructor */
    denseQR(Communicator* comm_, Threadpool* tp_, Taskflow<int>* notify_tf_): rank(comm_->comm_rank()),
    nranks(comm_->comm_size()), n_threads(tp_->size()), comm(comm_), tp(tp_), notify_tf(notify_tf_),
    dgeqrt_tf(tp), dtsqrt_tf(tp), dlarfb_tf(tp), dssrfb_tf(tp), gather_tf(tp), computeQ_tf(tp) {

        // Mat and T size ???

        // From dgeqrt
        am_dgeqrt_2_dlarfb = comm->make_active_msg(
            [&](view<double> &V, view<double> &tau, view<int>& js, int& k){
                Mat.at({k,k}) = Map<MatrixXd>(V.data(), n, n); // Check strides
                T.at({k,k})   = Map<MatrixXd>(tau.data(), n, n);
                for (auto& j: js){
                    dlarfb_tf.fulfill_promise({k,j});
                }
            }
        );

        am_dgeqrt_2_dtsqrt = comm->make_active_msg(
            [&](view<double> &R, int& k){
                Mat.at({k,k}) = Map<MatrixXd>(R.data(), n, n); // Check strides
                dtsqrt_tf.fulfill_promise({k+1,k});
            }
        );

        // From dsqrt
        am_dtsqrt_2_dtsqrt = comm->make_active_msg(
            [&](view<double> &R, int& i, int& k){
                Mat.at({k,k}) = Map<MatrixXd>(R.data(), n, n); // Check strides
                dtsqrt_tf.fulfill_promise({i,k});
            }
        );

        am_dtsqrt_2_dssrfb = comm->make_active_msg(
            [&](view<double> &V_ik, view<double> &T_ik, int& i, view<int>& js, int& k){
                Mat.at({i,k}) = Map<MatrixXd>(V_ik.data(), n, n);
                T.at({i,k}) = Map<MatrixXd>(T_ik.data(), n, n);

                for(auto& j: js){
                    dssrfb_tf.fulfill_promise({i,j,k});
                }
            }
        );

        // From dlarfb
        am_dlarfb_2_dssrfb = comm->make_active_msg(
            [&](view<double> &R, int& i, int&j,  int& k){
                Mat.at({k,j}) = Map<MatrixXd>(R.data(), n, n); // Check strides
                dssrfb_tf.fulfill_promise({i,j,k});
            }
        );

        // From dssrfb
        am_dssrfb_2_dssrfb = comm->make_active_msg(
            [&](view<double> &R, int& i, int&j,  int& k){
                Mat.at({k,j}) = Map<MatrixXd>(R.data(), n, n); // Check strides
                dssrfb_tf.fulfill_promise({i,j,k});
            }
        );

        // Define taskflows
        dgeqrt_tf.set_mapping([&] (int k){
                return (k % n_threads);
            })
            .set_indegree([](int){
                return 1;
            })
            .set_task([&] (int k) {
                int info = LAPACKE_dgeqrt(LAPACK_COL_MAJOR, n, n, n, Mat.at({k,k}).data(), n, T.at({k,k}).data(), n);
                assert(info == 0);
            })
            .set_fulfill([&](int k) {
                // Dependencies -- dlarfb

                map<int, vector<int>> to_fulfill;

                for(int j = k+1; j<N; j++) { // A[k][j] blocks
                    int r = (k%p)+ (j%q)*p;
                    if(to_fulfill.count(r) == 0) {
                        to_fulfill[r] = {j};
                    } else {
                        to_fulfill[r].push_back(j);
                    }
                }

                // First dtsqrt
                if (k+1 < M){
                   int r = ((k+1)%p)+ (k%q)*p;
                   if (r == rank){
                       dtsqrt_tf.fulfill_promise({k+1, k});
                   }
                   else {
                       auto R_kk = view<double>(Mat.at({k,k}).data(), n * n );
                       am_dgeqrt_2_dtsqrt->send(r, R_kk, k);

                   }
                }

                // gather on node 0
                gather_tf.fulfill_promise({k,k});

                // Send data and trigger tasks -- dlarfb
                for (auto& p: to_fulfill){
                    int r = p.first; // rank
                    if (r == rank){
                        for(auto& j: p.second){
                            dlarfb_tf.fulfill_promise({k, j});
                        }
                    }
                    else {
                        auto V_kk = view<double>(Mat.at({k,k}).data(), n * n );
                        auto T_kk = view<double>(T.at({k,k}).data(), n*n);
                        auto jsv = view<int>(p.second.data(), p.second.size());
                        am_dgeqrt_2_dlarfb->send(r, V_kk, T_kk, jsv, k);
                    }
                }

            })
            .set_name([](int j) {
                return "geqrt_" + to_string(j);
            })
            .set_priority([&](int) {
                return 4;
            });

        dtsqrt_tf.set_mapping([&] (int2 ik){
                return ((ik[0] + ik[1] * N) % n_threads);
            })
            .set_indegree([](int2 ik){
                return (ik[1] == 0 ? 0 : 1) + 1;
            })
            .set_task([&] (int2 ik) {
                int i = ik[0];
                int k = ik[1];
                int info = LAPACKE_dtpqrt(LAPACK_COL_MAJOR, n, n, 0, n, Mat.at({k,k}).data(), n, Mat.at({i,k}).data(), n, T.at({i,k}).data(), n);
                assert(info == 0);
            })
            .set_fulfill([&](int2 ik) {
                int i = ik[0];
                int k = ik[1];
                // Dependencies
                map<int, vector<int>> to_fulfill;

                // Next dtsqrt
                if (i+1 < M){
                    // int r = block2rank({i+1,k});
                    int r = ((i+1)%p)+ (k%q)*p;
                    if (r == rank){
                        dtsqrt_tf.fulfill_promise({i+1, k});
                    }
                    else {
                        auto R_kk = view<double>(Mat.at({k,k}).data(), n * n );
                        int inext = i+1;
                        am_dtsqrt_2_dtsqrt->send(r, R_kk, inext, k) ; // send A{k,k}
                    }
                }

                // gather on node 0
                gather_tf.fulfill_promise({i,k});

                // dssrfb
                for(int j = k+1; j<N; j++) {
                    // int r = block2rank({i,j}); // ssrfb
                    int r = (i%p)+ (j%q)*p;
                    if(to_fulfill.count(r) == 0) {
                        to_fulfill[r] = {j};
                    } else {
                        to_fulfill[r].push_back(j);
                    }
                }

                // Send data and trigger tasks
                for (auto& p: to_fulfill){
                    int r = p.first; // rank
                    if (r == rank){
                        for(auto& j: p.second){
                            dssrfb_tf.fulfill_promise({i,j,k});
                        }
                    }
                    else {
                        auto V_ik = view<double>(Mat.at({i,k}).data(), n * n );
                        auto T_ik = view<double>(T.at({i,k}).data(), n*n);
                        auto jsv = view<int>(p.second.data(), p.second.size());

                        am_dtsqrt_2_dssrfb->send(r, V_ik, T_ik, i, jsv, k);
                    }
                }

            })
            .set_name([](int2 ik) {
                return "tsqrt_" + to_string(ik[0]) + "_" +to_string(ik[1]);
            })
            .set_priority([&](int2) {
                return 3;
            });

        // larfb
        dlarfb_tf.set_mapping([&] (int2 kj){
                return ((kj[0] + kj[1]*N) % n_threads);
            })
            .set_indegree([](int2 kj){
                return (kj[0] == 0 ? 0 : 1) + 1;
            })
            .set_task([&] (int2 kj) {

                int k=kj[0]; // who is sending
                int j=kj[1]; // me

                int info = LAPACKE_dlarfb(LAPACK_COL_MAJOR, 'L', 'T', 'F', 'C', n, n, n, Mat.at({k,k}).data(), n, T.at({k,k}).data(), n, Mat.at({k,j}).data(), n);
                assert(info == 0);

            })
            .set_fulfill([&](int2 kj) {
                int k = kj[0];
                int j = kj[1];

                if (k+1 < M){
                    // int r = block2rank({k+1, j});
                    int r = ((k+1)%p)+ (j%q)*p;
                    if (r == rank){
                        dssrfb_tf.fulfill_promise({k+1, j, k});
                    }
                    else {
                        auto R_kj = view<double>(Mat.at({k,j}).data(), n * n );
                        int knext = k+1;
                        am_dlarfb_2_dssrfb->send(r, R_kj, knext, j, k); //send R_kj
                    }
                }

            })
            .set_name([](int2 kj) {
                return "dlarfb_" + to_string(kj[0]) + "_" +to_string(kj[1]);
            })
            .set_priority([&](int2) {
                return 2;
            });

        dssrfb_tf.set_mapping([&] (int3 ijk){
                return ((ijk[0] + ijk[1] * N + ijk[2] * N * N) % n_threads);
            })
            .set_indegree([](int3 ijk){
                int i = ijk[0];
                int j = ijk[1];
                int k = ijk[2];

                return (k==0 ? 2 : 3);

            })
            .set_task([&] (int3 ijk) {
                int i = ijk[0];
                int j = ijk[1];
                int k = ijk[2];

                int info = LAPACKE_dtpmqrt(LAPACK_COL_MAJOR, 'L', 'T', n, n, n, 0, n,
                                           Mat.at({i,k}).data(), n,
                                           T.at({i,k}).data(), n,
                                           Mat.at({k,j}).data(), n,
                                           Mat.at({i,j}).data(), n);
                assert(info == 0);

            })
            .set_fulfill([&](int3 ijk) {
                int i = ijk[0];
                int j = ijk[1];
                int k = ijk[2];

                if (i+1 < M){
                    // int r = block2rank({i+1,j});
                    int r = ((i+1)%p)+ (j%q)*p;
                    if (r == rank){
                        dssrfb_tf.fulfill_promise({i+1,j,k});
                    }
                    else {
                        auto R_kj = view<double>(Mat.at({k,j}).data(), n * n );
                        int inext = i+1;
                        am_dssrfb_2_dssrfb->send(r, R_kj, inext, j, k);
                    }
                }

                if (i == k+1) {
                    if (j == k+1){
                        // same rank
                        dgeqrt_tf.fulfill_promise(i); // i = k+1
                    }
                    else { // j > k+1
                        assert(j > k+1);
                        dlarfb_tf.fulfill_promise({i, j}); // i = k+1
                    }
                }
                else { // i > k+1
                    assert(i > k+1);
                    if (j == k+1) {
                        dtsqrt_tf.fulfill_promise({i, j}); // j = k+1
                    }
                    else {
                        assert(j > k+1);
                        dssrfb_tf.fulfill_promise({i, j, k+1});
                    }
                }

            })
            .set_name([](int3 ijk) {
                return "dssrfb_" + to_string(ijk[0]) + "_" +to_string(ijk[1]) + "_" + to_string(ijk[2]);
            })
            .set_priority([&](int3) {
                return 1;
            });

        am_gather_0 = comm->make_active_msg(
        [&](view<double> &R_j, view<double> &V_ij, view<double> &T_ij,  int& i, int& j) {
            A->block(0, j*n, (j+1)*n, n) = Map<MatrixXd>(R_j.data(), (j+1)*n, n) ;
            // MatrixXd Rtemp = Map<MatrixXd>(R_j.data(), (j+1)*n, n) ;
            // for (int k=0; k<j+1; ++k){
            //     Mat.at({k,j}) = Rtemp.block(k*n, 0, n, n);
            // }
            A->block(i*n, j*n, n, n) = Map<MatrixXd>(V_ij.data(), n, n); // Last block
            Tmat->block(i*n, j*n, n, n) = Map<MatrixXd>(T_ij.data(), n, n);
            // Mat.at({i,j}) = Map<MatrixXd>(V_ij.data(), n, n); // Last block
            // T.at({i,j}) = Map<MatrixXd>(T_ij.data(), n, n);
            computeQ_tf.fulfill_promise(0);
        });

        am_gather_1 = comm->make_active_msg(
        [&](view<double> &V_ij, view<double> &T_ij,  int& i, int& j) {
            A->block(i*n, j*n, n, n) = Map<MatrixXd>(V_ij.data(), n, n); // Last block
            Tmat->block(i*n, j*n, n, n) = Map<MatrixXd>(T_ij.data(), n, n);

            // Mat.at({i,j}) = Map<MatrixXd>(V_ij.data(), n, n); // Last block
            // T.at({i,j}) = Map<MatrixXd>(T_ij.data(), n, n);
            computeQ_tf.fulfill_promise(0);

        });

        am_gather_2 = comm->make_active_msg(
        [&](view<double> &T_ij,  int& i, int& j) {
            Tmat->block(i*n, j*n, n, n) = Map<MatrixXd>(T_ij.data(), n, n);
            // T.at({i,j}) = Map<MatrixXd>(T_ij.data(), n, n);
            computeQ_tf.fulfill_promise(0);

        });

        // gather
        gather_tf.set_mapping([&](int2 ij) {
                return ( (ij[0] + ij[1]*N) % n_threads );
            })
            .set_indegree([](int2) {
                return 1;
            })
            .set_task([&](int2 ij) {
                int i = ij[0];
                int j = ij[1];

                if(rank != 0) {
                    if (i == M-1) { // Last row contains all the updated R[:, j]
                        MatrixXd Atemp = MatrixXd::Zero( (j+1)*n, n);
                        for (int k=0; k< j+1; ++k){
                            Atemp.block(k*n, 0, n, n) = Mat.at({k,j}); // R[:,j] of A = QR
                        }
                        auto R_j = view<double>(Atemp.data(), (j+1)*n*n);

                        auto V_ij = view<double>(Mat.at({i,j}).data(), n*n);
                        auto T_ij = view<double>(T.at({i,j}).data(), n*n);
                        am_gather_0->send(0, R_j, V_ij, T_ij, i, j);
                    }
                    else if (i > j){ // Just send the V_ij, T_ij
                        auto V_ij = view<double>(Mat.at({i,j}).data(), n*n);
                        auto T_ij = view<double>(T.at({i,j}).data(), n*n);
                        am_gather_1->send(0, V_ij, T_ij, i, j);
                    }
                    else {
                        assert(i == j);
                        auto T_ij = view<double>(T.at({i,j}).data(), n*n);
                        am_gather_2->send(0, T_ij, i, j);
                    }
                }
                else {
                    if (i == M-1){
                        for (int k=0; k< j+1; ++k){
                            A->block(k*n, j*n, n, n) = Mat.at({k,j}); // R[:,j] of A = QR
                        }
                        A->block(i*n, j*n, n, n) = Mat.at({i, j}); // Last block
                        Tmat->block(i*n, j*n, n, n) = T.at({i,j});
                        computeQ_tf.fulfill_promise(0);
                    }
                    else if (i > j){
                        A->block(i*n, j*n, n, n) = Mat.at({i,j}); // just that block; contains V
                        Tmat->block(i*n, j*n, n, n) = T.at({i,j});
                        computeQ_tf.fulfill_promise(0);

                    }
                    else {
                        assert(i == j);
                        // Send T
                        Tmat->block(i*n, j*n, n, n) = T.at({i,j});
                        computeQ_tf.fulfill_promise(0);

                    }
                }
            })
            .set_name([](int2 ij) {
                return "gather_"+ to_string(ij[0]) + "_" +to_string(ij[1]);
            });

            computeQ_tf.set_task([&] (int k){
                for (int j = N-1; j > -1; --j){

                    for (int k=0; k<N; ++k){
                        for (int i=M-1; i > j; --i){
                            MatrixXd Aij = A->block(i*n, j*n, n, n);
                            MatrixXd Tij = Tmat->block(i*n, j*n, n, n);
                            // assert(Q->cols() == N*n);
                            MatrixXd Qi = Q->block(i*n, k*n, n, n);
                            MatrixXd Qj = Q->block(j*n, k*n, n, n);

                            int info = LAPACKE_dtpmqrt(LAPACK_COL_MAJOR, 'L', 'N', n, n, n, 0, n,
                                                       Aij.data(), n,
                                                       Tij.data(), n,
                                                       Qj.data(), n,
                                                       Qi.data(), n);
                            Q->block(i*n, k*n, n, n) = Qi;
                            Q->block(j*n, k*n, n, n) = Qj;

                            assert(info ==0);
                        }

                    }

                    MatrixXd Ajj = A->block(j*n, j*n, n, n);
                    MatrixXd Tjj = Tmat->block(j*n, j*n, n, n);
                    MatrixXd Qjj = Q->block(j*n, 0, n, N*n);

                    int info = LAPACKE_dlarfb(LAPACK_COL_MAJOR, 'L', 'N', 'F', 'C', n, N*n, n, Ajj.data(), n,
                                              Tjj.data(), n, Qjj.data(), n);

                    Q->block(j*n, 0, n, N*n) = Qjj;

                    assert(info == 0);

                }
            })
            .set_indegree([&] (int k){
                return (M* (M +1)/2 - (M-N)*(M-N+1)/2);
            })
            .set_mapping([&] (int k){
                return (k % n_threads);
            })
            .set_name([&](int k) {
                return "computeQ_tf_" + to_string(k);
            })
            .set_fulfill([&](int k){
                notify_tf->fulfill_promise(0);
            });

    }

    /** Function **/
    void run(MatrixXd* Y_, MatrixXd* Tmat_, MatrixXd* Q_, int block_size, int p1, int q1){
        this->A = Y_; // A to be performed QR on
        this->Tmat = Tmat_;
        this->Q = Q_; // thin Q

        p = p1;
        q = q1;

        M = ceil(Y_->rows()/block_size);
        N = ceil(Y_->cols()/block_size);

        n = block_size;

        // Mapper
        auto block2rank = [&](int2 ij){
            int i = ij[0];
            int j = ij[1];
            int ii = i % p;
            int jj = j % q;
            int r = ii + jj * p;
            assert(r <= nranks);
            return r;
        };

        for(int i = 0; i < M; i++) {
            for (int j=0; j < N; j++){
                if(block2rank({i,j}) == rank) {
                    Mat[{i,j}] = Y_->block(i*n, j*n, n, n);
                    if (i>=j) T[{i,j}] = MatrixXd::Zero(n,n);

                } else {
                    Mat[{i,j}] = MatrixXd::Zero(0,0);
                    if (i>=j) T[{i,j}] = MatrixXd::Zero(0,0);
                }
            }
        }

        if (rank == 0){
            dgeqrt_tf.fulfill_promise(0);
        }

    }

};


//          Copyright Gavin Band 2008 - 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <boost/ptr_container/ptr_vector.hpp>
#include <boost/function.hpp>
#include "config/config.hpp"
#include "Eigen/Core"
#include "Eigen/Eigenvalues"
#include "genfile/VariantIdentifyingData.hpp"
#include "genfile/VariantDataReader.hpp"
#include "genfile/SNPDataSourceProcessor.hpp"
#include "genfile/SingleSNPGenotypeProbabilities.hpp"
#include "statfile/BuiltInTypeStatSink.hpp"
#include "statfile/BuiltInTypeStatSource.hpp"
#include "appcontext/FileUtil.hpp"
#include "appcontext/get_current_time_as_string.hpp"
#include "appcontext/OptionProcessor.hpp"
#include "appcontext/UIContext.hpp"
#include "components/SampleSummaryComponent/SampleStorage.hpp"
#include "components/RelatednessComponent/PCAComputer.hpp"
#include "components/RelatednessComponent/LapackEigenDecomposition.hpp"
#include "components/RelatednessComponent/names.hpp"
#include "components/RelatednessComponent/write_matrix_to_stream.hpp"

PCAComputer::PCAComputer(
	appcontext::OptionProcessor const& options,
	genfile::CohortIndividualSource const& samples,
	appcontext::UIContext& ui_context
):
	m_options( options ),
	m_ui_context( ui_context ),
	m_samples( samples ),
	m_number_of_samples( samples.get_number_of_individuals() ),
	m_number_of_PCs_to_compute( std::min( m_options.get< std::size_t >( "-PCs" ), samples.get_number_of_individuals() ) ),
	m_threshhold( 0.9 )
{}

void PCAComputer::compute( Eigen::MatrixXd const& matrix, std::size_t const number_of_snps, std::string const& name ) {
	m_number_of_snps = number_of_snps ;
	m_kinship_matrix = matrix ;
	m_filename = name ;

	std::cerr << "PCAComputer::set_relatedness_matrix(): set relatedness matrix to:\n"
		<< m_kinship_matrix.block( 0, 0, 10, 10) << ".\n" ;

	compute_PCA() ;
}

namespace {
	genfile::VariantEntry get_eigendecomposition_header( std::string const& prefix, int value ) {
		if( value == 0 ) {
			return genfile::VariantEntry( "eigenvalue" ) ;
		}
		else {
			return genfile::VariantEntry( prefix + genfile::string_utils::to_string( value ) ) ;
		}
	}
}

void PCAComputer::load_matrix(
	genfile::CohortIndividualSource const& samples,
	std::string const& filename,
	Eigen::MatrixXd* matrix,
	std::size_t* number_of_snps,
	appcontext::UIContext& ui_context
) {
	statfile::BuiltInTypeStatSource::UniquePtr source = statfile::BuiltInTypeStatSource::open( filename ) ;
	std::size_t number_of_samples = 0 ;
	load_matrix_metadata( samples, *source, &number_of_samples, number_of_snps, ui_context ) ;

	// Read the matrix, making sure the samples come in the same order as in the sample file.
	matrix->resize( samples.get_number_of_individuals(), samples.get_number_of_individuals() ) ;
	std::vector< std::size_t > sample_column_indices( samples.get_number_of_individuals() ) ;
	for( std::size_t sample_i = 0; sample_i < samples.get_number_of_individuals(); ++sample_i ) {
		sample_column_indices[sample_i] = source->index_of_column( pca::get_concatenated_sample_ids( &samples, sample_i )) ;
		if( sample_i > 0 ) {
			if( sample_column_indices[sample_i] <= sample_column_indices[sample_i-1] ) {
				ui_context.logger() << "!! Error in PCAComputer::load_matrix(): column order does not match samples.\n" ;
				throw genfile::MalformedInputError( source->get_source_spec(), 0, sample_column_indices[ sample_i ] ) ;
			}
		}
	}

	// Now apply sample exclusions.
	for( std::size_t sample_i = 0; sample_i < samples.get_number_of_individuals(); ++sample_i ) {
		std::string id ;
		// find row corresponding to next sample.
		for( (*source) >> id; (*source) && id != pca::get_concatenated_sample_ids( &samples, sample_i ); (*source) >> statfile::ignore_all() >> id ) ;
		if( !(*source) || source->number_of_rows_read() == source->number_of_rows() ) {
			throw genfile::MalformedInputError( source->get_source_spec(), source->number_of_rows_read(), 0 ) ;
		}
		if( source->number_of_rows_read() != ( sample_column_indices[ sample_i ] - 1 ) ) {
			ui_context.logger()
				<< "!! Error( PCAComputer::load_matrix ): sample " << samples.get_entry( sample_i, "id_1" )
				<< " is on row "
				<< source->number_of_rows_read()
				<< " but column "
				<< sample_column_indices[ sample_i ]
				<< ".\n" ;
			throw genfile::MalformedInputError( source->get_source_spec(), source->number_of_rows_read(), 0 ) ;
		}
		for( std::size_t sample_j = 0; sample_j < samples.get_number_of_individuals(); ++sample_j ) {
			(*source)
				>> statfile::ignore( sample_column_indices[ sample_j ] - source->current_column() )
				>> (*matrix)( sample_i, sample_j )
			;
		}
		(*source) >> statfile::ignore_all() ;
	}
}

void PCAComputer::load_long_form_matrix(
	genfile::CohortIndividualSource const& samples,
	std::string const& filename,
	Eigen::MatrixXd* matrix,
	std::size_t* number_of_snps,
	appcontext::UIContext& ui_context
) {
	statfile::BuiltInTypeStatSource::UniquePtr source = statfile::BuiltInTypeStatSource::open( filename ) ;
	std::size_t number_of_samples = 0 ;
	load_matrix_metadata( samples, *source, &number_of_samples, number_of_snps, ui_context ) ;

	matrix->resize( samples.get_number_of_individuals(), samples.get_number_of_individuals() ) ;
	Eigen::MatrixXd non_missingness( matrix->rows(), matrix->cols() ) ;
	non_missingness.setZero() ;

	std::map< std::string, int > samples_to_indices ;
	std::map< std::string, int > samples_filled ;
	for( std::size_t sample_i = 0; sample_i < samples.get_number_of_individuals(); ++sample_i ) {
		std::string const sample = samples.get_entry( sample_i, "ID_1" ).as< std::string >() ;
		samples_to_indices[ sample ] = sample_i ;
		samples_filled[ sample ] = 0 ;
	}

	{
		std::string sample1, sample2 ;
		int pairwise_complete_obs = 0 ;
		double value = 0 ;
		// Read lower diagonal
		while( (*source) >> sample1 >> sample2 >> pairwise_complete_obs >> value ) {
			int const i = samples_to_indices[ sample1 ] ;
			int const j = samples_to_indices[ sample2 ] ;
			if( non_missingness( i, j ) == 1 ) {
				throw genfile::MalformedInputError( source->get_source_spec(), source->number_of_rows_read() ) ;
			} else {
				(*matrix)( i, j ) = value ;
				non_missingness( i, j ) = 1 ;
			}
			(*source) >> statfile::ignore_all() ;
		}

		// Fill in upper diagonal
		for( int i = 0; i < matrix->rows(); ++i ) {
			for( int j = i+1; j < matrix->cols(); ++j ) {
				(*matrix)( i, j ) = (*matrix)( j, i ) ;
				non_missingness( i, j ) = non_missingness( j, i ) ;
			}
		}

#if DEBUG_PCA_COMPUTER
		ui_context.logger() << "Matrix is:\n"
			<< (*matrix).block( 0, 0, 10, 10 )
			<< "\n" ;

		ui_context.logger() << "Non-missingness is:\n"
			<< non_missingness.block( 0, 0, 10, 10 )
			<< "\n" ;
#endif

		if( non_missingness.array().minCoeff() == 0 ) {
			int count = 0 ;
			for( int i = 0; i < matrix->rows() && count < 20; ++i ) {
				for( int j = 0; j < matrix->cols() && count < 20; ++j ) {
					if( non_missingness( i, j ) != 1 ) {
						ui_context.logger() << "PCAComputer::load_long_form_matrix(): sample pair "
							<< samples.get_entry( i, "ID_1" )
							<< " : "
							<< samples.get_entry( j, "ID_1" )
							<< " seems unrepresented in \""
							<< filename
							<< "\".\n" ;
						++count ;
					}
				}
			}
			throw genfile::BadArgumentError( "PCAComputer::load_long_form_matrix()", "filename=\"" + filename + "\"", "not every sample was represented in the matrix." ) ;
		}
	}
}

void PCAComputer::load_matrix_metadata(
	genfile::CohortIndividualSource const& samples,
	statfile::BuiltInTypeStatSource& source,
	std::size_t* number_of_samples,
	std::size_t* number_of_snps,
	appcontext::UIContext& ui_context
) {
	using namespace genfile::string_utils ;
	// Read the metadata
	std::size_t number_of_samples_ = 0, number_of_snps_ = 0 ;
	std::vector< std::string > metadata = split( source.get_descriptive_text(), "\n" ) ;
	for( std::size_t i = 0; i < metadata.size(); ++i ) {
		std::vector< std::string > elts = split_and_strip( metadata[i], ":", " " ) ;
		if( elts.size() == 2 && elts[0] == "Number of SNPs" ) {
			number_of_snps_ = to_repr< std::size_t >( elts[1] ) ;
		}
		else if( elts.size() == 2 && elts[0] == "Number of samples" ) {
			number_of_samples_ = to_repr< std::size_t >( elts[1] ) ;
		}
	}
	if( number_of_samples_ != samples.get_number_of_individuals() ) {
		throw genfile::MismatchError(
			"PCAComputer::load_matrix_metadata()",
			source.get_source_spec(),
			"number of samples: " + to_string( number_of_samples_ ),
			"expected number: " + to_string( samples.get_number_of_individuals() )
		) ;
	}
	*number_of_samples = number_of_samples_ ;
	*number_of_snps = number_of_snps_ ;
}

genfile::VariantEntry PCAComputer::get_pca_name( std::size_t i ) {
	return std::string( "PC_" ) + genfile::string_utils::to_string( i + 1 ) ;
}

void PCAComputer::compute_PCA() {
	Eigen::MatrixXd kinship_eigendecomposition( m_number_of_samples, m_number_of_samples + 1 ) ;
#if HAVE_LAPACK
	if( m_options.check( "-use-eigen" ))
#endif
	{
		m_ui_context.logger() << "========================================================================\n" ;
		m_ui_context.logger() << "PCAComputer: Computing eigenvalue decomposition of " << m_number_of_samples << "x" << m_number_of_samples << " kinship matrix using Eigen...\n" ;
		Eigen::SelfAdjointEigenSolver< Eigen::MatrixXd > solver( m_kinship_matrix ) ;
		if( solver.info() == Eigen::Success ) {
			m_ui_context.logger() << "PCAComputer: Done, writing results...\n" ;
		} else if( solver.info() == Eigen::NumericalIssue ) {
			m_ui_context.logger() << "PCAComputer: Oh dear, numerical issue, writing results...\n" ;
		} else if( solver.info() == Eigen::NoConvergence ) {
			m_ui_context.logger() << "PCAComputer: Oh dear, no convergence, writing results...\n" ;
		} else {
			m_ui_context.logger() << "PCAComputer: Oh dear, unknown error, writing results...\n" ;
		}
		kinship_eigendecomposition.block( 0, 0, m_number_of_samples, 1 ) = solver.eigenvalues().reverse() ;
		kinship_eigendecomposition.block( 0, 1, m_number_of_samples, m_number_of_samples ) = Eigen::Reverse< Eigen::MatrixXd, Eigen::Horizontal >( solver.eigenvectors() ) ;
	}
#if HAVE_LAPACK
	else { // -use-eigen not specified.
		Eigen::VectorXd eigenvalues( m_number_of_samples ) ;
		Eigen::MatrixXd eigenvectors( m_number_of_samples, m_number_of_samples ) ;
		m_ui_context.logger() << "PCAComputer: Computing eigenvalue decomposition of kinship matrix using lapack...\n" ;
		lapack::compute_eigendecomposition( m_kinship_matrix, &eigenvalues, &eigenvectors ) ;
		//lapack::compute_partial_eigendecomposition( m_kinship_matrix, &eigenvalues, &eigenvectors, m_number_of_PCs_to_compute ) ;
		kinship_eigendecomposition.block( 0, 0, m_number_of_samples, 1 ) = eigenvalues.reverse() ;
		kinship_eigendecomposition.block( 0, 1, m_number_of_samples, m_number_of_samples ) = Eigen::Reverse< Eigen::MatrixXd, Eigen::Horizontal >( eigenvectors ) ;
	}
#endif

	// Verify the decomposition.
	{
		std::size_t size = std::min( std::size_t(10), m_number_of_samples ) ;
		m_ui_context.logger() << "Top-left of U U^t is:\n" ;
		{

			Eigen::MatrixXd UUT = kinship_eigendecomposition.block( 0, 1, size, m_number_of_samples ) ;
			UUT *= kinship_eigendecomposition.block( 0, 1, m_number_of_samples, size ).transpose() ;
			pca::write_matrix_to_stream( m_ui_context.logger(), UUT ) ;
		}
		m_ui_context.logger() << "Top-left of original kinship matrix is:\n" ;
		pca::write_matrix_to_stream( m_ui_context.logger(), m_kinship_matrix.block( 0, 0, size, size )) ;

		Eigen::VectorXd v = kinship_eigendecomposition.block( 0, 0, m_number_of_samples, 1 ) ;
		Eigen::MatrixXd reconstructed_kinship_matrix = kinship_eigendecomposition.block( 0, 1, size, m_number_of_samples ) ;
		reconstructed_kinship_matrix *= v.asDiagonal() ;
		reconstructed_kinship_matrix *= kinship_eigendecomposition.block( 0, 1, m_number_of_samples, size ).transpose() ;
		// Knock out the upper triangle for comparison with the original kinship matrix.
		for( std::size_t i = 0; i < size; ++i ) {
			for( std::size_t j = i+1; j < size; ++j ) {
				reconstructed_kinship_matrix(i,j) = std::numeric_limits< double >::quiet_NaN() ;
			}
		}

		m_ui_context.logger() << "Top-left of reconstructed kinship matrix is:\n" ;
		pca::write_matrix_to_stream( m_ui_context.logger(), reconstructed_kinship_matrix.block( 0, 0, size, size )) ;

		m_ui_context.logger() << "Verifying the decomposition...\n" ;
		double diff = 0.0 ;
		for( std::size_t i = 0; i < size; ++i ) {
			for( std::size_t j = 0; j <= i; ++j ) {
				diff = std::max( diff, std::abs( reconstructed_kinship_matrix(i,j) - m_kinship_matrix( i, j ))) ;
			}
		}
		m_ui_context.logger() << "...maximum discrepancy in lower diagonal is " << diff << ".\n" ;
		if( diff != diff ) {
			m_ui_context.logger() << "...yikes, there were NaNs.\n" ;
		} else if( diff > 0.01 ) {
			m_ui_context.logger() << "...warning: diff > 0.01.\n" ;
		}
	}

	using genfile::string_utils::to_string ;

	send_UDUT(
		"Number of SNPs: " + to_string( m_number_of_snps ) + "\n" +
			"Number of samples: " + to_string( m_number_of_samples ) + "\n"
			"Note: this file contains an eigenvalue decomposition of the matrix in the file \"" + m_filename + "\"\n" +
			"The first column contains the eigenvalues of the decomposition, and subsequent columns contain the eigenvectors.",
		m_number_of_snps,
		kinship_eigendecomposition,
		boost::function< genfile::VariantEntry ( int ) >(),
		boost::bind(
			&get_eigendecomposition_header,
			"v",
			_1
		)
	) ;

	if( m_number_of_PCs_to_compute > 0 ) {
		//
		// Let X  be the L\times n matrix (L SNPs, n samples) of (mean-centred, scaled) genotypes.
		// We want the projection of columns of X onto the matrix S whose columns are unit eigenvectors of (1/L) X X^t
		// where L is the number of SNPs.
		// to compute the row of the matrix S of unit eigenvectors of X X^t that corresponds to the current SNP.
		// The matrix S is given by
		//               1
		//       S = ------- X U D^{-1/2}
		//           \sqrt(L)
		// where
		//       (1/L) X^t X = U D U^t
		// is the eigenvalue decomposition of (1/L) X^t X that we are passed in via set_UDUT (and L is the number of SNPs).
		//
		// The projections are then given by S^t X = \sqrt(L) U D^{1/2}
		// We also wish to compute the correlation between the SNP and the PCA component.
		// With S as above, the PCA components are the projections of columns of X onto columns of S.
		// If we want samples to correspond to columns, this is
		//   S^t X
		// which can be re-written
		//   sqrt(L) U D^{1/2}
		// i.e. we may as well compute the correlation with columns of U.

		m_number_of_PCs_to_compute = std::min( m_number_of_PCs_to_compute, m_number_of_samples ) ;
		m_number_of_PCs_to_compute = std::min( m_number_of_PCs_to_compute, m_number_of_snps ) ;

		// PCAs are given by U D^{1/2}.
		Eigen::VectorXd PCA_eigenvalues = kinship_eigendecomposition.block( 0, 0, m_number_of_PCs_to_compute, 1 ) ;
		Eigen::VectorXd v = PCA_eigenvalues.array().sqrt() ;
		Eigen::MatrixXd PCAs
			= kinship_eigendecomposition.block( 0, 1, m_number_of_samples, m_number_of_PCs_to_compute ) * v.asDiagonal() ;

		send_PCs(
			"Number of SNPs: " + to_string( m_number_of_snps ) + "\n" +
			"Number of samples: " + to_string( m_number_of_samples ) + "\n" +
			"Note: the PCs computed here are 1/√L times the projection of samples onto unit eigenvectors of the variance-covariance matrix\n"
			"    (1/L) X Xᵗ\n"
			"where X is the L x N matrix of genotypes, L is the number of SNPs, and N the number of samples.\n"
			"The constant 1/√L ensures that the PCs do not grow with the number of SNPs.\n"
			"The PCAs are here computed as U D^½ where U and D are the matrices in the UDUᵗ decomposition of the kinship matrix\n"
			"in \"" + m_options.get< std::string >( "-UDUT" ) + "\".\n",
			PCA_eigenvalues,
			PCAs,
			boost::bind(
				&pca::get_concatenated_sample_ids,
				&m_samples,
				_1
			),
			&PCAComputer::get_pca_name
		) ;
	}

	m_ui_context.logger() << "========================================================================\n" ;
}

void PCAComputer::send_UDUT_to( UDUTCallback callback ) {
	m_UDUT_signal.connect( callback ) ;
}

void PCAComputer::send_PCs_to( SampleStorage::SharedPtr storage ) {
	m_PC_storage = storage ;
}

void PCAComputer::send_UDUT( std::string description, std::size_t number_of_snps, Eigen::MatrixXd const& UDUT, GetNames row_names, GetNames column_names ) {
	m_UDUT_signal( description, number_of_snps, UDUT, row_names, column_names ) ;
}

void PCAComputer::send_PCs(
	std::string description,
	Eigen::VectorXd const& eigenvalues,
	Eigen::MatrixXd const& PCAs,
	GetNames pca_row_names,
	GetNames pca_column_names
) {
	for( int i = 0; i < PCAs.rows(); ++i ) {
		for( int j = 0; j < PCAs.cols(); ++j ) {
			m_PC_storage->store_per_sample_data(
				"PCAComputer",
				std::size_t(i),
				pca_column_names(j).as< std::string>(),
				description,
				PCAs(i,j)
			) ;
		}
	}
}


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

  Defines the sign_of meta-function

  @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_DETAIL_DISPATCH_META_SIGN_OF_HPP_INCLUDED
#define BOOST_SIMD_DETAIL_DISPATCH_META_SIGN_OF_HPP_INCLUDED

#include <boost/simd/detail/dispatch/meta/primitive_of.hpp>
#include <type_traits>

namespace boost { namespace dispatch
{
  namespace ext
  {
    template<typename T, typename Enable = void> struct  sign_of
    {
      using primitive = boost::dispatch::primitive_of_t<T>;
      using signed_t  = std::is_signed<primitive>;
      using float_t   = std::is_floating_point<primitive>;
      using type      = typename std::conditional < (signed_t::value || float_t::value)
                                                  , signed
                                                  , unsigned
                                                  >::type;
    };
  }

  /*!
    @ingroup group-introspection
    @brief Return sign of a type

    If a type @c T can contains value with a sign (i.e signed integers and floating points-like
    types), sign_of<T> evaluates to @c signed. Otherwise, it evaluates @c unsigned.

    @tparam T Type to introspect.
  **/
  template<typename T>
  struct sign_of : ext::sign_of<typename std::decay<T>::type>
  {};

  /// Eager shortcut for sign_of
  template<typename T> using sign_of_t = typename sign_of<T>::type;
} }

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2014-2020, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_IS_VALID_HAS_DUPLICATES_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_IS_VALID_HAS_DUPLICATES_HPP

#include <boost/core/ignore_unused.hpp>
#include <boost/range/begin.hpp>
#include <boost/range/end.hpp>
#include <boost/range/size.hpp>

#include <boost/geometry/core/closure.hpp>

#include <boost/geometry/policies/compare.hpp>
#include <boost/geometry/policies/is_valid/default_policy.hpp>

#include <boost/geometry/views/closeable_view.hpp>
#include <boost/geometry/algorithms/validity_failure_type.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace is_valid
{

template <typename Range, closure_selector Closure, typename CSTag>
struct has_duplicates
{
    template <typename VisitPolicy>
    static inline bool apply(Range const& range, VisitPolicy& visitor)
    {
        boost::ignore_unused(visitor);

        typedef typename closeable_view<Range const, Closure>::type view_type;
        typedef typename boost::range_const_iterator
            <
                view_type const
            >::type const_iterator;

        view_type view(range);

        if ( boost::size(view) < 2 )
        {
            return ! visitor.template apply<no_failure>();
        }

        geometry::equal_to
            <
                typename boost::range_value<Range>::type,
                -1,
                CSTag
            > equal;

        const_iterator it = boost::const_begin(view);
        const_iterator next = it;
        ++next;
        for (; next != boost::const_end(view); ++it, ++next)
        {
            if ( equal(*it, *next) )
            {
                return ! visitor.template apply<failure_duplicate_points>(*it);
            }
        }
        return ! visitor.template apply<no_failure>();
    }
};

}} // namespace detail::is_valid
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_IS_VALID_HAS_DUPLICATES_HPP


// tableau.hpp
#ifndef TABLEAU_H_
#define TABLEAU_H_
/* ------------------------------------------------------------------------- */
// Includes
/* ------------------------------------------------------------------------- */
#include <vector>
#include <boost/python.hpp>
/* ------------------------------------------------------------------------- */

namespace py = boost::python;

/* ------------------------------------------------------------------------- */
// Type Definitions
/* ------------------------------------------------------------------------- */
typedef std::vector< double > vec1D;
typedef std::vector< std::vector< double > > vec2D;
/* ------------------------------------------------------------------------- */

class Tableau {
	private:
		vec2D tableau_;
		int stages_;
		vec1D weights_;
		vec2D rk_mat_;
		vec1D nodes_;

	public:
		/* ----------------------------------------------------------------- */
		// Constructors, destructor
		/* ----------------------------------------------------------------- */
		Tableau();
		Tableau(vec2D&);
		Tableau(py::list&);
		Tableau(const Tableau&);
		~Tableau();

		void reset(vec2D&); // "constructor" to use after declaration
		void reset(py::list&); // "constructor" to use after declaration
		/* ----------------------------------------------------------------- */

		void setTableau(vec2D&);
		void setTableau(py::list&);

		vec2D getTableau();

		void setStages();
		int getStages();

		void setNodes();
		vec1D getNodes();

		void setrkMat();
		vec2D getrkMat();

		void setWeights();
		vec1D getWeights();
};

#endif


#include "stdafx.h"

#include "problem.hpp"
#include "utility.hpp"

#include <fstream>
#include <utility>
#include <vector>

#include <boost/algorithm/string.hpp>

struct advent_2017_6 : problem
{
	advent_2017_6() noexcept : problem(2017, 6) {
	}

	using memory_type = std::vector<std::size_t>;

protected:
	memory_type memory;

	void prepare_input(std::ifstream& fin) override {
		std::string line;
		std::getline(fin, line);

		std::vector<std::string> elements;
		boost::split(elements, line, [](char c) { return c == '\t'; });
		std::transform(std::begin(elements), std::end(elements), std::back_inserter(memory), [](const std::string& element) {
			return std::stoull(element);
		});
	}

	std::size_t rebalance_iterations = 0;
	std::size_t cycle_length         = 0;

	void precompute() override {
		memory_type my_memory(memory);
		using cyc_it = utility::cyclic_iterator<memory_type::iterator>;
		std::vector<memory_type> previous_configurations;
		for(;;) {
			++rebalance_iterations;
			auto highest       = std::max_element(std::begin(my_memory), std::end(my_memory));
			std::size_t blocks = std::exchange(*highest, 0);
			for(cyc_it mem_cyc(++highest, std::begin(my_memory), std::end(my_memory)); blocks != 0; ++mem_cyc) {
				*mem_cyc += 1;
				--blocks;
			}
			if(auto it = std::find(std::begin(previous_configurations), std::end(previous_configurations), my_memory);
			   it != previous_configurations.end()) {
				cycle_length = gsl::narrow<std::size_t>(std::distance(it, std::end(previous_configurations)));
				break;
			} else {
				previous_configurations.push_back(my_memory);
			}
		}
	}

	std::string part_1() override {
		return std::to_string(rebalance_iterations);
	}

	std::string part_2() override {
		return std::to_string(cycle_length);
	}
};

REGISTER_SOLVER(2017, 6);


/* ----------------------------------------------------------------------------
 * Copyright 2020, Massachusetts Institute of Technology, * Cambridge, MA 02139
 * All Rights Reserved
 * Authors: Yulun Tian, et al. (see README for the full author list)
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */
#include <cassert>
#include <DPGO/PGOLogger.h>
#include <Eigen/Geometry>
#include <utility>

namespace DPGO {

PGOLogger::PGOLogger(std::string logDir) : logDirectory(std::move(logDir)) {}

PGOLogger::~PGOLogger() = default;

void PGOLogger::logMeasurements(std::vector<RelativeSEMeasurement> &measurements, const std::string &filename) {
  if (measurements.empty()) return;

  std::ofstream file;
  file.open(logDirectory + filename);
  if (!file.is_open()) return;

  size_t d = measurements[0].R.rows();
  if (d == 2) return;

  // Insert header row
  file << "robot_src,pose_src,robot_dst,pose_dst,qx,qy,qz,qw,tx,ty,tz,kappa,tau,is_known_inlier,weight\n";

  for (RelativeSEMeasurement m: measurements) {
    // Convert rotation matrix to quaternion
    Eigen::Matrix3d R = m.R;
    Eigen::Quaternion<double> quat(R);
    file << m.r1 << ",";
    file << m.p1 << ",";
    file << m.r2 << ",";
    file << m.p2 << ",";
    file << quat.x() << ",";
    file << quat.y() << ",";
    file << quat.z() << ",";
    file << quat.w() << ",";
    file << m.t(0) << ",";
    file << m.t(1) << ",";
    file << m.t(2) << ",";
    file << m.kappa << ",";
    file << m.tau << ",";
    file << m.isKnownInlier << ",";
    file << m.weight << "\n";
  }

  file.close();
}

void PGOLogger::logTrajectory(unsigned int d, unsigned int n, const Matrix &T, const std::string &filename) {
  if (d == 2) return;
  assert(T.rows() == d);
  assert(T.cols() == (d + 1) * n);
  std::ofstream file;
  file.open(logDirectory + filename);
  if (!file.is_open()) return;

  // Insert header row
  file << "pose_index,qx,qy,qz,qw,tx,ty,tz\n";

  for (size_t i = 0; i < n; ++i) {
    Eigen::Matrix3d R = T.block(0, i * (d + 1), d, d);
    Eigen::Quaternion<double> quat(R);
    Matrix t = T.block(0, i * (d + 1) + d, d, 1);
    file << i << ",";
    file << quat.x() << ",";
    file << quat.y() << ",";
    file << quat.z() << ",";
    file << quat.w() << ",";
    file << t(0) << ",";
    file << t(1) << ",";
    file << t(2) << "\n";
  }

  file.close();
}

Matrix PGOLogger::loadTrajectory(const std::string &filename) {
  std::ifstream infile(logDirectory + filename);
  std::cout << "Loading trajectory from " << logDirectory + filename << "..." << std::endl;
  if (!infile.is_open()) {
    std::cout << "Could not open specified file!" << std::endl;
    return Matrix(0, 0);
  }

  std::unordered_map<uint32_t, Matrix> Tmap;

  // Scalars that will be populated
  uint32_t pose_id = 0;
  uint32_t num_poses = 0;
  double qx, qy, qz, qw;
  double tx, ty, tz;

  std::string line;
  std::string token;

  // Skip first line (headers)
  std::getline(infile, line);

  // Iterate over remaining lines
  while (std::getline(infile, line)) {
    std::istringstream ss(line);
    num_poses++;

    std::getline(ss, token, ',');
    pose_id = std::stoi(token);

    std::getline(ss, token, ',');
    qx = std::stod(token);
    std::getline(ss, token, ',');
    qy = std::stod(token);
    std::getline(ss, token, ',');
    qz = std::stod(token);
    std::getline(ss, token, ',');
    qw = std::stod(token);
    Eigen::Quaternion<double> quat(qw, qx, qy, qz);
    quat.normalize();

    std::getline(ss, token, ',');
    tx = std::stod(token);
    std::getline(ss, token, ',');
    ty = std::stod(token);
    std::getline(ss, token, ',');
    tz = std::stod(token);
    Eigen::Vector3d tVec;
    tVec << tx, ty, tz;

    Matrix Ti(3, 4);
    Ti.block(0, 0, 3, 3) = quat.toRotationMatrix();
    Ti.block(0, 3, 3, 1) = tVec;
    Tmap.emplace(pose_id, Ti);
  }

  Matrix T = Matrix(3, 4 * num_poses);
  for (unsigned i = 0; i < num_poses; ++i) {
    T.block(0, 4 * i, 3, 4) = Tmap.at(i);
  }

  std::cout << "Loaded " << num_poses << " poses." << std::endl;
  return T;
}

std::vector<RelativeSEMeasurement> PGOLogger::loadMeasurements(const std::string &filename, bool load_weight) {
  std::vector<RelativeSEMeasurement> measurements;
  std::cout << "Loading measurements from " << logDirectory + filename << "..." << std::endl;
  std::ifstream infile(logDirectory + filename);

  if (!infile.is_open()) {
    std::cout << "Could not open specified file!" << std::endl;
    return measurements;
  }

  // Scalars that will be filled upon reading each measurement
  uint32_t robot_src, robot_dst, pose_src, pose_dst;
  double qx, qy, qz, qw;
  double tx, ty, tz;
  double kappa, tau, weight;
  bool is_known_inlier;

  std::string line;
  std::string token;

  // Skip first line (headers)
  std::getline(infile, line);

  // Iterate over remaining lines
  while (std::getline(infile, line)) {
    std::istringstream ss(line);

    std::getline(ss, token, ',');
    robot_src = std::stoi(token);
    std::getline(ss, token, ',');
    pose_src = std::stoi(token);
    std::getline(ss, token, ',');
    robot_dst = std::stoi(token);
    std::getline(ss, token, ',');
    pose_dst = std::stoi(token);

    std::getline(ss, token, ',');
    qx = std::stod(token);
    std::getline(ss, token, ',');
    qy = std::stod(token);
    std::getline(ss, token, ',');
    qz = std::stod(token);
    std::getline(ss, token, ',');
    qw = std::stod(token);
    Eigen::Quaternion<double> quat(qw, qx, qy, qz);
    quat.normalize();

    std::getline(ss, token, ',');
    tx = std::stod(token);
    std::getline(ss, token, ',');
    ty = std::stod(token);
    std::getline(ss, token, ',');
    tz = std::stod(token);
    Eigen::Vector3d tVec;
    tVec << tx, ty, tz;

    std::getline(ss, token, ',');
    kappa = std::stod(token);
    std::getline(ss, token, ',');
    tau = std::stod(token);
    std::getline(ss, token, ',');
    is_known_inlier = std::stoi(token);
    std::getline(ss, token, ',');
    weight = std::stod(token);

    RelativeSEMeasurement m(robot_src, robot_dst, pose_src, pose_dst,
                            quat.toRotationMatrix(), tVec,
                            kappa, tau);
    m.isKnownInlier = is_known_inlier;
    if (load_weight)
      m.weight = weight;

    measurements.push_back(m);
  }

  printf("Loaded %zu measurements.\n", measurements.size());
  return measurements;
}

}