/* 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/gui/gui.hpp>
#include <tdp/gui/quickView.h>
#include <tdp/icp/icp.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 <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>

using namespace gtsam;
using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
using symbol_shorthand::V; // Vel   (xdot,ydot,zdot)
using symbol_shorthand::B; // Bias  (ax,ay,az,gx,gy,gz)

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";

  if( argc > 1 ) {
    input_uri = std::string(argv[1]);
    calibPath = (argc > 2) ? std::string(argv[2]) : "";
    imu_input_uri =  (argc > 3)? std::string(argv[3]) : "";
  }

  // 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();

  Eigen::Matrix3f R_ir;
  R_ir << 0, 0,-1,
       0,-1, 0,
       -1, 0, 0;
  tdp::SE3f T_ir(R_ir,Eigen::Vector3f::Zero());

  tdp::GUI gui(1200,800,video);
  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 = h+h%64;
  float f = 550;
  float uc = (w-1.)/2.;
  float vc = (h-1.)/2.;

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

  CameraT camR(Eigen::Vector4f(f,f,uc,vc));
  CameraT camD(Eigen::Vector4f(f,f,uc,vc));

  if (calibPath.size() > 0) {
    tdp::Rig<CameraT> rig;
    rig.FromFile(calibPath,false);
    std::vector<pangolin::VideoInterface*>& streams = video.InputStreams();
    rig.CorrespondOpenniStreams2Cams(streams);
    // camera model for computing point cloud and normals
    camR = rig.cams_[rig.rgbStream2cam_[0]];
    camD = camR; //rig.cams_[rig.dStream2cam_[0]];
    if (rig.T_ris_.size() > 0) T_ir = rig.T_ris_[0];
  }

  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::View& viewN3D = pangolin::CreateDisplay()
    .SetHandler(new pangolin::Handler3D(s_cam));
  gui.container().AddDisplay(viewN3D);

  tdp::ManagedHostImage<float> d(wc, hc);
  tdp::ManagedHostImage<Eigen::Matrix<uint8_t,3,1>> n2D(wc,hc);
  memset(n2D.ptr_,0,n2D.SizeBytes());
  tdp::ManagedHostImage<Eigen::Vector3f> n2Df(wc,hc);
  tdp::ManagedHostImage<Eigen::Vector3f> n(wc,hc);

  tdp::ManagedDeviceImage<uint16_t> cuDraw(w, h);
  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);

  tdp::ManagedHostImage<float> dEst(wc, hc);
  tdp::ManagedDeviceImage<float> cuDEst(wc, hc);
  dEst.Fill(0.);
  cuDEst.CopyFrom(dEst);
  tdp::ManagedDeviceImage<float> cuDView(wc, hc);
  tdp::ManagedDeviceImage<tdp::Vector3fda> cuPcView(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<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::ManagedDeviceImage<float> cuPcErr(wc, hc);
  tdp::ManagedDeviceImage<float> cuAngErr(wc, hc);
  tdp::ManagedHostImage<float> pcErr(wc, hc);
  tdp::ManagedHostImage<float> angErr(wc, hc);

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

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

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

  tdp::ManagedHostImage<float> dispDepthPyr(dPyr.Width(0)+dPyr.Width(1), hc);
  tdp::QuickView viewDepthPyr(dispDepthPyr.w_,dispDepthPyr.h_);
  gui.container().AddDisplay(viewDepthPyr);

  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 viewNormalsPyr(dispNormals2dPyr.w_,dispNormals2dPyr.h_);
  gui.container().AddDisplay(viewNormalsPyr);

  pangolin::Var<float> depthSensorScale("ui.depth sensor scale",1e-3,1e-4,1e-3);
  pangolin::Var<float> tsdfDmin("ui.d min",0.10,0.0,0.1);
  pangolin::Var<float> tsdfDmax("ui.d max",4.,0.1,4.);

  pangolin::Var<bool> dispNormalsPyrEst("ui.disp normal est", false, true);
  pangolin::Var<bool> runFusion("ui.run Fusion",true,true);

  pangolin::Var<bool>  resetTSDF("ui.reset TSDF", false, false);
  pangolin::Var<bool>  saveTSDF("ui.save TSDF", false, false);
  pangolin::Var<bool> fuseTSDF("ui.fuse TSDF",true,true);

  pangolin::Var<float> tsdfMu("ui.mu",0.5,0.,1.);
  pangolin::Var<float> tsdfWThr("ui.w thr",25.,1.,20.);
  pangolin::Var<float> tsdfWMax("ui.w max",200.,1.,300.);
  pangolin::Var<int>   tsdfSliceD("ui.TSDF slice D",dTSDF/2,0,dTSDF-1);
  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<bool> resetICP("ui.reset ICP",false,false);
  pangolin::Var<bool>  runICP("ui.run ICP", true, true);
  pangolin::Var<float> icpAngleThr_deg("ui.icp angle thr",15,0.,90.);
  pangolin::Var<float> icpDistThr("ui.icp dist thr",0.10,0.,1.);
  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<int>   icpErrorLvl("ui.ICP error vis lvl",0,0,2);

  pangolin::Var<bool> runSAM("ui.run SAM",true,true);
  pangolin::Var<bool> samIMU("ui.add IMU SAM",true,true);

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

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

  gui.verbose = true;

  tdp::SE3f T_mo;
  tdp::SE3f T_mo_0 = T_mo;
  tdp::SE3f T_mo_prev = T_mo_0;
  tdp::SE3f T_wr_imu_prev;
  size_t numFused = 0;

  tdp::ThreadedValue<bool> runWorker(true);
  std::thread workThread([&]() {
        while(runWorker.Get()) {
          if (pangolin::Pushed(saveTSDF)) {
            tdp::ManagedHostVolume<tdp::TSDFval> tmpTSDF(wTSDF, hTSDF, dTSDF);
            tmpTSDF.CopyFrom(cuTSDF);
            std::cout << "start writing TSDF to " << tsdfOutputPath << std::endl;
            tdp::SaveVolume(tmpTSDF, tsdfOutputPath);
            std::cout << "done writing TSDF to " << tsdfOutputPath << std::endl;
          }
          std::this_thread::sleep_for(std::chrono::microseconds(100));
        }
      });

  // Assemble initial quaternion through gtsam constructor ::quaternion(w,x,y,z);
  Rot3 prior_rotation = Rot3::Quaternion(1,0,0,0);
  Point3 prior_point(0,0,0);
  Pose3 prior_pose(prior_rotation, prior_point);
  Vector3 prior_velocity(0,0,0);
  imuBias::ConstantBias prior_imu_bias; // assume zero initial bias

  Values initial_values;
  int correction_count = 0;
  initial_values.insert(X(correction_count), prior_pose);
  initial_values.insert(V(correction_count), prior_velocity);
  initial_values.insert(B(correction_count), prior_imu_bias);

  // Assemble prior noise model and add it the graph.
  noiseModel::Diagonal::shared_ptr pose_noise_model =
    noiseModel::Diagonal::Sigmas((Vector(6)
          << 0.01, 0.01, 0.01, 0.01, 0.01, 0.01).finished()); // rad,rad,rad,m, m, m
  noiseModel::Diagonal::shared_ptr velocity_noise_model =
    noiseModel::Isotropic::Sigma(3,0.1); // m/s
  noiseModel::Diagonal::shared_ptr bias_noise_model =
    noiseModel::Isotropic::Sigma(6,1e-2);

  // Add all prior factors (pose, velocity, bias) to the graph.
  NonlinearFactorGraph *graph = new NonlinearFactorGraph();
  graph->add(PriorFactor<Pose3>(X(correction_count), prior_pose,
        pose_noise_model));
  graph->add(PriorFactor<Vector3>(V(correction_count),
        prior_velocity,velocity_noise_model));
  graph->add(PriorFactor<imuBias::ConstantBias>(B(correction_count),
        prior_imu_bias,bias_noise_model));

  // We use the sensor specs to build the noise model for the IMU factor.
  double accel_noise_sigma = 0.0003924;
  double gyro_noise_sigma = 0.000205689024915;
  double accel_bias_rw_sigma = 0.004905;
  double gyro_bias_rw_sigma = 0.000001454441043;
  Matrix33 measured_acc_cov = Matrix33::Identity(3,3) * pow(accel_noise_sigma,2);
  Matrix33 measured_omega_cov = Matrix33::Identity(3,3) * pow(gyro_noise_sigma,2);
  Matrix33 integration_error_cov = Matrix33::Identity(3,3)*1e-8; // error committed in integrating position from velocities
  Matrix33 bias_acc_cov = Matrix33::Identity(3,3) * pow(accel_bias_rw_sigma,2);
  Matrix33 bias_omega_cov = Matrix33::Identity(3,3) * pow(gyro_bias_rw_sigma,2);
  Matrix66 bias_acc_omega_int = Matrix::Identity(6,6)*1e-5; // error in the bias used for preintegration

  boost::shared_ptr<PreintegratedCombinedMeasurements::Params> p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
  // PreintegrationBase params:
  p->accelerometerCovariance = measured_acc_cov; // acc white noise in continuous
  p->integrationCovariance = integration_error_cov; // integration uncertainty continuous
  // should be using 2nd order integration
  // PreintegratedRotation params:
  p->gyroscopeCovariance = measured_omega_cov; // gyro white noise in continuous
  // PreintegrationCombinedMeasurements params:
  p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
  p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
  p->biasAccOmegaInt = bias_acc_omega_int;

  PreintegrationType *imu_preintegrated_;
#ifdef USE_COMBINED
  imu_preintegrated_ = new PreintegratedCombinedMeasurements(p, prior_imu_bias);
#else
  imu_preintegrated_ = new PreintegratedImuMeasurements(p, prior_imu_bias);
#endif

  // Store previous state for the imu integration and the latest predicted outcome.
  NavState prev_state(prior_pose, prior_velocity);
  NavState prop_state = prev_state;
  imuBias::ConstantBias prev_bias = prior_imu_bias;

  double dt = 0.005;  // The real system has noise, but here, results are nearly
                      // exactly the same, so keeping this for simplicity.
                      //
  //
  tdp::ThreadedValue<bool> receiveImu(true);
  tdp::ThreadedValue<size_t> numReceived(0);
  tdp::ThreadedValue<int64_t> t_host_video(0);
  std::thread receiverThread (
    [&]() {
      tdp::ImuObs imuObs;
      tdp::ImuObs imuObsPrev;
//      bool calibrated = false;
//      Eigen::Vector3f gyro_bias = Eigen::Vector3f::Zero();
      while(receiveImu.Get()) {
        if (imu->GrabNext(imuObs)) {
          while(t_host_video.Get() < imuObs.t_host) {
            std::this_thread::sleep_for(std::chrono::microseconds(10));
          }
          double dt = (t_host_video.Get() - imuObs.t_host)*1e-9;
          if (numReceived.Get() > 0 && dt < 0.1)  {
            double dt = (imuObs.t_device- imuObsPrev.t_device)*1e-9;
            std::cout << "imu observation " <<  imuObs.t_host
              << " dt: " << dt << std::endl;
            imu_preintegrated_->integrateMeasurement(
              (T_ir.rotation().Inverse()*imuObs.acc).cast<double>(),
              (T_ir.rotation().Inverse()*imuObs.omega).cast<double>(),
              dt);
          }
          imuObsPrev = imuObs;
          numReceived.Increment();
        }
        //std::this_thread::sleep_for(std::chrono::microseconds(100));
      }
    });

  tdp::SE3f T_mo_isam;

  // Stream and display video
  while(!pangolin::ShouldQuit())
  {
    if (runFusion.GuiChanged() && !runFusion) {
      T_mo_0 = tdp::SE3f();
      T_mo = T_mo_0;
      T_mo_prev = T_mo_0;
    }

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

    gui.NextFrames();
    tdp::Image<uint16_t> dRaw;
    int64_t t_host_us_d = 0;
    if (!gui.ImageD(dRaw,0,&t_host_us_d)) continue;
    t_host_video.Set(t_host_us_d*1000);
//    tdp::SE3f T_wr_imu = T_ir.Inverse() * imuInterp.Ts_wi_[t_host_us_d*1000]*T_ir;
    std::cout << " depth frame at " << t_host_us_d << " us" << std::endl;

    tdp::Vector3fda grid0(grid0x,grid0y,grid0z);
    tdp::Vector3fda gridE(gridEx,gridEy,gridEz);
    tdp::Vector3fda dGrid = gridE - grid0;
    dGrid(0) /= (wTSDF-1);
    dGrid(1) /= (hTSDF-1);
    dGrid(2) /= (dTSDF-1);

    if (gui.verbose) std::cout << "setup pyramids" << std::endl;
    TICK("Setup Pyramids");
    cuDraw.CopyFrom(dRaw);
    ConvertDepthGpu(cuDraw, cuD, depthSensorScale, tsdfDmin, tsdfDmax);
    // construct pyramid
    tdp::ConstructPyramidFromImage<float,3>(cuD, cuDPyr, 0.03);
    tdp::Depth2PCsGpu(cuDPyr,camD,pcs_c);
    // compute normals
    tdp::Depth2Normals(cuDPyr,camD,ns_c);
    TOCK("Setup Pyramids");

//    if (icpImu && imu)
//      T_mo = (T_wr_imu * T_wr_imu_prev.Inverse()) * T_mo;

    if (runICP && (!runFusion || numFused > 30)) {
      if (gui.verbose) std::cout << "icp" << std::endl;
      TICK("ICP");
      std::vector<size_t> maxIt{icpIter0,icpIter1,icpIter2};
      tdp::ICP::ComputeProjective(pcs_m, ns_m, pcs_c, ns_c, T_mo, tdp::SE3f(),
          camD, maxIt, icpAngleThr_deg, icpDistThr, gui.verbose);
      TOCK("ICP");

//      std::cout << "T_mo update: " << std::endl
//        << T_mo * T_mo_prev.Inverse() << std::endl;
//      std::cout << "IMU : " << std::endl
//        << T_wr_imu * T_wr_imu_prev.Inverse() << std::endl;
    }
    std::cout << "T_mo after ICP: " << std::endl
      << T_mo  << std::endl;

    if (runSAM && numFused > 30) {

      correction_count++;
      if (samIMU) {
        // Adding IMU factor and GPS factor and optimizing.
#ifdef USE_COMBINED
        PreintegratedCombinedMeasurements *preint_imu_combined = dynamic_cast<PreintegratedCombinedMeasurements*>(imu_preintegrated_);
        CombinedImuFactor imu_factor(X(correction_count-1), V(correction_count-1),
            X(correction_count  ), V(correction_count  ),
            B(correction_count-1), B(correction_count  ),
            *preint_imu_combined);
        graph->add(imu_factor);
#else
        PreintegratedImuMeasurements *preint_imu = dynamic_cast<PreintegratedImuMeasurements*>(imu_preintegrated_);
        ImuFactor imu_factor(X(correction_count-1), V(correction_count-1),
            X(correction_count  ), V(correction_count  ),
            B(correction_count-1),
            *preint_imu);
        graph->add(imu_factor);
        imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
        graph->add(BetweenFactor<imuBias::ConstantBias>(B(correction_count-1),
              B(correction_count  ),
              zero_bias, bias_noise_model));

        preint_imu->print();
#endif
      }

      noiseModel::Diagonal::shared_ptr correction_noise = noiseModel::Isotropic::Sigma(6,0.01);

      tdp::SE3f dT_icp = T_mo * T_mo_prev.Inverse();

      std::cout << "adding ICP factor "
        << std::endl
        << dT_icp.matrix3x4() << std::endl;
      BetweenFactor<Pose3> icp_factor(X(correction_count-1),
          X(correction_count),
          Pose3(dT_icp.matrix().cast<double>()),
          correction_noise);
//      GPSFactor gps_factor(X(correction_count),
//                           Point3(gps(0),  // N,
//                                  gps(1),  // E,
//                                  gps(2)), // D,
//                           correction_noise);
//      graph->add(gps_factor);
      graph->add(icp_factor);

      if (samIMU) {
        // Now optimize and compare results.
        prop_state = imu_preintegrated_->predict(prev_state, prev_bias);
        initial_values.insert(X(correction_count), prop_state.pose());
        initial_values.insert(V(correction_count), prop_state.v());
        initial_values.insert(B(correction_count), prev_bias);
      } else {
        initial_values.insert(X(correction_count), Pose3(T_mo.matrix().cast<double>()));
        initial_values.insert(V(correction_count), Vector3(0.,0.,0.));
        initial_values.insert(B(correction_count), prev_bias);
      }

      std::cout << "now optimizing" << std::endl;
      LevenbergMarquardtOptimizer optimizer(*graph, initial_values);
      Values result = optimizer.optimize();

      if (samIMU) {
        // Overwrite the beginning of the preintegration for the next step.
        prev_state = NavState(result.at<Pose3>(X(correction_count)),
            result.at<Vector3>(V(correction_count)));
        prev_bias = result.at<imuBias::ConstantBias>(B(correction_count));
        // Reset the preintegration object.
        imu_preintegrated_->resetIntegrationAndSetBias(prev_bias);
      }

      // Print out the position and orientation error for comparison.
      T_mo_isam = tdp::SE3f(prev_state.pose().matrix().cast<float>());

      std::cout << "ISAM "
        << std::endl
        << T_mo_isam.matrix3x4() << std::endl;

      std::ofstream out("./graph.viz");
      graph->saveGraph(out);
      out.close();
    }

    if (runFusion && (fuseTSDF || numFused <= 30)) {
      if (gui.verbose) std::cout << "add to tsdf" << std::endl;
      TICK("Add To TSDF");
      tdp::TSDF::AddToTSDF(cuTSDF, cuD, T_mo, camD, grid0, dGrid,
          tsdfMu, tsdfWMax);
      numFused ++;
      TOCK("Add To TSDF");
    }

    if (runFusion) {
      if (gui.verbose) std::cout << "ray trace" << std::endl;
      TICK("Ray Trace TSDF");
      tdp::TSDF::RayTraceTSDF(cuTSDF, pcs_m.GetImage(0),
          ns_m.GetImage(0), T_mo, camD, grid0, dGrid, tsdfMu, tsdfWThr);
      // get pc in model coordinate system
      tdp::CompletePyramid<tdp::Vector3fda,3>(pcs_m);
      TOCK("Ray Trace TSDF");
      tdp::CompleteNormalPyramid<3>(ns_m);
    }

    if (pangolin::Pushed(resetTSDF)) {
      TSDF.Fill(tdp::TSDFval(-1.01,0.));
      dEst.Fill(0.);
      cuTSDF.CopyFrom(TSDF);
      numFused = 0;
      T_mo = T_mo_0;
      T_mo_prev = T_mo;
      std::cout << "resetting ICP and TSDF" << std::endl;
    }
    if (pangolin::Pushed(resetICP)) {
      T_mo = T_mo_0;
      T_mo_prev = T_mo;
      std::cout << "resetting ICP" << std::endl;
    }

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

    TICK("Draw 3D");
    // Render point cloud from viewpoint of origin
    tdp::SE3f T_mv;
    tdp::TSDF::RayTraceTSDF(cuTSDF, cuDView, nEstdummy, T_mv, camView, grid0,
        dGrid, tsdfMu, tsdfWThr);
    tdp::Depth2PCGpu(cuDView,camView,cuPcView);

    glEnable(GL_DEPTH_TEST);
    viewPc3D.Activate(s_cam);

    Eigen::AlignedBox3f box(grid0,gridE);
    glColor4f(1,0,0,0.5f);
    pangolin::glDrawAlignedBox(box);

    // imu
//    pangolin::glSetFrameOfReference(T_wr_imu.matrix());
//    pangolin::glDrawAxis(0.2f);
//    pangolin::glUnsetFrameOfReference();

    // render global view of the model first
    pangolin::glDrawAxis(0.1f);
    if (showPcView) {
      {
        pangolin::CudaScopedMappedPtr cuPcbufp(cuPcbuf);
        cudaMemset(*cuPcbufp,0,hc*wc*sizeof(tdp::Vector3fda));
        cudaMemcpy(*cuPcbufp, cuPcView.ptr_, cuPcView.SizeBytes(),
            cudaMemcpyDeviceToDevice);
      }
      glColor3f(0,0,1);
      pangolin::RenderVbo(cuPcbuf);
    }
    // render model and observed point cloud
    if (showPcModel) {
      {
        pangolin::CudaScopedMappedPtr cuPcbufp(cuPcbuf);
        cudaMemset(*cuPcbufp,0,hc*wc*sizeof(tdp::Vector3fda));
        tdp::Image<tdp::Vector3fda> pc0 = pcs_m.GetImage(icpErrorLvl);
        cudaMemcpy(*cuPcbufp, pc0.ptr_, pc0.SizeBytes(),
            cudaMemcpyDeviceToDevice);
      }
      glColor3f(0,1,0);
      pangolin::RenderVbo(cuPcbuf);
    }

    pangolin::glDrawAxis(T_mo_isam.matrix(), 0.15f);

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

    viewN3D.Activate(s_cam);
    // render global view of the model first
    pangolin::glDrawAxis(0.1f);
    if (showPcView) {
      {
        pangolin::CudaScopedMappedPtr cuPcbufp(cuPcbuf);
        cudaMemset(*cuPcbufp,0,hc*wc*sizeof(tdp::Vector3fda));
        cudaMemcpy(*cuPcbufp, nEstdummy.ptr_, nEstdummy.SizeBytes(),
            cudaMemcpyDeviceToDevice);
      }
      glColor3f(0,0,1);
      pangolin::RenderVbo(cuPcbuf);
    }
    // render model and observed point cloud
    if (showPcModel) {
      {
        pangolin::CudaScopedMappedPtr cuPcbufp(cuPcbuf);
        cudaMemset(*cuPcbufp,0,hc*wc*sizeof(tdp::Vector3fda));
        tdp::Image<tdp::Vector3fda> pc0 = ns_m.GetImage(icpErrorLvl);
        cudaMemcpy(*cuPcbufp, pc0.ptr_, pc0.SizeBytes(),
            cudaMemcpyDeviceToDevice);
      }
      glColor3f(0,1,0);
      pangolin::RenderVbo(cuPcbuf);
    }
    Eigen::Matrix4f R_mo = T_mo.matrix();
    R_mo.topRightCorner(3,1).fill(0.);
    pangolin::glSetFrameOfReference(R_mo);
    // render current camera second in the propper frame of
    // reference
    pangolin::glDrawAxis(0.1f);
    if (showPcCurrent) {
      {
        pangolin::CudaScopedMappedPtr cuPcbufp(cuPcbuf);
        cudaMemset(*cuPcbufp,0,hc*wc*sizeof(tdp::Vector3fda));
        tdp::Image<tdp::Vector3fda> pc0 = ns_c.GetImage(icpErrorLvl);
        cudaMemcpy(*cuPcbufp, pc0.ptr_, pc0.SizeBytes(),
            cudaMemcpyDeviceToDevice);
      }
      glColor3f(1,0,0);
      pangolin::RenderVbo(cuPcbuf);
    }
    pangolin::glUnsetFrameOfReference();
    TOCK("Draw 3D");

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

    tsdfDEst.CopyFrom(cuDEst);
    viewTsdfDEst.SetImage(tsdfDEst);

    tdp::Image<tdp::TSDFval> cuTsdfSlice =
      cuTSDF.GetImage(std::min((int)cuTSDF.d_-1,tsdfSliceD.Get()));
    tdp::ManagedHostImage<tdp::TSDFval> tsdfSliceRaw(cuTsdfSlice.w_,
        cuTsdfSlice.h_);
    tsdfSliceRaw.CopyFrom(cuTsdfSlice);
    for (size_t i=0; i<tsdfSliceRaw.Area(); ++i)
      tsdfSlice[i] = tsdfSliceRaw[i].f;
    viewTsdfSliveView.SetImage(tsdfSlice);

    tdp::PyramidToImage<float,3>(cuDPyr,dispDepthPyr);
    viewDepthPyr.SetImage(dispDepthPyr);

    if (dispNormalsPyrEst) {
      tdp::PyramidToImage<tdp::Vector3fda,3>(ns_m,cuDispNormalsPyr);
    } else {
      tdp::PyramidToImage<tdp::Vector3fda,3>(ns_c,cuDispNormalsPyr);
    }
    tdp::Normals2Image(cuDispNormalsPyr, cuDispNormals2dPyr);
    dispNormals2dPyr.CopyFrom(cuDispNormals2dPyr);
    viewNormalsPyr.SetImage(dispNormals2dPyr);

    TOCK("Draw 2D");

    if (!runFusion) {
      tdp::SO3fda R_mo = T_mo.rotation();
      for (size_t lvl=0; lvl<3; ++lvl) {
        tdp::Image<tdp::Vector3fda> pc = pcs_c.GetImage(lvl);
        tdp::Image<tdp::Vector3fda> n = ns_c.GetImage(lvl);
        tdp::TransformPc(T_mo, pc);
        tdp::TransformPc(R_mo, n);
      }
      pcs_m.CopyFrom(pcs_c);
      ns_m.CopyFrom(ns_c);
    }
    if (!gui.paused()) {
//      T_wr_imu_prev = T_wr_imu;
      T_mo_prev = T_mo;
    }

    // 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();
  }
  runWorker.Set(false);
  workThread.join();
  receiveImu.Set(false);
  receiverThread.join();
  if (imu) imu->Stop();
  delete imu;
  return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TRIGONOMETRIC_FUNCTIONS_SIMD_COMMON_ATAN2_HPP_INCLUDED
#define NT2_TRIGONOMETRIC_FUNCTIONS_SIMD_COMMON_ATAN2_HPP_INCLUDED

#include <nt2/trigonometric/functions/atan2.hpp>
#include <nt2/trigonometric/functions/simd/common/impl/invtrig.hpp>
#include <nt2/include/functions/simd/tofloat.hpp>
#include <nt2/include/functions/simd/atan.hpp>
#include <nt2/include/functions/simd/divides.hpp>
#include <nt2/include/functions/simd/multiplies.hpp>
#include <nt2/include/functions/simd/minus.hpp>
#include <nt2/include/functions/simd/if_else.hpp>
#include <nt2/include/functions/simd/if_allbits_else.hpp>
#include <nt2/include/functions/simd/is_eqz.hpp>
#include <nt2/include/functions/simd/is_inf.hpp>
#include <nt2/include/functions/simd/is_nan.hpp>
#include <nt2/include/functions/simd/logical_and.hpp>
#include <nt2/include/functions/simd/copysign.hpp>
#include <nt2/include/functions/simd/signnz.hpp>
#include <nt2/include/functions/simd/is_gtz.hpp>
#include <nt2/include/functions/simd/abs.hpp>
#include <nt2/include/functions/simd/is_ltz.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/sdk/meta/as_floating.hpp>
#include <boost/simd/sdk/config.hpp>

namespace nt2 { namespace ext
{
  namespace impl = nt2::details::internal;
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::atan2_, boost::simd::tag::simd_
                            , (A0)(X)
                            , ((simd_<arithmetic_<A0>,X>))
                              ((simd_<arithmetic_<A0>,X>))
                            )
  {
    typedef typename meta::as_floating<A0>::type result_type;
    NT2_FUNCTOR_CALL_REPEAT(2)
    {
      return nt2::atan2(nt2::tofloat(a0), nt2::tofloat(a1));
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::atan2_, boost::simd::tag::simd_
                            , (A0)(X)
                            , ((simd_<uint_<A0>,X>))
                              ((simd_<uint_<A0>,X>))
                            )
  {

    typedef typename meta::as_floating<A0>::type result_type;
    inline result_type operator()(const typename A0::native_type a0_n,
                                  const typename A0::native_type a1_n) const
    {
      const A0 a0 = a0_n;
      const A0 a1 = a1_n;
      result_type z = nt2::atan(tofloat(a0)/nt2::tofloat(a1));
      return nt2::if_zero_else(nt2::is_eqz(a0), z);
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::atan2_, boost::simd::tag::simd_
                            , (A0)(X)
                            , ((simd_<floating_<A0>,X>))
                              ((simd_<floating_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    inline result_type operator()(const typename A0::native_type a0_n,
                                  const typename A0::native_type a1_n) const
    {
      A0 a0 = a0_n;
      A0 a1 = a1_n;
      typedef typename meta::as_logical<A0>::type lA0;
      lA0 test =  nt2::logical_and(nt2::is_inf(a0),  nt2::is_inf(a1));
      a0 =  nt2::if_else(test, nt2::copysign(One<A0>(), a0), a0);
      a1 =  nt2::if_else(test, nt2::copysign(One<A0>(), a1), a1);
      A0 z = impl::invtrig_base<result_type,radian_tag, tag::simd_type>::kernel_atan(a0/a1);
      //A0 z = atan(abs(a0/a1));  // case a1 > 0,  a0 > 0
      z = nt2::if_else(nt2::is_gtz(a1), z, nt2::Pi<A0>()-z)*nt2::signnz(a0);
      z =  nt2::if_else( nt2::is_eqz(a0),
                         nt2::if_else_zero( nt2::is_ltz(a1),  nt2::Pi<A0>()),
                         z);
#ifdef BOOST_SIMD_NO_NANS
      return z;
#else
      return  nt2::if_nan_else( nt2::logical_or( nt2::is_nan(a0),  nt2::is_nan(a1)), z);
#endif
    }
  };
} }

#endif


#include <cuda.h>
#include <cuda_runtime_api.h>
#include <blitz.h>

#include "blitz_cudnn.h"

using namespace blitz;

// algorithms for forward and backwards convolutions
cudnnHandle_t cudnn_handle;
cudnnConvolutionFwdAlgo_t forward_algorithm;
cudnnConvolutionBwdFilterAlgo_t backward_filter_algorithm;
cudnnConvolutionBwdDataAlgo_t backward_data_algorithm;
cudnnTensorDescriptor_t input_desc, output_desc;
cudnnFilterDescriptor_t filter_desc;
cudnnConvolutionDescriptor_t conv_desc;
float *cudnn_alpha, *cudnn_beta;
// N C H W
Shape input_shape(4);
// K C R S
Shape filter_shape(4);
// N K P Q
Shape output_shape(4);
// cpu workspace
Shape workspace_shape_cpu(1);
// gpu workspace
Shape workspace_shape_gpu(1);
// init timer
float elapsed_time_gpu;
CUevent event_start, event_stop;

void set_forward_algorithm(const string& name) {
  if (name == "direct") {
    forward_algorithm = CUDNN_CONVOLUTION_FWD_ALGO_DIRECT;
  } else if (name == "fft_tiling") {
    forward_algorithm = CUDNN_CONVOLUTION_FWD_ALGO_FFT_TILING;
  } else if (name == "fft") {
    forward_algorithm = CUDNN_CONVOLUTION_FWD_ALGO_FFT;
  } else if (name == "gemm_pre") {
    forward_algorithm = CUDNN_CONVOLUTION_FWD_ALGO_IMPLICIT_PRECOMP_GEMM;
  } else if (name == "gemm_implicit") {
    forward_algorithm = CUDNN_CONVOLUTION_FWD_ALGO_IMPLICIT_GEMM;
  } else if (name == "gemm") {
    forward_algorithm = CUDNN_CONVOLUTION_FWD_ALGO_GEMM;
  } else if (name == "winograd") {
    forward_algorithm = CUDNN_CONVOLUTION_FWD_ALGO_WINOGRAD;
  }
}

void parse_forward_algorithm() {
  switch (forward_algorithm) {
    case CUDNN_CONVOLUTION_FWD_ALGO_IMPLICIT_GEMM:
      std::cout << "Implicit GEMM" << std::endl;
      break;
    case CUDNN_CONVOLUTION_FWD_ALGO_IMPLICIT_PRECOMP_GEMM:
      std::cout << "Implicit precomputed GEMM" << std::endl;
      break;
    case CUDNN_CONVOLUTION_FWD_ALGO_GEMM:
      std::cout << "Explicit GEMM" << std::endl;
      break;
    case CUDNN_CONVOLUTION_FWD_ALGO_DIRECT:
      std::cout << "Direct" << std::endl;
      break;
    case CUDNN_CONVOLUTION_FWD_ALGO_FFT:
      std::cout << "FFT" << std::endl;
      break;
    case CUDNN_CONVOLUTION_FWD_ALGO_FFT_TILING:
      std::cout << "FFT tiling" << std::endl;
      break;
    case CUDNN_CONVOLUTION_FWD_ALGO_WINOGRAD:
      std::cout << "Winograd" << std::endl;
      break;
    default:
      break;
  }
}

void set_backward_data_algorithm(const string& name) {
  if (name == "fft_tiling") {
    backward_data_algorithm = CUDNN_CONVOLUTION_BWD_DATA_ALGO_FFT_TILING;
  } else if (name == "fft") {
    backward_data_algorithm = CUDNN_CONVOLUTION_BWD_DATA_ALGO_FFT;
  } else if (name == "gemm_deter") {
    backward_data_algorithm = CUDNN_CONVOLUTION_BWD_DATA_ALGO_1;
  } else if (name == "gemm_non_deter") {
    backward_data_algorithm = CUDNN_CONVOLUTION_BWD_DATA_ALGO_0;
  } else if (name == "winograd") {
    backward_data_algorithm = CUDNN_CONVOLUTION_BWD_DATA_ALGO_WINOGRAD;
  }
}

void parse_backward_data_algorithm() {
  switch (backward_data_algorithm) {
    case CUDNN_CONVOLUTION_BWD_DATA_ALGO_0:
      std::cout << "GEMM non deteriministic" << std::endl;
      break;
    case CUDNN_CONVOLUTION_BWD_DATA_ALGO_1:
      std::cout << "GEMM deteriministic" << std::endl;
      break;
    case CUDNN_CONVOLUTION_BWD_DATA_ALGO_FFT:
      std::cout << "FFT" << std::endl;
      break;
    case CUDNN_CONVOLUTION_BWD_DATA_ALGO_FFT_TILING:
      std::cout << "FFT tiling" << std::endl;
      break;
    case CUDNN_CONVOLUTION_BWD_DATA_ALGO_WINOGRAD:
      std::cout << "Winograd" << std::endl;
      break;
    default:
      break;
  }
}

void set_input_shape_nchw(size_t N, size_t C, size_t H, size_t W) {
  input_shape[0] = N;
  input_shape[1] = C;
  input_shape[2] = H;
  input_shape[3] = W;
}

void set_filter_shape_kcrs(size_t K, size_t C, size_t R, size_t S) {
  filter_shape[0] = K;
  filter_shape[1] = C;
  filter_shape[2] = R;
  filter_shape[3] = S;
}

void set_output_shape_nkpq(size_t N, size_t K, size_t P, size_t Q) {
  output_shape[0] = N;
  output_shape[1] = K;
  output_shape[2] = P;
  output_shape[3] = Q;
}

void convolution_backward(const string& kernel, size_t iter) {
  // set up gpu
  GPUTensor<float> input_gpu(input_shape);
  GPUTensor<float> filter_gpu(filter_shape);
  GPUTensor<float> output_gpu(output_shape);
  size_t workspace_size = 0;
  // set algorithms
  if (kernel == "auto") {  // fastest
    CUDNN_CHECK(cudnnGetConvolutionBackwardDataAlgorithm(cudnn_handle,
      filter_desc,
      output_desc,
      conv_desc,
      input_desc,
      CUDNN_CONVOLUTION_BWD_DATA_PREFER_FASTEST,
      2e9,  // memoryLimitInBytes,
      &backward_data_algorithm));
  }  else {
    set_backward_data_algorithm(kernel);
  }
  parse_backward_data_algorithm();
  // config workspace
  CUDNN_CHECK(cudnnGetConvolutionBackwardDataWorkspaceSize(cudnn_handle,
    filter_desc,
    output_desc,
    conv_desc,
    input_desc,
    backward_data_algorithm,
    &workspace_size));
  std::cout << "workspace_size: " << workspace_size << std::endl;
  Shape workspace_shape(1);
  workspace_shape[0] = workspace_size;
  GPUTensor<float> workspace_gpu(workspace_shape);
  CUDNN_CHECK(cudnnConvolutionBackwardData(cudnn_handle,
    reinterpret_cast<void*>(cudnn_alpha),
    filter_desc,
    filter_gpu.data(),
    output_desc,
    output_gpu.data(),
    conv_desc,
    backward_data_algorithm,
    workspace_gpu.data(),
    workspace_size,
    reinterpret_cast<void*>(cudnn_beta),
    input_desc,
    input_gpu.data()));
  BLITZ_GPU_TIMER_START(elapsed_time_gpu, event_start, event_stop);
  for (size_t i = 1; i < iter; ++i) {
    CUDNN_CHECK(cudnnConvolutionBackwardData(cudnn_handle,
      reinterpret_cast<void*>(cudnn_alpha),
      filter_desc,
      filter_gpu.data(),
      output_desc,
      output_gpu.data(),
      conv_desc,
      backward_data_algorithm,
      workspace_gpu.data(),
      workspace_size,
      reinterpret_cast<void*>(cudnn_beta),
      input_desc,
      input_gpu.data()));
  }
  BLITZ_GPU_TIMER_END(elapsed_time_gpu, event_start, event_stop);
  std::cout << (iter - 1) * 2 * filter_shape.size() * output_shape[0] * output_shape[2] * output_shape[3] << std::endl;
  BLITZ_GPU_TIMER_INFO((iter - 1) * 2 * filter_shape.size() * output_shape[0] * output_shape[2] * output_shape[3], elapsed_time_gpu);
}

void convolution_update(const string& kernel, size_t iter) {
}

void convolution_forward(const string& kernel, size_t iter) {
  // set up gpu
  GPUTensor<float> input_gpu(input_shape);
  GPUTensor<float> filter_gpu(filter_shape);
  GPUTensor<float> output_gpu(output_shape);
  size_t workspace_size = 0;
  // set algorithms
  if (kernel == "auto") {  // fastest
    CUDNN_CHECK(cudnnGetConvolutionForwardAlgorithm(cudnn_handle,
      input_desc,
      filter_desc,
      conv_desc,
      output_desc,
      CUDNN_CONVOLUTION_FWD_PREFER_FASTEST,
      2e9,  // memoryLimitInBytes,
      &forward_algorithm));
  }  else {
    set_forward_algorithm(kernel);
  }
  parse_forward_algorithm();
  // config workspace
  CUDNN_CHECK(cudnnGetConvolutionForwardWorkspaceSize(cudnn_handle,
    input_desc,
    filter_desc,
    conv_desc,
    output_desc,
    forward_algorithm,
    &workspace_size));
  std::cout << "workspace_size: " << workspace_size << std::endl;
  Shape workspace_shape(1);
  workspace_shape[0] = workspace_size;
  GPUTensor<float> workspace_gpu(workspace_shape);
  CUDNN_CHECK(cudnnConvolutionForward(cudnn_handle,
    reinterpret_cast<void*>(cudnn_alpha),
    input_desc,
    input_gpu.data(),
    filter_desc,
    filter_gpu.data(),
    conv_desc,
    forward_algorithm,
    workspace_gpu.data(),
    workspace_size,
    reinterpret_cast<void*>(cudnn_beta),
    output_desc,
    output_gpu.data()));
  BLITZ_GPU_TIMER_START(elapsed_time_gpu, event_start, event_stop);
  for (size_t i = 1; i < iter; ++i) {
    CUDNN_CHECK(cudnnConvolutionForward(cudnn_handle,
      reinterpret_cast<void*>(cudnn_alpha),
      input_desc,
      input_gpu.data(),
      filter_desc,
      filter_gpu.data(),
      conv_desc,
      forward_algorithm,
      workspace_gpu.data(),
      workspace_size,
      reinterpret_cast<void*>(cudnn_beta),
      output_desc,
      output_gpu.data()));
  }
  BLITZ_GPU_TIMER_END(elapsed_time_gpu, event_start, event_stop);
  BLITZ_GPU_TIMER_INFO((iter - 1) * 2 * filter_shape.size() * output_shape[0] * output_shape[2] * output_shape[3], elapsed_time_gpu);
}

int main(int argc, char** argv) {
  const size_t NUM_ARGS = 16;
  // phase kernel N C H W R S K P Q pad_h pad_w str_h str_w
  if (argc != NUM_ARGS + 1) {
    std::cerr << "Not enough args!" << std::endl;
    exit(1);
  }
  // get args
  const std::string phase = std::string(argv[1]);
  const std::string kernel = std::string(argv[2]);
  const size_t N = atoi(argv[3]);
  const size_t C = atoi(argv[4]);
  const size_t H = atoi(argv[5]);
  const size_t W = atoi(argv[6]);
  const size_t R = atoi(argv[7]);
  const size_t S = atoi(argv[8]);
  const size_t K = atoi(argv[9]);
  const size_t P = atoi(argv[10]);
  const size_t Q = atoi(argv[11]);
  const size_t pad_h = atoi(argv[12]);
  const size_t pad_w = atoi(argv[13]);
  const size_t str_h = atoi(argv[14]);
  const size_t str_w = atoi(argv[15]);
  const size_t iterations = atoi(argv[16]);
  // set shapes
  set_input_shape_nchw(N, C, H, W);
  set_filter_shape_kcrs(K, C, R, S);
  set_output_shape_nkpq(N, K, P, Q);
  // create val
  cudnn_alpha = new float(1.0);
  cudnn_beta = new float(0.0);
  // create handle
  cudnnCreate(&cudnn_handle);
  // create descriptors
  createTensor4dDesc<float>(&input_desc);
  createTensor4dDesc<float>(&output_desc);
  createFilterDesc<float>(&filter_desc);
  createConvolution2DDesc<float>(&conv_desc);
  // set descriptors
  setTensor4dNHWCDesc<float>(&input_desc, N, C, H, W);
  setTensor4dNHWCDesc<float>(&output_desc, N, K, P, Q);
  setFilterDesc<float>(&filter_desc, K, C, R, S);
  setConvolution2DDesc<float>(&conv_desc, pad_h, pad_w, str_h, str_w);
  // run convolution
  if (phase == "forward") {
    convolution_forward(kernel, iterations);
  } else if (phase == "backward") {
    convolution_backward(kernel, iterations);
  } else if (phase == "update") {
    convolution_update(kernel, iterations);
  }
  return 0;
}


/* C program for soundscape generation. (C) P.B.L. Meijer 1996 */
/* hificode.c modified for camera input using OpenCV. (C) 2013 */
/* Last update: March 19, 2016; released under the Creative    */
/* Commons Attribution 4.0 International License (CC BY 4.0),  */
/* see http://www.seeingwithsound.com/im2sound.htm for details */

#define PY_ARRAY_UNIQUE_SYMBOL soundvision_c_ARRAY_API

#include <boost/python.hpp>
#include <pyboostcvconverter/pyboostcvconverter.hpp>

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include <opencv2/opencv.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/imgcodecs/imgcodecs.hpp>

#define FNAME "/Users/admin/UBC/CPEN391/project2/tracking/hificode.wav"    /* User-defined parameters   */
#define FL     500   /* Lowest  frequency (Hz) in soundscape   */
#define FH    5000   /* Highest frequency (Hz)                 */
#define FS   44100   /* Sample  frequency (Hz)                 */
#define T     1.05   /* Image to sound conversion time (s)     */
#define D        1   /* Linear|Exponential=0|1 distribution    */
#define HIFI     1   /* 8-bit|16-bit=0|1 sound quality         */
#define STEREO   1   /* Mono|Stereo=0|1 sound selection        */
#define DELAY    1   /* Nodelay|Delay=0|1 model   (STEREO=1)   */
#define FADE     1   /* Relative fade No|Yes=0|1  (STEREO=1)   */
#define DIFFR    1   /* Diffraction No|Yes=0|1    (STEREO=1)   */
#define BSPL     1   /* Rectangular|B-spline=0|1 time window   */
#define BW       0   /* 16|2-level=0|1 gray format in *P[]     */
#define CAM      1   /* Use OpenCV camera input No|Yes=0|1     */
#define VIEW     1   /* Screen view for debugging No|Yes=0|1   */
#define CONTRAST 2   /* Contrast enhancement, 0=none           */

#define C_ALLOC(number, type) ((type *) calloc((number),sizeof(type)) )
#define TwoPi 6.283185307179586476925287
#define HIST  (1+HIFI)*(1+STEREO)
#define WHITE 1.00
#define BLACK 0.00

/* Soundscape resolution M rows x N columns */
#if CAM
/* 176 x 64 for live camera view */
#define M     64
#define N    176
#else
/* 64 x 64 for hard-coded image */
#define M     64
#define N     64
#endif

FILE *fp;
unsigned long ir=0L, ia=9301L, ic=49297L, im=233280L;
void wi(unsigned int i) { int b1,b0; b0=i%256; b1=(i-b0)/256; putc(b0,fp); putc(b1,fp); }
void wl(long l) { unsigned int i1,i0; i0=l%65536L; i1=(l-i0)/65536L; wi(i0); wi(i1); }
double rnd(void){ ir = (ir*ia+ic) % im; return ir / (1.0*im); }

namespace soundvision_c {

   using namespace boost::python;

   int playSound(char *filename) {  /* Play soundscape */
      char command[256] = "";
      sprintf(command, "afplay %s", filename);
      int status = system(command);
      return status;
   }

   // PyObject *readCamera(){
   //    cap.open(0);
   //    if (!cap.isOpened()) {
   //       fprintf(stderr,"Could not open camera %d\n", cam_id);
   //       return 1;
   //    }
   //    /* Setting standard capture size, may fail; resize later */
   //    cap.read(frame);   Dummy read needed with some devices
   //    cap.set(CV_CAP_PROP_FRAME_WIDTH , 176);
   //    cap.set(CV_CAP_PROP_FRAME_HEIGHT, 144);

   //    if (VIEW) {  /* Screen views only for debugging */
   //       namedWindow("Large", CV_WINDOW_AUTOSIZE);
   //       namedWindow("Small", CV_WINDOW_AUTOSIZE);
   //    }
   // }

   int i, j, d=D, ss, key=0;
   long k=0L, l, ns=2L*(long)(0.5*FS*T), m=ns/N,
      sso=HIFI?0L:128L, ssm=HIFI?32768L:128L;
   double a, t, dt=1.0/FS, s, y, yp, z, tau1, tau2, x, theta,
      scale=0.5/sqrt((double)M), q, q2, r, sl, sr, tl, tr, yl, ypl, yr, ypr,
      zl, zr, hrtf, hrtfl, hrtfr, v=340.0,  /* v = speed of sound (m/s) */
      hs=0.20;  /* hs = characteristic acoustical size of head (m) */
   double *w    = C_ALLOC(M, double);
   double *phi0 = C_ALLOC(M, double);
   double **A = C_ALLOC(M, double *);

   void setup(){
      for (i=0; i<M; i++) {
         A[i] = C_ALLOC(N, double);  /* M x N pixel matrix */
      }

      /* Set lin|exp (0|1) frequency distribution and random initial phase */
      if (d) {
         for (i=0; i<M; i++) {
            w[i] = TwoPi * FL * pow(1.0* FH/FL,1.0*i/(M-1));
         }
      } else {
         for (i=0; i<M; i++) {
            w[i] = TwoPi * FL + TwoPi * (FH-FL) * i/(M-1);
         }
      }

      for (i=0; i<M; i++){
         phi0[i] = TwoPi * rnd();
      }
   }

   void process_frame(PyObject *frameNDArray) {
      cv::Mat frame, gray;
      frame = soundvision_c::fromNDArrayToMat(frameNDArray);
      cv::Mat tmp;
      cv::cvtColor(frame,tmp,CV_BGR2GRAY);
      if (frame.rows != M || frame.cols != N) cv::resize(tmp, gray, cv::Size(N,M)); else gray=tmp;

      if (CAM) {  /* Set live camera image */
         int px;
         double avg;
         avg = 0.0;
         for (i=0; i<M; i++) {
            for (j=0; j<N; j++) {
               avg += gray.at<uchar>(M-1-i,j);
            }
         }
         avg = avg / (N * M);
         for (i=0; i<M; i++) {
            for (j=0; j<N; j++) {
               px = gray.at<uchar>(M-1-i,j);
               px += CONTRAST*(px - avg);
               if (px > 255) px = 255;
               if (px <   0) px =   0;
               gray.at<uchar>(M-1-i,j) = px;
               if (px == 0) A[i][j] = 0;
               else A[i][j] = pow(10.0,(px/16-15)/10.0);  /* 2dB steps */
            }
         }
      }

      if (VIEW) {  /* Screen views only for debugging */
      /* imwrite("hificodeLarge.jpg", frame); */
         cv::imshow("Large", frame);
         cv::moveWindow("Large", 20, 20);
      /* imwrite("hificodeSmall.jpg", gray); */
         cv::imshow("Small", gray);
         cv::moveWindow("Small", 220, 20);
      }

      key = cv::waitKey((int)(10));

      /* Write 8/16-bit mono/stereo .wav file */
      fp = fopen(FNAME,"wb"); fprintf(fp,"RIFF"); wl(ns*HIST+36L);
      fprintf(fp,"WAVEfmt "); wl(16L); wi(1); wi(STEREO?2:1); wl(0L+FS);
      wl(0L+FS*HIST); wi(HIST); wi(HIFI?16:8); fprintf(fp,"data"); wl(ns*HIST);
      tau1 = 0.5 / w[M-1]; tau2 = 0.25 * tau1*tau1;
      y = yl = yr = z = zl = zr = 0.0;
      /* Not optimized for speed */
      while (k < ns && !STEREO) {
         if (BSPL) { q = 1.0 * (k%m)/(m-1); q2 = 0.5*q*q; }
         j = k / m; if (j>N-1) j=N-1; s = 0.0; t = k * dt;
         if (k < ns/(5*N)) s = (2.0*rnd()-1.0) / scale;  /* "click" */
         else for (i=0; i<M; i++) {
            if (BSPL) {  /* Quadratic B-spline for smooth C1 time window */
               if (j==0) a = (1.0-q2)*A[i][j]+q2*A[i][j+1];
               else if (j==N-1) a = (q2-q+0.5)*A[i][j-1]+(0.5+q-q2)*A[i][j];
               else a = (q2-q+0.5)*A[i][j-1]+(0.5+q-q*q)*A[i][j]+q2*A[i][j+1];
            }
            else a = A[i][j];  /* Rectangular time window */
            s += a * sin(w[i] * t + phi0[i]);
         }
         yp = y; y = tau1/dt + tau2/(dt*dt);
         y  = (s + y * yp + tau2/dt * z) / (1.0 + y); z = (y - yp) / dt;
         l  = sso + 0.5 + scale * ssm * y; /* y = 2nd order filtered s */
         if (l >= sso-1+ssm) l = sso-1+ssm; if (l < sso-ssm) l = sso-ssm;
         ss = (unsigned int) l;
         if (HIFI) wi(ss); else putc(ss,fp);
         k++;
      }
      while (k < ns && STEREO) {
         if (BSPL) { q = 1.0 * (k%m)/(m-1); q2 = 0.5*q*q; }
         j = k / m; if (j>N-1) j=N-1;
         r = 1.0 * k/(ns-1);  /* Binaural attenuation/delay parameter */
         theta = (r-0.5) * TwoPi/3; x = 0.5 * hs * (theta + sin(theta));
         tl = tr = k * dt; if (DELAY) tr += x / v; /* Time delay model */
         x  = fabs(x); sl = sr = 0.0; hrtfl = hrtfr = 1.0;
         for (i=0; i<M; i++) {
            if (DIFFR) {
               /* First order frequency-dependent azimuth diffraction model */
               if (TwoPi*v/w[i] > x) hrtf = 1.0; else hrtf = TwoPi*v/(x*w[i]);
               if (theta < 0.0) { hrtfl =  1.0; hrtfr = hrtf; }
               else             { hrtfl = hrtf; hrtfr =  1.0; }
            }
            if (FADE) {
               /* Simple frequency-independent relative fade model */
               hrtfl *= (1.0-0.7*r);
               hrtfr *= (0.3+0.7*r);
            }
            if (BSPL) {
               if (j==0) a = (1.0-q2)*A[i][j]+q2*A[i][j+1];
               else if (j==N-1) a = (q2-q+0.5)*A[i][j-1]+(0.5+q-q2)*A[i][j];
               else a = (q2-q+0.5)*A[i][j-1]+(0.5+q-q*q)*A[i][j]+q2*A[i][j+1];
            }
            else a = A[i][j];
            sl += hrtfl * a * sin(w[i] * tl + phi0[i]);
            sr += hrtfr * a * sin(w[i] * tr + phi0[i]);
         }
         if (k < ns/(5*N)) sl = (2.0*rnd()-1.0) / scale;  /* Left "click" */
         if (tl < 0.0) sl = 0.0;
         if (tr < 0.0) sr = 0.0;
         ypl = yl; yl = tau1/dt + tau2/(dt*dt);
         yl  = (sl + yl * ypl + tau2/dt * zl) / (1.0 + yl); zl = (yl - ypl) / dt;
         ypr = yr; yr = tau1/dt + tau2/(dt*dt);
         yr  = (sr + yr * ypr + tau2/dt * zr) / (1.0 + yr); zr = (yr - ypr) / dt;
         l   = sso + 0.5 + scale * ssm * yl;
         if (l >= sso-1+ssm) l = sso-1+ssm; if (l < sso-ssm) l = sso-ssm;
         ss  = (unsigned int) l;
         if (HIFI) wi(ss); else putc(ss,fp);  /* Left channel */
         l   = sso + 0.5 + scale * ssm * yr;
         if (l >= sso-1+ssm) l = sso-1+ssm; if (l < sso-ssm) l = sso-ssm;
         ss  = (unsigned int) l;
         if (HIFI) wi(ss); else putc(ss,fp);  /* Right channel */
         k++;
      }
      fclose(fp);

      playSound((char*)FNAME);  /* Play the soundscape */
   /* remove("hificode.wav"); */

      k=0;  /* Reset sample count */
   }

   static void *init_ar() {
      Py_Initialize();

      import_array();
      return NUMPY_IMPORT_ARRAY_RETVAL;
   }

   BOOST_PYTHON_MODULE (soundvision_c) {
        //using namespace XM;
        init_ar();

        setup();

        //initialize converters
        to_python_converter<cv::Mat,
                soundvision_c::matToNDArrayBoostConverter>();
        soundvision_c::matFromNDArrayBoostConverter();

        //expose module-level functions
         def("process_frame", process_frame);

    }
}


/**

Copyright (c) 2016, Allgeyer Tobias, Aumann Florian, Borella Jocelyn, Meißner Pascal, Qattan Mohamad
All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.

3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

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

#include <pcl/point_cloud.h>
#include <pcl/filters/frustum_culling.h>
#include <pcl/impl/point_types.hpp>

#include <pcl/filters/impl/frustum_culling.hpp>

namespace fake_object_recognition {

Rating::Rating(double fovx, double fovy, double ncp, double fcp) :
    fovx_(fovx),
    fovy_(fovy),
    ncp_(ncp),
    fcp_(fcp),
    camera_orientation_(Eigen::Vector3d(0.0, 0.0, 1.0)) { }

bool Rating::ratePose(const geometry_msgs::Pose &pose, double threshold_d, double threshold_x, double threshold_y) {
    //Use distance and agle ratings to rate pose.
    double rating_d = getDistanceRating(pose);
    ROS_DEBUG_STREAM("Distance Rating: " << rating_d);
    double rating_x = getAngleRating(pose, true);
    ROS_DEBUG_STREAM("Angle rating in xz-plane: " << rating_x);
    double rating_y = getAngleRating(pose, false);
    ROS_DEBUG_STREAM("Angle rating in yz-plane: " << rating_y);

    return rating_d > threshold_d && rating_x > threshold_x && rating_y > threshold_y;
}

double Rating::getDistanceRating(const geometry_msgs::Pose &object_pose) {
    double rating = 0.0;
    Eigen::Vector3d object_position(object_pose.position.x, object_pose.position.y, object_pose.position.z);
    double dot_cam_obj = camera_orientation_.dot(object_position);

    double dist_to_mid = std::abs(dot_cam_obj - ((fcp_ + ncp_) / 2.0));
    double dist_threshold = (fcp_ - ncp_) / 2.0;

    if (dist_to_mid < dist_threshold) {
        rating = 0.5 + 0.5 * cos((dist_to_mid * M_PI) / dist_threshold);
    }

    return rating;
}

  double Rating::getAngleRating(const geometry_msgs::Pose &object_pose, bool direction) {
    double rating = 0.0;

    Eigen::Vector3d object_position(object_pose.position.x, object_pose.position.y, object_pose.position.z);

    double angle_max;

    //Azimut
    if(direction){
      //Project object position on xz-plane of camera frame.
      object_position.y() = 0.0;
      angle_max = (fovx_ * M_PI) / 180.0;
    }
    //Elevation
    else {
      //Project object position on xz-plane of camera frame.
      object_position.x() = 0.0;
      angle_max = (fovy_ * M_PI) / 180.0;
    }

    object_position.normalize();

    double angle = std::acos(camera_orientation_.dot(object_position));

    if (angle < angle_max) {
        rating = 0.5 + 0.5 * std::cos((angle * M_PI) / angle_max);
    }

    return rating;
}

bool Rating::getBBRating(const std::array<geometry_msgs::Point, 8> &bounding_box) {
    pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud = boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
    unsigned int bb_size = bounding_box.size(); // always 8
    for (unsigned int i = 0; i < bb_size; i++) {
        pcl::PointXYZ pt = pcl::PointXYZ(bounding_box.at(i).x, bounding_box.at(i).y, bounding_box.at(i).z);
        point_cloud->push_back(pt);
    }

    pcl::FrustumCulling<pcl::PointXYZ> fc;
    fc.setHorizontalFOV(fovx_);
    fc.setVerticalFOV(fovy_);
    fc.setNearPlaneDistance(ncp_);
    fc.setFarPlaneDistance(fcp_);

    /* Camera pose: since visual axis is always set to (0,0,1) in ctor, a fixed matrix is used here; it is a homogenous matrix rotating around the y-axis by 90 degrees
     * because of the assumption that the camera is pointing in the x direction in the beginning. */
    Eigen::Matrix4f cam;
    cam <<  0.0, 0.0, -1.0, 0.0,
            0.0, 1.0,  0.0, 0.0,
            1.0, 0.0,  0.0, 0.0,
            0.0, 0.0,  0.0, 1.0;
    fc.setCameraPose(cam);

    fc.setInputCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr(point_cloud));

    pcl::PointCloud<pcl::PointXYZ>::Ptr culled_cloud = boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
    fc.filter(*culled_cloud);

    return (culled_cloud->size() == bb_size);
}

double Rating::getNormalRating(const geometry_msgs::Pose &object_pose, const std::vector<geometry_msgs::Point> &normals) {
    //similar to NBV - DefaulRatingModule
    // (never called with empty normals because of check in rateBBandNormal)
    Eigen::Vector3d v1, v2;
    double angleThreshold = M_PI * 0.5;
    v1 << object_pose.position.x, object_pose.position.y, object_pose.position.z; // Vector describing the position of the object.
    v1 = (-1) * v1;
    v1.normalize(); // Vector from camera to object

    float xNorm = v1.lpNorm<2>();
    if (xNorm == 0) {
        ROS_DEBUG_STREAM("Object appears to be inside camera.");
        return 0.0;
    }
    // find maximum rating:
    float best_rated = 0.0;
    for (unsigned int i = 0; i < normals.size(); i++) {
        v2 << normals.at(i).x, normals.at(i).y, normals.at(i).z;
        v2.normalize();
        float yNorm = v2.lpNorm<2>();
        if (yNorm != 0) { // Otherwise normal does not actually exist
            float cosine = v1.dot(v2) / xNorm / yNorm;
            float angle = std::acos(cosine);
            if (angle < angleThreshold) {
                float rating = 0.5 + 0.5 * std::cos(angle * M_PI / angleThreshold);
                if (rating > best_rated) { best_rated = rating; }
            }
        }
    }
    ROS_DEBUG_STREAM("getNormalRating: best_rated = " << best_rated);
    return best_rated;
}

bool Rating::rateBBandNormal(const geometry_msgs::Pose &object_pose, const std::array<geometry_msgs::Point, 8> &bounding_box, const std::vector<geometry_msgs::Point> &normals, double threshold) {
    if (!getBBRating(bounding_box)) {       // if not all 8 corner points of bounding box are inside the frustum:
        return false;                       // Return immediately.
    }
    else if (normals.empty()) {             // if there are no normals, e.g. because an object has no clear ones:
        return true;                        // since all bounding box corner points are inside frustum: return true.
    }
    else {
        return (getNormalRating(object_pose, normals) > threshold); // Otherwise rate normals.
    }
}

}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#include "Domain/CoordinateMaps/BulgedCube.hpp"

#include <boost/none.hpp>
#include <boost/optional.hpp>
#include <cmath>
#include <exception>
#include <functional>  // for std::reference_wrapper
#include <limits>
#include <pup.h>

#include "DataStructures/Tensor/EagerMath/DeterminantAndInverse.hpp"
#include "DataStructures/Tensor/Tensor.hpp"
#include "ErrorHandling/Assert.hpp"
#include "NumericalAlgorithms/RootFinding/TOMS748.hpp"
#include "Utilities/ConstantExpressions.hpp"
#include "Utilities/DereferenceWrapper.hpp"
#include "Utilities/GenerateInstantiations.hpp"
#include "Utilities/Gsl.hpp"
#include "Utilities/MakeWithValue.hpp"

namespace {
class RootFunction {
 public:
  RootFunction(const double radius, const double sphericity,
               const double physical_r_squared, const double x_sq,
               const double y_sq, const double z_sq) noexcept
      : radius_(radius),
        sphericity_(sphericity),
        physical_r_squared_(physical_r_squared),
        x_sq_(x_sq),
        y_sq_(y_sq),
        z_sq_(z_sq) {
    ASSERT(physical_r_squared_ > 0.0,
           "The RootFunction for the inverse map of BulgedCube is only valid "
           "for a radius greater than zero. The zero-radius case should be "
           "handled explicitly without call the root finder.");
  }

  double operator()(const double rho) const noexcept {
    const double x_sq_over_r_sq = x_sq_ / physical_r_squared_;
    const double y_sq_over_r_sq = y_sq_ / physical_r_squared_;
    const double z_sq_over_r_sq = z_sq_ / physical_r_squared_;
    return sqrt(physical_r_squared_) -
           radius_ * rho *
               (1.0 / sqrt(3.0) +
                sphericity_ *
                    (1.0 / sqrt(1.0 + square(rho) *
                                          (x_sq_over_r_sq + y_sq_over_r_sq)) +
                     1.0 / sqrt(1.0 + square(rho) *
                                          (x_sq_over_r_sq + z_sq_over_r_sq)) +
                     1.0 / sqrt(1.0 + square(rho) *
                                          (y_sq_over_r_sq + z_sq_over_r_sq)) -
                     1.0 / sqrt(2.0 + square(rho) * x_sq_over_r_sq) -
                     1.0 / sqrt(2.0 + square(rho) * y_sq_over_r_sq) -
                     1.0 / sqrt(2.0 + square(rho) * z_sq_over_r_sq)));
  }
  double get_r_sq() const noexcept { return physical_r_squared_; }

 private:
  const double radius_;
  const double sphericity_;
  const double physical_r_squared_;
  const double x_sq_;
  const double y_sq_;
  const double z_sq_;
};

boost::optional<double> scaling_factor(RootFunction&& rootfunction) noexcept {
  const double physical_r_squared = rootfunction.get_r_sq();
  try {
    constexpr double tol = 10.0 * std::numeric_limits<double>::epsilon();
    // Use a small nonzero number since the inverse map is singular at r==0 and
    // that case is handled separately.
    constexpr double lower_bound = std::numeric_limits<double>::min();
    // upper_bound = sqrt(3) + tol
    constexpr double upper_bound = 1.7320508075688772 + tol;
    double rho =
        // NOLINTNEXTLINE(clang-analyzer-core)
        RootFinder::toms748(rootfunction, lower_bound, upper_bound, tol, tol);
    rho /= sqrt(physical_r_squared);
    return rho;
  } catch (std::exception& exception) {
    return boost::none;
  }
}
}  // namespace

namespace domain {
namespace CoordinateMaps {
BulgedCube::BulgedCube(const double radius, const double sphericity,
                       const bool use_equiangular_map) noexcept
    : radius_(radius),
      sphericity_(sphericity),
      use_equiangular_map_(use_equiangular_map),
      is_identity_(radius_ == sqrt(3.0) and sphericity_ == 0.0 and
                   not use_equiangular_map_) {
  ASSERT(radius > 0.0, "The radius of the cube must be greater than zero");
  ASSERT(sphericity >= 0.0 and sphericity < 1.0,
         "The sphericity must be strictly less than one.");
}

template <typename T>
std::array<tt::remove_cvref_wrap_t<T>, 3> BulgedCube::operator()(
    const std::array<T, 3>& source_coords) const noexcept {
  using ReturnType = tt::remove_cvref_wrap_t<T>;
  const auto physical_coordinates = [this](
      const ReturnType& cap_xi, const ReturnType& cap_eta,
      const ReturnType& cap_zeta) noexcept {
    const auto one_over_rho_xi = 1.0 / sqrt(2.0 + square(cap_xi));
    const auto one_over_rho_eta = 1.0 / sqrt(2.0 + square(cap_eta));
    const auto one_over_rho_zeta = 1.0 / sqrt(2.0 + square(cap_zeta));
    const auto one_over_rho_xi_eta =
        1.0 / sqrt(1.0 + square(cap_xi) + square(cap_eta));
    const auto one_over_rho_xi_zeta =
        1.0 / sqrt(1.0 + square(cap_xi) + square(cap_zeta));
    const auto one_over_rho_eta_zeta =
        1.0 / sqrt(1.0 + square(cap_eta) + square(cap_zeta));
    const ReturnType radial_scaling_factor =
        radius_ * (1.0 / sqrt(3.0) +
                   sphericity_ * (one_over_rho_eta_zeta + one_over_rho_xi_zeta +
                                  one_over_rho_xi_eta - one_over_rho_xi -
                                  one_over_rho_eta - one_over_rho_zeta));

    ReturnType physical_x = radial_scaling_factor * cap_xi;
    ReturnType physical_y = radial_scaling_factor * cap_eta;
    ReturnType physical_z = radial_scaling_factor * cap_zeta;
    return std::array<ReturnType, 3>{
        {std::move(physical_x), std::move(physical_y), std::move(physical_z)}};
  };

  if (use_equiangular_map_) {
    return physical_coordinates(
        tan(M_PI_4 * dereference_wrapper(source_coords[0])),
        tan(M_PI_4 * dereference_wrapper(source_coords[1])),
        tan(M_PI_4 * dereference_wrapper(source_coords[2])));
  }
  return physical_coordinates(dereference_wrapper(source_coords[0]),
                              dereference_wrapper(source_coords[1]),
                              dereference_wrapper(source_coords[2]));
}

boost::optional<std::array<double, 3>> BulgedCube::inverse(
    const std::array<double, 3>& target_coords) const noexcept {
  const double& physical_x = target_coords[0];
  const double& physical_y = target_coords[1];
  const double& physical_z = target_coords[2];
  const double x_sq = square(physical_x);
  const double y_sq = square(physical_y);
  const double z_sq = square(physical_z);
  const double physical_r_squared = x_sq + y_sq + z_sq;
  // For physical_r_squared==0 we know that we are at the origin x=y=z=0, and we
  // know analytically that this map maps the origin to itself. Handling the
  // case r==0 case separately simplifies the root finding procedure for the
  // scaling_factor.
  if (UNLIKELY(physical_r_squared == 0.0)) {
    return {{{0.0, 0.0, 0.0}}};
  }

  // We are not at the origin, find the scaling factor (does a root-find)
  const auto scaling_factor =
      // NOLINTNEXTLINE(clang-analyzer-core)
      ::scaling_factor(
      RootFunction{radius_, sphericity_, physical_r_squared, x_sq, y_sq, z_sq});
  if (not scaling_factor) {
    return boost::none;
  }
  if (use_equiangular_map_) {
    return {{{2.0 * M_2_PI * atan(physical_x * scaling_factor.get()),
              2.0 * M_2_PI * atan(physical_y * scaling_factor.get()),
              2.0 * M_2_PI * atan(physical_z * scaling_factor.get())}}};
  }
  return {
      {{physical_x * scaling_factor.get(), physical_y * scaling_factor.get(),
        physical_z * scaling_factor.get()}}};
}

template <typename T>
std::array<tt::remove_cvref_wrap_t<T>, 3> BulgedCube::xi_derivative(
    const std::array<T, 3>& source_coords) const noexcept {
  using ReturnType = tt::remove_cvref_wrap_t<T>;
  const auto derivative_lambda = [this](
      const ReturnType& cap_xi, const ReturnType& cap_eta,
      const ReturnType& cap_zeta, const auto& cap_xi_deriv) {
    const ReturnType one_over_rho_xi_cubed =
        pow<3>(1.0 / sqrt(2.0 + square(cap_xi)));
    const ReturnType one_over_rho_eta = 1.0 / sqrt(2.0 + square(cap_eta));
    const ReturnType one_over_rho_zeta = 1.0 / sqrt(2.0 + square(cap_zeta));
    const ReturnType one_over_rho_xi_eta_cubed =
        pow<3>(1.0 / sqrt(1.0 + square(cap_xi) + square(cap_eta)));
    const ReturnType one_over_rho_xi_zeta_cubed =
        pow<3>(1.0 / sqrt(1.0 + square(cap_xi) + square(cap_zeta)));
    const ReturnType one_over_rho_eta_zeta =
        1.0 / sqrt(1.0 + square(cap_eta) + square(cap_zeta));
    const ReturnType common_factor =
        sphericity_ * radius_ * cap_xi * cap_xi_deriv *
        (one_over_rho_xi_cubed - one_over_rho_xi_eta_cubed -
         one_over_rho_xi_zeta_cubed);

    const ReturnType physical_x =
        radius_ * cap_xi_deriv *
        (1.0 / sqrt(3.0) +
         sphericity_ *
             (((1.0 + square(cap_eta)) * one_over_rho_xi_eta_cubed +
               (1.0 + square(cap_zeta)) * one_over_rho_xi_zeta_cubed -
               2.0 * one_over_rho_xi_cubed) +
              one_over_rho_eta_zeta - one_over_rho_eta - one_over_rho_zeta));
    const ReturnType physical_y = cap_eta * common_factor;
    const ReturnType physical_z = cap_zeta * common_factor;

    return std::array<ReturnType, 3>{{physical_x, physical_y, physical_z}};
  };
  if (use_equiangular_map_) {
    return derivative_lambda(
        tan(M_PI_4 * dereference_wrapper(source_coords[0])),
        tan(M_PI_4 * dereference_wrapper(source_coords[1])),
        tan(M_PI_4 * dereference_wrapper(source_coords[2])),
        ReturnType{M_PI_4 *
                   (1.0 + square(tan(M_PI_4 *
                                     dereference_wrapper(source_coords[0]))))});
  }
  return derivative_lambda(dereference_wrapper(source_coords[0]),
                           dereference_wrapper(source_coords[1]),
                           dereference_wrapper(source_coords[2]), 1.0);
}

template <typename T>
tnsr::Ij<tt::remove_cvref_wrap_t<T>, 3, Frame::NoFrame> BulgedCube::jacobian(
    const std::array<T, 3>& source_coords) const noexcept {
  const auto dX_dxi = xi_derivative(source_coords);
  const auto dX_deta = xi_derivative(
      std::array<std::reference_wrapper<const tt::remove_cvref_wrap_t<T>>, 3>{
          {std::cref(dereference_wrapper(source_coords[1])),
           std::cref(dereference_wrapper(source_coords[0])),
           std::cref(dereference_wrapper(source_coords[2]))}});
  const auto dX_dzeta = xi_derivative(
      std::array<std::reference_wrapper<const tt::remove_cvref_wrap_t<T>>, 3>{
          {std::cref(dereference_wrapper(source_coords[2])),
           std::cref(dereference_wrapper(source_coords[1])),
           std::cref(dereference_wrapper(source_coords[0]))}});
  auto jacobian_matrix =
      make_with_value<tnsr::Ij<tt::remove_cvref_wrap_t<T>, 3, Frame::NoFrame>>(
          dereference_wrapper(source_coords[0]), 0.0);

  get<0, 0>(jacobian_matrix) = dX_dxi[0];
  get<0, 1>(jacobian_matrix) = dX_deta[1];
  get<0, 2>(jacobian_matrix) = dX_dzeta[2];
  get<1, 0>(jacobian_matrix) = dX_dxi[1];
  get<1, 1>(jacobian_matrix) = dX_deta[0];
  get<1, 2>(jacobian_matrix) = dX_dzeta[1];
  get<2, 0>(jacobian_matrix) = dX_dxi[2];
  get<2, 1>(jacobian_matrix) = dX_deta[2];
  get<2, 2>(jacobian_matrix) = dX_dzeta[0];
  return jacobian_matrix;
}

template <typename T>
tnsr::Ij<tt::remove_cvref_wrap_t<T>, 3, Frame::NoFrame>
BulgedCube::inv_jacobian(const std::array<T, 3>& source_coords) const noexcept {
  const auto jac = jacobian(source_coords);
  return determinant_and_inverse(jac).second;
}

void BulgedCube::pup(PUP::er& p) noexcept {
  p | radius_;
  p | sphericity_;
  p | use_equiangular_map_;
  p | is_identity_;
}

bool operator==(const BulgedCube& lhs, const BulgedCube& rhs) noexcept {
  return lhs.radius_ == rhs.radius_ and lhs.sphericity_ == rhs.sphericity_ and
         lhs.use_equiangular_map_ == rhs.use_equiangular_map_ and
         lhs.is_identity_ == rhs.is_identity_;
}

bool operator!=(const BulgedCube& lhs, const BulgedCube& rhs) noexcept {
  return not(lhs == rhs);
}

// Explicit instantiations
/// \cond
#define DTYPE(data) BOOST_PP_TUPLE_ELEM(0, data)

#define INSTANTIATE(_, data)                                                  \
  template std::array<tt::remove_cvref_wrap_t<DTYPE(data)>, 3> BulgedCube::   \
  operator()(const std::array<DTYPE(data), 3>& source_coords) const noexcept; \
  template tnsr::Ij<tt::remove_cvref_wrap_t<DTYPE(data)>, 3, Frame::NoFrame>  \
  BulgedCube::jacobian(const std::array<DTYPE(data), 3>& source_coords)       \
      const noexcept;                                                         \
  template tnsr::Ij<tt::remove_cvref_wrap_t<DTYPE(data)>, 3, Frame::NoFrame>  \
  BulgedCube::inv_jacobian(const std::array<DTYPE(data), 3>& source_coords)   \
      const noexcept;

GENERATE_INSTANTIATIONS(INSTANTIATE, (double, DataVector,
                                      std::reference_wrapper<const double>,
                                      std::reference_wrapper<const DataVector>))

#undef DTYPE
#undef INSTANTIATE
/// \endcond
}  // namespace CoordinateMaps
}  // namespace domain


#include <unsupported/Eigen/NonLinearOptimization>
#include <Eigen/Dense>
#include "lp_lib.h" //http://lpsolve.sourceforge.net/5.5/

#include "obj.hh"
#include "vec3.h"
#include <array>
#include <vector>
#include <fstream>
#include <string>
#include <iostream>
#include <sstream>

using namespace std;

#include <itkImageFileReader.h>
#include <itkSignedMaurerDistanceMapImageFilter.h>
#include <itkOrImageFilter.h>
#include <itkAndImageFilter.h>
#include <itkMinimumMaximumImageCalculator.h>
#include <itkBinaryBallStructuringElement.h>
#include <itkBinaryThresholdImageFilter.h>
#include <itkBinaryDilateImageFilter.h>
#include <itkBinaryNotImageFilter.h>
#include <itkImageToVTKImageFilter.h>
#include <itkConstantPadImageFilter.h>
#include <itkRescaleIntensityImageFilter.h>

typedef itk::Image<float, 3> InternalImageType;
typedef itk::Image<unsigned char, 3> VisualizingImageType;
typedef itk::OrImageFilter<VisualizingImageType> orType;
typedef itk::BinaryBallStructuringElement
    <VisualizingImageType::PixelType, 3> StructuringElementType;
//VisualizingImageType::Pointer centers=0, fg=0, bg=0, roi=0, maskPW=0, maskGC=0, seeds=0, tempBG=0;

string base, folderManual, autoBase, filename;
const int bufsize=99999;
char buf[bufsize+1], *tmp;
int seedID=2; //first V.B. seed id =2, backgroundID=1, 0 rest (to be segmented)
int ind1=0, ind2=0;

#define setImpreciseTolerances(filter)\
filter->SetCoordinateTolerance(1e-2);\
filter->SetDirectionTolerance(1e-2);

using namespace Eigen;

const unsigned degree=3;
struct Vertebra
{
    vec3 center;
    double volume, radius, surface;
    int labelIndex;
    static const char* labels[];
    Cell *qe;
    int crushedFlag, spondylFlag;
    double crushed, spondyl;

    double Vertebra::calcCurrentRadius() //average distance of vertices to center
    {
        CellVertexIterator cvi(qe);
        Vertex *v;
        radius=0;
        while ( (v=cvi.next())!=0 )
            radius+=(center-v->pos).length();
        radius/=qe->countVertices();
        return radius;
    }

    //calculates center, volume and surface area
    void Vertebra::calcNewCenter()
    {
        double tv;
        volume=0;
        surface=0;
        vec3 newC(0,0,0), tc, a,b,c;
        CellFaceIterator cfi(qe);
        Face *f;
        while ( (f=cfi.next()) != 0 )
        {
            FaceEdgeIterator fei(f);
            Edge *e=fei.next();
            a=e->Org()->pos;
            e=fei.next();
            b=e->Org()->pos;
            e=fei.next();
            c=e->Org()->pos;
            tv=(a-center) * ((b-center)^(c-center)) / 6.0;
            //volume of tetrahedron consisting of this face and the old center
            tc=(a+b+c+center)/4.0; //center of mass of this tetrahedron
            newC+=(tc*tv);
            volume+=tv;
            surface+=((b-a)^(c-a)).length()/2; //vector product is double surface area
        }
        center=newC/volume;
    }
};

const char* Vertebra::labels[]=
    {"S5", "S4", "S3", "S2", "S1", "L5", "L4", "L3", "L2", "L1",
    "T12", "T11", "T10", "T9", "T8", "T7", "T6", "T5", "T4", "T3", "T2", "T1",
    "C7", "C6", "C5", "C4", "C3", "C2", "C1"};

vector<Vertebra *> vertebra;

//disease thresholds:
double crushedVertebraThreshold=0.2;
double significantSpondylolisthesis=0.25;

using namespace std;

double polyEval(const Eigen::VectorXd P, const double x) //evaluate using Horner's method
{
    double result=0;
    for (int i=P.size()-1; i>=0; i--)
        result=result*x+P[i];
    return result;
}

double dEval(const Eigen::VectorXd P, const double x) //evaluate polynomial's derivative
{
    double result=0;
    for (int i=P.size()-1; i>0; i--)
        result=result*x+i*P[i];
    return result;
}

Eigen::VectorXd derive(const Eigen::VectorXd P) //symbolic derivative of a polynomial
{
    Eigen::VectorXd result=Eigen::VectorXd::Zero(P.size()-1);
    for (int i=0; i<result.size(); i++)
        result[i]=P[i+1]*(i+1);
    return result;
}

void polynomialFitL1Norm(unsigned degree, std::vector<double> x, std::vector<double> y, VectorXd &p)
{
//2D L-1 norm minimization formulated as a linear program:
//
//minimize    e0+e1+...+en
//
//subject to  p0+x0*p1+x0^2*p2+x0^3*p3+e0>=y0
//            p0+x0*p1+x0^2*p2+x0^3*p3-e0<=y0
//            ...
//            p0+xn*p1+xn^2*p2+xn^3*p3+en>=yn
//            p0+xn*p1+xn^2*p2+xn^3*p3-en<=yn

    lprec *lp=make_lp(0, degree+1+y.size());

    char buffer[3]="pX";
    for (int k=0; k<=degree; k++)
    {
        buffer[1]='0'+k;
        set_col_name(lp, 1+k, buffer);
        set_unbounded(lp, 1+k);
    }
    buffer[0]='e';
    for (int k=0; k<y.size(); k++)
    {
        if (k<10)
            buffer[1]='0'+k;
        else
            buffer[1]='A'+k-10; //hexadecimal digits
        set_col_name(lp, 1+degree+1+k, buffer);
    }

    set_add_rowmode(lp, TRUE);
    std::vector<REAL> row(degree+1+y.size()+1);
    for (int i=0; i<y.size(); i++)
    {
        std::fill(row.begin(), row.end(), 0);
        double zk=1;
        for (int k=0; k<=degree; k++)
        {
            row[1+k]=zk;
            zk*=x[i];
        }

        row[1+degree+1+i]=1;
        add_constraint(lp,&row[0],GE,y[i]);

        row[1+degree+1+i]=-1;
        add_constraint(lp,&row[0],LE,y[i]);
    }
    set_add_rowmode(lp, FALSE);

    std::fill(row.begin(), row.end(), 0);
    for (int k=0; k<y.size(); k++)
        row[1+degree+1+k]=1;
    set_obj_fn(lp,&row[0]);

    set_minim(lp);
    write_lp(lp, "p.lp"); //debug
    set_verbose(lp, 1); //only critical errors
    solve(lp);

    p.resize(degree+1);
    get_variables(lp, &row[0]);
    for (int k=0; k<=degree; k++)
        p[k]=row[k];
    delete_lp(lp);
}

void polynomialFitL1Norm(unsigned degree, std::vector<double> volumes, VectorXd &p)
{
    std::vector<double> x(volumes.size());
    for (int i=0; i<x.size(); i++)
        x[i]=i;
    polynomialFitL1Norm(degree, x, volumes, p);
}

void polynomialFitL1Norm(unsigned degree, std::vector<Vertebra *> vertebra, VectorXd &xpoly, VectorXd &ypoly)
{
    std::vector<double> x(vertebra.size());
    for (int i=0; i<x.size(); i++)
        x[i]=vertebra[i]->center[2];

    std::vector<double> y(vertebra.size());
    for (int i=0; i<y.size(); i++)
        y[i]=vertebra[i]->center[0];
    polynomialFitL1Norm(degree, x, y, xpoly); //x-axis polynomial

    for (int i=0; i<y.size(); i++)
        y[i]=vertebra[i]->center[1];
    polynomialFitL1Norm(degree, x, y, ypoly); //y-axis polynomial
}

unsigned binomial(unsigned n, unsigned k)
{
    if (k==0 || k==n)
        return 1;
    else
        return binomial(n-1,k-1)+binomial(n-1,k);
}

VectorXd translatePolynomial(const VectorXd P, double minusTz)
{
    VectorXd r(P.size()); //allocate result
    for (int i=0; i<P.size(); i++)
    {
        r[i]=0;
        for (int k=P.size()-1; k>=i; k--)
            r[i]=r[i]*minusTz+binomial(k,i)*P[k];
    }
    return r;
}

void polynomialFit(unsigned degree, const std::vector<Vertebra *> vertebra, Eigen::VectorXd &x, Eigen::VectorXd &y, bool ignoreOrientations)
//x,y - output polynomial coefficients for A0+A1*z+A2*z^2+...+An*Z^n
{
    vec3 cTrans;
    for (int i=0; i<vertebra.size(); i++)
        cTrans+=vertebra[i]->center;
    cTrans/=vertebra.size();
    for (int i=0; i<vertebra.size(); i++)
        vertebra[i]->center-=cTrans; //centroid translation, numerically stable

    polynomialFitL1Norm(degree, vertebra, x, y);
    //if (ignoreOrientations)
    //    polynomialFitLinear(degree, vertebra, x, y);
    //else
    //    polynomialFitNonLinear(degree, vertebra, x, y);

    for (int i=0; i<vertebra.size(); i++)
        vertebra[i]->center+=cTrans; //counter centroid translation
    x=translatePolynomial(x,-cTrans[2]);
    x[0]+=cTrans[0];
    y=translatePolynomial(y,-cTrans[2]);
    y[0]+=cTrans[1];
}

void TheilSenEstimator(vector<double> y, double &slope, double &yIntercept)
{
    if (y.empty())
        return;
    if (y.size()==1)
    {
        slope=0;
        yIntercept=y[0];
        return;
    }

	vector<double> m;
	for (int i=0; i<y.size()-1; i++)
		for (int k=i+1; k<y.size(); k++)
			m.push_back((y[k]-y[i])/(k-i)); //slopes
	std::sort(m.begin(), m.end());
	slope=m[m.size()/2]; //median of slopes

	m.clear();
	for (int i=0; i<y.size(); i++)
		m.push_back(y[i]-slope*i); //y-intercepts
	std::sort(m.begin(), m.end());
	yIntercept=m[m.size()/2]; //median of y-intercepts
}

double distanceToPoly(vec3 p, Eigen::VectorXd x, Eigen::VectorXd y, vec3& closestPoint)
{
    closestPoint[0]=polyEval(x, p[2]);
    closestPoint[1]=polyEval(y, p[2]);
    closestPoint[2]=p[2];
    vec3 pa=closestPoint-p;
    double dot, dAlen, paLen;
    do
    {
        vec3 dA(dEval(x, closestPoint[2]), dEval(y, closestPoint[2]), 1);
        dot=dA*pa;
        dAlen=dA.length();
        paLen=pa.length();
        closestPoint-=dA*(dot/dAlen);
        pa=closestPoint-p;
    } while (pa.length()<paLen && abs(dot)/(dAlen*paLen)>0.1);
    //process converges and cos>0.1
    return pa.length();
}

void diagnose(const std::vector<Vertebra *> vertebra, const Eigen::VectorXd x, const Eigen::VectorXd y, std::string filename_base)
{
    ofstream diag((filename_base+".txt").c_str());
    double lowerZ=vertebra[0]->center[2];
    double upperZ=vertebra[vertebra.size()-1]->center[2];

    //crushed vertebral bodies, v1
    std::vector<double> vols;
    for (int i=0; i<vertebra.size(); i++)
        vols.push_back(vertebra[i]->volume);
    Eigen::VectorXd volF; //expected volume function
    polynomialFitL1Norm(3, vols, volF);
    for (int i=vertebra.size()-1; i>=0; i--)
    {
        double expectedVolume=polyEval(volF,i);
        vertebra[i]->crushed=100*abs(expectedVolume-vertebra[i]->volume)/(expectedVolume);

        if (vertebra[i]->volume-(expectedVolume)<-crushedVertebraThreshold*vertebra[i]->volume)
            vertebra[i]->crushedFlag=1; //crushed
        else if (vertebra[i]->volume-(expectedVolume)>crushedVertebraThreshold*vertebra[i]->volume)
            vertebra[i]->crushedFlag=0; //oversegmented
        else
            vertebra[i]->crushedFlag=0; //nothing special
    }

    //spondylolisthesis
    vertebra[vertebra.size()-1]->spondyl=0;
    vertebra[vertebra.size()-1]->spondylFlag=0;
    for (int i=vertebra.size()-2; i>=0; i--)
    {
        vec3 pp1,pp2; //poly point
        double dm1=distanceToPoly(vertebra[i]->center,x,y,pp1);
        distanceToPoly(vertebra[i+1]->center,x,y,pp2);
        vec3 d1=pp1-vertebra[i]->center, d2=vertebra[i+1]->center-pp2;
        vec3 cd=d1+d2;

        if (cd.length()<2*vertebra[i]->radius*significantSpondylolisthesis)
            vertebra[i]->spondylFlag=0;
        else
            vertebra[i]->spondylFlag=1;

        vertebra[i]->spondyl=50*cd.length()/vertebra[i]->radius;
    }

    //scoliosis, cobb angle >20° warrents further tracking, >30° warrents action
    int maxi=0,maxk=0;
    double maxCobb=0;
    for (int i=1; i<vertebra.size()-2; i++) //skip first and last
    {
        vec3 vi=vec3(dEval(x,vertebra[i]->center[2]),0,1); //projection to XZ plane
        for (int k=i+1; k<vertebra.size()-1; k++)
        {
            vec3 vk=vec3(dEval(x,vertebra[k]->center[2]),0,1); //projection to XZ plane
            double cobb=(180/M_PI)*acos(vi*vk/(vi.length()*vk.length()));
            if (cobb>=maxCobb)
            {
                maxCobb=cobb;
                maxi=i;
                maxk=k;
            }
        }
    }

    diag.precision(1);
    diag<<std::fixed;
    diag<<maxCobb<<" "<<vertebra[maxk]->labelIndex<<' ';
    diag<<vertebra[maxi]->labelIndex<<"  CobbAngle° startIndex endIndex";

    //output per-vertebra info
    for (int i=vertebra.size()-1; i>=0; i--)
    {
        diag<<endl<<vertebra[i]->volume<<" ";
        diag.precision(4);
        diag<<vertebra[i]->center[0]<<" "<<vertebra[i]->center[1]<<" "<<vertebra[i]->center[2]<<" ";
        diag.precision(1);
        diag<<vertebra[i]->crushed<<" "<<vertebra[i]->spondyl<<" ";
        diag<<vertebra[i]->crushedFlag<<" "<<vertebra[i]->spondylFlag;
        diag<<"  ("<<Vertebra::labels[vertebra[i]->labelIndex]<<") volume center crushed% spondyl% crushedFlag spondylFlag";
    }

    diag.close();
}

VisualizingImageType::Pointer openExact(string filename)
{
    typedef itk::ImageFileReader<VisualizingImageType> ReaderType;
    ReaderType::Pointer reader=ReaderType::New();
    reader->SetFileName(filename);
    reader->Update();
    return reader->GetOutput();
}

VisualizingImageType::Pointer openWithRescale(string filename)
{
    typedef itk::ImageFileReader<InternalImageType> ReaderType;
    ReaderType::Pointer reader=ReaderType::New();
    reader->SetFileName(filename);
    reader->Update();
    typedef itk::RescaleIntensityImageFilter<InternalImageType,VisualizingImageType> scalerType;
    scalerType::Pointer rescale=scalerType::New();
    rescale->SetInput(reader->GetOutput());
    rescale->Update();
    return rescale->GetOutput();
}

vec3 VBcenter(VisualizingImageType::Pointer ref)
{
    itk::ImageRegionConstIteratorWithIndex<VisualizingImageType> it(ref, ref->GetLargestPossibleRegion());
    it.GoToBegin();
    itk::ContinuousIndex<double,3> ind;
    unsigned long long i=0,j=0,k=0, sumCount=0;
    while (!it.IsAtEnd())
    {
        if (it.Value()!=0)
        {
            VisualizingImageType::PixelType val=it.Value();
            ind=it.GetIndex();
            i+=ind[0]*val;
            j+=ind[1]*val;
            k+=ind[2]*val;
            sumCount+=val;
        }
        ++it;
    }
    ind[0]=(long double)i/sumCount;
    ind[1]=(long double)j/sumCount;
    ind[2]=(long double)k/sumCount;
    VisualizingImageType::PointType p;
    ref->TransformContinuousIndexToPhysicalPoint(ind,p);
    return vec3(p[0],p[1],p[2]);
}

int main( int argc, char **argv )
{
    if (argc<4)
        return 1; //error

    string v1(argv[2]); //start vertebra
    string v2(argv[3]); //end vertebra
    while (Vertebra::labels[ind1]!=v1)
        ind1++;
    while (Vertebra::labels[ind2]!=v2)
        ind2++;
    if (ind1>ind2)
        swap(ind1, ind2);

    folderManual=string(argv[1]);

    for (int ind=ind1; ind<=ind2; ind++)
    {
        VisualizingImageType::Pointer ref=openExact(folderManual+Vertebra::labels[ind]+".mha");
        vertebra.push_back(new Vertebra);
        vertebra[ind-ind1]->labelIndex=ind;
        vertebra[ind-ind1]->center=VBcenter(ref); //from mask
        vertebra[ind-ind1]->qe=objReadCell((folderManual+Vertebra::labels[ind]+".obj").c_str());
        //objWriteCell(vertebra[ind-ind1]->qe, (folderManual+Vertebra::labels[ind]+".obj").c_str()); //debug
        vertebra[ind-ind1]->calcNewCenter(); //from poly
        vertebra[ind-ind1]->calcCurrentRadius();
    }

    Eigen::VectorXd x,y;
    polynomialFit(4,vertebra, x,y, true);
    diagnose(vertebra, x,y, folderManual+"diag");
    cout<<"Finished "+folderManual;
}


#include <boost/hana.hpp>

int main()
{
    constexpr auto t1{ boost::hana::make_tuple(boost::hana::int_c<5>, boost::hana::int_c<2>, boost::hana::int_c<3>, boost::hana::int_c<1>, boost::hana::int_c<4>) };
    constexpr auto r1{ boost::hana::index_if(t1, boost::hana::equal.to(boost::hana::int_c<3>)) };
    static_assert(r1.value().value == 2);

    constexpr auto t2{ boost::hana::tuple_c<int, 5, 2, 3> };
    constexpr auto r2{ boost::hana::index_if(t2, boost::hana::equal.to(boost::hana::int_c<6>)) };
    static_assert(r2.value_or(-1) == -1);
}


#ifndef BOOST_PREPROCESSOR_ARITHMETIC_MOD_HPP
#define BOOST_PREPROCESSOR_ARITHMETIC_MOD_HPP

/* Copyright (C) 2001
 * Housemarque Oy
 * http://www.housemarque.com
 *
 * Permission to copy, use, modify, sell and distribute this software is
 * granted provided this copyright notice appears in all copies. This
 * software is provided "as is" without express or implied warranty, and
 * with no claim as to its suitability for any purpose.
 *
 * See http://www.boost.org for most recent version.
 */

#include <boost/preprocessor/comparison/less_equal.hpp>

/**
Expands to the remainder of X and Y.

Both X and Y must expand to integer literals
in the range [0, BOOST_PP_LIMIT_MAG].

For example, BOOST_PP_MOD(4,3) expands to 1 (a
single token).

Uses

BOOST_PP_WHILE()

Test

arithmetic_test.cpp

*/
#define BOOST_PP_MOD(X,Y) BOOST_PP_MOD_D(0,X,Y)

/**
Can be used inside BOOST_PP_WHILE().
 */
#define BOOST_PP_MOD_D(D,X,Y) BOOST_PP_TUPLE_ELEM(2,0,BOOST_PP_WHILE##D(BOOST_PP_MOD_C,BOOST_PP_MOD_F,(X,Y)))
#define BOOST_PP_MOD_C(D,P) BOOST_PP_LESS_EQUAL_D(D,BOOST_PP_TUPLE2_ELEM1 P,BOOST_PP_TUPLE2_ELEM0 P)
#define BOOST_PP_MOD_F(D,P) (BOOST_PP_SUB_D(D,BOOST_PP_TUPLE2_ELEM0 P,BOOST_PP_TUPLE2_ELEM1 P),BOOST_PP_TUPLE2_ELEM1 P)

/*
Obsolete. Use BOOST_PP_MOD().
 */
#define BOOST_PREPROCESSOR_MOD(X,Y) BOOST_PP_MOD(X,Y)
#endif


#include <boost/bind.hpp>
#include <boost/ref.hpp>
#include <boost/parameter.hpp>
#include <string>
#include <functional>

BOOST_PARAMETER_NAME(s1)
BOOST_PARAMETER_NAME(s2)
BOOST_PARAMETER_NAME(s3)

template <typename ArgumentPack>
std::string f(ArgumentPack const& args)
{
    std::string const& s1 = args[_s1];
    std::string const& s2 = args[_s2];
    typename boost::parameter::binding<
        ArgumentPack, tag::s3, std::string
    >::type s3 = args[
        _s3 || boost::bind(
            std::plus<std::string>()
          , boost::ref(s1)
          , boost::ref(s2)
        )
    ];
    return s3;
}

#include <boost/core/lightweight_test.hpp>

int main()
{
    std::string x = f((_s1="hello,", _s2=" world", _s3="hi world"));
    BOOST_TEST_EQ(x, std::string("hi world"));
    return boost::report_errors();
}


#pragma once

#include <Eigen/Dense>

#include "pnc/draco_pnc/draco_state_provider.hpp"
#include "pnc/whole_body_controllers/task.hpp"

namespace feedback_source {
constexpr int kCom = 0;
constexpr int kIcp = 1;
} // namespace feedback_source

class DracoCenterOfMassTask : public Task {
public:
  DracoCenterOfMassTask(RobotSystem *_robot, int _feedback_source);

  virtual ~DracoCenterOfMassTask(){};

  Eigen::Vector2d icp_des;
  Eigen::Vector2d icp_dot_des;

private:
  void update_cmd();
  void update_jacobian();

  DracoStateProvider *sp_;

  int feedback_source_;
};