/* Copyright (c) 2016, Julian Straub <jstraub@csail.mit.edu> Licensed
 * under the MIT license. See the license file LICENSE.
 */
#include <thread>
#include <pangolin/pangolin.h>
#include <pangolin/video/video_record_repeat.h>
#include <pangolin/gl/gltexturecache.h>
#include <pangolin/gl/glpixformat.h>
#include <pangolin/handler/handler_image.h>
#include <pangolin/utils/file_utils.h>
#include <pangolin/utils/timer.h>
#include <pangolin/gl/gl.h>
#include <pangolin/gl/glcuda.h>
#include <pangolin/gl/gldraw.h>

#include <Eigen/Dense>
#include <tdp/camera/rig.h>
#include <tdp/data/image.h>
#include <tdp/data/managed_image.h>
#include <tdp/data/managed_pyramid.h>
#include <tdp/data/managed_volume.h>
#include <tdp/data/pyramid.h>
#include <tdp/data/volume.h>
#include <tdp/directional/hist.h>
#include <tdp/gl/gl_draw.h>
#include <tdp/gui/gui_base.hpp>
#include <tdp/gui/quickView.h>
#include <tdp/icp/icp.h>
#include <tdp/icp/icpTexture.h>
#include <tdp/manifold/SE3.h>
#include <tdp/nvidia/helper_cuda.h>
#include <tdp/preproc/convolutionSeparable.h>
#include <tdp/preproc/depth.h>
#include <tdp/preproc/normals.h>
#include <tdp/preproc/pc.h>
#include <tdp/tsdf/tsdf.h>
#include <tdp/utils/Stopwatch.h>
#include <tdp/inertial/imu_factory.h>
#include <tdp/inertial/imu_interpolator.h>
#include <tdp/manifold/SO3.h>
#include <tdp/preproc/grad.h>
#include <tdp/preproc/grey.h>
#include <tdp/slam/keyframe.h>
#include <tdp/slam/keyframe_slam.h>
#include <tdp/gl/shaders.h>
#include <tdp/rtmf/vMFMMF.h>
#include <tdp/utils/colorMap.h>
#include <tdp/io/tinyply.h>
#include <tdp/preproc/convert.h>
#include <tdp/features/keyframe.h>
#include <tdp/marching_cubes/marching_cubes.h>

typedef tdp::CameraPoly3f CameraT;
//typedef tdp::Cameraf CameraT;

int main( int argc, char* argv[] )
{
  std::string input_uri = "openni2://";
  std::string output_uri = "pango://video.pango";
  std::string calibPath = "";
  std::string imu_input_uri = "";
  std::string tsdfOutputPath = "tsdf.raw";
  bool runOnce = false;

  if( argc > 1 ) {
    input_uri = std::string(argv[1]);
    calibPath = (argc > 2) ? std::string(argv[2]) : "";
    if (argc > 3 && std::string(argv[3]).compare("-1") == 0 ) runOnce = true;
//    imu_input_uri =  (argc > 3)? std::string(argv[3]) : "";
  }

  pangolin::Uri uri = pangolin::ParseUri(input_uri);
  if (!uri.scheme.compare("file")) {
    std::cout << uri.scheme << std::endl;
    if (pangolin::FileExists(uri.url+std::string("imu.pango"))
     && pangolin::FileExists(uri.url+std::string("video.pango"))) {
//      imu_input_uri = input_uri + std::string("imu.pango");
      tsdfOutputPath = uri.url + tsdfOutputPath;
      input_uri = input_uri + std::string("video.pango");
    } else if (pangolin::FileExists(uri.url+std::string("video.pango"))) {
      input_uri = input_uri + std::string("video.pango");
      tsdfOutputPath = uri.url + tsdfOutputPath;
    }
  }

  std::cout << input_uri << std::endl;
  std::cout << imu_input_uri << std::endl;

  Stopwatch::getInstance().setCustomSignature(1243984912);

  // Open Video by URI
  pangolin::VideoRecordRepeat video(input_uri, output_uri);
  const size_t num_streams = video.Streams().size();

  if(num_streams == 0) {
    pango_print_error("No video streams from device.\n");
    return 1;
  }

  // optionally connect to IMU if it is found.
  tdp::ImuInterface* imu = nullptr;
  if (imu_input_uri.size() > 0)
    imu = tdp::OpenImu(imu_input_uri);
  if (imu) imu->Start();
  tdp::ImuInterpolator imuInterp(imu,nullptr);
  imuInterp.Start();

  tdp::GuiBase gui(1200,800,video);
  gui.container().SetLayout(pangolin::LayoutEqual);

  size_t dTSDF = 512;
  size_t wTSDF = 512;
  size_t hTSDF = 512;

  tdp::Rig<CameraT> rig;
  if (calibPath.size() > 0) {
    rig.FromFile(calibPath,false);
    std::vector<pangolin::VideoInterface*>& streams = video.InputStreams();
    rig.CorrespondOpenniStreams2Cams(streams);
  } else {
    return 2;
  }

  size_t w = video.Streams()[gui.iRGB[0]].Width();
  size_t h = video.Streams()[gui.iRGB[0]].Height();
  size_t wc = (w+w%64); // for convolution
  size_t hc = rig.NumCams()*(h+h%64);
  wc += wc%64;
  hc += hc%64;

  tdp::Camera<float> camView(Eigen::Vector4f(220,220,319.5,239.5));
  // Define Camera Render Object (for view / scene browsing)
  pangolin::OpenGlRenderState s_cam(
      pangolin::ProjectionMatrix(640,480,420,420,319.5,239.5,0.1,1000),
      pangolin::ModelViewLookAt(0,0.5,-3, 0,0,0, pangolin::AxisNegY)
      );
  // Add named OpenGL viewport to window and provide 3D Handler
  pangolin::View& viewPc3D = pangolin::CreateDisplay()
    .SetHandler(new pangolin::Handler3D(s_cam));
  gui.container().AddDisplay(viewPc3D);

  pangolin::OpenGlRenderState camNormals(
      pangolin::ProjectionMatrix(640,3*480,420,3*420,320,3*240,0.1,1000),
      pangolin::ModelViewLookAt(0,0.5,-3, 0,0,0, pangolin::AxisNegY)
      );
  pangolin::View& viewNormals = pangolin::CreateDisplay()
    .SetHandler(new pangolin::Handler3D(camNormals));
  gui.container().AddDisplay(viewNormals);

  pangolin::View& containerTracking = pangolin::Display("tracking");
  containerTracking.SetLayout(pangolin::LayoutEqual);
  tdp::QuickView viewKf(wc, hc);
  containerTracking.AddDisplay(viewKf);
  tdp::QuickView viewCurrent(wc, hc);
  containerTracking.AddDisplay(viewCurrent);
  gui.container().AddDisplay(containerTracking);

  pangolin::View& containerLoopClosure = pangolin::Display("loopClosures");
  containerLoopClosure.SetLayout(pangolin::LayoutEqual);
  pangolin::OpenGlRenderState camLoopClose(
      pangolin::ProjectionMatrix(640,3*480,420,3*420,320,3*240,0.1,1000),
      pangolin::ModelViewLookAt(0,0.5,-3, 0,0,0, pangolin::AxisNegY)
      );
  pangolin::View& viewLoopClose = pangolin::CreateDisplay()
    .SetHandler(new pangolin::Handler3D(camLoopClose));
  containerLoopClosure.AddDisplay(viewLoopClose);
  tdp::QuickView viewDebugA(wc, hc);
  containerLoopClosure.AddDisplay(viewDebugA);
  tdp::QuickView viewDebugB(wc, hc);
  containerLoopClosure.AddDisplay(viewDebugB);
  tdp::QuickView viewDebugC(wc, hc);
  containerLoopClosure.AddDisplay(viewDebugC);
  tdp::QuickView viewDebugD(wc, hc);
  containerLoopClosure.AddDisplay(viewDebugD);

  tdp::QuickView viewDebugE(wc, hc);
  containerLoopClosure.AddDisplay(viewDebugE);
  tdp::QuickView viewDebugF(wc, hc);
  containerLoopClosure.AddDisplay(viewDebugF);

  gui.container().AddDisplay(containerLoopClosure);

  pangolin::View& plotters = pangolin::Display("plotters");
  plotters.SetLayout(pangolin::LayoutEqualVertical);
//  pangolin::DataLog logInliers;
//  pangolin::Plotter plotInliers(&logInliers, -100.f,1.f, 0, 130000.f,
//      10.f, 0.1f);
//  plotters.AddDisplay(plotInliers);
//  pangolin::DataLog logCost;
//  pangolin::Plotter plotCost(&logCost, -100.f,1.f, -10.f,1.f, 10.f, 0.1f);
//  plotters.AddDisplay(plotCost);
//  pangolin::DataLog logRmse;
//  pangolin::Plotter plotRmse(&logRmse, -100.f,1.f, 0.f,0.2f, 0.1f, 0.1f);
//  plotters.AddDisplay(plotRmse);
  pangolin::DataLog logdH;
  pangolin::Plotter plotdH(&logdH, -100.f,1.f, .5f,1.5f, .1f, 0.1f);
  plotters.AddDisplay(plotdH);
  pangolin::DataLog logEntropy;
  pangolin::Plotter plotH(&logEntropy, -100.f,1.f, -80.f,-40.f, .1f, 0.1f);
  plotters.AddDisplay(plotH);
  gui.container().AddDisplay(plotters);

  tdp::ManagedHostImage<float> d(wc, hc);
  tdp::ManagedHostImage<tdp::Vector3bda> n2D(wc,hc);
  memset(n2D.ptr_,0,n2D.SizeBytes());
  tdp::ManagedHostImage<tdp::Vector3fda> n2Df(wc,hc);
  tdp::ManagedHostImage<tdp::Vector3fda> n(wc,hc);
  tdp::ManagedHostImage<tdp::Vector3bda> rgb(wc,hc);
  tdp::ManagedHostImage<tdp::Vector3fda> pc(wc, hc);

  tdp::ManagedDeviceImage<tdp::Vector3bda> cuRgb(wc,hc);
  tdp::ManagedDeviceImage<float> cuGrey(wc,hc);
  tdp::ManagedDeviceImage<float> cuGreydv(wc,hc);
  tdp::ManagedDeviceImage<float> cuGreydu(wc,hc);
  tdp::ManagedDeviceImage<tdp::Vector3fda> cuGrad3D(wc,hc);

  tdp::ManagedHostImage<float> greyA(wc, hc);
  tdp::ManagedHostImage<float> greyB(wc, hc);
  tdp::ManagedHostImage<float> photoErrBefore(wc, hc);
  tdp::ManagedHostImage<float> photoErrAfter(wc, hc);
  tdp::ManagedDeviceImage<float> cuPhotoErrAfter(wc, hc);

  tdp::SE3f T_abSuccess;

  tdp::ManagedDeviceImage<tdp::Vector3fda> cuPcA(wc, hc);
  tdp::ManagedDeviceImage<tdp::Vector3fda> cuNA(wc, hc);
  tdp::ManagedDeviceImage<tdp::Vector3fda> cuPcB(wc, hc);
  tdp::ManagedDeviceImage<tdp::Vector3fda> cuNB(wc, hc);
  tdp::ManagedHostImage<int> assoc_ba(wc,hc);
  tdp::ManagedDeviceImage<int> cuAssoc_ba(wc,hc);

  tdp::ManagedDeviceImage<uint16_t> cuDraw(wc, hc);
  tdp::ManagedDeviceImage<float> cuD(wc, hc);

  tdp::ManagedHostVolume<tdp::TSDFval> TSDF(wTSDF, hTSDF, dTSDF);
  TSDF.Fill(tdp::TSDFval(-1.01,0.));
  tdp::ManagedDeviceVolume<tdp::TSDFval> cuTSDF(wTSDF, hTSDF, dTSDF);
  cuTSDF.CopyFrom(TSDF);

  // mesh buffers
  pangolin::GlBuffer meshVbo;
  pangolin::GlBuffer meshCbo;
  pangolin::GlBuffer meshIbo;

  tdp::ManagedDeviceImage<float> cuDView(wc, hc);
  tdp::ManagedDeviceImage<tdp::Vector3fda> cuPcView(wc, hc);

  tdp::ManagedHostImage<float> greyDu(wc, hc);
  tdp::ManagedHostImage<float> greyDv(wc, hc);
  tdp::ManagedDeviceImage<float> cuGreyDv(wc,hc);
  tdp::ManagedDeviceImage<float> cuGreyDu(wc,hc);

  // ICP stuff
  tdp::ManagedHostPyramid<float,3> dPyr(wc,hc);
  tdp::ManagedHostPyramid<float,3> dPyrEst(wc,hc);
  tdp::ManagedDevicePyramid<float,3> cuDPyr(wc,hc);
  tdp::ManagedDevicePyramid<float,3> cuDPyrEst(wc,hc);
  tdp::ManagedDevicePyramid<tdp::Vector3fda,3> pcs_m(wc,hc);
  tdp::ManagedDevicePyramid<tdp::Vector3fda,3> pcs_c(wc,hc);
  tdp::ManagedDevicePyramid<tdp::Vector3fda,3> ns_m(wc,hc);
  tdp::ManagedDevicePyramid<tdp::Vector3fda,3> ns_c(wc,hc);
  tdp::ManagedDevicePyramid<tdp::Vector3fda,3> gs_m(wc,hc);
  tdp::ManagedDevicePyramid<tdp::Vector3fda,3> gs_c(wc,hc);

  tdp::ManagedDevicePyramid<float,3> cuPyrGrey_c(wc,hc);
  tdp::ManagedDevicePyramid<float,3> cuPyrGrey_m(wc,hc);
  tdp::ManagedDevicePyramid<uint8_t,3> cuPyrGreyB_c(wc,hc);
  tdp::ManagedDevicePyramid<tdp::Vector2fda,3> cuPyrGradGrey_c(wc,hc);
  tdp::ManagedDevicePyramid<tdp::Vector2fda,3> cuPyrGradGrey_m(wc,hc);

  tdp::ManagedDeviceImage<tdp::Vector3fda> nEstdummy(wc,hc);

  pangolin::GlBufferCudaPtr cuPcbuf(pangolin::GlArrayBuffer, wc*hc,
      GL_FLOAT, 3, cudaGraphicsMapFlagsNone, GL_DYNAMIC_DRAW);

  pangolin::GlBuffer vbo(pangolin::GlArrayBuffer,wc*hc,GL_FLOAT,3);
  pangolin::GlBuffer vboSuccessA(pangolin::GlArrayBuffer,wc*hc,GL_FLOAT,3);
  pangolin::GlBuffer vboSuccessB(pangolin::GlArrayBuffer,wc*hc,GL_FLOAT,3);
  pangolin::GlBuffer cbo(pangolin::GlArrayBuffer,wc*hc,GL_UNSIGNED_BYTE,3);

  tdp::ManagedHostImage<float> tsdfDEst(wc, hc);
  tdp::ManagedHostImage<float> tsdfSlice(wTSDF, hTSDF);
//  tdp::QuickView viewTsdfDEst(wc,hc);
//  gui.container().AddDisplay(viewTsdfDEst);

  tdp::ManagedHostImage<float> dispDepthPyr(dPyr.Width(0)+dPyr.Width(1), hc);

  tdp::ManagedDeviceImage<tdp::Vector3fda> cuDispNormalsPyr(ns_m.Width(0)+ns_m.Width(1), hc);
  tdp::ManagedDeviceImage<tdp::Vector3bda> cuDispNormals2dPyr(ns_m.Width(0)+ns_m.Width(1), hc);
  tdp::ManagedHostImage<tdp::Vector3bda> dispNormals2dPyr(ns_m.Width(0)+ns_m.Width(1), hc);

  tdp::QuickView viewGrad3DPyr(dispNormals2dPyr.w_,dispNormals2dPyr.h_);
  gui.container().AddDisplay(viewGrad3DPyr);
  viewGrad3DPyr.Show(false);

  pangolin::Var<float> depthSensorScale("ui.depth sensor scale",1e-3,1e-4,1e-3);
  pangolin::Var<float> dMin("ui.d min",0.10,0.0,0.1);
  pangolin::Var<float> dMax("ui.d max",6.,0.1,10.);
  pangolin::Var<bool> useRgbCamParasForDepth("ui.use RGB cam", true, true);

  pangolin::Var<bool> dispNormalsPyrEst("ui.disp normal est", false, true);
  pangolin::Var<int>   dispLvl("ui.disp lvl",0,0,2);

  pangolin::Var<bool> runSlamFusion("ui.run SLAM fusion", false,true);
  pangolin::Var<bool>  runICP("ui.run ICP", true, true);
  pangolin::Var<bool>  recomputeBoundingBox("ui.compute BB", false, false);
  pangolin::Var<bool> printGraph("ui.print graph",false,false);

  pangolin::Var<bool> resetICP("ui.reset ICP",false,false);
  pangolin::Var<float> icpAngleThr_deg("ui.icp angle thr",25,0.,90.);
  pangolin::Var<float> icpDistThr("ui.icp dist thr",0.20,0.,1.);
  pangolin::Var<bool> trackClosestKf("ui.track closest KF",false,true);
  pangolin::Var<int>   icpIter0("ui.ICP iter lvl 0",10,0,10);
  pangolin::Var<int>   icpIter1("ui.ICP iter lvl 1",7,0,10);
  pangolin::Var<int>   icpIter2("ui.ICP iter lvl 2",5,0,10);

  pangolin::Var<bool>  icpRgb("ui.run ICP RGB", true, true);

  pangolin::Var<bool> useOptimizedPoses("ui.use opt poses", true,true);
  pangolin::Var<bool> tryLoopClose("ui.loop close", true,true);
  pangolin::Var<bool> retryAllLoopClosures("ui.retry all loop close", false,false);
  pangolin::Var<bool> retryLoopClosure("ui.retry loop close", false,false);
  pangolin::Var<int>   loopCloseA("ui.loopClose A",0,0,10);
  pangolin::Var<int>   loopCloseB("ui.loopClose B",1,0,10);
  pangolin::Var<int>   numLoopClose("ui.Num loopClose",0,0,0);
  pangolin::Var<int>   maxLoopClosures("ui.maxLoopClosures",40,0,30);

  pangolin::Var<bool> useRansac("ui.Ransac", true,true);
  pangolin::Var<float> ransacMaxIt("ui.max it",3000,1,1000);
  pangolin::Var<float> ransacThr("ui.thr",0.20,0.01,1.0);
  pangolin::Var<float> ransacInlierThr("ui.inlier thr",6,1,20);
  pangolin::Var<int> briefMatchThr("ui.BRIEF match",65,0,100);
  pangolin::Var<int> fastLvl("ui.FAST lvl",0,0,2);
  pangolin::Var<int> fastB("ui.FAST b",30,0,100);
  pangolin::Var<float> harrisThr("ui.harris thr",0.1,0.001,2.0);
  pangolin::Var<float> kappaHarris("ui.kappa harris",0.08,0.04,0.15);

  pangolin::Var<bool>  useANN("ui.use ANN", false,true);
  pangolin::Var<bool>  showAfterOpt("ui.show after opt", false,true);
  pangolin::Var<float> keyFrameDistThresh("ui.KF dist thr", 0.75, 0.35, 0.5);
  pangolin::Var<float> keyFrameAngleThresh("ui.KF angle thr", 35, 15, 50);
  pangolin::Var<float> dEntropyThr("ui.dH Thr", 0.95, 0.5, 1.);
  pangolin::Var<int>   icpDownSample("ui.ICP downsample",100,1,200);
  pangolin::Var<float> loopCloseDistThresh( "ui.loop dist thr", 0.80, 0.01, 0.5);
  pangolin::Var<float> loopCloseAngleThresh("ui.loop angle thr", 140, 1, 180);
  pangolin::Var<float> icpLoopCloseAngleThr_deg("ui.icpLoop angle thr",20,0.,90.);
  pangolin::Var<float> icpLoopCloseDistThr("ui.icpLoop dist thr",0.30,0.,1.);
  pangolin::Var<int>   icpLoopCloseIter0("ui.icpLoop iter lvl 0",30,0,30);
  pangolin::Var<int>   icpLoopCloseOverlapLvl("ui.overlap lvl",0,0,2);
  pangolin::Var<float> icpLoopCloseOverlapThr("ui.overlap thr",0.30,0.,1.);
  pangolin::Var<float> rmseChangeThr("ui.dRMSE thr", -0.05,-1.,1.);
  pangolin::Var<float> rmseThr("ui.RMSE thr", 0.11,0.,1.);
  pangolin::Var<float> icpLoopCloseErrThr("ui.err thr",0,-10,0.);
  pangolin::Var<float> icpRgbLambda("ui.icp rgb lamb",0.1,0.,1.);

  pangolin::Var<bool> runKfOnlyFusion("ui.run KF Fusion",true,false);
  pangolin::Var<bool> saveKfs("ui.save KFs",true,false);

  pangolin::Var<bool> computePhotometricError("ui.comp Phot Err",false,true);

  pangolin::Var<bool>  runMarchingCubes("ui.run Marching Cubes", false, false);
  pangolin::Var<float> marchCubesfThr("ui.f Thr", 1.0,0.,1.);
  pangolin::Var<float> marchCubeswThr("ui.weight Thr", 0,0,10);
  pangolin::Var<float> histScale("ui.hist scale",40.,1.,100.);

  pangolin::Var<bool>  resetTSDF("ui.reset TSDF", false, false);
  pangolin::Var<bool>  saveTSDF("ui.save TSDF", false, false);
  pangolin::Var<float> tsdfMu("ui.mu",0.05,0.,0.05);
  pangolin::Var<float> tsdfWThr("ui.w thr",25.,1.,20.);
  pangolin::Var<float> tsdfWMax("ui.w max",200.,1.,300.);
  pangolin::Var<float> grid0x("ui.grid0 x",-3.0,-2.,0);
  pangolin::Var<float> grid0y("ui.grid0 y",-3.0,-2.,0);
  pangolin::Var<float> grid0z("ui.grid0 z",-3.0,-2.,0);
  pangolin::Var<float> gridEx("ui.gridE x", 3.0,2,3);
  pangolin::Var<float> gridEy("ui.gridE y", 3.0,2,3);
  pangolin::Var<float> gridEz("ui.gridE z", 3.0,2,3);
  pangolin::Var<int>   mmfId("ui.MMF id",0,0,2);

  pangolin::Var<bool> showPcModel("ui.show model",true,true);
  pangolin::Var<bool> showPcCurrent("ui.show current",true,true);

  pangolin::Var<bool>  icpRot("ui.run ICP Rot", false, true);
  pangolin::Var<bool>  icpImu("ui.use IMU to warm start ICP", false, true);
  pangolin::Var<bool>  icpRotOverwrites("ui.ICP Rot Overwrites", false, true);
  pangolin::Var<int>   icpRotIter0("ui.ICP rot iter lvl 0",10,0,10);
  pangolin::Var<int>   icpRotIter1("ui.ICP rot iter lvl 1",7,0,10);
  pangolin::Var<int>   icpRotIter2("ui.ICP rot iter lvl 2",5,0,10);

  tdp::Vector3fda grid0(grid0x,grid0y,grid0z);
  tdp::Vector3fda gridE(gridEx,gridEy,gridEz);
  tdp::Vector3fda dGrid = gridE - grid0;

  size_t numKfsPrev = 0;
  tdp::SE3f T_mo_0;
  tdp::SE3f T_mo = T_mo_0;
  tdp::SE3f T_wG;  // from grid to world

  tdp::SE3f T_ac; // current to active KF
  Eigen::Matrix<float,6,6> Sigma_ac = Eigen::Matrix<float,6,6>::Zero();

  tdp::KeyframeSLAM kfSLAM;
  std::vector<tdp::KeyFrame> kfs;
  std::vector<tdp::BinaryKF> binaryKfs;
  std::vector<float> logHs;
  std::vector<tdp::SE3f> T_mos;

  std::list<std::pair<int,int>> loopClose;
  std::map<std::pair<int,int>, float> rmses;

  gui.verbose = false;

  int idActive = 0;

  tdp::ThreadedValue<bool> runSave(true);
  std::thread workThread([&]() {
        while(runSave.Get()) {
          if (pangolin::Pushed(saveTSDF)) {
            TSDF.CopyFrom(cuTSDF);
            std::cout << "start writing TSDF to " << tsdfOutputPath << std::endl;
            tdp::TSDF::SaveTSDF(TSDF, grid0, dGrid, T_wG, tsdfOutputPath);
            std::cout << "done writing TSDF to " << tsdfOutputPath << std::endl;
          }
          std::this_thread::sleep_for(std::chrono::microseconds(100));
        }
      });

  tdp::ThreadedValue<bool> runLoopClosure(true);
  std::mutex mut;

  auto loopCloseKfs = [&](int idA, int idB) -> bool {
      tdp::KeyFrame& kfA = kfs[idA];
      tdp::KeyFrame& kfB = kfs[idB];
      Eigen::Matrix<float,6,1> se3 = kfA.T_wk_.Log(kfB.T_wk_);
      if ( se3.head<3>().norm()*180./M_PI < loopCloseAngleThresh
        && se3.tail<3>().norm()           < loopCloseDistThresh) {

        tdp::SE3f T_ab = kfA.T_wk_.Inverse() * kfB.T_wk_;
        std::cout << " checking " << idA << " -> " << idB
          << ": " << se3.head<3>().norm()*180./M_PI << " "
          << se3.tail<3>().norm()
          << std::endl;

        photoErrBefore.Fill(0.);
        float overlapBefore, rmseBefore;

        TICK("Overlap");
        cudaMemset(cuPhotoErrAfter.ptr_, 0, cuPhotoErrAfter.SizeBytes());
        Overlap(kfA, kfB, rig, icpLoopCloseOverlapLvl, overlapBefore,
            rmseBefore, nullptr, &cuPhotoErrAfter);
        photoErrBefore.CopyFrom(cuPhotoErrAfter);
        TOCK("Overlap");

        if (overlapBefore > icpLoopCloseOverlapThr || useRansac) {

          size_t numInliers = 0;
          if (useRansac && !MatchKFs(binaryKfs[idA], binaryKfs[idB],
                briefMatchThr, ransacMaxIt, ransacThr,
                ransacInlierThr, T_ab, numInliers)) {
            std::cout << "RANSAC failed -- aborting loop closure " << std::endl;
            return false;
          } else {
            std::cout << GREEN << "Ransac Succeded "
              << NORMAL << std::endl;

          }

          TICK("LoopClosure");
          float err=0.;
          float count=10000;
          Eigen::Matrix<float,6,6> Sigma_ab = 1e-6*Eigen::Matrix<float,6,6>::Identity();
          if (useANN) {
            cuPcA.CopyFrom(kfA.pc_);
            cuNA.CopyFrom(kfA.n_);
            cuPcB.CopyFrom(kfB.pc_);
            cuNB.CopyFrom(kfB.n_);
            tdp::ICP::ComputeANN(kfA.pc_, cuPcA, cuNA, kfB.pc_, cuPcB, cuNB,
              assoc_ba, cuAssoc_ba, T_ab, icpLoopCloseIter0,
              icpLoopCloseAngleThr_deg, icpLoopCloseDistThr,
              icpDownSample, gui.verbose, err, count);
            count *= icpDownSample;
          } else {
            // TODO test
            tdp::ManagedDevicePyramid<tdp::Vector3fda,3> pcs_m(wc,hc);
            tdp::ManagedDevicePyramid<tdp::Vector3fda,3> pcs_c(wc,hc);
            tdp::ManagedDevicePyramid<tdp::Vector3fda,3> ns_m(wc,hc);
            tdp::ManagedDevicePyramid<tdp::Vector3fda,3> ns_c(wc,hc);

            pcs_m.CopyFrom(kfA.pyrPc_);
            ns_m.CopyFrom(kfA.pyrN_);
            pcs_c.CopyFrom(kfB.pyrPc_);
            ns_c.CopyFrom(kfB.pyrN_);

            std::vector<size_t> maxIt = {icpIter0, icpIter1, icpIter2};
//            tdp::ICP::ComputeProjective(pcs_m, ns_m, pcs_c,
//                ns_c, T_ab, tdp::SE3f(),
//              camD, maxIt, icpLoopCloseAngleThr_deg,
//              icpLoopCloseDistThr, true | gui.verbose);
            std::vector<float> errPerLvl;
            std::vector<float> countPerLvl;
            if (icpRgb) {
              tdp::ManagedDevicePyramid<float,3> cuPyrGrey_c(wc,hc);
              tdp::ManagedDevicePyramid<float,3> cuPyrGrey_m(wc,hc);
              tdp::ManagedDevicePyramid<tdp::Vector2fda,3> cuPyrGradGrey_c(wc,hc);
              tdp::ManagedDevicePyramid<tdp::Vector2fda,3> cuPyrGradGrey_m(wc,hc);
              cuPyrGrey_m.CopyFrom(kfA.pyrGrey_);
              cuPyrGrey_c.CopyFrom(kfB.pyrGrey_);
              cuPyrGradGrey_m.CopyFrom(kfA.pyrGradGrey_);
              cuPyrGradGrey_c.CopyFrom(kfB.pyrGradGrey_);
              tdp::IcpTexture::ComputeProjective<CameraT>(pcs_m, ns_m,
                  cuPyrGradGrey_m, cuPyrGrey_m, pcs_c, ns_c, cuPyrGradGrey_c,
                  cuPyrGrey_c, rig, rig.rgbStream2cam_, maxIt, icpAngleThr_deg,
                  icpDistThr, icpRgbLambda, gui.verbose, T_ab, Sigma_ab,
                  errPerLvl, countPerLvl);
            } else {
              if (useRgbCamParasForDepth) {
                tdp::ICP::ComputeProjective<CameraT>(pcs_m, ns_m, pcs_c, ns_c,
                    rig, rig.rgbStream2cam_, maxIt, icpLoopCloseAngleThr_deg,
                    icpLoopCloseDistThr,
                    gui.verbose, T_ab, Sigma_ab, errPerLvl, countPerLvl);
              } else {
                tdp::ICP::ComputeProjective<CameraT>(pcs_m, ns_m, pcs_c, ns_c,
                    rig, rig.dStream2cam_, maxIt, icpLoopCloseAngleThr_deg,
                    icpLoopCloseDistThr,
                    gui.verbose, T_ab, Sigma_ab, errPerLvl, countPerLvl);
              }
            }
            count = countPerLvl[0];
            err = errPerLvl[0];
          }
          TOCK("LoopClosure");

//          photoErrAfter.Fill(0.);
          cudaMemset(cuPhotoErrAfter.ptr_, 0, cuPhotoErrAfter.SizeBytes());
          float overlapAfter, rmseAfter;
          Overlap(kfA, kfB, rig, icpLoopCloseOverlapLvl,
              overlapAfter, rmseAfter, &T_ab, &cuPhotoErrAfter);
          photoErrAfter.CopyFrom(cuPhotoErrAfter);

          std::cout << "Overlap " << overlapBefore << " -> " << overlapAfter
            << " RMSE " << rmseBefore << " -> " << rmseAfter
            << " dRMSE " << (rmseBefore-rmseAfter)/rmseBefore
            << std::endl;

          if (idA == idB-1 ||
              (err == err
              && err < icpLoopCloseErrThr
              && count > 3000
              && (overlapAfter > icpLoopCloseOverlapThr)
              && (useRansac || (rmseBefore-rmseAfter)/rmseBefore > rmseChangeThr)
              && rmseAfter < rmseThr)) {
            std::cout << GREEN << "successfull loop closure "
              << NORMAL << std::endl;
//              << T_ab.matrix3x4()
//              << "Sigma_ab" << std::endl << Sigma_ab << std::endl;
            kfSLAM.AddLoopClosure(idA, idB, T_ab, Sigma_ab);
//            kfSLAM.AddLoopClosure(ids.second, ids.first, T_ab.Inverse());
//            loopClosures.emplace(std::make_pair(ids.first, ids.second), T_ab);

            // update views
            viewDebugA.SetImage(photoErrBefore);
            viewDebugB.SetImage(photoErrAfter);
            viewDebugC.SetImage(kfA.pyrGrey_.GetImage(0));
            viewDebugD.SetImage(kfB.pyrGrey_.GetImage(0));
            vboSuccessA.Upload(kfA.pc_.ptr_,kfA.pc_.SizeBytes(), 0);
            vboSuccessB.Upload(kfB.pc_.ptr_,kfB.pc_.SizeBytes(), 0);

            T_abSuccess = T_ab;

            std::cout << "optimizing graph" << std::endl;
            kfSLAM.Optimize();
            if (useOptimizedPoses) {
              T_ac = kfSLAM.GetPose(idActive).Inverse()*kfs[idActive].T_wk_*T_ac;
              for (size_t i=0; i < kfs.size(); ++i) {
                kfs[i].T_wk_ = kfSLAM.GetPose(i);
              }
            }

            if (computePhotometricError) {
              for (auto& it : kfSLAM.loopClosures_) {
                tdp::KeyFrame& kfA = kfs[it.first];
                tdp::KeyFrame& kfB = kfs[it.second];
                float overlap, rmse;
                TICK("Overlap");
                //        cudaMemset(cuPhotoErrAfter.ptr_, 0, cuPhotoErrAfter.SizeBytes());
                Overlap(kfA, kfB, rig, icpLoopCloseOverlapLvl, overlap,
                    rmse, nullptr, nullptr); //&cuPhotoErrAfter);
                //        photoErrBefore.CopyFrom(cuPhotoErrAfter);
                TOCK("Overlap");
                std::cout << it.first << " to " << it.second << ":\tRMSE " << rmse
                  << "\toverlap " << overlap << std::endl;
                rmses[it] = rmse;
              }

              auto mapComp = [](const std::pair<std::pair<int,int>,float>& a,
                  const std::pair<std::pair<int,int>,float>& b) -> bool {
                return a.second < b.second; };
              std::pair<int,int> idMax =
                std::max_element(rmses.begin(), rmses.end(),
                    mapComp)->first;

              viewDebugE.SetImage(kfs[idMax.first].pyrGrey_.GetImage(0));
              viewDebugF.SetImage(kfs[idMax.second].pyrGrey_.GetImage(0));
            }
            return true;
          } else {
            std::cout << "unsuccessfull loop closure: "
              << "error=" << err << ", "
              << "count=" << count << ", "
              << "overlapAfter=" << overlapAfter << ", "
              << "rmseChange=" << (rmseBefore-rmseAfter)/rmseBefore << ", "
              << "rmseAfter=" << rmseAfter << std::endl;
          }
        } else {
          std::cout << "aborting loop closure because overlap " << overlapBefore
            << " is to small" << std::endl;
        }
      } else {
        std::cout << " skipping " << idA << " -> " << idB
          << ": " << se3.head<3>().norm()*180./M_PI << " "
          << se3.tail<3>().norm()          << std::endl;
      }
      return false;
  };

  auto computeLoopClosures = [&]() {
    size_t I = loopClose.size()/10 +1;
    for(size_t i=0; i < I; ++i) {
      std::pair<int,int> ids(-1,-1);
      if (loopClose.size() > 0) {
        ids = loopClose.front();
        loopClose.pop_front();
      }
      if (ids.first < 0 && ids.second < 0) {
        break;
      }
      if (loopCloseKfs(ids.first, ids.second)) {
        break;
      }
    }
  };

  std::thread loopClosureThread([&]() {
      while (runLoopClosure.Get()) {
//        computeLoopClosures();
        std::this_thread::sleep_for(std::chrono::seconds(1));
      }
      });

  if (gui.verbose) std::cout << "starting main loop" << std::endl;

  tdp::GeodesicHist<4> normalHist;
  tdp::vMFMMF<1> mmf(30.);
  size_t Nmmf = 1000000;
  tdp::ManagedHostImage<tdp::Vector3fda> nMmf(Nmmf,1);
  tdp::ManagedDeviceImage<tdp::Vector3fda> cuNMmf(Nmmf,1);
  pangolin::GlBuffer vboNMmf(pangolin::GlArrayBuffer,Nmmf,GL_FLOAT,3);

  kfs.reserve(1000);
  binaryKfs.reserve(1000);

  // Stream and display video
  while(!pangolin::ShouldQuit())
  {
    grid0 = tdp::Vector3fda(grid0x,grid0y,grid0z);
    gridE = tdp::Vector3fda(gridEx,gridEy,gridEz);
    dGrid = gridE - grid0;
    dGrid(0) /= (wTSDF-1);
    dGrid(1) /= (hTSDF-1);
    dGrid(2) /= (dTSDF-1);

    numLoopClose = loopClose.size();

    if (pangolin::Pushed(printGraph)) {
      kfSLAM.PrintValues();
      kfSLAM.PrintGraph();
    }

    if ((runSlamFusion.GuiChanged() && runSlamFusion)
       || (gui.finished() && !runSlamFusion && loopClose.size() == 0)) {
      T_mos.clear();
      T_mo = kfs[0].T_wk_;
      idActive = 0;
      gui.Seek(0);
      gui.finished_ = false;
      resetTSDF = true;
      runSlamFusion = true;
      recomputeBoundingBox = true;
      trackClosestKf = true;
    }

    if (pangolin::Pushed(recomputeBoundingBox)) {
      for (size_t i=0; i<Nmmf; ++i) {
        tdp::Vector3fda ni ;
        do {
          Eigen::Vector2f ids =
            0.5*(Eigen::Vector2f::Random()+Eigen::Vector2f::Ones());
          int32_t idKf = floor(ids(0)*kfs.size());
          int32_t idPt = floor(ids(1)*kfs[idKf].pyrN_.GetImage(2).Area());
          ni = kfs[idKf].T_wk_.rotation()*kfs[idKf].pyrN_.GetImage(2)[idPt];
        } while (!tdp::IsValidData(ni));
        nMmf[i] = ni;
      }
      cuNMmf.CopyFrom(nMmf);
      mmf.Compute(cuNMmf, 100, true);
      size_t idMax = std::distance(mmf.Ns_.begin(),
          std::max_element(mmf.Ns_.begin(), mmf.Ns_.end()));
      std::cout << "largest MF: " << mmf.Ns_[idMax] << std::endl;
      std::cout << mmf.Rs_[idMax] << std::endl;
      for (size_t k=0; k<3; ++k) {
        std::cout << mmf.Rs_[k] << std::endl;
      }
      mmfId = idMax;
      T_wG.rotation() = mmf.Rs_[mmfId];
//      T_wG.translation() = T_mos[0].translation();

      grid0.fill(1e9);
      gridE.fill(-1e9);
      for (size_t i=0; i<Nmmf; ++i) {
        tdp::Vector3fda pi;
        do {
          Eigen::Vector2f ids =
            0.5*(Eigen::Vector2f::Random()+Eigen::Vector2f::Ones());
          int32_t idKf = floor(ids(0)*kfs.size());
          int32_t idPt = floor(ids(1)*kfs[idKf].pyrPc_.GetImage(2).Area());
          pi = T_wG.Inverse()*kfs[idKf].T_wk_*kfs[idKf].pyrPc_.GetImage(2)[idPt];
        } while (!tdp::IsValidData(pi));
        grid0 = grid0.array().min(pi.array());
        gridE = gridE.array().max(pi.array());
      }
      grid0x = grid0(0);
      grid0y = grid0(1);
      grid0z = grid0(2);
      gridEx = gridE(0);
      gridEy = gridE(1);
      gridEz = gridE(2);
      dGrid = gridE - grid0;
      dGrid(0) /= (wTSDF-1);
      dGrid(1) /= (hTSDF-1);
      dGrid(2) /= (dTSDF-1);
      vboNMmf.Upload(nMmf.ptr_,nMmf.SizeBytes(), 0);

      normalHist.Reset();
      normalHist.ComputeGpu(cuNMmf);
    }

    if (pangolin::Pushed(resetTSDF)) {
      TSDF.Fill(tdp::TSDFval(-1.01,0.));
      cuTSDF.CopyFrom(TSDF);
      std::cout << "resetting TSDF" << std::endl;
    }

    if (kfs.size() > 1 && pangolin::Pushed(retryAllLoopClosures)) {
      for (int i=0; i<kfs.size()-1; ++i)
        for (int j=i+1; j<kfs.size()-1; ++j)
          loopClose.emplace_back(i,j);
    }

    if (loopCloseA < kfs.size() && loopCloseB < kfs.size()
        && pangolin::Pushed(retryLoopClosure)) {
        loopClose.emplace_front(loopCloseA,loopCloseB);
    }

    if (pangolin::Pushed(runKfOnlyFusion)) {
      for (size_t i=0; i<kfs.size(); ++i) {
        if (true || gui.verbose)
          std::cout << "add KF " << i << " to tsdf" << std::endl;
        const auto& kfA = kfs[i];
        const tdp::SE3f& T_mk = kfA.T_wk_;
        cuD.CopyFrom(kfA.d_);
        TICK("Add To TSDF");
//        AddToTSDF(cuTSDF, cuD, T_mk, camD, grid0, dGrid, tsdfMu, tsdfWMax);
        rig.AddToTSDF(cuD, T_wG.Inverse()*T_mk, useRgbCamParasForDepth,
            grid0, dGrid, tsdfMu, tsdfWMax, cuTSDF);
        TOCK("Add To TSDF");
      }
    }

    glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
    glColor3f(1.0f, 1.0f, 1.0f);

    gui.NextFrames();

    if (gui.frame < 1) continue;

    int64_t t_host_us_d = 0;
    if (gui.verbose) std::cout << "setup pyramids" << std::endl;
    TICK("Setup Pyramids");
    if (gui.verbose) std::cout << "collect d" << std::endl;
    rig.CollectD(gui, dMin, dMax, cuDraw, cuD, t_host_us_d);
    if (gui.verbose) std::cout << "compute pc" << std::endl;
    rig.ComputePc(cuD, true, pcs_c);
    if (gui.verbose) std::cout << "compute n" << std::endl;
    rig.ComputeNormals(cuD, true, ns_c);
    if (gui.verbose) std::cout << "collect rgb" << std::endl;
    rig.CollectRGB(gui, rgb) ;
    cuRgb.CopyFrom(rgb);
    tdp::Rgb2Grey(cuRgb,cuGrey, 1./255.);

    tdp::Image<tdp::Vector2fda> cuGradGrey_c = cuPyrGradGrey_c.GetImage(0);
    tdp::Gradient(cuGrey, cuGreyDu, cuGreyDv, cuGradGrey_c);
    greyDu.CopyFrom(cuGreyDu);
    greyDv.CopyFrom(cuGreyDv);
    tdp::ConstructPyramidFromImage(cuGrey, cuPyrGrey_c,
        cudaMemcpyDeviceToDevice);
    tdp::CompletePyramid(cuPyrGradGrey_c);

    TOCK("Setup Pyramids");

    if(kfs.size() > 0 && !gui.finished()) {
      if (trackClosestKf) {
        // Find closest KF
        int iMin = 0;
        //      float distMin = 1e9;
        //      float angMin = 1e9;
        float valMin = 1e9;
        for (int i=0; i<kfs.size(); ++i) {
          Eigen::Matrix<float,6,1> se3 = kfs[i].T_wk_.Log(T_mo);
          float dist = se3.tail<3>().norm();
          float ang = se3.head<3>().norm();
          //        if (ang < angMin && dist < distMin) {
          //          distMin = dist;
          //          angMin = ang;
          if (2*ang+dist < valMin) {
            valMin = 2*ang+dist;
            iMin = i;
          }
        }
        if (iMin != idActive) {
          std::cout << "switching to tracking against KF " << iMin << std::endl;
          T_ac = kfs[iMin].T_wk_.Inverse()*kfs[idActive].T_wk_*T_ac;
          std::cout << T_ac << std::endl;
          idActive = iMin;
          viewKf.SetImage(kfs[idActive].rgb_);
        }
      }

      tdp::KeyFrame& kf = kfs[idActive];
      pcs_m.CopyFrom(kf.pyrPc_);
      ns_m.CopyFrom(kf.pyrN_);
      // TODO:
      //      gs_m.CopyFrom(gs_c);

      if (gui.verbose) std::cout << "icp" << std::endl;
      TICK("ICP");
      std::vector<size_t> maxIt{icpIter0,icpIter1,icpIter2};
      std::vector<float> errPerLvl;
      std::vector<float> countPerLvl;
      Eigen::Matrix<float,6,6> dSigma_ac = 1e-6*Eigen::Matrix<float,6,6>::Identity();
//      T_ac = tdp::SE3f();
      if (icpRgb) {
        cuPyrGrey_m.CopyFrom(kf.pyrGrey_);
        cuPyrGradGrey_m.CopyFrom(kf.pyrGradGrey_);
        tdp::IcpTexture::ComputeProjective<CameraT>(pcs_m, ns_m,
            cuPyrGradGrey_m, cuPyrGrey_m, pcs_c, ns_c, cuPyrGradGrey_c,
            cuPyrGrey_c, rig, rig.rgbStream2cam_, maxIt, icpAngleThr_deg,
            icpDistThr, icpRgbLambda, gui.verbose, T_ac, dSigma_ac,
            errPerLvl, countPerLvl);
      } else {
//        tdp::ICP::ComputeProjective(pcs_m, ns_m, pcs_c, ns_c, T_ac, tdp::SE3f(),
//            camD, maxIt, icpAngleThr_deg, icpDistThr, gui.verbose);
        if (useRgbCamParasForDepth) {
          tdp::ICP::ComputeProjective<CameraT>(pcs_m, ns_m, pcs_c, ns_c,
              rig, rig.rgbStream2cam_, maxIt, icpAngleThr_deg, icpDistThr,
              gui.verbose, T_ac, dSigma_ac, errPerLvl, countPerLvl);
        } else {
          tdp::ICP::ComputeProjective<CameraT>(pcs_m, ns_m, pcs_c, ns_c,
              rig, rig.dStream2cam_, maxIt, icpAngleThr_deg,
              icpDistThr,
              gui.verbose, T_ac, dSigma_ac, errPerLvl, countPerLvl);
        }
//      } else {
//        tdp::ICP::ComputeProjective(pcs_m, ns_m, gs_m, pcs_c, ns_c,
//            gs_c, T_ac, tdp::SE3f(), camD, maxIt, icpAngleThr_deg,
//            icpDistThr, gui.verbose);
      }
      TOCK("ICP");
      T_mo = kfs[idActive].T_wk_*T_ac;
      T_mos.push_back(T_mo);
//      Sigma_ac += dSigma_ac;
      Sigma_ac = dSigma_ac;

    }

    if (!runSlamFusion) {
      Eigen::Matrix<float,6,1> se3 = Eigen::Matrix<float,6,1>::Zero();
      float dH = 0.;
      if (kfs.size() > 0) {
        se3 = kfs[idActive].T_wk_.Log(T_mo);
        float logH = ((Sigma_ac.eigenvalues()).array().log().sum()).real();
        // capture the entropy of the transformation right after new KF
        if (kfs.size() > numKfsPrev) {
          logHs.push_back(logH);
          numKfsPrev = kfs.size();
        }
//        std::cout <<  "idactive: " << idActive
//          << " " << logHs.size()
//          << " : " << numKfsPrev
//          << " " << kfs.size()
//          << std::endl;
        dH = logH / logHs[idActive];
        logdH.Log(dH, dEntropyThr);
        logEntropy.Log(logH, logHs[idActive]);
      }
      if ( (kfs.size() == 0)
          || se3.head<3>().norm()*180./M_PI > keyFrameAngleThresh
          || se3.tail<3>().norm() > keyFrameDistThresh
          || dH < dEntropyThr) {
        std::cout << "adding keyframe " << kfs.size()
          << " angle: " << se3.head<3>().norm()*180./M_PI
          << " dist: " << se3.tail<3>().norm()
          << " dH: " << dH
          << " T_mk: " << std::endl << T_mo
          << std::endl;

//        tdp::ConstructPyramidFromImage(cuGrey, pyrGrey);
        numKfsPrev = kfs.size();
        kfs.emplace_back(pcs_c, ns_c, cuPyrGrey_c, cuPyrGradGrey_c,
            rgb, cuD, T_mo);

        tdp::Convert(cuPyrGrey_c, cuPyrGreyB_c, 255., 0.);
        binaryKfs.emplace_back(cuPyrGreyB_c,pcs_c);
        binaryKfs.back().Extract(kfs.size()-1, fastLvl, fastB,
            kappaHarris, harrisThr);

        for (int i=kfs.size()-3;
            i > std::max(-1,(int)kfs.size()-maxLoopClosures-1); --i) {
          loopClose.emplace_front(kfs.size()-1,i);
        }

        T_mo = kfs[kfs.size()-1].T_wk_;

        if (kfs.size() == 1) {
          std::cout << "first KF -> adding origin" << std::endl;
          kfSLAM.AddOrigin(T_mo);
        } else {
          std::cout << "not first KF -> adding new pose" << std::endl;
          std::cout << Sigma_ac << std::endl;
//          Sigma_ac = 1e-3*Eigen::Matrix<float,6,6>::Identity();
          kfSLAM.AddPose(T_mo);
          kfSLAM.AddLoopClosure(idActive, kfs.size()-1, T_ac, Sigma_ac);
//          std::cout << "not first KF -> adding ICP odom "
//            << kfs.size()-1 << " to " << idActive
//            << std::endl;
//          kfSLAM.AddIcpOdometry(idActive, kfs.size()-1, T_ac);
//          if (!loopCloseKfs(idActive, kfs.size()-1)) {
//            std::cout << "warning not closing consecutive pose Loop!" << std::endl;
//          }
        }

        idActive = kfs.size()-1;

        // sort to loop close closest frames (temporally) first
        loopClose.sort(
            [&](const std::pair<int,int>& a, const std::pair<int,int>& b) {
              return std::min(idActive-a.first, idActive-a.second)
                   < std::min(idActive-b.first, idActive-b.second);
            });
        std::cout << loopClose.front().first << ", " << loopClose.front().second
          << ";  "<< loopClose.back().first << ", " << loopClose.back().second
          << std::endl;

        viewKf.SetImage(kfs[idActive].rgb_);

        if (saveKfs) {
          float overlap = 0.;
          if (idActive > 0) {
            const auto& kfA = kfs[idActive-1];
            const auto& kfB = kfs[idActive];
            float rmse = 0.;
            tdp::Overlap(kfA, kfB, rig, 0, overlap, rmse, &T_ac);
          }

          tdp::ManagedHostImage<tdp::Vector3fda> pc(wc, hc);
          pc.CopyFrom(pcs_c.GetImage(0));
          tdp::ManagedHostImage<tdp::Vector3fda> n(wc, hc);
          n.CopyFrom(ns_c.GetImage(0));
          std::stringstream plyPath;
          plyPath << "./frame_" << std::setfill('0') << std::setw(10)
            << idActive << ".ply";
          tdp::SavePointCloud(plyPath.str(), pc, n, rgb, false);
          std::stringstream cfgPath;
          cfgPath << "./config_" << std::setfill('0') << std::setw(10)
            << idActive << ".txt";
          std::ofstream fout(cfgPath.str());
          fout << "q_abx q_aby q_abz q_abw t_abx t_aby t_abz overlap fillA fillB" << std::endl;
          fout << T_ac.rotation().vector()(0) << " "
            << T_ac.rotation().vector()(1) << " "
            << T_ac.rotation().vector()(2) << " "
            << T_ac.rotation().vector()(3) << " "
            << T_ac.translation()(0) << " "
            << T_ac.translation()(1) << " "
            << T_ac.translation()(2) << " "
            << overlap << " " << 1. << " " << 1. << std::endl;
          fout.close();
        }
        T_ac = tdp::SE3f();
        Sigma_ac.fill(0.);
      }
      if (tryLoopClose) {
        computeLoopClosures();
      }
    } else {
      if (!gui.finished()) {
        TICK("Add To TSDF");
        rig.AddToTSDF(cuD, T_wG.Inverse()*T_mo, useRgbCamParasForDepth,
            grid0, dGrid, tsdfMu, tsdfWMax, cuTSDF);
        TOCK("Add To TSDF");
      }
    }

    if (pangolin::Pushed(runMarchingCubes)
        || (runSlamFusion && gui.finished() && meshVbo.num_elements == 0)) {
      TSDF.CopyFrom(cuTSDF);
      tdp::ComputeMesh(TSDF, grid0, dGrid,
          T_wG, meshVbo, meshCbo, meshIbo, marchCubeswThr, marchCubesfThr);
      saveTSDF = true;
      if (runOnce) break;
    }

    if (gui.verbose) std::cout << "draw 3D" << std::endl;

    TICK("Draw 3D");

    glEnable(GL_DEPTH_TEST);
    if (viewPc3D.IsShown()) {
      viewPc3D.Activate(s_cam);

      T_wG.rotation() = tdp::SO3f(mmf.Rs_[mmfId]);
      pangolin::glSetFrameOfReference(T_wG.matrix());
      Eigen::AlignedBox3f box(grid0,gridE);
      glColor4f(1,0,0,0.5f);
      pangolin::glDrawAlignedBox(box);
      pangolin::glUnsetFrameOfReference();

      pangolin::glDrawAxis(kfs[idActive].T_wk_.matrix(),0.08f);
      pangolin::glDrawAxis(T_mo.matrix(), 0.05f);
      glColor4f(1.,1.,0.,0.6);
      glDrawPoses(T_mos,-1);
      for (size_t i=0; i<kfs.size(); ++i) {
        tdp::SE3f& T_wk = kfs[i].T_wk_;
        pangolin::glDrawAxis(T_wk.matrix(), 0.03f);
      }
      for (size_t i=0; i<kfSLAM.size(); ++i) {
        tdp::SE3f T_wk = kfSLAM.GetPose(i);
        pangolin::glDrawAxis(T_wk.matrix(), 0.03f);
      }

      if (loopCloseA < kfSLAM.size()) {
        tdp::SE3f T_wk = kfSLAM.GetPose(loopCloseA);
        pangolin::glDrawFrustrum(rig.cams_[0].GetKinv(), 640,480,
            T_wk.matrix(), 0.03f);
      }
      if (loopCloseB < kfSLAM.size()) {
        tdp::SE3f T_wk = kfSLAM.GetPose(loopCloseB);
        pangolin::glDrawFrustrum(rig.cams_[0].GetKinv(), 640,480,
            T_wk.matrix(), 0.03f);
      }

      if (!useOptimizedPoses) {
        glColor4f(1.,0.3,0.3,0.6);
        for (auto& it : kfSLAM.loopClosures_) {
          tdp::SE3f& T_wk_A = kfs[it.first].T_wk_;
          tdp::SE3f& T_wk_B = kfs[it.second].T_wk_;
          tdp::glDrawLine(T_wk_A.translation(), T_wk_B.translation());
        }
      }

      auto mapComp = [](const std::pair<std::pair<int,int>,float>& a,
          const std::pair<std::pair<int,int>,float>& b) -> bool {
            return a.second < b.second; };
      float rmseMin = std::min_element(rmses.begin(), rmses.end(),
          mapComp)->second;
      float rmseMax = std::max_element(rmses.begin(), rmses.end(),
          mapComp)->second;
      for (auto& it : kfSLAM.loopClosures_) {
        tdp::SE3f T_wk_A = kfSLAM.GetPose(it.first);
        tdp::SE3f T_wk_B = kfSLAM.GetPose(it.second);
        if (rmses.find(it) != rmses.end()) {
          tdp::Vector3bda c = tdp::ColorMapHot(
              (rmses[it]-rmseMin)/(rmseMax-rmseMin));
          glColor4f(c(0)/255.,c(1)/255.,c(2)/255.,1.);
        } else {
          glColor4f(0.,0.0,1.0,1.0);
        }
        tdp::glDrawLine(T_wk_A.translation(), T_wk_B.translation());
      }
      // render model and observed point cloud
      if (showPcModel && kfs.size() > 0) {
        tdp::KeyFrame& kf = kfs[idActive];
        pcs_m.CopyFrom(kf.pyrPc_);
        ns_m.CopyFrom(kf.pyrN_);

        pangolin::glSetFrameOfReference(kfs[idActive].T_wk_.matrix());
        {
          pangolin::CudaScopedMappedPtr cuPcbufp(cuPcbuf);
          cudaMemset(*cuPcbufp,0,hc*wc*sizeof(tdp::Vector3fda));
          tdp::Image<tdp::Vector3fda> pc0 = pcs_m.GetImage(dispLvl);
          cudaMemcpy(*cuPcbufp, pc0.ptr_, pc0.SizeBytes(),
              cudaMemcpyDeviceToDevice);
        }
        glColor3f(0,1,0);
        pangolin::RenderVbo(cuPcbuf);
        pangolin::glUnsetFrameOfReference();
      }
      // render current camera second in the propper frame of
      // reference
      if (showPcCurrent) {
        pangolin::glSetFrameOfReference(T_mo.matrix());
        {
          pangolin::CudaScopedMappedPtr cuPcbufp(cuPcbuf);
          cudaMemset(*cuPcbufp,0,hc*wc*sizeof(tdp::Vector3fda));
          tdp::Image<tdp::Vector3fda> pc0 = pcs_c.GetImage(dispLvl);
          cudaMemcpy(*cuPcbufp, pc0.ptr_, pc0.SizeBytes(),
              cudaMemcpyDeviceToDevice);
        }
        glColor3f(1,0,0);
        pangolin::RenderVbo(cuPcbuf);
        pangolin::glUnsetFrameOfReference();
      }

      if (meshVbo.num_elements > 0
          && meshCbo.num_elements > 0
          && meshIbo.num_elements > 0) {
        meshVbo.Bind();
        glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, 0);
        meshCbo.Bind();
        glVertexAttribPointer(1, 3, GL_UNSIGNED_BYTE, GL_TRUE, 0, 0);
        glEnableVertexAttribArray(0);
        glEnableVertexAttribArray(1);

        auto& shader = tdp::Shaders::Instance()->normalMeshShader_;
        shader.Bind();
        pangolin::OpenGlMatrix P = s_cam.GetProjectionMatrix();
        pangolin::OpenGlMatrix MV = s_cam.GetModelViewMatrix();
        shader.SetUniform("P",P);
        shader.SetUniform("MV",MV);

        meshIbo.Bind();
        glDrawElements(GL_TRIANGLES, meshIbo.num_elements*3,
            meshIbo.datatype, 0);
        meshIbo.Unbind();

        shader.Unbind();
        glDisableVertexAttribArray(1);
        glDisableVertexAttribArray(0);
        meshCbo.Unbind();
        meshVbo.Unbind();
      }

    }

    if (viewNormals.IsShown()) {
      viewNormals.Activate(camNormals);

      pangolin::glDrawAxis(0.1f);
      glColor4f(1.f,0.f,0.f,0.5f);
      pangolin::RenderVbo(vboNMmf);

      for (size_t i=0; i<mmf.Rs_.size(); ++i) {
        tdp::SE3f T_wmmf(tdp::SO3f(mmf.Rs_[i]));
        pangolin::glDrawAxis(T_wmmf.matrix(),1.f);
      }
      normalHist.geoGrid_.Render3D();
      normalHist.Render3D(histScale, false);
    }

    if (viewLoopClose.IsShown() && kfs.size() > 1) {
      viewLoopClose.Activate(camLoopClose);

      pangolin::glDrawAxis(0.1f);
      glColor4f(1.f,0.f,0.f,0.5f);
      pangolin::RenderVbo(vboSuccessA);

      pangolin::glSetFrameOfReference(T_abSuccess.matrix());
      pangolin::glDrawAxis(0.1f);
      glColor4f(0.f,1.f,0.f,0.5f);
      pangolin::RenderVbo(vboSuccessB);
      pangolin::glUnsetFrameOfReference();

    }

    TOCK("Draw 3D");

    TICK("Draw 2D");
    glLineWidth(1.5f);
    glDisable(GL_DEPTH_TEST);

    if (viewKf.IsShown()) {
      viewKf.RenderImage();
    }
    if (viewCurrent.IsShown()) {
      viewCurrent.SetImage(rgb);
    }
    if (viewDebugA.IsShown()) {
      viewDebugA.RenderImage();
    }
    if (viewDebugB.IsShown()) {
      viewDebugB.RenderImage();
    }
    if (viewDebugC.IsShown()) {
      viewDebugC.RenderImage();
    }
    if (viewDebugD.IsShown()) {
      viewDebugD.RenderImage();
    }
    if (viewDebugE.IsShown()) {
      viewDebugE.RenderImage();
    }
    if (viewDebugF.IsShown()) {
      viewDebugF.RenderImage();
    }

    if (viewGrad3DPyr.IsShown()) {
      tdp::PyramidToImage<tdp::Vector3fda,3>(gs_c,cuDispNormalsPyr);
      tdp::RenormalizeSurfaceNormals(cuDispNormalsPyr, 1e-3);
      tdp::Normals2Image(cuDispNormalsPyr, cuDispNormals2dPyr);
      dispNormals2dPyr.CopyFrom(cuDispNormals2dPyr);
      viewGrad3DPyr.SetImage(dispNormals2dPyr);
    }

    plotdH.ScrollView(1,0);
    plotH.ScrollView(1,0);

    TOCK("Draw 2D");

    // leave in pixel orthographic for slider to render.
    pangolin::DisplayBase().ActivatePixelOrthographic();
    if(video.IsRecording()) {
      pangolin::glRecordGraphic(pangolin::DisplayBase().v.w-14.0f,
          pangolin::DisplayBase().v.h-14.0f, 7.0f);
    }
    Stopwatch::getInstance().sendAll();
    pangolin::FinishFrame();
  }

  saveTSDF = true;

  imuInterp.Stop();
  if (imu) imu->Stop();
  delete imu;

  std::this_thread::sleep_for(std::chrono::microseconds(500));

  runSave.Set(false);
  workThread.join();
  runLoopClosure.Set(false);
  loopClosureThread.join();
  return 0;
}


/*
 * This file is part of the M. xanthus polarity simulator
 * Copyright (c) 2020 Filipe Tostevin
 *
 * This file contains the main routine that creates all simulator
 * objects and calls the propagation routines.
 */

#include <iostream>
#include <cstdlib>
#include <boost/property_tree/ini_parser.hpp>
#include <boost/program_options.hpp>
#include "state.h"
#include "propagator.h"

using namespace std;
namespace po = boost::program_options;
namespace pt = boost::property_tree;

int main(
	int argc,
	char* argv[]
){
	// Command line options and help
	po::options_description desc("Allowed options");
	desc.add_options()
		("help,h", "print help message")
		("conf,c", po::value<string>(), "parameter configuration file")
		("mutants,m", "simulate deletion mutants")
	;
	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if(vm.count("help")) {
		cout << desc << "\n";
		return(EXIT_SUCCESS);
	}

	// Read the input file given by the "--conf" option, or otherwise from
	// "params.in"
	pt::ptree ptree;
	string defaultconfig="params.in";
	if(vm.count("conf")) {
		cout << "Reading parameters from: " << vm["conf"].as<string>() << endl;
		pt::read_ini(vm["conf"].as<string>(), ptree);
	} else {
		cout << "No parameter file specified. Reading parameters from default file: " << defaultconfig << endl;
		pt::read_ini(defaultconfig, ptree);
	}

	// Get total simulation time from input file
	const double t_total=ptree.get<double>("t_total");
	const double sampling_interval=ptree.get<double>("sampling_interval");

	// Define the system state variable and initialize all elements to zero
	state conc(NSPEC,NPOOL,0);

	// Set up simulation infrastructure
	propagator P;
	// Read in parameter values from input file
	for(auto p: ptree.get_child("model_parameters")) {
		P.params.emplace(p.first, ptree.get<double>("model_parameters."+p.first));
	}
	//Run a test call of the propagator to check that all required parameters
	//exist
	try{ P(conc, conc, 0); }
	catch(...) {
		cout << "Error in model definition, check that all parameters are specified in the input file." << endl;
		exit(EXIT_FAILURE);
	}

	// Deletion mutants are defined by an integer that when represented as a
	// binary string defines the presence/absence of each protein,
	// i.e. if the number of proteins in NSPEC=3, then
	// mutant_number=1==b100 indicates only the first-listed protein is present,
	// mutant_number=7==b111 indicates that all proteins are present
	// Here we define the first simulation to run depending on whether we are
	// simulating all mutants, or just the wild-type
	int mutant_number;
	if(vm.count("mutants")) {
		cout << "Simulating deletion mutants" << endl;
		mutant_number=1;
	} else {
		cout << "Simulating only wild-type" << endl;
		mutant_number=(1<<NSPEC) -1;
	}

	// Loop over all mutants until we reach wild-type
	while(mutant_number < 1<<NSPEC) {
		cout << mutant_number << endl;
		//Each mutant will be output to a separate data file.
		//Decode mutant number bitwise into whether protein is present or not and
		//reconstruct readable output string from it, to use in the name of the
		//output file
		string mutant_string="";
		for(int s=0; s<NSPEC; ++s) {
			if( !( (mutant_number >> s) & 1) ) {
				mutant_string+=".d" + species_names[s];
			}
		}
		ofstream datafile("data.txt"+mutant_string);

		//Headings for output file
		datafile << "t ";
		for(int s=0; s<NSPEC; ++s) {
			datafile << species_names[s] << "1 "
			         << species_names[s] << "2 "
			         << species_names[s] << "t ";
		} datafile << endl;

		for(int s=0; s<NSPEC; ++s) {
			// Initialize total amounts of each protein to 1 or 0 according to the
			// deletion condition
			if( (mutant_number >> s) & 1 ) {
				conc(s,TOTAL)=1;
			} else {
				conc(s,TOTAL)=0;
			}
			// Initialize polar fractions
			conc(s,POLE1)=ptree.get<double>("initial_fractions_"+species_names[s]+".pole1")*conc(s,TOTAL);
			conc(s,POLE2)=ptree.get<double>("initial_fractions_"+species_names[s]+".pole2")*conc(s,TOTAL);
		}

		// Start at t=0
		double t=0;
		write_state(conc, t, datafile);

		// Run until the end of the simulation time t_total
		while(t<t_total) {
			// Call the ODE integrator to run until the next output time point
			boost::numeric::odeint::integrate(boost::ref(P), conc, t, t+sampling_interval, 0.2*sampling_interval);
			t+=sampling_interval;

			write_state(conc, t, datafile);
		}

		datafile.close();
		++mutant_number;
	}

	return EXIT_SUCCESS;
}


#include<iostream>
#include<fstream>
#include<vector>
#include<string>
#include<sstream>
#include<cstring>
#include<algorithm>
#include <numeric>
#include<map>
#include<stdio.h>
#include<getopt.h>
#include<cmath>
#include<set>
#include <bitset>
#include<unordered_set>
#include <boost/dynamic_bitset.hpp>
#include <boost/algorithm/string.hpp>
#include <omp.h>
#include <thread>

using namespace std;
using namespace boost;

class Dataset {
private:
const char* delimiter = " \t";
vector<string> pop_name, nonpop_name, famid, subid, chrom, snp, allele1, allele2, MA, ref_id, ref_pop, ref_pop_name;
vector<int> position, ref_pop_cnt, id_pop, pop_pos, nonpop_pos, pop_snp_idx;
vector<bool> snp_pop;
vector<vector<int>> allele1_cnt, miss_cnt;
vector<vector<double>> miss_frq, maf;
int pop_num, nonpop_num, batch_size, indi_num, snp_num, remain_snp_num, ref_indi_num, ref_pop_num, pop_snp_num, out_snp_num, max_thread_num;
double maf_min, maf_max, miss_max;
bool error_flag = false;
string bed_file, fam_file, bim_file, ref_file, out_file;
ifstream bedfile, famfile, bimfile, reffile, outfile;
set<string> subid_set, ref_pop_set, ref_id_set;
unordered_multiset<string> ref_pop_multiset;
map<string, int> id_pop_map;

public:
int flag[25]{};
string arg[25];
ofstream logFile;
int thread_num;

void help()
{
	cout << "Population-Specific SNP Screener (PSNPS)" << endl;
	cout << "Options:" << endl;
	cout << "--bfile\t\t" << "Input genotype file in plink binary format." << endl;
	cout << "--ref-pop\t" << "Input file that describes reference populations. The file is expected to have two columns without headers:" << endl;
	cout << "\t\t" << "the first is individual ID and the second is the population that the individual belongs to." << endl;
	cout << "--pop\t\t" << "Screen SNPs that are specific to the specified population." << endl;
	cout << "\t\t" << "If multiple populations are specified, SNPs polymorphic in all specified populations and monmophic in unspecified populations will be found." << endl;
	cout << "--snp-num\t" << "Number of population-specific SNPs to be saved [default: all SNPs specific to the specified population(s)]." << endl;
	cout << "--freq\t\t" << "Calculate and output allele frequencies in the reference populations." << endl;
	cout << "--out\t\t" << "Output file for saving population-specific SNPs or allele frequencies [default: psnps]." << endl;
	cout << "--maf-min\t" << "Exclude SNPs with MAFs smaller than the specified value in the population(s) specified by --pop [default: 0]." << endl;
	cout << "\t\t" << "Minor alleles are determined by the total samples of the reference populations." << endl;
	cout << "--maf-max\t" << "Exclude SNPs with MAFs larger than the specified value in the population(s) specified by --pop [default: 0.5]." << endl;
	cout << "--miss-max\t" << "Exclude SNPs with missing rates larger than the specified value in the reference populations [default: 0]." << endl;
	cout << "--batch-size\t" << "Number of SNPs to be processed in a batch [default: 10000]." << endl;
	cout << "--thread-num\t" << "Number of threads on which the program will be running [default: thread number in your machine - 1]." << endl;
}

Dataset(int argc, char *argv[])
{
	int opt, longindex;
	const char *optstring = "";
	struct option long_options[] =
	{
		{ "bfile", required_argument,  NULL, 0},
		{ "ref-pop", required_argument,  NULL, 1},
		{ "out", required_argument, NULL, 2},
		{ "pop", required_argument, NULL, 3},
		{ "maf-min", required_argument, NULL, 4},
		{ "maf-max", required_argument, NULL, 5},
		{ "miss-max", required_argument, NULL, 6},
		{ "batch-size", required_argument, NULL, 7},
		{ "freq", no_argument, NULL, 8},
		{ "snp-num", required_argument, NULL, 9},
		{ "thread-num", required_argument, NULL, 10},
		{ "help", no_argument, NULL, 11},
		{0, 0, 0, 0}
	};

	while ((opt = getopt_long(argc, argv, optstring, long_options, &longindex)) != -1)
	{
		switch (opt)
		{
		case 0: flag[0] = 1; arg[0] = optarg; break;
		case 1: flag[1] = 1; arg[1] = optarg; break;
		case 2: flag[2] = 1; arg[2] = optarg; break;
		case 3:
		{
			flag[3] = 1;
			pop_name.push_back(optarg);
			for (int i = optind; i < argc; i++)
			{
			if (argv[i][0] == '-') break;
			else pop_name.push_back(argv[i]);
			}
			pop_num = pop_name.size();
			break;
		}
		case 4: flag[4] = 1; arg[4] = optarg; break;
		case 5: flag[5] = 1; arg[5] = optarg; break;
		case 6: flag[6] = 1; arg[6] = optarg; break;
		case 7: flag[7] = 1; arg[7] = optarg; break;
		case 8: flag[8] = 1; break;
		case 9: flag[9] = 1; arg[9] = optarg; break;
		case 10: flag[10] = 1; arg[10] = optarg; break;
		case 11:  help(); exit(0); break;
		default: break;
		}
	}

	if (flag[2] == 1) out_file = arg[2];
	else out_file = "psnps";
	logFile.open(out_file + ".log", ios::out);
	cout << "Population-Specific SNP Screener (PSNPS)" << endl;
	cout << "Options specified:" << endl;
	logFile << "Population-Specific SNP Screener (PSNPS)" << endl;
	logFile << "Options specified:" << endl;
	for (int i = 0; i < 3; i++)
	{
		if (flag[i] == 1)
		{
			cout << "--" << long_options[i].name << " " << arg[i] << endl;
			logFile << "--" << long_options[i].name << " " << arg[i] << endl;
		}
	}
	if (flag[3] == 1)
	{
		cout << "--" << long_options[3].name;
		logFile << "--" << long_options[3].name;
		for (int i = 0; i < pop_num; i++)
		{
			cout << " " << pop_name[i];
			logFile << " " << pop_name[i];
		}
		cout << endl;
		logFile << endl;
	}
	for (int i = 4; i < 8; i++)
	{
		if (flag[i] == 1)
		{
			cout << "--" << long_options[i].name << " " << arg[i] << endl;
			logFile << "--" << long_options[i].name << " " << arg[i] << endl;
		}
	}
	if (flag[8] == 1)
	{
		cout << "--" << long_options[8].name << endl;
		logFile << "--" << long_options[8].name << endl;
	}
	for (int i = 9; i < 11; i++)
	{
		if (flag[i] == 1)
		{
			cout << "--" << long_options[i].name << " " << arg[i] << endl;
			logFile << "--" << long_options[i].name << " " << arg[i] << endl;
		}
	}

	if (flag[0] == 1)
	{
		bed_file = arg[0] + ".bed";
		bedfile.open(bed_file.c_str(), ios::in | ios::binary);
		if(!bedfile)
		{
			cout << "ERROR: " << bed_file << " was not found!" << endl;
			logFile << "ERROR: " << bed_file << " was not found!" << endl;
			error_flag = true;
		}
		else bedfile.close();
		fam_file = arg[0] + ".fam";
		famfile.open(fam_file.c_str(), ios::in);
		if(!famfile)
		{
			cout << "ERROR: " << fam_file << " was not found!" << endl;
			logFile << "ERROR: " << fam_file << " was not found!" << endl;
			error_flag = true;
		}
		else famfile.close();
		bim_file = arg[0] + ".bim";
		bimfile.open(bim_file.c_str(), ios::in);
		if(!bimfile)
		{
			cout << "ERROR: " << bim_file << " was not found!" << endl;
			logFile << "ERROR: " << bim_file << " was not found!" << endl;
			error_flag = true;
		}
		else bimfile.close();
	}
	else
	{
		cout << "ERROR: " << "use --bfile to specify genotype data." << endl;
		logFile << "ERROR: " << "use --bfile to specify genotype data." << endl;
		error_flag = true;
	}
	if (flag[1] == 1)
	{
		ref_file = arg[1];
		reffile.open(ref_file.c_str(), ios::in);
		if(!reffile)
		{
			cout << "ERROR: " << ref_file << " was not found!" << endl;
			logFile << "ERROR: " << ref_file << " was not found!" << endl;
			error_flag = true;
		}
		else reffile.close();
	}
	else
	{
		cout << "ERROR: " << "use --ref-pop to specify the file that describes reference populations." << endl;
		logFile << "ERROR: " << "use --ref-pop to specify the file that describes reference populations." << endl;
		error_flag = true;
	}
	if (flag[3] == 1)
	{
		for (int i = 0; i < pop_num; i++)
		{
			for (int j = i + 1; j < pop_num; j++)
			{
				if (pop_name[i] == pop_name[j])
				{
					cout << "ERROR: duplicated population " << pop_name[i] << " was specified by --pop." << endl;
					logFile << "ERROR: duplicated population " << pop_name[i] << " was specified by --pop." << endl;
					error_flag = true;
				}
			}
		}
	}
	if (flag[4] == 1)
	{
		maf_min =  atof(arg[4].c_str());
		if (maf_min < 0 || maf_min > 1)
		{
			cout << "ERROR: --maf-min should be between 0 and 1." << endl;
			logFile << "ERROR: --maf-min should be between 0 and 1." << endl;
			error_flag = true;
		}
	}
	else maf_min = 0.0;
	if (flag[5] == 1)
	{
		maf_max =  atof(arg[5].c_str());
		if (maf_max < 0 || maf_max > 1)
		{
			cout << "ERROR: --maf-max should be between 0 and 1." << endl;
			logFile << "ERROR: --maf-max should be between 0 and 1." << endl;
			error_flag = true;
		}
	}
	else maf_max = 0.5;
	if (flag[4] == 1 && flag[5] == 1 && maf_min >= maf_max)
	{
		cout << "ERROR: --maf-min should be smaller than --maf-max." << endl;
		logFile << "ERROR: --maf-min should be smaller than --maf-max." << endl;
		error_flag = true;
	}
	if (flag[6] == 1)
	{
		miss_max =  atof(arg[6].c_str());
		if (miss_max < 0 || miss_max > 1)
		{
			cout << "ERROR: --miss-max should be between 0 and 1." << endl;
			logFile << "ERROR: --miss-max should be between 0 and 1." << endl;
			error_flag = true;
		}
	}
	else miss_max = 0;
	if (flag[7] == 1)
	{
		batch_size =  atoi(arg[7].c_str());
		if (batch_size <= 0)
		{
			cout << "ERROR: --batch-size should be larger than 0." << endl;
			logFile << "ERROR: --batch-size should be larger than 0." << endl;
			error_flag = true;
		}
	}
	else batch_size = 10000;
	if (flag[9] == 1)
	{
		out_snp_num =  atoi(arg[9].c_str());
		if (out_snp_num <= 0)
		{
			cout << "ERROR: --snp-num should be larger than 0." << endl;
			logFile << "ERROR: --snp-num should be larger than 0." << endl;
			error_flag = true;
		}
	}
	max_thread_num = std::thread::hardware_concurrency();
	if (flag[10] == 1)
	{
		thread_num =  atoi(arg[10].c_str());
		if (thread_num < 1 || thread_num > max_thread_num)
		{
			cout << "ERROR: --thread-num should be between 1 and " << max_thread_num << "." << endl;
			logFile << "ERROR: --thread-num should be between 1 and " << max_thread_num << "." << endl;
			error_flag = true;
		}
	}
	else if (max_thread_num > 1) thread_num = max_thread_num - 1;
	else thread_num = 1;
	if (flag[3] == 0 && flag[8] == 0)
	{
		cout << "ERROR: use --pop to screen population-specific SNPs or --freq to calculate allele frequencies in the reference populations." << endl;
		logFile << "ERROR: use --pop to screen population-specific SNPs or --freq to calculate allele frequencies in the reference populations." << endl;
		error_flag = true;
	}

	if (error_flag)
	{
		logFile.close();
		exit(0);
	}
}

~Dataset() {logFile.close();}

void readFAM()
{
	string thisline;
	vector<string> parsedline;
	cout << "Loading FAM file [" + fam_file + "]... " << flush;
	logFile << "Loading FAM file [" + fam_file + "]... " << flush;
	famfile.open(fam_file.c_str(), ios::in);
	while (getline(famfile, thisline, '\n'))
	{
		split(parsedline, thisline, is_any_of(delimiter));
		famid.push_back(parsedline[0]);
		subid.push_back(parsedline[1]);
		subid_set.insert(parsedline[1]);
	}
	famfile.close();
	indi_num = subid.size();
	if (indi_num != subid_set.size())
	{
		cout << "ERROR: duplicated individual ID found in [" + fam_file + "]." << endl;
		logFile << "ERROR: duplicated individual ID found in [" + fam_file + "]." << endl;
		exit(0);
	}
	cout << "done." << endl;
	logFile << "done." << endl;
	cout << indi_num << " individuals loaded from FAM file [" + fam_file + "]." << endl;
	logFile << indi_num << " individuals loaded from FAM file [" + fam_file + "]." << endl;
}

void readBIM()
{
	string thisline;
	vector<string> parsedline;
	cout << "Loading BIM file [" + bim_file + "]... " << flush;
	logFile << "Loading BIM file [" + bim_file + "]... " << flush;
	snp_num = 0;
	bimfile.open(bim_file.c_str(), ios::in);
	while (getline(bimfile, thisline, '\n')) snp_num ++;
	chrom.resize(snp_num);
	snp.resize(snp_num);
	position.resize(snp_num);
	allele1.resize(snp_num);
	allele2.resize(snp_num);
	int i = 0;
	bimfile.clear();
	bimfile.seekg(0, ios::beg);
	while (getline(bimfile, thisline, '\n'))
	{
		split(parsedline, thisline, is_any_of(delimiter));
		chrom[i] = parsedline[0];
		snp[i] = parsedline[1];
		position[i] = atoi((parsedline[3]).c_str());
		allele1[i] = parsedline[4];
		allele2[i] = parsedline[5];
		i ++;
	}
	bimfile.close();
	snp_num = snp.size();
	cout << "done." << endl;
	logFile << "done." << endl;
	cout << snp_num << " SNPs loaded from BIM file [" + bim_file + "]." << endl;
	logFile << snp_num << " SNPs loaded from BIM file [" + bim_file + "]." << endl;
}

void readREF()
{
	string thisline;
	vector<string> parsedline;
	set<string>::iterator tmpiter;
	cout << "Loading reference-population file [" + ref_file + "]... " << flush;
	logFile << "Loading reference-population file [" + ref_file + "]... " << flush;
	reffile.open(ref_file.c_str(), ios::in);
	while (getline(reffile, thisline, '\n'))
	{
		split(parsedline, thisline, is_any_of(delimiter));
		ref_id.push_back(parsedline[0]);
		ref_id_set.insert(parsedline[0]);
		ref_pop.push_back(parsedline[1]);
		ref_pop_set.insert(parsedline[1]);
		ref_pop_multiset.insert(parsedline[1]);
	}
	reffile.close();
	ref_indi_num = ref_id.size();
	if (ref_indi_num != ref_id_set.size())
	{
		cout << "ERROR: duplicated individual ID found in [" + ref_file + "]." << endl;
		logFile << "ERROR: duplicated individual ID found in [" + ref_file + "]." << endl;
		exit(0);
	}
	for (tmpiter = ref_pop_set.begin(); tmpiter != ref_pop_set.end(); tmpiter++)
	{
		ref_pop_name.push_back(*tmpiter);
		ref_pop_cnt.push_back(ref_pop_multiset.count(*tmpiter));
	}
	ref_pop_num = ref_pop_name.size();
	cout << "done." << endl;
	logFile << "done." << endl;
	cout <<  ref_pop_num << " reference populations and " << ref_indi_num << " individuals loaded from the reference population file [" + ref_file + "]." << endl;
	logFile << ref_pop_num << " reference populations and " << ref_indi_num << " individuals loaded from the reference population file [" + ref_file + "]." << endl;
	cout << "Population (Sample Size):" << endl;
	logFile << "Population (Sample Size):" << endl;
	for (int i = 0; i < ref_pop_num; i++)
	{
		cout << ref_pop_name[i] << " (" << ref_pop_cnt[i] << ")" << endl;
		logFile << ref_pop_name[i] << " (" << ref_pop_cnt[i] << ")" << endl;
	}
	for (int i = 0; i < ref_indi_num; i++)
	{
		if (subid_set.find(ref_id[i]) == subid_set.end())
		{
			cout << "ERROR: individual " << ref_id[i] << " in the reference-population file [" + ref_file + "] was not found in FAM file [" << fam_file << "]." << endl;
			logFile << "ERROR: individual " << ref_id[i] << " in the reference-population file [" + ref_file + "] was not found in FAM file [" << fam_file << "]." << endl;
			exit(0);
		}
		int pop_tmp = -1;
		for (int j = 0; j < ref_pop_num; j++)
		{
			if (ref_pop[i] == ref_pop_name[j]) pop_tmp = j;
		}
		id_pop_map.insert(make_pair(ref_id[i], pop_tmp));
	}
	id_pop.resize(indi_num);
	map<string, int>::iterator it;
	for (int i = 0; i < indi_num; i++)
	{
		id_pop[i] = -1;
		it =  id_pop_map.find(subid[i]);
		if (it != id_pop_map.end()) id_pop[i] = it->second;
	}
	pop_pos.resize(pop_num);
	for (int i = 0; i < ref_pop_num; i++)
	{
		bool pop_found = false;
		for (int j = 0; j < pop_num; j++)
		{
			if (pop_name[j] == ref_pop_name[i])
			{
				pop_found = true;
				pop_pos[j] = i;
			}
		}
		if (!pop_found)
		{
			nonpop_name.push_back(ref_pop_name[i]);
			nonpop_pos.push_back(i);
		}
	}
	nonpop_num = nonpop_name.size();
	for (int i = 0; i < pop_num; i++)
	{
		if (pop_pos[i] == -1)
		{
			cout << "ERROR: population " << pop_name[i] << " specified by --pop was not found in the [" << ref_file << "]." << endl;
			logFile << "ERROR: population " << pop_name[i] << " specified by --pop was not found in the [" << ref_file << "]." << endl;
			exit(0);
		}
	}
	subid_set.clear();
	ref_id_set.clear();
	ref_pop_set.clear();
	ref_pop_multiset.clear();
}

void allele_stat()
{
	allele1_cnt.resize(snp_num);
	maf.resize(snp_num);
	MA.resize(snp_num);
	miss_cnt.resize(snp_num);
	miss_frq.resize(snp_num);
	char tmp1char[1];
	bedfile.open(bed_file.c_str(), ios::in | ios::binary);
	cout << "Calculating allele frequencies... " << flush;
	logFile << "Calculating allele frequencies... " << flush;
	bedfile.read(tmp1char, 1);
	bedfile.read(tmp1char, 1);
	bedfile.read(tmp1char, 1);
	int line_byte_size = ceil(double(indi_num) / 4);
	int batch_snp_size = batch_size;
	int batch_byte_size = batch_snp_size * line_byte_size;
	int batch_num = ceil(double(snp_num) / batch_snp_size);
	char *batch_char = new char[batch_byte_size];
	string batch_str;
	for (int batch_idx = 1; batch_idx <= batch_num; batch_idx ++ )
	{
		int snp_start = (batch_idx - 1) * batch_snp_size;
		int snp_end = batch_idx * batch_snp_size;
		if (batch_idx == batch_num)
		{
			snp_end = snp_num;
			batch_byte_size = (snp_end - snp_start) * line_byte_size;
		}
		bedfile.read(batch_char, batch_byte_size);
		batch_str.assign(batch_char, batch_byte_size);
		dynamic_bitset<unsigned char> batch_bit(batch_str.begin(), batch_str.end());

		#pragma omp parallel for
		for (int i = snp_start; i < snp_end; ++ i)
		{
			allele1_cnt[i].resize(ref_pop_num);
			maf[i].resize(ref_pop_num);
			miss_cnt[i].resize(ref_pop_num);
			miss_frq[i].resize(ref_pop_num);
			for (int j = 0; j < ref_pop_num; j++)
			{
				allele1_cnt[i][j] = 0;
				miss_cnt[i][j] = 0;
			}
			int pos = (line_byte_size) * (i - snp_start) * 8;
			for (int j = 0; j < indi_num; ++ j)
			{
				bool tmp1bool = batch_bit[pos ++ ];
				bool tmp2bool = batch_bit[pos ++ ];
				if (!tmp1bool && !tmp2bool)
				{
					for (int k = 0; k < ref_pop_num; k++)
					{
						if (id_pop[j] == k) allele1_cnt[i][k] += 2;
					}
					continue;
				}
				if (!tmp1bool && tmp2bool)
				{
					for (int k = 0; k < ref_pop_num; k++)
					{
						if (id_pop[j] == k) allele1_cnt[i][k] ++;
					}
					continue;
				}
				if (tmp1bool && !tmp2bool)
				{
					for (int k = 0; k < ref_pop_num; k++)
					{
						if (id_pop[j] == k) miss_cnt[i][k] ++;
					}
				}
			}
			double allele1_total = accumulate(allele1_cnt[i].begin(), allele1_cnt[i].end(), 0);
			int miss_total = accumulate(miss_cnt[i].begin(), miss_cnt[i].end(), 0);
			double A1freq = 0.0;
			if (miss_total != ref_indi_num) A1freq = allele1_total / double(ref_indi_num - miss_total) / 2.0;
			for (int k = 0; k < ref_pop_num; k++)
			{
				if(miss_cnt[i][k] == ref_pop_cnt[k])
            				{
					maf[i][k] = 0.0;
					miss_frq[i][k] = 1.0;
				}
				else
				{
					maf[i][k] = double(allele1_cnt[i][k]) / double(ref_pop_cnt[k] - miss_cnt[i][k]) / 2.0;
					miss_frq[i][k] = double(miss_cnt[i][k]) / ref_pop_cnt[k];
				}
				if(A1freq > 0.5)
				{
					MA[i] = allele2[i];
					if(miss_cnt[i][k] < ref_pop_cnt[k]) maf[i][k] = 1.0 - maf[i][k];
				}
				else
				{
					MA[i] = allele1[i];
				}
			}
		}
	}
	delete [] batch_char;
	bedfile.close();

	int low_MAF_num = 0;
	int high_MAF_num = 0;
	int miss_frq_num = 0;
	remain_snp_num = 0;
	snp_pop.resize(snp_num);
	for(int i = 0; i < snp_num; ++ i)
	{
		snp_pop[i] = true;
		for (int j = 0; j < pop_num; j++)
		{
			if(maf[i][pop_pos[j]] < maf_min)
			{
				low_MAF_num ++;
				snp_pop[i] = false;
			}
			if(maf[i][pop_pos[j]] > maf_max)
			{
				high_MAF_num ++;
				snp_pop[i] = false;
			}
		}
		bool miss_frq_flag = false;
		for (int j = 0; j < ref_pop_num; j++)
		{
			if (miss_frq[i][j] > miss_max)
			{
				miss_frq_flag = true;
				snp_pop[i] = false;
			}
		}
		if (miss_frq_flag) miss_frq_num ++;
		if(snp_pop[i])  remain_snp_num++;
	}
	cout << "done." << endl;
	logFile << "done." << endl;
	cout << low_MAF_num << " SNPs removed due to MAFs < " << maf_min << ".\n";
	logFile << low_MAF_num << " SNPs removed due to MAFs < " << maf_min << ".\n";
	cout << high_MAF_num << " SNPs removed due to MAFs > " << maf_max << ".\n";
	logFile << high_MAF_num << " SNPs removed due to MAFs > " << maf_max << ".\n";
	cout << miss_frq_num << " SNPs removed due to missing rate > " << miss_max << ".\n";
	logFile << miss_frq_num << " SNPs removed due to missing rate > " << miss_max << ".\n";
	if(remain_snp_num == 0)
	{
		cout << "ERROR: 0 SNPs pass filters." << endl;
		logFile << "ERROR: 0 SNPs pass filters." << endl;
		exit(0);
	}
	cout << remain_snp_num << " SNPs passed filters." << endl;
	logFile << remain_snp_num << " SNPs passed filters." << endl;
}

void pop_snp()
{
	pop_snp_num = 0;
	for(int i = 0; i < snp_num; ++ i)
	{
		if (snp_pop[i])
		{
			for (int j = 0; j < pop_num; j++)
			{
				if (maf[i][pop_pos[j]] == 0) snp_pop[i] =false;
			}
			for (int j = 0; j < nonpop_num; j++)
			{
				if (maf[i][nonpop_pos[j]] > 0) snp_pop[i] = false;
			}
			if (snp_pop[i])
			{
				pop_snp_idx.push_back(i);
				pop_snp_num++;
			}
		}
	}
	cout << pop_snp_num << " SNPs are specific to " << pop_name[0];
	for (int i = 1; i < pop_num; i++) cout << ", " << pop_name[i];
	cout << "." << endl;
	logFile << pop_snp_num << " SNPs are specific to " << pop_name[0];
	for (int i = 1; i < pop_num; i++) logFile << ", " << pop_name[i];
	logFile << "." << endl;
}

void allele_freq_output()
{
	ofstream outfile;
	outfile.open((out_file + ".frq").c_str(), ios::out);
	cout << "Saving allele frequencies to [" + out_file + ".frq]... " << flush;
	logFile << "Saving allele frequencies to [" + out_file + ".frq]... ";
	outfile << "chrom\t" << "pos\t" << "SNP\t" << "A1\t" << "A2\t" << "MA";
	for (int i = 0; i < ref_pop_num; i++) outfile << "\t" << ref_pop_name[i] << "_MAF\t"  << ref_pop_name[i] << "_miss_rate";
	outfile << endl;
	for(int i = 0; i<snp_num; ++ i)
	{
		outfile << chrom[i] << "\t";
		outfile << position[i] << "\t";
		outfile << snp[i] << "\t";
		outfile << allele1[i] << "\t";
		outfile << allele2[i] << "\t";
		outfile << MA[i];
		for (int j = 0; j < ref_pop_num; j++) outfile << "\t" << maf[i][j] << "\t" << miss_frq[i][j];
		outfile << endl;
	}
	cout << "done." << endl;
	logFile << "done." << endl;
	outfile.close();
}

void pop_snp_output()
{
	ofstream snpfile;
	string snp_file;
	snp_file = out_file + "." + pop_name[0];
	for (int i = 1; i < pop_num; i++) snp_file = snp_file + "." + pop_name[i];
	snpfile.open(snp_file.c_str(), ios::out);
	if (flag[9] == 1 && pop_snp_num > out_snp_num)
	{
		random_shuffle(pop_snp_idx.begin(), pop_snp_idx.end());
		cout << "Saving randomly selected " << out_snp_num << " population-specific SNPs to [" + snp_file + "]... " << flush;
		logFile << "Saving randomly selected " << out_snp_num << " population-specific SNPs to [" + snp_file + "]... ";
	}
	else
	{
		out_snp_num = pop_snp_num;
		cout << "Saving the " << out_snp_num << " population-specific SNPs to [" + snp_file + "]... " << flush;
		logFile << "Saving the " << out_snp_num << " population-specific SNPs to [" + snp_file + "]... ";
	}

	for(int i = 0; i < out_snp_num; i++)
	{
		snpfile << snp[pop_snp_idx[i]];
		for (int j = 0; j < pop_num; j++) snpfile << "\t" << pop_name[j] << "\t" << maf[pop_snp_idx[i]][pop_pos[j]];
		snpfile << endl;
	}
	cout << "done." << endl;
	logFile << "done." << endl;
	snpfile.close();
}
};

int main(int argc, char ** argv)
{
	time_t current_time;
	char * current_time_str;
	Dataset dat(argc, argv);
	stringstream ss;
	ss << dat.thread_num;
	setenv("OMP_NUM_THREADS", ss.str().c_str(), 1);
	omp_set_num_threads(dat.thread_num);
	cout << "The program will be running on " << dat.thread_num << " threads." << endl;
	dat.logFile << "The program will be running on " << dat.thread_num << " threads." << endl;
	current_time = time(0);
	current_time_str = ctime(&current_time);
	cout << "Start time: " << current_time_str << flush;
	dat.logFile << "Start time: " << current_time_str;
	dat.readFAM();
	dat.readBIM();
	dat.readREF();
	if (dat.flag[8] == 1 || dat.flag[3] == 1) dat.allele_stat();
	if (dat.flag[8] == 1) dat.allele_freq_output();
	if (dat.flag[3] == 1)
	{
		dat.pop_snp();
		dat.pop_snp_output();
	}
	current_time = time(0);
	current_time_str = ctime(&current_time);
	cout << "End time: " << current_time_str << endl;
	dat.logFile << "End time: " << current_time_str << endl;
	return 0;
}


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

#include <iostream>

#include <core/Message.h>
#include <crypto/PrivateKey.h>
#include <crypto/PublicKey.h>
#include <rpos/Proof.h>
#include <rpos/Seed.h>
#include <rpos/Vrf.h>

using namespace ultrainio;
using namespace std;

// Proof
BOOST_AUTO_TEST_SUITE(proof_test_suite)

    BOOST_AUTO_TEST_CASE(normal_flow) {
        PrivateKey privateKey;
        PublicKey publicKey;
        PrivateKey::generate(publicKey, privateKey);
        Seed seed(std::string("preHash"), 40, static_cast<ConsensusPhase>(1), 0);
        Proof proof = Vrf::vrf(privateKey, seed, Vrf::kProposer);

        BOOST_CHECK(proof.getRand() > 0.0 && proof.getRand() < 1.0);
        BOOST_CHECK(Vrf::verify(publicKey, proof, seed, Vrf::kProposer));
        BOOST_CHECK(!Vrf::verify(publicKey, proof, seed, Vrf::kVoter));

        string hexProofStr = std::string(proof);
        BOOST_CHECK(Proof(hexProofStr).getPriority() == proof.getPriority());

        string faultHexStr(hexProofStr.data(), hexProofStr.length() - 8);
        faultHexStr += "00000000"; // 8
        Proof faultProof(faultHexStr);
        BOOST_CHECK(faultProof.getPriority() == faultProof.getPriority());
        BOOST_CHECK(!Vrf::verify(publicKey, faultProof, seed, Vrf::kProposer));
    }

BOOST_AUTO_TEST_SUITE_END()


/*MIT License

Copyright (c) 2020 WX96

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 <pcl/io/pcd_io.h>
#include <pcl/features/integral_image_normal.h>
#include <boost/thread/thread.hpp>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/common/transforms.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/impl/passthrough.hpp>
#include <pcl/visualization/impl/point_cloud_geometry_handlers.hpp>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/extract_indices.h>
#include <iostream>
#include <unordered_map>
#include <vector>
#include <algorithm>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc.hpp>
#include <queue>
#include "ground_remove2.h"

#define PCL_NO_PRECOMPILE
using namespace std;
const float PI = 3.1415926;

float Polar_angle_cal(float x, float y) {
	float temp_tangle = 0;
	if (x == 0 && y == 0) {
		temp_tangle = 0;
	} else if (y >= 0) {
		temp_tangle = (float) atan2(y, x);
	} else if (y <= 0) {
		temp_tangle = (float) atan2(y, x) + 2 * PI;
	}
	return temp_tangle;
}
//HSV转rgb
vector<float> hsv2rgb(vector<float>& hsv) {
	vector<float> rgb(3);
	float R, G, B, H, S, V;
	H = hsv[0];
	S = hsv[1];
	V = hsv[2];
	if (S == 0) {
		rgb[0] = rgb[1] = rgb[2] = V;
	} else {

		int i = int(H * 6);
		float f = (H * 6) - i;
		float a = V * (1 - S);
		float b = V * (1 - S * f);
		float c = V * (1 - S * (1 - f));
		i = i % 6;
		switch (i) {
		case 0: {
			rgb[0] = V;
			rgb[1] = c;
			rgb[2] = a;
			break;
		}
		case 1: {
			rgb[0] = b;
			rgb[1] = V;
			rgb[2] = a;
			break;
		}
		case 2: {
			rgb[0] = a;
			rgb[1] = V;
			rgb[2] = c;
			break;
		}
		case 3: {
			rgb[0] = a;
			rgb[1] = b;
			rgb[2] = V;
			break;
		}
		case 4: {
			rgb[0] = c;
			rgb[1] = a;
			rgb[2] = V;
			break;
		}
		case 5: {
			rgb[0] = V;
			rgb[1] = a;
			rgb[2] = b;
			break;
		}
		}
	}

	return rgb;
}

//可视化

template<typename T> string toString(const T& t) {
	ostringstream oss;
	oss << t;
	return oss.str();
}

template<typename PointInT>
float CalculateRangeXY(const PointInT pointIn) {

	return sqrt(pointIn.x * pointIn.x + pointIn.y * pointIn.y);
}

template<typename PointInT>
float CalculateRangeZXY(const PointInT pointIn) {

	return sqrt(
			pointIn.x * pointIn.x + pointIn.y * pointIn.y
					+ (pointIn.z-2.1) * (pointIn.z-2.1));
}

struct Range {

	float range_xy;
	float range_zxy;
	int ring_i;
	int frame_j;
	int count_num;

};

void CloudFilter(const pcl::PointCloud<Velodyne_32_HDLE>& cloudIn,
		pcl::PointCloud<Velodyne_32_HDLE>& cloudOut, float x_min, float x_max,
		float y_min, float y_max, float z_min, float z_max) {
	cloudOut.header = cloudIn.header;
	cloudOut.sensor_orientation_ = cloudIn.sensor_orientation_;
	cloudOut.sensor_origin_ = cloudIn.sensor_origin_;
	cloudOut.points.clear();
	//1) set parameters for removing cloud reflect on ego vehicle
	float x_limit_min = -1.8, x_limit_max = 1.8, y_limit_forward = 5.0,
			y_limit_backward = -4.5;
	//2 apply the filter
	for (int i = 0; i < cloudIn.size(); ++i) {
		float x = cloudIn.points[i].x;
		float y = cloudIn.points[i].y;
		float z = cloudIn.points[i].z;
		// whether on ego vehicle
		if ((x > x_limit_min && x < x_limit_max && y > y_limit_backward
				&& y < y_limit_forward))
			continue;
		if ((x > x_min && x < x_max && y > y_min && y < y_max && z > z_min
				&& z < z_max)) {

			cloudOut.points.push_back(cloudIn.points[i]);

		}

	}
}

int total_frame = 0;

void Transform2RangeImage(const pcl::PointCloud<pcl::PointXYZI>& cloudIn,
		pcl::PointCloud<Velodyne_32_HDLE>& ng_cloudOut,
		pcl::PointCloud<Velodyne_32_HDLE>& g_cloudOut, unordered_map<int, Range> &unordered_map_out) {

	total_frame = int(cloudIn.points.size() / 32) - 1;
	pcl::PointCloud<Velodyne_32_HDLE>::Ptr cloud2range(
				new pcl::PointCloud<Velodyne_32_HDLE>);

	pcl::PointCloud<Velodyne_32_HDLE>::Ptr cloud_filtered(
				new pcl::PointCloud<Velodyne_32_HDLE>);
        cloud2range->points.resize(cloudIn.points.size());

	for (int j = 0; j < total_frame; ++j) {
		int num_odd = 16;                //基数从16位开始排
		int num_even = 0;                //偶数从头

		for (int i = 0; i < 32; ++i) {
			if (float(i % 2) == 0.0) {
				cloud2range->points[j * 32 + i].x = cloudIn.points[j * 32 + i].x ;
				cloud2range->points[j * 32 + i].y = cloudIn.points[j * 32 + i].y ;
				cloud2range->points[j * 32 + i].z = cloudIn.points[j * 32 + i].z ;
				cloud2range->points[j * 32 + i].ring =num_even;
				cloud2range->points[j * 32 + i].pcaketnum =j;
				num_even++;
			} else {
				cloud2range->points[j * 32 + i].x  = cloudIn.points[j * 32 + i].x;
				cloud2range->points[j * 32 + i].y = cloudIn.points[j * 32 + i].y ;
				cloud2range->points[j * 32 + i].z = cloudIn.points[j * 32 + i].z ;
				cloud2range->points[j * 32 + i].ring =num_odd;
				cloud2range->points[j * 32 + i].pcaketnum =j;
				num_odd++;
			}
		} //按索引顺序排列

        }

	cloud2range->height = 1;
	cloud2range->width = cloud2range->points.size();

	float xmin = -35, xmax = 35, ymin = -30, ymax = 30, zmin = -1.0, zmax = 3.0;
        CloudFilter(*cloud2range, *cloud_filtered, xmin, xmax, ymin, ymax, zmin, zmax);

        GroundRemove2 ground_remove(3, 20, 1.0, 0.15);

        ground_remove.RemoveGround2(*cloud_filtered, g_cloudOut, ng_cloudOut);

	float x_limit_min = -1, x_limit_max = 1, y_limit_forward = 4.0,
			y_limit_backward = -3;

        for(int i=0; i<ng_cloudOut.points.size(); ++i){

            float x = ng_cloudOut.points[i].x;
	    float y = ng_cloudOut.points[i].y;
	    float z = ng_cloudOut.points[i].z;
       	    float distance = CalculateRangeXY( ng_cloudOut.points[i]);
            /*if (distance > 30)
				continue;
			if ((x > x_limit_min && x < x_limit_max && y > y_limit_backward
					&& y < y_limit_forward))
				continue;

			if ((z < -1 || z > 3))
				continue;*/

                        int ringnum = ng_cloudOut.points[i].ring;

			int image_index = ng_cloudOut.points[i].pcaketnum * 32 + (31 - ringnum);
			Range r;
			r.ring_i = 31 -ringnum;
			r.frame_j = ng_cloudOut.points[i].pcaketnum;
			r.count_num = i;
			r.range_xy = ng_cloudOut.points[i].z;
			r.range_zxy = CalculateRangeZXY(ng_cloudOut.points[i]);
			unordered_map_out.insert(make_pair(image_index, r));

        }

}

void find_neighbors(int Ix, int Iy, vector<int>& neighborudindex,
		vector<int>& neighborlrindex) {
	//cout<<"silinyu " << Ix<< " "<<Iy<<endl;
	for (int x = Ix - 2; x <= Ix + 2; ++x) {
		//cout<<"DDDDDDw "<<x<<endl;
		if (x == Ix)
			continue;
		int px = x;
		if (x < 0) {
			px = total_frame-1;
                        //cout<<"LL"<<endl;
		}
		if (x > total_frame-1) {
			px = 0;
                       // cout<<"RR"<<endl;
		}
		//cout<<px << " "<<Iy<<endl;
		neighborlrindex.push_back(px * 32 + Iy);
	}

	for (int y = Iy - 1; y <= Iy + 1; ++y) {
		if (y == Iy)
			continue;
		if (y < 0 || y > 31)
			continue;
		//cout<<Ix << " "<<y<<endl;
		neighborudindex.push_back((Ix * 32 + y));
	}
}

void mergeClusters(vector<int>& cluster_indices, int idx1, int idx2) {
	for (int i = 0; i < cluster_indices.size(); i++) {
		if (cluster_indices[i] == idx1) {
			cluster_indices[i] = idx2;
		}
	}
}

bool compare_index(pair<int, Range> a, pair<int, Range> b) {
	return a.first < b.first;
} //升序

vector<int> range_cluster(unordered_map<int, Range> &unordered_map_in, double ringnum) {

	int current_cluster = 0;
	vector<int> cluster_indices = vector<int>(total_frame * 32, -1);
	float horizon_angle_resolution = 0.16 * PI / 180;
	float vertical_angle_resolution = 1.33 * PI / 180;
        vector<pair<int, Range>> tr(unordered_map_in.begin(), unordered_map_in.end());
        sort(tr.begin(), tr.end(), compare_index);
        for(int i =0 ; i< tr.size(); ++i){
        }

	int for_num = 0;
	float theta_thresh = 8 * PI / 180;
	float theta_thresh2 =30 * PI / 180;

	for (int i = 0; i < tr.size(); ++i) {
			unordered_map<int, Range>::iterator it_find;
			it_find = unordered_map_in.find(tr[i].first);

			if (it_find != unordered_map_in.end() && cluster_indices[tr[i].first] == -1) {
				queue<vector<int>> q;
				vector<int> indexxy(2);
				indexxy[0] = it_find->second.frame_j;
				indexxy[1] = it_find->second.ring_i;
				q.push(indexxy);
				while (q.size()>0) {

					if (cluster_indices[q.front()[0] * 32 + q.front()[1]]
							!= -1) {
					        q.pop();
						continue;
					}

					cluster_indices[q.front()[0] * 32 + q.front()[1]] =
							current_cluster;
					vector<int> neighborudid;
					vector<int> neighborlfid;
					unordered_map<int, Range>::iterator it_findo;
					it_findo = unordered_map_in.find(q.front()[0] * 32 + q.front()[1]);
					find_neighbors(q.front()[0], q.front()[1], neighborudid,
							neighborlfid);

					if (neighborudid.size() > 0) {
						for (int in = 0; in < neighborudid.size(); ++in) {
							unordered_map<int, Range>::iterator it_findn;
							it_findn = unordered_map_in.find(neighborudid[in]);
							if (it_findn != unordered_map_in.end()) {
								float d1 = max(it_findo->second.range_zxy,
										it_findn->second.range_zxy);
								float d2 = min(it_findo->second.range_zxy,
										it_findn->second.range_zxy);

								float angle = fabs((float) atan2(d2* sin(vertical_angle_resolution),d1- d2* cos(vertical_angle_resolution)));
                                                                float dmax = (it_findo->second.range_zxy) * sin(1.33*PI/180)/sin(50*PI/180 -1.33*PI/180) + 3*0.2;
							        if (it_findo->second.range_xy>1.2 && fabs(d2-d1) < dmax) {
									vector<int> indexxy(2);
									indexxy[0] = it_findn->second.frame_j;
									indexxy[1] = it_findn->second.ring_i;
									q.push(indexxy);
								}else if (angle > theta_thresh2) {
									vector<int> indexxy(2);
									indexxy[0] = it_findn->second.frame_j;
									indexxy[1] = it_findn->second.ring_i;
									q.push(indexxy);
								}
							}
						}
					}

					if (neighborlfid.size() > 0) {
						for (int in = 0; in < neighborlfid.size(); ++in) {
							unordered_map<int, Range>::iterator it_findn;
							it_findn = unordered_map_in.find(neighborlfid[in]);
							if (it_findn != unordered_map_in.end()) {
								float d1 = max(it_findo->second.range_zxy,
										it_findn->second.range_zxy);
								float d2 = min(it_findo->second.range_zxy,
										it_findn->second.range_zxy);

								float angle = fabs((float) atan2(d2* sin(horizon_angle_resolution),d1- d2* cos(horizon_angle_resolution)));
                                                                float dmax = (it_findo->second.range_zxy) * sin(360/ringnum*PI/180)/sin(30*PI/180 -360/ringnum*PI/180) + 3*0.2;
							        //if (fabs(it_findo->second.range_zxy-it_findn->second.range_zxy) < dmax) {
								if (angle > theta_thresh) {
									//cluster_indices[neighborlfid[in]] =
											//current_cluster;
									vector<int> indexxy(2);
									indexxy[0] = it_findn->second.frame_j;
									indexxy[1] = it_findn->second.ring_i;
                                                                        //cout<<"LLL2 "<<indexxy[0]<<" "<<indexxy[1]<<endl;
									q.push(indexxy);
								}
							}
						}
					}
					q.pop();
				}
				current_cluster++;
			}

	}

	return cluster_indices;
}

bool compare_cluster(pair<int, int> a, pair<int, int> b) {
	return a.second < b.second;
} //升序

bool most_frequent_value(vector<int> values, vector<int> &cluster_index) {
	unordered_map<int, int> histcounts;
	for (int i = 0; i < values.size(); i++) {
		if (histcounts.find(values[i]) == histcounts.end()) {
			histcounts[values[i]] = 1;
		} else {
			histcounts[values[i]] += 1;
		}
	}

	int max = 0, maxi;
	vector<pair<int, int>> tr(histcounts.begin(), histcounts.end());
	sort(tr.begin(), tr.end(), compare_cluster);
	for (int i = 0; i < tr.size(); ++i) {
		if (tr[i].second > 10) {
			cluster_index.push_back(tr[i].first);
		}
	}

	return true;
}

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

	pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(
			new pcl::PointCloud<pcl::PointXYZI>);
	pcl::PointCloud<Velodyne_32_HDLE>::Ptr cloud_ng(
			new pcl::PointCloud<Velodyne_32_HDLE>);
	pcl::PointCloud<Velodyne_32_HDLE>::Ptr cloud_g(
			new pcl::PointCloud<Velodyne_32_HDLE>);

	pcl::PointCloud<Velodyne_32_HDLE>::Ptr cloud_new(
			new pcl::PointCloud<Velodyne_32_HDLE>);

	ifstream inputfile;
	inputfile.open(argv[1], ios::binary);
	if (!inputfile) {
		cerr << "Open input file error!" << endl;
		exit(-1);
	}

	inputfile.seekg(0, ios::beg);

	for (int i = 0; inputfile.good() && !inputfile.eof(); i++) {
		pcl::PointXYZI p;
		inputfile.read((char *) &p.x, 3 * sizeof(float));
		inputfile.read((char *) &p.intensity, sizeof(float));
		cloud->points.push_back(p);
	}

	cloud->height = 1;
	cloud->width = cloud->points.size();

        double ringnum=(cloud->points.size()/32)-1;
	unordered_map<int, Range> range_image;
	Transform2RangeImage(*cloud, *cloud_ng, *cloud_g, range_image);

	cloud_ng->height = 1;
	cloud_ng->width = cloud_ng->points.size();
	cloud_ng->is_dense = false;

	cloud_g->height = 1;
	cloud_g->width = cloud_ng->points.size();
	cloud_g->is_dense = false;
	vector<int> cluster_index = range_cluster(range_image,ringnum);
	cv::Mat bvimage = cv::Mat::zeros(32, total_frame, CV_8UC1);
	cv::Mat range_imagec = cv::Mat::zeros(32, total_frame, CV_8UC3);

	for (auto it = range_image.begin(); it != range_image.end(); ++it) {
		int index = it->second.count_num;
		//cout<<"final "<<index<<endl;
		cloud_new->points.push_back(cloud_ng->points[index]);
		float anglepix = Polar_angle_cal(cloud_ng->points[index].x,
				cloud_ng->points[index].y);
		bvimage.at<uchar>(it->second.ring_i, it->second.frame_j) =
				it->second.range_zxy / 30 * 256;

	}
	cv::resize(bvimage, bvimage, cv::Size(1000, 100));
        cv::imwrite("range_image2.png", bvimage);

	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(
			new pcl::visualization::PCLVisualizer("pcd")); //PCLVisualizer 可视化类
	viewer->setBackgroundColor(0.8, 0.8, 0.8);
	viewer->addCoordinateSystem(1);
	//pcl::visualization::PointCloudColorHandlerCustom <Velodyne_32_HDLE
			//> color(cloud_g, 0,0, 0);
	//viewer->addPointCloud(cloud_g, color, "cloud");
	vector<int> cluster_id;
	most_frequent_value(cluster_index, cluster_id);
        cv::RNG rng(12345);

	for (int k = 0; k < cluster_id.size(); ++k) {
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr Colorcloud2(
				new pcl::PointCloud<pcl::PointXYZRGB>);
		vector<float> hsv(3);
		hsv[0] = float(k) / float(cluster_id.size());
		hsv[1] = 1;
		hsv[2] = 1;

                int r = rng.uniform(0, 255);
                int g = rng.uniform(0, 255);
                int b = rng.uniform(0, 255);
		vector<float> rgb = hsv2rgb(hsv);
		for (int i = 0; i < total_frame; ++i) {
			for (int j = 0; j < 32; ++j) {
				if (cluster_index[i * 32 + j] == cluster_id[k]
						&& cluster_id[k] != -1) {
					unordered_map<int, Range>::iterator it_find;
					it_find = range_image.find(i * 32 + j);
					if (it_find != range_image.end()) {
						pcl::PointXYZRGB p;
						p.x = cloud_ng->points[it_find->second.count_num].x;
						p.y = cloud_ng->points[it_find->second.count_num].y;
						p.z = cloud_ng->points[it_find->second.count_num].z;
						p.r = 255;
						p.g = 0;
						p.b = 0;
						Colorcloud2->points.push_back(p);
                                                range_imagec.at<cv::Vec3b>(it_find->second.ring_i, it_find->second.frame_j) = cv::Vec3b(r,g,b);
					}
				}
			}
		}

		if (Colorcloud2->points.size() > 5) {
			Colorcloud2->height = 1;
			Colorcloud2->width = Colorcloud2->points.size();
			float xmax1 = Colorcloud2->points[0].x;
			float xmin1 = Colorcloud2->points[0].x;
			float ymax1 = Colorcloud2->points[0].y;
			float ymin1 = Colorcloud2->points[0].y;
			float zmax1 = Colorcloud2->points[0].z;
			float zmin1 = Colorcloud2->points[0].z;
			//find the xmax, xmin, ymax, ymin
			for (int i = 0; i < Colorcloud2->points.size(); ++i) {
				if (Colorcloud2->points[i].z > zmax1) {
					zmax1 = Colorcloud2->points[i].z;
				}
				if (Colorcloud2->points[i].z < zmin1) {
					zmin1 = Colorcloud2->points[i].z;
				}
				if (Colorcloud2->points[i].x > xmax1) {
					xmax1 = Colorcloud2->points[i].x;
				}
				if (Colorcloud2->points[i].x < xmin1) {
					xmin1 = Colorcloud2->points[i].x;
				}
				if (Colorcloud2->points[i].y > ymax1) {
					ymax1 = Colorcloud2->points[i].y;
				}
				if (Colorcloud2->points[i].y < ymin1) {
					ymin1 = Colorcloud2->points[i].y;
				}

			}

			if (zmin1 < 0)
				zmin1 = 0; //make sure object is up the ground

			double depth = zmax1 - zmin1;

			Eigen::Vector3f eulerAngle(0.0, 0.0, 0.0);
			Eigen::AngleAxisf rollAngle(
					Eigen::AngleAxisf(eulerAngle(2), Eigen::Vector3f::UnitX()));
			Eigen::AngleAxisf pitchAngle(
					Eigen::AngleAxisf(eulerAngle(1), Eigen::Vector3f::UnitY()));
			Eigen::AngleAxisf yawAngle(
					Eigen::AngleAxisf(eulerAngle(0), Eigen::Vector3f::UnitZ()));

			const Eigen::Quaternionf bboxQ1(yawAngle * pitchAngle * rollAngle);

			Eigen::Vector3f translation;
			translation(0) = (xmax1 - xmin1) / 2 + xmin1;
			translation(1) = (ymax1 - ymin1) / 2 + ymin1;
			translation(2) = (zmax1 - zmin1) / 2 + zmin1;
			double length = ymax1 - ymin1;
			double width = xmax1 - xmin1;

			pcl::visualization::PointCloudColorHandlerCustom < pcl::PointXYZRGB
					> color2(Colorcloud2, (b), (g),
							(r));
			viewer->addPointCloud(Colorcloud2, color2, "cloud2" + toString(k));
		}
	}

	cout << range_image.size() << endl;

	int count_num = 0;
	for (auto it = range_image.begin(); it != range_image.end(); ++it) {
		count_num++;
	}

        //cv::imshow("bv", range_imagec);
	//cv::waitKey(0);
	cv::resize(range_imagec, range_imagec, cv::Size(1000, 100));
        cv::imwrite("range_image3.png", range_imagec);
	while (!viewer->wasStopped()) {
		viewer->spin();
	}

        return 0;
}