/**
 * @author Eric Cousineau <eacousineau@gmail.com>, member of Dr. Aaron
 * Ames's AMBER Lab
 */
#include <Eigen/Dense>

// See test_runtime_config for more information

namespace test_runtime_config_clean_eigen
{

void resize_matrix()
{
    Eigen::MatrixXd blank;
    blank.resize(5, 5);
}

}


/*
 *  Copyright (c) 2008--2011, Universitaet Bremen
 *  All rights reserved.
 *
 *  Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
 *
 *  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.
 *   * Neither the name of the Universitaet Bremen nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER 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.
 */
/**
 * @file mtk/types/SOn.hpp
 * @brief Standard Orthogonal Groups i.e.\ rotatation groups.
 */
#ifndef SON_H_
#define SON_H_

#include "vect.hpp"
#include "../src/mtkmath.hpp"

#include <Eigen/Geometry>

namespace MTK {

/**
 * Two-dimensional orientations represented as scalar.
 * There is no guarantee that the representing scalar is within any interval,
 * but the result of boxminus will always have magnitude @f$\le\pi @f$.
 */
template<class _scalar = double, int Options = Eigen::AutoAlign>
struct SO2 : public Eigen::Rotation2D<_scalar> {
	enum {DOF = 1, DIM = 2};

	typedef _scalar scalar;
	typedef Eigen::Rotation2D<scalar> base;
	typedef vect<DIM, scalar, Options> vect_type;

	//using base::operator=;

	//! Construct from angle
	SO2(const scalar& angle = 0) : base(angle) {	}

	//! Construct from Eigen::Rotation2D
	SO2(const base& src) : base(src) {}

	/**
	 * Construct from 2D vector.
	 * Resulting orientation will rotate the first unit vector to point to vec.
	 */
	SO2(const vect_type &vec) : base(atan2(vec[1], vec[0])) {};

	//! Calculate @c this->inverse() * @c r
	SO2 operator%(const base &r) const {
		return base::inverse() * r;
	}

	//! Calculate @c this->inverse() * @c r
	template<class Derived>
	vect_type operator%(const Eigen::MatrixBase<Derived> &vec) const {
		return base::inverse() * vec;
	}

	//! Calculate @c *this * @c r.inverse()
	SO2 operator/(const SO2 &r) const {
		return *this * r.inverse();
	}

	//! Gets the angle as scalar.
	operator scalar() const {
		return base::angle();
	}

	//! @name Manifold requirements
	//{
	void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
		base::angle() += scale * vec[0];
	}
	void boxminus(MTK::vectview<scalar, DOF> res, const SO2<scalar>& other) const {
		res[0] = MTK::normalize(base::angle() - other.angle(), scalar(MTK::pi));
	}
	//}

	friend std::istream& operator>>(std::istream &is, SO2<scalar>& ang){
		return is >> ang.angle();
	}

};

/**
 * Three-dimensional orientations represented as Quaternion.
 * It is assumed that the internal Quaternion always stays normalized,
 * should this not be the case, call inherited member function @c normalize().
 */
template<class _scalar = double, int Options = Eigen::AutoAlign>
struct SO3 : public Eigen::Quaternion<_scalar, Options> {
	enum {DOF = 3, DIM = 3};
	typedef _scalar scalar_type;
	typedef Eigen::Quaternion<scalar_type, Options> base;
	typedef Eigen::Quaternion<scalar_type> Quaternion;
	typedef vect<DIM, scalar_type, Options> vectorized_type;

	//using base::operator=;

	//! Calculate @c this->inverse() * @c r
	template<class OtherDerived> EIGEN_STRONG_INLINE
	Quaternion operator%(const Eigen::QuaternionBase<OtherDerived> &r) const {
		return base::conjugate() * r;
	}

	//! Calculate @c this->inverse() * @c r
	template<class Derived>
	vectorized_type operator%(const Eigen::MatrixBase<Derived> &vec) const {
		return base::conjugate() * vec;
	}

	//! Calculate @c this * @c r.conjugate()
	template<class OtherDerived> EIGEN_STRONG_INLINE
	Quaternion operator/(const Eigen::QuaternionBase<OtherDerived> &r) const {
		return *this * r.conjugate();
	}

	/**
	 * Construct from real part and three imaginary parts.
	 * Quaternion is normalized after construction.
	 */
	SO3(const scalar_type& w, const scalar_type& x, const scalar_type& y, const scalar_type& z) : base(w, x, y, z) {
		base::normalize();
	}

	/**
	 * Construct from Eigen::Quaternion.
	 * @note Non-normalized input may result result in spurious behavior.
	 */
	SO3(const base& src = base::Identity()) : base(src) {}

	/**
	 * Construct from rotation matrix.
	 * @note Invalid rotation matrices may lead to spurious behavior.
	 */
	template<class Derived>
	SO3(const Eigen::MatrixBase<Derived>& matrix) : base(matrix) {}

	/**
	 * Construct from arbitrary rotation type.
	 * @note Invalid rotation matrices may lead to spurious behavior.
	 */
	template<class Derived>
	SO3(const Eigen::RotationBase<Derived, 3>& rotation) : base(rotation.derived()) {}

	//! @name Manifold requirements
	//{
	SO3 operator+(const vectorized_type& vec) const {
		SO3 delta = exp(vec);
		return delta * *this;
	}

    void operator+=(const vectorized_type& vec) {
        *this = *this + vec;
    }

	auto operator-(const SO3<scalar_type>& other) const {
		return SO3::log(*this * other.conjugate());
	}
	//}

	friend std::ostream& operator<<(std::ostream &os, const SO3<scalar_type, Options>& q){
		return os << q.coeffs().transpose() << " ";
	}
	friend std::istream& operator>>(std::istream &is, SO3<scalar_type, Options>& q){
		vect<4,scalar_type> coeffs;
		is >> coeffs;
		q.coeffs() = coeffs.normalized();
		return is;
	}

	//! @name Helper functions
	//{
	/**
	 * Calculate the exponential map. In matrix terms this would correspond
	 * to the Rodrigues formula.
	 */
	// FIXME vectview<> can't be constructed from every MatrixBase<>, use const Vector3x& as workaround
//	static SO3 exp(MTK::vectview<const scalar, 3> dvec, scalar scale = 1){
	static SO3 exp(const Eigen::Matrix<scalar_type, 3, 1>& dvec, scalar_type scale = 1){
		SO3 res;
		res.w() = MTK::exp<scalar_type, 3>(res.vec(), dvec, scalar_type(scale / 2));
		return res;
	}
	/**
	 * Calculate the inverse of @c exp.
	 * Only guarantees that <code>exp(log(x)) == x </code>
	 */
	static typename base::Vector3 log(const SO3 &orient){
		typename base::Vector3 res;
		MTK::log<scalar_type, 3>(res, orient.w(), orient.vec(), scalar_type(2), true);
		return res;
	}
	//}
};

namespace internal {
template<class Scalar, int Options>
struct UnalignedType<SO2<Scalar, Options > >{
	typedef SO2<Scalar, Options | Eigen::DontAlign> type;
};

template<class Scalar, int Options>
struct UnalignedType<SO3<Scalar, Options > >{
	typedef SO3<Scalar, Options | Eigen::DontAlign> type;
};

}  // namespace internal

}  // namespace MTK

#endif /*SON_H_*/


#include <Eigen/Core>
#include <iostream>

using namespace Eigen;

// [circulant_func]
template<class ArgType>
class circulant_functor {
  const ArgType &m_vec;
public:
  circulant_functor(const ArgType& arg) : m_vec(arg) {}

  const typename ArgType::Scalar& operator() (Index row, Index col) const {
    Index index = row - col;
    if (index < 0) index += m_vec.size();
    return m_vec(index);
  }
};
// [circulant_func]

// [square]
template<class ArgType>
struct circulant_helper {
  typedef Matrix<typename ArgType::Scalar,
                 ArgType::SizeAtCompileTime,
                 ArgType::SizeAtCompileTime,
                 ColMajor,
                 ArgType::MaxSizeAtCompileTime,
                 ArgType::MaxSizeAtCompileTime> MatrixType;
};
// [square]

// [makeCirculant]
template <class ArgType>
CwiseNullaryOp<circulant_functor<ArgType>, typename circulant_helper<ArgType>::MatrixType>
makeCirculant(const Eigen::MatrixBase<ArgType>& arg)
{
  typedef typename circulant_helper<ArgType>::MatrixType MatrixType;
  return MatrixType::NullaryExpr(arg.size(), arg.size(), circulant_functor<ArgType>(arg.derived()));
}
// [makeCirculant]

// [main]
int main()
{
  Eigen::VectorXd vec(4);
  vec << 1, 2, 4, 8;
  Eigen::MatrixXd mat;
  mat = makeCirculant(vec);
  std::cout << mat << std::endl;
}
// [main]


#include "gtest/gtest.h"

#include <Eigen/Geometry>

#include "Predicates.h"
#include "Utilities.h"

using namespace FluidSim3D;

//
// Orient2d tests
//

TEST(PREDICATES_TEST, ORIENT_2D_ZERO_DET_TEST)
{
	exactinit();

	int testCases = 1000;
	for (int testIndex = 0; testIndex < testCases; ++testIndex)
	{
		Vec2d startPoint = 1e5 * Vec2d::Random();
		Vec2d endPoint = 1e5 * Vec2d::Random();

		Vec2d testPoint = startPoint;

		EXPECT_EQ(orient2d(&startPoint[0], &endPoint[0], &testPoint[0]), 0.);

		testPoint = endPoint;
		EXPECT_EQ(orient2d(&startPoint[0], &endPoint[0], &testPoint[0]), 0.);
	}
}

TEST(PREDICATES_TEST, ORIENT_2D_POSITIVE_DET_TEST)
{
	exactinit();

	int testCases = 1000;
	for (int testIndex = 0; testIndex < testCases; ++testIndex)
	{
		Vec2d startPoint = 1e5 * Vec2d::Random();
		Vec2d endPoint = 1e5 * Vec2d::Random();

		if (startPoint == endPoint)
			continue;

		Vec2d vec = endPoint - startPoint;
		Vec2d norm(-vec[1], vec[0]);

		double offsetScalar = 1e-12;
		Vec2d testPoint = startPoint + offsetScalar * norm;

		EXPECT_GT(orient2d(&startPoint[0], &endPoint[0], &testPoint[0]), 0.);

		testPoint = endPoint + offsetScalar * norm;
		EXPECT_GT(orient2d(&startPoint[0], &endPoint[0], &testPoint[0]), 0.);

		testPoint = .5 * (startPoint + endPoint) + offsetScalar * norm;
		EXPECT_GT(orient2d(&startPoint[0], &endPoint[0], &testPoint[0]), 0.);
	}
}

TEST(PREDICATES_TEST, ORIENT_2D_NEGATIVE_DET_TEST)
{
	exactinit();

	int testCases = 1000;
	for (int testIndex = 0; testIndex < testCases; ++testIndex)
	{
		Vec2d startPoint = 1e5 * Vec2d::Random();
		Vec2d endPoint = 1e5 * Vec2d::Random();

		if (startPoint == endPoint)
			continue;

		Vec2d vec = endPoint - startPoint;
		Vec2d norm(-vec[1], vec[0]);

		double offsetScalar = -1e-12;
		Vec2d testPoint = startPoint + offsetScalar * norm;

		EXPECT_LT(orient2d(&startPoint[0], &endPoint[0], &testPoint[0]), 0.);

		testPoint = endPoint + offsetScalar * norm;
		EXPECT_LT(orient2d(&startPoint[0], &endPoint[0], &testPoint[0]), 0.);

		testPoint = .5 * (startPoint + endPoint) + offsetScalar * norm;
		EXPECT_LT(orient2d(&startPoint[0], &endPoint[0], &testPoint[0]), 0.);
	}
}

//
// Orient3d tests
//

TEST(PREDICATES_TEST, ORIENT_3D_ZERO_DET_TEST)
{
	exactinit();

	int testCases = 1000;
	for (int testIndex = 0; testIndex < testCases; ++testIndex)
	{
		Vec3d v0 = 1e5 * Vec3d::Random();
		Vec3d v1 = 1e5 * Vec3d::Random();
        Vec3d v2 = 1e5 * Vec3d::Random();

		if (v0 == v1 || v1 == v2 || v2 == v0)
			continue;

		Vec3d testPoint = v0;
		EXPECT_EQ(orient3d(v0.data(), v1.data(), v2.data(), testPoint.data()), 0.);

        testPoint = v1;
        EXPECT_EQ(orient3d(v0.data(), v1.data(), v2.data(), testPoint.data()), 0.);

        testPoint = v2;
        EXPECT_EQ(orient3d(v0.data(), v1.data(), v2.data(), testPoint.data()), 0.);
	}
}

TEST(PREDICATES_TEST, ORIENT_3D_POSITIVE_DET_TEST)
{
	exactinit();

	int testCases = 1000;
	for (int testIndex = 0; testIndex < testCases; ++testIndex)
	{
		Vec3d v0 = 1e5 * Vec3d::Random();
		Vec3d v1 = 1e5 * Vec3d::Random();
        Vec3d v2 = 1e5 * Vec3d::Random();

		if (v0 == v1 || v1 == v2 || v2 == v0)
			continue;

        Vec3d vec0 = v1 - v0;
        Vec3d vec1 = v2 - v0;
        Vec3d norm = vec0.cross(vec1).normalized();

		double offsetScalar = 1e-10;

		Vec3d testPoint = v0 + offsetScalar * norm;
		EXPECT_LT(orient3d(v0.data(), v1.data(), v2.data(), testPoint.data()), 0.);

		testPoint = v1 + offsetScalar * norm;
		EXPECT_LT(orient3d(v0.data(), v1.data(), v2.data(), testPoint.data()), 0.);

        testPoint = v2 + offsetScalar * norm;
		EXPECT_LT(orient3d(v0.data(), v1.data(), v2.data(), testPoint.data()), 0.);

		testPoint = (1. / 3.) * (v0 + v1 + v2) + offsetScalar * norm;
		EXPECT_LT(orient3d(v0.data(), v1.data(), v2.data(), testPoint.data()), 0.);
	}
}

TEST(PREDICATES_TEST, ORIENT_3D_NEGATIVE_DET_TEST)
{
	exactinit();

	int testCases = 1000;
	for (int testIndex = 0; testIndex < testCases; ++testIndex)
	{
		Vec3d v0 = 1e5 * Vec3d::Random();
		Vec3d v1 = 1e5 * Vec3d::Random();
        Vec3d v2 = 1e5 * Vec3d::Random();

		if (v0 == v1 || v1 == v2 || v2 == v0)
			continue;

        Vec3d vec0 = v1 - v0;
        Vec3d vec1 = v2 - v0;
        Vec3d norm = vec0.cross(vec1).normalized();

		double offsetScalar = 1e-10;

		Vec3d testPoint = v0 - offsetScalar * norm;
		EXPECT_GT(orient3d(v0.data(), v1.data(), v2.data(), testPoint.data()), 0.);

		testPoint = v1 - offsetScalar * norm;
		EXPECT_GT(orient3d(v0.data(), v1.data(), v2.data(), testPoint.data()), 0.);

        testPoint = v2 - offsetScalar * norm;
		EXPECT_GT(orient3d(v0.data(), v1.data(), v2.data(), testPoint.data()), 0.);

		testPoint = (1. / 3.) * (v0 + v1 + v2) - offsetScalar * norm;
		EXPECT_GT(orient3d(v0.data(), v1.data(), v2.data(), testPoint.data()), 0.);
	}
}

TEST(PREDICATES_TEST, EXACT_TRIANGLE_INTERSECTION_CROSSING_TRIANGLE_TEST)
{
	exactinit();

	int testCases = 100;
	int offsetCases = 100;
	for (int testIndex = 0; testIndex < testCases; ++testIndex)
	{
		Vec3d rayStart = 1e5 * Vec3d::Random();

		for (auto axis : { Axis::XAXIS, Axis::YAXIS, Axis::ZAXIS })
		{
            int axisIndex = axis == Axis::XAXIS ? 0 : axis == Axis::YAXIS ? 1 : 2;

			Vec3d offset = 1e-10 * rayStart.cwiseAbs();

			for (int offsetIndex = 0; offsetIndex < offsetCases; ++offsetIndex)
			{
				Vec3d v0 = rayStart;
                v0[axisIndex] += offset[axisIndex] + std::fabs(1e5 * double(std::rand()) / double(RAND_MAX));

				Vec3d v1 = v0;
                v1[(axisIndex + 1) % 3] += offset[(axisIndex + 1) % 3] + std::fabs(1e5 * double(std::rand()) / double(RAND_MAX));

				Vec3d v2 = v0;
                v2[(axisIndex + 1) % 3] -= offset[(axisIndex + 1) % 3] + std::fabs(1e5 * double(std::rand()) / double(RAND_MAX));

                v0[(axisIndex + 2) % 3] += offset[(axisIndex + 2) % 3] + std::fabs(1e5 * double(std::rand()) / double(RAND_MAX));
                v1[(axisIndex + 2) % 3] -= offset[(axisIndex + 2) % 3] + std::fabs(1e5 * double(std::rand()) / double(RAND_MAX));
                v2[(axisIndex + 2) % 3] -= offset[(axisIndex + 2) % 3] + std::fabs(1e5 * double(std::rand()) / double(RAND_MAX));

				EXPECT_EQ(exactTriIntersect(rayStart, v0, v1, v2, axis), IntersectionLabels::YES);

				std::swap(v0, v1);

				EXPECT_EQ(exactTriIntersect(rayStart, v0, v1, v2, axis), IntersectionLabels::YES);
			}
		}
	}
}
//
//TEST(PREDICATES_TEST, EXACT_TRIANGLE_INTERSECTION_CROSSING_POINT_TEST)
//{
//	exactinit();
//
//	int testCases = 1000;
//	int offsetCases = 1000;
//	for (int testIndex = 0; testIndex < testCases; ++testIndex)
//	{
//		Vec3d rayStart = 1e5 * Vec2d::Random();
//
//		for (auto axis : { Axis::XAXIS, Axis::YAXIS, Axis::ZAXIS })
//		{
//			double offset = 1e-12 * rayStart.cwiseAbs().maxCoeff();
//
//            int axisIndex = axis == Axis::XAXIS ? 0 : axis == Axis::YAXIS ? 1 : 2;
//
//            Vec3d v0 = rayStart;
//            v0[axisIndex] += offset + std::fabs(1e5 * double(std::rand()) / double(RAND_MAX));
//
//
//
//			// Start down-left, end up-right
//			for (int offsetIndex = 0; offsetIndex < offsetCases; ++offsetIndex)
//			{
//				Vec2d startPoint = rayStart;
//				Vec2d endPoint = rayStart;
//
//				startPoint += Vec2d(-offset, -offset);
//				endPoint += Vec2d(offset, offset);
//
//				EXPECT_EQ(exactTriIntersect(startPoint, endPoint, rayStart, axis), IntersectionLabels::ON);
//
//				std::swap(startPoint, endPoint);
//
//				EXPECT_EQ(exactTriIntersect(startPoint, endPoint, rayStart, axis), IntersectionLabels::ON);
//			}
//
//			// Start up-left, end down-right
//			for (int offsetIndex = 0; offsetIndex < offsetCases; ++offsetIndex)
//			{
//				Vec2d startPoint = rayStart;
//				Vec2d endPoint = rayStart;
//
//				startPoint += Vec2d(-offset, offset);
//				endPoint += Vec2d(offset, -offset);
//
//				EXPECT_EQ(exactEdgeIntersect(startPoint, endPoint, rayStart, axis), IntersectionLabels::ON);
//
//				std::swap(startPoint, endPoint);
//
//				EXPECT_EQ(exactEdgeIntersect(startPoint, endPoint, rayStart, axis), IntersectionLabels::ON);
//			}
//
//			// Start up, end down
//			for (int offsetIndex = 0; offsetIndex < offsetCases; ++offsetIndex)
//			{
//				Vec2d startPoint = rayStart;
//				Vec2d endPoint = rayStart;
//
//				if (axis == Axis::XAXIS)
//				{
//					startPoint[1] += offset;
//					endPoint[1] -= offset;
//				}
//				else
//				{
//					startPoint[0] += offset;
//					endPoint[0] -= offset;
//				}
//
//				EXPECT_EQ(exactEdgeIntersect(startPoint, endPoint, rayStart, axis), IntersectionLabels::ON);
//
//				std::swap(startPoint, endPoint);
//
//				EXPECT_EQ(exactEdgeIntersect(startPoint, endPoint, rayStart, axis), IntersectionLabels::ON);
//			}
//		}
//	}
//}


#include <boost/numeric/conversion/cast.hpp>
#include <boost/math/special_functions/factorials.hpp>
#include "generalprimary.h"
#include "primarynumber.h"
#include "primaryvariable.h"
#include "servicecontainer.h"
#include "primaryfunction.h"
#include "primaryexpression.h"

double compile_time_expression_structure::generalPrimary::value(double prime) const
{
    if (isFactorial)
    {
        if (prime<0.0)
            throw std::logic_error("factorial applied to neg. value");
        using boost::numeric_cast;
        unsigned int uif = numeric_cast<unsigned int>(prime);
        double duif = numeric_cast<double>(uif);
        if (prime-duif > std::numeric_limits<double>::min())
            throw std::logic_error("factorial applied to not integer value");
        return boost::math::factorial<double>(uif);
    }
    else
    {
        return prime;
    }
}

std::unique_ptr<compile_time_expression_structure::generalPrimary>
compile_time_expression_structure::generalPrimary::createPrimary
(compile_time_expression_structure::serviceContainer& sc)
{
    Token t {sc.pop()};
    std::unique_ptr<generalPrimary> gp{nullptr};
    switch (t.kind) {
        case TokenKind::number:
        {
            gp = std::make_unique<primaryNumber>(t.value);
            break;
        }
        case TokenKind::variable:
        {
            // first find in constants
            auto gcit {global_constants_id.find(t.name)};
            if (gcit == cend(global_constants_id))
            {
                auto vit {global_variables_id.find(t.name)};
                if (vit == cend(global_variables_id))
                    throw std::logic_error("undefined variable literal " + t.name);
                // not constant
                gp = std::make_unique<primaryVariable>(vit->second, sc);
                break;
            }
            auto cit = global_constants.find(gcit->second);
            if (cit == cend(global_constants))
                throw std::logic_error("undefined symbolic literal");
            const double Value = cit->second;
            gp = std::make_unique<primaryNumber>(Value);
            break;
        }
        case TokenKind::function:
        {
            auto gfidIt {global_functions_id.find(t.name)};
            if (gfidIt == cend(global_functions_id))
                throw std::logic_error("unknown function name");
            gp = std::make_unique<primaryFunction>(gfidIt->second, sc);
            break;
        }
        case TokenKind::bracket_left0:
//        case TokenKind::bracket_left1:
        {
            gp = std::make_unique<primaryExpression>(sc);
            //double d {expression(ist)};
            Token t2 {sc.pop()};
            if (t2.kind != TokenKind::bracket_right0)
                throw std::logic_error("bracket_right expected");
            break;
        }
//        case TokenKind::bracket_left1:
//        {
//            double d {expression()};
//            t = ts.pop();
//            if (t.kind != TokenKind::bracket_right1)
//                throw std::logic_error("bracket_right expected");
//            result = d;
//            break;
//        }
        default:
            throw std::logic_error("primary is expected");
    }

    Token tp {sc.pop()};
    if (tp.kind == TokenKind::factorial)
    {
        gp->setFactorial(true);
    }
    else {
        sc.push(tp);
    }
    return gp;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
//
// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.
//
// 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)

#include <geometry_test_common.hpp>

#include <boost/geometry/extensions/nsphere/nsphere.hpp>

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

#include <boost/geometry/strategies/strategies.hpp>

#include <boost/geometry/io/wkt/read.hpp>

typedef bg::model::d2::point_xy<double> point_type;
typedef bg::model::nsphere<point_type, double> circle_type;
typedef bg::model::box<point_type> box_type;

template <typename Geometry>
void test_circle(std::string const& wkt_geometry, bool expected_within, bool expected_covered_by)
{
    circle_type circle(point_type(1.0, 1.0), 3.0);
    //bg::assign(circle, 1.0, 1.0, 3.0);

    Geometry geometry;
    bg::read_wkt(wkt_geometry, geometry);

    bool detected = bg::within(circle, geometry);

    BOOST_CHECK_MESSAGE(detected == expected_within,
        "circle (1,1) with radius 3 within : " << wkt_geometry
        << " -> Expected: " << expected_within
        << " detected: " << detected);

    detected = bg::covered_by(circle, geometry);

    BOOST_CHECK_MESSAGE(detected == expected_covered_by,
        "circle (1,1) with radius 3 covered_by : " << wkt_geometry
        << " -> Expected: " << expected_covered_by
        << " detected: " << detected);
}

void test_circles()
{
    test_circle<box_type>("BOX(1 1, 1.1 1.1)", false, false);
    test_circle<box_type>("BOX(2 1, 4 4)", false, false);
    test_circle<box_type>("BOX(-2 -2, 4 4)", false, true);
    test_circle<box_type>("BOX(-2.1 -2.1, 4.1 4.1)", true, true);
}

int test_main( int , char* [] )
{
    test_circles();

    return 0;
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    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.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      120116    B. Tong Minh      File added.
 *      120324    K. Kumar          Boostified unit tests; updated file header to new standard.
 *      120522    E. Heeren         Changed Tolerances.
 *
 *    References
 *
 *    Notes
 *
 */

#define BOOST_TEST_MAIN

#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "Tudat/Basics/testMacros.h"

#include "Tudat/Mathematics/BasicMathematics/numericalDerivative.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_basic_functions )

typedef Eigen::MatrixXd( *DerivativeCallback )( const Eigen::VectorXd& );
typedef Eigen::VectorXd( *FunctionEvaluationCallback )( const Eigen::VectorXd& );

//! Run a test case with the exponential density.
void compareAnalyticalAndNumericalDerivative( const Eigen::VectorXd& input,
                                              DerivativeCallback analyticalCallback,
                                              FunctionEvaluationCallback numericalCallback )
{
    using numerical_derivatives::computeCentralDifference;

    // Compute the expected partial.
    Eigen::MatrixXd analyticalDerivative = analyticalCallback( input );

    // Test 2nd-order numerical derivative.
    {
        Eigen::MatrixXd numericalDerivative = computeCentralDifference( input, numericalCallback );
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( analyticalDerivative, numericalDerivative,
                                           1.0e-7 );
    }

    // Test 4th-order numerical derivative.
    {
        Eigen::MatrixXd numericalDerivative = computeCentralDifference(
                    input, numericalCallback, 0.0, 0.0,
                    numerical_derivatives::order4 );
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( analyticalDerivative, numericalDerivative,
                                           1.0e-9 );

    }

    // Test 8th-order numerical derivative.
    {
        Eigen::MatrixXd numericalDerivative = computeCentralDifference(
                    input, numericalCallback, 0.0, 0.0,
                    numerical_derivatives::order8 );
        TUDAT_CHECK_MATRIX_CLOSE_FRACTION( analyticalDerivative, numericalDerivative,
                                           1.0e-9 );
    }
}

//! Callback that returns the analytical derivative of exp( ||r|| ).
Eigen::MatrixXd exponentialDensityAnalyticalDerivative( const Eigen::VectorXd& position )
{
    return position.transpose( ) / position.norm( ) * std::exp( position.norm( ) );
}

//! Callback to test numerical derivative calculation, returns exp( ||r|| ).
Eigen::VectorXd exponentialDensity( const Eigen::VectorXd& position )
{
    return Eigen::VectorXd::Constant( 1, std::exp( position.norm( ) ) );
}

//! Callback that returns the analytical derivative of -r/||r||.
Eigen::MatrixXd constantGravityAnalyticalDerivative( const Eigen::VectorXd& position )
{
    Eigen::MatrixXd partial = position * position.transpose( ) / std::pow( position.norm( ), 3.0 );
    partial.diagonal( ) = position.array( ).pow( 2.0 ) / std::pow( position.norm( ), 3.0 )
            - 1.0 / position.norm( );
    return partial;
}

//! Callback to test numerical derivative calculation, returns -r/||r||.
Eigen::VectorXd constantGravity( const Eigen::VectorXd& position )
{
    return -position / position.norm( );
}

BOOST_AUTO_TEST_CASE( testNumericalDerivatives )
{
    // Test numerical derivatives.
    Eigen::MatrixXd positions( 10, 3 );
    positions << 0.0416284088706, 0.365492068944, 0.805197604602,
                 1.63074391170,   8.04179355586,  6.74984731916,
                 55.4620045731,   86.8094364606,  95.4087064974,
                 41.3971344853,   80.6456253401,  359.560049206,
                 6389.36995846,   1891.72249537,  3768.41346114,
                 18357.5991764,   5355.13286809,  24582.5658116,
                 19887.9880951,   390769.463405,  949457.32454,
                 3634565.52581,   5564841.99331,  3208769.36002,
                 45156443.1799,   3463879.96686,  97241290.6455,
                 478348640.774,   6705325.08872,  335953979.068;

    // Loop through the input data and pass position data to check function.
    for ( int i = 0; i < positions.cols( ); i++ )
    {
        Eigen::VectorXd position = positions.block< 1, 3 >( i, 0 ).transpose( );

        compareAnalyticalAndNumericalDerivative( position,
                                                 &exponentialDensityAnalyticalDerivative,
                                                 &exponentialDensity );

        compareAnalyticalAndNumericalDerivative( position,
                                                 &constantGravityAnalyticalDerivative,
                                                 &constantGravity );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


/* Copyright (c) 2012-2019 Big Ladder Software LLC. All rights reserved.
 * See the LICENSE file for additional terms and conditions. */

#ifndef GEOMETRY_H_
#define GEOMETRY_H_

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/multi/geometries/multi_point.hpp>
#include <boost/geometry/multi/geometries/multi_polygon.hpp>

#include "Functions.hpp"
#include "libkiva_export.h"

namespace Kiva {

typedef boost::geometry::model::point<double, 2, boost::geometry::cs::cartesian> Point;
typedef boost::geometry::model::polygon<Point, true, false> Polygon;
typedef boost::geometry::model::ring<Point, true, false> Ring;
typedef boost::geometry::model::multi_polygon<Polygon> MultiPolygon;
typedef boost::geometry::model::multi_point<Point> MultiPoint;
typedef boost::geometry::model::linestring<Point> Line;
typedef boost::geometry::model::box<Point> Box;
typedef boost::geometry::model::point<double, 3, boost::geometry::cs::cartesian> Point3;
typedef boost::geometry::model::polygon<Point3, true, false> Polygon3;

namespace geom {
enum Direction { X_NEG, X_POS, Y_NEG, Y_POS };

enum Turn { LEFT, RIGHT };
} // namespace geom

bool isRectilinear(Polygon poly);
Polygon offset(Polygon poly, double dist);
geom::Direction getDirectionIn(Polygon poly, std::size_t vertex);
geom::Direction getDirectionOut(Polygon poly, std::size_t vertex);
geom::Turn getTurn(Polygon poly, std::size_t vertex);
MultiPolygon mirrorX(MultiPolygon poly, double x);
MultiPolygon mirrorY(MultiPolygon poly, double y);
Polygon symmetricUnit(Polygon poly);
bool isXSymmetric(Polygon poly);
bool isYSymmetric(Polygon poly);
double getXmin(Polygon poly, std::size_t vertex);
double getYmin(Polygon poly, std::size_t vertex);
double getXmax(Polygon poly, std::size_t vertex);
double getYmax(Polygon poly, std::size_t vertex);

bool LIBKIVA_EXPORT comparePointsX(Point first, Point second);
bool LIBKIVA_EXPORT comparePointsY(Point first, Point second);
bool LIBKIVA_EXPORT pointOnPoly(Point point, Polygon poly);
bool LIBKIVA_EXPORT isConvex(Polygon poly);

double LIBKIVA_EXPORT getDistance(Point a, Point b);

double getAngle(Point a, Point b, Point c);

} // namespace Kiva

#endif /* GEOMETRY_H_ */


#include <mlpack/core.hpp>
#include <mlpack/methods/cf/svd_wrapper.hpp>

#include <boost/test/unit_test.hpp>
#include "old_boost_test_definitions.hpp"

BOOST_AUTO_TEST_SUITE(ArmadilloSVDTest);

using namespace std;
using namespace mlpack;
using namespace mlpack::cf;
using namespace arma;

/**
 * Test armadillo SVD for normal factorization
 */
BOOST_AUTO_TEST_CASE(ArmadilloSVDNormalFactorizationTest)
{
  mat test = randu<mat>(20, 20);

  SVDWrapper<> svd;
  arma::mat W, H, sigma;
  double result = svd.Apply(test, W, sigma, H);

  BOOST_REQUIRE_LT(result, 0.01);

  test = randu<mat>(50, 50);
  result = svd.Apply(test, W, sigma, H);

  BOOST_REQUIRE_LT(result, 0.01);
}

/**
 * Test armadillo SVD for low rank matrix factorization
 */
BOOST_AUTO_TEST_CASE(ArmadilloSVDLowRankFactorizationTest)
{
  mat W_t = randu<mat>(30, 3);
  mat H_t = randu<mat>(3, 40);

  // create a row-rank matrix
  mat test = W_t * H_t;

  SVDWrapper<> svd;
  arma::mat W, H;
  double result = svd.Apply(test, 3, W, H);

  BOOST_REQUIRE_LT(result, 0.01);
}

BOOST_AUTO_TEST_SUITE_END();


#define CATCH_CONFIG_MAIN

#include "CALPHADFreeEnergyFunctionsBinary.h"
#include "InterpolationType.h"

#include "catch.hpp"

#include <boost/optional/optional.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>

#include <string>

#include <omp.h>

namespace pt = boost::property_tree;

TEST_CASE("CALPHAD binary kks in a loop", "[binary kks loop]")
{
    std::cout << "Run test with " << omp_get_max_threads() << " threads"
              << std::endl;

    Thermo4PFM::EnergyInterpolationType energy_interp_func_type
        = Thermo4PFM::EnergyInterpolationType::PBG;
    Thermo4PFM::ConcInterpolationType conc_interp_func_type
        = Thermo4PFM::ConcInterpolationType::PBG;

    const double Tmin     = 1300.;
    const double Tmax     = 1500.;
    const int nTintervals = 10;
    const double deltaT   = (Tmax - Tmin) / (double)nTintervals;

    std::cout << " Read CALPHAD database..." << std::endl;
    pt::ptree calphad_db;
    try
    {
        pt::read_json("../thermodynamic_data/calphadAuNi.json", calphad_db);
    }
    catch (std::exception& e)
    {
        std::cerr << "exception caught: " << e.what() << std::endl;
    }

    boost::optional<pt::ptree&> newton_db;

    // nominal concentration for which to solve KKS eqs.
    const double nominalc = 0.2;

    // initial guesses
    const double init_guess[2] = { 0.5, 0.5 };

    std::vector<double> cl(nTintervals + 1);
    std::vector<double> cs(nTintervals + 1);

    Thermo4PFM::CALPHADFreeEnergyFunctionsBinary* cafe
        = new Thermo4PFM::CALPHADFreeEnergyFunctionsBinary(calphad_db,
            newton_db, energy_interp_func_type, conc_interp_func_type);

    {
        // serial loop
        for (int i = 0; i < nTintervals + 1; i++)
        {
            const double temperature = Tmin + i * deltaT;

            double phi = 0.5;
            double conc[2];

            // compute concentrations in each phase
            cafe->computePhaseConcentrations(
                temperature, &nominalc, &phi, conc);

            std::cout << "Temperature = " << temperature << std::endl;
            std::cout << "Concentrations: cl = " << conc[0]
                      << " and cs = " << conc[1] << "..." << std::endl;

            cl[i] = conc[0];
            cs[i] = conc[1];
        }
    }

    std::vector<short> nitscpu(nTintervals + 1);

// parallel loop
#pragma omp parallel for
    for (int i = 0; i < nTintervals + 1; i++)
    {
        const double temperature = Tmin + i * deltaT;

        double phi     = 0.5;
        double conc[2] = { init_guess[0], init_guess[1] };

        // compute concentrations in each phase
        nitscpu[i] = cafe->computePhaseConcentrations(
            temperature, &nominalc, &phi, conc);

        CHECK(conc[0] == Approx(cl[i]).margin(1.e-6));
        CHECK(conc[1] == Approx(cs[i]).margin(1.e-6));
    }

    short* nits  = new short[nTintervals + 1];
    double* xdev = new double[2 * (nTintervals + 1)];

// clang-format off
#pragma omp target map(to : cafe [0:1]) \
                   map(to : init_guess[:2]) \
                   map(from : xdev[:2 * (nTintervals + 1)]) \
                   map(from : nits[:nTintervals + 1])
// clang-format on
#pragma omp parallel for
    for (int i = 0; i < nTintervals + 1; i++)
    {
        const double temperature = Tmin + i * deltaT;

        double phi = 0.5;
        double c0  = nominalc;

        xdev[2 * i]     = init_guess[0];
        xdev[2 * i + 1] = init_guess[1];

        // compute concentrations in each phase
        nits[i] = cafe->computePhaseConcentrations(
            temperature, &c0, &phi, &xdev[2 * i]);
    }

    for (int i = 0; i < nTintervals + 1; i++)
    {
        std::cout << "Number of Newton iterations: " << nits[i] << std::endl;
        std::cout << "Device: x=" << xdev[2 * i] << "," << xdev[2 * i + 1]
                  << std::endl;
        CHECK(xdev[2 * i] == Approx(cl[i]).margin(1.e-6));
        CHECK(xdev[2 * i + 1] == Approx(cs[i]).margin(1.e-6));
        CHECK(nits[i] == nitscpu[i]);
    }
    delete[] nits;
    delete[] xdev;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Example: Affine Transformation (translate, scale, rotate)
//
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.
//
// 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)

#include <ctime> // for std::time
#include <algorithm>
#include <fstream>
#include <iostream>
#include <limits>
#include <sstream>

#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/algorithms/centroid.hpp>
#include <boost/geometry/strategies/transform.hpp>
#include <boost/geometry/strategies/transform/matrix_transformers.hpp>
#include <boost/geometry/io/wkt/read.hpp>

#if defined(HAVE_SVG)
#  include <boost/geometry/io/svg/write_svg.hpp>
#endif

#include <boost/bind.hpp>
#include <boost/random.hpp>
#include <boost/range.hpp>
#include <boost/shared_ptr.hpp>

using namespace boost::geometry;

struct random_style
{
    random_style()
        : rng(static_cast<int>(std::time(0))), dist(0, 255), colour(rng, dist)
    {}

    std::string fill(double opacity = 1)
    {
        std::ostringstream oss;
        oss << "fill:rgba(" << colour() << "," << colour() << "," << colour() << "," << opacity << ");";
        return oss.str();
    }

    std::string stroke(int width, double opacity = 1)
    {
        std::ostringstream oss;
        oss << "stroke:rgba(" << colour() << "," << colour() << "," << colour() << "," << opacity << ");";
        oss << "stroke-width:" << width  << ";";
        return oss.str();
    }

    template <typename T>
    std::string text(T x, T y, std::string const& text)
    {
        std::ostringstream oss;
        oss << "<text x=\"" << static_cast<int>(x) - 90 << "\" y=\"" << static_cast<int>(y) << "\" font-family=\"Verdana\">" << text << "</text>";
        return oss.str();
    }

    boost::mt19937 rng;
    boost::uniform_int<> dist;
    boost::variate_generator<boost::mt19937&, boost::uniform_int<> > colour;
};

template <typename OutputStream>
struct svg_output
{
    svg_output(OutputStream& os, double opacity = 1) : os(os), opacity(opacity)
    {
        os << "<?xml version=\"1.0\" standalone=\"no\"?>\n"
            << "<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\"\n"
            << "\"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n"
            << "<svg width=\"100%\" height=\"100%\" version=\"1.1\"\n"
            << "xmlns=\"http://www.w3.org/2000/svg\">" << std::endl;
    }

    ~svg_output()
    {
        os << "</svg>" << std::endl;
    }

    template <typename G>
    void put(G const& g, std::string const& label)
    {
        std::string style_str(style.fill(opacity) + style.stroke(5, opacity));
#if defined(HAVE_SVG)
        os << boost::geometry::svg(g, style_str) << std::endl;
#endif
        if (!label.empty())
        {
            typename point_type<G>::type c;
            centroid(g, c);
            os << style.text(static_cast<int>(get<0>(c)), static_cast<int>(get<1>(c)), label);
        }
    }

private:

    OutputStream& os;
    double opacity;
    random_style style;
};

int main()
{
    using namespace boost::geometry::strategy::transform;

    typedef boost::geometry::model::d2::point_xy<double> point_2d;

    try
    {
        std::string file("06_b_transformation_example.svg");
        std::ofstream ofs(file.c_str());
        svg_output<std::ofstream> svg(ofs, 0.5);

        // G1 - create subject for affine transformations
        model::polygon<point_2d> g1;
        read_wkt("POLYGON((50 250, 400 250, 150 50, 50 250))", g1);
        std::clog << "source box:\t" << boost::geometry::dsv(g1) << std::endl;
        svg.put(g1, "g1");

        // G1 - Translate -> G2
        translate_transformer<double, 2, 2> translate(0, 250);
        model::polygon<point_2d> g2;
        transform(g1, g2, translate);
        std::clog << "translated:\t" << boost::geometry::dsv(g2) << std::endl;
        svg.put(g2, "g2=g1.translate(0,250)");

        // G2 - Scale -> G3
        scale_transformer<double, 2, 2> scale(0.5, 0.5);
        model::polygon<point_2d> g3;
        transform(g2, g3, scale);
        std::clog << "scaled:\t" << boost::geometry::dsv(g3) << std::endl;
        svg.put(g3, "g3=g2.scale(0.5,0.5)");

        // G3 - Combine rotate and translate -> G4
        rotate_transformer<degree, double, 2, 2> rotate(45);

        // Compose matrix for the two transformation
        // Create transformer attached to the transformation matrix
        ublas_transformer<double, 2, 2>
                combined(boost::numeric::ublas::prod(rotate.matrix(), translate.matrix()));
                //combined(rotate.matrix());

        // Apply transformation to subject geometry point-by-point
        model::polygon<point_2d> g4;
        transform(g3, g4, combined);

        std::clog << "rotated & translated:\t" << boost::geometry::dsv(g4) << std::endl;
        svg.put(g4, "g4 = g3.(rotate(45) * translate(0,250))");

        std::clog << "Saved SVG file:\t" << file << std::endl;
    }
    catch (std::exception const& e)
    {
        std::cerr << e.what() << std::endl;
    }
    catch (...)
    {
        std::cerr << "unknown error" << std::endl;
    }
    return 0;
}


/**
 * @file intrinsics.hpp
 * @copyright Copyright (c) 2020 Nicolas Pope, MIT License
 * @author Nicolas Pope
 */

#pragma once

#include "../defines.hpp"

#include <Eigen/Eigen>

namespace voltu
{

/** Intrinsic camera paramters */
struct Intrinsics
{
	unsigned int width;
	unsigned int height;
	float principle_x;
	float principle_y;
	float focal_x;
	float focal_y;

	/** Projection matrix */
	PY_API Eigen::Matrix3d matrix();
	/** Size (width, height) */
	PY_API Eigen::Vector2i size();
};

/** Stereo camera intrinsic parameters.
 *
 * Baseline can be used to estimate depth accuracy for known depth. Assuming 1px
 * disparity accuracy, accuracy of given depth z, the accuracy of depth is
 * z^2/(f*T), where f is focal length and T baseline.
 */
struct StereoIntrinsics : public Intrinsics
{
	float baseline;
	float min_depth;
	float max_depth;
};

}


#include <iostream>
#include <Eigen/Dense>

using Eigen::Matrix3f;
using Eigen::Matrix3d;
using namespace std;

void PrintMat(const Matrix3d& m) {
  cout << "Matrix : " << m << endl;
}

Eigen::Matrix3f GetMat() {
  Matrix3f m;
  m << 1, 2, 3,
       4, 5, 6,
       7, 8, 9;
  return m;
}

char const* Greet() {
  return "Hello, world ! ";
}


#include <boost/program_options.hpp>
#include <cmath>
#include <complex>
#include <cstdint>
#include <omp.h>
#include <opencv2/opencv.hpp>
#include <vector>

using complex = std::complex<double>;
constexpr double PI = 3.14159'26535'89793'23846'26433'83279'50288;

std::uint8_t colu(double t0) {
  double t = std::fmod(std::fmod(t0, 3.0) + 3.0, 3.0);
  if (t < 2) {
    return static_cast<std::uint8_t>(
        std::lround((1 - std::cos(t * PI)) * (255 / 2.0)));
  } else {
    return 0;
  }
}

cv::Vec3b color(double t0) {
  double t = t0;
  return {colu(t), colu(t + 1), colu(t + 2)};
}

double abslog(double x) {
  if (x == 0) {
    return 0;
  }
  auto l = std::log(std::abs(x) + 1);
  auto sign = x < 0 ? -1 : 1;
  return l * sign;
}

class mandel_maker {
  cv::Vec3d pix_at(double x, double y) {
    complex z{0.0, 0.0};
    auto c = complex{x, y};
    std::vector<cv::Point2f> points;
    points.reserve(rep);
    constexpr double LIMIT = 20;
    for (int ix = 0; ix < rep; ++ix) {
      z = z*z*z/(std::abs(z)+1e-10)+c;
      if (LIMIT < std::abs(z)) {
        return {static_cast<double>(ix), std::arg(z), std::abs(z)};
      }
      points.emplace_back(cv::Point2f(abslog(real(z)), abslog(imag(z))));
    }
    std::vector<cv::Point2f> hull;
    cv::convexHull(points, hull);
    auto area = cv::contourArea(hull);
    return {-1, std::arg(z), area};
  }
  int rep;
  int w, h;
  double x0, dx;
  double y0, dy;

public:
  explicit mandel_maker(int rep_, int w_, int h_, double x0_, double x1_,
                        double y0_)
      : rep(rep_), w(w_), h(h_), x0(x0_), dx((x1_ - x0_) / w_), y0(y0_),
        dy((x1_ - x0_) * h_ / w_ / h_) {}
  cv::Mat make() {
    cv::Mat im = cv::Mat::zeros(w, h, CV_64FC3);
#pragma omp parallel for
    for (int iy = 0; iy < h; ++iy) {
      double y = y0 + dy * iy;
      for (int ix = 0; ix < w; ++ix) {
        double x = x0 + ix * dx;
        auto col = pix_at(x, y);
        im.at<cv::Vec3d>(iy, ix) = col;
      }
    }
    return im;
  }
};

cv::Rect2d rect(char const *cmd) {
  double w0 = 1 << 5;
  cv::Rect2d r(-w0 / 2, -w0 / 2, w0, w0);
  for (; *cmd; ++cmd) {
    if (*cmd=='s'){
      auto w = r.width;
      auto h = r.height;
      auto dw = w/10;
      auto dh = h/10;
      r = cv::Rect2d( r.x+dw, r.y+dh, w-dw*2, h-dh*2 );
      continue;
    }
    double w = r.width / 2;
    double h = r.height / 2;
    double x, y;
    int c = *cmd - '0';
    switch (c % 3) {
    case 0:
      x = r.x;
      break;
    case 1:
      x = r.x + w / 2;
      break;
    case 2:
      x = r.x + w;
      break;
    }
    switch (c / 3) {
    case 0:
      y = r.y;
      break;
    case 1:
      y = r.y + h / 2;
      break;
    case 2:
      y = r.y + h;
      break;
    }
    r = cv::Rect2d(x, y, w, h);
  }
  return r;
}

void save_image(char const *filename, cv::Mat const &im) {
  cv::FileStorage fs(filename, cv::FileStorage::WRITE);
  fs << "image" << im;
}

std::pair<std::int32_t, std::int32_t> find_minmax(cv::Mat const &im) {
  int min = INT32_MAX;
  int max = INT32_MIN;
  auto end = im.end<std::int32_t>();
  for (auto it = im.begin<std::int32_t>(); it != end; ++it) {
    int32_t col = *it;
    if (col < 0) {
      continue;
    }
    if (col < min) {
      min = col;
    }
    if (max < col) {
      max = col;
    }
  }
  return {min, max};
}

cv::Mat colorize(cv::Mat const &src) {
  cv::Mat dest = cv::Mat::zeros(src.rows, src.cols, CV_8UC3);
#pragma omp parallel for
  for (int y = 0; y < src.rows; ++y) {
    for (int x = 0; x < src.cols; ++x) {
      auto col{src.at<cv::Vec3d>(y, x)};
      if (col[0] < 0) {
        // auto c0 = color((col[2]*0.1 + col[1] / (PI * 2) * 3 + 3) * 3) / 2;
        auto c0 = color(std::log(col[2]));
        dest.at<cv::Vec3b>(y, x) = c0 / 2;
      } else {
        auto c0 = cv::Vec3b{255, 255, 255} - color(col[1] / (PI * 2) * 3 + 3);
        auto c1 = cv::Vec3b{255, 255, 255} - color(col[0] * 0.006 + 1);
        constexpr double W = 0.2;
        dest.at<cv::Vec3b>(y, x) = c0 * W + c1 * (1 - W);
      }
    }
  }
  return dest;
}

namespace po = boost::program_options;

po::options_description calc_options() {
  po::options_description desc("calc options");
  desc.add_options()                                                     //
      ("help", "produce help message")                                   //
      ("size,s", po::value<double>()->default_value(60), "size in mm")   //
      ("dpi,d", po::value<double>()->default_value(96), "DPI")           //
      ("rep,r", po::value<int>()->default_value(1000), "repeat count")   //
      ("pos,p", po::value<std::string>()->default_value(""), "position") //
      ;
  return desc;
}

po::options_description col_options() {
  po::options_description desc("col options");
  desc.add_options()                   //
      ("help", "produce help message") //
      ("infile,i", po::value<std::string>()->default_value("data/hoge.yaml"),
       "input yaml file") //
      ("outfile,o", po::value<std::string>()->default_value("data/hoge.png"),
       "output image file") //
      ;
  return desc;
}

int calc(int argc, char const *argv[]) {
  auto desc = calc_options();
  try {
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
      std::cout << desc << "\n";
      return 0;
    }
    double realsize = vm["size"].as<double>();
    double dpi = vm["dpi"].as<double>();
    auto pix = lround(realsize / 25.4 * dpi);
    omp_set_num_threads(16);
    int rep = vm["rep"].as<int>();
    cv::Rect2d rc = rect(vm["pos"].as<std::string>().c_str());
    auto mm = mandel_maker(rep, pix, pix, rc.x, rc.br().x, rc.y);
    cv::Mat im = mm.make();
    cv::imwrite("data/hoge.png", colorize(im));
    save_image("data/hoge.yaml", im);
    return 0;
  } catch (po::error &e) {
    std::cout << e.what() << std::endl;
    return 0;
  }
}

int colorize(int argc, char const *argv[]) {
  auto desc = col_options();
  try {
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
      std::cout << desc << "\n";
      return 0;
    }
    auto infile = vm["infile"].as<std::string>();
    auto outfile = vm["outfile"].as<std::string>();
    std::cout << infile << ", " << outfile << "\n";

    cv::FileStorage fs(infile, cv::FileStorage::READ);
    cv::Mat im;
    fs["image"] >> im;
    cv::imwrite(outfile, colorize(im));
    return 0;
  } catch (po::error &e) {
    std::cout << e.what() << std::endl;
    return 0;
  }
}
void show_help() {
  std::cout << calc_options() << "\n" << col_options() << "\n";
}

int main(int argc, char const *argv[]) {
  if (argc < 2) {
    show_help();
    return 0;
  }
  auto cmd = std::string(argv[1]);
  if (cmd == "calc") {
    return calc(argc - 1, argv + 1);
  } else if (cmd == "col") {
    return colorize(argc - 1, argv + 1);
  } else {
    show_help();
    return 0;
  }
}


#pragma once
/*!	\file	integer.hpp
	\brief	Integer class declaration.
	\author	Garth Santor
	\date	2021-10-29
	\copyright	Garth Santor, Trinh Han

=============================================================
Declarations of the Integer class derived from Operand.

=============================================================
Revision History
-------------------------------------------------------------

Version 2021.10.02
	C++ 20 validated

Version 2019.11.05
	C++ 17 cleanup

Version 2014.10.29
	C++ 11 refactor.
	Converted Integer::value_type to boost::multiprecision::cpp_int
	Removed BinaryInteger

Version 2012.11.15
	Added BinaryInteger.

Version 2012.11.13
	C++ 11 cleanup.

Version 2010.11.09
	Switched boost::shared_ptr<> to std::shared_ptr<>.
	Added TOKEN_PTR_TYPE macro.
	Switched __int64 to long long.

Version 2009.11.25
	Alpha release.

=============================================================

Copyright Garth Santor/Trinh Han

The copyright to the computer program(s) herein
is the property of Garth Santor/Trinh Han, Canada.
The program(s) may be used and/or copied only with
the written permission of Garth Santor/Trinh Han
or in accordance with the terms and conditions
stipulated in the agreement/contract under which
the program(s) have been supplied.
=============================================================*/

#include <ee/operand.hpp>
#include <boost/multiprecision/cpp_int.hpp>

/*! Integer token. */
class Integer : public Operand {
public:
	using value_type = boost::multiprecision::cpp_int;
	DEF_POINTER_TYPE(Integer)
private:
	value_type	value_;
public:
	Integer( value_type value = 0 )
		: value_( value ) { }

	[[nodiscard]]	value_type	value() const { return value_; }
	[[nodiscard]]	string_type	str() const override;
};


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>
#include <limits>

TEST(MathFunctions, inv_cloglog) {
  EXPECT_EQ(1 - std::exp(-std::exp(3.7)), stan::math::inv_cloglog(3.7));
  EXPECT_EQ(1 - std::exp(-std::exp(0.0)), stan::math::inv_cloglog(0.0));
  EXPECT_EQ(1 - std::exp(-std::exp(-2.93)), stan::math::inv_cloglog(-2.93));
}

TEST(MathFunctions, inv_cloglog_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_PRED1(boost::math::isnan<double>, stan::math::inv_cloglog(nan));
}


#pragma once

// Eigen
#include <Eigen/Core>
#include <Eigen/Dense>
#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/NumericalDiff>

// std
#include <iostream>
#include "darkroom/Sensor.hpp"

using namespace Eigen;
using namespace std;

namespace InYourGibbousPhase2 {
// Generic functor for Eigen Levenberg-Marquardt minimizer
    template<typename _Scalar, int NX = Dynamic, int NY = Dynamic>
    struct Functor {
        typedef _Scalar Scalar;
        enum {
            InputsAtCompileTime = NX,
            ValuesAtCompileTime = NY
        };
        typedef Eigen::Matrix<Scalar, InputsAtCompileTime, 1> InputType;
        typedef Eigen::Matrix<Scalar, ValuesAtCompileTime, 1> ValueType;
        typedef Eigen::Matrix<Scalar, ValuesAtCompileTime, InputsAtCompileTime> JacobianType;

        const int m_inputs, m_values;

        Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}

        Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}

        int inputs() const { return m_inputs; }

        int values() const { return m_values; }
    };

    struct InYourGibbousPhase2 : Functor<double> {
        /**
         * Default amount of sensors needed for Eigen templated structure
         * @param numberOfSensors you can however choose any number of sensors here
         */
        InYourGibbousPhase2(int trajectoryPoints = 4);

        /**
         * This is the function that is called in each iteration
         * @param x contains the calibration values [phase0 phase1 tilt0 tilt1 curve0 curve1 gibphase0 gibphase1 gibmag0 gibmag1]
         * @param fvec the error function (the difference between the sensor positions)
         * @return
         */
        int operator()(const VectorXd &x, VectorXd &fvec) const;

        vector<vector<Vector2d>> angles_measured_trajectory;
        vector<vector<Vector3d>> relative_positions_trajectory;
        double distance_y = 2.0;
        int trajectoryPoints = 4;
        bool lighthouse;
        enum {phase_horizontal , phase_vertical, tilt_horizontal, tilt_vertical, curve_horizontal, curve_vertical,
            gibphase_horizontal, gibphase_vertical, gibmag_horizontal, gibmag_vertical};
    };
}


#define NOMINMAX
#include "interactions.hpp"
#include "vsh_translation.hpp"
#include "indices.hpp"
#include <cmath>
#include <Eigen/IterativeLinearSolvers>
#include <omp.h>

using std::complex;
using namespace std::complex_literals;

using Eigen::Vector3d;

ComplexVector matrix_vector_product(const Ref<const ComplexMatrix>& A, const Ref<const ComplexVector>& x) {
    int size = x.size();
    ComplexVector ret(size);
#pragma omp parallel for
    for (int i = 0; i < size; i++) {
        ret(i) = A.row(i)*x;
    }

    return ret;
}

complex<double> dot_product(const Ref<const ComplexVector>& x, const Ref<const ComplexVector>& y) {
    int size = x.size();
    double real_part = 0;
    double imag_part = 0;
#pragma omp parallel for reduction(+:real_part,imag_part)
    for (int i = 0; i < size; i++) {
        real_part += x(i).real()*y(i).real() + x(i).imag()*y(i).imag();
        imag_part += x(i).real()*y(i).imag() - x(i).imag()*y(i).real();
    }

    return std::complex<double>(real_part, imag_part);
}

ComplexVector bicgstab(const Ref<const ComplexMatrix>& A, const Ref<const ComplexVector>& b,
        int maxiter, double tolerance) {

    int size = b.size();

    // step 1
    ComplexVector x_prev = b;
    ComplexVector r_prev = b - matrix_vector_product(A, x_prev);

    double error = (r_prev).norm();
    if (error < tolerance)
        return x_prev;

    // step 2
    ComplexVector r_hat = r_prev;

    // step 3
    complex<double> rho_prev = 1;
    complex<double> alpha    = 1;
    complex<double> w_prev   = 1;

    // step 4
    ComplexVector v_prev = ComplexVector::Zero(size);
    ComplexVector p_prev = ComplexVector::Zero(size);

    // step 5
    int current_iteration = 1;

    while (true) {
        //complex<double> rho_i = r_hat.dot(r_prev);
        complex<double> rho_i = dot_product(r_hat, r_prev);
        complex<double> beta = (rho_i/rho_prev)*(alpha/w_prev);
        ComplexVector pi = r_prev + beta*(p_prev - w_prev*v_prev);
        ComplexVector vi = matrix_vector_product(A, pi);

        //alpha = rho_i/r_hat.dot(vi);
        alpha = rho_i/dot_product(r_hat, vi);
        ComplexVector h = x_prev + alpha*pi;

        ComplexVector s = r_prev - alpha*vi;
        ComplexVector t = matrix_vector_product(A, s);
        //complex<double> w_i = t.dot(s)/t.dot(t);
        complex<double> w_i = dot_product(t,s)/dot_product(t,t);
        ComplexVector xi = h + w_i*s;
        ComplexVector ri = s - w_i*t;

        error = ri.norm();
        if (error < tolerance || current_iteration > maxiter)
            return xi;

        x_prev   = xi;
        r_prev   = ri;
        rho_prev = rho_i;
        v_prev   = vi;
        p_prev   = pi;
        w_prev   = w_i;

        current_iteration += 1;
    }
}

ComplexVector solve_linear_system(const Ref<const ComplexMatrix>& agg_tmatrix,
        const Ref<const ComplexVector>& p_src, solver method) {

    ComplexMatrix interaction_matrix = agg_tmatrix;
    for (int i = 0; i < interaction_matrix.cols(); i++)
        interaction_matrix(i,i) += 1;

    switch (method) {
        case solver::exact:  // TODO: implment
        default:
        case solver::bicgstab:
            //Eigen::BiCGSTAB<ComplexMatrix> solver;
            //solver.setTolerance(1e-5);
            //solver.compute(interaction_matrix);
            //return solver.solveWithGuess(p_src, p_src);
            return bicgstab(interaction_matrix, p_src);
    }
}

ComplexMatrix sphere_aggregate_tmatrix(const Ref<const position_t>& positions,
        const Ref<const ComplexMatrix>& mie, double k) {

    int lmax = mie.cols()/2;
    int rmax = lmax_to_rmax(lmax);
    int Nparticles = positions.rows();
    int size = 2*rmax*Nparticles;

    ComplexMatrix agg_tmatrix = ComplexMatrix::Zero(size, size);

    if (Nparticles == 1)
        return agg_tmatrix;

    int N = Nparticles*(Nparticles-1)/2;
    Array ivals(N);
    Array jvals(N);
    int counter = 0;

    for (int i = 0; i < Nparticles; i++) {
        for (int j = i+1; j < Nparticles; j++) {
            ivals(counter) = i;
            jvals(counter) = j;
            counter += 1;
        }
    }

    auto vsh_precompute = create_vsh_cache_map(lmax);

    #pragma omp parallel for
    for (int ij = 0; ij < N; ij++) {
        int i = ivals(ij);
        int j = jvals(ij);

        Vector3d dji = positions.row(i) - positions.row(j);

        double rad = dji.norm();
        double theta = acos(dji(2)/rad);
        double phi = atan2(dji(1), dji(0));

        vsh_translation_insert_pair(agg_tmatrix, mie, i, j, rad, theta, phi, k, vsh_precompute);
    }

    return agg_tmatrix;
}

ComplexMatrix particle_aggregate_tmatrix(const Ref<const position_t>& positions,
        const tmatrix_t& tmatrix, double k) {

    int rmax = tmatrix.dimensions()[1]/2;
    int lmax = rmax_to_lmax(rmax);

    int Nparticles = positions.rows();
    int size = 2*rmax*Nparticles;

    ComplexMatrix agg_tmatrix = ComplexMatrix::Zero(size, size);

    if (Nparticles == 1)
        return agg_tmatrix;

    int N = Nparticles*(Nparticles-1)/2;
    Array ivals(N);
    Array jvals(N);
    int counter = 0;

    for (int i = 0; i < Nparticles; i++) {
        for (int j = i+1; j < Nparticles; j++) {
            ivals(counter) = i;
            jvals(counter) = j;
            counter += 1;
        }
    }

    auto vsh_precompute = create_vsh_cache_map(lmax);

    #pragma omp parallel for
    for (int ij = 0; ij < N; ij++) {
        int i = ivals(ij);
        int j = jvals(ij);

        Vector3d dji = positions.row(i) - positions.row(j);

        double rad = dji.norm();
        double theta = acos(dji(2)/rad);
        double phi = atan2(dji(1), dji(0));

        vsh_translation_insert_pair(agg_tmatrix, tmatrix, i, j, rad, theta, phi, k, vsh_precompute);
    }

    return agg_tmatrix;
}

ComplexMatrix reflection_matrix_nia(const Ref<const position_t>& positions,
        const Ref<const ComplexMatrix>& mie, double k, complex<double> reflection, double z) {

    int lmax = mie.cols()/2;
    int rmax = lmax_to_rmax(lmax);
    int Nparticles = positions.rows();
    int size = 2*rmax*Nparticles;

    ComplexMatrix R_matrix = ComplexMatrix::Zero(size, size);

    for (int i = 0; i < Nparticles; i++) {
        Vector3d pi = positions.row(i);
        pi(2) -= 2*(pi(2) - z);
        for (int j = 0; j < Nparticles; j++) {
            Vector3d pj = positions.row(j);
            Vector3d dji = pi - pj;

            double rad = dji.norm();
            double theta = acos(dji(2)/rad);
            double phi = atan2(dji(1), dji(0));

            for (int n = 1; n < lmax+1; n++) {
                for (int m = -n; m < n+1; m++) {
                    for (int v = 1; v < lmax+1; v++) {
                        for (int u = -v; u < v+1; u++) {
                            auto transfer = vsh_translation(m, n, u, v, rad, theta, phi, k, vsh_mode::outgoing);
                            for (int a = 0; a < 2; a++) {
                                for (int b = 0; b < 2; b++) {
                                    complex<double> factor = -reflection*pow(-1, m + n + a + 1);
                                    complex<double> val = transfer[(a+b)%2];
                                    int idx = i*(2*rmax) + a*(rmax) + n*(n+2) - n + m - 1;
                                    int idy = j*(2*rmax) + b*(rmax) + v*(v+2) - v + u - 1;
                                    R_matrix(idx, idy) = factor*val*mie(j, b*lmax + v-1);
                                }
                            }
                        }
                    }
                }
            }
        }
    }

    return R_matrix;
}


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_basic_types.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

BOOST_AUTO_TEST_SUITE(opentissue_math_quaternion_rotate);

BOOST_AUTO_TEST_CASE(simple_test)
{
  typedef OpenTissue::math::BasicMathTypes<double,size_t> math_types;

  typedef math_types::value_traits     value_traits;
  typedef math_types::real_type        T;
  typedef math_types::vector3_type     V;
  typedef math_types::quaternion_type  Q;

  {
    // Set up a test rotation
    Q q;
    T const phi = value_traits::pi_half();
    V const m = V( 1.0, 0.0, 0.0 );
    q.Ru( phi, m);
    V const n = V( 0.0, 1.0, 0.0 );
    // Try to rotate n-vector
    V const k = rotate( q, n);
    // Test if result is what we expect
    BOOST_CHECK( fabs( k(0) ) < 10e-10 );
    BOOST_CHECK( fabs( k(1) ) < 10e-10 );
    BOOST_CHECK( fabs( 1.0 - k(2) ) < 10e-10 );
  }
  {
    // Set up a test rotation
    Q q;
    T const phi = value_traits::pi_half();
    V const m = V( 1.0, 0.0, 0.0 );
    q.Ru( phi, m);
    V const n = V( 0.0, 0.0, 1.0 );
    // Try to rotate n-vector
    V const k = rotate( q, n);
    // Test if result is what we expect
    BOOST_CHECK( fabs( k(0) ) < 10e-10 );
    BOOST_CHECK( fabs( 1.0 + k(1) ) < 10e-10 );
    BOOST_CHECK( fabs( k(2) ) < 10e-10 );
  }
  {
    // Set up a test rotation
    Q q;
    T const phi = value_traits::pi_half();
    V const m = V( 1.0, 0.0, 0.0 );
    q.Ru( phi, m);
    V const n = V( 0.0, -1.0, 0.0 );
    // Try to rotate n-vector
    V const k = rotate( q, n);
    // Test if result is what we expect
    BOOST_CHECK( fabs( k(0) ) < 10e-10 );
    BOOST_CHECK( fabs( k(1) ) < 10e-10 );
    BOOST_CHECK( fabs( 1.0 + k(2) ) < 10e-10 );
  }
  {
    // Set up a test rotation
    Q q;
    T const phi = value_traits::pi_half();
    V const m = V( 1.0, 0.0, 0.0 );
    q.Ru( phi, m);
    V const n = V( 0.0, 0.0, -1.0 );
    // Try to rotate n-vector
    V const k = rotate( q, n);
    // Test if result is what we expect
    BOOST_CHECK( fabs( k(0) ) < 10e-10 );
    BOOST_CHECK( fabs( 1.0 - k(1) ) < 10e-10 );
    BOOST_CHECK( fabs( k(2) ) < 10e-10 );
  }
  {
    // Set up a test rotation
    Q q;
    T const phi = value_traits::pi_half();
    V const m = V( 0.0, 1.0, 0.0 );
    q.Ru( phi, m);
    V const n = V( 1.0, 0.0, 0.0 );
    // Try to rotate n-vector
    V const k = rotate( q, n);
    // Test if result is what we expect
    BOOST_CHECK( fabs( k(0) ) < 10e-10 );
    BOOST_CHECK( fabs( k(1) ) < 10e-10 );
    BOOST_CHECK( fabs( 1.0 + k(2) ) < 10e-10 );
  }
  {
    // Set up a test rotation
    Q q;
    T const phi = value_traits::pi_half();
    V const m = V( 0.0, 0.0, 1.0 );
    q.Ru( phi, m);
    V const n = V( 1.0, 0.0, 0.0 );
    // Try to rotate n-vector
    V const k = rotate( q, n);
    // Test if result is what we expect
    BOOST_CHECK( fabs( k(0) ) < 10e-10 );
    BOOST_CHECK( fabs( 1.0 - k(1) ) < 10e-10 );
    BOOST_CHECK( fabs( k(2) ) < 10e-10 );
  }
}

BOOST_AUTO_TEST_SUITE_END();


// Copyright (c) Microsoft Corporation.
// Licensed under the MIT License.

#include "PoseInterpolator.h"

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

#include "Utils/cv.h"
#include <boost/optional.hpp>

#include <arcana/analysis/object_trace.h>
#include <arcana/analysis/data_point.h>

namespace E = Eigen;

namespace mage
{
    using seconds = std::chrono::duration<double>;

    struct alignas(16) PoseInterpolator::Impl
    {
        mage::SensorSample::Timestamp m_gyroTime{};
        mage::SensorSample::Timestamp m_accelTime{};

        mage::Pose m_previousPose;

        std::vector<std::pair<E::Vector3d, mage::SensorSample::Timestamp>, E::aligned_allocator<E::Vector3d>> m_positionHistory;

        Eigen::Vector3d Acceleration = Eigen::Vector3d::Zero();
        Eigen::Vector3d Velocity = Eigen::Vector3d::Zero();
        Eigen::Vector3d Position = Eigen::Vector3d::Zero();

        E::AngleAxisd AngularVelocity = E::AngleAxisd::Identity();
        Eigen::Quaterniond Orientation = Eigen::Quaterniond::Identity();

        void ProcessAngularVelocityUntil(mage::SensorSample::Timestamp ts)
        {
            if (m_gyroTime == mage::SensorSample::Timestamp{})
                m_gyroTime = ts;

            seconds dt = ts - m_gyroTime;

            E::AngleAxisd av = AngularVelocity;
            av.angle() *= dt.count();

            Orientation = Orientation * av;
            Orientation.normalize();

            m_gyroTime = ts;
        }

        void ProcessLinearVelocityUntil(mage::SensorSample::Timestamp ts)
        {
            if (m_accelTime == mage::SensorSample::Timestamp{})
                m_accelTime = ts;

            seconds dt = ts - m_accelTime;

            Position += Velocity * dt.count();

            m_accelTime = ts;
        }

        void ProcessPose(mage::SensorSample::Timestamp ts, const mage::Pose& pose)
        {
            assert(m_positionHistory.empty() || ts >= m_positionHistory.back().second);

            ProcessAngularVelocityUntil(ts);
            ProcessLinearVelocityUntil(ts);

            auto transform = Decompose(pose.GetInverseViewMatrix());

            Orientation = transform.second.cast<double>();
            Position = ToMap(transform.first).cast<double>();

            if (!m_positionHistory.empty())
            {
                seconds dt = ts - m_positionHistory.back().second;
                auto previous = Decompose(m_previousPose.GetInverseViewMatrix());

                E::Vector3d previousPosition = ToMap(previous.first).cast<double>();

                Velocity = (Position - previousPosition) / dt.count();

                AngularVelocity = (transform.second * previous.second.inverse()).cast<double>();
                AngularVelocity.angle() /= dt.count();
            }

            m_previousPose = pose;

            m_positionHistory.push_back(make_pair(Position, ts));
            if (m_positionHistory.size() == 3)
            {
                auto dt = seconds(m_positionHistory[2].second - m_positionHistory[1].second).count();

                Velocity = (m_positionHistory[2].first - m_positionHistory[1].first) / dt;

                E::Vector3d accel = (m_positionHistory[2].first - 2 * m_positionHistory[1].first + m_positionHistory[0].first) / (dt * dt);

                FIRE_OBJECT_TRACE("IMU Linear Accel.PoseInterpolator", this, (mira::make_data_point<float>(
                    m_positionHistory[1].second,
                    (float)accel.norm()
                )));

                m_positionHistory.erase(m_positionHistory.begin());
            }

            FIRE_OBJECT_TRACE("IMU Linear Velocity.PoseInterpolator", this, (mira::make_data_point<float>(
                ts,
                (float)Velocity.norm()
            )));
        }
    };

    PoseInterpolator::PoseInterpolator()
        : m_impl{ std::allocate_shared<Impl, Eigen::aligned_allocator<Impl>>({}) }
    {}

    PoseInterpolator::~PoseInterpolator() = default;

    mage::Pose PoseInterpolator::GetPose() const
    {
        Eigen::Vector3f position{ m_impl->Position.cast<float>() };
        return mage::Pose(FromQuatAndTrans(m_impl->Orientation.cast<float>(), { position.x(), position.y(), position.z() }));
    }

    void PoseInterpolator::PredictUpTo(mage::SensorSample::Timestamp ts)
    {
        m_impl->ProcessAngularVelocityUntil(ts);
        m_impl->ProcessLinearVelocityUntil(ts);
    }

    void PoseInterpolator::AddPose(const mage::Pose& pose, mage::SensorSample::Timestamp timestamp)
    {
        m_impl->ProcessPose(timestamp, pose);
    }
}


#include <iostream>
#include <mtl/dense1D.h>
#include <mtl/mtl.h>

/*
  example output:

  2

  */

int
main()
{
  //begin
  mtl::dense1D< float > x(10, 3.0);
  x[5] = 2.0;
  float s = mtl::min(x);
  std::cout << s << std::endl;
  // end
  return 0;
}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#pragma once

#include <stdint.h>
#include <algorithm>
#include <vector>
#include <Eigen/Dense>

#include <boost/random/mersenne_twister.hpp>

#include <dpMM/global.hpp>
#include <dpMM/normal.hpp>

using  namespace Eigen;
using std::min;
using std::max;
using std::cout;
using std::endl;

#ifdef BOOST_OLD
using boost::mt19937;
#else
using boost::mt19937;
#endif

// TODO Template needs fixing - probably just remove
template <typename T>
class Sphere
{
public:
  Sphere(uint32_t D) : D_(D), north_(Matrix<T,Dynamic,1>::Zero(D))
  {north_(D_-1) = 1.0;};

  //assumes north is size D and last entry is 1
  Sphere(Matrix<T,Dynamic,1> north) : D_(north.rows()), north_(north)
  {};
  virtual ~Sphere() {};

  /*
   * normal in tangent space around p rotate to the north pole
   * -> the last dimension will always be 0
   * -> return only first 2 dims
   */
  virtual Matrix<T,Dynamic,Dynamic> Log_p_north(const Matrix<T,Dynamic,1>& p,
      const Matrix<T,Dynamic,Dynamic>& q) const;
  /*
   * rotate points x in tangent plane around north pole down to p
   */
  Matrix<T,Dynamic,Dynamic> rotate_north2p(const Matrix<T,Dynamic,1>& p,
      const Matrix<T,Dynamic,Dynamic>& xNorth) const;

  /* rotate points x in tangent plane around p to north pole and
   * return 2D coordinates */
  Matrix<T,Dynamic,Dynamic> rotate_p2north(const Matrix<T,Dynamic,1>& p,
      const Matrix<T,Dynamic,Dynamic>& x) const;
  void rotate_p2north( const Matrix<T,Dynamic,Dynamic>& ps,
    const Matrix<T,Dynamic,Dynamic>& x, Matrix<T,Dynamic,Dynamic>& xNorth,
    const VectorXu& z, uint32_t K) const;
  void rotate_p2north(const Matrix<T,Dynamic,1>& p,
    const Matrix<T,Dynamic,Dynamic>& x_p, Matrix<T,Dynamic,Dynamic>& xNorth,
    const VectorXu& z, uint32_t k) const;

  /* compute rotation from TpS^2 to north pole on sphere */
  Matrix<T,Dynamic,Dynamic> north_R_TpS2(const Matrix<T,Dynamic,1>& p) const;

  /* compute logarithm map from sphere to T_pS */
  Matrix<T,Dynamic,Dynamic> Log_p(const Matrix<T,Dynamic,1>& p,
      const Matrix<T,Dynamic,Dynamic>& q) const;
  Matrix<T,Dynamic,1> Log_p_single(const Matrix<T,Dynamic,1>& p,
      const Matrix<T,Dynamic,1>& q) const;
  /* compute logarithm map from sphere to T_pS skips points where w_i==0 */
  Matrix<T,Dynamic,Dynamic> Log_p(const Matrix<T,Dynamic,1>& p,
    const Matrix<T,Dynamic,Dynamic>& q, const Matrix<T,Dynamic,1>& w) const;
  /* compute logmap for q where z_i ==k; leaves other x_i untouched */
  void Log_p(const Matrix<T,Dynamic,1>& p,
    const Matrix<T,Dynamic,Dynamic>& q, const VectorXu& z, uint32_t k,
    Matrix<T,Dynamic,Dynamic>& x, uint32_t zDivider=1) const;
  /* compute logarithm map from sphere to T_pS around several points p
   * indicated by labels z; fill in matrix x */
  void Log_ps(const Matrix<T,Dynamic,Dynamic>& p,
    const Matrix<T,Dynamic,Dynamic>& q, const VectorXu& z,
    Matrix<T,Dynamic,Dynamic>& x) const;

  /* compute exponential map from T_pS to sphere */
  Matrix<T,Dynamic,Dynamic> Exp_p(const Matrix<T,Dynamic,1>& p,
      const Matrix<T,Dynamic,Dynamic>& x) const;
  Matrix<T,Dynamic,1> Exp_p_single(const Matrix<T,Dynamic,1>& p,
    const Matrix<T,Dynamic,1>& x) const;

  Matrix<T,Dynamic,1> sampleUnif(mt19937* pRndGen);

  // http://en.wikipedia.org/wiki/N-sphere
  // http://keisan.casio.com/exec/system/1223381019
  T logSurfaceArea() const {
    return (LOG_2+0.5*D_*LOG_PI - boost::math::lgamma(0.5*D_));};

  uint32_t D() const {return D_;};
  const Matrix<T,Dynamic,1>& north() const {return north_;};
protected:

  T invSincDot(T dot) const;

  //static const double MIN_DOT = -0.95;
  //static const double MAX_DOT = 0.95;
  static double const MIN_DOT;
  static double const MAX_DOT;
  uint32_t D_; // dimension of ambient space; sphere is D-1 dimensional.
  Matrix<T,Dynamic,1> north_;
};

/* rotation from point A to B; percentage specifies how far the rotation will
 * bring us towards B [0,1] */
template<typename T>
inline Matrix<T,Dynamic,Dynamic> rotationFromAtoB(const Matrix<T,Dynamic,1>& a,
    const Matrix<T,Dynamic,1>& b, T percentage=1.0)
{
  assert(b.size() == a.size());

  uint32_t D_ = b.size();
  Matrix<T,Dynamic,Dynamic> bRa(D_,D_);

  T dot = b.transpose()*a;
  ASSERT(fabs(dot) <=1.0, "a="<<a.transpose()<<" |.| "<<a.norm()
      <<" b="<<b.transpose()<<" |.| "<<b.norm()
      <<" -> "<<dot);
  dot = max(static_cast<T>(-1.0),min(static_cast<T>(1.0),dot));
//  cout << "dot="<<dot<<" | |"<<fabs(dot+1.)<<endl;
  if(fabs(dot -1.) < 1e-6)
  {
    // points are almost the same -> just put identity
    bRa =  Matrix<T,Dynamic,Dynamic>::Identity(D_,D_);
//    bRa(0,0) = cos(percentage*PI);
//    bRa(1,1) = cos(percentage*PI);
//    bRa(0,1) = -sin(percentage*PI);
//    bRa(1,0) = sin(percentage*PI);
  }else if(fabs(dot +1.) <1e-6)
  {
    // direction does not matter since points are on opposing sides of sphere
    // -> pick one and rotate by percentage;
    bRa = -Matrix<T,Dynamic,Dynamic>::Identity(D_,D_);
    bRa(0,0) = cos(percentage*PI*0.5);
    bRa(1,1) = cos(percentage*PI*0.5);
    bRa(0,1) = -sin(percentage*PI*0.5);
    bRa(1,0) = sin(percentage*PI*0.5);
  }else{
    T alpha = acos(dot) * percentage;
//    cout << "alpha="<<alpha<<endl;

    Matrix<T,Dynamic,1> c(D_);
    c = a - b*dot;
    ASSERT(c.norm() >1e-5, "c="<<c.transpose()<<" |.| "<<c.norm());
    c /= c.norm();
    Matrix<T,Dynamic,Dynamic> A = b*c.transpose() - c*b.transpose();

	Matrix<T,Dynamic,Dynamic> temp = b*b.transpose() + c*c.transpose();
	T temp2 = cos(alpha)-1.;
    bRa = Matrix<T,Dynamic,Dynamic>::Identity(D_,D_) + sin(alpha)*A +
      (temp2)*(temp);
  }
  return bRa;
}


#include <iostream>
#include <cstdio>
#include <cstdlib>
#include <vector>
#include <Eigen/Dense>
#include <iomanip>
#include <cmath>
#include <math.h>
#include <float.h>
#include <algorithm>
#include "ANN.cpp"

using namespace Eigen;

MatrixXd getTril(MatrixXd& mat) {
	MatrixXd tril = mat.triangularView<Lower>();
	for (int i = 0; i < tril.rows(); i++)
		tril(i,i) = 0;
	return tril;
}

void appendVec2Mat(MatrixXd& mat, VectorXd vec) {
	mat.conservativeResize(mat.rows(), mat.cols()+1);
	mat.col(mat.cols()-1) = vec;
}

VectorXd StdVec2Eigen(std::vector <double> vec){
  VectorXd outvec(vec.size());
  for(int i=0;i<vec.size();i++)
    outvec(i)=vec[i];
  return outvec;
}

std::vector <double> EigenVec2Std(VectorXd vec){
  std::vector <double> outvec(vec.rows(),0.0);
  for(int i=0;i<vec.rows();i++)
    outvec[i]=vec(i);
  return outvec;
}

class ObjFunc{
public:
  	void init(ANN& input) {
  		nnet = input;
  		target = VectorXd::Constant(20,0);
  	}
  	void setTarget(VectorXd input) {
  		for (int i = 0; i < 10; i++) {
  			target(2*i) = input(i);
  			target(2*i + 1) = input(i);
  		}
  	}
 //  	VectorXd evalANN(VectorXd input){
 //  		std::vector<double> temperature = nnet.ComputeANN(EigenVec2Std(input));
	// 	VectorXd output(10);
	// 	for(int i=0;i<10;i++){
	// 		output(i)=	0.5*(0.15*temperature[3*i] +
	// 					0.85*temperature[3*i+1] +
	// 					0.85*temperature[3*i+2] +
	// 					0.15*temperature[3*i+3]);
	// 	}
	// 	return output;
	// }
  	VectorXd evalANN(VectorXd input){
  		std::vector<double> temperature = nnet.ComputeANN(EigenVec2Std(input));
		VectorXd output(20);
		for(int i=0;i<10;i++){
			output(2 * i) = (temperature[3*i] + temperature[3*i + 1]) * 0.015 / 2 +
			(1.5 * temperature[3*i + 1] + 0.5 * temperature[3*i + 2]) * 0.035 / 2;
			output(2 * i) /= 0.05;
			output(2 * i + 1) = (temperature[3*i + 2] + temperature[3*i + 3]) * 0.015 / 2 +
			(0.5 * temperature[3*i + 1] + 1.5 * temperature[3*i + 2]) * 0.035 / 2;
			output(2*i + 1) /= 0.05;
		}
		return output;
	}
	void eval(double& f, VectorXd& g, VectorXd input){
		VectorXd output = evalANN(input);
		// std::cout << output << std::endl;
		int n = input.rows();
		f = (output - target).dot(output - target);
		VectorXd gradient = VectorXd::Constant(n,0.0);
		double delta = 0.01;
		// std::cout << "BP\n";
		for (int i = 0; i < n; i++) {
			VectorXd d_input = VectorXd::Constant(n,0.0);
			d_input(i) = delta;
			VectorXd temp_output1 = evalANN(input + d_input);
			VectorXd temp_output2 = evalANN(input - d_input);
			gradient(i) = (temp_output1 - target).dot(temp_output1 - target) -
							(temp_output2 - target).dot(temp_output2 - target);
			gradient(i) /= 2*delta;
		}
		g = gradient;
	}
private:
	ANN nnet;
	VectorXd target;
};

void quickSort(std::vector<double>& arr, std::vector<int>& indices, int left, int right) {
	int i = left, j = right;
	double pivot = arr[(left + right) / 2];
	/*partition*/
	while (i <= j) {
		while (arr[i] < pivot)
			i++;
		while (arr[j] > pivot)
			j--;
		if (i <= j) {
			double tmp_v = arr[i];
			int tmp_i = indices[i];
			arr[i] = arr[j];
			indices[i] = indices[j];
			arr[j] = tmp_v;
			indices[j] = tmp_i;
			i++;
			j--;
		}
	}
	/*recursion*/
	if (left < j)
		quickSort(arr, indices, left, j);
	if (i < right)
		quickSort(arr, indices, i, right);
}

struct LBFGSBParam{
	int m;
	int max_iters;
	double tol;
	bool display;
	bool xhistory;
};

struct BPOutput{
	VectorXd t;
	VectorXd d;
	std::vector<int> F;
};

struct CauchyOutput{
	VectorXd xc;
	VectorXd c;
};

struct SubspaceMinOutput{
	VectorXd xbar;
	bool line_search_flag;
};

struct LBFGSB_Output {
	VectorXd x;
	double obj;
};

class LBFGSB{
public:
	LBFGSBParam param;
	LBFGSB_Output solve(ObjFunc& obj, VectorXd x0, VectorXd l, VectorXd u, LBFGSBParam params);
private:
	double f;
	VectorXd g;
	double getOptimality(VectorXd x, VectorXd g, VectorXd l, VectorXd u);
	BPOutput getBreakpoints(VectorXd x, VectorXd g, VectorXd l, VectorXd u);
	CauchyOutput getCauchyPoint(VectorXd x, VectorXd g, VectorXd l, VectorXd u,
								double theta, MatrixXd W, MatrixXd M);
	double findAlpha(VectorXd l, VectorXd u, VectorXd xc, VectorXd du,
					std::vector<int> free_vars_idx);
	SubspaceMinOutput subspaceMin(VectorXd x, VectorXd g, VectorXd l, VectorXd u,
						VectorXd xc, VectorXd c, double theta, MatrixXd W, MatrixXd M);
	double strongWolfe(ObjFunc& obj, VectorXd x0, double f0, VectorXd g0, VectorXd p);
	double alphaZoom(ObjFunc& obj, VectorXd x0, double f0, VectorXd g0, VectorXd p,
					double alpha_lo, double alpha_hi);
};

double LBFGSB::getOptimality(VectorXd x, VectorXd g, VectorXd l, VectorXd u){
	VectorXd projected_g = x - g;
	for(int i = 0; i < x.rows(); i++) {
		if (projected_g(i) < l(i))
			projected_g(i) = l(i);
		else if (projected_g(i) > u(i))
			projected_g(i) = u(i);
	}
	projected_g = projected_g - x;
	return projected_g.cwiseAbs().maxCoeff();
}

BPOutput LBFGSB::getBreakpoints(VectorXd x, VectorXd g, VectorXd l, VectorXd u){
	int nn = x.rows();
	VectorXd t = VectorXd::Constant(nn,0.0);
	VectorXd d = -g;
	for (int i = 0; i < nn; i++) {
		if (g(i) < 0)
			t(i) = (x(i) - u(i)) / g(i);
		else if (g(i) > 0)
			t(i) = (x(i) - l(i)) / g(i);
		else
			t(i) = DBL_MAX;
		if (t(i) < DBL_EPSILON)
			d(i) = 0.0;
	}
	// Sort elements of t and store indices in F
	std::vector<int> Fvec(nn, 0);
	std::vector<double> tvec(nn,0.0);
	for (int i = 0; i < nn; i++){
		tvec[i] = t(i);
		Fvec[i] = i;
		// std::cout << tvec[i] << " " << Fvec[i] <<std::endl;
	}
	//std::cout<<nn<<std::endl;
	// std::cout<<"BP_quicksort\n";
	quickSort(tvec, Fvec, 0, nn-1);
	// std::cout<<"BP_after_quicksort\n";

	BPOutput bpout;
	bpout.t = t;
	bpout.d = d;
	bpout.F = Fvec;
	return bpout;
}

CauchyOutput LBFGSB::getCauchyPoint(VectorXd x, VectorXd g, VectorXd l, VectorXd u,
								double theta, MatrixXd W, MatrixXd M){

	//std::cout << "BP0" << std::endl;
	BPOutput bpout = getBreakpoints(x, g, l, u);
	// std::cout << "t = " << std::endl << bpout.t << std::endl;
	// std::cout << "d = " << std::endl << bpout.d << std::endl;
	// std::cout << "F = " << std::endl;
	// for (int i = 0; i < bpout.F.size(); i++) {
	// 	std::cout << bpout.F[i] << std::endl;
	// }
	VectorXd xc = x;
	//std::cout << "BP01" << std::endl;
	VectorXd p = W.transpose() * bpout.d;
	VectorXd c = VectorXd::Constant(W.cols(), 0.0);
	double fp = - bpout.d.transpose() * bpout.d;
	double fpp = -theta * fp - p.dot(M * p);
	double fpp0 = -theta * fp;
	double dt_min = -fp / fpp;
	double t_old = 0;
	int i;
	//std::cout << "BP1" << std::endl;
	for (int j = 0; j < x.rows(); j++) {
		i = j;
		if (bpout.F[i] > 0)
			break;
	}
	int b = bpout.F[i];
	double t = bpout.t(b);
	double dt = t - t_old;
	//std::cout << "BP2" << std::endl;

	while ( (dt_min > dt) && (i < x.rows()) ) {
		if (bpout.d(b) > 0)
			xc(b) = u(b);
		else if (bpout.d(b) < 0)
			xc(b) = l(b);
		//std::cout << "BP3" << std::endl;
		double zb = xc(b) - x(b);
		c = c + dt * p;
		double gb = g(b);
		VectorXd wbt = W.row(b);
		fp += dt * fpp + gb * gb + theta * gb * zb - gb * wbt.dot(M * c);
		fpp -= theta * gb * gb - 2.0 * gb * wbt.dot(M * p) - gb * gb * wbt.dot(M * wbt);
		fpp = std::max(DBL_EPSILON * fpp0, fpp);
		p += gb * wbt;
		bpout.d(b) = 0.0;
		dt_min = -fp / fpp;
		t_old = t;
		i++;
		if (i < x.rows()){
			b = bpout.F[i];
			t = bpout.t(b);
			dt = t - t_old;
		}
	}
	// Perform final updates
	dt_min = std::max(dt_min, 0.0);
	t_old = t_old + dt_min;
	for (int j = 0; j < xc.rows(); j++) {
		int idx = bpout.F[j];
		xc(idx) += t_old * bpout.d(idx);
	}
	c += dt_min * p;

	CauchyOutput cpout;
	cpout.c = c;
	cpout.xc = xc;
	return cpout;
}

double LBFGSB::findAlpha(VectorXd l, VectorXd u, VectorXd xc, VectorXd du,
				std::vector<int> free_vars_idx) {
	// INPUTS:
	//  l: [n,1] lower bound constraint vector.
	//  u: [n,1] upper bound constraint vector.
	//  xc: [n,1] generalized Cauchy point.
	//  du: [num_free_vars,1] solution of unconstrained minimization.
	// OUTPUTS:
	//  alpha_star: positive scaling parameter.
	double alpha_star = 1;
	int n = free_vars_idx.size();
	for (int i = 0; i < n; i++) {
		int idx = free_vars_idx[i];
		if (du(i) > 0)
			alpha_star = std::min(alpha_star, (u(idx) - xc(idx)) / du(i));
		else
			alpha_star = std::min(alpha_star, (l(idx) - xc(idx)) / du(i));
	}
	return alpha_star;
}

SubspaceMinOutput LBFGSB::subspaceMin(VectorXd x, VectorXd g, VectorXd l, VectorXd u,
								VectorXd xc, VectorXd c, double theta, MatrixXd W, MatrixXd M){
	SubspaceMinOutput subminout;
	subminout.line_search_flag = true;

	int n = x.rows();
	std::vector<int> free_vars_idx;
	std::vector<VectorXd> Z;
	for (int i = 0; i < xc.rows(); i++) {
		if ((xc(i) != u(i)) && (xc(i) != l(i))) {
			free_vars_idx.push_back(i);
			VectorXd unit = VectorXd::Constant(n,0);
			unit(i) = 1;
			Z.push_back(unit);
		}
	}

	int num_free_vars = free_vars_idx.size();
	if (num_free_vars == 0) {
		subminout.xbar = xc;
		subminout.line_search_flag = false;
		return subminout;
	}

	MatrixXd ZZ = MatrixXd::Zero(n, num_free_vars);
	for (int i = 0; i < num_free_vars; i++)
		ZZ.col(i) = Z[i];

	// compute the reduced gradient of mk restricted to free variables
	MatrixXd WTZ = W.transpose() * ZZ;
	VectorXd rr = g + theta * (xc - x) - W*M*c;
	VectorXd r = VectorXd::Constant(num_free_vars, 0.0);
	for (int i = 0; i < num_free_vars; i++)
		r(i) = rr(free_vars_idx[i]);

	// form intermediate variables
	double invtheata = 1.0 / theta;
	VectorXd v = M * WTZ * r;
	MatrixXd N = invtheata * WTZ * WTZ.transpose();
	int N_size = N.rows();
	N = MatrixXd::Identity(N_size, N_size) - M * N;
	v = N.inverse() * v;
	VectorXd du = -invtheata * r - invtheata * invtheata * WTZ.transpose()*v;

	// find alpha star
	double alpha_star = findAlpha(l, u, xc, du, free_vars_idx);
	VectorXd d_star = alpha_star * du;
	subminout.xbar = xc;
	for (int i = 0; i < num_free_vars; i++) {
		int idx = free_vars_idx[i];
		subminout.xbar(idx) += d_star(i);
	}
	return subminout;
}

double LBFGSB::strongWolfe(ObjFunc& obj, VectorXd x0, double f0, VectorXd g0, VectorXd p){
	double alpha; // return value;
	double c1 = 1e-4;
	double c2 = 0.9;
	double alpha_max = 2.5;
	double alpha_im1 = 0;
	double alpha_i = 1;
	double f_im1 = f0;
	double dphi0 = g0.dot(p);
	int i = 0;
	int max_iters = 20;
	VectorXd x;
	double f_i;
	VectorXd g_i;
	// search for alpha that satisfies strong Wolfe conditions
	while (true) {
		x = x0 + alpha_i * p;
		obj.eval(f_i, g_i, x);
		if ((f_i > f0 + c1 * dphi0) || ( (i > 1) && (f_i >= f_im1) )) {
			alpha = alphaZoom(obj, x0, f0, g0, p, alpha_im1, alpha_i);
			break;
		}
		double dphi = g_i.dot(p);
		if (fabs(dphi) <= -c2 * dphi0) {
			alpha = alpha_i;
			break;
		}
		if (dphi >= 0) {
			alpha = alphaZoom(obj, x0, f0, g0, p, alpha_i, alpha_im1);
			break;
		}

		// update
		alpha_im1 = alpha_i;
		f_im1 = f_i;
		alpha_i += 0.8 * (alpha_max - alpha_i);

		if (i > max_iters) {
			alpha = alpha_i;
			break;
		}
		i++;
	}
	return alpha;
}

double LBFGSB::alphaZoom(ObjFunc& obj, VectorXd x0, double f0, VectorXd g0, VectorXd p,
				double alpha_lo, double alpha_hi) {
	double alpha; // return value
	double c1 = 1e-4;
	double c2 = 0.9;
	int i = 0;
	int max_iters = 20;
	double dphi0 = g0.dot(p);
	double alpha_i;
	VectorXd x;
	double f_i;
	VectorXd g_i;

	while (true) {
		alpha_i = 0.5 * (alpha_lo + alpha_hi);
		alpha = alpha_i;
		x = x0 + alpha_lo * p;
		obj.eval(f_i, g_i, x);
		VectorXd x_lo = x0 + alpha_lo * p;
		double f_lo;
		VectorXd dummy_g;
		obj.eval(f_lo, dummy_g, x_lo);
		if ( (f_i > f0 + c1 * alpha_i * dphi0) || (f_i >= f_lo) )
			alpha_hi = alpha_i;
		else {
			double dphi = g_i.dot(p);
			if (fabs(dphi) <= -c2 * dphi0) {
				alpha = alpha_i;
				break;
			}
			if (dphi * (alpha_hi - alpha_lo) >= 0) {
				alpha_hi = alpha_lo;
			}
			alpha_lo = alpha_i;
		}
		i++;
		if (i > max_iters) {
			alpha = alpha_i;
			break;
		}
	}
	return alpha;
}

LBFGSB_Output LBFGSB::solve(ObjFunc& obj, VectorXd x0, VectorXd l,
							VectorXd u, LBFGSBParam params) {

    int rank=0;
    rank=MPI::COMM_WORLD.Get_rank();

	int n = x0.rows();
	MatrixXd Y(n,0);
	MatrixXd S(n,0);
	MatrixXd W = MatrixXd::Zero(n,1);
	MatrixXd M = MatrixXd::Zero(1,1);
	double theta = 1;

	// initialize obj vars
	VectorXd x = x0;
	double f;
	VectorXd g;
	obj.eval(f, g, x);
	int k = 0;
	double opt;

	if (params.display && rank == 0) {
		std::cout << "iter\t\tf(x)\t\toptimality\n";
		std::cout << "-------------------------------------\n";
		opt = getOptimality(x, g, l, u);
		std::cout << k << "\t\t" << f << "\t\t" << opt << "\t\t" << std::endl;
	}

	MatrixXd xhist(n,0);
	//std::cout << "BP0" << std::endl;
	if (params.xhistory) {
		appendVec2Mat(xhist, x0);
	}

	VectorXd x_old;
	VectorXd g_old;
	VectorXd y;
	VectorXd s;
	while ((getOptimality(x, g, l, u) > params.tol) && (k < params.max_iters) ) {
		x_old = x;
		g_old = g;
		//std::cout << "BP01" << std::endl;

		CauchyOutput cpout = getCauchyPoint(x, g, l, u, theta, W, M);
		VectorXd xc = cpout.xc;
		VectorXd c = cpout.c;
		// std::cout << "xc = " << std::endl;
		// std::cout << xc << std::endl;
		// std::cout << "c = " << std::endl;
		// std::cout << c << std::endl;
		// std::cout << "BP1" << std::endl;
		SubspaceMinOutput subminout = subspaceMin(x, g, l, u, xc, c, theta, W, M);
		//std::cout << "BP2" << std::endl;

		VectorXd xbar = subminout.xbar;
		bool line_search_flag = subminout.line_search_flag;

		double alpha = 1.0;
		if (line_search_flag) {
			alpha = strongWolfe(obj, x, f, g, xbar - x);
		}
		x += alpha * (xbar - x);
		//std::cout << "BP3" << std::endl;

		// update LBFGS data structures
		obj.eval(f, g, x);
		y = g - g_old;
		s = x - x_old;
		double curv = fabs(s.dot(y));
		if (curv < DBL_EPSILON) {
			if (params.display && rank == 0) {
    			std::cout << (" warning: negative curvature detected\n");
    			std::cout << ("          skipping L-BFGS update\n");
    		}
    		k++;
    		continue;
		}

		if (Y.cols() < params.m) {
			appendVec2Mat(Y, y);
			appendVec2Mat(S, s);
		} else {
			Y.block(0, 0, n, params.m - 1) = Y.block(0, 1, n, params.m - 1);
			S.block(0, 0, n, params.m - 1) = S.block(0, 1, n, params.m - 1);
			Y.col(params.m - 1) = y;
			S.col(params.m - 1) = s;
		}
		//std::cout << "BP4" << std::endl;
		theta = y.dot(y) / y.dot(s);
		//std::cout << "BP401" << std::endl;
		MatrixXd temp = Y;
		//std::cout << "BP402" << std::endl;
		temp.conservativeResize(temp.rows(), temp.cols()+S.cols());
		temp.block(0, Y.cols(), S.rows(), S.cols()) = theta * S;
		//std::cout << "BP403" << std::endl;
		W = temp;
		//std::cout << W << std::endl;
		//std::cout << "BP41" << std::endl;
		MatrixXd A = S.transpose() * Y;
		MatrixXd L = getTril(A);
		MatrixXd D = -1 * A.diagonal().asDiagonal();
		int D_size = D.rows();
		int L_size = L.rows();

		//std::cout << "BP42" << std::endl;

		MatrixXd MM(D_size + L_size, D_size + L_size);
		MM.block(0, 0, D_size, D_size) = D;
		MM.block(0, D_size, D_size, L_size) = L.transpose();
		MM.block(D_size, 0, L_size, D_size) = L;
		MM.block(D_size, D_size, L_size, L_size) = theta * S.transpose() * S;
		//std::cout << "BP43" << std::endl;
		M = MM.inverse();
		//std::cout << "BP5" << std::endl;
		// update the iteration
		k++;
		if (params.xhistory) {
			appendVec2Mat(xhist, x);
		}
		if (params.display && rank == 0) {
			opt = getOptimality(x, g, l, u);
			std::cout << k << "\t\t" << f << "\t\t" << opt << "\t\t" << std::endl;
		}
	}
	if (k == params.max_iters && params.display && rank == 0) {
		std::cout << " warning: maximum number of iterations reached\n";
	}

	if (getOptimality(x,g,l,u) < params.tol && params.display && rank == 0) {
		std::cout << " stopping because convergence tolerance met!\n";
	}

	LBFGSB_Output out;
	out.x = x;
	out.obj = f;
	return out;
}

//705.023 701.723 697.852 691.804  692.91 667.241 667.139 672.044 674.512 680.647

// int main(int argc, char** argv){
// 	VectorXd x(10);
// 	x = VectorXd::Constant(10,150);
// 	VectorXd l = VectorXd::Constant(10,0.0);
// 	VectorXd u = VectorXd::Constant(10,420.0);

// 	LBFGSBParam params;
// 	params.m = 10;
// 	params.tol = 1e-2;
// 	params.max_iters = 50;
// 	params.display = true;
// 	params.xhistory = false;

// 	ObjFunc obj;
// 	obj.init();
// 	VectorXd base = VectorXd::Constant(10,673);
// 	VectorXd increment(10);
// 	increment << 40, 40, 40, 40, 40, 20, 20, 20, 20, 20;
// 	obj.setTarget(base + increment);
// 	// obj.target << 50, 30, 60, 50, 60, 70, 1, 90, 50, 50;
// 	LBFGSB opt;
// 	std::cout << "starting to solve\n";
// 	x = opt.solve(obj, x, l, u, params);
// 	std::cout << x.transpose() << std::endl;

// 	VectorXd output = obj.evalANN(x);
// 	std::cout << (output - base).transpose() << std::endl;

// 	// LBFGSB optimizer;
// 	// // double opt = optimizer.getOptimality(x,g,l,u);
// 	// // BPOutput bpout = optimizer.getBreakpoints(x,g,l,u);
// 	// // std::cout<<bpout.t.transpose()<<std::endl;
// 	// // std::cout<<bpout.d.transpose()<<std::endl;
// 	// // for (int i = 0; i < bpout.F.size(); i++)
// 	// // 	std::cout<<bpout.F[i]<<"  ";
// 	// // std::cout<<std::endl;
// 	// MatrixXd W = MatrixXd::Random(10,20);
// 	// MatrixXd M = MatrixXd::Identity(20,20);
// 	// VectorXd row = M.row(10);
// 	// std::cout<<row<<std::endl;

// 	// CauchyOutput cpout = optimizer.getCauchyPoint(x,g,l,u, 0.1, W, M);
// 	// std::cout<<cpout.xc.transpose()<<std::endl;
// 	// std::cout<<cpout.c.transpose()<<std::endl;
// 	// using namespace std;
// 	// MatrixXd mat(2,0);
// 	// VectorXd vec(2);
// 	// vec << 1, 1;
// 	// cout << mat.rows() << " " << mat.cols() << endl;
// 	// mat.conservativeResize(mat.rows(), mat.cols()+1);
// 	// mat.col(mat.cols()-1) = vec;
// 	// cout << mat << endl;
// 	return 0;
// }


#include <lib/utility.hpp>

#include <boost/mp11.hpp>

#include <type_traits>

using boost::mp11::index_sequence;
using boost::mp11::integer_sequence;

using xzr::utility::make_index_range;
using xzr::utility::make_integer_range;

namespace
{
static_assert(std::is_same<make_integer_range<int, 0, 0>, integer_sequence<int>>::value, "");
static_assert(std::is_same<make_integer_range<unsigned, 0, 0>, integer_sequence<unsigned>>::value, "");

static_assert(std::is_same<make_integer_range<int, 1, 1>, integer_sequence<int>>::value, "");
static_assert(std::is_same<make_integer_range<unsigned, 5, 5>, integer_sequence<unsigned>>::value, "");

static_assert(std::is_same<make_integer_range<int, 1, 5>, integer_sequence<int, 1, 2, 3, 4>>::value, "");
static_assert(std::is_same<make_integer_range<unsigned, 5, 8>, integer_sequence<unsigned, 5, 6, 7>>::value, "");

static_assert(std::is_same<make_integer_range<int, -3, 3>, integer_sequence<int, -3, -2, -1, 0, 1, 2>>::value, "");
static_assert(std::is_same<make_integer_range<int, 3, -3>, integer_sequence<int, 3, 2, 1, 0, -1, -2>>::value, "");

static_assert(std::is_same<make_index_range<42, 42>, index_sequence<>>::value, "");
static_assert(std::is_same<make_index_range<3, 7>, index_sequence<3, 4, 5, 6>>::value, "");

static_assert(std::is_same<make_index_range<7, 3>, index_sequence<7, 6, 5, 4>>::value, "");
} // namespace


/*
 *  divide_by_inplace_test.cpp
 *  MTL
 *
 *  Created by Hui Li (huil@Princeton.EDU)
 *
 */

#include <iostream>
#include <cmath>
#include <complex>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>
#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/laplacian_setup.hpp>
#include <boost/numeric/mtl/operation/print.hpp>
#include <boost/numeric/mtl/operation/operators.hpp>
#include <boost/numeric/mtl/operation/divide_by_inplace.hpp>

using namespace std;

template <typename MatrixA, typename MatrixB>
void test(MatrixA& a, MatrixB& b, unsigned dim1, unsigned dim2, const char* name)
{
    unsigned size= dim1 * dim2;
    MTL_THROW_IF(size == 0, mtl::runtime_error("Matrix size must be larger than 0 to make the test meaningful."));

    const unsigned max_print_size= 25;
    cout << "\n" << name << "\n";
    laplacian_setup(a, dim1, dim2);
    laplacian_setup(b, dim1, dim2);

    // right_scale_inplace(a, 2.0);
    a*= 2.0;
    if (size <= max_print_size)
		cout << "A= \n\n" << a << "\n";

    typename mtl::Collection<MatrixA>::value_type eight(8.0);
    MTL_THROW_IF(a[0][0] != eight, mtl::runtime_error("Scaling with scalar wrong"));

    a /= 2.0; // divide_by_inplace(a, 2.0);
    a*= b;   // right_scale_inplace(a, b);

    if (size <= max_print_size)
		cout << "A= \n\n" << a << "B= \n\n" << b << "\n";

    // Check for stencil below in the middle of the matrix
    //        1
    //     2 -8  2
    //  1 -8 20 -8  1
    //     2 -8  2
    //        1
    if (dim1 == 5 && dim2 == 5) {
	typename mtl::Collection<MatrixA>::value_type twenty(20.0), two(2.0), one(1.0),
	                                              zero(0.0), minus_eight(-8.0);
	MTL_THROW_IF(a[12][12] != twenty, mtl::runtime_error("wrong diagonal"));
	MTL_THROW_IF(a[12][13] != minus_eight, mtl::runtime_error("wrong east neighbor"));
	MTL_THROW_IF(a[12][14] != one, mtl::runtime_error("wrong east east neighbor"));
	MTL_THROW_IF(a[12][15] != zero, mtl::runtime_error("wrong zero-element"));
	MTL_THROW_IF(a[12][17] != minus_eight, mtl::runtime_error("wrong south neighbor"));
	MTL_THROW_IF(a[12][18] != two, mtl::runtime_error("wrong south east neighbor"));
	MTL_THROW_IF(a[12][22] != one, mtl::runtime_error("wrong south south neighbor"));
    }
}

int main(int argc, char* argv[])
{
    using namespace mtl;
    unsigned dim1= 5, dim2= 5;

    if (argc > 2) {dim1= atoi(argv[1]);dim2= atoi(argv[2]);}
    unsigned size= dim1 * dim2;

    compressed2D<double>                                 cr(size, size);
    compressed2D<double, mat::parameters<col_major> > cc(size, size);

    dense2D<double>                                      dr(size, size);
    dense2D<double, mat::parameters<col_major> >      dc(size, size);

    test(cr, dr, dim1, dim2, "Row-major sparse scaled with row-major dense");
    test(cr, dc, dim1, dim2, "Row-major sparse scaled with column-major dense");
    test(cc, dr, dim1, dim2, "Column-major sparse scaled with row-major dense");
    test(cc, dc, dim1, dim2, "Column-major sparse scaled with column-major dense");

    test(dr, cr, dim1, dim2, "Row-major dense scaled with row-major sparse");
    test(dr, cc, dim1, dim2, "Row-major dense scaled with column-major sparse");
    test(dc, cr, dim1, dim2, "Column-major dense scaled with row-major sparse");
    test(dc, cc, dim1, dim2, "Column-major dense scaled with column-major sparse");

    return 0;
}