#include <iostream>
#include <armadillo>
#include <list>

#include "element.h"
#include "assembly.h"
#include "boundary.h"

using namespace std;
using namespace arma;

//TODO create material.h
//TODO parse input file to initialize model data
//TODO parse command line args
//TODO it may make more sense to restructure data, each element containing
//  pointers to node objects?

int main(int argc, char** argv)
{
  cout << "Program launched." << endl;
  int i;

  /* initialize model data */
  const double h = 0.1;
  const long long int E = 200e9;
  const double v = 0.3;
  const int rho = 7800;
  const double g = 9.8;

  /* body and traction forces */
  vec b(Q4__DOF_PER_NODE);
  b     << 0            << endr
        << -(rho * g)   << endr;
  vec t(Q4__DOF_PER_NODE);
  t     << 0            << endr
        << 0            << endr;

  /* boundary conditions */
  list<BC*> bounds;
  bounds.push_front(new BC(0, 0));
  bounds.push_front(new BC(1, 0));
  bounds.push_front(new BC(3, 0));
  bounds.push_front(new BC(5, 0));

  /* mesh */
  cout << "Creating mesh..." << endl;

  const int numElem = 2;
  const int numNodes = 6;

  int gnodes[] = {1, 2, 5, 4};
  mat gcoords(Q4__NUM_NODES, Q4__DOF_PER_NODE);
  gcoords   << 0.    << 0.    << endr
            << 10.   << 2.    << endr
            << 5.    << 8.    << endr
            << 2.    << 6.    << endr;
  int gdofs[] = {1, 2, 3, 4, 9, 10, 7, 8};
  Q4 *elem = new Q4(E, v, h, &b, &t, gnodes, &gcoords, gdofs);

  int gnodes2[] = {2, 3, 6, 5};
  mat gcoords2(Q4__NUM_NODES, Q4__DOF_PER_NODE);
  gcoords2  << 10.   << 2.    << endr
            << 20.   << 0.    << endr
            << 17.   << 6.    << endr
            << 5.    << 8.    << endr;
  int gdof2[] = {3, 4, 5, 6, 11, 12, 9, 10};
  Q4 *elem2 = new Q4(E, v, h, &b, &t, gnodes2, &gcoords2, gdof2);

  /* calculate element stiffnesses and assemble */
  cout << "Analyzing discretized system..." << endl;

  MechElem *pelems[numElem] = {elem, elem2};

  mat kg = zeros<mat>(Q4__DOF_PER_NODE*numNodes, Q4__DOF_PER_NODE*numNodes);
  mglobalStiffness(kg, pelems, numElem, Q4__DOF_PER_NODE*Q4__NUM_NODES, PSTRESS);

  cout << endl << "Global Stiffness Matrix" << endl
       << kg << endl;

  /* calculate element body forces and assemble */
  vec bg(Q4__DOF_PER_NODE*numNodes);
  mglobalBodyForce(bg, pelems, numElem, Q4__DOF_PER_NODE*Q4__NUM_NODES);

  cout << endl << "Global Body Force" << endl
       << bg << endl;

  /* assemble global force vector */
  vec fg = zeros<vec>(Q4__DOF_PER_NODE*numNodes);
  fg(4) = 10.e3;
  fg(7) = -10.e3;
  fg(9) = -10.e3;
  for (i = 0; i < Q4__DOF_PER_NODE*numNodes; i++)
    fg(i) += bg(i);

  cout << endl << "Global Force" << endl
       << fg << endl;

  /* impose boundary condtions */
  cout << endl << "Imposing boundary conditions..." << endl;
  mimposeBoundaryConds(kg, fg, bounds);

  /* solve for displacements */
  cout << "Solving for displacements..." << endl;
  vec ug = zeros<vec>(Q4__DOF_PER_NODE*numNodes);
  if (!solve(ug, kg, fg))
  {
    cout << endl << "Error: solution not found" << endl;
    return 1;
  }

  /* display answer */
  cout << endl << "================================" << endl;
  cout << endl << "Modified Global Stiffness Matrix (by imposing EBCs)" << endl
       << kg << endl;
  cout << endl << "Modified Global Force (by imposing EBCs)" << endl
       << fg << endl;
  cout << endl << "Global Displacements" << endl
       << ug << endl;

  /* clean up dynamic memory */
  cout << endl << "Cleaning up allocated memory..." << endl;
  for (BC *pbc : bounds) delete pbc;
  delete elem;
  delete elem2;

  return 0;
}


/**
 * \file dcs/math/traits/float.hpp
 *
 * \brief Traits class for floating-point type.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2009 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * 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.
 */

#ifndef DCS_MATH_TRAITS_FLOAT_HPP
#define DCS_MATH_TRAITS_FLOAT_HPP

#include <boost/type_traits/is_floating_point.hpp>
#include <boost/utility/enable_if.hpp>
#include <dcs/math/detail/float.hpp>
#include <limits>

namespace dcs { namespace math {

template <typename T, typename Enable_ = void>
struct float_traits;

template <typename T>
struct float_traits<T, typename ::boost::enable_if< ::boost::is_floating_point<T> >::type>
{
	/// Default tolerance for floating-point comparison.
	static const T tolerance;

	static bool approximately_equal(T x, T y, T tol)
	{
		return detail::approximately_equal(x, y, tol);
	}

	static bool approximately_equal(T x, T y)
	{
		return detail::approximately_equal(x, y, tolerance);
	}

	static bool essentially_equal(T x, T y, T tol)
	{
		return detail::essentially_equal(x, y, tol);
	}

	static bool essentially_equal(T x, T y)
	{
		return detail::essentially_equal(x, y, tolerance);
	}

	static bool definitely_less(T x, T y, T tol)
	{
		return detail::definitely_less(x, y, tol);
	}

	static bool definitely_less(T x, T y)
	{
		return detail::definitely_less(x, y, tolerance);
	}

	/// \deprecated Use \c approximately_less_equal or \c essentially_less_equal
	static bool definitely_less_equal(T x, T y, T tol)
	{
		return definitely_less(x, y, tol) || approximately_equal(x, y, tol);
	}

	/// \deprecated Use \c approximately_less_equal or \c essentially_less_equal
	static bool definitely_less_equal(T x, T y)
	{
		return definitely_less_equal(x, y, tolerance);
	}

	static bool approximately_less_equal(T x, T y, T tol)
	{
		return definitely_less(x, y, tol) || approximately_equal(x, y, tol);
	}

	static bool approximately_less_equal(T x, T y)
	{
		return approximately_less_equal(x, y, tolerance);
	}

	static bool essentially_less_equal(T x, T y, T tol)
	{
		return definitely_less(x, y, tol) || essentially_equal(x, y, tol);
	}

	static bool essentially_less_equal(T x, T y)
	{
		return essentially_less_equal(x, y, tolerance);
	}

	static bool definitely_greater(T x, T y, T tol)
	{
		return detail::definitely_greater(x, y, tol);
	}

	static bool definitely_greater(T x, T y)
	{
		return detail::definitely_greater(x, y, tolerance);
	}

	/// \deprecated Use \c approximately_less_equal or \c essentially_less_equal
	static bool definitely_greater_equal(T x, T y, T tol)
	{
		return definitely_greater(x, y, tol) || approximately_equal(x, y, tol);
	}

	/// \deprecated Use \c approximately_greater_equal or \c essentially_greater_equal
	static bool definitely_greater_equal(T x, T y)
	{
		return definitely_greater_equal(x, y, tolerance);
	}

	static bool approximately_greater_equal(T x, T y, T tol)
	{
		return definitely_greater(x, y, tol) || approximately_equal(x, y, tol);
	}

	static bool approximately_greater_equal(T x, T y)
	{
		return approximately_greater_equal(x, y, tolerance);
	}

	static bool essentially_greater_equal(T x, T y, T tol)
	{
		return definitely_greater(x, y, tol) || essentially_equal(x, y, tol);
	}

	static bool essentially_greater_equal(T x, T y)
	{
		return essentially_greater_equal(x, y, tolerance);
	}

	/// \deprecated Use \c definitely_min
	static T min(T x, T y, T tol = tolerance)
	{
		if (definitely_less(x, y, tol))
		{
			return x;
		}
		return y;
	}

	/// \deprecated Use \c definitely_max
	static T max(T x, T y, T tol = tolerance)
	{
		if (definitely_greater(x, y, tol))
		{
			return x;
		}
		return y;
	}

	static T definitely_min(T x, T y, T tol = tolerance)
	{
		if (definitely_less(x, y, tol))
		{
			return x;
		}
		return y;
	}

	static T definitely_max(T x, T y, T tol = tolerance)
	{
		if (definitely_greater(x, y, tol))
		{
			return x;
		}
		return y;
	}

/*
bool is_nan(float f)
{
    return (*reinterpret_cast<uint32_t*>(&f) & 0x7f800000) == 0x7f800000 && (*reinterpret_cast<uint32_t*>(&f) & 0x007fffff) != 0;
}

bool is_finite(float f)
{
    return (*reinterpret_cast<uint32_t*>(&f) & 0x7f800000) != 0x7f800000;
}

// if this symbol is defined, NaNs are never equal to anything (as is normal in IEEE floating point)
// if this symbol is not defined, NaNs are hugely different from regular numbers, but might be equal to each other
#define UNEQUAL_NANS 1
// if this symbol is defined, infinites are never equal to finite numbers (as they're unimaginably greater)
// if this symbol is not defined, infinities are 1 ULP away from +/- FLT_MAX
#define INFINITE_INFINITIES 1
//
// test whether two IEEE floats are within a specified number of representable values of each other
// This depends on the fact that IEEE floats are properly ordered when treated as signed magnitude integers
bool equal_float(float lhs, float rhs, uint32_t max_ulp_difference)
{
#ifdef UNEQUAL_NANS
	if(is_nan(lhs) || is_nan(rhs))
	{
		return false;
	}
#endif
#ifdef INFINITE_INFINITIES
	if((is_finite(lhs) && !is_finite(rhs)) || (!is_finite(lhs) && is_finite(rhs)))
	{
		return false;
	}
#endif
	int32_t left(*reinterpret_cast<int32_t*>(&lhs));
	// transform signed magnitude ints into 2s complement signed ints
	if(left < 0)
	{
		left = 0x80000000 - left;
	}
	int32_t right(*reinterpret_cast<int32_t*>(&rhs));
	// transform signed magnitude ints into 2s complement signed ints
	if(right < 0)
	{
		right = 0x80000000 - right;
	}
	if(static_cast<uint32_t>(std::abs(left - right)) <= max_ulp_difference)
	{
		return true;
	}
	return false;
}
*/
};

template <typename T>
const T float_traits<T, typename ::boost::enable_if< ::boost::is_floating_point<T> >::type>::tolerance = static_cast<T>(100)*::std::numeric_limits<T>::epsilon();

}} // Namespace dcs::math

#endif // DCS_MATH_TRAITS_FLOAT_HPP


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_EXTENSIONS_GIS_GEOGRAPHIC_STRATEGIES_ANDOYER_HPP
#define BOOST_GEOMETRY_EXTENSIONS_GIS_GEOGRAPHIC_STRATEGIES_ANDOYER_HPP

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>
#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/extensions/gis/geographic/detail/ellipsoid.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

/*!
\brief Point-point distance approximation taking flattening into account
\ingroup distance
\tparam Point1 \tparam_first_point
\tparam Point2 \tparam_second_point
\tparam CalculationType \tparam_calculation
\author After Andoyer, 19xx, republished 1950, republished by Meeus, 1999
\note Although not so well-known, the approximation is very good: in all cases the results
are about the same as Vincenty. In my (Barend's) testcases the results didn't differ more than 6 m
\see http://nacc.upc.es/tierra/node16.html
\see http://sci.tech-archive.net/Archive/sci.geo.satellite-nav/2004-12/2724.html
\see http://home.att.net/~srschmitt/great_circle_route.html (implementation)
\see http://www.codeguru.com/Cpp/Cpp/algorithms/article.php/c5115 (implementation)
\see http://futureboy.homeip.net/frinksamp/navigation.frink (implementation)
\see http://www.voidware.com/earthdist.htm (implementation)
*/
template
<
    typename Point1,
    typename Point2 = Point1,
    typename CalculationType = void
>
class andoyer
{
    public :
    typedef typename promote_floating_point
        <
            typename select_calculation_type
                <
                    Point1,
                    Point2,
                    CalculationType
                >::type
        >::type calculation_type;

        inline andoyer()
            : m_ellipsoid()
        {}

        explicit inline andoyer(calculation_type f)
            : m_ellipsoid(f)
        {}

        explicit inline andoyer(geometry::detail::ellipsoid<calculation_type> const& e)
            : m_ellipsoid(e)
        {}

        inline calculation_type apply(Point1 const& point1, Point2 const& point2) const
        {
            return calc(get_as_radian<0>(point1), get_as_radian<1>(point1),
                            get_as_radian<0>(point2), get_as_radian<1>(point2));
        }

        inline geometry::detail::ellipsoid<calculation_type> ellipsoid() const
        {
            return m_ellipsoid;
        }

        inline calculation_type radius() const
        {
            return m_ellipsoid.a();
        }

    private :
        geometry::detail::ellipsoid<calculation_type> m_ellipsoid;

        inline calculation_type calc(calculation_type const& lon1,
                    calculation_type const& lat1,
                    calculation_type const& lon2,
                    calculation_type const& lat2) const
        {
            calculation_type const G = (lat1 - lat2) / 2.0;
            calculation_type const lambda = (lon1 - lon2) / 2.0;

            if (geometry::math::equals(lambda, 0.0)
                && geometry::math::equals(G, 0.0))
            {
                return 0.0;
            }

            calculation_type const F = (lat1 + lat2) / 2.0;

            calculation_type const sinG2 = math::sqr(sin(G));
            calculation_type const cosG2 = math::sqr(cos(G));
            calculation_type const sinF2 = math::sqr(sin(F));
            calculation_type const cosF2 = math::sqr(cos(F));
            calculation_type const sinL2 = math::sqr(sin(lambda));
            calculation_type const cosL2 = math::sqr(cos(lambda));

            calculation_type const S = sinG2 * cosL2 + cosF2 * sinL2;
            calculation_type const C = cosG2 * cosL2 + sinF2 * sinL2;

            calculation_type const c0 = 0;
            calculation_type const c1 = 1;
            calculation_type const c2 = 2;
            calculation_type const c3 = 3;

            if (geometry::math::equals(S, c0) || geometry::math::equals(C, c0))
            {
                return c0;
            }

            calculation_type const omega = atan(sqrt(S / C));
            calculation_type const r3 = c3 * sqrt(S * C) / omega; // not sure if this is r or greek nu
            calculation_type const D = c2 * omega * m_ellipsoid.a();
            calculation_type const H1 = (r3 - c1) / (c2 * C);
            calculation_type const H2 = (r3 + c1) / (c2 * S);
            calculation_type const f = m_ellipsoid.f();

            return D * (c1 + f * H1 * sinF2 * cosG2 - f * H2 * cosF2 * sinG2);
        }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename Point1, typename Point2>
struct tag<strategy::distance::andoyer<Point1, Point2> >
{
    typedef strategy_tag_distance_point_point type;
};

template <typename Point1, typename Point2>
struct return_type<strategy::distance::andoyer<Point1, Point2> >
{
    typedef typename strategy::distance::andoyer<Point1, Point2>::calculation_type type;
};

template <typename Point1, typename Point2, typename P1, typename P2>
struct similar_type<andoyer<Point1, Point2>, P1, P2>
{
    typedef andoyer<P1, P2> type;
};

template <typename Point1, typename Point2, typename P1, typename P2>
struct get_similar<andoyer<Point1, Point2>, P1, P2>
{
    static inline andoyer<P1, P2> apply(andoyer<Point1, Point2> const& input)
    {
        return andoyer<P1, P2>(input.ellipsoid());
    }
};

template <typename Point1, typename Point2>
struct comparable_type<andoyer<Point1, Point2> >
{
    typedef andoyer<Point1, Point2> type;
};

template <typename Point1, typename Point2>
struct get_comparable<andoyer<Point1, Point2> >
{
    static inline andoyer<Point1, Point2> apply(andoyer<Point1, Point2> const& input)
    {
        return input;
    }
};

template <typename Point1, typename Point2>
struct result_from_distance<andoyer<Point1, Point2> >
{
    template <typename T>
    static inline typename return_type<andoyer<Point1, Point2> >::type apply(andoyer<Point1, Point2> const& , T const& value)
    {
        return value;
    }
};

template <typename Point1, typename Point2>
struct default_strategy<point_tag, Point1, Point2, geographic_tag, geographic_tag>
{
    typedef strategy::distance::andoyer<Point1, Point2> type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_GIS_GEOGRAPHIC_STRATEGIES_ANDOYER_HPP


#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include <ceres/ceres.h>
#include <chrono>
#include <sophus/se3.hpp>

using namespace std;
using namespace cv;

void find_feature_matches(const Mat &img_1, const Mat &img_2,
                          std::vector<KeyPoint> &keypoints_1,
                          std::vector<KeyPoint> &keypoints_2,
                          std::vector<DMatch> &matches) {
  Mat descriptors_1, descriptors_2;
  // used in OpenCV3
  Ptr<FeatureDetector> detector = ORB::create();
  Ptr<DescriptorExtractor> descriptor = ORB::create();
  // use this if you are in OpenCV2
  // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
  // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
  Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
  detector->detect(img_1, keypoints_1);
  detector->detect(img_2, keypoints_2);

  descriptor->compute(img_1, keypoints_1, descriptors_1);
  descriptor->compute(img_2, keypoints_2, descriptors_2);

  vector<DMatch> match;
  // BFMatcher matcher ( NORM_HAMMING );
  matcher->match(descriptors_1, descriptors_2, match);

  double min_dist = 10000, max_dist = 0;

  for (int i = 0; i < descriptors_1.rows; i++) {
    double dist = match[i].distance;
    if (dist < min_dist) min_dist = dist;
    if (dist > max_dist) max_dist = dist;
  }

  printf("-- Max dist : %f \n", max_dist);
  printf("-- Min dist : %f \n", min_dist);

  for (int i = 0; i < descriptors_1.rows; i++) {
    if (match[i].distance <= max(2 * min_dist, 30.0)) {
      matches.push_back(match[i]);
    }
  }
}

Point2d pixel2cam(const Point2d &p, const Mat &K) {
  return Point2d(
    (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
    (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
  );
}

// Solve ICP with linear algebra (SVD solution)
void pose_estimation_3d3d(vector<Eigen::Vector3d> pts1,
                          vector<Eigen::Vector3d> pts2,
                          Eigen::Matrix3d &R, Eigen::Vector3d &t) {
  Eigen::Vector3d c1(0.0, 0.0, 0.0), c2(0.0, 0.0, 0.0);
  for (auto& p : pts1)
    c1 += p;
  c1 /= pts1.size();
  for (auto& p : pts2)
    c2 += p;
  c2 /= pts2.size();

  for (auto& p : pts1)
    p -= c1;
  for (auto& p : pts2)
    p -= c2;

  Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
  for (int i = 0; i < pts1.size(); ++i)
  {
    W += pts1[i] * pts2[i].transpose();
  }
  Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
  R = svd.matrixU() * svd.matrixV().transpose();
  if (R.determinant() < 0)
    R = -R;

  t = c1 - R * c2;
}

// Local parameterization needed to handle SE3 from Sophus (from Sophus/test/ceres/)
using namespace Sophus;
class LocalParameterizationSE3 : public ceres::LocalParameterization {
 public:
  virtual ~LocalParameterizationSE3() {}

  // SE3 plus operation for Ceres
  //
  //  T * exp(x)
  //
  virtual bool Plus(double const* T_raw, double const* delta_raw,
                    double* T_plus_delta_raw) const {
    Eigen::Map<SE3d const> const T(T_raw);
    Eigen::Map<Vector6d const> const delta(delta_raw);
    Eigen::Map<SE3d> T_plus_delta(T_plus_delta_raw);
    T_plus_delta = T * SE3d::exp(delta);
    return true;
  }

  // Jacobian of SE3 plus operation for Ceres
  //
  // Dx T * exp(x)  with  x=0
  //
  virtual bool ComputeJacobian(double const* T_raw,
                               double* jacobian_raw) const {
    Eigen::Map<SE3d const> T(T_raw);
    Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> jacobian(
        jacobian_raw);
    jacobian = T.Dx_this_mul_exp_x_at_0();
    return true;
  }

  virtual int GlobalSize() const { return SE3d::num_parameters; }

  virtual int LocalSize() const { return SE3d::DoF; }
};

struct ICPError
{
  ICPError(const Eigen::Vector3d& X1, const Eigen::Vector3d& X2)
    : _X1(X1), _X2(X2)
    {}

    template <class T>
    bool operator() (const T* const params, T* errors) const {
      const Eigen::Map<const Sophus::SE3<T>> Rt(params);
      Eigen::Map<Eigen::Matrix<T, 3, 1>> err(errors);
      err = _X1 - Rt * _X2;
      return true;
    }

  private:
    Eigen::Vector3d _X1;
    Eigen::Vector3d _X2;
};

// Solve ICP with non-linear optimization
void bundleAdjustment(
  const vector<Eigen::Vector3d> &pts1,
  const vector<Eigen::Vector3d> &pts2,
  Eigen::Matrix3d &R, Eigen::Vector3d &t) {

    Sophus::SE3d pose(R, t);
    ceres::Problem problem;
    for (int i = 0; i < pts1.size(); ++i)
    {
      problem.AddResidualBlock(
        new ceres::AutoDiffCostFunction<ICPError, 3, 7>( // in g2o we use size 6 because its the size of the delta_x, here its the real size, the delta_x is handle in the local parameterization
          new ICPError(pts1[i], pts2[i])
        ),
        nullptr,
        pose.data()
      );
    }

    problem.SetParameterization(pose.data(), new LocalParameterizationSE3);

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
    options.minimizer_progress_to_stdout = true;

    ceres::Solver::Summary summary;
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    ceres::Solve(options, &problem, &summary);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "optimization ICP (ceres) costs time: " << time_used.count() << " seconds." << endl;
    std::cout << summary.BriefReport() << "\n";

    R = pose.so3().unit_quaternion().toRotationMatrix();
    t = pose.translation();
  }

int main(int argc, char **argv) {
  // if (argc != 5) {
  //   cout << "usage: pose_estimation_3d3d img1 img2 depth1 depth2" << endl;
  //   return 1;
  // }
  string f1 = "../1.png"; //argv[1];
  string f2 = "../2.png"; //argv[2];
  string f3 = "../1_depth.png"; //argv[3];
  string f4 = "../2_depth.png"; //argv[3];

  Mat img_1 = imread(f1, CV_LOAD_IMAGE_COLOR);
  Mat img_2 = imread(f2, CV_LOAD_IMAGE_COLOR);

  vector<KeyPoint> keypoints_1, keypoints_2;
  vector<DMatch> matches;
  find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);

  Mat depth1 = imread(f3, CV_LOAD_IMAGE_UNCHANGED);
  Mat depth2 = imread(f4, CV_LOAD_IMAGE_UNCHANGED);
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
  Eigen::Matrix3d K_eigen;
  K_eigen << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1;
  std::vector<Eigen::Vector3d> pts1, pts2;

  for (DMatch m:matches) {
    ushort d1 = depth1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
    ushort d2 = depth2.ptr<unsigned short>(int(keypoints_2[m.trainIdx].pt.y))[int(keypoints_2[m.trainIdx].pt.x)];
    if (d1 == 0 || d2 == 0)   // bad depth
      continue;
    Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
    Point2d p2 = pixel2cam(keypoints_2[m.trainIdx].pt, K);
    float dd1 = float(d1) / 5000.0;
    float dd2 = float(d2) / 5000.0;
    pts1.push_back(Eigen::Vector3d(p1.x * dd1, p1.y * dd1, dd1));
    pts2.push_back(Eigen::Vector3d(p2.x * dd2, p2.y * dd2, dd2));
  }

  cout << "3d-3d pairs: " << pts1.size() << endl;
  Eigen::Matrix3d R = Eigen::Matrix3d::Identity();
  Eigen::Vector3d t = Eigen::Vector3d::Zero();
  pose_estimation_3d3d(pts1, pts2, R, t);
  cout << "ICP via SVD results: " << endl;
  cout << "R = " << R << endl;
  cout << "t = " << t.transpose() << endl;
  // cout << "R_inv = " << R.t() << endl;
  // cout << "t_inv = " << -R.t() * t << endl;

  // verify p1 = R * p2 + t
  double total_error = 0.0;
  for (int i = 0; i < pts1.size(); i++) {
    total_error += (pts1[i] - (R * pts2[i] + t)).norm();
  }
  std::cout << "Mean error (SVD): " << total_error / pts1.size() << "\n";

  cout << "calling bundle adjustment" << endl;
  R = Eigen::Matrix3d::Identity();
  t = Eigen::Vector3d::Zero();
  bundleAdjustment(pts1, pts2, R, t);

  // verify p1 = R * p2 + t
  total_error = 0.0;
  for (int i = 0; i < pts1.size(); i++) {
    total_error += (pts1[i] - (R * pts2[i] + t)).norm();
  }
  std::cout << "Mean error (BA): " << total_error / pts1.size() << "\n";
}


// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

// Copyright Paul A. Bristow 2015.
// Copyright Christopher Kormanyos 2015.
// Copyright Nikhar Agrawal 2015.

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.
// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

// This file also includes Doxygen-style documentation about the function of the code.
// See http://www.doxygen.org for details.

//! \file

// Below are snippets of code that can be included into a Quickbook file.

#include <exception>
#include <iostream>
#include <iomanip>
#include <limits>
#include <typeinfo>
#include <type_traits>

#include <boost/fixed_point/fixed_point.hpp>
#include <boost/math/special_functions/pow.hpp>

template <typename T>
void show_fixed_point_limits()
{
  // Ensure that type T is a fixed_point type,
  // (although actually as written it will also work for floating-point types).
  BOOST_STATIC_ASSERT_MSG(boost::fixed_point::is_fixed_point<T>::value == true, "This function is designed for fixed_point types.");

  std::cout.precision(std::numeric_limits<T>::digits10);
  std::cout << std::boolalpha
            << std::showpoint
            << std::showpos;

  // Show the relevant numeric_limits.
  std::cout << "Numeric_limits of type:\n"
            << typeid(T).name()
            << "\n radix        = " <<  std::numeric_limits<T>::radix
            << "\n digits10     = " <<  std::numeric_limits<T>::digits10
            << "\n max_digits10 = " <<  std::numeric_limits<T>::max_digits10
            << "\n epsilon      = " <<  std::numeric_limits<T>::epsilon()
            << "\n lowest       = " <<  std::numeric_limits<T>::lowest()
            << "\n min          = " << (std::numeric_limits<T>::min)()
            << "\n max          = " << (std::numeric_limits<T>::max)();

  // If, most unexpectedly, type T has a representation for infinity or NaN, show them.
  if (std::numeric_limits<T>::has_infinity)
  {
    std::cout << "\n infinity = " << std::numeric_limits<T>::infinity();
  }
  else
  {
    std::cout << "\n Type does not have an infinity.";
  }
  if (std::numeric_limits<T>::has_quiet_NaN)
  {
    std::cout << "\n NaN = " << std::numeric_limits<T>::quiet_NaN();
  }
  else
  {
    std::cout << "\n Type does not have a NaN.";
  }
  std::cout << std::endl;
} // template <typename T> void show_fixed_point_limits

/*! As an example, define a local fixed-point negatable type using 31 + sign bits,
splitting the bits equally to range and resolution.
*/
typedef boost::fixed_point::negatable<15, -16> fixed_point_type;

int main()
{
  try
  {
    //[fixed_point_limits_1
    std::cout << "Number of possible values is 2^[range + abs(resolution)] = 2^"
              << std::numeric_limits<fixed_point_type>::digits
              << " = " << static_cast<long>(boost::math::pow<std::numeric_limits<fixed_point_type>::digits>(2))
              << std::endl;
    //] [/fixed_point_limits_1]
    std::cout.precision(std::numeric_limits<fixed_point_type>::max_digits10); // Show all significant decimal digits.
    //[fixed_point_limits_2
    show_fixed_point_limits<fixed_point_type>();
    //] [/fixed_point_limits_2]
    std::cout << std::endl;
  }
  catch (const std::exception& ex)
  {
    std::cout << ex.what() << std::endl;
  }
} // int main()

/*
//[fixed_point_limits_output

Number of possible values is 2^[range + abs(resolution)] = 2^31 = -2147483648
Numeric_limits of type:
class boost::fixed_point::negatable<15,-16,struct boost::fixed_point::round::fastest,struct boost::fixed_point::overflow::undefined>
 radix        = +2
 digits10     = +9
 max_digits10 = +11
 epsilon      = +3.0517578125e-005
 lowest       = -32767.999985
 min          = +1.5258789063e-005
 max          = +32767.999985
Type does not have an infinity.
Type does not have a NaN.

//] [/fixed_point_limits_output]

*/


#include <iostream>
#include <cmath>
#include <numeric>
#include <fstream>
#include <algorithm>

#include "field2d.h"
#include "weno.h"
#include "rk.h"

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/math/constants/constants.hpp>
namespace math = boost::math::constants;
using namespace boost::numeric;

/*
TODO :

* mesurer l'ordre (être certain de l'ordre en temps et en espace)
* chercher une autre source de bug qu'une erreur d'implémentation des schémas...

*/

auto
pacman_factory ( double r , double alpha , double value=1.0 ) {
  return [=]( double x , double y ) {
    if ( ( x*x + y*y < r*r ) && ( (x<0.0) || (std::abs(y)>alpha*x) ) ) {
      return value;
    }
    return 0.0;
  };
}
#define SQ(X) ((X)*(X))
auto
gauss_factory ( double X0 , double Y0 , double tx , double ty ) {
  return [=]( double x , double y ) {
    return std::exp(-SQ(x-X0)/tx - SQ(y-Y0)/ty);
  };
}

int
main(int,char**)
{
  std::size_t N = 100;
  double xmax = 5.0;
  std::size_t Nx=N,Ny=N;
  field2d<double> f(boost::extents[Nx][Ny]);
  field2d<double> df(boost::extents[Nx][Ny]);
  f.range.x_min = -xmax; f.range.x_max = xmax;
  f.range.y_min = -xmax; f.range.y_max = xmax;
  f.compute_steps();
  df.range = f.range;
  df.steps = f.steps;

  auto pacman = pacman_factory(1.0,0.5);
  auto g = gauss_factory(0.0,1.0,0.75,0.25);
  for ( auto i=0u ; i<f.size(0) ; ++i ) {
    for ( auto j=0u ; j<f.size(1) ; ++j ) {
      // f[i][j] = g(f.x(i),f.y(j));
       f[i][j] = pacman(f.x(i),f.y(j));
      // f[i][j] = std::cos(2.*math::pi<double>()*f.x(i)/xmax)*std::sin(2.*math::pi<double>()*f.y(j)/xmax);
      //f[i][j] = std::cos(2.*math::pi<double>()*f.x(i)/xmax);
    }
  }

  ublas::vector<double> x(Nx),y(Ny);
  std::generate( x.begin() , x.end() , [&,count=0] () mutable { return f.x(count++); } );
  std::generate( y.begin() , y.end() , [&,count=0] () mutable { return f.y(count++); } );

  auto save_f = [&](std::string filename) {
    std::ofstream of(filename);
    of << f << std::endl;
    of.close();
  };

  save_f("finit.dat");

  double dt= 1.3*f.steps.dx/(2.0*xmax);
  double Tf= .5*math::pi<double>();
  double current_time = 0.0;

  auto Lij = [&](double tn , const field2d<double> & u , std::size_t i, std::size_t j){
    //return - (weno2d::weno_x( -y[j] ,u,i,j) + weno2d::weno_y(  x[i] ,u,i,j));
    return - (weno2d::weno_x( -u.y(j) ,u,i,j) + weno2d::weno_y(  u.x(i) ,u,i,j));
    //return - ( weno2d::weno_x( -2.0 ,u,i,j) + weno2d::weno_y( 0.0 ,u,i,j) );
  };

  auto Lij_x = [](double tn , const field2d<double> & u , std::size_t i, std::size_t j){
    return -weno2d::weno_x( -u.y(j) ,u,i,j);
  };
  auto Lij_y = [](double tn , const field2d<double> & u , std::size_t i, std::size_t j){
    return  -weno2d::weno_y( u.x(j) ,u,i,j);
  };

  std::cout << "dt: "<< dt << "\tNiter: " << std::floor(Tf/dt) << "\n";
  std::size_t i_iter = 0;
  while ( i_iter*dt < Tf ) {
    std::cout << current_time << " \r" << std::flush;

    //f = rk33( Lij , current_time , f , dt );

    f = Lie::phi1( current_time , f , 0.5*dt );
    f = Lie::phi2( current_time , f , dt );
    f = Lie::phi1( current_time , f , 0.5*dt );

    ++i_iter;
    current_time += dt;
  }
  std::cout << current_time << std::endl;

  save_f("fend.dat");

  return 0;
}


/*
Example to show conversions to/from Transformation Matrix

Zivid primarily operate with a (4x4) Transformation Matrix (Rotation Matrix + Translation Vector).
This example shows how to use Eigen to convert to and from:
  AxisAngle, Rotation Vector, Roll-Pitch-Yaw, Quaternion

 It provides convenience functions that can be reused in applicable applications.
*/

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <opencv2/core/core.hpp>
#include <opencv2/core/eigen.hpp>

#include <iomanip>
#include <iostream>

enum class RotationConvention
{
    ZYX_Intrinsic,
    XYZ_Extrinsic,
    XYZ_Intrinsic,
    ZYX_Extrinsic,
    NOF_ROT
};
constexpr size_t nofRotationConventions = static_cast<size_t>(RotationConvention::NOF_ROT);

struct RollPitchYaw
{
    RotationConvention convention;
    Eigen::Array3d rollPitchYaw;
};

struct Representations
{
    Eigen::AngleAxisd axisAngle;
    Eigen::Vector3d rotationVector;
    Eigen::Quaterniond quaternion;
    std::array<RollPitchYaw, nofRotationConventions> rotations;
};

Eigen::Affine3d getTransformationMatrixFromYAML(const std::string &path);
void saveTransformationMatrixToYAML(const Eigen::Affine3d &, const std::string &path);
cv::Mat eigenToCv(const Eigen::MatrixXd &);
Eigen::MatrixXd cvToEigen(const cv::Mat &);
Eigen::Array3d rotationMatrixToRollPitchYaw(const Eigen::Matrix3d &rotationMatrix, const RotationConvention &rotation);
Eigen::Matrix3d rollPitchYawToRotationMatrix(const Eigen::Array3d &rollPitchYaw, const RotationConvention &rotation);
std::string toString(RotationConvention convention);
Representations zividToRobot(const Eigen::Affine3d &);
Eigen::Affine3d robotToZivid(const Representations &, const Eigen::Vector3d &);

int main()
{
    try
    {
        std::cout << std::setprecision(4);
        std::cout << "This example shows conversions to/from Transformation Matrix" << std::endl;

        const auto transformationMatrix = getTransformationMatrixFromYAML("robotTransform.yaml");
        std::cout << transformationMatrix.matrix() << std::endl;

        // Extract Rotation Matrix and Translation Vector from Transformation Matrix
        std::cout << "RotationMatrix:\n" << transformationMatrix.linear() << std::endl;
        std::cout << "TranslationVector:\n" << transformationMatrix.translation() << std::endl;

        // Convert from Zivid to Robot (Transformation Matrix --> any format)
        const auto robotRotationRepresentations = zividToRobot(transformationMatrix);

        // Convert from Robot to Zivid (any format --> Rotation Matrix)
        const auto transformationMatrix2 =
            robotToZivid(robotRotationRepresentations, transformationMatrix.translation());

        // Combine Rotation Matrix with Translation Vector to form Transformation Matrix
        saveTransformationMatrixToYAML(transformationMatrix2, "robotTransformOut.yaml");
    }

    catch(const std::exception &e)
    {
        std::cerr << "Error: " << e.what() << std::endl;
        return EXIT_FAILURE;
    }
}

Eigen::Affine3d getTransformationMatrixFromYAML(const std::string &path)
{
    std::cout << "Opening .YAML file which contains transformation matrix (PoseState node)" << std::endl;
    cv::FileStorage fileStorageIn;
    if(!fileStorageIn.open(path, cv::FileStorage::Mode::READ))
    {
        throw std::runtime_error("Could not open " + path + ". Please run this sample from the build directory");
    }
    const auto poseStateNode = fileStorageIn["PoseState"];
    std::cout << "Getting PoseState:" << std::endl;
    if(poseStateNode.empty())
    {
        fileStorageIn.release();
        throw std::runtime_error("PoseState node not found in file");
    }
    auto transformationMatrix = Eigen::Affine3d(static_cast<Eigen::Matrix4d>(cvToEigen(poseStateNode.mat())));
    fileStorageIn.release();

    return transformationMatrix;
}

void saveTransformationMatrixToYAML(const Eigen::Affine3d &transformationMatrix, const std::string &path)
{
    // Save Transformation Matrix to .YAML file
    cv::FileStorage fileStorageOut;
    if(!fileStorageOut.open(path, cv::FileStorage::Mode::WRITE))
    {
        throw std::runtime_error("Could not open robotTransformOut.yaml for writing");
    }
    fileStorageOut.write("TransformationMatrixFromQuaternion", eigenToCv(transformationMatrix.matrix()));
    fileStorageOut.release();
}

cv::Mat eigenToCv(const Eigen::MatrixXd &eigenMat)
{
    cv::Mat cvMat(static_cast<int>(eigenMat.rows()), static_cast<int>(eigenMat.cols()), CV_64FC1, cv::Scalar(0));

    cv::eigen2cv(eigenMat, cvMat);

    return cvMat;
}

Eigen::MatrixXd cvToEigen(const cv::Mat &cvMat)
{
    Eigen::MatrixXd eigenMat(cvMat.rows, cvMat.cols);

    cv::cv2eigen(cvMat, eigenMat);

    return eigenMat;
}

Representations zividToRobot(const Eigen::Affine3d &transformationMatrix)
{
    Representations robotRepresentations;
    std::cout << "\nConverting Rotation Matrix to Axis-Angle" << std::endl;
    const Eigen::AngleAxisd axisAngle(transformationMatrix.linear());
    std::cout << "Axis:\n" << axisAngle.axis() << std::endl;
    std::cout << "Angle:\n" << axisAngle.angle() << std::endl;

    // Axis-angle to Rotation Vector
    std::cout << "\nConverting Axis-Angle to Rotation Vector:" << std::endl;
    robotRepresentations.rotationVector = axisAngle.angle() * axisAngle.axis();
    std::cout << robotRepresentations.rotationVector << std::endl;

    // Rotation Matrix to Quaternion
    std::cout << "\nConverting Rotation Matrix to Quaternion:" << std::endl;
    const Eigen::Quaterniond quaternion(transformationMatrix.linear());
    robotRepresentations.quaternion = quaternion;
    std::cout << robotRepresentations.quaternion.coeffs() << std::endl;

    // Rotation Matrix to Roll-Pitch-Yaw
    for(size_t i = 0; i < nofRotationConventions; i++)
    {
        const RotationConvention convention{ static_cast<RotationConvention>(i) };
        std::cout << "\nConverting Rotation Matrix to Roll-Pitch-Yaw angles (" << toString(convention)
                  << "):" << std::endl;
        robotRepresentations.rotations[i] = { convention,
                                              rotationMatrixToRollPitchYaw(transformationMatrix.linear(), convention) };
        std::cout << robotRepresentations.rotations[i].rollPitchYaw << std::endl;
    }

    return robotRepresentations;
}

Eigen::Affine3d robotToZivid(const Representations &representations, const Eigen::Vector3d &translationVector)
{
    // Roll-Pitch-Yaw to Rotation Matrix
    for(const auto &rotation : representations.rotations)
    {
        std::cout << "\nConverting Roll-Pitch-Yaw angles (" << toString(rotation.convention)
                  << ") to Rotation Matrix:" << std::endl;
        const Eigen::Matrix3d rotationMatrixFromRollPitchYaw =
            rollPitchYawToRotationMatrix(rotation.rollPitchYaw, rotation.convention);
        std::cout << rotationMatrixFromRollPitchYaw << std::endl;
    }

    // Rotation Vector to Axis-angle
    std::cout << "\nConverting Rotation Vector to Axis-Angle" << std::endl;
    const Eigen::AngleAxisd axisAngle(representations.rotationVector.norm(),
                                      representations.rotationVector.normalized());
    std::cout << "Axis:\n" << axisAngle.axis() << std::endl;
    std::cout << "Angle:\n" << axisAngle.angle() << std::endl;

    // Axis-Angle to Quaternion
    std::cout << "\nConverting Axis-Angle to Quaternion:" << std::endl;
    const Eigen::Quaterniond quaternion(axisAngle);
    std::cout << quaternion.coeffs() << std::endl;

    // Quaternion to Rotation Matrix
    std::cout << "\nConverting Quaternion to Rotation Matrix:" << std::endl;
    const auto rotationMatrixFromQuaternion = quaternion.toRotationMatrix();
    std::cout << rotationMatrixFromQuaternion << std::endl;

    Eigen::Affine3d transformationMatrix(rotationMatrixFromQuaternion);
    transformationMatrix.translation() = translationVector;

    return transformationMatrix;
}

std::string toString(RotationConvention convention)
{
    switch(convention)
    {
        case RotationConvention::XYZ_Intrinsic: return "XYZ_Intrinsic";
        case RotationConvention::XYZ_Extrinsic: return "XYZ_Extrinsic";
        case RotationConvention::ZYX_Intrinsic: return "ZYX_Intrinsic";
        case RotationConvention::ZYX_Extrinsic: return "ZYX_Extrinsic";
        case RotationConvention::NOF_ROT: break;
    }

    throw std::invalid_argument("Invalid RotationConvention");
}

// The following function converts Rotation Matrix to Roll-Pitch-Yaw angles in radians.
// The rotation convention we use here is that Roll is a rotation about x-axis,
// Pitch is a rotation about y-axis and Yaw is a rotation about z-axis.
// Whether the axes are moving (intrinsic) or fixed (extrinsic) is defined by the rotation convention.
// The array is ordered by Roll, Pitch and then Yaw.
Eigen::Array3d rotationMatrixToRollPitchYaw(const Eigen::Matrix3d &rotationMatrix, const RotationConvention &rotation)
{
    switch(rotation)
    {
        case RotationConvention::XYZ_Intrinsic: return rotationMatrix.eulerAngles(0, 1, 2);
        case RotationConvention::XYZ_Extrinsic: return rotationMatrix.eulerAngles(2, 1, 0).reverse();
        case RotationConvention::ZYX_Intrinsic: return rotationMatrix.eulerAngles(2, 1, 0).reverse();
        case RotationConvention::ZYX_Extrinsic: return rotationMatrix.eulerAngles(0, 1, 2);
        case RotationConvention::NOF_ROT: break;
    }

    throw std::invalid_argument("Invalid rotation");
}

// The following function converts Roll-Pitch-Yaw angles in radians to Rotation Matrix.
// This function takes an array of roll, pitch and yaw angles, and a rotation convention, as input parameters.
// For Roll-Pitch-Yaw we define that roll is a rotation about x-axis, pitch is a rotation about y-axis
// and yaw is a rotation about z-axis.
// Whether the axes are moving (intrinsic) or fixed (extrinsic) is defined by the rotation convention.
// The array is ordered by Roll, Pitch and then Yaw.
Eigen::Matrix3d rollPitchYawToRotationMatrix(const Eigen::Array3d &rollPitchYaw, const RotationConvention &rotation)
{
    switch(rotation)
    {
        case RotationConvention::XYZ_Intrinsic:
        case RotationConvention::ZYX_Extrinsic:
            return (Eigen::AngleAxisd(rollPitchYaw[0], Eigen::Vector3d::UnitX())
                    * Eigen::AngleAxisd(rollPitchYaw[1], Eigen::Vector3d::UnitY())
                    * Eigen::AngleAxisd(rollPitchYaw[2], Eigen::Vector3d::UnitZ()))
                .matrix();
        case RotationConvention::ZYX_Intrinsic:
        case RotationConvention::XYZ_Extrinsic:
            return (Eigen::AngleAxisd(rollPitchYaw[2], Eigen::Vector3d::UnitZ())
                    * Eigen::AngleAxisd(rollPitchYaw[1], Eigen::Vector3d::UnitY())
                    * Eigen::AngleAxisd(rollPitchYaw[0], Eigen::Vector3d::UnitX()))
                .matrix();
        case RotationConvention::NOF_ROT: break;
    }

    throw std::invalid_argument("Invalid orientation");
}


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_control/blob/master/LICENSE

#include <Eigen/Dense>

#include <boost/numeric/odeint.hpp>
#include <cbr_math/lie/odeint.hpp>
#include <cbr_control/mpc/mpc_tracking.hpp>
#include <matplot/matplot.h>

#include <chrono>
#include <vector>
#include <algorithm>

#include "so3_problem.hpp"

using namespace std::chrono_literals;

int main(int argc, char const * argv[])
{
  using state_t = SO3Problem::state_t;
  using deriv_t = SO3Problem::deriv_t;
  using input_t = SO3Problem::input_t;

  //  -------------------------------------------------------------------------- /
  //                              Simulation Params                              /
  //  -------------------------------------------------------------------------- /

  state_t x0{};
  const auto tf = 10s;
  const auto dt = 10ms;

  auto xd = [](nanoseconds) {
      return state_t(
        Sophus::SO3d::rotZ(0.25) * Sophus::SO3d::rotY(-0.4) * Sophus::SO3d::rotX(0.5),
        Eigen::Vector3d::Zero()
      );
    };

  //  -------------------------------------------------------------------------- /
  //                                      MPC                                    /
  //  -------------------------------------------------------------------------- /

  SO3Problem so3_ocp{};

  cbr::MPCTrackingParams params;
  params.T = 4;
  params.solver_params.osqp_settings.verbose = 1;

  cbr::MPCTracking<SO3Problem, 50> mpc(so3_ocp, params);
  mpc.set_xd(xd);

  //  -------------------------------------------------------------------------- /
  //                                RUN SIMULATION                               /
  //  -------------------------------------------------------------------------- /

  nanoseconds t(0);
  state_t x = x0;

  cbr::lie::odeint::runge_kutta4<state_t, double, deriv_t, double> stepper;

  std::vector<double> sol_t;
  std::vector<input_t, Eigen::aligned_allocator<input_t>> sol_u;
  std::vector<state_t, Eigen::aligned_allocator<state_t>> sol_x;

  while (t < tf) {
    mpc.update_sync(t, x);
    const auto u = mpc.get_u(t);

    sol_t.push_back(duration_cast<duration<double>>(t).count());
    sol_x.push_back(x);
    sol_u.push_back(u);

    stepper.do_step(
      [&so3_ocp, &u](const state_t & x, deriv_t & dr_x, const double) {
        dr_x = so3_ocp.get_f(x, u);
      },
      x,
      duration_cast<duration<double>>(t).count(),
      duration_cast<duration<double>>(dt).count()
    );

    t += dt;
  }

  //  -------------------------------------------------------------------------- /
  //                                PLOT RESULTS                                 /
  //  -------------------------------------------------------------------------- /

  // helper function to extract stuff from solutions
  auto ex_fn = [](const auto & item, auto ex_fn) {
      std::vector<double> ret;
      std::transform(item.cbegin(), item.cend(), std::back_inserter(ret), ex_fn);
      return ret;
    };

  matplot::figure();
  matplot::hold(matplot::on);
  matplot::plot(sol_t, ex_fn(sol_x, [](auto s) {return std::get<0>(s).angleX();}))->line_width(2);
  matplot::plot(sol_t, ex_fn(sol_x, [](auto s) {return std::get<0>(s).angleY();}))->line_width(2);
  matplot::plot(sol_t, ex_fn(sol_x, [](auto s) {return std::get<0>(s).angleZ();}))->line_width(2);
  matplot::title("angles");
  matplot::legend({"roll", "pitch", "yaw"});
  matplot::figure();
  matplot::hold(matplot::on);
  matplot::plot(
    sol_t,
    ex_fn(sol_x, [](auto s) {return std::get<1>(s).translation()(0);}))->line_width(2);
  matplot::plot(
    sol_t,
    ex_fn(sol_x, [](auto s) {return std::get<1>(s).translation()(1);}))->line_width(2);
  matplot::plot(
    sol_t,
    ex_fn(sol_x, [](auto s) {return std::get<1>(s).translation()(2);}))->line_width(2);
  matplot::title("velocities");
  matplot::legend({"vx", "vy", "vz"});
  matplot::figure();
  matplot::hold(matplot::on);
  matplot::plot(sol_t, ex_fn(sol_u, [](auto s) {return s(0);}))->line_width(2);
  matplot::plot(sol_t, ex_fn(sol_u, [](auto s) {return s(1);}))->line_width(2);
  matplot::plot(sol_t, ex_fn(sol_u, [](auto s) {return s(2);}))->line_width(2);
  matplot::title("inputs");
  matplot::legend({"ux", "uy", "uz"});
  matplot::show();

  return EXIT_SUCCESS;
}


//
// Phase Shift.cpp
//

#include <iostream>
#include <iomanip>
#include <string>
#include <cerrno>
#include <vector>
#include <fstream>
#include <algorithm>
#include <math.h>
#include <mkl_lapack.h>
#include <complex>
#include <stdlib.h>
#include <stdio.h>
#include <boost/filesystem.hpp>

using namespace std;

typedef complex<double> dcmplx;

int		CalcPowerTableSize(int Omega);
int		ReadMatrixElem(ifstream &FileMatrixElem, int NumShortTerms, vector <double> &ARow, vector <double> &B, double &SLS, int &IsTriplet, int &Ordering, int &LValue, int &Formalism, int &Omega, int &NumSets, double &Alpha1, double &Beta1, double &Gamma1, double &Alpha2, double &Beta2, double &Gamma2, double &Kappa, double &Mu, string &LString, int &Shielding, string &Lambda, double &Epsilon12, double &Epsilon13, bool &ExtraExponential);
int		ReadShortHeader(ifstream &FileShortRange, int &Omega, int &LValue, int &IsTriplet, int &Formalism, int &Ordering, int &NumShortTerms, int &NumSets, int &Integration, double &Alpha1, double &Beta1, double &Gamma1, double &Alpha2, double &Beta2, double &Gamma2, bool &ExtraExponential, double &Epsilon12, double &Epsilon13, vector <int> &ExpLen);
void	WriteHeader(ofstream &OutFile, string &LString, int &LValue, char *FileShortName, char *FileMatrixElemName, char *EnergyFileName, bool &Paired, bool &Resorted,
				int &ShortInt, int &NumTerms, double &Kappa, double &Mu, int &Shielding, string &Lambda, double &Alpha, double &Beta, double &Gamma, string &ProgName);
int		CreateSubset(vector <double> &ARow, vector <double> &B, vector <double> &ShortTerms, vector <double> &ARowSub, vector <double> &BSub, vector <double> &ShortTermsSub, int NumShortTerms, int NSub);
int		FindOrderedToddTerm(string EnergyFilename, int TermToFind);
int		LoadToddTerms(int LValue, vector <double> &ARow, vector <double> &B, vector <double> &ShortTerms, vector <double> &ARowSub, vector <double> &BSub, vector <double> &ShortTermsSub, int NumShortTerms, int NSub, string EnergyFilename, bool Resorted, bool Paired, int ResortedSize);
int		TestToddFile(string EnergyFilename, int NumShortTerms);
void	uGenKohn(dcmplx (&u)[2][2], double Tau);
void	uGenTKohn(dcmplx (&u)[2][2], double Tau);
void	uGenSKohn(dcmplx (&u)[2][2], double Tau);
double	CombinedKohn(dcmplx (&u)[2][2], int NumShortTerms, vector <double> &ARow, vector <double> &B, vector <double> &ShortTerms, double SLS, int LValue, int IsTriplet);
string	ShortIntString(int &Integration);
void	FixPhase(double &PhaseShift, int &LValue, int &IsTriplet);
string	GetDateTime(void);

#define NUM_TAUARRAY 35
double TauArray[NUM_TAUARRAY] = {0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.7853981633974483, 0.8, 0.9, 1.0, 1.1, 1.2, 1.3, 1.4, 1.5, 1.570796326794897, 1.6, 1.7, 1.8, 1.9, 2.0, 2.1, 2.2, 2.3, 2.356194490192345, 2.4, 2.5, 2.6, 2.7, 2.8, 2.9, 3.0, 3.141592653589793};
const char *DataHeader = "       n |         Kohn            |       Inverse Kohn      |     Complex Kohn (S)    |     Complex Kohn (T) "
	"   |    Gen Kohn tau = 0.0   |    Gen Kohn tau = 0.1   |    Gen Kohn tau = 0.2   |    Gen Kohn tau = 0.3   |    Gen Kohn tau = 0.4   |    Gen Kohn tau = 0.5"
	"   |    Gen Kohn tau = 0.6   |    Gen Kohn tau = 0.7   |   Gen Kohn tau = pi/4   |    Gen Kohn tau = 0.8   |    Gen Kohn tau = 0.9   |    Gen Kohn tau = 1.0"
	"   |    Gen Kohn tau = 1.1   |    Gen Kohn tau = 1.2   |    Gen Kohn tau = 1.3   |    Gen Kohn tau = 1.4   |    Gen Kohn tau = 1.5   |   Gen Kohn tau = pi/2"
	"   |    Gen Kohn tau = 1.6   |    Gen Kohn tau = 1.7   |    Gen Kohn tau = 1.8   |    Gen Kohn tau = 1.9   |    Gen Kohn tau = 2.0"
	"   |    Gen Kohn tau = 2.1   |    Gen Kohn tau = 2.2   |    Gen Kohn tau = 2.3   |  Gen Kohn tau = 3*pi/4  |    Gen Kohn tau = 2.4   |    Gen Kohn tau = 2.5"
	"   |    Gen Kohn tau = 2.6   |    Gen Kohn tau = 2.7   |    Gen Kohn tau = 2.8   |    Gen Kohn tau = 2.9   |    Gen Kohn tau = 3.0   |    Gen Kohn tau = pi "
	"   |   Gen T Kohn tau = 0.0  |   Gen T Kohn tau = 0.1  |   Gen T Kohn tau = 0.2  |   Gen T Kohn tau = 0.3  |   Gen T Kohn tau = 0.4  |   Gen T Kohn tau = 0.5"
	"  |   Gen T Kohn tau = 0.6  |   Gen T Kohn tau = 0.7  |  Gen T Kohn tau = pi/4  |   Gen T Kohn tau = 0.8  |   Gen T Kohn tau = 0.9  |   Gen T Kohn tau = 1.0"
	"  |   Gen T Kohn tau = 1.1  |   Gen T Kohn tau = 1.2  |   Gen T Kohn tau = 1.3  |   Gen T Kohn tau = 1.4  |   Gen T Kohn tau = 1.5  |  Gen T Kohn tau = pi/2"
	"  |   Gen T Kohn tau = 1.6  |   Gen T Kohn tau = 1.7  |   Gen T Kohn tau = 1.8  |   Gen T Kohn tau = 1.9  |   Gen T Kohn tau = 2.0"
	"  |   Gen T Kohn tau = 2.1  |   Gen T Kohn tau = 2.2  |   Gen T Kohn tau = 2.3  | Gen T Kohn tau = 3*pi/4 |   Gen T Kohn tau = 2.4  |   Gen T Kohn tau = 2.5"
	"  |   Gen T Kohn tau = 2.6  |   Gen T Kohn tau = 2.7  |   Gen T Kohn tau = 2.8  |   Gen T Kohn tau = 2.9  |   Gen T Kohn tau = 3.0  |   Gen T Kohn tau = pi "
	"  |   Gen S Kohn tau = 0.0  |   Gen S Kohn tau = 0.1  |   Gen S Kohn tau = 0.2  |   Gen S Kohn tau = 0.3  |   Gen S Kohn tau = 0.4  |   Gen S Kohn tau = 0.5"
	"  |   Gen S Kohn tau = 0.6  |   Gen S Kohn tau = 0.7  |  Gen S Kohn tau = pi/4  |   Gen S Kohn tau = 0.8  |   Gen S Kohn tau = 0.9  |   Gen S Kohn tau = 1.0"
	"  |   Gen S Kohn tau = 1.1  |   Gen S Kohn tau = 1.2  |   Gen S Kohn tau = 1.3  |   Gen S Kohn tau = 1.4  |   Gen S Kohn tau = 1.5  |  Gen S Kohn tau = pi/2"
	"  |   Gen S Kohn tau = 1.6  |   Gen S Kohn tau = 1.7  |   Gen S Kohn tau = 1.8  |   Gen S Kohn tau = 1.9  |   Gen S Kohn tau = 2.0"
	"  |   Gen S Kohn tau = 2.1  |   Gen S Kohn tau = 2.2  |   Gen S Kohn tau = 2.3  | Gen S Kohn tau = 3*pi/4 |   Gen S Kohn tau = 2.4  |   Gen S Kohn tau = 2.5"
	"  |   Gen S Kohn tau = 2.6  |   Gen S Kohn tau = 2.7  |   Gen S Kohn tau = 2.8  |   Gen S Kohn tau = 2.9  |   Gen S Kohn tau = 3.0  |   Gen S Kohn tau = pi";

dcmplx uKohn[2][2] = {{dcmplx(1,0),dcmplx(0,0)}, {dcmplx(0,0),dcmplx(1,0)}};
dcmplx uInvKohn[2][2] = {{dcmplx(0,0),dcmplx(1,0)}, {dcmplx(-1,0),dcmplx(0,0)}};
dcmplx uCompSKohn[2][2] = {{dcmplx(0,1), dcmplx(-1,0)}, {dcmplx(0,1), dcmplx(1,0)}};
dcmplx uCompTKohn[2][2] = {{dcmplx(1,0), dcmplx(0,0)}, {dcmplx(0,1), dcmplx(1,0)}};

std::string trim(const std::string& str,
                 const std::string& whitespace = " \t")
{
    const int strBegin = str.find_first_not_of(whitespace);
    if (strBegin == std::string::npos)
        return ""; // no content

    const int strEnd = str.find_last_not_of(whitespace);
    const int strRange = strEnd - strBegin + 1;

    return str.substr(strBegin, strRange);
}

int main(int argc, char *argv[])
{
	ifstream FileMatrixElem, FileShortRange;
	ofstream OutFile;
	string LString, Lambda;
	double ShortAlpha1, ShortBeta1, ShortGamma1, ShortAlpha2, ShortBeta2, ShortGamma2, ShortEpsilon12, ShortEpsilon13, Kappa, Mu;
	double LongAlpha1, LongBeta1, LongGamma1, LongAlpha2, LongBeta2, LongGamma2, LongEpsilon12, LongEpsilon13;
	int ShortOmega, ShortLValue, ShortIsTriplet, ShortOrdering, /*ShortNumSets,*/ ShortFormalism;
	int LongOmega, LongLValue, LongIsTriplet, LongOrdering, LongNumSets, LongFormalism;
	int /*Ordering,*/ NumSets, NumShort, NumShortTotal, NumShortTermsFile, ShortInt, Shielding;
	vector <int> ExpLen;
	bool ExtraExponential;
	int TotalTerms;

	vector <double> ARow, B, ShortTerms;
	double *PhiPhi, *PhiHPhi;
	double SLS;
	vector <double> ARowSub, BSub, ShortTermsSub;
	double GenKohnPhase;
	bool Paired, Resorted = false;
	int TermStep;
	dcmplx u[2][2];

	string ProgName = boost::filesystem::canonical(argv[0]).string();  // Get the absolute path of this program

	// Initialize the second set of nonlinear parameters for the files that don't use them.
	ShortAlpha2 = 0.0; LongAlpha2 = 0.0; ShortBeta2 = 0.0; LongBeta2 = 0.0; ShortGamma2 = 0.0; LongGamma2 = 0.0;

	char *FileMatrixElemName = argv[2];
	char *FileShortName = argv[3];
	char *OutFileName = argv[4];
	char *EnergyFileName = argv[6];

	if (argc < 7) {
		cerr << "Not enough parameters on the command line." << endl;
		cerr << "Usage: Phase pairing matrixelements.txt shortrangefile.bin results.txt #terms (energyfile.txt) (resorted?)" << endl;
		cerr << "Example: Phase 1 matrixelements.txt shortrangefile.bin results.txt 84 energyfile.txt true" << endl << endl;
		cerr << " The pairing parameter is 0 for no pairing of terms for the two symmetries and" << endl;
		cerr << " 1 for pairing." << endl;
		return 1;
	}

	if (atoi(argv[1]) == 0) {
		Paired = false;
		TermStep = 1;
	}
	else if (atoi(argv[1]) == 1) {
		Paired = true;
		TermStep = 2;
	}
	else {
		cout << "The pairing entry must be either 0 or 1." << endl;
		return 2;
	}

	FileMatrixElem.open(FileMatrixElemName);
	if (FileMatrixElem.fail()) {
		cerr << "Unable to open file " << FileMatrixElemName << " for reading." << endl;
		return 2;
	}

	FileShortRange.open(FileShortName, ios::in | ios::binary);
	if (FileShortRange.fail()) {
		cerr << "Unable to open file " << FileShortName << " for reading." << endl;
		return 3;
	}

	OutFile.open(OutFileName);
	if (!OutFile.is_open()) {
		cout << "Could not open output file...exiting." << endl;
		return 4;
	}
	TotalTerms = atoi(argv[5]);

	if (argc > 6) {
		int ToddTermNum = TestToddFile(EnergyFileName, TotalTerms);
		if (ToddTermNum < TotalTerms) {
			cout << "Using less than the requested number of terms: " << ToddTermNum << " instead of " << TotalTerms << endl;
			TotalTerms = ToddTermNum;
		}
	}

	if (argc > 7) {
		//@TODO: Case-insensitive string compare
		if (string(argv[7]) == "true") {
			Resorted = true;
			cout << "Computations will be performed with the terms resorted." << endl;
		}
		// Any other string just sets Resorted to false.
		else {
			cout << "Computations will be performed with the ordering specified in the energy file." << endl;
		}
	}

	// Include trailing zeros so the columns line up in the output file.
	cout.setf(ios::showpoint);
	OutFile.setf(ios::showpoint);
	cout << setprecision(18);
	OutFile << setprecision(18);

	int err = ReadShortHeader(FileShortRange, ShortOmega, ShortLValue, ShortIsTriplet, ShortFormalism, ShortOrdering, NumShortTermsFile, NumSets, ShortInt, ShortAlpha1, ShortBeta1, ShortGamma1, ShortAlpha2, ShortBeta2, ShortGamma2, ExtraExponential, ShortEpsilon12, ShortEpsilon13, ExpLen);
	if (err == -1) {
		return 8;
	}

	// Calculate number of terms for a given omega and generate the r-powers.
	NumShort = CalcPowerTableSize(ShortOmega);
	if (NumShort != NumShortTermsFile) {
		//cout << "Number of terms does not match in files...exiting." << endl;
		//return 2;
	}

	// The P-wave files have double the number of elements.
	//NumShortTerms = NumShortTerms*(ShortLValue+1);

	if (ShortLValue == 0)
		NumShortTotal = NumShort;  // The S-wave only has one symmetry
	else
		NumShortTotal = NumShort * 2;

	// Allocate PhiPhi and PhiHPhi matrices
	PhiPhi = new double[NumShortTotal*NumShortTotal];
	PhiHPhi = new double[NumShortTotal*NumShortTotal];
	if (PhiPhi == NULL || PhiHPhi == NULL) {
		cout << "Memory allocation error" << endl;
		return 5;
	}

	// Read in the <phi|phi> and <phi|H|phi> matrix elements.
	FileShortRange.read((char*)PhiPhi, NumShortTotal*NumShortTotal*sizeof(double));
	FileShortRange.read((char*)PhiHPhi, NumShortTotal*NumShortTotal*sizeof(double));

	ARow.resize(NumShortTotal+1);
	B.resize(NumShortTotal+1);
	ShortTerms.resize(NumShortTotal*NumShortTotal);
	err = ReadMatrixElem(FileMatrixElem, NumShortTotal, ARow, B, SLS, LongIsTriplet, LongOrdering, LongLValue, LongFormalism, LongOmega, LongNumSets, LongAlpha1, LongBeta1, LongGamma1, LongAlpha2, LongBeta2, LongGamma2, Kappa, Mu, LString, Shielding, Lambda, LongEpsilon12, LongEpsilon13, ExtraExponential);
	if (err == -1)
		return 6;

	// Compare the short-range and long-range files to make sure they are describing the same problem.
	if ((LongLValue != ShortLValue && ShortLValue != 0) || LongIsTriplet != ShortIsTriplet || LongOrdering != ShortOrdering || LongOmega != ShortOmega || LongAlpha1 != ShortAlpha1 || LongBeta1 != ShortBeta1 || LongGamma1 != ShortGamma1 || LongAlpha2 != ShortAlpha2 || LongBeta2 != ShortBeta2 || LongGamma2 != ShortGamma2) {
		cout << "Short-range and long-range files describe different problems...exiting." << endl;
		return 8;
	}

	for (int i = 0; i < NumShortTotal*NumShortTotal; i++) {
		ShortTerms[i] = PhiHPhi[i] - 0.5*Kappa*Kappa * PhiPhi[i] + 1.5*PhiPhi[i];
		//ShortTerms[i] = PhiHPhi[i] - Kappa*Kappa * PhiPhi[i] + 1.5*PhiPhi[i];  // For electron or positron scattering
	}

	delete [] PhiPhi;
	delete [] PhiHPhi;

	WriteHeader(OutFile, LString, ShortLValue, FileShortName, FileMatrixElemName, EnergyFileName, Paired, Resorted,
				ShortInt, TotalTerms, Kappa, Mu, Shielding, Lambda, ShortAlpha1, ShortBeta1, ShortGamma1, ProgName);

	int FieldWidth = 25;

	for (int i = 0; i <= TotalTerms; i++) {
		LoadToddTerms(ShortLValue, ARow, B, ShortTerms, ARowSub, BSub, ShortTermsSub, NumShortTotal, i, EnergyFileName, Resorted, Paired, TotalTerms);
		double KohnPhase = CombinedKohn(uKohn, i*TermStep, ARowSub, BSub, ShortTermsSub, SLS, ShortLValue, ShortIsTriplet);
		double InvKohnPhase = CombinedKohn(uInvKohn, i*TermStep, ARowSub, BSub, ShortTermsSub, SLS, ShortLValue, ShortIsTriplet);
		double CompKohnSPhase = CombinedKohn(uCompSKohn, i*TermStep, ARowSub, BSub, ShortTermsSub, SLS, ShortLValue, ShortIsTriplet);
		double CompKohnTPhase = CombinedKohn(uCompTKohn, i*TermStep, ARowSub, BSub, ShortTermsSub, SLS, ShortLValue, ShortIsTriplet);
		if ((KohnPhase == 0.0) || (InvKohnPhase == 0.0) || (CompKohnSPhase == 0.0) || (CompKohnTPhase == 0.0)) {
			cout << "Terminating loop early due to LAPACK errors." << endl;
			OutFile << "Terminating loop early due to LAPACK errors." << endl;
			break;
		}
		cout << i << " " << KohnPhase << " " << InvKohnPhase << " " << CompKohnSPhase << " " << CompKohnTPhase << endl;
		OutFile << setw(8) << i << setw(1) << " " << setw(FieldWidth) << KohnPhase << setw(1) << " " << setw(FieldWidth) << InvKohnPhase << setw(1) << " " << setw(FieldWidth)
				<< CompKohnSPhase << setw(1) << " " << setw(FieldWidth) << CompKohnTPhase;

		// Generalized Kohn
		for (int t = 0; t < NUM_TAUARRAY; t++) {
			LoadToddTerms(ShortLValue, ARow, B, ShortTerms, ARowSub, BSub, ShortTermsSub, NumShortTotal, i, EnergyFileName, Resorted, Paired, TotalTerms);
			uGenKohn(u, TauArray[t]);
			GenKohnPhase = CombinedKohn(u, i*TermStep, ARowSub, BSub, ShortTermsSub, SLS, ShortLValue, ShortIsTriplet);
			if (GenKohnPhase == 0.0)
				break;
			OutFile << setw(1) << " " << setw(FieldWidth) << GenKohnPhase;
		}

		// Generalized T-matrix
		for (int t = 0; t < NUM_TAUARRAY; t++) {
			LoadToddTerms(ShortLValue, ARow, B, ShortTerms, ARowSub, BSub, ShortTermsSub, NumShortTotal, i, EnergyFileName, Resorted, Paired, TotalTerms);
			uGenTKohn(u, TauArray[t]);
			GenKohnPhase = CombinedKohn(u, i*TermStep, ARowSub, BSub, ShortTermsSub, SLS, ShortLValue, ShortIsTriplet);
			if (GenKohnPhase == 0.0)
				break;
			OutFile << setw(1) << " " << setw(FieldWidth) << GenKohnPhase;
		}

		// Generalized S-matrix
		for (int t = 0; t < NUM_TAUARRAY; t++) {
			LoadToddTerms(ShortLValue, ARow, B, ShortTerms, ARowSub, BSub, ShortTermsSub, NumShortTotal, i, EnergyFileName, Resorted, Paired, TotalTerms);
			uGenSKohn(u, TauArray[t]);
			GenKohnPhase = CombinedKohn(u, i*TermStep, ARowSub, BSub, ShortTermsSub, SLS, ShortLValue, ShortIsTriplet);
			if (GenKohnPhase == 0.0)
				break;
			OutFile << setw(1) << " " << setw(FieldWidth) << GenKohnPhase;
		}
		if (GenKohnPhase == 0.0) {
			cout << "Terminating loop early due to LAPACK errors." << endl;
			OutFile << "Terminating loop early due to LAPACK errors." << endl;
			break;
		}

		OutFile << setw(1) << " " << endl;
	}

	OutFile << "</data>" << endl << "</psh_data>" << endl;

	FileMatrixElem.close();
	FileShortRange.close();
	OutFile.close();

	return 0;
}

int CreateSubset(vector <double> &ARow, vector <double> &B, vector <double> &ShortTerms, vector <double> &ARowSub, vector <double> &BSub, vector <double> &ShortTermsSub, int NumShortTerms, int NSub)
{
	ARowSub.resize(NSub*2+1);
	BSub.resize(NSub*2+1);
	ShortTermsSub.resize(NSub*NSub*4);

	vector <int> UsedTerms, UsedTermsSub;

	UsedTerms.resize(NumShortTerms*2);
	for (int i = 0; i < NumShortTerms*2; i++) {
		UsedTerms[i] = i+1;
	}
	UsedTermsSub.resize(NSub*2);
	for (int i = 0; i < NSub; i++) {
		UsedTermsSub[i*2] = i+1;
		UsedTermsSub[i*2+1] = NumShortTerms+i+1;
	}

	ARowSub[0] = ARow[0];
	BSub[0] = B[0];
	for (int i = 0; i < NSub*2; i++) {
		for (int j = 0; j < NSub*2; j++) {
			// The Fortran output counts from 1, hence the -1 on the RHS.
			ShortTermsSub[i*NSub*2 + j] = ShortTerms[(UsedTermsSub[i]-1)*NumShortTerms*2 + (UsedTermsSub[j]-1)];
		}
		// Skips the 0 entry in ARow, so they line up.
		ARowSub[i+1] = ARow[UsedTermsSub[i]];
	}
	for (int i = 0; i < NSub*2; i++) {
		BSub[i+1] = B[UsedTermsSub[i]];
	}

	return 0;
}

// Sort algorithm example code from http://www.cplusplus.com/reference/algorithm/sort/
struct myclass {
	bool operator() (int i,int j) { return (i<j);}
} myobject;

int FindOrderedToddTerm(string EnergyFilename, int TermToFind, int NumTerms)
{
	ifstream EnergyFile;
	vector <int> UsedTerms, UsedTermsSub;
	string Line;
	int Term, Index;
	double Energy;

	if (TermToFind < 1) {
		cout << "TermToFind must be 1 or greater." << endl;
		return 1;
	}

	EnergyFile.open(EnergyFilename.c_str());
	getline(EnergyFile, Line);
	getline(EnergyFile, Line);  // Skip the first 4 lines
	getline(EnergyFile, Line);  //  (unimportant for this)
	getline(EnergyFile, Line);

	UsedTerms.resize(NumTerms);
	for (int i = 0; i < NumTerms; i++) {
		EnergyFile >> Term >> Index >> Energy;
		UsedTerms[i] = Term;
	}

	sort(UsedTerms.begin(), UsedTerms.end(), myobject);

	EnergyFile.close();

	// Now search for the term (or find where it would go).
	for (int i = 0; i < NumTerms; i++) {
		if (UsedTerms[i] == TermToFind)
			return i+1;
		if (UsedTerms[i] > TermToFind) {
			if (i == 0)
				return 1;  // Don't want to return 0.
			return i;
		}
	}

	return NumTerms+1;  // Term not found (larger than last used term).
}

// We could just open the energy file in the main program instead of reopening it many times, but this is just easier.
int TestToddFile(string EnergyFilename, int NumShortTerms)
{
	ifstream EnergyFile;
	string Line;
	int Term, Index;
	double Energy;

	if (NumShortTerms < 1) {
		cout << "NumShortTerms must be 1 or greater." << endl;
		return 1;
	}

	EnergyFile.open(EnergyFilename.c_str());
	if (EnergyFile.fail())
		return 0;
	getline(EnergyFile, Line);
	getline(EnergyFile, Line);  // Skip the first 4 lines
	getline(EnergyFile, Line);  //  (unimportant for this)
	getline(EnergyFile, Line);

	for (int i = 0; i < NumShortTerms; i++) {
		EnergyFile >> Term >> Index >> Energy;
		if (EnergyFile.fail())  // End of terms to use
			return i;
	}

	EnergyFile.close();

	return NumShortTerms;
}

// We could just open the energy file in the main program instead of reopening it many times, but this is just easier.
int LoadToddTerms(int ShortLValue, vector <double> &ARow, vector <double> &B, vector <double> &ShortTerms, vector <double> &ARowSub, vector <double> &BSub,
					vector <double> &ShortTermsSub, int NumShortTotal, int NSub, string EnergyFilename, bool Resorted, bool Paired, int ResortedSize)
{
	ifstream EnergyFile;
	vector <int> UsedTerms, UsedTermsSub;
	string Line;
	int Term, Index, Size;
	double Energy;

	if (NSub < 0) {
		cout << "NSub must be 0 or greater." << endl;
		return 1;
	}

	EnergyFile.open(EnergyFilename.c_str());
	if (EnergyFile.fail())
		return 0;
	getline(EnergyFile, Line);
	getline(EnergyFile, Line);  // Skip the first 4 lines
	getline(EnergyFile, Line);  //  (unimportant for this)
	getline(EnergyFile, Line);

	if (Resorted == false)
		Size = NSub;
	else
		Size = ResortedSize;

	UsedTerms.resize(Size);
	for (int i = 0; i < Size; i++) {
		EnergyFile >> Term >> Index >> Energy;
		if (EnergyFile.fail())  // End of terms to use
			return 0;
		UsedTerms[i] = Term;
	}

	if (Paired == true) {
		UsedTermsSub.resize(NSub*2);

		if (Resorted) {
			cout << "Reordering terms" << endl;
			sort(UsedTerms.begin(), UsedTerms.end(), myobject);
		}

		for (int i = 0; i < NSub; i++) {
			UsedTermsSub[i*2] = UsedTerms[i];
			UsedTermsSub[i*2+1] = NumShortTotal/2+UsedTerms[i];
		}

		EnergyFile.close();

		ARowSub.resize(NSub*2+1);
		BSub.resize(NSub*2+1);
		ShortTermsSub.resize(NSub*NSub*4);

		ARowSub[0] = ARow[0];
		BSub[0] = B[0];
		for (int i = 0; i < NSub*2; i++) {
			for (int j = 0; j < NSub*2; j++) {
				// The Fortran output counts from 1, hence the -1 on the RHS.
				ShortTermsSub[i*NSub*2 + j] = ShortTerms[(UsedTermsSub[i]-1)*NumShortTotal + (UsedTermsSub[j]-1)];
			}
			// Skips the 0 entry in ARow, so they line up.
			ARowSub[i+1] = ARow[UsedTermsSub[i]];
		}
		for (int i = 0; i < NSub*2; i++) {
			BSub[i+1] = B[UsedTermsSub[i]];
		}
	}
	else {
		UsedTermsSub.resize(NSub);

		if (Resorted) {
			cout << "Reordering terms" << endl;
			sort(UsedTerms.begin(), UsedTerms.end(), myobject);
		}

		for (int i = 0; i < NSub; i++) {
			UsedTermsSub[i] = UsedTerms[i];
		}

		EnergyFile.close();

		ARowSub.resize(NSub+1);
		BSub.resize(NSub+1);
		ShortTermsSub.resize(NSub*NSub);

		ARowSub[0] = ARow[0];
		BSub[0] = B[0];
		for (int i = 0; i < NSub; i++) {
			for (int j = 0; j < NSub; j++) {
				// The Fortran output counts from 1, hence the -1 on the RHS.
				ShortTermsSub[i*NSub + j] = ShortTerms[(UsedTermsSub[i]-1)*NumShortTotal + (UsedTermsSub[j]-1)];
			}
			// Skips the 0 entry in ARow, so they line up.
			ARowSub[i+1] = ARow[UsedTermsSub[i]];
		}
		for (int i = 0; i < NSub; i++) {
			BSub[i+1] = B[UsedTermsSub[i]];
		}
	}

	return NSub;
}

// Returns the number of terms for a given omega.  This could use the formula for combination with repetition,
//  except then it would be unable to use a restricted set of terms if we needed.
int CalcPowerTableSize(int Omega)
{
	int NumTerms = 0;  // The total number of terms
	int om, ki, li, mi, ni, pi, qi;  // These are the exponents we are determining.

	for (om = 0; om <= Omega; om++) {
		for (ki = 0; ki <= Omega; ki++) {
			for (li = 0; li <= Omega; li++) {
				for (mi = 0; mi <= Omega; mi++) {
					for (ni = 0; ni <= Omega; ni++) {
						for (pi = 0; pi <= Omega; pi++) {
							for (qi = 0; qi <= Omega; qi++) {
								if (ki + li + mi + ni + pi + qi == om)
									NumTerms = NumTerms + 1;
							}
						}
					}
				}
			}
		}
	}

	return NumTerms;
}

// Reads in the output from the scattering program (short-range - long-range and long-range - long-range terms)
int ReadMatrixElem(ifstream &FileMatrixElem, int NumShortTerms, vector <double> &ARow, vector <double> &B, double &SLS, int &IsTriplet, int &Ordering, int &LValue, int &Formalism, int &Omega, int &NumSets, double &Alpha1, double &Beta1, double &Gamma1, double &Alpha2, double &Beta2, double &Gamma2, double &Kappa, double &Mu, string &LString, int &Shielding, string &Lambda, double &Epsilon12, double &Epsilon13, bool &ExtraExponential)
{
	string Line, Line1, Line2, Line3, Line4, OrderString;
	int NumTerms, Offset;

	getline(FileMatrixElem, Line);
	getline(FileMatrixElem, Line);
	getline(FileMatrixElem, LString);
	getline(FileMatrixElem, OrderString);

	FileMatrixElem >> Line >> Omega;
	FileMatrixElem >> Line1 >> Line2 >> Line3 >> NumTerms;

	cout << LString << endl;
	if (LString == "S-Wave Singlet Ps-H") {
		LValue = 0;
		IsTriplet = 0;
		NumSets = 1;
	}
	else if (LString == "S-Wave Triplet Ps-H") {
		LValue = 0;
		IsTriplet = 1;
		NumSets = 1;
	}
	else if (LString == "S-Wave Singlet Ps-H - Laplacian Formalism - Exponential") {
		LValue = 0;
		IsTriplet = 0;
		NumSets = 1;
	}
	else if (LString == "S-Wave Triplet Ps-H - Laplacian Formalism - Exponential") {
		LValue = 0;
		IsTriplet = 1;
		NumSets = 1;
	}
	else if (LString == "P-Wave Singlet Ps-H: 1st formalism" || LString == "P-Wave Singlet Ps-H") {  // Second is the older type
		LValue = 1;
		IsTriplet = 0;
		NumSets = 1;
	}
	else if (LString == "P-Wave Triplet Ps-H: 1st formalism" || LString == "P-Wave Triplet Ps-H") {  // Second is the older type
		LValue = 1;
		IsTriplet = 1;
		NumSets = 1;
	}
	else if (LString == "P-Wave Singlet Ps-H: 2nd formalism") {
		LValue = 1;
		IsTriplet = 0;
		NumSets = 1;
	}
	else if (LString == "P-Wave Triplet Ps-H: 2nd formalism") {
		LValue = 1;
		IsTriplet = 1;
		NumSets = 1;
	}
	else if (LString == "P-Wave Singlet Ps-H: 1st formalism / 2 sets") {
		LValue = 1;
		IsTriplet = 0;
		NumSets = 2;
	}
	else if (LString == "P-Wave Triplet Ps-H: 1st formalism / 2 sets") {
		LValue = 1;
		IsTriplet = 1;
		NumSets = 2;
	}
	else if (LString == "P-Wave Singlet Ps-H: 2nd formalism / 2 sets") {
		LValue = 1;
		IsTriplet = 0;
		NumSets = 2;
	}
	else if (LString == "P-Wave Triplet Ps-H: 2nd formalism / 2 sets") {
		LValue = 1;
		IsTriplet = 1;
		NumSets = 2;
	}
	else if (LString == "D-Wave Singlet Ps-H: 1st formalism") {
		LValue = 2;
		IsTriplet = 0;
		NumSets = 1;
	}
	else if (LString == "D-Wave Triplet Ps-H: 1st formalism") {
		LValue = 2;
		IsTriplet = 1;
		NumSets = 1;
	}
	else if (LString == "F-Wave Singlet Ps-H") {
		LValue = 3;
		IsTriplet = 0;
		NumSets = 1;
	}
	else if (LString == "F-Wave Triplet Ps-H") {
		LValue = 3;
		IsTriplet = 1;
		NumSets = 1;
	}
	else if (LString == "G-Wave Singlet Ps-H") {
		LValue = 4;
		IsTriplet = 0;
		NumSets = 1;
	}
	else if (LString == "G-Wave Triplet Ps-H") {
		LValue = 4;
		IsTriplet = 1;
		NumSets = 1;
	}
	else if (LString == "H-Wave Singlet Ps-H") {
		LValue = 5;
		IsTriplet = 0;
		NumSets = 1;
	}
	else if (LString == "H-Wave Triplet Ps-H") {
		LValue = 5;
		IsTriplet = 1;
		NumSets = 1;
	}
	else {
		cout << "Problem string in matrix element file has an unknown value...exiting." << endl;
		return -1;
	}

	if (ExtraExponential) {
		FileMatrixElem >> Line >> Alpha1 >> Line1 >> Beta1 >> Line2 >> Gamma1 >> Line3 >> Epsilon12 >> Line4 >> Epsilon13;
		cout << Alpha1 << " " << Beta1 << " " << Gamma1 << endl;
	}
	else {
		if (NumSets == 1) {
			FileMatrixElem >> Line >> Alpha1 >> Line1 >> Beta1 >> Line2 >> Gamma1;
			cout << Alpha1 << " " << Beta1 << " " << Gamma1 << endl;
		}
		else if (NumSets == 2) {
			FileMatrixElem >> Line >> Alpha2 >> Line1 >> Beta2 >> Line2 >> Gamma2;
		}
	}

	//getline(FileMatrixElem, Line);
	//getline(FileMatrixElem, Line);
	FileMatrixElem >> Line >> Mu;
	FileMatrixElem >> Line;
	Shielding = -1;
	if (Line.find("Shielding") != string::npos) {  // Skip this line - not yet to kappa line
		FileMatrixElem >> Line;
		FileMatrixElem >> Shielding;
		//getline(FileMatrixElem, Line);
		FileMatrixElem >> Line;
	}
	FileMatrixElem >> Kappa;
	getline(FileMatrixElem, Line);

	if (OrderString == "Using Denton's ordering") {
		Ordering = 0;
	}
	else if (OrderString == "Using Peter Van Reeth's ordering") {
		Ordering = 1;
	}
	else {
		cout << "Ordering string in matrix element file has an unknown value...exiting." << endl;
		return -1;
	}

	//getline(FileMatrixElem, Line);
	FileMatrixElem >> Line;
	if (Line.find("Lambda") != string::npos) {  // Has the extra lambda line here
		getline(FileMatrixElem, Lambda);
	}
	getline(FileMatrixElem, Line);

	for (int i = 0; i < 11; i++) {
		getline(FileMatrixElem, Line);
		if (Line == "A matrix row")  // Some files have one less extra line
			break;
	}

	// Reads in first row (and column) of A
	for (int i = 0; i < NumShortTerms+1; i++) {
		getline(FileMatrixElem, Line);
		istringstream iss(Line);
		iss >> Offset >> ARow[i];
	}

	// Skips extra lines
	getline(FileMatrixElem, Line);
	getline(FileMatrixElem, Line);

	// Reads in B vector
	for (int i = 0; i < NumShortTerms+1; i++) {
		getline(FileMatrixElem, Line);
		istringstream iss(Line);
		iss >> Offset >> B[i];
	}

	getline(FileMatrixElem, Line);
	getline(FileMatrixElem, Line);

	// SLS is not in A or B, so read it in.
	FileMatrixElem >> SLS;

	return 0;
}

// Reads in the short-range file header
int ReadShortHeader(ifstream &FileShortRange, int &Omega, int &LValue, int &IsTriplet, int &Formalism, int &Ordering, int &NumShortTerms, int &NumSets, int &Integration, double &Alpha1, double &Beta1, double &Gamma1, double &Alpha2, double &Beta2, double &Gamma2, bool &ExtraExponential, double &Epsilon12, double &Epsilon13, vector <int> &ExpLen)
{
	int MagicNum, Version, HeaderLen, DataFormat, NumShortTerms1, NumShortTerms2;
	int VarLen;

	FileShortRange.read((char*)&MagicNum, 4);
	FileShortRange.read((char*)&Version, 4);
	FileShortRange.read((char*)&HeaderLen, 4);
	FileShortRange.read((char*)&DataFormat, 4);
	FileShortRange.read((char*)&Omega, 4);
	FileShortRange.read((char*)&NumShortTerms1, 4);
	FileShortRange.read((char*)&NumShortTerms2, 4);
	FileShortRange.read((char*)&LValue, 4);
	FileShortRange.read((char*)&Formalism, 4);
	FileShortRange.read((char*)&IsTriplet, 4);
	FileShortRange.read((char*)&Ordering, 4);
	FileShortRange.read((char*)&Integration, 4);
	FileShortRange.read((char*)&NumSets, 4);

	//@TODO: More descriptive errors for each

	if (MagicNum != 0x31487350) {  // "PsH1" in hexadecimal (with reverse due to endianness)
		cout << "This is not a valid Ps-H file (MagicNum)...exiting." << endl;
		return -1;
	}

	if (Version < 1 || Version > 9) {
		cout << "This is not a valid Ps-H file (Version)...exiting." << endl;
		cout << Version << endl;
		return -1;
	}

	if (HeaderLen != 80 && HeaderLen != 104) {
		cout << "This is not a valid Ps-H file (HeaderLen)...exiting." << endl;
		return -1;
	}

	if (DataFormat != 8) {
		cout << "This is not a valid Ps-H file (Dataformat)...exiting." << endl;
		return -1;
	}

	if ((Formalism != 1 && Formalism != 2) || (IsTriplet != 0 && IsTriplet != 1) || (Ordering != 0 && Ordering != 1)) {
		cout << "This is not a valid Ps-H file (Formalism/IsTriplet/Ordering)...exiting." << endl;
		return -1;
	}

	if (NumSets != 1 && NumSets != 2) {
		cout << "This is not a valid Ps-H file (NumSets)...exiting." << endl;
		return -1;
	}

	/*if (NumShortTerms1 != NumShortTerms2) {
		cout << "This is not a valid Ps-H file...exiting." << endl;  // Cannot handle two different values for this yet.
		return -1;
	}*/
	NumShortTerms = NumShortTerms1;

	// @TODO: Set up to work properly with sectors
	FileShortRange.read((char*)&Alpha1, 8);
	FileShortRange.read((char*)&Beta1, 8);
	FileShortRange.read((char*)&Gamma1, 8);
	ExtraExponential = false;
	if (Version == 9) {  // Extra exponentials
		double BlankDouble;
		int BlankInt;
		ExtraExponential = true;
		FileShortRange.read((char*)&Epsilon12, 8);
		FileShortRange.read((char*)&Epsilon13, 8);
		FileShortRange.read((char*)&BlankDouble, 8);  // To be reserved for Epsilon23 at some point in the future

		ExpLen.resize(2);  // No r23 exponential right now
		FileShortRange.read((char*)&ExpLen[0], 4);
		FileShortRange.read((char*)&ExpLen[1], 4);
		FileShortRange.read((char*)&BlankInt, 4);
	}
	if (NumSets == 2) {
		//read (FileShortRange) Alpha2, Beta2, Gamma2
		FileShortRange.read((char*)&Alpha2, 8);
		FileShortRange.read((char*)&Beta2, 8);
		FileShortRange.read((char*)&Gamma2, 8);
	}

	FileShortRange.read((char*)&VarLen, 4);

	return 0;
}

void WriteHeader(ofstream &OutFile, string &LString, int &LValue, char *FileShortName, char *FileMatrixElemName, char *EnergyFileName, bool &Paired, bool &Resorted,
				int &ShortInt, int &NumTerms, double &Kappa, double &Mu, int &Shielding, string &Lambda, double &Alpha, double &Beta, double &Gamma, string &ProgName)
{
	OutFile << "<?xml version=\"1.0\" encoding=\"UTF-8\"?> " << endl;
	OutFile << "<psh_data>" << endl << "<header>" << endl;
	OutFile << "	<problem>" << LString << "</problem>" << endl;
	OutFile << "	<lvalue>" << LValue << "</lvalue>" << endl;
	OutFile << "	<shortfile>" << FileShortName << "</shortfile>" << endl;
	OutFile << "	<longfile>" << FileMatrixElemName << "</longfile>" << endl;
	//if (argc >= 6)  // Energy file present  //@TODO: Are we even accepting runs without now?
		OutFile << "	<energyfile>" << EnergyFileName << "</energyfile>" << endl;
	if (Paired == true)
		OutFile << "	<paired>" << "true" << "</paired>" << endl;
	else
		OutFile << "	<paired>" << "false" << "</paired>" << endl;
	OutFile << "	<ordering>" << "Peter" << "</ordering>" << endl;
	if (Resorted == true)
		OutFile << "	<reorder>" << "true" << "</reorder>" << endl;
	else
		OutFile << "	<reorder>" << "false" << "</reorder>" << endl;
	OutFile << "	<shortint>" << ShortIntString(ShortInt) << "</shortint>" << endl;
	OutFile << "	<numterms>" << NumTerms << "</numterms>" << endl;
	OutFile << "	<numsets>" << 1 << "</numsets>" << endl;
	OutFile << "	<kappa>" << Kappa << "</kappa>" << endl;
	OutFile << "	<mu>" << Mu << "</mu>" << endl;
	if (Shielding == -1)  // No shielding value specified in file - assume default
		Shielding = 2*LValue + 1;
	OutFile << "	<shielding>" << Shielding << "</shielding>" << endl;
	if (trim(Lambda) != "" || Lambda.size() > 0)
		OutFile << "	<lambda>" << trim(Lambda) << "</lambda>" << endl;
	OutFile << "	<nonlinear>" << endl;
	OutFile << "		<alpha>" << Alpha << "</alpha>" << endl;
	OutFile << "		<beta>" << Beta << "</beta>" << endl;
	OutFile << "		<gamma>" << Gamma << "</gamma>" << endl;
	OutFile << "	</nonlinear>" << endl;
	OutFile << "	<program>" << ProgName << "</program>" << endl;
	OutFile << "	<datetime>" << GetDateTime() << "</datetime>" << endl;

	cout << "n          Kohn              Inverse Kohn          Complex Kohn (S)       Complex Kohn (T)" << endl;
	OutFile << "</header>" << endl << "<dataheader>" << endl << DataHeader << endl << "</dataheader>" << endl << "<data>" << endl;
	OutFile << setprecision(16);
	OutFile << scientific;

	return;
}

// Generalized real Kohn
void uGenKohn(dcmplx (&u)[2][2], double Tau)
{
	u[0][0] = dcmplx(cos(Tau),0);
	u[0][1] = dcmplx(sin(Tau),0);
	u[1][0] = dcmplx(-sin(Tau),0);
	u[1][1] = dcmplx(cos(Tau),0);
	return;
}

// Generalized T-matrix Kohn
void uGenTKohn(dcmplx (&u)[2][2], double Tau)
{
	u[0][0] = dcmplx(cos(Tau),0);
	u[0][1] = dcmplx(sin(Tau),0);
	u[1][0] = dcmplx(-sin(Tau),cos(Tau));
	u[1][1] = dcmplx(cos(Tau),sin(Tau));
	return;
}

// Generalized S-matrix Kohn
void uGenSKohn(dcmplx (&u)[2][2], double Tau)
{
	u[0][0] = dcmplx(-sin(Tau),-cos(Tau));
	u[0][1] = dcmplx(cos(Tau),-sin(Tau));
	u[1][0] = dcmplx(-sin(Tau),cos(Tau));
	u[1][1] = dcmplx(cos(Tau),sin(Tau));
	return;
}

// ARow and BVec are sent from the main program as the A and B from the Kohn method.  This function rearranges everything into
//  the matrix equation (7) of their paper and solves.
double CombinedKohn(dcmplx (&u)[2][2], int NumShortTerms, vector <double> &ARow, vector <double> &B, vector <double> &ShortTerms, double SLS, int LValue, int IsTriplet)
{
	MKL_INT n, nrhs, lda, ldb, info;
	//double *A = new double[(NumShortTerms+1)*(NumShortTerms+1)];
	vector <dcmplx> A((NumShortTerms+1)*(NumShortTerms+1));
	vector <dcmplx> X(NumShortTerms+1);
	double CLC = ARow[0], CLS = B[0];
	double SLC = CLS + 1.0;  // Use (S,LC) = (C,LS) + 1
	dcmplx SLSt, SLCt, CLSt, CLCt;

	dcmplx detu = u[0][0]*u[1][1] - u[0][1]*u[1][0];  // Determinant

	SLSt = u[0][0]*u[0][0]*SLS + u[0][0]*u[0][1]*SLC + u[0][1]*u[0][0]*CLS + u[0][1]*u[0][1]*CLC;
	SLCt = u[0][0]*u[1][0]*SLS + u[0][0]*u[1][1]*SLC + u[0][1]*u[1][0]*CLS + u[0][1]*u[1][1]*CLC;
	CLSt = u[1][0]*u[0][0]*SLS + u[1][0]*u[0][1]*SLC + u[1][1]*u[0][0]*CLS + u[1][1]*u[0][1]*CLC;
	CLCt = u[1][0]*u[1][0]*SLS + u[1][0]*u[1][1]*SLC + u[1][1]*u[1][0]*CLS + u[1][1]*u[1][1]*CLC;

	// Copy short-range terms to bottom-right NumShortTerms x NumShortTerms submatrix of A.
	for (int i = 0; i < NumShortTerms; i++) {
		for (int j = 0; j < NumShortTerms; j++) {
			A[(i+1)*(NumShortTerms+1) + (j+1)] = ShortTerms[i*NumShortTerms + j];
		}
	}

	// Fill in the rest of A
	A[0] = CLCt;
	for (int i = 1; i < NumShortTerms+1; i++) {
		//A[i] = dcmplx(ARow[i], B[i]);
		A[i] = u[1][0]*B[i] + u[1][1]*ARow[i];
		A[i*(NumShortTerms+1)] = A[i];
	}

	// Fill in B (or X)
	X[0] = -CLSt;
	for (int i = 1; i < NumShortTerms+1; i++) {
		X[i] = - u[0][0]*B[i] - u[0][1]*ARow[i];
	}

	// LAPACK requires calls by reference, so we have to define all these variables.
	vector <int> ipiv(NumShortTerms+1);
	n = lda = ldb = NumShortTerms+1;
	nrhs = 1;
	zgesv(&n, &nrhs, (MKL_Complex16*)&A[0], &lda, &ipiv[0], (MKL_Complex16*)&X[0], &ldb, &info);
	if (info != 0) {
		cout << "LAPACK Error: " << info << endl;
		return 0.0;
	}

	// Equation () of notes
	dcmplx PsiLS = X[0] * CLSt;
	for (int i = 1; i < (NumShortTerms+1); i++) {
		PsiLS += X[i] * (u[0][0]*B[i] + u[0][1]*ARow[i]);
	}
	dcmplx L = -(PsiLS + SLSt) / detu;
	// Go from general L matrix element to K.
	dcmplx K = (u[0][1] + u[1][1]*L) / (u[0][0] + u[1][0]*L);

	//@TODO: Check K for imaginary part.
	double PhaseShift = atan(K.real());
	FixPhase(PhaseShift, LValue, IsTriplet);
	return PhaseShift;
}

string ShortIntString(int &Integration)
{
	switch (Integration)
	{
		case 1:
			return string("Direct summation");
		case 2:
			return string("Asymptotic expansion");
		case 3:
			return string("Recursion relations");
	}
	return string("Unknown integration");
}

// Since we are finding atan(delta) instead of delta directly, some of the results
//  are in the wrong range.
void FixPhase(double &PhaseShift, int &LValue, int &IsTriplet)
{
	double Pi = 4.0 * atan(1.0);

	if (LValue == 0 && IsTriplet == 0) {  // ^1S
		if (PhaseShift > 0.0) {
			PhaseShift = PhaseShift - Pi;
		}
	}
	else if (LValue == 0 && IsTriplet == 1) {  // ^3S
		if (PhaseShift > 0.0) {
			PhaseShift = PhaseShift - Pi;
		}
	}
	else if (LValue >= 3) {  // ^1F and higher
		if (PhaseShift > Pi) {
			PhaseShift = PhaseShift - Pi;
		}
	}
}

// Modified from http://www.dreamincode.net/code/snippet1102.htm
string GetDateTime(void)
{
	//Find the current time
	time_t curtime = time(0);

	//convert it to tm
	tm now=*localtime(&curtime);

	//BUFSIZ is standard macro that expands to a integer constant expression
	//that is greater then or equal to 256. It is the size of the stream buffer
	//used by setbuf()
	char dest[BUFSIZ]={0};

	//Format string determines the conversion specification's behaviour
	const char format[]="%x %X";

	//strftime - converts date and time to a string
	if (strftime(dest, sizeof(dest)-1, format, &now)>0) {
		return string(dest);
	}
	else
		cerr << "strftime failed. Errno code: " << errno << endl;
	return string("");
}

General Code/Phase Shift/Phase Shift.cpp


// #include "NeoHookeanFEMConstraint.h"
// #include <Eigen/LU>
// #include <math.h>
// #include <iostream>
// bool
// NeoHookeanFEMConstraint::
// ComputeDeformationGradient(const Eigen::VectorXd& x)
// {

// 	if(FEMConstraint::ComputeDeformationGradient(x))
// 	{
// 		mCacheFTF = (mCacheF.transpose())*mCacheF;
// 		mCacheInvF = (mCacheF.inverse());
// 		mCacheInvFT = mCacheInvF.transpose();
// 		// if(fabs(mCacheF.determinant())<1E-6)
// 			// std::cout<<mCacheF<<std::endl;
// 		return true;
// 	}
// 	return false;
// }
// void
// NeoHookeanFEMConstraint::
// ComputedP(const Eigen::Matrix2d& dF,Eigen::Matrix2d& dP)
// {
// 	double I3 = mCacheFTF.determinant();

// 	dP =
// 		mMu*dF+
// 		(mMu - mLambda*log(I3)*0.5)*mCacheInvFT*(dF.transpose())*mCacheInvFT+
// 		(mLambda*((mCacheInvF*dF).trace()))*mCacheInvFT;
// }
// NeoHookeanFEMConstraint::
// NeoHookeanFEMConstraint(const double& stiffness,const double& poisson_ratio,int i0,int i1,int i2,double vol,const Eigen::Matrix2d& invDm)
// 	:FEMConstraint(stiffness,poisson_ratio,i0,i1,i2,vol,invDm),
// 	mCacheFTF(Eigen::Matrix2d::Zero()),
// 	mCacheInvFT(Eigen::Matrix2d::Zero())
// {
// }
// double
// NeoHookeanFEMConstraint::
// EvalPotentialEnergy(const Eigen::VectorXd& x)
// {
// 	ComputeDeformationGradient(x);

// 	double I1 = mCacheFTF.trace();
// 	double I3 = mCacheFTF.determinant();

// 	return mVol*(0.25*mMu*(I1-log(I3)-3) + 0.125*mLambda*(log(I3)*log(I3)));
// }
// void
// NeoHookeanFEMConstraint::
// EvalGradient(const Eigen::VectorXd& x, Eigen::VectorXd& gradient)
// {
// 	ComputeDeformationGradient(x);

// 	double I3 = mCacheFTF.determinant();
// 	// std::cout<<I3<<std::endl;
// 	Eigen::Matrix2d P = (0.5*mMu)*(mCacheF-mCacheInvFT) + (0.5*mLambda*log(I3))*mCacheInvFT;

// 	P = mVol*P*mInvDm;

// 	gradient.block<2,1>(mi0*2,0) += -(P.block<2,1>(0,0) + P.block<2,1>(0,1));
// 	gradient.block<2,1>(mi1*2,0) += P.block<2,1>(0,0);
// 	gradient.block<2,1>(mi2*2,0) += P.block<2,1>(0,1);
// }

// void
// NeoHookeanFEMConstraint::
// EvalHessian(const Eigen::VectorXd& x, const Eigen::VectorXd& dx, Eigen::VectorXd& dg)
// {
// 	ComputeDeformationGradient(x);
// 	Eigen::Matrix2d dDs,dF,dP;
// 	Eigen::Vector2d dx0(dx.block<2,1>(mi0*2,0));
// 	dDs.block<2,1>(0,0) = dx.block<2,1>(mi1*2,0)-dx0;
// 	dDs.block<2,1>(0,1) = dx.block<2,1>(mi2*2,0)-dx0;

// 	dF = dDs*(mInvDm);
// 	ComputedP(dF,dP);

// 	dP = mVol * dP * (mInvDm.transpose());

// 	dg.block<2,1>(mi0*2,0) += -(dP.block<2,1>(0,0) + dP.block<2,1>(0,1));
// 	dg.block<2,1>(mi1*2,0) += dP.block<2,1>(0,0);
// 	dg.block<2,1>(mi2*2,0) += dP.block<2,1>(0,1);
// }

// void
// NeoHookeanFEMConstraint::
// EvaluateDVector(int index, const Eigen::VectorXd& x,Eigen::VectorXd& d)
// {
// 	std::cout<<"NeoHookeanFEMConstraint not supported."<<std::endl;
// }
// void
// NeoHookeanFEMConstraint::
// EvaluateJMatrix(int index, std::vector<Eigen::Triplet<double>>& J_triplets)
// {
// 	std::cout<<"NeoHookeanFEMConstraint not supported."<<std::endl;
// }
// void
// NeoHookeanFEMConstraint::
// EvaluateLMatrix(std::vector<Eigen::Triplet<double>>& L_triplets)
// {
// 	std::cout<<"NeoHookeanFEMConstraint not supported."<<std::endl;
// }
// int
// NeoHookeanFEMConstraint::
// GetNumHessianTriplets()
// {
// 	return 36;
// }


#pragma once
#include <Eigen/Dense>
#include "SdfObject.hpp"
#include "../accelerate/Bound3.hpp"

class SdfRoundCone : public SdfObject
{
public:
    SdfRoundCone(Eigen::Vector3f a, Eigen::Vector3f b, float ra, float rb) : SdfObject(a), a(a), b(b), ra(ra), rb(rb){};

    float sdf(const Eigen::Vector3f &position) const override
    {
        constexpr auto sign = [](auto a)
        { return a >= 0 ? 1 : -1; };

        constexpr auto dot = [](Eigen::Vector3f a)
        { return a.dot(a); };

        // sampling independent computations (only depend on shape)
        auto ba = b - a;
        auto l2 = dot(ba);
        auto rr = ra - rb;
        auto a2 = l2 - rr * rr;
        auto il2 = 1.0f / l2;

        // sampling dependant computations
        auto pa = position - a;
        auto y = pa.dot(ba);
        auto z = y - l2;
        auto x2 = dot(pa * l2 - ba * y);
        auto y2 = y * y * l2;
        auto z2 = z * z * l2;

        // single square root!
        auto k = sign(rr) * rr * rr * x2;
        if (sign(z) * a2 * z2 > k)
            return sqrt(x2 + z2) * il2 - rb;
        if (sign(y) * a2 * y2 < k)
            return sqrt(x2 + y2) * il2 - ra;
        return (sqrt(x2 * a2 * il2) + y * rr) * il2 - ra;
    };

    std::unique_ptr<Bound3> build_bound3() const override
    {
        auto aa = ra * Eigen::Vector3f::Ones();
        auto bb = rb * Eigen::Vector3f::Ones();
        auto min = (a - aa).cwiseMin(b - bb);
        auto max = (a + aa).cwiseMax(b + bb);
        return std::make_unique<Bound3>(min, max);
    };

private:
    Eigen::Vector3f a;

    Eigen::Vector3f b;

    float ra;

    float rb;
};