/* ----------------------------------------------------------------------------
 * Copyright 2017, Massachusetts Institute of Technology,
 * Cambridge, MA 02139
 * All Rights Reserved
 * Authors: Luca Carlone, et al. (see THANKS for the full author list)
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

/**
 * @file   testBasicRegularPlane3Factor.cpp
 * @brief  test BasicRegularPlane3Factor
 * @author Antoni Rosinol Vidal
 */

#include <algorithm>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <random>

#include <gtsam/base/numericalDerivative.h>
#include <boost/assign/std/vector.hpp>
#include <boost/bind.hpp>

#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>

#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/slam/PriorFactor.h>

#include <gflags/gflags.h>
#include <glog/logging.h>
#include <gtest/gtest.h>

#include "kimera-vio/VioBackEndParams.h"
#include "kimera-vio/factors/PointPlaneFactor.h"

using namespace std;
using namespace gtsam;
using namespace VIO;

/// Test tolerance
static constexpr double tol = 1e-5;
/// Delta increment for numerical derivative
static constexpr double delta_value = 1e-5;

/* -------------------------------------------------------------------------- */
// Set parameters for ISAM 2 incremental smoother.
void setIsam2Params(const VioBackEndParams& vio_params,
                    gtsam::ISAM2Params* isam_param) {
  CHECK_NOTNULL(isam_param);
  // iSAM2 SETTINGS
  gtsam::ISAM2GaussNewtonParams gauss_newton_params;
  // TODO remove this hardcoded value...
  gauss_newton_params.wildfireThreshold = -1.0;
  // gauss_newton_params.setWildfireThreshold(0.001);

  gtsam::ISAM2DoglegParams dogleg_params;
  // dogleg_params.setVerbose(false); // only for debugging.

  if (vio_params.useDogLeg_) {
    isam_param->optimizationParams = dogleg_params;
  } else {
    isam_param->optimizationParams = gauss_newton_params;
  }

  // Here there was commented code about setRelinearizeThreshold.
  isam_param->setCacheLinearizedFactors(false);
  isam_param->setEvaluateNonlinearError(true);
  isam_param->relinearizeThreshold = vio_params.relinearizeThreshold_;
  isam_param->relinearizeSkip = vio_params.relinearizeSkip_;
  // isam_param->enablePartialRelinearizationCheck = true;
  isam_param->findUnusedFactorSlots = true;
  // isam_param->cacheLinearizedFactors = true;
  // isam_param->enableDetailedResults = true;   // only for debugging.
  isam_param->factorization = gtsam::ISAM2Params::CHOLESKY;  // QR
  isam_param->print("isam_param");
  // isam_param.evaluateNonlinearError = true;  // only for debugging.
}

/**
 * Test that error does give the right result when it is zero.
 */
TEST(testPointPlaneFactor, ErrorIsZero) {
  Key pointKey(1);
  Key planeKey(2);
  noiseModel::Diagonal::shared_ptr regularityNoise =
      noiseModel::Diagonal::Sigmas(Vector1::Constant(0.1));
  PointPlaneFactor factor(pointKey, planeKey, regularityNoise);

  Unit3 plane_normal(2.4, 1.2, 1.9);
  double distance(2.3);

  // Set the linearization point
  Point3 point(distance * plane_normal.unitVector()[0],
               distance * plane_normal.unitVector()[1],
               distance * plane_normal.unitVector()[2]);
  OrientedPlane3 plane(plane_normal, distance);

  // Use the factor to calculate the Jacobians
  Vector error = factor.evaluateError(point, plane);
  Vector1 expected_error = Vector1::Constant(0.0);

  ASSERT_TRUE(assert_equal(expected_error, error, tol));
}

/**
 * Test that error does give the right result when it is not zero.
 */
TEST(testPointPlaneFactor, ErrorOtherThanZero) {
  Key pointKey(1);
  Key planeKey(2);
  noiseModel::Diagonal::shared_ptr regularityNoise =
      noiseModel::Diagonal::Sigmas(Vector1::Constant(0.1));
  PointPlaneFactor factor(pointKey, planeKey, regularityNoise);

  Unit3 plane_normal(2.4, 1.2, 1.9);
  double distance(2.3);

  // Set the linearization point
  Point3 point(-2.3, 2.1, 0.0);
  OrientedPlane3 plane(plane_normal, distance);

  // Use the factor to calculate the Jacobians
  Vector error = factor.evaluateError(point, plane);
  Vector1 expected_error = Vector1::Constant(-3.2124498);

  ASSERT_TRUE(assert_equal(expected_error, error, tol));
}

/**
 * Test that analytical jacobians equal numerical ones.
 *
 */
TEST(testPointPlaneFactor, Jacobians) {
  // Create the factor with a measurement that is 3 pixels off in x
  Key pointKey(1);
  Key planeKey(2);
  noiseModel::Diagonal::shared_ptr regularityNoise =
      noiseModel::Diagonal::Sigmas(Vector1::Constant(0.1));
  PointPlaneFactor factor(pointKey, planeKey, regularityNoise);

  double plane_normal[3] = {2.4, 1.2, 1.9};
  double distance(2.3);

  // Set the linearization point
  Point3 point(4.3, 3.2, 1.9);
  OrientedPlane3 plane(Unit3(plane_normal[0], plane_normal[1], plane_normal[2]),
                       distance);

  // Use the factor to calculate the Jacobians
  gtsam::Matrix H1Actual, H2Actual;
  factor.evaluateError(point, plane, H1Actual, H2Actual);

  // Calculate numerical derivatives
  Matrix H1Expected = numericalDerivative21<Vector, Point3, OrientedPlane3>(
      boost::bind(&PointPlaneFactor::evaluateError, &factor, _1, _2,
                  boost::none, boost::none),
      point, plane, delta_value);

  Matrix H2Expected = numericalDerivative22<Vector, Point3, OrientedPlane3>(
      boost::bind(&PointPlaneFactor::evaluateError, &factor, _1, _2,
                  boost::none, boost::none),
      point, plane, delta_value);

  // Verify the Jacobians are correct
  ASSERT_TRUE(assert_equal(H1Expected, H1Actual, tol));
  ASSERT_TRUE(assert_equal(H2Expected, H2Actual, tol));
}

/**
 * Test that analytical jacobians equal numerical ones, with negative values.
 *
 */
TEST(testPointPlaneFactor, JacobiansNegative) {
  // Create the factor with a measurement that is 3 pixels off in x
  Key pointKey(1);
  Key planeKey(2);
  noiseModel::Diagonal::shared_ptr regularityNoise =
      noiseModel::Diagonal::Sigmas(Vector1::Constant(0.1));
  PointPlaneFactor factor(pointKey, planeKey, regularityNoise);

  double plane_normal[3] = {2.4, 1.2, 1.9};
  double distance(-2.3);

  // Set the linearization point
  Point3 point(4.3, 3.2, 1.9);
  OrientedPlane3 plane(Unit3(plane_normal[0], plane_normal[1], plane_normal[2]),
                       distance);

  // Use the factor to calculate the Jacobians
  gtsam::Matrix H1Actual, H2Actual;
  factor.evaluateError(point, plane, H1Actual, H2Actual);

  // Calculate numerical derivatives
  Matrix H1Expected = numericalDerivative21<Vector, Point3, OrientedPlane3>(
      boost::bind(&PointPlaneFactor::evaluateError, &factor, _1, _2,
                  boost::none, boost::none),
      point, plane, delta_value);

  Matrix H2Expected = numericalDerivative22<Vector, Point3, OrientedPlane3>(
      boost::bind(&PointPlaneFactor::evaluateError, &factor, _1, _2,
                  boost::none, boost::none),
      point, plane, delta_value);

  // Verify the Jacobians are correct
  ASSERT_TRUE(assert_equal(H1Expected, H1Actual, tol));
  ASSERT_TRUE(assert_equal(H2Expected, H2Actual, tol));
}

/**
 * Test that optimization works for plane parameters.
 * Three landmarks, with prior factors, and a plane constrained together
 * using the landmark-plane factor.
 *
 */
TEST(testPointPlaneFactor, PlaneOptimization) {
  NonlinearFactorGraph graph;

  Point3 priorMean1(0.0, 0.0, 1.0);  // prior at origin
  noiseModel::Diagonal::shared_ptr priorNoise =
      noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
  graph.emplace_shared<PriorFactor<Point3>>(1, priorMean1, priorNoise);
  Point3 priorMean2(1.0, 0.0, 1.0);  // prior at origin
  graph.emplace_shared<PriorFactor<Point3>>(2, priorMean2, priorNoise);
  Point3 priorMean3(0.0, 1.0, 1.0);  // prior at origin
  graph.emplace_shared<PriorFactor<Point3>>(3, priorMean3, priorNoise);

  noiseModel::Isotropic::shared_ptr regularityNoise =
      noiseModel::Isotropic::Sigma(1, 0.5);
  graph.emplace_shared<PointPlaneFactor>(1, 4, regularityNoise);
  graph.emplace_shared<PointPlaneFactor>(2, 4, regularityNoise);
  graph.emplace_shared<PointPlaneFactor>(3, 4, regularityNoise);

  Values initial;
  initial.insert(1, Point3(0.0, 19.0, 3.0));
  initial.insert(2, Point3(-1.0, 2.0, 2.0));
  initial.insert(3, Point3(0.3, -1.0, 8.0));
  initial.insert(4, OrientedPlane3(0.1, 0.2, 0.9, 0.0));

  GaussNewtonParams params;
  // params.setVerbosity("ERROR");
  params.setMaxIterations(20);
  params.setRelativeErrorTol(-std::numeric_limits<double>::max());
  // params.setErrorTol(-std::numeric_limits<double>::max());
  params.setAbsoluteErrorTol(-std::numeric_limits<double>::max());

  Values result = GaussNewtonOptimizer(graph, initial, params).optimize();

  Values expected;
  expected.insert(1, priorMean1);
  expected.insert(2, priorMean2);
  expected.insert(3, priorMean3);
  expected.insert(4, OrientedPlane3(0.0, 0.0, 1.0, 1.0));

  ASSERT_TRUE(assert_equal(expected, result, tol));
}

/**
 * Test that optimization works for landmark parameters.
 * Three landmarks, with prior factor with high uncertainty in z, and a plane,
 * with a confident prior factor, constrained together using the
 * landmark-plane factor.
 *
 */
TEST(testBasicRegularPlane3Factor, LandmarkOptimization) {
  NonlinearFactorGraph graph;

  // Set landmark priors, with high uncertainty on z.
  Point3 priorMean1(0.0, 0.0, 2.0);
  noiseModel::Diagonal::shared_ptr priorNoise =
      noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 100.0));
  graph.emplace_shared<PriorFactor<Point3>>(1, priorMean1, priorNoise);
  Point3 priorMean2(1.0, 0.0, 0.0);
  graph.emplace_shared<PriorFactor<Point3>>(2, priorMean2, priorNoise);
  Point3 priorMean3(0.0, 1.0, -2.0);
  graph.emplace_shared<PriorFactor<Point3>>(3, priorMean3, priorNoise);

  // Horizontal Plane with prior.
  Key planeKey = 4;
  OrientedPlane3 priorMean(0.0, 0.0, 1.0, 1.0);
  noiseModel::Diagonal::shared_ptr priorNoisePlane =
      noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.01));
  graph.emplace_shared<PriorFactor<OrientedPlane3>>(planeKey, priorMean,
                                                    priorNoisePlane);

  noiseModel::Isotropic::shared_ptr regularityNoise =
      noiseModel::Isotropic::Sigma(1, 0.1);
  graph.emplace_shared<PointPlaneFactor>(1, planeKey, regularityNoise);
  graph.emplace_shared<PointPlaneFactor>(2, planeKey, regularityNoise);
  graph.emplace_shared<PointPlaneFactor>(3, planeKey, regularityNoise);

  Values initial;
  initial.insert(1, Point3(0.0, 0.0, -3.0));
  initial.insert(2, Point3(1.0, 2.0, 2.0));
  initial.insert(3, Point3(0.3, 1.0, 8.0));
  initial.insert(4, OrientedPlane3(0.1, 0.2, 0.9, 0.0));

  GaussNewtonParams params;
  // params.setVerbosity("ERROR");
  params.setMaxIterations(20);
  params.setRelativeErrorTol(-std::numeric_limits<double>::max());
  params.setAbsoluteErrorTol(-std::numeric_limits<double>::max());

  Values result = GaussNewtonOptimizer(graph, initial, params).optimize();

  Values expected;
  expected.insert(1, Point3(0.0, 0.0, 1.0));
  expected.insert(2, Point3(1.0, 0.0, 1.0));
  expected.insert(3, Point3(0.0, 1.0, 1.0));
  expected.insert(4, OrientedPlane3(0.0, 0.0, 1.0, 1.0));

  ASSERT_TRUE(assert_equal(expected, result, tol));
}

TEST(testPointPlaneFactor, MultiplePlanesOptimization) {
  NonlinearFactorGraph graph;

  noiseModel::Diagonal::shared_ptr priorNoise =
      noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
  Point3 priorMean1(0.0, 0.0, 1.0);  // prior at origin
  graph.emplace_shared<PriorFactor<Point3>>(1, priorMean1, priorNoise);
  Point3 priorMean2(1.0, 0.0, 1.0);  // prior at origin
  graph.emplace_shared<PriorFactor<Point3>>(2, priorMean2, priorNoise);
  Point3 priorMean3(0.0, 1.0, 1.0);  // prior at origin
  graph.emplace_shared<PriorFactor<Point3>>(3, priorMean3, priorNoise);

  noiseModel::Isotropic::shared_ptr regularityNoise =
      noiseModel::Isotropic::Sigma(1, 0.5);
  graph.emplace_shared<PointPlaneFactor>(1, 4, regularityNoise);
  graph.emplace_shared<PointPlaneFactor>(2, 4, regularityNoise);
  graph.emplace_shared<PointPlaneFactor>(3, 4, regularityNoise);

  // Add new plane.
  Point3 priorMeanA(0.0, 0.0, 2.0);  // prior at origin
  graph.emplace_shared<PriorFactor<Point3>>(5, priorMeanA, priorNoise);
  Point3 priorMeanB(1.0, 0.0, 2.0);  // prior at origin
  graph.emplace_shared<PriorFactor<Point3>>(6, priorMeanB, priorNoise);
  Point3 priorMeanC(0.0, 1.0, 2.0);  // prior at origin
  graph.emplace_shared<PriorFactor<Point3>>(7, priorMeanC, priorNoise);

  graph.emplace_shared<PointPlaneFactor>(5, 8, regularityNoise);
  graph.emplace_shared<PointPlaneFactor>(6, 8, regularityNoise);
  graph.emplace_shared<PointPlaneFactor>(7, 8, regularityNoise);

  Values initial;
  initial.insert(1, Point3(0.0, 19.0, 3.0));
  initial.insert(2, Point3(-1.0, 2.0, 2.0));
  initial.insert(3, Point3(0.3, -1.0, 8.0));
  initial.insert(4, OrientedPlane3(0.1, 0.2, 0.9, 0.0));
  initial.insert(5, Point3(0.0, 19.0, 2.0));
  initial.insert(6, Point3(-1.0, 2.0, 2.0));
  initial.insert(7, Point3(0.3, -1.0, 2.0));
  initial.insert(8, OrientedPlane3(0.1, 0.2, 0.9, 2.0));

  GaussNewtonParams params;
  // params.setVerbosity("ERROR");
  params.setMaxIterations(20);
  params.setRelativeErrorTol(-std::numeric_limits<double>::max());
  // params.setErrorTol(-std::numeric_limits<double>::max());
  params.setAbsoluteErrorTol(-std::numeric_limits<double>::max());

  Values result = GaussNewtonOptimizer(graph, initial, params).optimize();

  Values expected;
  expected.insert(1, priorMean1);
  expected.insert(2, priorMean2);
  expected.insert(3, priorMean3);
  expected.insert(4, OrientedPlane3(0.0, 0.0, 1.0, 1.0));
  expected.insert(5, priorMeanA);
  expected.insert(6, priorMeanB);
  expected.insert(7, priorMeanC);
  expected.insert(8, OrientedPlane3(0.0, 0.0, 1.0, 2.0));

  ASSERT_TRUE(assert_equal(expected, result, tol));
}

TEST(testPointPlaneFactor, MultiplePlanesIncrementalOptimization) {
  NonlinearFactorGraph graph;

  noiseModel::Diagonal::shared_ptr priorNoise =
      noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
  Point3 priorMean1(0.0, 0.0, 1.0);  // prior at origin
  graph.push_back(
      boost::make_shared<PriorFactor<Point3>>(1, priorMean1, priorNoise));
  Point3 priorMean2(1.0, 0.0, 1.0);  // prior at origin
  graph.push_back(
      boost::make_shared<PriorFactor<Point3>>(2, priorMean2, priorNoise));
  Point3 priorMean3(0.0, 1.0, 1.0);  // prior at origin
  graph.push_back(
      boost::make_shared<PriorFactor<Point3>>(3, priorMean3, priorNoise));

  noiseModel::Isotropic::shared_ptr regularityNoise =
      noiseModel::Isotropic::Sigma(1, 0.5);
  graph.push_back(
      boost::make_shared<gtsam::PointPlaneFactor>(1, 4, regularityNoise));
  graph.push_back(
      boost::make_shared<gtsam::PointPlaneFactor>(2, 4, regularityNoise));
  graph.push_back(
      boost::make_shared<gtsam::PointPlaneFactor>(3, 4, regularityNoise));
  Values initial;
  initial.insert(1, Point3(0.0, 19.0, 3.0));
  initial.insert(2, Point3(-1.0, 2.0, 2.0));
  initial.insert(3, Point3(0.3, -1.0, 8.0));
  initial.insert(4, OrientedPlane3(0.1, 0.2, 0.9, 0.0));

  std::map<Key, double> timestamps;
  for (const auto& keyValue : initial) {
    timestamps[keyValue.key] = 0;
  }

  gtsam::ISAM2Params isam_param;
  VioBackEndParams vioParams = VioBackEndParams();
  setIsam2Params(vioParams, &isam_param);
  gtsam::IncrementalFixedLagSmoother smoother(vioParams.horizon_, isam_param);
  try {
    // Update smoother.
    gtsam::FactorIndices delete_slots;
    smoother.update(graph, initial, timestamps, delete_slots);
    // Another extra iteration.
    for (size_t i = 0; i < 3; i++) {
      smoother.update(gtsam::NonlinearFactorGraph(), gtsam::Values(),
                      gtsam::FixedLagSmoother::KeyTimestampMap(),
                      gtsam::FastVector<size_t>());
    }
  } catch (const gtsam::IndeterminantLinearSystemException& e) {
    std::cout << e.what();

    const gtsam::Key& var = e.nearbyVariable();
    gtsam::Symbol symb(var);

    std::cout << "ERROR: Variable has type '" << symb.chr() << "' "
              << "and index " << symb.index() << std::endl;

    smoother.getFactors().print("Smoother's factors:\n[\n\t");
    std::cout << " ]" << std::endl;

    throw;
  }

  // Get result.
  Values result;
  result = smoother.calculateEstimate();

  Values expected;
  expected.insert(1, priorMean1);
  expected.insert(2, priorMean2);
  expected.insert(3, priorMean3);
  expected.insert(4, OrientedPlane3(0.0, 0.0, 1.0, 1.0));

  ASSERT_TRUE(assert_equal(expected, result, tol));

  // Clean old factors.
  graph.resize(0);

  // Add new plane.
  Point3 priorMeanA(0.0, 0.0, 2.0);  // prior at origin
  graph.push_back(
      boost::make_shared<PriorFactor<Point3>>(5, priorMeanA, priorNoise));
  Point3 priorMeanB(1.0, 0.0, 2.0);  // prior at origin
  graph.push_back(
      boost::make_shared<PriorFactor<Point3>>(6, priorMeanB, priorNoise));
  Point3 priorMeanC(0.0, 1.0, 2.0);  // prior at origin
  graph.push_back(
      boost::make_shared<PriorFactor<Point3>>(7, priorMeanC, priorNoise));

  // graph.emplace_shared<PointPlaneFactor>(5, 8, regularityNoise);
  // graph.emplace_shared<PointPlaneFactor>(6, 8, regularityNoise);
  // graph.emplace_shared<PointPlaneFactor>(7, 8, regularityNoise);
  graph.push_back(
      boost::make_shared<gtsam::PointPlaneFactor>(5, 8, regularityNoise));
  graph.push_back(
      boost::make_shared<gtsam::PointPlaneFactor>(6, 8, regularityNoise));
  graph.push_back(
      boost::make_shared<gtsam::PointPlaneFactor>(7, 8, regularityNoise));

  initial.clear();
  initial.insert(5, Point3(0.0, 19.0, 2.0));
  initial.insert(6, Point3(-1.0, 2.0, 2.0));
  initial.insert(7, Point3(0.3, -1.0, 2.0));
  initial.insert(8, OrientedPlane3(0.1, 0.2, 0.9, 2.0));

  timestamps.clear();
  for (const auto& keyValue : initial) {
    timestamps[keyValue.key] = 0;
  }

  try {
    // Update smoother.
    gtsam::FactorIndices delete_slots;
    smoother.update(graph, initial, timestamps, delete_slots);
    // Another extra iteration.
    for (size_t i = 0; i < 3; i++) {
      smoother.update(gtsam::NonlinearFactorGraph(), gtsam::Values(),
                      gtsam::FixedLagSmoother::KeyTimestampMap(),
                      gtsam::FastVector<size_t>());
    }
  } catch (const gtsam::IndeterminantLinearSystemException& e) {
    std::cout << e.what();

    const gtsam::Key& var = e.nearbyVariable();
    gtsam::Symbol symb(var);

    std::cout << "ERROR: Variable has type '" << symb.chr() << "' "
              << "and index " << symb.index() << std::endl;

    smoother.getFactors().print("Smoother's factors:\n[\n\t");
    std::cout << " ]" << std::endl;

    throw;
  }

  // Get result.
  result = smoother.calculateEstimate();

  expected.insert(5, priorMeanA);
  expected.insert(6, priorMeanB);
  expected.insert(7, priorMeanC);
  expected.insert(8, OrientedPlane3(0.0, 0.0, 1.0, 2.0));

  ASSERT_TRUE(assert_equal(expected, result, tol));
}


/*
* Copyright (c) 2016 Carnegie Mellon University, Author <dimatura@cmu.edu>
*
* For License information please see the LICENSE file in the root directory.
*
*/

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

#include <Eigen/Core>

#include <geom_cast/geom_cast.hpp>

#include <gtest/gtest.h>

using namespace boost::assign;
using namespace Eigen;
using namespace ca;

TEST(geom_cast, points_3_2) {
  Eigen::Vector3d v1(2, 8, 1);
  EXPECT_FLOAT_EQ( v1.x(), 2. );
  EXPECT_FLOAT_EQ( v1.y(), 8. );

  pcl::PointXY v2 = ca::point_cast<pcl::PointXY>(v1);

  EXPECT_FLOAT_EQ( v2.x, 2. );
  EXPECT_FLOAT_EQ( v2.y, 8. );

  pcl::PointXYZ v3(2, 8, 1);
  pcl::PointXY v4 = ca::point_cast<pcl::PointXY>(v3);
  EXPECT_FLOAT_EQ( v4.x, 2. );
  EXPECT_FLOAT_EQ( v4.y, 8. );

  cv::Vec2d v5 = ca::point_cast<cv::Vec2d>(v1);

  EXPECT_FLOAT_EQ( v5[0], 2. );
  EXPECT_FLOAT_EQ( v5[1], 8. );
}

TEST(geom_cast, points3) {

  Eigen::Vector3d food(2, 8, 0);
  Eigen::Vector3f foof(2, 8, 0);

  tf::Vector3 v1 = ca::point_cast<tf::Vector3>(food);

  EXPECT_FLOAT_EQ( v1.x(), 2. );
  EXPECT_FLOAT_EQ( v1.y(), 8. );
  EXPECT_FLOAT_EQ( v1.z(), 0. );

  tf::Vector3 v2 = ca::point_cast<tf::Vector3>(foof);

  EXPECT_FLOAT_EQ( v2.x(), 2. );
  EXPECT_FLOAT_EQ( v2.y(), 8. );
  EXPECT_FLOAT_EQ( v2.z(), 0. );

  geometry_msgs::Point v3 = ca::point_cast<geometry_msgs::Point>(foof);

  EXPECT_FLOAT_EQ( v3.x,  2.);
  EXPECT_FLOAT_EQ( v3.y,  8.);
  EXPECT_FLOAT_EQ( v3.z,  0.);

  geometry_msgs::Point v4 = ca::point_cast<geometry_msgs::Point>(v2);

  EXPECT_FLOAT_EQ( v4.x, 2.);
  EXPECT_FLOAT_EQ( v4.y, 8.);
  EXPECT_FLOAT_EQ( v4.z, 0.);

  pcl::PointXYZ v5 = ca::point_cast<pcl::PointXYZ>(foof);

  EXPECT_FLOAT_EQ( v5.x,  2.);
  EXPECT_FLOAT_EQ( v5.y,  8.);
  EXPECT_FLOAT_EQ( v5.z,  0.);

  Eigen::Vector3d v6 = ca::point_cast<Eigen::Vector3d>(v5);

  EXPECT_FLOAT_EQ( v6.x(), 2. );
  EXPECT_FLOAT_EQ( v6.y(), 8. );
  EXPECT_FLOAT_EQ( v6.z(), 0. );

  Eigen::Vector3d v7 = ca::point_cast<Eigen::Vector3d>(v1);

  EXPECT_FLOAT_EQ( v7.x(), 2. );
  EXPECT_FLOAT_EQ( v7.y(), 8. );
  EXPECT_FLOAT_EQ( v7.z(), 0. );

  float * v8 = static_cast<float*>(malloc(sizeof(float)*3));
  v8[0] = 2; v8[1] = 8; v8[2] = 0;
  pcl::PointXYZ v9 = ca::point_cast<pcl::PointXYZ>(v8);

  EXPECT_FLOAT_EQ( v7.x(),  2.);
  EXPECT_FLOAT_EQ( v7.y(),  8.);
  EXPECT_FLOAT_EQ( v7.z(),  0.);

  float v10[3] = { 2., 8., 0.};
  pcl::PointXYZ v11 = ca::point_cast<pcl::PointXYZ>(v10);
  EXPECT_FLOAT_EQ( v11.x,  2.);
  EXPECT_FLOAT_EQ( v11.y,  8.);
  EXPECT_FLOAT_EQ( v11.z,  0.);

  cv::Point3_<float> v12 = ca::point_cast<cv::Point3_<float> >(v6);
  EXPECT_FLOAT_EQ( v12.x,  2.);
  EXPECT_FLOAT_EQ( v12.y,  8.);
  EXPECT_FLOAT_EQ( v12.z,  0.);

  cv::Vec<double, 3> v13 = ca::point_cast<cv::Vec3d>(v4);
  EXPECT_FLOAT_EQ( v13[0],  2.);
  EXPECT_FLOAT_EQ( v13[1],  8.);
  EXPECT_FLOAT_EQ( v13[2],  0.);
}

TEST(geom_cast, rotations) {
  // eigen ctor is wxyz
  Eigen::Quaterniond q1(1, 0, 0, 0);
  EXPECT_FLOAT_EQ(q1.x(), 0.);
  EXPECT_FLOAT_EQ(q1.y(), 0.);
  EXPECT_FLOAT_EQ(q1.z(), 0.);
  EXPECT_FLOAT_EQ(q1.w(), 1.);

  geometry_msgs::Quaternion q2 = ca::rot_cast<geometry_msgs::Quaternion>(q1);
  EXPECT_FLOAT_EQ(q2.x, 0.);
  EXPECT_FLOAT_EQ(q2.y, 0.);
  EXPECT_FLOAT_EQ(q2.z, 0.);
  EXPECT_FLOAT_EQ(q2.w, 1.);

  tf::Quaternion q3 = ca::rot_cast<tf::Quaternion>(q1);

  EXPECT_FLOAT_EQ(q3.x(), 0.);
  EXPECT_FLOAT_EQ(q3.y(), 0.);
  EXPECT_FLOAT_EQ(q3.z(), 0.);
  EXPECT_FLOAT_EQ(q3.w(), 1.);

  Eigen::Quaternionf q4 = ca::rot_cast<Eigen::Quaternionf>(q3);

  EXPECT_FLOAT_EQ(q4.x(), 0.);
  EXPECT_FLOAT_EQ(q4.y(), 0.);
  EXPECT_FLOAT_EQ(q4.z(), 0.);
  EXPECT_FLOAT_EQ(q4.w(), 1.);

}

int main(int argc, char *argv[]) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


/* Copyright © 2017 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at https://opensource.org/licenses/BSD-3-Clause
 */
#ifndef TURI_DML_DATA_COLUMN_STATISTICS_H_
#define TURI_DML_DATA_COLUMN_STATISTICS_H_

#include <flexible_type/flexible_type.hpp>
#include <logger/assertions.hpp>
#include <serialization/serialization_includes.hpp>
#include <ml_data/ml_data_column_modes.hpp>
#include <parallel/pthread_tools.hpp>
#include <boost/thread/lock_algorithms.hpp>
#include <set>

namespace turi { namespace ml_data_internal {

extern size_t ML_DATA_STATS_PARALLEL_ACCESS_THRESHOLD;

/**
 * \ingroup mldata
 * \internal
 * \addtogroup mldata ML Data Internal
 * \{
 */

/**
 * column_metadata contains "meta data" concerning indexing of a single column
 * of an SFrame. A collection of meta_data column objects is "all" the
 * metadata required in the ml_data container.
 */
class column_statistics {

 public:

  column_statistics(){}

  /**
   *  Default constructor.
   */
  column_statistics(std::string column_name,
                    ml_column_mode mode,
                    flex_type_enum original_column_type);

  ////////////////////////////////////////////////////////////
  // Functions to access the statistics

  /** Returns the number of seen by the methods collecting the
   *  statistics.
   */
  size_t num_observations() const {
    return total_row_count;
  }

  /* The count; index here is the index obtained by one of the
   * map_value_to_index functions previously.
   */
  size_t count(size_t index=0) const {
    // Use this to
    if(mode == ml_column_mode::NUMERIC
       || mode == ml_column_mode::NUMERIC_VECTOR) {

      return total_row_count;
    } else {
      size_t ret = index < counts.size() ? counts[index] : 0;
      DASSERT_LE(ret, total_row_count);
      return ret;
    }
  }

  /* The mean; index here is the index obtained by one of the
   * map_value_to_index functions previously.
   */
  double mean(size_t index=0) const {
    if(mode == ml_column_mode::CATEGORICAL
       || mode == ml_column_mode::CATEGORICAL_VECTOR) {

      double p = double(count(index)) / (std::max(1.0, double(total_row_count)));

      DASSERT_LE(p, 1.0);
      DASSERT_GE(p, 0.0);

      return p;

    } else {

      if(total_row_count)
        DASSERT_TRUE(!statistics.empty());

      return index < statistics.size() ? statistics[index].mean : 0;
    }
  }

  /* The variance; index here is the index obtained by one of the
   * map_value_to_index functions previously.
   */
  double stdev(size_t index=0) const {
    if(mode == ml_column_mode::CATEGORICAL
       || mode == ml_column_mode::CATEGORICAL_VECTOR) {

      double stdev = 0;
      double p = mean(index);

      DASSERT_LE(p, 1.0);
      DASSERT_GE(p, 0.0);

      if (total_row_count > 1) {
        stdev = std::sqrt(total_row_count * p * (1 - p) / (total_row_count-1));
      } else {
        stdev = 0;
      }

      DASSERT_FALSE(std::isnan(stdev));

      return stdev;

    } else {

      if(total_row_count) {
        DASSERT_TRUE(!statistics.empty());
      }

      return index < statistics.size() ? statistics[index].stdev : 0;
    }
  }

  ////////////////////////////////////////////////////////////
  // Routines for updating the statistics.  This is done online, while
  // new categories are being added, etc., so we have to be

  /// Initialize the statistics -- counting, mean, and stdev
  void initialize();

  /// Update categorical statistics for a batch of categorical indices.
  void update_categorical_statistics(size_t thread_idx, const std::vector<size_t>& cat_index_vect) GL_HOT {

    DASSERT_TRUE(mode == ml_column_mode::CATEGORICAL
                 || mode == ml_column_mode::CATEGORICAL_VECTOR);

    std::vector<size_t>& counts = by_thread_element_counts[thread_idx];

    // The input indices are assured to be sorted
    DASSERT_TRUE(std::is_sorted(cat_index_vect.begin(), cat_index_vect.end()));

    size_t cv_idx = 0;

    for(cv_idx = 0;
        cv_idx < cat_index_vect.size()
            && cat_index_vect[cv_idx] < parallel_threshhold;
        ++cv_idx) {

      size_t idx = cat_index_vect[cv_idx];

      check_local_array_size(idx, counts);

      if(cv_idx == 0 || (idx != cat_index_vect[cv_idx - 1]) ) {
        ++(counts[idx]);
      }
    }

    if(cv_idx != cat_index_vect.size() ) {
      for(; cv_idx < cat_index_vect.size(); ++cv_idx) {
        size_t idx = cat_index_vect[cv_idx] - parallel_threshhold;

        if(idx >= global_element_counts.size() || idx >= global_size)
          adjust_global_array_size(idx, global_element_counts);

        if(cv_idx == 0 || (idx != cat_index_vect[cv_idx - 1]) ) {
          std::lock_guard<simple_spinlock> el_lg(global_element_locks[get_lock_index(idx)]);
          ++(global_element_counts[idx]);
        }
      }
    }

    ++by_thread_row_counts[thread_idx];
  }

  /// Update categorical statistics for a batch of real values.
  void update_numeric_statistics(size_t thread_idx, const std::vector<double>& value_vect) GL_HOT {

    DASSERT_TRUE(mode == ml_column_mode::NUMERIC
                 || mode == ml_column_mode::NUMERIC_VECTOR
                 || mode == ml_column_mode::NUMERIC_ND_VECTOR);

    // Silently ignore columns of empty vectors.  Note that all the
    // vectors in a column must be empty for this to work.
    if(value_vect.empty()) {
      return;
    }

    // Numeric statistics are always cached on a thread-local basis
    // and ignore the parallel_threshhold parameter.
    size_t& count = by_thread_row_counts[thread_idx];
    auto& stats = by_thread_mean_var_acc[thread_idx];

    if(stats.empty()) {

      DASSERT_EQ(size_t(count), 0);

      stats.resize(value_vect.size());

      for(size_t i = 0; i < value_vect.size(); ++i) {
        stats[i].mean    = value_vect[i];
        stats[i].var_sum = 0;
      }

    } else {
      DASSERT_EQ(stats.size(), value_vect.size());

      // Efficient and Stable Mean+Stdev computation
      // --------------------------------------------------------------------
      // This way of computing stdev goes back to a 1962 paper by B. P.
      // Welford and is presented in Donald Knuth's Art of Computer
      // Programming, Vol 2, page 232, 3rd edition. It is way more accurate
      // and stable than computing it "directly".
      //
      // Online update rule:
      //
      // M_k = M_{k-1} + (x_k - M_{k-1})/k
      // S_k = S_{k-1} + (x_k - M_{k-1})*(x_k - M_k).
      //
      // Here: M_k is the estimate of the mean and S_k/(k-1) is the estimate
      // of the variance:
      //
      for(size_t i = 0; i < value_vect.size(); i++){
        double& mean = stats[i].mean;
        double& var_sum = stats[i].var_sum;

        double old_mean = mean;
        double v = value_vect[i];

        mean += (v - old_mean) / (count + 1);

        // stdev here is actually the variance times the counts; it's
        // converted to the stdev later
        var_sum += (v - old_mean) * (v - mean);
      }
    }

    ++count;
  }

  /// Update statistics after observing a dictionary.
  void update_dict_statistics(size_t thread_idx, const std::vector<std::pair<size_t, double> >& dict) GL_HOT {

    DASSERT_TRUE(mode == ml_column_mode::DICTIONARY);

    // Does not compile on windows; comment out for now
    // // The input array is sorted here.
    // DASSERT_TRUE(std::is_sorted(dict.begin(), dict.end(),
    //                             [](std::pair<size_t, double> p1, std::pair<size_t, double> p2) {
    //                               return p1.first < p2.first;
    //                             }));

    size_t& row_count = by_thread_row_counts[thread_idx];

    // The update function for all this
    auto update_f = [&](size_t& count, double& mean, double& var_sum, double v) {

      // Efficient and Stable Mean+Stdev computation on Sparse data
      // --------------------------------------------------------------------
      // We combine means and variances of 2 samples as follows. The
      // first sample contains all the non-zeros while the second
      // contains only zeros.
      //
      // M_{a+b} = m * M_a / (m + n)
      // S_{a+b} = S_{a} + M_a^2 - M_{a+b}^2 * (a+b)
      //
      // This computation is O(m) (insted of being O(n+m)).
      //

      if(count == 0) {
        count = 1;
        mean  = v;
        var_sum = 0;

      } else {
        double old_mean = mean;

        ++count;
        mean += (v - old_mean) / count;
        var_sum += (v - old_mean) * (v - mean);
      }
    };

    // First, go through and work with the common elements.
    std::vector<size_t>& counts = by_thread_element_counts[thread_idx];
    std::vector<element_statistics_accumulator>& stats = by_thread_mean_var_acc[thread_idx];

    size_t d_idx = 0;

    for(; d_idx < dict.size() && dict[d_idx].first < parallel_threshhold; ++d_idx) {
      size_t idx = dict[d_idx].first;
      double v   = dict[d_idx].second;

      check_local_array_size(idx, counts);
      check_local_array_size(idx, stats);

      update_f(counts[idx], stats[idx].mean, stats[idx].var_sum, v);
    }

    // Okay, there are infrequent elements here, so we need to look at the global update deal.
    if(d_idx != dict.size()) {

      for(; d_idx < dict.size(); ++d_idx) {

        size_t idx = dict[d_idx].first - parallel_threshhold;
        double v   = dict[d_idx].second;

        if(idx >= global_element_counts.size()
           || idx >= global_mean_var_acc.size()
           || idx >= global_size) {
          adjust_global_array_size(idx, global_element_counts);
          adjust_global_array_size(idx, global_mean_var_acc);
        }

        std::lock_guard<simple_spinlock> el_lg(global_element_locks[get_lock_index(idx)]);

        update_f(global_element_counts[idx],
                 global_mean_var_acc[idx].mean,
                 global_mean_var_acc[idx].var_sum, v);
      }
    }

    ++row_count;
  }

  /// Perform final computations on the different statistics.  Must be
  /// called after all the data is filled.
  void finalize();

  /**  Reindex, typically according to a global map of things.
   */
  void reindex(const std::vector<size_t>& new_index_map, size_t new_column_size);

  /**  Merges in statistics from another column_statistics object.
   */
  void merge_in(const column_statistics& other);

 private:
  /// Perform finalizations; split between the threadlocal stuff and the global stuff.
  std::pair<bool, bool> _get_using_flags() const;

  void _finalize_threadlocal(size_t, bool, bool);
  void _finalize_global(size_t, bool, bool);

 public:
  ////////////////////////////////////////////////////////////////////////////////
  // Stuff for the saving and loading

  /** Returns the current serialization version of this model.
   */
  size_t get_version() const { return 3; }

  /**
   *  Serialize the object (save).
   */
  void save_impl(turi::oarchive& oarc) const;

  /**
   *  Load the object.
   */
  void load_version(turi::iarchive& iarc, size_t version);

  /** For debugging purposes.
   */
  bool is_equal(const column_statistics* other_ptr) const;

  /**
   * Equality testing -- slow!  Use for debugging/testing
   */
  bool operator==(const column_statistics& other) const;

  /**
   * Inequality testing -- slow!  Use for debugging/testing
   */
  bool operator!=(const column_statistics& other) const;

#ifndef NDEBUG
  void _debug_check_is_approx_equal(std::shared_ptr<column_statistics> other) const;
#else
  void _debug_check_is_approx_equal(std::shared_ptr<column_statistics> other) const {}
#endif

 private:

  // Store the basic column data.  This allows us to do error checking
  // and error reporting intelligently.

  std::string column_name;
  ml_column_mode mode;
  flex_type_enum original_column_type;

  ////////////////////////////////////////////////////////////////////////////////
  // The categorical variables

  // Put all of these in one single structure, and store a vector of
  // these structures.  Since these are always accessed together, this
  // reduces cache misses on the lookups -- we now only have one fetch
  // instead of three.

  std::vector<size_t> counts;

  struct element_statistics : public turi::IS_POD_TYPE {
    double mean  = 0;   /**< Mean of column. */
    double stdev = 0;  /**< Stdev of column. */
  };

  std::vector<element_statistics> statistics;
  size_t total_row_count = 0;

  // The issue with having seperate accumulators for each thread is
  // that it can take an inordinate amount of memory.  Thus we use
  // parallel access for the first million or so items, which are
  // likely to be the most common.  For the rest, we use a larger one
  // with locking.

  size_t parallel_threshhold = 1024*1024;

  std::vector<size_t> by_thread_row_counts;

  struct element_statistics_accumulator : public turi::IS_POD_TYPE {
    double mean    = 0;  /**< Mean of column. */
    double var_sum = 0;  /**< Stdev of column. */
  };

  std::vector<std::vector<size_t> > by_thread_element_counts;
  std::vector<std::vector<element_statistics_accumulator> > by_thread_mean_var_acc;

  // The locks are done by locking the lock given by the write index
  // modulus this value.
  static constexpr size_t n_locks = 64; // Should be power of 2

  std::array<simple_spinlock, n_locks> global_element_locks;
  std::vector<size_t> global_element_counts;
  std::vector<element_statistics_accumulator> global_mean_var_acc;

  size_t global_size = 0;

  /**  Return the index of the appropriate lock.
   *
   */
  inline size_t get_lock_index(size_t idx) const {
    size_t lock_idx = (idx % n_locks);
    return lock_idx;
  }

  /**  Possibly resize the local array.
   */
  template <typename T>
  inline void check_local_array_size(size_t idx, std::vector<T>& v) {
    DASSERT_LT(idx, parallel_threshhold);

      // See if a resize is needed.
    if(idx >= v.size() ) {

      if(UNLIKELY(v.capacity() < idx + 1)) {
        size_t new_capacity = std::min(parallel_threshhold, 3 * (idx + 1) / 2);

        // If it's likely to go to max capacity, just make it that to avoid future resizes.
        if(new_capacity > parallel_threshhold / 2)
          new_capacity = parallel_threshhold;

        v.reserve(new_capacity);
      }

      v.resize(idx + 1);
    }
  }

  /**  Check global array size. Possibly resize it.
   */
  template <typename T>
  void adjust_global_array_size(size_t idx, std::vector<T>& v) {

    // If needed, increase the value of global_size.
    atomic_set_max(global_size, idx + 1);

    // See if a resize is needed.
    if(UNLIKELY(idx >= v.size() )) {

      // Grow aggressively, since a resize is really expensive.
      size_t new_size = 2 * (parallel_threshhold + idx + 1);

      {
        std::array<std::unique_lock<simple_spinlock>, n_locks> all_locks;
        for(size_t i = 0; i < n_locks; ++i)
          all_locks[i] = std::unique_lock<simple_spinlock>(global_element_locks[i], std::defer_lock);

        // Ensure nothing is happening with the vector by locking all
        // locks in a thread safe way.  This prevents any thread from
        // accessing it while we resize it.
        boost::lock(all_locks.begin(), all_locks.end());

        // It's possible that another thread beat us to it.
        if(v.size() < idx + 1)
          v.resize(new_size);

        // The destructor of all_locks takes care of the unlocking.
      }
    }
  }

};

/// \}
}}

BEGIN_OUT_OF_PLACE_SAVE(arc, std::shared_ptr<ml_data_internal::column_statistics>, m) {
  if(m == nullptr) {
    arc << false;
  } else {
    arc << true;

    // Save the version number
    size_t version = m->get_version();
    arc << version;

    m->save_impl(arc);
  }

} END_OUT_OF_PLACE_SAVE()

BEGIN_OUT_OF_PLACE_LOAD(arc, std::shared_ptr<ml_data_internal::column_statistics>, m) {
  bool is_not_nullptr;
  arc >> is_not_nullptr;
  if(is_not_nullptr) {

    m.reset(new ml_data_internal::column_statistics);

    size_t version;
    arc >> version;

    m->load_version(arc, version);

  } else {
    m = std::shared_ptr<ml_data_internal::column_statistics>(nullptr);
  }
} END_OUT_OF_PLACE_LOAD()

#endif


/*
  ~200hz, escritura en: cout and file
*/
#include <iostream>
#include <boost/asio.hpp>
#include "vmu931/commands.hpp"
#include "vmu931/sensor.hpp"
#include "vmu931/types.hpp"
#include <boost/date_time.hpp>
#include <fstream>

//#define PI 3.1415926535
//#define G 9.79991 //g at fuenlabrada
#define PI std::acos(-1)
#define G 9.81f //g at sea level

  boost::posix_time::ptime now = boost::date_time::not_a_date_time;
  boost::posix_time::ptime last = boost::date_time::not_a_date_time;

int main()
{
    std::ofstream myfile;
    myfile.open("Data.txt");

    last = boost::posix_time::microsec_clock::universal_time();
    boost::asio::io_service io_service;
    boost::asio::serial_port serial_port(io_service, "/dev/ttyACM1");
    //boost::asio::serial_port serial_port(io_service, "/dev/tty.usbmodem931901443E");

    vmu931::Sensor sensor(std::move(serial_port));

    sensor.register_sink([&myfile](vmu931::Accelerometers accel) {
        myfile <<" " <<(accel.x)*G <<" " <<(accel.y)*G <<" " <<(accel.z)*G;
        std::cout <<" accelX=" <<(accel.x)*G <<" " <<(accel.y)*G <<" " <<(accel.z)*G;
    });

    sensor.register_sink([&myfile](vmu931::Gyroscopes gyro) {
        myfile <<" " <<(gyro.x)*PI/180 <<" " <<(gyro.y)*PI/180 <<" " <<(gyro.z)*PI/180;
        std::cout <<" gyroX=" <<(gyro.x)*PI/180 <<" " <<(gyro.y)*PI/180 <<" " <<(gyro.z)*PI/180;
    });

    sensor.register_sink([](vmu931::Magnetometers magneto) {
        std::cout << "magneto: x=" << magneto.x << " y=" << magneto.y << " z=" << magneto.z <<"\n";
    });

    sensor.register_sink([&myfile](vmu931::EulerAngles euler) {
        myfile <<" " <<euler.x <<" " <<euler.y <<" " <<euler.z;
        std::cout <<" eulX=" <<euler.x <<" " <<euler.y <<" " <<euler.z;
    });

    sensor.register_sink([&myfile](vmu931::Quaternions quat) {
      now = boost::posix_time::microsec_clock::universal_time();
      auto diff = now-last;
      last = now;
        myfile <<now <<" " <<(diff.total_microseconds())*0.000001;
        std::cout <<now <<" " <<(diff.total_microseconds())*0.000001;
        myfile <<" " <<quat.w <<" " <<quat.x <<" " <<quat.y <<" " <<quat.z;
        std::cout <<" quatW=" <<quat.w <<" " <<quat.x <<" " <<quat.y <<" " <<quat.z;
    });

    sensor.register_sink([&myfile](vmu931::Heading h) {
        myfile <<" " <<h.heading <<std::endl;
        std::cout <<" head=" <<h.heading <<std::endl;
    });

    sensor.register_sink([](std::string s) {
        std::cout << "message: " << s << "\n";
    });
    sensor.register_sink([](vmu931::Status status) {
        std::cout << "VMU931 status:\n"
            << " - streams: " << status.streaming() << "\n"
            << " - gyroscopes: " << status.resolution_gyroscopes() << " dps\n"
            << " - accelerometers: " << status.resolution_accelerometers() << " g\n"
            << " - output rate: " << status.output_rate() << " Hz\n";
    });

    io_service.post([&sensor]() {
      sensor.set_streams({
        vmu931::commands::Accelerometers,
        vmu931::commands::Gyroscopes,
        //vmu931::commands::Magnetometers,
        vmu931::commands::EulerAngles,
        vmu931::commands::Quaternions,
        vmu931::commands::Heading,
        //vmu931::commands::Status
      });
    });

    std::cout << "Start reading VMU931 sensor stream..." <<std::endl;
    myfile <<"UNITS: Diftime[sec] /*/ accel[m/s²] /*/ gyro[rad/s] /*/ euler[degrees] /*/ heading[degrees]" <<std::endl;
    std::cout <<"UNITS: Diftime[sec] /*/ accel[m/s²] /*/ gyro[rad/s] /*/ euler[degrees] /*/ heading[degrees]" <<std::endl;
    myfile <<"g= "<<G <<" | pi= " <<PI <<std::endl;
    std::cout <<"g= "<<G <<" | pi= " <<PI <<std::endl;

    myfile <<"Date Time " <<"Diftime " <<"/quatW " <<"quatX " <<"quatY " <<"quatZ " <<"/accelX " <<"accelY " <<"accelZ "
      <<"/gyroX " <<"gyroY " <<"gyroZ " <<"/eulerX "  <<"eulerY " <<"eulerZ " <<"/heading" <<std::endl;
    std::cout <<"Date Time " <<"Diftime " <<"/quatW " <<"quatX " <<"quatY " <<"quatZ " <<"/accelX " <<"accelY " <<"accelZ "
      <<"/gyroX " <<"gyroY " <<"gyroZ " <<"/eulerX "  <<"eulerY " <<"eulerZ " <<"/heading" <<std::endl;

    io_service.run();

  return 0;
}


//Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.

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

#ifndef UUID_995547FAAE0E11DE8CF511E755D89593
#define UUID_995547FAAE0E11DE8CF511E755D89593

#include <boost/qvm/detail/determinant_impl.hpp>
#include <boost/qvm/mat_traits.hpp>
#include <boost/qvm/static_assert.hpp>

namespace
boost
    {
    namespace
    qvm
        {
        namespace
        qvm_detail
            {
            template <class A>
            BOOST_QVM_INLINE_OPERATIONS
            typename deduce_mat<A>::type
            cofactor_impl( A const & a )
                {
                BOOST_QVM_STATIC_ASSERT(mat_traits<A>::rows==mat_traits<A>::cols);
                int const N=mat_traits<A>::rows;
                typedef typename mat_traits<A>::scalar_type T;
                T c[N-1][N-1];
                typedef typename deduce_mat<A>::type R;
                R b;
                for( int j=0; j!=N; ++j )
                    {
                    for( int i=0; i!=N; ++i )
                        {
                        int i1=0;
                        for( int ii=0; ii!=N; ++ii )
                            {
                            if( ii==i )
                                continue;
                            int j1=0;
                            for( int jj=0; jj!=N; ++jj )
                                {
                                if( jj==j )
                                    continue;
                                c[i1][j1] = mat_traits<A>::read_element_idx(ii,jj,a);
                                ++j1;
                                }
                            ++i1;
                            }
                        T det = determinant_impl(c);
                        if( (i+j)&1 )
                            det=-det;
                        mat_traits<R>::write_element_idx(i,j,b) = det;
                        }
                    }
                return b;
                }
            }
        }
    }

#endif


///////////////////////////////////////////////////////////////////////////////
// statistics::survival::data::random::batch.hpp                             //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_SURVIVAL_DATA_RANDOM_BATCH_HPP_ER_2009
#define BOOST_STATISTICS_SURVIVAL_DATA_RANDOM_BATCH_HPP_ER_2009
#include <iostream> // TODO remove
#include <boost/format.hpp>
#include <boost/range.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/gen_to_random.hpp>
#include <boost/statistics/model/wrap/aggregate/model_covariate_parameter.hpp>
#include <boost/statistics/survival/data/data/record.hpp>
#include <boost/statistics/survival/data/random/failure_time.hpp>
#include <boost/statistics/survival/data/meta/random_input.hpp>

#include <boost/iterator/range_cycle.hpp>

namespace boost{
namespace statistics{
namespace survival{
namespace data{
namespace random{

    // Given model and parameter, simulates records (et,ft) like this:
    // et is drawn from a random clock
    // x is random
    // ft is drawn from the p(ft|x,p,model)
    //
    // Models RandomDistribution
    template<
        typename T,                                         // float
        typename M,                                         // model
        typename P,                                         // parameter
        typename RdC,  // RandomDistribution                // clock
        typename RdX,  // RandomDistribution                // covariate
        typename I = typename meta::random_input<M>::type
    >
    class batch : public statistics::model::model_parameter_<M,P>{
        public:
        typedef T                                           value_type;
        typedef I                                           input_type;
        typedef record<value_type>                          result_type;
        typedef typename RdX::result_type                   covariate_type;

        typedef statistics::model::model_parameter_<M,P>    model_parameter_;
        typedef statistics
            ::model::model_covariate_parameter_<M,covariate_type,P>
                                                model_covariate_parameter_;

        // Constructor
        batch();
        batch( model_parameter_ mp,const RdC&,const RdX& );
        batch( const batch& );
        batch& operator=( const batch& );

        // Random generation
        template<typename U>
        result_type operator()(U& u);

        // Access
        const covariate_type& covariate()const; //last generated covariate

        private:
        RdC rdc_;
        RdX rdx_;
        covariate_type x_;
        template<typename U> void gen_x(U&);
    };

    // Implementation //

    //Construction
    template<
        typename T,typename M,typename P, typename RdC,typename RdX,typename I>
    batch<T,M,P,RdC,RdX,I>::batch():model_parameter_(){}

    template<
        typename T,typename M,typename P, typename RdC,typename RdX,typename I>
    batch<T,M,P,RdC,RdX,I>::batch(
        model_parameter_ mp, const RdC& rdc,const RdX& rdx
    ):model_parameter_(mp),rdc_(rdc),rdx_(rdx),x_(){}

    template<
        typename T,typename M,typename P, typename RdC,typename RdX,typename I>
    batch<T,M,P,RdC,RdX,I>::batch(
        const batch& that
    ):model_parameter_(that),rdc_(that.rdc_),rdx_(that.rdx_),x_(that.x_){}

    template<
        typename T,typename M,typename P, typename RdC,typename RdX,typename I>
    batch<T,M,P,RdC,RdX,I>&
    batch<T,M,P,RdC,RdX,I>::operator=(
        const batch& that
    ){
        if(&that!=this){
            model_parameter_::operator=( that.model_parameter );
            rdc_ = (that.rdc_);
            rdx_ = (that.rdx_);
            x_ = that.x_;
        }
        return (*this);
    }

    // Generation
    template<
        typename T,typename M,typename P, typename RdC,typename RdX,typename I>
    template<typename U>
    typename batch<T,M,P,RdC,RdX,I>::result_type
    batch<T,M,P,RdC,RdX,I>::operator()(U& u){
        this->gen_x(u);
        value_type et = (this->rdc_)(u); //entry time
        BOOST_ASSERT(!( et < static_cast<value_type>(0) ) );
        const model_parameter_& mp
            = static_cast<const model_parameter_&>(*this);
        value_type ft = random::failure_time<value_type>(
            boost::statistics::model::make_model_covariate_parameter(
                mp.model(),
                this->covariate(),
                mp.parameter()
            ),
            u
        );
        return result_type(et,ft);
    }

    // Access
    template<
        typename T,typename M,typename P, typename RdC,typename RdX,typename I>
    const typename batch<T,M,P,RdC,RdX,I>::covariate_type&
    batch<T,M,P,RdC,RdX,I>::covariate()const{ return this->x_; }

    // Private
    template<
        typename T,typename M,typename P, typename RdC,typename RdX,typename I>
    template<typename U>
    void batch<T,M,P,RdC,RdX,I>::gen_x(U& urng){
        typedef variate_generator<U&,RdX> vg_;
        // Necessary in the likely case that RdX::input_type != I
        vg_ vg(urng,rdx_);
        this->x_ = vg();
    }

}// random
}// data
}// survival
}// statistics
}// boost

#endif


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

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-ieee
    This function object returns @c x with the sign of @c y.

    @par Header <boost/simd/function/copysign.hpp>

    @see sign, signnz, is_positive

    @par Example:

      @snippet copysign.cpp copysign

    @par Possible output:

      @snippet copysign.txt copysign

  **/
  Value copysign(Value const& x, Value const& y);
} }
#endif

#include <boost/simd/function/scalar/copysign.hpp>
#include <boost/simd/function/simd/copysign.hpp>

#endif


#ifndef UTIL_HH
#define UTIL_HH

#include <algorithm>
#include <vector>

// Visual Studio math.h doesn't have log1p and log1pf functions varjokal 24.3.2010
// This needs to be defined for VS varjokal 24.3.2010
#ifdef _MSC_VER
#include <boost/math/tr1.hpp>
using namespace boost::math::tr1;
#endif

#include <math.h>

/** Common utility functions. */
namespace util {

  /** Function evaluation object for search function(s) */
  class FuncEval {
  public:
    virtual double evaluate_function(double p) = 0;
    virtual ~FuncEval() {}
  };
  /** Binary search for finding the parameter value with which the
   * given function evaluates to max_value within the given accuracy. */
  double bin_search_param_max_value(double lower_bound, double low_value,
                                    double upper_bound, double up_value,
                                    double max_value, double value_acc,
                                    double param_acc, FuncEval &f);

  /** The square of the value. */
  template <typename T>
  T sqr(T a)
  {
    return a * a;
  }

  /** Median of the values in a vector.
   * \note For odd number (2n + 1) of values, the n'th value is returned.
   */
  template <typename T>
  T
  median(std::vector<T> v)
  {
    std::sort(v.begin(), v.end());
    return v[v.size() / 2];
  }

  /** Absolute value. */
  template <typename T>
  T
  abs(const T &value)
  {
    if (value < 0)
      return -value;
    return value;
  }

  /** Maximum of two values. */
  template <typename T>
  T
  max(const T &a, const T &b)
  {
    if (a < b)
      return b;
    return a;
  }

  inline float
  log10addf(float a, float b)
  {
    // M_LN10 is not part of C++ standard and not defined in every compiler.
    const float LN10 = 2.30258509299404568402;
    const float LOG10TOe = LN10;
    const float LOGeTO10 = 1.0 / LN10;

    a = a * LOG10TOe;
    b = b * LOG10TOe;

    float delta = a - b;
    if (delta > 64.0) {
      b += 64;
      delta = -delta;
    }
    return (b + log1pf(exp(delta))) * LOGeTO10;
  }

  inline double
  log10add(double a, double b)
  {
    // M_LN10 is not part of C++ standard and not defined in every compiler.
    const double LN10 = 2.30258509299404568402;
    const double LOG10TOe = LN10;
    const double LOGeTO10 = 1.0 / LN10;

    a = a * LOG10TOe;
    b = b * LOG10TOe;

    double delta = a - b;
    if (delta > 64.0) {
      b += 64;
      delta = -delta;
    }
    return (b + log1p(exp(delta))) * LOGeTO10;
  }

  inline float
  logaddf(float a, float b)
  {
    float delta = a - b;
    if (delta > 0) {
      b = a;
      delta = -delta;
    }
    return b + log1pf(expf(delta));
  }

  inline double
  logadd(double a, double b)
  {
    double delta = a - b;
    if (delta > 0) {
      b = a;
      delta = -delta;
    }
    return b + log1p(exp(delta));
  }

  static const double tiny_for_log = 1e-50;
  inline double safe_log(double x)
  {
    if (x < tiny_for_log)
      return log(tiny_for_log);
    else
      return log(x);
  }

  /** Compute modulo of two values so that negative arguments are
   * handled correctly. */
  inline int modulo(int a, int b)
  {
    int result = a % b;
    if (result < 0)
      result += b;
    return result;
  }

  inline float sinc(float x)
  {
    // M_PI is not part of C++ standard and not defined in every compiler.
    const double PI = 3.14159265358979323846;
    if (fabs(x) < 1e-8)
      return 1;
    double y = PI*x;
    return sin(y)/y;
  }

};

#endif /* UTIL_HH */


#include "ktamura.h"

#include "kprogressbar.h"
#include "common.h"
#include "kutility.h"
#include "kpicinfo.h"
#include "kimagecvt.h" //for image convert

#include <boost/math/special_functions/powm1.hpp>
#include <boost/math/constants/constants.hpp>

#include <vector>
#include <map>
#include <utility>
#include <limits>
#include <cmath>

#include <QRegularExpression>
#include <QCoreApplication>
#include <QDir>

using std::vector;
using std::map;
using std::make_pair;

KTamura::KTamura(QString input)
    :m_iMaxExtend(32)
{
    QRegularExpression re("[\\\\/]");

    GDALDataset * piDataset = (GDALDataset *) GDALOpen(input.toUtf8().constData(), GA_ReadOnly );

    K_OPEN_ASSERT(piDataset,input.toStdString());

    m_sOutputRoot = getDirRoot(input);

    QString tempName=input.right(input.length()-input.lastIndexOf(re)-1);
    m_sFileNoExtName=tempName.left(tempName.lastIndexOf("."));

    GDALDataset *poDataset = NULL;

    if((poDataset = KImageCvt::img2gray(piDataset,poDataset,m_sOutputRoot+"temp"+QDir::separator()+m_sFileNoExtName+"-gray")) == NULL) { std::cout<<"image convert failed\a"<<std::endl; exit(1); }

    m_lXSize = piDataset->GetRasterXSize();
    m_lYSize = piDataset->GetRasterYSize();
    m_fOrgArray = (float *) CPLMalloc(sizeof(float)*m_lXSize*m_lYSize);
    m_fExtArray = (float *) CPLMalloc(sizeof(float)*(m_lXSize+2*m_iMaxExtend)*(m_lYSize+2*m_iMaxExtend));

    // gray only
    GDALRasterBand * poBand = poDataset->GetRasterBand(1);
    poBand->RasterIO( GF_Read, 0, 0, m_lXSize, m_lYSize, m_fOrgArray, m_lXSize, m_lYSize, GDT_Float32, 0, 0 );

    KUtility::reflectExtend<float>(m_fOrgArray,m_fExtArray,m_lXSize,m_lYSize,m_iMaxExtend);

    GDALClose(piDataset);
    GDALClose(poDataset);
}

KTamura::~KTamura()
{
    CPLFree(m_fOrgArray);
    CPLFree(m_fExtArray);
}

void KTamura::build()
{

    m_dCoarseness = calCoarseness();
    m_dContrast = calContrast();
    m_dDirectionality = calDirectionality();
}

double KTamura::calCoarseness()
{
    map<int,std::pair<double,double> > mapAvrGray;
    double coarseness = 0.;
    float * tempOut = new float[m_lXSize*m_lYSize];

    unsigned long int lineLength = m_lXSize+2*m_iMaxExtend;
    KProgressBar progressBar("CalCoarseness",m_lXSize*m_lYSize,80);
    K_PROGRESS_START(progressBar);
    long int orgY = m_iMaxExtend;
    for(unsigned long int desY = 0;orgY<m_lYSize+m_iMaxExtend;++orgY,++desY){
        long int orgX = m_iMaxExtend;
        for(unsigned long int desX = 0;orgX<m_lXSize+m_iMaxExtend;++orgX,++desX){
            mapAvrGray.clear();
            double hDiff = abs(m_fExtArray[orgY*lineLength+orgX-1]-m_fExtArray[orgY*lineLength+orgX+1]);
            double vDiff = abs(m_fExtArray[(orgY-1)*lineLength+orgX]-m_fExtArray[(orgY+1)*lineLength+orgX]);
            mapAvrGray[0] = make_pair(hDiff,vDiff);

            for(int k = 1;k < 6;k++){
                long binSize = boost::math::powm1(2,k)+1;
                hDiff = abs(getAverageGray(m_fExtArray,lineLength,orgX-binSize/2,orgY,binSize)-getAverageGray(m_fExtArray,lineLength,orgX+binSize/2,orgY,binSize));
                vDiff = abs(getAverageGray(m_fExtArray,lineLength,orgX,orgY-binSize/2,binSize)-getAverageGray(m_fExtArray,lineLength,orgX,orgY+binSize/2,binSize));
                mapAvrGray[k] = make_pair(hDiff,vDiff);
            }

            double eMax = (std::numeric_limits<double>::min)();
            int kMax=-1;
            // find the max E
            for(map<int,std::pair<double,double> >::iterator it = mapAvrGray.begin();
                it != mapAvrGray.end(); ++it){
                double tempMax = std::max(it->second.first,it->second.second);
                if(eMax<tempMax){ kMax=it->first; eMax=tempMax; }
            }
            // find optimal E & k
            int tempKMax=kMax;
            for(map<int,std::pair<double,double> >::iterator it = mapAvrGray.begin();
                it != mapAvrGray.end(); ++it){
                if(it->first>kMax){
                    double tempMax = std::max(it->second.first,it->second.second);
                    if(tempMax>0.9*eMax){
                        if(tempKMax<it->first) tempKMax = it->first;
                    }
                }
            }
            kMax=tempKMax;
            tempOut[desY*m_lXSize+desX] = boost::math::powm1(2,kMax)+1;
            progressBar.autoUpdate();
        }
    }
    K_PROGRESS_END(progressBar);

    for(long iY = 0;iY<m_lYSize;++iY){
        for(long iX = 0;iX<m_lXSize;++iX){
            coarseness += tempOut[iY*m_lXSize+iX];
        }
    }

    coarseness = coarseness / (m_lXSize*m_lYSize);

    delete [] tempOut;
    return coarseness;
}

double KTamura::calContrast()
{
    double alpha4 = 0.;
    double miu4 = 0.;
    double delta2 = 0.;
    double delta = 0.;
    double avr = 0.;
    double contrast = 0.;
    /* alpha4 = Sum((Yi - Yavr)^4)/((N-1)*delta^4) */
    // get Average
    for(long int iY = 0;iY<m_lYSize;++iY){
        for(long int iX = 0;iX<m_lXSize;++iX){
            avr+=m_fOrgArray[iY*m_lXSize+iX];
        }
    }
    avr /= (m_lYSize*m_lXSize);
    // get Variance and mean fourth
    for(long int iY = 0;iY<m_lYSize;++iY){
        for(long int iX = 0;iX<m_lXSize;++iX){
            double tempValue = m_fOrgArray[iY*m_lXSize+iX];
            double temp = 0.;
            temp = abs(tempValue-avr);
            delta2 += temp * temp;
            miu4 += delta2 * delta2;
        }
    }
    delta2 /= (m_lYSize*m_lXSize);
    delta = boost::math::powm1(delta2,0.5)+1;
    miu4 /= (m_lYSize*m_lXSize - 1);
    alpha4 = miu4 / (delta2*delta2);
    contrast = delta/(boost::math::powm1(alpha4,0.25)+1);
    return contrast;
}

double KTamura::calDirectionality()
{
    const int quantizationLevel = 16;
    const int thresT = 12;
    double directionality = 0.;
    float * tempDeltaG = new float[m_lXSize*m_lYSize];
    float * tempTheta = new float[m_lXSize*m_lYSize];
    long * histogramD = new long[quantizationLevel];
    long imageLength = m_lXSize*m_lYSize;
    unsigned long int lineLength = m_lXSize+2*m_iMaxExtend;
    //memset(histogramD,0,quantizationLevel);
    for(int index = 0;index < quantizationLevel;++index){
        histogramD[index]=0;
    }
    long int orgY = m_iMaxExtend;
    for(unsigned long int desY = 0;orgY<m_lYSize+m_iMaxExtend;++orgY,++desY){
        long int orgX = m_iMaxExtend;
        for(unsigned long int desX = 0;orgX<m_lXSize+m_iMaxExtend;++orgX,++desX){
            float tempDeltaH=0.;
            float tempDeltaV=0.;
            tempDeltaH = m_fExtArray[(orgY-1)*lineLength+orgX+1]+m_fExtArray[orgY*lineLength+orgX+1]+m_fExtArray[(orgY+1)*lineLength+orgX+1]-
                         m_fExtArray[(orgY-1)*lineLength+orgX-1]-m_fExtArray[orgY*lineLength+orgX-1]-m_fExtArray[(orgY+1)*lineLength+orgX-1];
            tempDeltaV = m_fExtArray[(orgY-1)*lineLength+orgX-1]+m_fExtArray[(orgY-1)*lineLength+orgX]+m_fExtArray[(orgY-1)*lineLength+orgX+1]-
                         m_fExtArray[(orgY+1)*lineLength+orgX-1]-m_fExtArray[(orgY+1)*lineLength+orgX]-m_fExtArray[(orgY+1)*lineLength+orgX+1];
            tempDeltaG[desY*m_lXSize+desX] = (abs(tempDeltaV) + abs(tempDeltaH))/2;
            if(tempDeltaH<0.0001&&tempDeltaH>0.) tempDeltaH=0.0001;
            if(tempDeltaH>-0.0001&&tempDeltaH<0.) tempDeltaH=-0.0001;
            if(tempDeltaH==0.) tempDeltaH+=0.00001;
            tempTheta[desY*m_lXSize+desX] = atan(tempDeltaV/tempDeltaH)+boost::math::constants::pi<float>()/2;
            //if(tempTheta[desY*m_lXSize+desX]<0) tempTheta[desY*m_lXSize+desX]+=boost::math::constants::pi<float>();
            //qDebug()<<"DeltaG"<<tempDeltaG[desY*m_lXSize+desX]<<"DeltaH:"<<tempDeltaH<<"DeltaV:"<<tempDeltaV<<"tempTheta:"<<tempTheta[desY*m_lXSize+desX];
        }
    }
    // get HistogramD
    float sumHistogramD = 0.;
    for(int index = 0;index < quantizationLevel;++index){
        //qDebug()<<"start:"<<histogramD[index];
        float thetaLow = (2*index)*boost::math::constants::pi<float>()/(2*quantizationLevel);
        float thetaHigh = (2*index+2)*boost::math::constants::pi<float>()/(2*quantizationLevel);
        //qDebug()<<"thetaLow"<<thetaLow<<"thetaHigh"<<thetaHigh;

        for(long imageIndex = 0;imageIndex < imageLength;++imageIndex){
            //qDebug()<<tempTheta[imageIndex];
            if(tempDeltaG[imageIndex]>thresT){
                if(tempTheta[imageIndex]>=thetaLow&&tempTheta[imageIndex]<thetaHigh){
                    histogramD[index]+=1;
                }
            }
        }
        //qDebug()<<"a:"<<a;
        //qDebug()<<"histogramD:"<<histogramD[index];
        sumHistogramD += histogramD[index];
    }
//    qDebug()<<sumHistogramD;
//    for(int index = 0;index < quantizationLevel;++index){
//        //qDebug()<<"1:"<<histogramD[index];
//        histogramD[index]=histogramD[index];
//        //qDebug()<<"2:"<<histogramD[index];
//    }
//    QString temp("");
//    for(int index = 0;index < quantizationLevel;++index){
//        temp+=QString("%1:%2 ").arg(index).arg(histogramD[index]);
//    }
//    qDebug()<<temp;
    // get Directionality
    // find all the valley and it's position
    vector<std::pair<int,float> > tempValleyArray;
    vector<std::pair<int,float> > valleyArray;
    vector<std::pair<int,float> > finalValley;
    int firstPeak = getNextValleyPeak(histogramD,0,quantizationLevel,true);
    int lastPos = firstPeak;
    tempValleyArray.push_back(make_pair(firstPeak,histogramD[firstPeak]));
    int tempPeakValley = getNextValleyPeak(histogramD,lastPos,quantizationLevel);
    while((lastPos>=firstPeak && tempPeakValley>lastPos) || tempPeakValley < firstPeak){
        tempValleyArray.push_back(make_pair(tempPeakValley, histogramD[tempPeakValley]));
        lastPos = tempPeakValley;
        tempPeakValley = getNextValleyPeak(histogramD, lastPos, quantizationLevel);
    }
    //qDebug()<<"tempValleyArray"<<tempValleyArray.size();
    //for(unsigned int index = 0;index < tempValleyArray.size();index++){
    //    qDebug()<<tempValleyArray[index].second;
    //}
    float maxPeakRate = (std::numeric_limits<float>::min)();
    int maxRatePos = -1;
    // remove the peak and valley which peak/valley<2.0
    for(unsigned int index = 0;index < tempValleyArray.size();index+=2){
        float tempRate = tempValleyArray[index].second/tempValleyArray[index+1].second;
        if(tempRate>maxPeakRate){ maxPeakRate = tempRate; maxRatePos = index; }
        if(tempRate>2.){
            if(0==index) valleyArray.push_back(tempValleyArray[tempValleyArray.size()-1]);
            else valleyArray.push_back(tempValleyArray[index-1]);
            valleyArray.push_back(tempValleyArray[index]);
            valleyArray.push_back(tempValleyArray[index+1]);
        }
    }
    if(0 == valleyArray.size() && maxPeakRate>1.){
        if(0==maxRatePos) valleyArray.push_back(tempValleyArray[tempValleyArray.size()-1]);
        else valleyArray.push_back(tempValleyArray[maxRatePos-1]);
        valleyArray.push_back(tempValleyArray[maxRatePos]);
        valleyArray.push_back(tempValleyArray[maxRatePos+1]);
    }
    // get the mian peak and the secondmain peak
    float maxPeak = (std::numeric_limits<float>::min)();
    int maxPos = -1;
    float secondMaxPeak = (std::numeric_limits<float>::min)();
    int secondMaxPos = -1;
    //qDebug()<<"valleySize:"<<valleyArray.size();
    for(unsigned int index = 1;index < valleyArray.size();index+=3){
        if(valleyArray[index].second>maxPeak){ maxPeak = valleyArray[index].second; maxPos=index; }
    }
    for(unsigned int index = 1;index < valleyArray.size();index+=3){
        if(static_cast<int>(index)!=maxPos && valleyArray[index].second>secondMaxPeak){ secondMaxPeak = valleyArray[index].second; secondMaxPos=index; }
    }
    //qDebug()<<"max:"<<maxPos;
    if(-1 == maxPos){
        delete [] tempDeltaG;
        delete [] tempTheta;
        delete [] histogramD;
        return 0.;
    }
    //for(unsigned int index = 0;index < tempValleyArray.size();index++)
    //        qDebug()<<index<<":"<<tempValleyArray[index].first;
    //for(unsigned int index = 0;index < valleyArray.size();index++)
    //    qDebug()<<index<<":"<<valleyArray[index].first;
    finalValley.push_back(valleyArray[maxPos-1]);
    finalValley.push_back(valleyArray[maxPos]);
    finalValley.push_back(valleyArray[maxPos+1]);
    if(-1 != secondMaxPos){
        if(maxPeak/secondMaxPeak<5.){
            finalValley.push_back(valleyArray[secondMaxPos-1]);
            finalValley.push_back(valleyArray[secondMaxPos]);
            finalValley.push_back(valleyArray[secondMaxPos+1]);
        }
    }

    int nPeaks = finalValley.size()/3;
//    vector<long> doubleHistogramD;
//    for(int index = 0;index<2*quantizationLevel;++index){
//        doubleHistogramD.push_back(histogramD[index%quantizationLevel]);
//    }
    for(unsigned int index = 1;index < finalValley.size();index+=3){
        vector<int> realOrder;
        int start=0;
        int peak=0;
        int end=0;
        realOrder.push_back(finalValley[index-1].first);
        realOrder.push_back(finalValley[index-1].first+quantizationLevel);
        realOrder.push_back(finalValley[index].first);
        realOrder.push_back(finalValley[index].first+quantizationLevel);
        realOrder.push_back(finalValley[index+1].first);
        realOrder.push_back(finalValley[index+1].first+quantizationLevel);

        std::sort(realOrder.begin(),realOrder.end());
        //for(unsigned int index = 0;index < realOrder.size();index++)
        //    qDebug()<<realOrder[index];
        vector<int>::iterator it=realOrder.begin();
        if((it=std::find(realOrder.begin(),realOrder.end(),finalValley[index].first))!=realOrder.begin()){
            it--;
            start = *it++;
            peak = *it++;
            end = *it++;
        }else{
//            if(realOrder[0]==realOrder[1]){
//                start = realOrder[0];
//                peak = realOrder[1];
//                end = realOrder[2];
//            }else{
            it=std::find(realOrder.begin(),realOrder.end(),finalValley[index].first+quantizationLevel);
            it--;
            start = *it++;
            peak = *it++;
            end = *it++;
//            }
        }
        double tempDir = 0.;
        for(int position = start;position<end+1;++position){
            double temp=abs(peak-position);
            tempDir += temp*temp*histogramD[position%quantizationLevel];
        }

        directionality+=tempDir;
    }
    // calculate sharpness of the peak
//    int nPeaks = finalValley.size()/3;
//    for(unsigned int index = 1;index < finalValley.size();index+=3){
//        double tempDir = 0.;
//        // first valley after peak
//        if(finalValley[index-1].first>finalValley[index].first){
//            for(int position = finalValley[index-1].first;position<quantizationLevel;++position){
//                double temp=quantizationLevel-position+finalValley[index].first;
//                tempDir += temp*temp*histogramD[position];
//            }
//            for(int position = 0;position<finalValley[index+1].first;++position){
//                double temp=position-finalValley[index].first;
//                tempDir += temp*temp*histogramD[position];
//            }
//            directionality+=tempDir;
//            continue;
//        }
//        // second valley before peak
//        if(finalValley[index+1].first<finalValley[index].first){
//            for(int position = finalValley[index-1].first;position<quantizationLevel;++position){
//                double temp=position-finalValley[index].first;
//                tempDir += temp*temp*histogramD[position];
//            }
//            for(int position = 0;position<finalValley[index+1].first;++position){
//                double temp=quantizationLevel+position-finalValley[index].first;
//                tempDir += temp*temp*histogramD[position];
//            }
//            directionality+=tempDir;
//            continue;
//        }
//        for(int position = finalValley[index-1].first;position<finalValley[index+1].first;++position){
//            double temp=position-finalValley[index].first;
//            tempDir += temp*temp*histogramD[position];
//        }
//        directionality+=tempDir;
//    }

    delete [] tempDeltaG;
    delete [] tempTheta;
    delete [] histogramD;
    //qDebug()<<nPeaks*directionality*1./sumHistogramD;
    return nPeaks*directionality*1./sumHistogramD;//*(boost::math::powm1(10,23)+1);
}

int KTamura::getNextValleyPeak(long *histogram, int start, int length,bool bePeak)
{
    static bool bePeakValley = false;
    if(bePeak) bePeakValley = false;
    bePeakValley = !bePeakValley;
    start %= length;
    int tempPos = start;
    for (int index = start; index < start + length; ++index){
        if (bePeakValley){
            if (histogram[index%length]<histogram[(index + 1) % length]){
                for (; index < start + length; ++index){
                    if (histogram[index%length] >= histogram[(index + 1) % length]) break;
                }
                tempPos = index;// %length;
                if (histogram[index%length] == histogram[(index + 1) % length]){
                    for (; index < start + length; ++index){
                        if (histogram[index%length]>histogram[(index + 1) % length]) break;
                    }
                }
                tempPos = ((tempPos + index) / 2) % length;
                break;
            }
        }
        else{
            if (histogram[index%length]>histogram[(index + 1) % length]){
                for (; index < start + length; ++index){
                    if (histogram[index%length] <= histogram[(index + 1) % length]) break;
                }
                tempPos = index;//%length;
                if (histogram[index%length] == histogram[(index + 1) % length]){
                    for (; index < start + length; ++index){
                        if (histogram[index%length]<histogram[(index + 1) % length]) break;
                    }
                }
                tempPos = ((tempPos + index) / 2) % length;
                break;
            }
        }
    }
    return tempPos;
}

double KTamura::getAverageGray(float * image, unsigned long lineLength, unsigned long lX, unsigned long lY, long size)
{
    double sumGray = 0.;
    for(unsigned long nY = lY-size/2; nY < lY+size/2; ++nY){
        for(unsigned long nX = lX-size/2; nX < lX+size/2; ++nX){
            sumGray += image[lineLength*nY+nX];
        }
    }
    sumGray = sumGray/(size*size);
    return sumGray;
}

QString KTamura::getDirRoot(QString filename)
{
    QString tempRet("");
    QRegularExpression re("[\\\\/]+");

    if(filename.contains('\\')||filename.contains('/')){
        QStringList tempList = filename.split(re);
        for(int pos = 0;pos<tempList.length()-1;++pos)
        {
            tempRet+=tempList[pos];
            tempRet+=QDir::separator();
        }
    }else{
        tempRet=QCoreApplication::applicationDirPath()+QDir::separator();
    }
    return tempRet;
}

QString KTamura::getSVMString(int start)
{
    QString temp("");
    for(int index= start;index<start+3;++index){
        temp+=QString("%1:%%2 ").arg(index).arg(index-start+1);
    }
    return QString(temp).arg(m_dCoarseness).arg(m_dContrast).arg(m_dDirectionality);
}


////////////////////////////////////////////////////////////////////////////////
#include "tri2hex.h"
#include <Eigen/Dense>
#include <vector>
#include <iostream>
////////////////////////////////////////////////////////////////////////////////

namespace cellogram {

// -----------------------------------------------------------------------------

void adjacency_list(const Eigen::MatrixXi &F, std::vector<std::vector<int>> &adj) {
	adj.clear();
	int n = F.maxCoeff() + 1;
	adj.resize(n);

	for (int i = 0; i < F.rows(); ++i) {
		for (int j = 0; j < 3; ++j) {
			int v1 = F(i, j);
			int v2 = F(i, (j + 1) % 3);
			adj[v1].push_back(v2);
			adj[v2].push_back(v1);
		}
	}

	for (int i = 0; i < n; i++)
	{
		std::sort(adj[i].begin(), adj[i].end());
		auto it = std::unique(adj[i].begin(), adj[i].end());
		adj[i].resize(std::distance(adj[i].begin(), it));
	}

}

void triangle_region_list(const Eigen::VectorXi &vertex_region_id, const Eigen::MatrixXi &F, Eigen::VectorXd &face_region_id) {
	face_region_id = Eigen::VectorXd::Zero(F.rows());
	for (size_t i = 0; i < vertex_region_id.size(); i++)
	{
		for (size_t j = 0; j < F.rows(); j++)
		{
			if (F(j, 0) == i || F(j, 1) == i || F(j, 2) == i)
			{
				face_region_id(j) = (double)vertex_region_id(i);
			}
		}
	}
}

// -----------------------------------------------------------------------------

} // namespace cellogram


#include <igl/opengl/glfw/Viewer.h>
#include <igl/opengl/glfw/imgui/ImGuiMenu.h>
#include <igl/opengl/glfw/imgui/ImGuiHelpers.h>
#include <imgui/imgui.h>

#include <boost/algorithm/string.hpp>

#include <igl/pathinfo.h>

#include "Dog/Dog.h"
#include "ModelState.h"
#include "ModelViewer.h"

using namespace std;

bool is_optimizing = true;

ModelState state;
ModelViewer modelViewer(state, state.DC);

const int DEFAULT_GRID_RES = 21;
int editedSubmeshI = -1; // -1 means the entire mesh, i means the i connected component submesh

void clear_all_and_set_default_params(igl::opengl::glfw::Viewer& viewer) {
  state.DC.init_from_new_dog(state.dog);
}

void run_optimization() {
  state.DC.single_optimization();
}

bool callback_key_down(igl::opengl::glfw::Viewer& viewer, unsigned char key, int modifiers)
{
  switch (key) {
  case ' ':
    is_optimizing = !is_optimizing;
    break;
  case 'S':
    state.DC.edit_mode = DogEditor::SELECT_POSITIONAL;
    break;
  case 'D':
    state.DC.edit_mode = DogEditor::TRANSLATE;
    break;
  case 'C':
    state.DC.reset_constraints();
    break;
  case 'R':
    state.DC.single_optimization();
    break;
  case 'E':
    exit(1);
    break;
  }
  return false;
}

bool callback_mouse_down(igl::opengl::glfw::Viewer& viewer, int button, int modifier) {
  if ((modelViewer.viewMode == ViewRulings) || (modelViewer.viewMode == ViewModeMeshWire)) return state.DC.dogEditor->callback_mouse_down();
  return false;
}
bool callback_mouse_move(igl::opengl::glfw::Viewer& viewer, int mouse_x, int mouse_y) {
  if ((modelViewer.viewMode == ViewRulings) || (modelViewer.viewMode == ViewModeMeshWire)) return  state.DC.dogEditor->callback_mouse_move(mouse_x, mouse_y);
  return false;
}
bool callback_mouse_up(igl::opengl::glfw::Viewer& viewer, int button, int modifier) {
  if ((modelViewer.viewMode == ViewRulings) || (modelViewer.viewMode == ViewModeMeshWire)) return  state.DC.dogEditor->callback_mouse_up();
  return false;
}

bool callback_pre_draw(igl::opengl::glfw::Viewer& viewer) {
  if ( ((state.DC.has_constraints())) && is_optimizing) run_optimization();
  modelViewer.render(viewer);
  return false;
}

int main(int argc, char *argv[]) {
  if (argc < 2) {
    cout << "Usage: dog_editor INPUT_PATH.OBJ or dog_editor planar (optional width and height)" << endl;
    exit(1);
  }
  const std::string input_path = argv[1];
  std::string dirname,basename,extension,filename; igl::pathinfo(input_path, dirname, basename, extension, filename);
  if (boost::iequals(basename, "planar")) {
    int x_res,y_res; x_res = y_res = DEFAULT_GRID_RES;
    if (argc > 2) {x_res = y_res = std::stoi(argv[2]);};
    state.init_from_planar(x_res,y_res);
  } else {
    // Assume obj/off or other types
    state.init_from_mesh(input_path);
  }

  // Set up viewer
  igl::opengl::glfw::Viewer viewer;
  state.DC.init_viewer(viewer);
  // Attach a menu plugin
  igl::opengl::glfw::imgui::ImGuiMenu menu;
  viewer.plugins.push_back(&menu);

  //cout<<"hi main.cpp line 99\n";
  /*
	menu.callback_draw_viewe_menu = [&]()
	{
		// Draw parent menu content
		menu.draw_viewer_menu();
      cout<<"hi main.cpp line 106\n";
	};
*/

  menu.callback_draw_custom_window = [&]()
  {
    // Define next window position + size
    //cout<<"hi main.cpp l104\n";
    ImGui::SetNextWindowPos(ImVec2(180.f * menu.menu_scaling(), 10), ImGuiSetCond_FirstUseEver);
    //cout<<"hi main.cpp l106\n";
    ImGui::SetNextWindowSize(ImVec2(210, 350), ImGuiSetCond_FirstUseEver);
    ImGui::Begin(
        "DOG", nullptr,
        ImGuiWindowFlags_NoSavedSettings
    );
    ImGui::Combo("View mode", (int *)(&modelViewer.viewMode), "MeshWire\0Gauss Map\0Rulings\0\0");
    ImGui::Combo("Edit mode", (int *)(&state.DC.edit_mode), "Select\0Translate\0None\0\0");

    ImGui::InputDouble("Bending", &state.DC.p.bending_weight, 0, 0, "%.4f");
    ImGui::InputDouble("Isometry", &state.DC.p.isometry_weight, 0, 0, "%.4f");
    ImGui::InputDouble("Edge regularization", &state.DC.p.reg_edge_weight, 0, 0, "%.4f");
    ImGui::InputDouble("Soft constraints", &state.DC.p.soft_pos_weight, 0, 0, "%.4f");

    ImGui::InputDouble("Constraints deviation", &state.DC.constraints_deviation);
    ImGui::InputDouble("objective", &state.DC.objective);
    ImGui::Checkbox("Is optimizing?", &is_optimizing);

    ImGui::InputDouble("Rulings length", &modelViewer.rulings_length);
    ImGui::InputInt("Rulings modulo", &modelViewer.rulings_mod);

    ImGui::End();
  };

  clear_all_and_set_default_params(viewer);

  //cout<<"hi main.cpp l129\n";
  viewer.data().set_mesh(state.dog.getVrendering(), state.dog.getFrendering());
  viewer.core.align_camera_center(state.dog.getVrendering(), state.dog.getFrendering());

  viewer.callback_key_down = callback_key_down;
  viewer.callback_pre_draw = callback_pre_draw; // calls at each frame
  viewer.core.is_animating = true;
  viewer.core.animation_max_fps = 30;
  viewer.callback_mouse_down = callback_mouse_down;
  viewer.callback_mouse_move = callback_mouse_move;
  viewer.callback_mouse_up = callback_mouse_up;
  viewer.data().line_width = 2;

  viewer.data().show_lines = false;
  viewer.launch();
}