/**
 * Copyright (c) 2011-2012, Georgia Tech Research Corporation
 * All rights reserved.
 *
 * Author: Tobias Kunz <tobias@gatech.edu>
 * Date: 05/2012
 *
 * Humanoid Robotics Lab      Georgia Institute of Technology
 * Director: Mike Stilman     http://www.golems.org
 *
 * Algorithm details and publications:
 * http://www.golems.org/node/1570
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */

#pragma once

#include <Eigen/Core>
#include <list>
#include <map>
#include <memory>
#include <vector>

#include <RTmotion/algorithm/trajectory.hpp>

namespace trajectory_processing
{
class PathSegment
{
public:
  PathSegment(double length = 0.0) : length_(length)
  {
  }
  virtual ~PathSegment()  // is required for destructing derived classes
  {
  }
  double getLength() const
  {
    return length_;
  }
  virtual Eigen::VectorXd getConfig(double s) const = 0;
  virtual Eigen::VectorXd getTangent(double s) const = 0;
  virtual Eigen::VectorXd getCurvature(double s) const = 0;
  virtual std::list<double> getSwitchingPoints() const = 0;
  virtual PathSegment* clone() const = 0;

  double position_;

protected:
  double length_;
};

class Path
{
public:
  Path(const std::list<Eigen::VectorXd>& path, double max_deviation = 0.0);
  Path(const Path& path);
  double getLength() const;
  Eigen::VectorXd getConfig(double s) const;
  Eigen::VectorXd getTangent(double s) const;
  Eigen::VectorXd getCurvature(double s) const;
  double getNextSwitchingPoint(double s, bool& discontinuity) const;
  std::list<std::pair<double, bool>> getSwitchingPoints() const;

private:
  PathSegment* getPathSegment(double& s) const;
  double length_;
  std::list<std::pair<double, bool>> switching_points_;
  std::list<std::unique_ptr<PathSegment>> path_segments_;
};

class Trajectory
{
public:
  /// @brief Generates a time-optimal trajectory
  Trajectory(const Path& path, const Eigen::VectorXd& max_velocity,
             const Eigen::VectorXd& max_acceleration, double time_step = 0.001);

  ~Trajectory(void);

  /** @brief Call this method after constructing the object to make sure the
     trajectory generation succeeded without errors. If this method returns
     false, all other methods have undefined behavior. **/
  bool isValid() const;

  /// @brief Returns the optimal duration of the trajectory
  double getDuration() const;

  /** @brief Return the position/configuration vector for a given point in time
   */
  Eigen::VectorXd getPosition(double time) const;
  /** @brief Return the velocity vector for a given point in time */
  Eigen::VectorXd getVelocity(double time) const;
  /** @brief Return the acceleration vector for a given point in time */
  Eigen::VectorXd getAcceleration(double time) const;

private:
  struct TrajectoryStep
  {
    TrajectoryStep()
    {
    }
    TrajectoryStep(double path_pos, double path_vel)
      : path_pos_(path_pos), path_vel_(path_vel)
    {
    }
    double path_pos_;
    double path_vel_;
    double time_;
  };

  bool getNextSwitchingPoint(double path_pos,
                             TrajectoryStep& next_switching_point,
                             double& before_acceleration,
                             double& after_acceleration);
  bool getNextAccelerationSwitchingPoint(double path_pos,
                                         TrajectoryStep& next_switching_point,
                                         double& before_acceleration,
                                         double& after_acceleration);
  bool getNextVelocitySwitchingPoint(double path_pos,
                                     TrajectoryStep& next_switching_point,
                                     double& before_acceleration,
                                     double& after_acceleration);
  bool integrateForward(std::list<TrajectoryStep>& trajectory,
                        double acceleration);
  void integrateBackward(std::list<TrajectoryStep>& start_trajectory,
                         double path_pos, double path_vel, double acceleration);
  double getMinMaxPathAcceleration(double path_position, double path_velocity,
                                   bool max);
  double getMinMaxPhaseSlope(double path_position, double path_velocity,
                             bool max);
  double getAccelerationMaxPathVelocity(double path_pos) const;
  double getVelocityMaxPathVelocity(double path_pos) const;
  double getAccelerationMaxPathVelocityDeriv(double path_pos);
  double getVelocityMaxPathVelocityDeriv(double path_pos);

  std::list<TrajectoryStep>::const_iterator
  getTrajectorySegment(double time) const;

  Path path_;
  Eigen::VectorXd max_velocity_;
  Eigen::VectorXd max_acceleration_;
  unsigned int joint_num_;
  bool valid_;
  std::list<TrajectoryStep> trajectory_;
  std::list<TrajectoryStep> end_trajectory_;  // non-empty only if the
                                              // trajectory generation failed.

  const double time_step_;

  mutable double cached_time_;
  mutable std::list<TrajectoryStep>::const_iterator cached_trajectory_segment_;
};

class TimeOptimalTrajectoryGeneration
{
public:
  TimeOptimalTrajectoryGeneration(const double path_tolerance = 0.1,
                                  const double resample_dt = 0.1);
  ~TimeOptimalTrajectoryGeneration();

  bool
  computeTimeStamps(RobotTrajectory& trajectory,
                    const double max_velocity_scaling_factor = 1.0,
                    const double max_acceleration_scaling_factor = 1.0) const;

private:
  const double path_tolerance_;
  const double resample_dt_;
};
}  // namespace trajectory_processing


#include <utility>
#include <boost/hana/tuple.hpp>
#include <boost/hana/functional/arg.hpp>
#include <boost/hana/equal.hpp>

// for print result
#include <iostream>
#include <boost/hana/for_each.hpp>

namespace hana = boost::hana;

template <auto ... args, auto ... i>
inline constexpr auto make_tuple_helper(std::index_sequence<i...>) noexcept
{
    return hana::make_tuple(hana::arg<i>(args...)...);
}

template <auto itr, auto end, auto ... args, auto ... result_indexes>
inline constexpr auto stalin_impl(std::index_sequence<result_indexes...>) noexcept
{
    if constexpr (sizeof...(args) == 0) return std::index_sequence<>{};

    else if constexpr (itr == end)
        return std::index_sequence<result_indexes...>{};

    else if constexpr (hana::arg<hana::arg<sizeof...(result_indexes)>(result_indexes...)>(args...)
         > hana::arg<itr + 1>(args...))

        return stalin_impl<itr + 1, end, args...>(
             std::index_sequence<result_indexes...>{}
        );

    else return stalin_impl<itr + 1, end, args...>(
        std::index_sequence<result_indexes..., itr + 1>{}
    );
}

template <auto ... args>
inline constexpr auto stalin_sort() noexcept
{
    return make_tuple_helper<args...>(
        stalin_impl<1, sizeof...(args), args...>(std::index_sequence<1>{})
    );
}

auto main() -> int
{
    static_assert(stalin_sort<>() == hana::make_tuple()); // sorted at compile time
    static_assert(stalin_sort<0>() == hana::make_tuple(0));
    static_assert(stalin_sort<2,3>() == hana::make_tuple(2, 3));
    static_assert(stalin_sort<2,1,3>() == hana::make_tuple(2, 3));
    static_assert(stalin_sort<2,1,3,3,2,5,6>() == hana::make_tuple(2,3,3,5,6));

    // print
    constexpr auto result = stalin_sort<3, 1, 4, 1, 5>();
    hana::for_each(result, [](const auto& x){ std::cout << x << " "; }); // 3 4 5
}


//==================================================================================================
/*!
  @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)
*/
//==================================================================================================
#include <boost/simd/pack.hpp>
#include <boost/simd/function/next.hpp>
#include <boost/simd/function/bits.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/constant/eps.hpp>
#include <boost/simd/constant/bitincrement.hpp>
#include <boost/simd/constant/two.hpp>

template <typename T, std::size_t N, typename Env>
void test(Env& $)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T, N>;

  T a1[N], b[N];
  for(std::size_t i = 0; i < N; ++i)
  {
    a1[i] = (i%2) ? T(i) : T(-i);
    b[i] = bs::next(a1[i]) ;
  }
  p_t aa1(&a1[0], &a1[0]+N);
  p_t bb (&b[0], &b[0]+N);
  STF_IEEE_EQUAL(bs::next(aa1), bb);
}

STF_CASE_TPL("Check next on pack" , STF_NUMERIC_TYPES)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T>;
  static const std::size_t N = bs::cardinal_of<p_t>::value;
  test<T, N>($);
  test<T, N/2>($);
  test<T, N*2>($);
}

STF_CASE_TPL (" next real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::next;
  using p_t = bs::pack<T>;
  using r_t = decltype(next(p_t()));

  // return type conformity test
  STF_TYPE_IS(r_t, p_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_EQUAL(next(bs::Inf<p_t>()), bs::Inf<r_t>());
  STF_EQUAL(next(bs::Minf<p_t>()), bs::Valmin<r_t>());
  STF_IEEE_EQUAL(next(bs::Nan<p_t>()), bs::Nan<r_t>());
  STF_EQUAL(next(bs::Valmax<p_t>()), bs::Inf<r_t>());
#endif
  STF_EQUAL(next(bs::Mone<p_t>()), bs::Mone<r_t>()+bs::Eps<r_t>()/2);
  STF_EQUAL(next(bs::One<p_t>()), bs::One<r_t>()+bs::Eps<r_t>());
  STF_EQUAL(next(bs::Zero<p_t>()), bs::Bitincrement<p_t>());
}

STF_CASE_TPL (" next unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::next;
  using p_t = bs::pack<T>;
  using r_t = decltype(next(p_t()));

  // return type conformity test
  STF_TYPE_IS(r_t, p_t);

  // specific values tests
  STF_EQUAL(next(bs::One<p_t>()), bs::Two<r_t>());
  STF_EQUAL(next(bs::Valmax<p_t>()), bs::Valmin<r_t>());
  STF_EQUAL(next(bs::Zero<p_t>()), bs::One<r_t>());
}

STF_CASE_TPL (" next signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::next;
  using p_t = bs::pack<T>;
  using r_t = decltype(next(p_t()));

  // return type conformity test
  STF_TYPE_IS(r_t, p_t);

  // specific values tests
  STF_EQUAL(next(bs::Mone<p_t>()), bs::Zero<r_t>());
  STF_EQUAL(next(bs::One<p_t>()), bs::Two<r_t>());
  STF_EQUAL(next(bs::Zero<p_t>()), bs::One<r_t>());
}


#define BOOST_TEST_MODULE "test_clementi_dihedral_potential"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <test/util/check_potential.hpp>
#include <mjolnir/forcefield/local/ClementiDihedralPotential.hpp>
#include <mjolnir/math/constants.hpp>

BOOST_AUTO_TEST_CASE(ClementiDihedral_double)
{
    using real_type = double;
    constexpr std::size_t N  = 1000;
    constexpr real_type   h  = 1e-6;
    constexpr real_type tol  = 1e-6;
    constexpr real_type   pi = mjolnir::math::constants<real_type>::pi();
    constexpr real_type   two_pi = 2.0 * pi;

    const real_type k1 = 1.0;
    const real_type k3 = 2.0;
    const real_type r0 = pi * 2. / 3.;

    mjolnir::ClementiDihedralPotential<real_type> potential(k1, k3, r0);

    const real_type x_min = -pi;
    const real_type x_max =  pi;

    mjolnir::test::check_potential(potential, x_min, x_max, tol, h, N);

    // periodicity
    const real_type dx = (x_max - x_min) / N;
    for(std::size_t i=0; i<N; ++i)
    {
        const real_type x = x_min + dx * i;
        BOOST_TEST(potential.potential(x) == potential.potential(x + two_pi),
                   boost::test_tools::tolerance(1e-8));
    }
}

BOOST_AUTO_TEST_CASE(ClementiDihedral_float)
{
    using real_type = float;
    constexpr std::size_t N  = 100;
    constexpr real_type   h  = 1e-2f;
    constexpr real_type tol  = 1e-2f;
    constexpr real_type   pi = mjolnir::math::constants<real_type>::pi();
    constexpr real_type   two_pi = 2.0 * pi;

    const real_type k1 = 1.0f;
    const real_type k3 = 2.0f;
    const real_type r0 = pi * 2. / 3.;

    mjolnir::ClementiDihedralPotential<real_type> potential(k1, k3, r0);

    const real_type x_min = 0.f;
    const real_type x_max = 2.f * pi;

    mjolnir::test::check_potential(potential, x_min, x_max, tol, h, N);

    // periodicity
    const real_type dx = (x_max - x_min) / N;
    for(std::size_t i=0; i<N; ++i)
    {
        const real_type x    = x_min + dx * i;
        BOOST_TEST(potential.potential(x) == potential.potential(x + two_pi),
                   boost::test_tools::tolerance(h));
   }
}


/*
 * Translates sensor_msgs/NavSat{Fix,Status} into nav_msgs/Odometry using UTM
 */

#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/NavSatStatus.h>
#include <sensor_msgs/NavSatFix.h>
#include <gps_common/conversions.h>
#include <nav_msgs/Odometry.h>

#include <geometry_msgs/TransformStamped.h>
#include <boost/shared_ptr.hpp>
#include <tf/transform_broadcaster.h>

using namespace gps_common;

static ros::Publisher odom_pub;
std::string frame_id, child_frame_id;
double rot_cov;
boost::shared_ptr<tf::TransformBroadcaster> tf_pub_;

void callback(const sensor_msgs::NavSatFixConstPtr& fix) {
  if (fix->status.status == sensor_msgs::NavSatStatus::STATUS_NO_FIX) {
    ROS_DEBUG_THROTTLE(60,"No fix.");
    return;
  }

  if (fix->header.stamp == ros::Time(0)) {
    return;
  }

  double northing, easting;
  std::string zone;

  LLtoUTM(fix->latitude, fix->longitude, northing, easting, zone);

  if (odom_pub) {
    nav_msgs::Odometry odom;
    odom.header.stamp = fix->header.stamp;

    if (frame_id.empty())
      odom.header.frame_id = fix->header.frame_id;
    else
      odom.header.frame_id = frame_id;

    odom.header.frame_id = "map";
    odom.child_frame_id = "base_link";

    odom.pose.pose.position.x = easting;
    odom.pose.pose.position.y = northing;
    odom.pose.pose.position.z = fix->altitude;

    odom.pose.pose.orientation.x = 0;
    odom.pose.pose.orientation.y = 0;
    odom.pose.pose.orientation.z = 0;
    odom.pose.pose.orientation.w = 1;

    // Use ENU covariance to build XYZRPY covariance
    boost::array<double, 36> covariance = {{
      fix->position_covariance[0],
      fix->position_covariance[1],
      fix->position_covariance[2],
      0, 0, 0,
      fix->position_covariance[3],
      fix->position_covariance[4],
      fix->position_covariance[5],
      0, 0, 0,
      fix->position_covariance[6],
      fix->position_covariance[7],
      fix->position_covariance[8],
      0, 0, 0,
      0, 0, 0, rot_cov, 0, 0,
      0, 0, 0, 0, rot_cov, 0,
      0, 0, 0, 0, 0, rot_cov
    }};

    odom.pose.covariance = covariance;

    odom_pub.publish(odom);

    geometry_msgs::TransformStamped tf;
    tf.header.frame_id = "map";
    tf.child_frame_id = "base_link";
    tf.header.stamp = ros::Time::now();
    tf.transform.translation.x = easting;
    tf.transform.translation.y = northing;
    tf.transform.translation.z = 0.0;
    tf.transform.rotation = odom.pose.pose.orientation;
    tf_pub_->sendTransform(tf);
  }
}

int main (int argc, char **argv) {
  ros::init(argc, argv, "utm_odometry_front_node");
  ros::NodeHandle node;
  ros::NodeHandle priv_node("~");

  priv_node.param<std::string>("frame_id", frame_id, "");
  priv_node.param<std::string>("child_frame_id", child_frame_id, "");
  priv_node.param<double>("rot_covariance", rot_cov, 99999.0);

  odom_pub = node.advertise<nav_msgs::Odometry>("odom_front", 10);

  ros::Subscriber fix_sub = node.subscribe("gps_front/fix", 10, callback);
  tf_pub_.reset(new tf::TransformBroadcaster);

  ros::spin();
}


#pragma once
#include "Core/Common.hpp"
#include "Vulkan/Utils.hpp"
#include <Eigen/Dense>

#define PI_CONST 3.141592653589f

struct TracingParameters
{
	SHADER_ALIGN Eigen::Vector3f CameraPosition;
	SHADER_ALIGN Eigen::Vector3f RayMin;
	SHADER_ALIGN Eigen::Vector3f RayDDx;
	SHADER_ALIGN Eigen::Vector3f RayDDy;
	SHADER_ALIGN Eigen::Vector3f MousePosition;
	int SelectionDiameter;
};

struct CameraSetup
{
	Eigen::Vector3f Position;
	Eigen::Vector3f Forward;
	Eigen::Vector3f Right;
};

class Camera
{
public:
	Camera(const Eigen::Vector3f& position, const Eigen::Vector3f& forward, const Eigen::Vector3f& right, float fov);
	Camera() = default;
	~Camera() = default;

	void Set(const CameraSetup& setup);
	void Rotate(const Eigen::Vector3f& axis, const float angle);
	void RotateLocal(const Eigen::Vector3f& axis, const float angle);

	void Move(const Eigen::Vector3f& offset);
	void MoveLocal(const Eigen::Vector3f& offset);

	Eigen::Vector3f& Position();
	Eigen::Vector3f& Forward();
	Eigen::Vector3f& Right();
	Eigen::Vector3f& Up();
	float& Fov();

	static float DegToRad(float value);
	static float RadToDeg(float value);

	void GetTracingParameters(uint32_t imageWidth, uint32_t imageHeight, TracingParameters& tracingParameters) const;

private:
	Eigen::Matrix3f GetLocalToGlobalMatrix() const;

	Eigen::Vector3f position_;
	Eigen::Vector3f forward_;
	Eigen::Vector3f right_;
	Eigen::Vector3f up_;
	float fov_;
};


/*
 * Copyright (C) 2018 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/

#include <cassert>
#include <iostream>
#include <set>

#include "plugins/robot.hh"

#include <ignition/common/SystemPaths.hh>
#include <ignition/plugin/SpecializedPluginPtr.hh>
#include <ignition/plugin/Loader.hh>

#ifdef HAVE_BOOST_PROGRAM_OPTIONS
#include <boost/program_options.hpp>
namespace bpo = boost::program_options;
#endif

/////////////////////////////////////////////////
// The macro that this uses is provided as a compile definition in the
// examples/CMakeLists.txt file.
const std::string PluginLibDir = IGN_PLUGIN_EXAMPLES_LIBDIR;

/////////////////////////////////////////////////
using namespace ignition::plugin::examples;

/////////////////////////////////////////////////
using RobotPluginPtr = ignition::plugin::SpecializedPluginPtr<
    Drive, ProximitySensor, GPSSensor, Compass, MapDatabase>;

/////////////////////////////////////////////////
using EnvironmentPluginPtr =
    ignition::plugin::SpecializedPluginPtr<Environment>;

using PointPair = std::pair<ignition::math::Vector2d, ignition::math::Vector2d>;

/////////////////////////////////////////////////
using IntersectionResult = std::pair<bool, ignition::math::Vector2d>;

/////////////////////////////////////////////////
const IntersectionResult NoIntersection =
    std::make_pair(false, ignition::math::Vector2d());

/////////////////////////////////////////////////
IntersectionResult CheckIntersection(
    const PointPair &_points,
    const Wall &_wall)
{
  std::array<double,2> A;
  std::array<double,2> B;
  std::array<double,2> C;

  std::size_t i = 0;
  for (const PointPair &p : {_points, _wall})
  {
    A[i] = p.second.Y() - p.first.Y();
    B[i] = p.second.X() - p.first.X();
    C[i] = A[i]*p.first.X() + B[i]*p.first.Y();
    ++i;
  }

  const double det = A[0]*B[1] - A[1]*B[0];
  if (std::abs(det) < 1e-10)
  {
    // Lines are parallel
    return NoIntersection;
  }

  const double x = (B[1]*C[0] - B[0]*C[1])/det;
  const double y = (A[0]*C[1] - A[1]*C[0])/det;

  for (const PointPair &p : {_points, _wall})
  {
    if (x < std::min(p.first.X(), p.second.X()))
      return NoIntersection;

    if (std::max(p.first.X(), p.second.X() ) < x)
      return NoIntersection;
  }

  return std::make_pair(true, ignition::math::Vector2d(x,y));
}

/////////////////////////////////////////////////
IntersectionResult CheckCollisions(
    const ignition::math::Vector2d &_x1,
    const ignition::math::Vector2d &_x0,
    const Layout &_layout)
{
  PointPair points = std::make_pair(_x1, _x0);

  for (const Wall &wall : _layout)
  {
    const IntersectionResult result = CheckIntersection(points, wall);
    if (result.first)
      return result;
  }

  return NoIntersection;
}

/////////////////////////////////////////////////
void Simulate(const EnvironmentPluginPtr &_environment,
              const RobotPluginPtr &_robot,
              const double _duration,
              const bool _printState)
{
  Environment *env = _environment->QueryInterface<Environment>();
  assert(env && "Environment interface is missing!");

  const Drive *drive = _robot->QueryInterface<Drive>();
  assert(drive && "The robot cannot drive!");

  // Get the layout from the environment.
  const Layout &layout = env->GenerateLayout();

  // If the robot has a MapUplink interface, send it the layout of the
  // environment.
  if (MapDatabase *uplink = _robot->QueryInterface<MapDatabase>())
    uplink->ReadMap(layout);

  double time = 0.0;
  ignition::math::Vector2d x;
  double theta = 0.0;

  while (time < _duration)
  {
    // Update the proximity sensor.
    if (ProximitySensor *proximity = _robot->QueryInterface<ProximitySensor>())
    {
      const double range = proximity->MaxRange();
      const ignition::math::Vector2d x_far = x + ignition::math::Vector2d(
            range*cos(theta), range*sin(theta));
      IntersectionResult result = CheckCollisions(x, x_far, layout);

      if (result.first)
      {
        proximity->ReadProximity( (x-result.second).Length() );
      }
      else
      {
        proximity->ReadProximity(std::numeric_limits<double>::infinity());
      }
    }

    // Update the GPS sensor
    if (GPSSensor *gps = _robot->QueryInterface<GPSSensor>())
    {
      gps->ReadGPS(x);
    }

    // Update the compass sensor
    if (Compass *compass = _robot->QueryInterface<Compass>())
    {
      compass->ReadCompass(theta);
    }

    // Save the last state
    ignition::math::Vector2d x_last = x;

    // Get the driving information
    const double dt = 1.0/drive->Frequency();
    const ignition::math::Vector3d &vel = drive->Velocity();

    // We integrate the position components with a half-step taken in the angle,
    // which should smooth out the behavior of sharp turns.
    const double halfAngle = theta + 0.5*vel[2]*dt;
    const double dx = vel[0]*cos(halfAngle)*dt + vel[1]*sin(halfAngle)*dt;
    const double dy = - vel[0]*sin(halfAngle)*dt + vel[1]*cos(halfAngle)*dt;

    x[0] += dx;
    x[1] += dy;
    theta += vel[2]*dt;

    const IntersectionResult collision = CheckCollisions(x, x_last, layout);
    if (collision.first)
    {
      std::cout << "The robot has crashed at ("
                << collision.second.X() << "m, "
                << collision.second.Y() << "m)! Time: "
                << time << "s  |==|  Velocity output: "
                << vel << "\n" << std::endl;

      x[0] = collision.second.X();
      x[1] = collision.second.Y();

      break;
    }

    time += dt;

    if (_printState)
    {
      std::cout << "Location: (" << x[0] << "m, " << x[1] << "m) | Yaw: "
                << 180.0*theta/M_PI << "-degrees | Time: " << time
                << "s  |==|  Velocity output: " << vel << std::endl;
    }
  }

  std::cout << "The simulation has finished (time: " << time << "s).\n"
            << " -- Final robot location: (" << x[0] << "m, " << x[1] << "m)\n"
            << " -- Yaw: " << 180.0*theta/M_PI << "-degrees\n" << std::endl;
}

/////////////////////////////////////////////////
int main(int argc, char *argv[])
{
  // Create an object that can search the system paths for the plugin libraries.
  ignition::common::SystemPaths paths;

  // Create a plugin loader
  ignition::plugin::Loader loader;

  // Add the build directory path for the plugin libraries so the SystemPaths
  // object will know to search through it.
  paths.AddPluginPaths(PluginLibDir);

  std::vector<std::string> knownRobotPlugins = { "CrashBot", "CautiousBot" };
  std::vector<std::string> knownEnvPlugins = { "BoxEnvironment" };

  std::string robotLib = knownRobotPlugins.front();
  std::string envLib = knownEnvPlugins.front();
  double duration = 300;
  bool printState = false;

#ifdef HAVE_BOOST_PROGRAM_OPTIONS

  std::string knownRobotString;
  for (const std::string &r : knownRobotPlugins)
    knownRobotString += r + "  ";

  std::string knownEnvString;
  for (const std::string &e : knownEnvPlugins)
    knownEnvString += e + "  ";

  std::string usage;
  usage +=
      "The 'robot' example creates a very simple 2D kinematic simulation of a\n"
      "robot, provided by a plugin, within an environment, also provided by a\n"
      "plugin. The robot may have any combination of several sensors, defined\n"
      "in plugins/robot.hh.\n\n"

      "Known robot plugin libraries include:  " + knownRobotString + "\n\n"

      "Known environment plugin libraries include:  " + knownEnvString + "\n\n"

      "Custom plugins can be used by passing in the custom plugin library\n"
      "directory to the -I flag";

  bpo::options_description desc(usage);
  desc.add_options()

      ("help,h", "Print this usage message")

      ("robot,r", bpo::value<std::string>(&robotLib),
       std::string("Robot plugin library (default: "+robotLib+")").c_str())

      ("environment,e", bpo::value<std::string>(&envLib),
       std::string("Environment plugin library (default: "+envLib+")").c_str())

      ("include-dirs,I", bpo::value<std::vector<std::string>>()->multitoken(),
       "Additional directories that may contain plugin libraries")

      ("duration,t", bpo::value<double>(&duration),
       std::string("How many seconds of simulation time to run for (default: "
                   +std::to_string(duration)+")").c_str())

      ("print,p", "Print out the state during each iteration")
      ;

  bpo::variables_map vm;
  bpo::store(bpo::command_line_parser(argc, argv).options(desc).run(), vm);
  bpo::notify(vm);

  if (vm.count("help") > 0)
  {
    std::cout << desc << std::endl;
    return 1;
  }

  if (vm.count("print") > 0)
  {
    printState = true;
  }

  if (vm.count("include-dirs"))
  {
    const std::vector<std::string> inputDirs =
        vm["include-dirs"].as<std::vector<std::string>>();

    for (const std::string &dir : inputDirs)
    {
      std::cout << "Including additional plugin directory: ["
                << dir << "]" << std::endl;
      paths.AddPluginPaths(dir);
    }
  }

#else

  std::cout << "boost::program_options was not found when this example was\n"
            << "compiled, so we will default to:\n"
            << " -- Robot:       " << robotLib <<"\n"
            << " -- Environment: " << envLib << "\n"
            << " -- Duration:    " << duration << "\n"
            << std::endl;

#endif

  std::unordered_set<std::string> robotPlugins = loader.LoadLib(
        paths.FindSharedLibrary(robotLib));

  std::unordered_set<std::string> drivePlugins =
      loader.PluginsImplementing("ignition::plugin::examples::Drive");

  RobotPluginPtr robot;
  // Get the first plugin from the robotLib library that provides a Drive
  // interface
  for (const std::string &plugin : robotPlugins)
  {
    if (drivePlugins.find(plugin) != drivePlugins.end())
    {
      robot = loader.Instantiate(plugin);
      break;
    }
  }

  if (robot.IsEmpty())
  {
    std::cerr << "The plugin library specified for the robot does not provide "
              << "a Drive interface, which is required for the robot to work.\n"
              << std::endl;
    return 2;
  }

  std::unordered_set<std::string> envPlugins = loader.LoadLib(
        paths.FindSharedLibrary(envLib));

  std::unordered_set<std::string> envInterfacePlugins =
      loader.PluginsImplementing("ignition::plugin::examples::Environment");

  EnvironmentPluginPtr environment;
  // Get the first plugin from the envLib library that provides an Environment
  // interface.
  for (const std::string &plugin : envPlugins)
  {
    if (envInterfacePlugins.find(plugin) != envInterfacePlugins.end())
    {
      environment = loader.Instantiate(plugin);
      break;
    }
  }

  if (environment.IsEmpty())
  {
    std::cerr << "The plugin library specified for the environment does not "
              << "provide an Environment interface, which is required for the "
              << "environment to be generated.\n" << std::endl;
    return 3;
  }

  Simulate(environment, robot, duration, printState);
}


#include <boost/math/policies/error_handling.hpp>


#include <iostream>
#include <ros/ros.h>
#include <calibrationMatrix.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <string>
#include <boost/signals2.hpp>
#include <boost/bind.hpp>
#include <opencv2/opencv.hpp>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>

#include "cv.h"

cv_bridge::CvImagePtr cvImageFromROS;
cv_bridge::CvImage out_msg;
cv_bridge::CvImagePtr diff;
image_transport::ImageTransport* it;
image_transport::ImageTransport* test;
image_transport::Publisher pub;
image_transport::Publisher pub2;
cv::Mat image;
cv::Mat image_undistorted;
cv::Mat DiffImage;
calibrationMatrix* multiplier;
ros::Publisher pub_info_left;

cv::Mat_<float> undistortDepthMapX, undistortDepthMapY;
cv::Mat_<float> id;
cv::Mat K;
cv::Mat DistCoef;

std::string _sub;
std::string _pub;
std::string _calib;

int numCols=640;
int numRows=480;
bool doUndistortion=true;

void initialize (){

  id = cv::Mat::eye(3, 3, CV_32FC1);
  K= cv::Mat ( 3, 3, CV_32FC1 ); K.setTo(0);
  K.at<float>(0,0) = 475.9396667480469;
  K.at<float>(1,1) = 475.9395446777344;
  K.at<float>(0,2) = 307.9508056640625;
  K.at<float>(1,2) = 245.95870971679688;
  K.at<float>(2,2) = 1.0f;

  DistCoef= cv::Mat( 5, 1, CV_32FC1 );
  // distortion cooefficients (rad tan)
  DistCoef.at<float>( 0 ) = 0.1646876037120819; // k1
  DistCoef.at<float>( 1 ) = -0.01863516867160797; // k2
  DistCoef.at<float>( 2 ) = 0.004235030617564917; // p1
  DistCoef.at<float>( 3 ) = 0.0034463282208889723; // p2
  DistCoef.at<float>( 4 ) = 0; // p3, can be 0.

  cv::initUndistortRectifyMap( K, DistCoef, id, K, cv::Size( numCols, numRows ), CV_32FC1, undistortDepthMapX, undistortDepthMapY );

  std::cout << "finished intiailization " << std::endl;

}

void undistort(cv::Mat img_depth,cv::Mat& image_undistorted){

    if ( doUndistortion )
    {

      //Convert to float
      img_depth.convertTo(img_depth, CV_32FC1);

      image_undistorted.create( img_depth.rows, img_depth.cols, img_depth.type());
      image_undistorted = 0.0f;

      // remap from distorted to undistorted image
      #pragma omp parallel for num_threads(6)
      for ( int i = 0; i < img_depth.rows; ++i )
      {
        for ( int j = 0; j < img_depth.cols; ++j )
        {
          int ux = std::floor(undistortDepthMapY(i,j));
          int vy = std::floor(undistortDepthMapX(i,j));
          if ( ux > 0 && vy > 0 && ux < img_depth.rows && vy < img_depth.cols )
            image_undistorted.at<ushort>(i,j) = img_depth.at<ushort>(ux,vy);
          else
            image_undistorted.at<ushort>(i,j) = 0;
        }
      }
    }
    else{
      img_depth.copyTo(image_undistorted);
    }

    //convert back to ushort
    image_undistorted.convertTo(image_undistorted, CV_16UC1);

    //std::cout << "finsished undistortion" << std::endl;

}

void callback(const sensor_msgs::ImageConstPtr &imgPtr){
    cvImageFromROS =cv_bridge::toCvCopy(imgPtr);
    diff =cv_bridge::toCvCopy(imgPtr);

    cvImageFromROS->image.copyTo(image);
    diff->image.copyTo(DiffImage);
    out_msg.header   = imgPtr->header;
    out_msg.encoding =  "16UC1"; //  sensor_msgs::image_encodings::MONO16; //"mono16";

    int cols=image.cols;
    int rows=image.rows;
    //std::cout << "is " << rows << " " << cols << std::endl;
    ushort v;
    cv::Point p;
    for(int i=0;i<cols;i++){
        for(int j=0;j<rows;j++){
            p.x=i;
            p.y=j;
            v=((float)image.at<ushort>(p));

            //cap the depth at 2 meters
            if (v>=2000){
              v=0;
              image.at<ushort>(p)=(ushort)v;
              continue;
            }

            v*=multiplier->cell(p.y,p.x,v);
            image.at<ushort>(p)=(ushort)v;
        }
    }

    //TODO Clean the horrible mess of a code
    numCols=cols;
    numRows=rows;
    undistort(image,image_undistorted);

    out_msg.image    =  image;
    pub.publish(out_msg.toImageMsg());

    //std::cout << "one is" << image_undistorted.rows << " " << image_undistorted.cols << std::endl;
    //std::cout << "one is" << DiffImage.rows << " " << DiffImage.cols << std::endl;

    out_msg.image    =  image_undistorted-DiffImage;
    pub2.publish(out_msg.toImageMsg());
    //std::cout<<"global diff: "<< cv::sum(out_msg.image)/(out_msg.image.rows*out_msg.image.cols)<<std::endl;

}

int main(int argc, char **argv)
{
    if(argc<3){
        std::cout<<"This ROS NODE is intended to use in realtime with a /depth/image_raw topic"<<std::endl;
        std::cout<<"The node will subscribe to a DEPTH IMAGE RAW topic and will republish the undistorted data"<<std::endl;
        std::cout<<"Usage:"<<std::endl;
        std::cout<<"rosrun easydepthcalibration driver_node _sub:=/camera/depth/image_raw _pub:=/calibrated/ _calib:=calibration_filename.ext"<<std::endl;
        std::cout<<"Note:"<<std::endl;

        exit(1);
    }
    ros::init(argc, argv, "xtionDriver",ros::init_options::AnonymousName);
    ros::NodeHandle n("~");
    n.param("sub", _sub, string("/camera/depth/image_raw"));
    n.param("pub", _pub, string("/cameraCalib/depth"));
    n.param("calib", _calib, string("calib.mal"));

    multiplier = new calibrationMatrix(const_cast<char*>(_calib.c_str()));
    //check if the multiplier was actually created
    if (multiplier->layers==0){
      std::cout << "multiplier matrix not created" << std::endl;
      return 2;
    }

    initialize();

    it= new image_transport::ImageTransport(n);
    test= new image_transport::ImageTransport(n);
    pub = it->advertise(_pub+"/image_raw", 10);
    pub2 = it->advertise(_pub+"/image_raw_diff", 10);
    std::cout<<"TOPIC SUBSCRIBED TO: "<<_sub <<std::endl;
    image_transport::Subscriber sub = test->subscribe(_sub, 1, &callback);
    ros::spin();
    return 1;
}


#include "surface_flow.h"
#include "fractional_laplacian.h"
#include "helpers.h"

#include "sobolev/h1.h"
#include "sobolev/h1_lbfgs.h"
#include "sobolev/h2.h"
#include "sobolev/l2_lbfgs.h"
#include "sobolev/bqn_lbfgs.h"
#include "sobolev/hs.h"
#include "sobolev/hs_schur.h"
#include "sobolev/hs_iterative.h"
#include "sobolev/constraints.h"
#include "spatial/convolution.h"

#include <Eigen/SparseCholesky>

namespace rsurfaces
{

    SurfaceFlow::SurfaceFlow(SurfaceEnergy *energy_)
    {
        energies.push_back(energy_);

        mesh = energy_->GetMesh();
        geom = energy_->GetGeom();
        stepCount = 0;

        origBarycenter = meshBarycenter(geom, mesh);
        std::cout << "Original barycenter = " << origBarycenter << std::endl;
        RecenterMesh();
        secretBarycenter = 0;
        obstacleEnergy = 0;

        verticesMutated = false;
        lbfgs = 0;
        bqn_B = 0;
    }

    void SurfaceFlow::AddAdditionalEnergy(SurfaceEnergy *extraEnergy)
    {
        energies.push_back(extraEnergy);
    }

    void SurfaceFlow::AddObstacleEnergy(SurfaceEnergy *obsEnergy)
    {
        obstacleEnergy = obsEnergy;
        AddAdditionalEnergy(obstacleEnergy);
    }

    void SurfaceFlow::UpdateEnergies()
    {
        for (SurfaceEnergy *energy : energies)
        {
            energy->Update();
        }
    }

    inline double guessStepSize(double gProjNorm)
    {
        // double initGuess = (gProjNorm < 1) ? 1.0 / sqrt(gProjNorm) : 1.0 / gProjNorm;
        double initGuess = 1.0 / gProjNorm;
        initGuess *= 2;
        return initGuess;
    }

    void SurfaceFlow::StepL2Unconstrained()
    {
        stepCount++;
        UpdateEnergies();

        Eigen::MatrixXd l2diff;
        l2diff.setZero(mesh->nVertices(), 3);
        AssembleGradients(l2diff);

        double initGuess = guessStepSize(l2diff.norm());
        LineSearch search(mesh, geom, energies, maxStepSize);
        search.BacktrackingLineSearch(l2diff, initGuess, 1);
    }

    void SurfaceFlow::StepL2Projected()
    {
        stepCount++;
        UpdateEnergies();

        Eigen::MatrixXd l2diff;
        l2diff.setZero(mesh->nVertices(), 3);
        AssembleGradients(l2diff);

        // Make a saddle matrix with just the identity in the corner
        std::vector<Triplet> triplets;
        for (size_t i = 0; i < mesh->nVertices(); i++)
        {
            size_t i3 = 3 * i;
            triplets.push_back(Triplet(i3, i3, 1));
            triplets.push_back(Triplet(i3 + 1, i3 + 1, 1));
            triplets.push_back(Triplet(i3 + 2, i3 + 2, 1));
        }

        size_t nConstraintRows = addConstraintTriplets(triplets, true);
        size_t dims = 3 * mesh->nVertices() + nConstraintRows;

        Eigen::SparseMatrix<double> A(dims, dims);
        A.setFromTriplets(triplets.begin(), triplets.end());

        Eigen::VectorXd l2col;
        l2col.setZero(dims);
        MatrixUtils::MatrixIntoColumn(l2diff, l2col);

        SparseFactorization factorizedA;
        factorizedA.Compute(A);

        l2col = factorizedA.Solve(l2col);
        MatrixUtils::ColumnIntoMatrix(l2col, l2diff);

        double initGuess = guessStepSize(l2diff.norm());
        LineSearch search(mesh, geom, energies, maxStepSize);
        search.BacktrackingLineSearch(l2diff, initGuess, 1);

        // Constraint projection
        l2col.setZero();
        size_t curRow = 3 * mesh->nVertices();

        for (Constraints::SimpleProjectorConstraint* spc : simpleConstraints)
        {
            spc->addErrorValues(l2col, mesh, geom, curRow);
            curRow += spc->nRows();
        }

        for (ConstraintPack pack : schurConstraints)
        {
            pack.constraint->addErrorValues(l2col, mesh, geom, curRow);
            curRow += pack.constraint->nRows();
        }

        l2col = factorizedA.Solve(l2col);

        VertexIndices inds = mesh->getVertexIndices();
        for (GCVertex v : mesh->vertices())
        {
            size_t base = inds[v] * 3;
            Vector3 corr{l2col(base), l2col(base + 1), l2col(base + 2)};
            geom->inputVertexPositions[v] -= corr;
        }

        geom->refreshQuantities();
    }

    double SurfaceFlow::evaluateEnergy()
    {
        return GetEnergyValue(energies);
    }

    void SurfaceFlow::AssembleGradients(Eigen::MatrixXd &dest)
    {
        AddGradientsToMatrix(energies, dest);
    }

    std::unique_ptr<Hs::HsMetric> SurfaceFlow::GetHsMetric()
    {
        std::unique_ptr<Hs::HsMetric> hs(new Hs::HsMetric(energies, obstacleEnergy, simpleConstraints, schurConstraints));
        hs->disableNearField = disableNearField;
        return hs;
    }

    void SurfaceFlow::StepProjectedGradientExact()
    {
        long timeStart = currentTimeMilliseconds();
        stepCount++;
        std::cout << "=== Iteration " << stepCount << " ===" << std::endl;
        std::cout << "Using Hs projected gradient method..." << std::endl;
        UpdateEnergies();

        // Assemble sum of L2 differentials of all energies involved
        // (including tangent-point energy)
        Eigen::MatrixXd l2diff, gradientProj;
        l2diff.setZero(mesh->nVertices(), 3);
        gradientProj.setZero(mesh->nVertices(), 3);

        AssembleGradients(l2diff);
        double gNorm = l2diff.norm();

        std::unique_ptr<Hs::HsMetric> hs = GetHsMetric();

        Eigen::MatrixXd M = hs->GetHsMatrixConstrained();

        // Flatten the gradient into a single column
        Eigen::VectorXd gradientCol;
        gradientCol.setZero(M.rows());

        // Solve the dense system
        MatrixUtils::MatrixIntoColumn(l2diff, gradientCol);
        Eigen::PartialPivLU<Eigen::MatrixXd> solver = M.partialPivLu();
        gradientCol = solver.solve(gradientCol);
        MatrixUtils::ColumnIntoMatrix(gradientCol, gradientProj);

        VertexIndices inds = mesh->getVertexIndices();

        double gProjNorm = gradientProj.norm();
        // Measure dot product of search direction with original gradient direction
        double gradDot = (l2diff.transpose() * gradientProj).trace() / (gNorm * gProjNorm);

        // Guess a step size
        // double initGuess = prevStep * 1.25;
        double initGuess = guessStepSize(gProjNorm);

        std::cout << "  * Initial step size guess = " << initGuess << std::endl;

        // Take the step using line search
        LineSearch search(mesh, geom, energies, maxStepSize);
        search.BacktrackingLineSearch(gradientProj, initGuess, gradDot);
        geom->refreshQuantities();

        // Reuse factorized matrix for constraint projection
        gradientCol.setZero();
        size_t curRow = 3 * mesh->nVertices();

        for (Constraints::SimpleProjectorConstraint *cons : simpleConstraints)
        {
            cons->addErrorValues(gradientCol, mesh, geom, curRow);
            curRow += cons->nRows();
        }

        for (const ConstraintPack &c : schurConstraints)
        {
            c.constraint->addErrorValues(gradientCol, mesh, geom, curRow);
            curRow += c.constraint->nRows();
        }

        gradientCol = solver.solve(gradientCol);

        VertexIndices verts = mesh->getVertexIndices();
        for (GCVertex v : mesh->vertices())
        {
            int base = 3 * verts[v];
            Vector3 vertCorr{gradientCol(base), gradientCol(base + 1), gradientCol(base + 2)};
            geom->inputVertexPositions[v] -= vertCorr;
        }

        long timeEnd = currentTimeMilliseconds();
        std::cout << "  Total time for gradient step = " << (timeEnd - timeStart) << " ms" << std::endl;
    }

    inline void printSolveInfo(size_t numNewton)
    {
        if (numNewton > 0)
        {
            std::cout << "  * With " << numNewton << " Newton constraint(s), Hs projection will require "
                      << (numNewton + 1) << " linear solves" << std::endl;
        }
        else
        {
            std::cout << "  * With no Newton constraints, Hs projection will require 1 linear solve" << std::endl;
        }
    }

    void SurfaceFlow::StepProjectedGradient()
    {
        ptic("SurfaceFlow::StepProjectedGradient");

        long timeStart = currentTimeMilliseconds();
        stepCount++;
        std::cout << "=== Iteration " << stepCount << " ===" << std::endl;
        std::cout << "Using Hs projected gradient method..." << std::endl;
        UpdateEnergies();

        // Assemble sum of L2 differentials of all energies involved
        // (including tangent-point energy)
        Eigen::MatrixXd l2diff, gradientProj;
        l2diff.setZero(mesh->nVertices(), 3);
        gradientProj.setZero(mesh->nVertices(), 3);

        AssembleGradients(l2diff);
        double gNorm = l2diff.norm();

        std::unique_ptr<Hs::HsMetric> hs = GetHsMetric();
        hs->allowBarycenterShift = allowBarycenterShift;
        printSolveInfo(hs->newtonConstraints.size());

        Vector3 shift{0, 0, 0};
        if (allowBarycenterShift)
        {
            shift = averageOfMatrixRows(geom, mesh, l2diff);
            std::cout << "Average shift of L2 diff = " << shift << std::endl;
        }

        Hs::ProjectViaSchur<Hs::SparseInverse>(*hs, l2diff, gradientProj);

        if (allowBarycenterShift)
        {
            addShiftToMatrixRows(gradientProj, mesh->nVertices(), shift);
        }

        VertexIndices inds = mesh->getVertexIndices();

        double gProjNorm = gradientProj.norm();
        // Measure dot product of search direction with original gradient direction
        double gradDot = (l2diff.transpose() * gradientProj).trace() / (gNorm * gProjNorm);

        // Guess a step size
        // double initGuess = prevStep * 1.25;
        double initGuess = guessStepSize(gProjNorm);

        std::cout << "  * Initial step size guess = " << initGuess << std::endl;

        // Take the step using line search
        LineSearch search(mesh, geom, energies, maxStepSize);
        double delta = search.BacktrackingLineSearch(gradientProj, initGuess, gradDot);

        if (schurConstraints.size() > 0)
        {
            Hs::ProjectSchurConstraints<Hs::SparseInverse>(*hs, 1);
        }

        if (allowBarycenterShift)
        {
            // The barycenter goes wherever it wants.
            // Assumes no pin constraints; free barycenter isn't meant to be used with pins.
        }
        else
        {
            hs->ProjectSimpleConstraints();
        }

        incrementSchurConstraints();
        geom->refreshQuantities();

        long timeEnd = currentTimeMilliseconds();
        std::cout << "  Total time for gradient step = " << (timeEnd - timeStart) << " ms" << std::endl;

        ptoc("SurfaceFlow::StepProjectedGradient");
    }

    void SurfaceFlow::StepProjectedGradientIterative()
    {
        ptic("SurfaceFlow::StepProjectedGradientIterative");

        long timeStart = currentTimeMilliseconds();
        stepCount++;
        std::cout << "=== Iteration " << stepCount << " ===" << std::endl;
        std::cout << "Using iterative Hs projected gradient method..." << std::endl;
        UpdateEnergies();

        // Assemble sum of L2 differentials of all energies involved
        // (including tangent-point energy)
        Eigen::MatrixXd l2diff, gradientProj;
        l2diff.setZero(mesh->nVertices(), 3);
        gradientProj.setZero(mesh->nVertices(), 3);
        AssembleGradients(l2diff);
        double gNorm = l2diff.norm();

        std::unique_ptr<Hs::HsMetric> hs = GetHsMetric();
        printSolveInfo(hs->newtonConstraints.size());

        Vector3 shift{0, 0, 0};
        if (allowBarycenterShift)
        {
            shift = averageOfMatrixRows(geom, mesh, l2diff);
            std::cout << "Average shift of L2 diff = " << shift << std::endl;
        }

        Hs::ProjectConstrainedHsIterativeMat(*hs, l2diff, gradientProj);

        if (allowBarycenterShift)
        {
            addShiftToMatrixRows(gradientProj, mesh->nVertices(), shift);
        }

        VertexIndices inds = mesh->getVertexIndices();

        double gProjNorm = gradientProj.norm();
        // Measure dot product of search direction with original gradient direction
        double gradDot = (l2diff.transpose() * gradientProj).trace() / (gNorm * gProjNorm);

        // Guess a step size
        // double initGuess = prevStep * 1.25;
        double initGuess = guessStepSize(gProjNorm);

        std::cout << "  * Initial step size guess = " << initGuess << std::endl;

        // Take the step using line search
        LineSearch search(mesh, geom, energies, maxStepSize);
        double delta = search.BacktrackingLineSearch(gradientProj, initGuess, gradDot);

        // Constraint projection
        if (schurConstraints.size() > 0)
        {
            // hs->ResetSchurComplement();
            std::cout << "  Projecting Newton constraints..." << std::endl;
            Hs::ProjectSchurConstraints<Hs::IterativeInverse>(*hs, 1);
        }
        if (allowBarycenterShift)
        {
            // The barycenter goes wherever it wants.
            // Assumes no pin constraints; free barycenter isn't meant to be used with pins.
        }
        else
        {
            hs->ProjectSimpleConstraints();
        }

        incrementSchurConstraints();
        geom->refreshQuantities();

        long timeEnd = currentTimeMilliseconds();
        std::cout << "  Total time for gradient step = " << (timeEnd - timeStart) << " ms" << std::endl;

        ptoc("SurfaceFlow::StepProjectedGradientIterative");
    }

    size_t SurfaceFlow::addConstraintTriplets(std::vector<Triplet> &triplets, bool includeSchur)
    {
        size_t curRow = 3 * mesh->nVertices();
        size_t nConstraintRows = 0;
        for (Constraints::SimpleProjectorConstraint *spc : simpleConstraints)
        {
            Constraints::addTripletsToSymmetric(*spc, triplets, mesh, geom, curRow);
            size_t nr = spc->nRows();
            curRow += nr;
            nConstraintRows += nr;
        }

        if (includeSchur)
        {
            for (ConstraintPack &c : schurConstraints)
            {
                Constraints::addTripletsToSymmetric(*c.constraint, triplets, mesh, geom, curRow);
                size_t nr = c.constraint->nRows();
                curRow += nr;
                nConstraintRows += nr;
            }
        }
        return nConstraintRows;
    }

    void SurfaceFlow::prefactorConstrainedLaplacian(SparseFactorization &factored, bool includeSchur)
    {
        Eigen::SparseMatrix<double> L;
        prefactorConstrainedLaplacian(L, factored, includeSchur);
    }

    void SurfaceFlow::prefactorConstrainedLaplacian(Eigen::SparseMatrix<double> &L, SparseFactorization &factored, bool includeSchur)
    {
        // Assemble triplets for the Laplacian
        std::vector<Triplet> h1Triplets, h1Triplets3x;
        H1::getTriplets(h1Triplets, mesh, geom, 1e-10);
        MatrixUtils::TripleTriplets(h1Triplets, h1Triplets3x);
        // Add constraint rows at bottom
        size_t nConstraintRows = addConstraintTriplets(h1Triplets3x, includeSchur);
        size_t dims = 3 * mesh->nVertices() + nConstraintRows;

        // Builds and factorize the Laplacian
        L.resize(dims, dims);
        L.setFromTriplets(h1Triplets3x.begin(), h1Triplets3x.end());
        factored.Compute(L);
    }

    void SurfaceFlow::StepH1ProjGrad()
    {
        long timeStart = currentTimeMilliseconds();
        stepCount++;
        std::cout << "=== Iteration " << stepCount << " ===" << std::endl;
        std::cout << "Using H1 projected gradient method..." << std::endl;
        UpdateEnergies();

        // Assemble sum of L2 differentials of all energies involved
        // (including tangent-point energy)
        Eigen::MatrixXd l2diff;
        l2diff.setZero(mesh->nVertices(), 3);
        AssembleGradients(l2diff);
        double gNorm = l2diff.norm();

        Vector3 shift{0, 0, 0};
        if (allowBarycenterShift)
        {
            shift = averageOfMatrixRows(geom, mesh, l2diff);
            std::cout << "Average shift of L2 diff = " << shift << std::endl;
        }

        SparseFactorization factorizedL;
        prefactorConstrainedLaplacian(factorizedL, true);
        size_t dims = factorizedL.nRows;
        std::cout << "Prefactorized" << std::endl;

        // Project the H1 gradient
        Eigen::VectorXd gradientVec;
        gradientVec.setZero(dims);
        MatrixUtils::MatrixIntoColumn(l2diff, gradientVec);

        gradientVec = factorizedL.Solve(gradientVec);
        Eigen::MatrixXd gradientProj;
        gradientProj.setZero(l2diff.rows(), l2diff.cols());
        MatrixUtils::ColumnIntoMatrix(gradientVec, gradientProj);

        if (allowBarycenterShift)
        {
            addShiftToMatrixRows(gradientProj, mesh->nVertices(), shift);
        }

        double gProjNorm = gradientProj.norm();
        // Measure dot product of search direction with original gradient direction
        double gradDot = (l2diff.transpose() * gradientProj).trace() / (gNorm * gProjNorm);
        // Guess a step size
        double initGuess = guessStepSize(gProjNorm);
        std::cout << "  * Initial step size guess = " << initGuess << std::endl;
        // Take the step using line search
        LineSearch search(mesh, geom, energies, maxStepSize);
        double delta = search.BacktrackingLineSearch(gradientProj, initGuess, gradDot);

        // Do corrective constraint projection by reusing the H1 metric
        H1::ProjectConstraints(mesh, geom, simpleConstraints, schurConstraints, factorizedL, 1);

        incrementSchurConstraints();
        geom->refreshQuantities();

        long timeEnd = currentTimeMilliseconds();
        std::cout << "  Total time for gradient step = " << (timeEnd - timeStart) << " ms" << std::endl;
    }

    void savePositions(MeshPtr &mesh, GeomPtr &geom, Eigen::MatrixXd &positions)
    {
        positions.setZero(mesh->nVertices(), 3);

        VertexIndices inds = mesh->getVertexIndices();

        for (GCVertex v : mesh->vertices())
        {
            Vector3 pos = geom->inputVertexPositions[v];
            size_t i = inds[v];
            positions(i, 0) = pos.x;
            positions(i, 1) = pos.y;
            positions(i, 2) = pos.z;
        }
    }

    void SurfaceFlow::StepAQP(double invKappa)
    {
        long timeStart = currentTimeMilliseconds();
        if (stepCount == 0 || verticesMutated)
        {
            // Reset Nesterov memory of previous step
            savePositions(mesh, geom, prevPositions1);
            savePositions(mesh, geom, prevPositions2);
        }
        else
        {
            double theta = (1 - sqrt(invKappa)) / (1 + sqrt(invKappa));
            // 1. Nesterov step
            Eigen::MatrixXd y_n = (1 + theta) * prevPositions1 - theta * prevPositions2;
            for (GCVertex v : mesh->vertices())
            {
                Vector3 pos = MatrixUtils::GetRowAsVector3(y_n, v.getIndex());
                geom->inputVertexPositions[v] = pos;
            }
            geom->refreshQuantities();
        }
        stepCount++;

        // 2. H1 gradient step following the Nesterov step
        std::cout << "=== Iteration " << stepCount << " ===" << std::endl;
        std::cout << "Using AQP..." << std::endl;
        UpdateEnergies();

        // Assemble sum of L2 differentials of all energies involved
        // (including tangent-point energy)
        Eigen::MatrixXd l2diff;
        l2diff.setZero(mesh->nVertices(), 3);
        AssembleGradients(l2diff);
        double gNorm = l2diff.norm();

        Vector3 shift{0, 0, 0};
        if (allowBarycenterShift)
        {
            shift = averageOfMatrixRows(geom, mesh, l2diff);
            std::cout << "Average shift of L2 diff = " << shift << std::endl;
        }

        SparseFactorization factorizedL;
        // Only use "simple" positional constraints (Nesterov would break hard constraints anyway)
        prefactorConstrainedLaplacian(factorizedL, false);
        size_t dims = factorizedL.nRows;

        // Project the H1 gradient
        Eigen::VectorXd gradientVec;
        gradientVec.setZero(dims);
        MatrixUtils::MatrixIntoColumn(l2diff, gradientVec);

        gradientVec = factorizedL.Solve(gradientVec);
        Eigen::MatrixXd gradientProj;
        gradientProj.setZero(l2diff.rows(), l2diff.cols());
        MatrixUtils::ColumnIntoMatrix(gradientVec, gradientProj);

        if (allowBarycenterShift)
        {
            addShiftToMatrixRows(gradientProj, mesh->nVertices(), shift);
        }

        double gProjNorm = gradientProj.norm();
        // Measure dot product of search direction with original gradient direction
        double gradDot = (l2diff.transpose() * gradientProj).trace() / (gNorm * gProjNorm);
        // Guess a step size
        double initGuess = guessStepSize(gProjNorm);
        std::cout << "  * Initial step size guess = " << initGuess << std::endl;
        // Take the step using line search
        LineSearch search(mesh, geom, energies, maxStepSize);
        double delta = search.BacktrackingLineSearch(gradientProj, initGuess, gradDot);

        // Make sure pins don't drift
        for (Constraints::SimpleProjectorConstraint *spc : simpleConstraints)
        {
            spc->ProjectConstraint(mesh, geom);
        }
        // Save previous positions
        prevPositions2 = prevPositions1;
        savePositions(mesh, geom, prevPositions1);
        geom->refreshQuantities();

        long timeEnd = currentTimeMilliseconds();
        std::cout << "  Total time for gradient step = " << (timeEnd - timeStart) << " ms" << std::endl;
    }

    void SurfaceFlow::StepH1LBFGS()
    {
        long timeStart = currentTimeMilliseconds();

        stepCount++;
        std::cout << "=== Iteration " << stepCount << " ===" << std::endl;
        std::cout << "Using H1 L-BFGS..." << std::endl;

        if (!lbfgs)
        {
            lbfgs = new H1_LBFGS(20, simpleConstraints);
        }

        if (verticesMutated)
        {
            std::cout << "  * Vertices were mutated; resetting memory" << std::endl;
            lbfgs->ResetMemory();
        }

        UpdateEnergies();
        // Assemble sum of L2 differentials of all energies involved
        // (including tangent-point energy)
        Eigen::MatrixXd l2diff, projected, positions;
        l2diff.setZero(mesh->nVertices(), 3);
        positions.setZero(mesh->nVertices(), 3);
        projected.setZero(mesh->nVertices(), 3);
        AssembleGradients(l2diff);
        double gNorm = l2diff.norm();

        Eigen::VectorXd l2diffvec(3 * mesh->nVertices());
        MatrixUtils::MatrixIntoColumn(l2diff, l2diffvec);

        savePositions(mesh, geom, positions);
        Eigen::VectorXd posvec(3 * mesh->nVertices());
        MatrixUtils::MatrixIntoColumn(positions, posvec);

        // Do the L-BFGS update with current position and gradient
        lbfgs->SetUpInnerProduct(mesh, geom);
        lbfgs->UpdateDirection(posvec, l2diffvec);
        double gProjNorm = lbfgs->direction().norm();

        MatrixUtils::ColumnIntoMatrix(lbfgs->direction(), projected);
        double gradDot = (l2diffvec.dot(lbfgs->direction())) / (gNorm * gProjNorm);
        std::cout << "  * Dot product = " << gradDot << std::endl;

        LineSearch search(mesh, geom, energies, maxStepSize);
        // Take the step using line search
        double initGuess = guessStepSize(gProjNorm);
        double delta = search.BacktrackingLineSearch(projected, initGuess, fmax(0, gradDot));

        if (gradDot < 0)
        {
            std::cout << "  * Negative dot product; resetting memory" << std::endl;
            lbfgs->ResetMemory();
        }

        // Make sure pins don't drift
        for (Constraints::SimpleProjectorConstraint *spc : simpleConstraints)
        {
            spc->ProjectConstraint(mesh, geom);
        }
        geom->refreshQuantities();

        long timeEnd = currentTimeMilliseconds();
        std::cout << "  Total time for gradient step = " << (timeEnd - timeStart) << " ms" << std::endl;
    }

    void SurfaceFlow::StepBQN()
    {
        long timeStart = currentTimeMilliseconds();

        stepCount++;
        std::cout << "=== Iteration " << stepCount << " ===" << std::endl;
        std::cout << "Using blended L-BFGS (BQN)..." << std::endl;

        if (!lbfgs)
        {
            // B = (total area) ^ (2(d-1) / d) for d = 3
            // (equation 13 of BCQN / Zhu et al.)
            double b = pow(totalArea(geom, mesh), 4.0 / 3.0);
            lbfgs = new BQN_LBFGS(20, simpleConstraints, b);
        }

        if (verticesMutated)
        {
            std::cout << "  * Vertices were mutated; resetting memory" << std::endl;
            lbfgs->ResetMemory();
        }

        UpdateEnergies();
        // Assemble sum of L2 differentials of all energies involved
        // (including tangent-point energy)
        Eigen::MatrixXd l2diff, projected, positions;
        l2diff.setZero(mesh->nVertices(), 3);
        positions.setZero(mesh->nVertices(), 3);
        projected.setZero(mesh->nVertices(), 3);
        AssembleGradients(l2diff);
        double gNorm = l2diff.norm();

        Eigen::VectorXd l2diffvec(3 * mesh->nVertices());
        MatrixUtils::MatrixIntoColumn(l2diff, l2diffvec);

        savePositions(mesh, geom, positions);
        Eigen::VectorXd posvec(3 * mesh->nVertices());
        MatrixUtils::MatrixIntoColumn(positions, posvec);

        // Do the L-BFGS update with current position and gradient
        lbfgs->SetUpInnerProduct(mesh, geom);
        lbfgs->UpdateDirection(posvec, l2diffvec);
        double gProjNorm = lbfgs->direction().norm();

        MatrixUtils::ColumnIntoMatrix(lbfgs->direction(), projected);
        double gradDot = (l2diffvec.dot(lbfgs->direction())) / (gNorm * gProjNorm);
        std::cout << "  * Dot product = " << gradDot << std::endl;

        LineSearch search(mesh, geom, energies, maxStepSize);
        // Take the step using line search
        double initGuess = guessStepSize(gProjNorm);
        double delta = search.BacktrackingLineSearch(projected, initGuess, fmax(0, gradDot));

        if (gradDot < 0)
        {
            std::cout << "  * Negative dot product; resetting memory" << std::endl;
            lbfgs->ResetMemory();
        }

        // Make sure pins don't drift
        for (Constraints::SimpleProjectorConstraint *spc : simpleConstraints)
        {
            spc->ProjectConstraint(mesh, geom);
        }
        geom->refreshQuantities();

        long timeEnd = currentTimeMilliseconds();
        std::cout << "  Total time for gradient step = " << (timeEnd - timeStart) << " ms" << std::endl;
    }

    void SurfaceFlow::StepH2Projected()
    {
        long timeStart = currentTimeMilliseconds();
        stepCount++;
        std::cout << "=== Iteration " << stepCount << " ===" << std::endl;
        std::cout << "Using H2 projected gradient..." << std::endl;
        UpdateEnergies();

        // Assemble sum of L2 differentials of all energies involved
        // (including tangent-point energy)
        Eigen::MatrixXd l2diff;
        l2diff.setZero(mesh->nVertices(), 3);
        AssembleGradients(l2diff);
        double gNorm = l2diff.norm();

        // Get bi-Laplacian triplets
        std::vector<Triplet> biTriplets, biTriplets3x;
        H2::getTriplets(biTriplets, mesh, geom, 0);
        MatrixUtils::TripleTriplets(biTriplets, biTriplets3x);
        size_t nConstraintRows = addConstraintTriplets(biTriplets3x, true);

        // Build tripled matrix
        size_t dims = 3 * mesh->nVertices() + nConstraintRows;
        Eigen::SparseMatrix<double> biLaplacian(dims, dims);
        biLaplacian.setFromTriplets(biTriplets3x.begin(), biTriplets3x.end());

        // Pre-factorize it
        SparseFactorization factorizedL;
        factorizedL.Compute(biLaplacian);

        // Reshape the gradient into a column
        Eigen::VectorXd gradientVec;
        gradientVec.setZero(dims);
        MatrixUtils::MatrixIntoColumn(l2diff, gradientVec);

        // Solve for the H2 gradient
        gradientVec = factorizedL.Solve(gradientVec);
        Eigen::MatrixXd gradientProj;
        gradientProj.setZero(l2diff.rows(), l2diff.cols());
        MatrixUtils::ColumnIntoMatrix(gradientVec, gradientProj);

        // Take the step using line search
        double gProjNorm = gradientProj.norm();
        double gradDot = (l2diff.transpose() * gradientProj).trace() / (gNorm * gProjNorm);
        double initGuess = guessStepSize(gProjNorm);
        std::cout << "  * Initial step size guess = " << initGuess << std::endl;
        LineSearch search(mesh, geom, energies, maxStepSize);
        double delta = search.BacktrackingLineSearch(gradientProj, initGuess, gradDot);

        // Do corrective constraint projection by reusing the metric
        H1::ProjectConstraints(mesh, geom, simpleConstraints, schurConstraints, factorizedL, 1);

        incrementSchurConstraints();
        geom->refreshQuantities();

        long timeEnd = currentTimeMilliseconds();
        std::cout << "  Total time for gradient step = " << (timeEnd - timeStart) << " ms" << std::endl;
    }

    void SurfaceFlow::RecenterMesh()
    {
        Vector3 center = meshBarycenter(geom, mesh);
        translateMesh(geom, mesh, origBarycenter - center);
    }

    void SurfaceFlow::ResetAllConstraints()
    {
        for (ConstraintPack &p : schurConstraints)
        {
            p.constraint->ResetFunction(mesh, geom);
        }
        for (Constraints::SimpleProjectorConstraint *c : simpleConstraints)
        {
            c->ResetFunction(mesh, geom);
        }
    }

    void SurfaceFlow::ResetAllPotentials()
    {
        for (SurfaceEnergy *energy : energies)
        {
            energy->ResetTargets();
        }
    }

    SurfaceEnergy *SurfaceFlow::BaseEnergy()
    {
        return energies[0];
    }

} // namespace rsurfaces


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

  @copyright 2015 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_EXPONENTIAL_HPP_INCLUDED
#define BOOST_SIMD_EXPONENTIAL_HPP_INCLUDED

namespace boost { namespace simd
{
  /*!
    @ingroup group-functions
    @defgroup group-exponential Exponential Functions

    Those functions provides algorithms for computing exponentials and logarithms.
  **/

  /*!
    @ingroup group-callable
    @defgroup group-callable-exponential Exponential Callable Objects
    Callable objects version of @ref group-exponential

    Their specific semantic limitations are similar to those of their function
    equivalents as described in the @ref group-exponential section.
  **/

} }

#include <boost/simd/functioncbrt.hpp>
#include <boost/simd/functionexp10.hpp>
#include <boost/simd/functionexp2.hpp>
#include <boost/simd/functionexp.hpp>
#include <boost/simd/functionexpm1.hpp>
#include <boost/simd/functionexprecnegc.hpp>
#include <boost/simd/functionexprecneg.hpp>
#include <boost/simd/functionexpx2.hpp>
#include <boost/simd/functionlog10.hpp>
#include <boost/simd/functionlog2.hpp>
#include <boost/simd/functionlog.hpp>
#include <boost/simd/functionlogspace_add.hpp>
#include <boost/simd/functionlogspace_sub.hpp>
#include <boost/simd/functionnthroot.hpp>
#include <boost/simd/functionpow2.hpp>
#include <boost/simd/functionpow_abs.hpp>
#include <boost/simd/functionpow.hpp>
#include <boost/simd/functionsignificants.hpp>

#endif


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

#pragma once

#include "OnsetDetectionFunctions.hpp"
#include "WindowFuncs.hpp"
#include "../util/FFT.hpp"
#include "../util/FluidEigenMappings.hpp"
#include "../util/MedianFilter.hpp"
#include "../../data/FluidIndex.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Eigen>
#include <algorithm>
#include <cassert>

namespace fluid {
namespace algorithm {

class OnsetSegmentation
{

public:
  using ArrayXd = Eigen::ArrayXd;
  using ArrayXcd = Eigen::ArrayXcd;

  OnsetSegmentation(index maxSize) : mODF{maxSize} {}

  void init(index windowSize, index fftSize, index filterSize)
  {
    mODF.init(windowSize, fftSize, filterSize);
    mDebounceCount = 1;
  }

  double processFrame(RealVectorView input, index function, index filterSize,
                      double threshold, index debounce = 0,
                      index frameDelta = 0)
  {
    double filteredFuncVal =
        mODF.processFrame(input, function, filterSize, frameDelta);
    double detected{0};

    if (filteredFuncVal > threshold && mPrevFuncVal < threshold &&
        mDebounceCount == 0)
    {
      detected = 1.0;
      mDebounceCount = debounce;
    }
    else
    {
      if (mDebounceCount > 0) mDebounceCount--;
    }
    mPrevFuncVal = filteredFuncVal;
    return detected;
  }

private:
  index                   mDebounceCount{1};
  OnsetDetectionFunctions mODF;
  double                  mPrevFuncVal{0.0};
};

} // namespace algorithm
} // namespace fluid


#include <CLI/CLI.hpp>

#include <Eigen/Dense>
#include <opencv2/highgui/highgui.hpp>
#include <sophus/se3.hpp>

#include <basalt/calibration/calibration.hpp>
#include <basalt/serialization/headers_serialization.h>

#include <basalt/utils/filesystem.h>

#include <nlohmann/json.hpp>

#include <sys/stat.h>

using json = nlohmann::json;

json se3ToJson(Sophus::SE3<double> se3) {
  auto tm = se3.matrix();
  json rows;
  for (int r = 0; r < tm.rows(); r++) {
    json row;
    for (int c = 0; c < tm.cols(); c++) {
      row.push_back(tm(r, c));
    }
    rows.push_back(row);
  }
  return rows;
}

int main(int argc, char **argv) {
  std::string calib_path;
  std::string output_path;

  CLI::App app{"Basalt calibration to standard calibration converter"};

  app.add_option("--calib-path", calib_path, "Path to Basalt calibration file")->required();
  app.add_option("--output-path", output_path, "Path to output file")->required();

  try {
    app.parse(argc, argv);
  } catch (const CLI::ParseError &e) {
    return app.exit(e);
  }

  if (!basalt::fs::exists(calib_path))
    std::cerr << "No file found " << calib_path << std::endl;

  basalt::Calibration<double> calib;
  std::ifstream is(calib_path, std::ios::binary);
  std::cout << std::setprecision(18);

  if (is.is_open()) {
    cereal::JSONInputArchive archive(is);
    archive(calib);
    for (size_t i = 0; i < calib.T_i_c.size(); i++) {
      std::cout << "T_i_c " << calib.T_i_c[i].matrix() << std::endl;
    }
    std::cout << "Loaded camera with " << calib.intrinsics.size() << " cameras"
              << std::endl;
  } else {
    std::cerr << "could not load camera calibration " << calib_path
              << std::endl;
    std::abort();
  }

  json calibJson;

  bool fisheye = false;
  for (size_t i = 0; i < calib.T_i_c.size(); i++) {
    auto params = calib.intrinsics[i].getParam();
    auto modelName = calib.intrinsics[i].getName();

    json cameraJson;

    if (modelName == "kb4") {
      fisheye = true;
      // fx, fy, cx, cy, k1, k2, k3, k4
      if (i == 0) {
        std::cout
          << "focalLengthX " << params(0) << ";" << std::endl
          << "focalLengthY " << params(1) << ";" << std::endl
          << "principalPointX " << params(2) << ";" << std::endl
          << "principalPointY " << params(3) << ";" << std::endl;
        std::cout
          << "distortionCoeffs ";
        for (int j = 4; j < 8; j++) {
          std::cout << params(j);
          if (j == 7) std::cout << ";" << std::endl;
          else std::cout << ",";
        }
      } else {
        std::cout
          << "secondFocalLengthX " << params(0) << ";" << std::endl
          << "secondFocalLengthY " << params(1) << ";" << std::endl
          << "secondPrincipalPointX " << params(2) << ";" << std::endl
          << "secondPrincipalPointY " << params(3) << ";" << std::endl;
        std::cout
          << "secondDistortionCoeffs ";
        for (int j = 4; j < 8; j++) {
          std::cout << params(j);
          if (j == 7) std::cout << ";" << std::endl;
          else std::cout << ",";
        }
      }
      cameraJson["model"] = "kannala-brandt4";
      cameraJson["focalLengthX"] = params(0);
      cameraJson["focalLengthY"] = params(1);
      cameraJson["principalPointX"] = params(2);
      cameraJson["principalPointY"] = params(3);
      cameraJson["distortionCoefficients"] = {params(4), params(5), params(6), params(7)};
    } else if (modelName == "ds") {
      cameraJson["model"] = "doublesphere";
      cameraJson["focalLengthX"] = params(0);
      cameraJson["focalLengthY"] = params(1);
      cameraJson["principalPointX"] = params(2);
      cameraJson["principalPointY"] = params(3);
      cameraJson["xi"] = params(4);
      cameraJson["alpha"] = params(5);
    }

    if (calib.vignette.size() > 0)
      for (size_t p = 0; p < calib.vignette[i].getKnots().size(); p++)
        cameraJson["vignette"].push_back(calib.vignette[i].getKnots()[p](0));

    if (i == 0)
      std::cout << "imuToCameraMatrix ";
    else
      std::cout << "secondImuToCameraMatrix ";
    auto tm = calib.T_i_c[i].inverse().matrix();
    for (int col = 0; col < tm.cols(); col++) {
      for (int row = 0; row < tm.rows(); row++) {
        std::cout << tm(row, col);
        if (row == 3 && col == 3) std::cout << ";" << std::endl;
        else std::cout << ",";
      }
    }
    cameraJson["imuToCamera"] = se3ToJson(calib.T_i_c[i].inverse());
    calibJson["cameras"].push_back(cameraJson);
  }
  if (fisheye)
    std::cout << "fisheyeCamera true;" << std::endl;

  json gyroJson;
  gyroJson["updateRate"] = calib.imu_update_rate;
  gyroJson["noiseStd"] = {calib.gyro_noise_std(0), calib.gyro_noise_std(1), calib.gyro_noise_std(2)};
  gyroJson["biasStd"] = {calib.gyro_bias_std(0), calib.gyro_bias_std(1), calib.gyro_bias_std(2)};
  for (int i = 0; i < 12; i++)
    gyroJson["calibrationBias"].push_back(calib.calib_gyro_bias.getParam()[i]);
  calibJson["gyroscope"] = gyroJson;

  json accJson;
  // accJson["updateRate"] = calib.imu_update_rate;
  accJson["noiseStd"] = {calib.accel_noise_std(0), calib.accel_noise_std(1), calib.accel_noise_std(2)};
  accJson["biasStd"] = {calib.accel_bias_std(0), calib.accel_bias_std(1), calib.accel_bias_std(2)};
  for (int i = 0; i < 9; i++)
    accJson["calibrationBias"].push_back(calib.calib_accel_bias.getParam()[i]);
  calibJson["accelerometer"] = accJson;

  std::ofstream os(output_path);
  os << calibJson.dump(4) << std::endl;

  return 0;
}


// Std includes
#include <iostream> // cout, endl
#include <vector>
#include <memory> // shared_ptr
#include <map>
// Thirdparties includes
#include <Eigen/Dense>
// Lib includes
#include "v0l/bin/file_data.h"
#include "m0sh/uniform.h"
#include "m0sh/structured_sub.h"
#include "fl0p/unstationary.h"

const unsigned int DIM = 3;

using TypeScalar = double;
using TypeVectorScalar = Eigen::Matrix<TypeScalar, 1, 1>;
using TypeVector = Eigen::Matrix<TypeScalar, DIM, 1>;
using TypeMatrix = Eigen::Matrix<TypeScalar, DIM, DIM>;
template<typename... Args>
using TypeRef = Eigen::Ref<Args...>;

template<typename ...Args>
using TypeContainer = std::vector<Args...>;
using TypeMesh = m0sh::Uniform<TypeVector, TypeRef, TypeContainer>;
using TypeMeshSub = m0sh::StructuredSub<TypeVector, TypeRef, TypeContainer>;
using TypeTimeMesh = m0sh::Uniform<TypeVectorScalar, TypeRef, TypeContainer>;
using TypeTimeMeshSub = m0sh::StructuredSub<TypeVectorScalar, TypeRef, TypeContainer>;
using TypeFlow = fl0w::fl0p::Unstationary<TypeVector, TypeMatrix, TypeRef, TypeMesh, TypeContainer, TypeMeshSub, TypeTimeMesh, TypeTimeMeshSub, TypeVectorScalar, v0l::FileData>;

void print(const TypeFlow& flow, const TypeVector& x, const TypeScalar& t) {
    std::cout << std::endl;
    std::cout << "flow.getVelocity(" << x.transpose() << ", " << t << ") = \n" << flow.getVelocity(x, t).transpose() << std::endl;
    //std::cout << "flow.getJacobian(" << x.transpose() << ", " << t << ") = \n" << flow.getJacobian(x, t) << std::endl;
    //std::cout << "flow.getVorticity(" << x.transpose() << ", " << t << ") = \n" << flow.getVorticity(x, t).transpose() << std::endl;
    //std::cout << "flow.getAcceleration(" << x.transpose() << ", " << t << ") = \n" << flow.getAcceleration(x, t).transpose() << std::endl;
    std::cout << std::endl;
}

int main () {
    std::vector<std::vector<v0l::FileData<float>>> velocity(2);
    for(std::size_t i = 0; i < velocity.size(); i++) {
        std::string fileName = std::string("../data/v050") + std::to_string(i) + ".vtk";
        velocity[i].emplace_back(fileName, 0);
        velocity[i].emplace_back(fileName, 1);
        velocity[i].emplace_back(fileName, 2);
    }
    // mesh data
    // // create lengths
    TypeVector origin;
    std::vector<double> lengths;
    for(std::size_t i = 0; i < velocity[0][0].meta.dimensions.size(); i++) {
        lengths.push_back(velocity[0][0].meta.dimensions[i] * velocity[0][0].meta.spacing[i]);
        origin[i] = velocity[0][0].meta.origin[i];
    }
    // // flow
    std::cout << "building flow..." << std::endl;
    TypeFlow flow(std::make_shared<TypeMesh>(velocity[0][0].meta.dimensions, lengths, origin, TypeContainer<bool>(DIM, true)), velocity, 1, std::make_shared<TypeTimeMesh>(std::vector<std::size_t>(1, 1), std::vector<double>(1, 0.02), TypeVectorScalar(0.0), std::vector<bool>(1, false)), 1);
    std::cout << "flow built !" << std::endl;
    // print
    print(flow, TypeVector({-0.5, -0.5, -0.5}), 0.0);
    print(flow, TypeVector({0.0, 0.0, 0.0}), 0.0);
    print(flow, TypeVector({0.5, 0.5, 0.5}), 0.0);
    print(flow, TypeVector({-0.5, -0.5, -0.5}), 0.01);
    print(flow, TypeVector({0.0, 0.0, 0.0}), 0.01);
    print(flow, TypeVector({0.5, 0.5, 0.5}), 0.01);
}


#ifndef PYRAMIDNONRIGIDREGISTRATION_HPP
#define PYRAMIDNONRIGIDREGISTRATION_HPP

#include <stdio.h>
#include <math.h>
#include <memory.h>
#include <time.h>
#include <Eigen/Dense>
#include "../global.hpp"
#include "NonrigidRegistration.hpp"
#include "Downsampler.hpp"
#include "ScaleShifter.hpp"

typedef Eigen::VectorXf VecDynFloat;
typedef Eigen::VectorXi VecDynInt;
typedef Eigen::Matrix< float, Eigen::Dynamic, registration::NUM_FEATURES> FeatureMat; //matrix Mx6 of type float
typedef Eigen::Matrix< int, Eigen::Dynamic, 3> FacesMat;

namespace registration{

class PyramidNonrigidRegistration
{
    /*
    # GOAL
    This class performs icp-based nonrigid registration between two oriented pointclouds.

    # INPUTS
    -ioFloatingFeatures
    -inTargetFeatures
    -inFloatingFlags
    -inTargetFlags

    # PARAMETERS
    -numNeighbours(=3):
    number of nearest neighbours

    # OUTPUT
    -outCorrespondingFeatures
    -outCorrespondingFlags
    */

    public:

        void set_input(FeatureMat &ioFloatingFeatures,
                       const FeatureMat &inTargetFeatures,
                       const FacesMat &inFloatingFaces,
                       const FacesMat &inTargetFaces,
                       const VecDynFloat &inFloatingFlags,
                       const VecDynFloat &inTargetFlags);

        void set_parameters(size_t numIterations = 60,
                            size_t numPyramidLayers = 3,
                            float downsampleFloatStart = 90.0f,
                            float downsampleTargetStart = 90.0f,
                            float downsampleFloatEnd = 0.0f,
                            float downsampleTargetEnd = 0.0f,
                            bool correspondencesSymmetric = true,
                            size_t correspondencesNumNeighbours = 5,
                            float _correspondencesFlagThreshold = 0.9f,
                            bool correspondencesEqualizePushPull = false,
                            float inlierKappa = 4.0f,
                            bool inlierUseOrientation = true,
                            float transformSigma = 3.0f,
                            size_t transformNumViscousIterationsStart = 200,
                            size_t transformNumViscousIterationsEnd = 1,
                            size_t transformNumElasticIterationsStart = 200,
                            size_t transformNumElasticIterationsEnd = 1);

        void update();

    protected:

    private:
        //# Inputs/Outputs
        FeatureMat * _ioFloatingFeatures = NULL;
        const FeatureMat * _inTargetFeatures = NULL;
        const FacesMat * _inFloatingFaces;
        const FacesMat * _inTargetFaces;
        const VecDynFloat * _inFloatingFlags = NULL;
        const VecDynFloat * _inTargetFlags = NULL;

        //# User Parameters
        //## Correspondences
        size_t _numIterations = 60;
        size_t _numPyramidLayers = 3;
        float _downsampleFloatStart = 90.0f; //percentage
        float _downsampleTargetStart = 90.0f; //percentage
        float _downsampleFloatEnd = 0.0f; //percentage
        float _downsampleTargetEnd = 0.0f; //percentage
        bool _correspondencesSymmetric = true;
        size_t _correspondencesNumNeighbours = 5;
        float _correspondencesFlagThreshold = 0.9f;
        bool _correspondencesEqualizePushPull = false;
        float _inlierKappa = 4.0f;
        bool _inlierUseOrientation = true;
        float _transformSigma = 3.0f;
        size_t _transformNumViscousIterationsStart = 200;
        size_t _transformNumViscousIterationsEnd = 1;
        size_t _transformNumElasticIterationsStart = 200;
        size_t _transformNumElasticIterationsEnd = 1;

        //# Internal Data structures

        //# Internal Parameters
        int _iterationsPerLayer = 0;
        //## Transformation
        float _viscousAnnealingRate = exp(log(float(_transformNumViscousIterationsEnd)/float(_transformNumViscousIterationsStart))/_numIterations);
        float _elasticAnnealingRate = exp(log(float(_transformNumElasticIterationsEnd)/float(_transformNumElasticIterationsStart))/_numIterations);
        std::vector<int> _viscousIterationsIntervals;
        std::vector<int> _elasticIterationsIntervals;
        //# Internal functions
};

}//namespace registration

#endif // PYRAMIDNONRIGIDREGISTRATION_HPP


// Copyright (C) 2009  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include "tester.h"
#include <dlib/svm.h>
#include <dlib/rand.h>
#include <dlib/string.h>
#include <vector>
#include <sstream>
#include <ctime>

namespace
{
    using namespace test;
    using namespace dlib;
    using namespace std;
    dlib::logger dlog("test.discriminant_pca");

    using dlib::equal;

    class discriminant_pca_tester : public tester
    {
        /*!
            WHAT THIS OBJECT REPRESENTS
                This object represents a unit test.  When it is constructed
                it adds itself into the testing framework.
        !*/
    public:
        discriminant_pca_tester (
        ) :
            tester (
                "test_discriminant_pca",       // the command line argument name for this test
                "Run tests on the discriminant_pca object.", // the command line argument description
                0                     // the number of command line arguments for this test
            )
        {
            thetime = time(0);
        }

        time_t thetime;
        dlib::rand rnd;

        template <typename dpca_type>
        void test1()
        {

            dpca_type dpca, dpca2, dpca3;

            DLIB_TEST(dpca.in_vector_size() == 0);
            DLIB_TEST(dpca.between_class_weight() == 1);
            DLIB_TEST(dpca.within_class_weight() == 1);

            // generate a bunch of 4 dimensional vectors and compute the normal PCA transformation matrix
            // and just make sure it is a unitary matrix as it should be.
            for (int i = 0; i < 5000; ++i)
            {
                dpca.add_to_total_variance(randm(4,1,rnd));
                DLIB_TEST(dpca.in_vector_size() == 4);
            }

            matrix<double> mat = dpca.dpca_matrix(1);

            DLIB_TEST(equal(mat*trans(mat), identity_matrix<double>(4)));

            mat = dpca.dpca_matrix(0.9);
            DLIB_TEST(equal(mat*trans(mat), identity_matrix<double>(mat.nr())));

            matrix<double> eig;
            dpca.dpca_matrix(mat, eig, 1);
            DLIB_TEST(equal(mat*trans(mat), identity_matrix<double>(4)));
            // check that all eigen values are grater than 0
            DLIB_TEST(min(eig > 0) == 1);
            DLIB_TEST(eig.size() == mat.nr());
            DLIB_TEST(is_col_vector(eig));
            // check that the eigenvalues are sorted
            double last = eig(0);
            for (long i = 1; i < eig.size(); ++i)
            {
                DLIB_TEST(last >= eig(i));
            }

            {
                matrix<double> mat = dpca.dpca_matrix_of_size(4);
                DLIB_TEST(equal(mat*trans(mat), identity_matrix<double>(4)));
            }
            {
                matrix<double> mat = dpca.dpca_matrix_of_size(3);
                DLIB_TEST(equal(mat*trans(mat), identity_matrix<double>(3)));
            }

            dpca.set_within_class_weight(5);
            dpca.set_between_class_weight(6);

            DLIB_TEST(dpca.in_vector_size() == 4);
            DLIB_TEST(dpca.within_class_weight() == 5);
            DLIB_TEST(dpca.between_class_weight() == 6);

            ostringstream sout;
            serialize(dpca, sout);
            istringstream sin(sout.str());
            deserialize(dpca2, sin);

            // now make sure the serialization worked
            DLIB_TEST(dpca.in_vector_size() == 4);
            DLIB_TEST(dpca.within_class_weight() == 5);
            DLIB_TEST(dpca.between_class_weight() == 6);
            DLIB_TEST(dpca2.in_vector_size() == 4);
            DLIB_TEST(dpca2.within_class_weight() == 5);
            DLIB_TEST(dpca2.between_class_weight() == 6);
            DLIB_TEST(equal(dpca.dpca_matrix(), dpca2.dpca_matrix(), 1e-10));
            DLIB_TEST(equal(mat, dpca2.dpca_matrix(1), 1e-10));
            DLIB_TEST(equal(dpca.dpca_matrix(1), mat, 1e-10));

            // now test swap
            dpca2.swap(dpca3);
            DLIB_TEST(dpca2.in_vector_size() == 0);
            DLIB_TEST(dpca2.between_class_weight() == 1);
            DLIB_TEST(dpca2.within_class_weight() == 1);

            DLIB_TEST(dpca3.in_vector_size() == 4);
            DLIB_TEST(dpca3.within_class_weight() == 5);
            DLIB_TEST(dpca3.between_class_weight() == 6);
            DLIB_TEST(equal(mat, dpca3.dpca_matrix(1), 1e-10));
            DLIB_TEST((dpca3 + dpca3).in_vector_size() == 4);
            DLIB_TEST((dpca3 + dpca3).within_class_weight() == 5);
            DLIB_TEST((dpca3 + dpca3).between_class_weight() == 6);

            dpca.clear();

            DLIB_TEST(dpca.in_vector_size() == 0);
            DLIB_TEST(dpca.between_class_weight() == 1);
            DLIB_TEST(dpca.within_class_weight() == 1);
        }

        template <typename dpca_type>
        void test2()
        {
            dpca_type dpca, dpca2, dpca3;

            typename dpca_type::column_matrix samp1(4), samp2(4);

            for (int i = 0; i < 5000; ++i)
            {
                dpca.add_to_total_variance(randm(4,1,rnd));
                DLIB_TEST(dpca.in_vector_size() == 4);

                // do this to subtract out the variance along the 3rd axis
                samp1 = 0,0,0,0;
                samp2 = 0,0,1,0;
                dpca.add_to_within_class_variance(samp1, samp2);
            }

            matrix<double> mat;

            dpca.set_within_class_weight(0);
            mat = dpca.dpca_matrix(1);
            DLIB_TEST(equal(mat*trans(mat), identity_matrix<double>(4)));
            DLIB_TEST(dpca.dpca_matrix(1).nr() == 4);
            dpca.set_within_class_weight(1000);
            DLIB_TEST(dpca.dpca_matrix(1).nr() == 3);

            // the 3rd column of the transformation matrix should be all zero since
            // we killed all the variation long the 3rd axis
            DLIB_TEST(sum(abs(colm(dpca.dpca_matrix(1),2))) < 1e-5);

            mat = dpca.dpca_matrix(1);
            DLIB_TEST(equal(mat*trans(mat), identity_matrix<double>(3)));

        }

        template <typename dpca_type>
        void test3()
        {
            dpca_type dpca, dpca2, dpca3;

            typename dpca_type::column_matrix samp1(4), samp2(4);

            for (int i = 0; i < 5000; ++i)
            {
                dpca.add_to_total_variance(randm(4,1,rnd));
                DLIB_TEST(dpca.in_vector_size() == 4);

                // do this to subtract out the variance along the 3rd axis
                samp1 = 0,0,0,0;
                samp2 = 0,0,1,0;
                dpca.add_to_within_class_variance(samp1, samp2);

                // do this to subtract out the variance along the 1st axis
                samp1 = 0,0,0,0;
                samp2 = 1,0,0,0;
                dpca.add_to_within_class_variance(samp1, samp2);
            }

            matrix<double> mat;

            dpca.set_within_class_weight(0);
            mat = dpca.dpca_matrix(1);
            DLIB_TEST(equal(mat*trans(mat), identity_matrix<double>(4)));
            DLIB_TEST(dpca.dpca_matrix(1).nr() == 4);
            dpca.set_within_class_weight(10000);
            DLIB_TEST(dpca.dpca_matrix(1).nr() == 2);

            // the 1st and 3rd columns of the transformation matrix should be all zero since
            // we killed all the variation long the 1st and 3rd axes
            DLIB_TEST(sum(abs(colm(dpca.dpca_matrix(1),2))) < 1e-5);
            DLIB_TEST(sum(abs(colm(dpca.dpca_matrix(1),0))) < 1e-5);

            mat = dpca.dpca_matrix(1);
            DLIB_TEST(equal(mat*trans(mat), identity_matrix<double>(2)));

        }

        template <typename dpca_type>
        void test4()
        {
            dpca_type dpca, dpca2, dpca3;

            dpca_type add_dpca1, add_dpca2, add_dpca3, add_dpca4, sum_dpca;

            typename dpca_type::column_matrix samp1(4), samp2(4), samp;

            for (int i = 0; i < 5000; ++i)
            {
                samp = randm(4,1,rnd);
                dpca.add_to_total_variance(samp);
                add_dpca4.add_to_total_variance(samp);
                DLIB_TEST(dpca.in_vector_size() == 4);

                // do this to subtract out the variance along the 3rd axis
                samp1 = 0,0,0,0;
                samp2 = 0,0,1,0;
                dpca.add_to_within_class_variance(samp1, samp2);
                add_dpca1.add_to_within_class_variance(samp1, samp2);

                // do this to subtract out the variance along the 1st axis
                samp1 = 0,0,0,0;
                samp2 = 1,0,0,0;
                dpca.add_to_within_class_variance(samp1, samp2);
                add_dpca2.add_to_within_class_variance(samp1, samp2);

                // do this to add the variance along the 3rd axis back in
                samp1 = 0,0,0,0;
                samp2 = 0,0,1,0;
                dpca.add_to_between_class_variance(samp1, samp2);
                add_dpca3.add_to_between_class_variance(samp1, samp2);
            }

            matrix<double> mat, mat2;

            sum_dpca = dpca_type() + dpca_type() + add_dpca1 + dpca_type() + add_dpca2 + add_dpca3 + add_dpca4;
            dpca.set_within_class_weight(0);
            dpca.set_between_class_weight(0);
            sum_dpca.set_within_class_weight(0);
            sum_dpca.set_between_class_weight(0);
            mat = dpca.dpca_matrix(1);
            DLIB_TEST(equal(mat, sum_dpca.dpca_matrix(1), 1e-10));
            DLIB_TEST(equal(mat*trans(mat), identity_matrix<double>(4)));
            DLIB_TEST(dpca.dpca_matrix(1).nr() == 4);
            dpca.set_within_class_weight(10000);
            sum_dpca.set_within_class_weight(10000);
            DLIB_TEST(dpca.dpca_matrix(1).nr() == 2);

            // the 1st and 3rd columns of the transformation matrix should be all zero since
            // we killed all the variation long the 1st and 3rd axes
            DLIB_TEST(sum(abs(colm(dpca.dpca_matrix(1),2))) < 1e-4);
            DLIB_TEST(sum(abs(colm(dpca.dpca_matrix(1),0))) < 1e-4);

            mat = dpca.dpca_matrix(1);
            DLIB_TEST(equal(mat*trans(mat), identity_matrix<double>(2)));
            DLIB_TEST_MSG(equal(mat, mat2=sum_dpca.dpca_matrix(1), 1e-9), max(abs(mat - mat2)));

            // now add the variance back in using the between class weight
            dpca.set_within_class_weight(0);
            dpca.set_between_class_weight(1);
            mat = dpca.dpca_matrix(1);
            DLIB_TEST(equal(mat*trans(mat), identity_matrix<double>(4)));
            DLIB_TEST(dpca.dpca_matrix(1).nr() == 4);
            dpca.set_within_class_weight (10000);
            dpca.set_between_class_weight(100000);
            sum_dpca.set_within_class_weight (10000);
            sum_dpca.set_between_class_weight(100000);
            DLIB_TEST(dpca.dpca_matrix(1).nr() == 3);

            // the first column should be all zeros
            DLIB_TEST(sum(abs(colm(dpca.dpca_matrix(1),0))) < 1e-5);

            mat = dpca.dpca_matrix(1);
            DLIB_TEST(equal(mat*trans(mat), identity_matrix<double>(3)));
            DLIB_TEST(equal(mat, sum_dpca.dpca_matrix(1)));

        }

        template <typename dpca_type>
        void test5()
        {
            dpca_type dpca, dpca2;
            typename dpca_type::column_matrix samp1(4), samp2(4);

            samp1 = 0,0,0,0;
            samp2 = 0,0,1,0;

            for (int i = 0; i < 5000; ++i)
            {
                dpca.add_to_between_class_variance(samp1, samp2);
                dpca2.add_to_total_variance(samp1);
                dpca2.add_to_total_variance(samp2);
            }

            matrix<double> mat, eig;
            dpca.dpca_matrix(mat, eig, 1);

            // make sure the eigenvalues come out the way they should for this simple data set
            DLIB_TEST(eig.size() == 1);
            DLIB_TEST_MSG(abs(eig(0) - 1) < 1e-10, abs(eig(0) - 1));

            dpca2.dpca_matrix(mat, eig, 1);

            // make sure the eigenvalues come out the way they should for this simple data set
            DLIB_TEST(eig.size() == 1);
            DLIB_TEST(abs(eig(0) - 0.25) < 1e-10);

        }

        void perform_test (
        )
        {
            ++thetime;
            typedef matrix<double,0,1> sample_type;
            typedef discriminant_pca<sample_type> dpca_type;

            dlog << LINFO << "time seed: " << thetime;
            rnd.set_seed(cast_to_string(thetime));

            test5<dpca_type>();

            for (int i = 0; i < 10; ++i)
            {
                print_spinner();
                test1<dpca_type>();
                print_spinner();
                test2<dpca_type>();
                print_spinner();
                test3<dpca_type>();
                print_spinner();
                test4<dpca_type>();
            }
        }
    };

    // Create an instance of this object.  Doing this causes this test
    // to be automatically inserted into the testing framework whenever this cpp file
    // is linked into the project.  Note that since we are inside an unnamed-namespace
    // we won't get any linker errors about the symbol a being defined multiple times.
    discriminant_pca_tester a;

}