// Copyright (c) 2018 by University Paris-Est Marne-la-Vallee
// ProductTools.hpp
// This file is part of the Garamon Generator.
// Authors: Stephane Breuils and Vincent Nozick
// Contact: vincent.nozick@u-pem.fr
//
// Licence MIT
// A a copy of the MIT License is given along with this program

/// \file ProductTools.hpp
/// \author Vincent Nozick, Stephane Breuils
/// \brief Geometric algebra "naive" implementation tools dedicated to build the optimized C++ products.

#ifndef GARAGEN_PRODUCTTOOLS_HPP
#define GARAGEN_PRODUCTTOOLS_HPP

#include <vector>
#include <array>
#include <map>
#include <utility>     // pairs
#include <Eigen/Dense> // use the metric which is dense
#include <Eigen/Sparse> // use the transformation matrices which are sparse
#include <iostream>
#if defined(_MSC_BUILD)
	#ifndef __builtin_popcount
		#define __builtin_popcount __popcnt
	#endif
#endif // __WINDOWS MSVC compiler__

/// In the computation of mv3 = mv1^mv2, this represents a quadruplet containing the indices of mv1,mv2,mv3 and a coefficient in a product between two blades.
/// e.g. suppose that a product is defined by mv3[2] += -3.0*mv1[3]*mv2[4]  then the first quadruplet associated with this product will be 3,4,1,-3.0
template<typename T>
struct productComponent{
    unsigned int indexOfMv1; // in the example above, indexOfMv1 would be 3
    unsigned int indexOfMv2; // in the example above, indexOfMv2 would be 4
    unsigned int indexOfMv3; // in the example above, indexOfMv2 would be 4
    T coefficient;           // in the example above, coefficient would be -1
};

template<typename C>
bool compareProductComponents(const C &a, const C &b)
//bool operator < (const productComponent &a, const productComponent &b) const
{
    // first compare on indexOfMv3
    if(a.indexOfMv3 > b.indexOfMv3) return false;
    if(a.indexOfMv3 < b.indexOfMv3) return true;

    // second compare on indexOfMv1
    if(a.indexOfMv1 > b.indexOfMv1) return false;
    if(a.indexOfMv1 < b.indexOfMv1) return true;

    // thrid compare on indexOfMv2
    if(a.indexOfMv2 > b.indexOfMv2) return false;
    if(a.indexOfMv2 < b.indexOfMv2) return true;

    return true;
}

class ProductTools {
public:

    /// constructor
    /// \param vectorSpaceDimension is the dimension of the vector space of the algebra
    ProductTools(const unsigned int vectorSpaceDimension);

    /// destructor
    ~ProductTools();

    /// \brief compute the Hamming weight of the xor index (blade) xorIndexMv (i.e the number of bits=1 in the binary value xorIndexMv)
    /// \param xorIndexMv is a xorIndex, i.e. a each of his bit refers to a basis blade of the vector space of the algebra
    static unsigned int hammingWeight(const unsigned int xorIndexMv);

    /// \brief return whether the outer product between the two blades whose indices are xorIndexMv1 and xorIndexMv2 is a null blade
    /// \param xorIndexMv1 is a xorIndex, i.e. a each of his bit refers to a basis blade of the vector space of the algebra
    /// \param xorIndexMv2 is a xorIndex, i.e. a each of his bit refers to a basis blade of the vector space of the algebra
    static bool outerProductExists(const unsigned int xorIndexMv1, const unsigned int xorIndexMv2);

    /// \brief return the sign of the outer product between the two blades whose indices are xorIndexMv1 and xorIndexMv2
    /// \param xorIndexMv1 is a xorIndex, i.e. a each of his bit refers to a basis blade of the vector space of the algebra
    /// \param xorIndexMv2 is a xorIndex, i.e. a each of his bit refers to a basis blade of the vector space of the algebra
    static int outerProductSign(const unsigned int xorIndexMv1, const unsigned int xorIndexMv2);

    /// \brief return whether the inner product exists
    /// \param xorIndexMv1 is a xorIndex, i.e. a each of his bit refers to a basis blade of the vector space of the algebra
    /// \param xorIndexMv2 is a xorIndex, i.e. a each of his bit refers to a basis blade of the vector space of the algebra
    static bool innerProductExists(const unsigned int xorIndexMv1, const unsigned int xorIndexMv2);

    /// \brief Compute the coefficient required in the computation of the inner and geometric products of 2 multivectors.
    /// \param xorIndexMv1 is a xorIndex, i.e. a each of his bit refers to a basis blade of the vector space of the algebr  in the algebra related orthogonal space
    /// \param xorIndexMv2 is a xorIndex, i.e. a each of his bit refers to a basis blade of the vector space of the algebra in the algebra related orthogonal space
    /// \param diagonalMetric defines the metric in the algebra related orthogonal space
    static double productCoefficientFromMetric(const unsigned int xorIndexMv1, const unsigned int xorIndexMv2,
                                               const Eigen::VectorXd &diagonalMetric);

    /// \brief Compute the position of the product between two XOR indices. Is common for the geometric, outer and inner product
    /// \param xorIndexMv1 is a xorIndex, i.e. a each of his bit refers to a basis blade of the vector space of the algebra
    /// \param xorIndexMv2 is a xorIndex, i.e. a each of his bit refers to a basis blade of the vector space of the algebra
    static unsigned int productResultXorIndex(const unsigned int xorIndexMv1, const unsigned int xorIndexMv2);

    /// \brief Generate all the components of the product mv1{gradeMv1} ^ mv2{gradeMv2}.
    /// \param xorIndexMv1 is a xorIndex, i.e. a each of his bit refers to a basis blade of the vector space of the algebr  in the algebra related orthogonal space
    /// \param xorIndexMv2 is a xorIndex, i.e. a each of his bit refers to a basis blade of the vector space of the algebra in the algebra related orthogonal space
    /// \return For each component of mv3=mv1^mv2 with grade gradeMv3=gradeMv1+gradeMv2, we return a list of quadruplet (indexMv1,indexMv2,indexMv3,coefficient).
    std::list<productComponent<double>> generateExplicitOuterProductList(const unsigned int gradeMv1, const unsigned int gradeMv2) const;

    /// \brief Generate all the components of the inner product mv1<gradeMv1> . mv2<gradeMv2> in a Euclidean space with diagonal metric. In practice, we compute here the Hestenes inner product.
    /// \param gradeMv1 the grade of the homogeneous multivector Mv1 to be considered
    /// \param gradeMv2 the grade of the homogeneous multivector Mv2 to be considered
    /// \param diagonalMetric defines the metric in the algebra related orthogonal space
    /// \return for each component of the result mv3=mv1.mv2 with grade gradeMv3=|gradeMv1-gradeMv2|, we return a list of quadruplet (indexMv1,indexMv2,indexMv3,coefficient).
    std::list<productComponent<double>> generateExplicitInnerProductListEuclideanSpace(const unsigned int gradeMv1,
                                                                                const unsigned int gradeMv2,
                                                                                const Eigen::VectorXd &diagonalMetric) const;

    /// \brief Generate all the components of the inner product mv1<gradeMv1> . mv2<gradeMv2>. In practice, we compute here the Hestenes inner product.
    /// \param gradeMv1 the grade of the homogeneous multivector Mv1 to be considered
    /// \param gradeMv2 the grade of the homogeneous multivector Mv2 to be considered
    /// \param transformationMatrixMv1 is the matrix that put a k-vector of grade "gradeMv1" to the algebra related orthogonal space
    /// \param transformationMatrixMv2 is the matrix that put a k-vector of grade "gradeMv2" to the algebra related orthogonal space
    /// \param transformationMatrixMv3 is the matrix that put back a k-vector of grade "gradeMv3" from the algebra related orthogonal space to the original space
    /// \param diagonalMetric defines the metric in the algebra related orthogonal space
    /// \return for each component of the result mv3=mv1.mv2 with grade gradeMv3=|gradeMv1-gradeMv2|, we return a list of quadruplet (indexMv1,indexMv2,indexMv3,coefficient).
    std::list<productComponent<double>> generateExplicitInnerProductList(const unsigned int gradeMv1,
                                                                         const unsigned int gradeMv2,
                                                                         const Eigen::SparseMatrix<double, 0> &transformationMatrixMv1,
                                                                         const Eigen::SparseMatrix<double, 0> &transformationMatrixMv2,
                                                                         const Eigen::SparseMatrix<double, 0> &transformationMatrixMv3,
                                                                         const Eigen::VectorXd &diagonalMetric) const;

    /// \brief Generate all the components of the geometric product mv1<gradeMv1> mv2<gradeMv2>.
    /// \param gradeMv1 the grade of the homogeneous multivector Mv1 to be considered
    /// \param gradeMv2 the grade of the homogeneous multivector Mv2 to be considered
    /// \param transformationMatrixMv1 is the matrix that put a k-vector of grade "gradeMv1" to the algebra related orthogonal space
    /// \param transformationMatrixMv2 is the matrix that put a k-vector of grade "gradeMv2" to the algebra related orthogonal space
    /// \param transformationMatrixMv3 is the matrix that put back a k-vector of grade "gradeMv3" from the algebra related orthogonal space to the original space
    /// \param diagonalMetric defines the metric in the algebra related orthogonal space
    /// \return a vector of products (each cell of the vector contains its associated grade products). For each component of the result mv3=mv1 mv2, we return a list of quadruplet (indexMv1,indexMv2,indexMv3,coefficient).
    std::vector<std::list<productComponent<double>>> generateExplicitGeometricProductListEuclideanSpace(const unsigned int gradeMv1,const unsigned int gradeMv2,
                                                                                const Eigen::VectorXd& diagonalMetric) const;

    /// \brief Generate all the components of the geometric product mv1<gradeMv1> mv2<gradeMv2>.
    /// \param gradeMv1 the grade of the homogeneous multivector Mv1 to be considered
    /// \param gradeMv2 the grade of the homogeneous multivector Mv2 to be considered
    /// \param transformationMatrixMv1 is the matrix that put a k-vector of grade "gradeMv1" to the algebra related orthogonal space
    /// \param transformationMatrixMv2 is the matrix that put a k-vector of grade "gradeMv2" to the algebra related orthogonal space
    /// \param transformationMatrixMv3 is the matrix that put back a k-vector of grade "gradeMv3" from the algebra related orthogonal space to the original space
    /// \param diagonalMetric defines the metric in the algebra related orthogonal space
    /// \return a vector of products (each cell of the vector contains its associated grade products). For each component of the result mv3=mv1 mv2, we return a list of quadruplet (indexMv1,indexMv2,indexMv3,coefficient).
    std::vector<std::list<productComponent<double>>> generateExplicitGeometricProductList(const unsigned int gradeMv1,const unsigned int gradeMv2,
                                                                                Eigen::SparseMatrix<double, Eigen::ColMajor>& transformationMatrixMv1,
                                                                                Eigen::SparseMatrix<double, Eigen::ColMajor>& transformationMatrixMv2,
                                                                                std::vector<Eigen::SparseMatrix<double, Eigen::ColMajor> >& transformationMatricesMv3,
                                                                                const Eigen::VectorXd& diagonalMetric) const;

    // tool to debug
    void displayGradeToXorIndices() const;

    /// \brief getter: return the grade knowing the Xor index
    unsigned int getGrade(unsigned int xorIndex) const;

    /// \brief getter: return the homogeneous index knowing the Xor index
    unsigned int getHomogeneousIndex(unsigned int xorIndex) const;

    /// \brief getter: return the xor index knowing the grade and the homogeneous index
    unsigned int getXorIndex(unsigned int grade, unsigned int homogeneousIndex) const;

protected:
    const unsigned int dimension;

    /// order induced by the xor:                 scal, 1, 2, 12, 3, 13, 23, 123
    /// Mapping between
    /// Xor index:                                   0, 1, 2,  3, 4,  5,  6, 7
    /// and grade and position in the sequence:      0, 0, 1,  0, 2,  1,  2, 0

    // defines the one to one correspondence between a xor index (index in the multivector) and the grade and position in the homogeneous vector
    // grade and position to xor index
    /// xorIndex = gradePositionToXorIndex[grade][pos];
    std::vector<std::vector<unsigned int> > gradePositionToXorIndex;

    /// array that converts xor index to grade and position
    /// first: grade
    /// second: position
    std::vector<std::pair<unsigned int,unsigned int> > xorIndexToGradeAndPosition; // number of pairs is 2^dimension
};

/// \brief generate a scale containing the sign of the norm of the pseudo-inverse
double getScaleInversePseudoScalar(const Eigen::MatrixXd &metric);

#endif //GARAGEN_PRODUCTTOOLS_HPP


/*
 * phase_chain.cpp
 *
 * Example of OMP parallelization with odeint
 *
 * Copyright 2013 Karsten Ahnert
 * Copyright 2013 Mario Mulansky
 * Copyright 2013 Pascal Germroth
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 */

#include <iostream>
#include <vector>
#include <boost/random.hpp>
#include <boost/timer/timer.hpp>
//[phase_chain_openmp_header
#include <omp.h>
#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/external/openmp/openmp.hpp>
//]

using namespace std;
using namespace boost::numeric::odeint;
using boost::timer::cpu_timer;
using boost::math::double_constants::pi;

//[phase_chain_vector_state
typedef std::vector< double > state_type;
//]

//[phase_chain_rhs
struct phase_chain
{
    phase_chain( double gamma = 0.5 )
    : m_gamma( gamma ) { }

    void operator()( const state_type &x , state_type &dxdt , double /* t */ ) const
    {
        const size_t N = x.size();
        #pragma omp parallel for schedule(runtime)
        for(size_t i = 1 ; i < N - 1 ; ++i)
        {
            dxdt[i] = coupling_func( x[i+1] - x[i] ) +
                      coupling_func( x[i-1] - x[i] );
        }
        dxdt[0  ] = coupling_func( x[1  ] - x[0  ] );
        dxdt[N-1] = coupling_func( x[N-2] - x[N-1] );
    }

    double coupling_func( double x ) const
    {
        return sin( x ) - m_gamma * ( 1.0 - cos( x ) );
    }

    double m_gamma;
};
//]

int main( int argc , char **argv )
{
    //[phase_chain_init
    size_t N = 131101;
    state_type x( N );
    boost::random::uniform_real_distribution<double> distribution( 0.0 , 2.0*pi );
    boost::random::mt19937 engine( 0 );
    generate( x.begin() , x.end() , boost::bind( distribution , engine ) );
    //]

    //[phase_chain_stepper
    typedef runge_kutta4<
                      state_type , double ,
                      state_type , double ,
                      openmp_range_algebra
                    > stepper_type;
    //]

    //[phase_chain_scheduling
    int chunk_size = N/omp_get_max_threads();
    omp_set_schedule( omp_sched_static , chunk_size );
    //]

    cpu_timer timer;
    //[phase_chain_integrate
    integrate_n_steps( stepper_type() , phase_chain( 1.2 ) ,
                       x , 0.0 , 0.01 , 100 );
    //]
    double run_time = static_cast<double>(timer.elapsed().wall) * 1.0e-9;
    std::cerr << run_time << "s" << std::endl;
    // copy(x.begin(), x.end(), ostream_iterator<double>(cout, "\n"));

    return 0;
}


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

#include <nt2/include/functor.hpp>
#include <boost/simd/constant/hierarchy.hpp>
#include <boost/simd/constant/register.hpp>

namespace nt2
{
  namespace tag
  {
   /*!
     @brief Sqrt_2opi generic tag

     Represents the Sqrt_2opi constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    BOOST_SIMD_CONSTANT_REGISTER( Sqrt_2opi, double
                                , 0, 0x3f4c422a
                                , 0x3fe9884533d43651ll
                                )
  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::Sqrt_2opi, Site> dispatching_Sqrt_2opi(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::Sqrt_2opi, Site>();
   }
   template<class... Args>
   struct impl_Sqrt_2opi;
  }
  /*!
    Constant  \f$\frac{\sqrt2}{\pi}\f$.

    @par Semantic:

    For type T0:

    @code
    T0 r = Sqrt_2opi<T0>();
    @endcode

    is similar to:

    @code
    T0 r = sqrt(Two<T0>())/Pi<T0>();
    @endcode

    @return a value of type T0
  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Sqrt_2opi, Sqrt_2opi);
}

#endif


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

/// \file
/// Declares function RootFinder::newton_raphson

#pragma once

#include <boost/math/tools/roots.hpp>
#include <functional>
#include <limits>

#include "DataStructures/DataVector.hpp"
#include "ErrorHandling/Exceptions.hpp"
#include "Utilities/MakeString.hpp"

namespace RootFinder {
/*!
 * \ingroup NumericalAlgorithmsGroup
 * \brief Finds the root of the function `f` with the Newton-Raphson method.
 *
 * `f` is a unary invokable that takes a `double` which is the current value at
 * which to evaluate `f`. `f` must return a `std::pair<double, double>` where
 * the first element is the function value and the second element is the
 * derivative of the function.  An example is below.
 *
 * \snippet Test_NewtonRaphson.cpp double_newton_raphson_root_find
 *
 * See the [Boost](http://www.boost.org/) documentation for more details.
 *
 * \requires Function `f` is invokable with a `double`
 * \note The parameter `digits` specifies the precision of the result in its
 * desired number of base-10 digits.
 *
 * \throws `convergence_error` if the requested precision is not met after
 * `max_iterations` iterations.
 */
template <typename Function>
double newton_raphson(const Function& f, const double initial_guess,
                      const double lower_bound, const double upper_bound,
                      const size_t digits, const size_t max_iterations = 50) {
  ASSERT(digits < std::numeric_limits<double>::digits10,
         "The desired accuracy of " << digits
                                    << " base-10 digits must be smaller than "
                                       "the machine numeric limit of "
                                    << std::numeric_limits<double>::digits10
                                    << " base-10 digits.");

  boost::uintmax_t max_iters = max_iterations;
  // clang-tidy: internal boost warning, can't fix it.
  const auto result = boost::math::tools::newton_raphson_iterate(  // NOLINT
      f, initial_guess, lower_bound, upper_bound,
      std::round(std::log2(std::pow(10, digits))), max_iters);
  if (max_iters >= max_iterations) {
    throw convergence_error(MakeString{}
                            << "newton_raphson reached max iterations of "
                            << max_iterations
                            << " without converging. Best result is: " << result
                            << " with residual " << f(result).first);
  }
  return result;
}

/*!
 * \ingroup NumericalAlgorithmsGroup
 * \brief Finds the root of the function `f` with the Newton-Raphson method on
 * each element in a `DataVector`.
 *
 * `f` is a binary invokable that takes a `double` as its first argument and a
 * `size_t` as its second. The `double` is the current value at which to
 * evaluate `f`, and the `size_t` is the current index into the `DataVector`s.
 *  `f` must return a `std::pair<double, double>` where the first element is
 * the function value and the second element is the derivative of the function.
 * Below is an example of how to root find different functions by indexing into
 * a lambda-captured `DataVector` using the `size_t` passed to `f`.
 *
 * \snippet Test_NewtonRaphson.cpp datavector_newton_raphson_root_find
 *
 * See the [Boost](http://www.boost.org/) documentation for more details.
 *
 * \requires Function `f` be callable with a `double` and a `size_t`
 * \note The parameter `digits` specifies the precision of the result in its
 * desired number of base-10 digits.
 *
 * \throws `convergence_error` if, for any index, the requested precision is not
 * met after `max_iterations` iterations.
 */
template <typename Function>
DataVector newton_raphson(const Function& f, const DataVector& initial_guess,
                          const DataVector& lower_bound,
                          const DataVector& upper_bound, const size_t digits,
                          const size_t max_iterations = 50) {
  ASSERT(digits < std::numeric_limits<double>::digits10,
         "The desired accuracy of " << digits
                                    << " base-10 digits must be smaller than "
                                       "the machine numeric limit of "
                                    << std::numeric_limits<double>::digits10
                                    << " base-10 digits.");
  const auto digits_binary = std::round(std::log2(std::pow(10, digits)));

  DataVector result_vector{lower_bound.size()};
  for (size_t i = 0; i < result_vector.size(); ++i) {
    boost::uintmax_t max_iters = max_iterations;
    // clang-tidy: internal boost warning, can't fix it.
    result_vector[i] = boost::math::tools::newton_raphson_iterate(  // NOLINT
        [&f, i ](double x) noexcept { return f(x, i); }, initial_guess[i],
        lower_bound[i], upper_bound[i], digits_binary, max_iters);
    if (max_iters >= max_iterations) {
      throw convergence_error(MakeString{}
                              << "newton_raphson reached max iterations of "
                              << max_iterations
                              << " without converging. Best result is: "
                              << result_vector[i] << " with residual "
                              << f(result_vector[i], i).first);
    }
  }
  return result_vector;
}

}  // namespace RootFinder


#include <Eigen/CholmodSupport>
#include <iomanip>
#include <iostream>
#include "../../include/Optimization/LineSearch.h"
#include "../../include/Optimization/LBFGSSolver.h"
#include "../../include/timer.h"

void OptSolver::lbfgsSolver(std::function<double(const Eigen::VectorXd&, Eigen::VectorXd*, Eigen::SparseMatrix<double>*, bool)> objFunc, std::function<double(const Eigen::VectorXd&, const Eigen::VectorXd&)> findMaxStep, Eigen::VectorXd& x0, int numIter, double gradTol, double xTol, double fTol, bool displayInfo, std::function<void(const Eigen::VectorXd&, double&, double&)> getNormFunc)
{
	std::cout << "LBFGS-Solver" << std::endl;
	const size_t m = 10;
	const size_t DIM = x0.rows();
	Eigen::MatrixXd sVector = Eigen::MatrixXd::Zero(DIM, m);
	Eigen::MatrixXd yVector = Eigen::MatrixXd::Zero(DIM, m);
	Eigen::VectorXd alpha = Eigen::VectorXd::Zero(m);
	Eigen::VectorXd grad(DIM), q(DIM), grad_old(DIM), s(DIM), y(DIM);
	double f = objFunc(x0, &grad, NULL, false);
	Eigen::VectorXd x_old = x0;

	if (displayInfo)
	{
		std::cout << "start energy: " << f << std::endl;
		std::cout << "start gradient norm: " << grad.template lpNorm<Eigen::Infinity>() << std::endl;
	}

	size_t iter = 0, globIter = 0;
	double H0k = 1;
	do {
		const double relativeEpsilon = static_cast<double>(0.0001) * std::max<double>(static_cast<double>(1.0), x0.norm());

		if (grad.norm() < gradTol)
		{
			std::cout << "gradient is too small, ||g||_2: " << grad.norm() << std::endl;
			break;
		}
		//Algorithm 7.4 (L-BFGS two-loop recursion)
		q = grad;
		const int k = std::min<double>(m, iter);

		// for i = k − 1, k − 2, . . . , k − m§
		for (int i = k - 1; i >= 0; i--) {
			// alpha_i <- rho_i*s_i^T*q
			const double rho = 1.0 / static_cast<Eigen::VectorXd>(sVector.col(i))
				.dot(static_cast<Eigen::VectorXd>(yVector.col(i)));
			alpha(i) = rho * static_cast<Eigen::VectorXd>(sVector.col(i)).dot(q);
			// q <- q - alpha_i*y_i
			q = q - alpha(i) * yVector.col(i);
		}

		// r <- H_k^0*q
		q = H0k * q;
		//for i k − m, k − m + 1, . . . , k − 1
		for (int i = 0; i < k; i++) {
			// beta <- rho_i * y_i^T * r
			const double rho = 1.0 / static_cast<Eigen::VectorXd>(sVector.col(i))
				.dot(static_cast<Eigen::VectorXd>(yVector.col(i)));
			const double beta = rho * static_cast<Eigen::VectorXd>(yVector.col(i)).dot(q);
			// r <- r + s_i * ( alpha_i - beta)
			q = q + sVector.col(i) * (alpha(i) - beta);
		}
		// stop with result "H_k*f_f'=q"
		double alpha_init = findMaxStep(x0, -q);
		double rate = LineSearch::backtrackingArmijo(x0, grad, -q, objFunc, alpha_init);

		// update guess
		x0 = x0 - rate * q;

		grad_old = grad;
		double fold = f;
		f = objFunc(x0, &grad, NULL, false);

		s = x0 - x_old;
		y = grad - grad_old;

		// update the history
		if (iter < m) {
			sVector.col(iter) = s;
			yVector.col(iter) = y;
		}
		else {

			sVector.leftCols(m - 1) = sVector.rightCols(m - 1).eval();
			sVector.rightCols(1) = s;
			yVector.leftCols(m - 1) = yVector.rightCols(m - 1).eval();
			yVector.rightCols(1) = y;
		}
		// update the scaling factor
		H0k = y.dot(s) / static_cast<double>(y.dot(y));

		x_old = x0;
		if (displayInfo)
		{
			std::cout << std::endl << "iter: " << globIter << ", linesearch rate: " << rate << std::endl;
			std::cout << std::setprecision(10) << "fold = " << fold << ", f = " << f << ", ||grad|| " << grad.norm() << ", delta x: " << rate * q.norm() << ", delta_f: " << fold - f << std::endl;
			if (getNormFunc)
			{
				double gradz, gradw;
				getNormFunc(grad, gradz, gradw);

				double updatez, updatew;
				getNormFunc(rate * q, updatez, updatew);
				std::cout << "z grad: " << gradz << ", w grad: " << gradw << ", z change: " << updatez << ", w change: " << updatew << std::endl;
			}
		}
		if (rate < 1e-8)
		{
			std::cout << "terminate with small line search rate (<1e-8): L2-norm = " << grad.norm() << std::endl;
			break;
		}

		if (grad.norm() < gradTol)
		{
			std::cout << "terminate with gradient L2-norm = " << grad.norm() << std::endl;
			break;
		}

		if (rate * q.norm() < xTol)
		{
			std::cout << "terminate with small variable change, gradient L2-norm = " << grad.norm() << std::endl;
			break;
		}

		if (fold - f < fTol)
		{
			std::cout << "terminate with small energy change, gradient L2-norm = " << grad.norm() << std::endl;
			break;
		}

		iter++;
		globIter++;
	} while (globIter < numIter);

}


/// Program KP
/** @file

\mainpage
This code solves the KP equation in dimensional units (SI) for the water wave
problem in a frame of reference moving with velocity c_0 = sqrt(g*h).

The code is pseudo-spectral and it assumes periodic boundary conditions, both in x and y.
It is a natural extension of the KdV code using the method developed in Fornberg, Bengt,
and G. B. Whitham, "A numerical and theoretical study of certain nonlinear wave phenomena".
Philosophical Transactions of the Royal Society of London A: Mathematical, Physical and
Engineering Sciences 289.1361 (1978): 373-404.

The integral term is done in Fourier space as 1/k F(k).

The stability condition required is \f$|\frac{\Delta t}{\Delta x}| |\frac{1}{\Delta x^2}+\frac{1}{\Delta y^2}| < \frac{1}{\pi ^3}\f$

C++ code, based off a Fortran code developed by Dr. Miguel Onorato.

Initial condition and simulation parameters set: Choice of Soliton, Lump or input for KPII via eta.csv file.
Output: free surface height \f$\eta\f$ via eta.csv for every nvis time step

Generating initial conditions done in MATLAB directly when running with MEX-functions

\author Athina Lange, based off code from Dr. Miguel Onorato
\date April 8th, 2018
*/

#include "global.h"
#include "Soliton.h"
#include "Lump.h"
#include "KP.h"
#include "csv.h"

#include <boost/program_options.hpp>
namespace po = boost::program_options;

int main(int argc, char* argv[]){

    try {

        //=================================================
        // VARIABLE DECLERATION
        //=================================================

	std::string initial_condition; // initial condition: "Soliton", "Lump" or "SeaStates" - default: "Soliton"

        int nx; // dimension of meshgrid: x - default: 128
        int ny; // dimension of meshgrid: y - default: 64
        double c0; // linear nondispersive wave velocity c0 = sqrt(g*h)
        double a; // soliton amplitude
        double h; // water depth  - default: 5.0
        double alpha; // nonlinear coeff
        double beta; // dispersive coeff
        double gama; // coeff in front of second derivative in y

        double delx; // spacing in x
        double dely; // spacing in y

        double deltat; // time step - default: 0.001
        int nstep; // number of steps - default: 500000 (500s)
        int nvis; // visualize output after nvis steps - default: 1000

        // Read eta from csv file (no header, single value per line)
        std::vector<double> eta;

        //=================================================
        // VARIABLE INITIALIZING
        //=================================================

        po::options_description desc("Options");
        desc.add_options()
                ("help,h", "help message")
                ("initial-condition", po::value<std::string>(&initial_condition)->default_value("soliton"), "initial condition: soliton, lump or seastates")
                ("nx", po::value<int>(&nx)->default_value(128), "number of points in x direction (even number)")
                ("ny", po::value<int>(&ny)->default_value(64), "number of points in y direction (even number)")
                ("h", po::value<double>(&h)->default_value(5.0), "water depth")
                ("deltat", po::value<double>(&deltat)->default_value(0.001), "time step")
                ("nstep", po::value<int>(&nstep)->default_value(500000), "number of steps")
                ("nvis", po::value<int>(&nvis)->default_value(1000), "visualize output after nvis steps");

        po::variables_map vm;
        po::store(po::parse_command_line(argc, argv, desc), vm);
        po::notify(vm);

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

        if (initial_condition == "soliton") {

            a = 0.1;
            c0 = sqrt(G * h);
            alpha = (3.0 / 2.0) * c0 / h; //assumed to be >=0
            beta = h * h * c0 / 6.0; //assumed to be > 0
            gama = 0.5 * c0;

            double xmin = 0.0;
            double xmax = 400.0;
            double ymin = 0.0;
            double ymax = 200.0;

            delx = (xmax - xmin) / nx;
            dely = (ymax - ymin) / ny;

            soliton(nx, ny, alpha, beta, a, delx, dely, xmin, ymin, eta); // Run soliton initial condition

        } else if (initial_condition == "lump") {

            a = 1.0;
            c0 = 0.0;
            alpha = 6.0;
            beta = 1.0;
            gama = -1.0;

            //Change parameters for meshgrid (256 x 256), time interval (0.0005) and run time (1000000) to satisfy stability condition
            nx = 256;
            ny = 256;
            deltat = 0.0005;
            nstep = 1000000;

            double xmin = -30.0;
            double xmax = 30.0;
            double ymin = -50.0;
            double ymax = 50.0;

            delx = (xmax - xmin) / nx;
            dely = (ymax - ymin) / ny;

            lump(nx, ny, a, h, delx, dely, xmin, ymin, eta); // Run lump initial condition

        } else if (initial_condition == "seastates") {

            a = 0.1;
            c0 = sqrt(G * h);
            alpha = (3.0 / 2.0) * c0 / h; //assumed to be >=0
            beta = h * h * c0 / 6.0; //assumed to be > 0
            gama = 0.5 * c0;

            double xmin = 0.0;
            double xmax = 400.0;
            double ymin = 0.0;
            double ymax = 200.0;

            delx = (xmax - xmin) / nx;
            dely = (ymax - ymin) / ny;

            read_wave_csv("eta.csv", eta); // Read value of free surface eta from file

        } else {
            std::cout << "initial-condition needs to be soliton, lump or seastates, not " << initial_condition << std::endl;
            return 1;
        }

        write_soliton_csv("Soliton.csv", nx, ny, eta); // Write initial condition data to file

        //=================================================
        // RUN PROPAGATOR
        //=================================================

        KP(nx, ny, h, alpha, beta, gama, delx, dely, deltat, nstep, nvis, eta); // Run KP propagator

        write_wave_csv("Wave_1000.csv", nx, ny, delx, dely, eta); // Write final free surface eta to file
    }

    catch(std::exception& e) {
        std::cerr << "error: " << e.what() << std::endl;
        return 1;
    }
    catch(...) {
        std::cerr << "Exception of unknown type!" << std::endl;
        return 1;
    }

    return 0;
}


#include <algorithm>

#ifdef _WIN32
    #pragma warning(push, 0)
    #include <boost/polygon/polygon.hpp>
    #pragma warning(pop)
#else
    #include <boost/polygon/polygon.hpp>
#endif

#include <cradle/geometry/angle.hpp>
#include <cradle/geometry/clipper.hpp>
#include <cradle/geometry/intersection.hpp>
#include <cradle/geometry/polygonal.hpp>

namespace cradle {

// COMMON FUNCTIONS

static vector<2,double> get_corner(box<2,double> const& box, int index)
{
    vector<2,double> corner;
    while(index >= 4)
    {
        index -= 4;
    }
    switch (index)
    {
        case 0:
            return box.corner;
        case 1:
            return box.corner + make_vector(box.size[0], 0.);
        case 2:
            return box.corner + make_vector(box.size[0], box.size[1]);
        case 3:
            return box.corner + make_vector(0., box.size[1]);
    }
    return make_vector(0.,0.);
}

// POLYGONS

polygon2 make_polygon2(std::vector<vertex2> const& vertices)
{
    polygon2 poly;
    initialize(&poly.vertices, vertices);
    return poly;
}

double get_area(polygon2 const& poly)
{
    return get_area_and_centroid(poly).first;
}

vector2d get_centroid(polygon2 const& poly)
{
    return get_area_and_centroid(poly).second;
}

std::pair<double, vector2d> get_area_and_centroid(polygon2 const& poly)
{
    double a = 0.0;
    double cx = 0.0;
    double cy = 0.0;
    for (polygon2_edge_view ev(poly); !ev.done(); ev.advance())
    {
        double cross = ev.p0()[0] * ev.p1()[1] - ev.p0()[1] * ev.p1()[0];
        a += cross;
        cx += (ev.p0()[0] + ev.p1()[0]) * cross;
        cy += (ev.p0()[1] + ev.p1()[1]) * cross;
    }
    double scale = 3.0 * a;
    return std::make_pair(std::fabs(0.5 * a),
        make_vector(cx / scale, cy / scale));
}

polygon2 scale(polygon2 const& poly, double factor)
{
    return scale(poly, make_vector(factor, factor));
}

struct point_scaling_fn
{
    vector<2,double> factor;
    vertex2 operator()(vertex2 const& src) const
    {
        return make_vector(src[0] * factor[0], src[1] * factor[1]);
    }
};

polygon2 scale(polygon2 const& poly, vector<2,double> const& factor)
{
    point_scaling_fn fn;
    fn.factor = factor;
    return map_points(poly, fn);
}

bool is_inside(polygon2 const& poly, vector<2,double> const& p)
{
    bool c = false;
    for (polygon2_edge_view ev(poly); !ev.done(); ev.advance())
    {
        if (((ev.p0()[1] <= p[1] && p[1] < ev.p1()[1]) ||
            (ev.p1()[1] <= p[1] && p[1] < ev.p0()[1]))
         && (p[0] < (ev.p1()[0] - ev.p0()[0]) * (p[1] - ev.p0()[1]) /
            (ev.p1()[1] - ev.p0()[1]) + ev.p0()[0]))
        {
            c = !c;
        }
    }
    return c;
}

bool is_inside(polygon2 const& parent_poly, polygon2 const& child_poly)
{
    for (auto const& point : child_poly.vertices)
    {
        if (!is_inside(parent_poly, point))
        {
            return false;
        }
    }
    return true;
}

bool is_ccw(polygon2 const& poly)
{
    assert(poly.vertices.size() > 2);
    vector<2,double> edge0 = poly.vertices[1] - poly.vertices[0];
    vector<2,double> edge1 = poly.vertices[2] - poly.vertices[1];
    return (edge0[0] * edge1[1]) - (edge0[1] * edge1[0]) > 0;
}

polygon2 as_polygon(box<2,double> const& box)
{
    polygon2 poly;
    vertex2* vertices = allocate(&poly.vertices, 4);
    vertices[0] = box.corner;
    vertices[1] = box.corner + make_vector(box.size[0], 0.);
    vertices[2] = box.corner + box.size;
    vertices[3] = box.corner + make_vector(0., box.size[1]);
    return poly;
}

polygon2 as_polygon(circle<double> const& circle, unsigned n_segments)
{
    polygon2 poly;
    vertex2* vertices = allocate(&poly.vertices, n_segments);
    for (unsigned i = 0; i < n_segments; ++i)
    {
        angle<double,radians> a(2 * pi * i / n_segments);
        vertices[i] =
            make_vector(cos(a), sin(a)) * circle.radius + circle.center;
    }
    return poly;
}

polygon2 as_polygon(triangle<2,double> const& tri)
{
    polygon2 poly;
    vertex2* vertices = allocate(&poly.vertices, 3);
    for (unsigned i = 0; i != 3; ++i)
        vertices[i] = tri[i];
    return poly;
}

// POLYSETS

void create_polyset(polyset* set, polygon2 const& poly)
{
    *set = polyset();
    set->polygons.push_back(poly);
}

polyset make_polyset(polygon2 const& poly)
{
    polyset p;
    create_polyset(&p, poly);
    return p;
}

void add_polygon(polyset& set, polygon2 const& poly)
{
    polyset addition;
    create_polyset(&addition, poly);
    if (set.polygons.empty())
        swap(set, addition);
    else
        do_set_operation(&set, set_operation::UNION, addition, set);
}
void add_hole(polyset& set, polygon2 const& hole)
{
    assert(!set.polygons.empty());
    polyset subtraction;
    create_polyset(&subtraction, hole);
    do_set_operation(&set, set_operation::DIFFERENCE, set, subtraction);
}

polyset remove_polyset_holes(
    polyset const& original_shape)
{
    polyset shape = original_shape;

    while (!shape.holes.empty())
    {
        do_set_operation(
            &shape,
            set_operation::UNION,
            shape,
            make_polyset(shape.holes.front()));
    }

    return shape;
}

polyset scale(polyset const& set, double factor)
{
    return scale(set, make_vector(factor, factor));
}
polyset scale(polyset const& set, cradle::vector<2,double> const& factor)
{
    point_scaling_fn fn;
    fn.factor = factor;
    return map_points(set, fn);
}

std::pair<double, vector2d> static
get_area_and_centroid(polyset const& set)
{
    double area = 0;
    auto centroid = make_vector(0., 0.);
    for (auto const& i : set.polygons)
    {
        auto pair = get_area_and_centroid(i);
        area += pair.first;
        centroid += pair.second * pair.first;
    }
    for (auto const& i : set.holes)
    {
        auto pair = get_area_and_centroid(i);
        area -= get_area(i);
        centroid -= pair.second * pair.first;
    }
    centroid /= area;
    return std::make_pair(area, centroid);
}

double get_area(polyset const& set)
{
    return get_area_and_centroid(set).first;
}

vector2d get_centroid(polyset const& set)
{
    auto pair = get_area_and_centroid(set);
    if (pair.first == 0)
        throw exception("centroid requested for empty polyset");
    return pair.second;
}

bool is_inside(polyset const& set, vector<2,double> const& p)
{
    int inside_count = 0;
    for (auto const& i : set.polygons)
    {
        if (is_inside(i, p))
            ++inside_count;
    }
    for (auto const& i : set.holes)
    {
        if (is_inside(i, p))
            --inside_count;
    }
    return inside_count > 0;
}

typedef line_segment<2, double> line_segment2;

double distance_to_polyset(vector<2,double> const& p, polyset const& set)
{
    double d2p = 1.0e10;
    size_t polygon_count = set.polygons.size();
    for (size_t k = 0; k < polygon_count; ++k)
    {
        size_t vertex_count = set.polygons[k].vertices.n_elements;
        auto v0 = set.polygons[k].vertices.elements[vertex_count - 1];
        for (size_t i = 0; i < vertex_count; ++i)
        {
            // Get segment end point
            auto v1 = set.polygons[k].vertices.elements[i];
            line_segment2 segment(v0, v1);
            auto len = length(segment);

            // Ensure the line segment has finite length
            if (len >= 1.0e-8)
            {
                // Discretize line segment and determine distance
                double spacing = 0.25;
                int split_count = std::min(std::max(int(std::ceil(len / spacing)), 2), 30);
                for (int j = 0; j < split_count; ++j)
                {
                    double dist = distance(segment, p);

                    if (dist < std::fabs(d2p))
                    {
                        bool pip = point_in_polygon(p, set.polygons[k]);
                        d2p = pip ? -dist : dist;
                    }
                }
            }

            // Increment segment start point
            v0 = v1;
        }
    }

    // Handle Holes too
    size_t hole_count = set.holes.size();
    for (size_t k = 0; k < hole_count; ++k)
    {
        size_t vertex_count = set.holes[k].vertices.n_elements;
        auto v0 = set.holes[k].vertices.elements[vertex_count - 1];
        for (size_t i = 0; i < vertex_count; ++i)
        {
            // Get segment end point
            auto v1 = set.holes[k].vertices.elements[i];
            line_segment2 segment(v0, v1);
            auto len = length(segment);

            // Ensure the line segment has finite length
            if (len >= 1.0e-8)
            {
                // Discretize line segment and determine distance
                double spacing = 0.25;
                int split_count = std::min(std::max(int(std::ceil(len / spacing)), 2), 30);
                for (int j = 0; j < split_count; ++j)
                {
                    double dist = distance(segment, p);

                    if (dist < std::fabs(d2p))
                    {
                        bool pip = point_in_polygon(p, set.holes[k]);
                        d2p = pip ? dist : -dist;
                    }
                }
            }

            // Increment segment start point
            v0 = v1;
        }
    }

    return d2p;
}

namespace {

    typedef boost::polygon::point_data<int> boost_point;

    typedef boost::polygon::polygon_data<int> boost_polygon;

    typedef boost::polygon::polygon_with_holes_data<int>
        boost_polygon_with_holes;

    typedef boost::polygon::polygon_set_data<int> boost_polygon_set;

    void to_boost_polygon(boost_polygon& bp, polygon2 const& poly)
    {
        size_t n_points = poly.vertices.size();
        bp.coords_.resize(n_points);
        for (size_t i = 0; i != n_points; ++i)
        {
            vector<2,double> const& p = poly.vertices[i];
            bp.coords_[i] = boost_polygon::point_type(
                int(p[0] / clipper_integer_precision),
                int(p[1] / clipper_integer_precision));
        }
    }

    void to_boost_polygon(boost_polygon_set& boost_set,
        polyset const& cradle_set)
    {
        using namespace boost::polygon::operators;
        boost_set.clear();
        for (auto const& i : cradle_set.polygons)
        {
            boost_polygon poly;
            to_boost_polygon(poly, i);
            boost_set ^= poly;
        }
        for (auto const& i : cradle_set.holes)
        {
            boost_polygon poly;
            to_boost_polygon(poly, i);
            boost_set ^= poly;
        }
    }

    vector<2,double>
    from_boost_point(boost_point const& p)
    {
        return make_vector<double>(
            double(p.x()) * clipper_integer_precision,
            double(p.y()) * clipper_integer_precision);
    }

    void from_boost_polygon(polygon2& poly, boost_polygon const& bp)
    {
        size_t n_points = bp.size();
        if (n_points != 0)
        {
            // The first and last vertices are the same, so skip the last.
            --n_points;
            vertex2* vertices = allocate(&poly.vertices, n_points);
            for (size_t i = 0; i != n_points; ++i)
                vertices[i] = from_boost_point(bp.coords_[i]);
        }
        else
            clear(&poly.vertices);
    }

    void from_boost_polygon(polyset& cradle_set,
        boost_polygon_set const& boost_set)
    {
        cradle_set = polyset();
        std::vector<boost_polygon_with_holes> boost_polys;
        boost_set.get(boost_polys);
        for (auto const& bp : boost_polys)
        {
            polygon2 poly;
            from_boost_polygon(poly, bp.self_);
            cradle_set.polygons.push_back(poly);
            for (auto const& bh : bp.holes_)
            {
                polygon2 hole;
                from_boost_polygon(hole, bh);
                cradle_set.holes.push_back(hole);
            }
        }
    }

    void from_boost_polygon(std::vector<polygon2>& polys,
        boost_polygon_set const& set)
    {
        std::vector<boost_polygon> boost_polys;
        set.get(boost_polys);
        size_t n_polys = boost_polys.size();
        polys.resize(n_polys);
        for (size_t i = 0; i != n_polys; ++i)
        {
            boost_polygon const& bp = boost_polys[i];
            polygon2& poly = polys[i];
            from_boost_polygon(poly, bp);
        }
    }

} // anonymous namespace

std::vector<polygon2> as_polygon_list(polyset const& set)
{
    boost_polygon_set boost_set;
    to_boost_polygon(boost_set, set);
    std::vector<polygon2> polygons;
    from_boost_polygon(polygons, boost_set);
    return polygons;
}

bool almost_equal(polyset const& set1, polyset const& set2, double tolerance)
{
    polyset xor_;
    do_set_operation(&xor_, set_operation::XOR, set1, set2);
    return almost_equal(get_area(xor_), 0., tolerance);
}

bool almost_equal(polyset const& set1, polyset const& set2)
{
    polyset xor_;
    do_set_operation(&xor_, set_operation::XOR, set1, set2);
    return almost_equal(get_area(xor_), 0.);
}

void do_set_operation(
    polyset* result,
    set_operation op,
    polyset const& set1,
    polyset const& set2)
{
    ClipperLib::Clipper clipper;
    clipper.AddPolygons(to_clipper(set1), ClipperLib::ptSubject);
    clipper.AddPolygons(to_clipper(set2), ClipperLib::ptClip);
    ClipperLib::ClipType clipper_op;
    switch (op)
    {
     case set_operation::UNION:
        clipper_op = ClipperLib::ctUnion;
        break;
     case set_operation::INTERSECTION:
        clipper_op = ClipperLib::ctIntersection;
        break;
     case set_operation::DIFFERENCE:
        clipper_op = ClipperLib::ctDifference;
        break;
     case set_operation::XOR:
        clipper_op = ClipperLib::ctXor;
        break;
     default:
         throw exception("set operation undefined");
    }
    ClipperLib::Polygons solution;
    clipper.Execute(clipper_op, solution);
    from_clipper(result, solution);
}

api(fun)
polyset
polyset_combination(set_operation op, std::vector<polyset> const& polysets)
{
    if (polysets.empty())
    {
        return polyset();
    }
    else if (polysets.size() == 1)
    {
        return polysets[0];
    }
    polyset tmp[2];
    int output_index = 0;
    polyset const* input = &polysets.front();
    auto i = polysets.begin();
    ++i;
    for (; i != polysets.end(); ++i)
    {
        output_index = 1 - output_index;
        auto output = &tmp[output_index];
        do_set_operation(output, op, *input, *i);
        input = output;
    }
    return tmp[output_index];
}

// Triangulate a convex Boost polygon.
// Note that this does NOT clear the list of triangles.
// It simply pushes more triangles onto the back of it.
static void
triangulate_boost_polygon(
    std::vector<triangle<2,double> >& tris, boost_polygon const& poly)
{
    size_t n_points = poly.size();
    if (n_points > 0)
    {
        // The first and last vertices are the same, so skip the last.
        --n_points;
        for (size_t i = 2; i < n_points; ++i)
        {
            triangle<2,double> tri;
            tri[0] = from_boost_point(poly.coords_[0]);
            tri[1] = from_boost_point(poly.coords_[i - 1]);
            tri[2] = from_boost_point(poly.coords_[i]);
            tris.push_back(tri);
        }
    }
}

std::vector<triangle<2,double> >
triangulate_polyset(polyset const& set)
{
    boost_polygon_set boost_set;
    to_boost_polygon(boost_set, set);
    std::vector<boost_polygon> trapezoids;
    boost_set.get_trapezoids(trapezoids);
    size_t n_trapezoids = trapezoids.size();
    std::vector<triangle<2,double> > tris;
    tris.reserve(n_trapezoids * 2);
    for (size_t i = 0; i != n_trapezoids; ++i)
        triangulate_boost_polygon(tris, trapezoids[i]);
    return tris;
}

void
expand(polyset* dst, polyset const& src, double amount)
{
    using namespace boost::polygon::operators;
    auto clipper_in = to_clipper(src);
    ClipperLib::Polygons clipper_out;
    OffsetPaths(clipper_in, clipper_out, amount / clipper_integer_precision,
        ClipperLib::jtRound, ClipperLib::etClosed);
    from_clipper(dst, clipper_out);
}

polyset polyset_expansion(polyset const& src, double amount)
{
    polyset result;
    expand(&result, src, amount);
    return result;
}

void create_polyset_from_polygons(polyset* set,
    std::vector<polygon2> const& polygons)
{
    using namespace boost::polygon::operators;
    boost_polygon_set boost_set;
    for (std::vector<polygon2>::const_iterator
        i = polygons.begin(); i != polygons.end(); ++i)
    {
        boost_polygon bp;
        to_boost_polygon(bp, *i);
        boost_set += bp;
    }
    from_boost_polygon(*set, boost_set);
}

// STRUCTURES

bool is_inside(structure_geometry_slice const& s, double p)
{
    return ((p >= s.position - (s.thickness / 2.)) &&
        (p < s.position + (s.thickness / 2.)));
}

void static
reset_structure_to_slice_list(
    structure_geometry* structure,
    slice_description_list const& slices)
{
    structure->slices.clear();
    structure->master_slice_list.clear();
    structure->master_slice_list = slices;
}

structure_geometry static
remove_empty_slices(structure_geometry const& structure)
{
    structure_geometry result;
    result.master_slice_list = structure.master_slice_list;
    for (auto const& slice : structure.slices)
    {
        if (!is_empty(slice.second))
        {
            result.slices.insert(slice);
        }
    }
    return result;
}

polyset
get_slice(structure_geometry const& structure, double position)
{
    auto slice = get_structure_slice(structure, position);
    if (slice)
    {
        return get(slice).region;
    }

    return polyset();
}

structure_geometry_slice
find_slice_at_exact_position(
    structure_polyset_list const& slices,
    double position,
    double thickness)
{
    auto s = slices.find(position);
    if (s != slices.end())
    {
        return structure_geometry_slice(position, thickness, s->second);
    }
    return structure_geometry_slice(position, thickness, polyset());
}

optional<structure_geometry_slice>
get_structure_slice(structure_geometry const& structure, double position)
{
    auto const& masters = structure.master_slice_list;
    auto const& slices = structure.slices;
    auto p = position;

    if (masters.size() == 0 ||
        p < masters[0].position - 0.5 * masters[0].thickness ||
        p > masters.back().position + 0.5 * masters.back().thickness)
    {
        return none;
    }

    for (size_t i = 1; i < masters.size(); ++i)
    {
        if (p < masters[i].position)
        {
            if (p - masters[i - 1].position < masters[i].position - p)
            {
                return
                    some(
                        find_slice_at_exact_position(
                            slices,
                            masters[i - 1].position,
                            masters[i - 1].thickness));
            }
            else
            {
                return
                    some(
                        find_slice_at_exact_position(
                            slices,
                            masters[i].position,
                            masters[i].thickness));
            }
        }
    }
    // Valid case when p is past the last slice position, but within its thickness
    return
        some(
            find_slice_at_exact_position(
                slices,
                masters.rbegin()->position,
                masters.rbegin()->thickness));
}

std::vector<structure_geometry_slice>
get_structure_slices(structure_geometry const& structure, double p_low, double p_high)
{
    auto const& masters = structure.master_slice_list;
    auto const& slices = structure.slices;

    std::vector<structure_geometry_slice> output;

    if (masters.size() == 0 ||
        p_low > masters.back().position + 0.5 * masters.back().thickness ||
        p_high < masters[0].position - 0.5 * masters[0].thickness)
    {
        return output;
    }

    // Find the start and end slice indices
    // Note: the initial value set below is critical to this functioning properly
    size_t i_start = masters.size();

    for (size_t i = 1; i < masters.size(); ++i)
    {
        if (p_low < masters[i].position)
        {
            if (p_low - masters[i - 1].position < masters[i].position - p_low)
            {
                i_start = i - 1;
            }
            else
            {
                i_start = i;
            }
            output.push_back(
                find_slice_at_exact_position(
                    slices,
                    masters[i_start].position,
                    masters[i_start].thickness));
            break;
        }
    }

    for (size_t i = i_start + 1; i < masters.size(); ++i)
    {
        if (p_high < masters[i].position)
        {
            if (p_high - masters[i - 1].position < masters[i].position - p_high)
            {
                // Last slice is already in the list, just return now
                return output;
            }
            else
            {
                // Add the last slice and then return
                output.push_back(
                    find_slice_at_exact_position(
                        slices,
                        masters[i].position,
                        masters[i].thickness));
                return output;
            }
        }

        // Slice is within limits, add it
        output.push_back(
            find_slice_at_exact_position(
                slices,
                masters[i].position,
                masters[i].thickness));
    }

    // All slices have been added (but never reached p_high position), just return
    return output;
}

void static
fold_in(double& volume, vector3d& centroid,
    std::pair<double,vector2d> const& area_and_centroid,
    double z, double thickness)
{
    volume += area_and_centroid.first * thickness;
    centroid += unslice(area_and_centroid.second, 2, z) *
        area_and_centroid.first * thickness;
}

void static
fold_in_above(double& volume, vector3d& centroid,
    std::pair<double,vector2d> const& area_and_centroid,
    double z, double thickness)
{
    fold_in(volume, centroid, area_and_centroid, z + thickness / 2, thickness);
}

void static
fold_in_below(double& volume, vector3d& centroid,
    std::pair<double,vector2d> const& area_and_centroid,
    double z, double thickness)
{
    fold_in(volume, centroid, area_and_centroid, z - thickness / 2, thickness);
}

std::pair<double,vector3d> static
get_volume_and_centroid(structure_geometry const& structure)
{
    auto volume = 0.;
    auto centroid = make_vector(0., 0., 0.);

    auto const& masters = structure.master_slice_list;
    auto const& slices = structure.slices;

    auto begin_m = masters.begin();
    auto end_m = masters.end();
    auto i = begin_m;
    auto end_slice = slices.end();

    while (i != end_m)
    {
        auto slice = slices.find(i->position);
        if (slice != end_slice && get_area(slice->second) != 0.)
        {
            break;
        }
        ++i;
    }

    for (; i != end_m; ++i)
    {
        auto slice = slices.find(i->position);
        if (slice == end_slice) { continue; }

        auto i_info = get_area_and_centroid(slice->second);

        // Add lower half of slice thickness
        auto temp = i;
        if (i == begin_m)
        {
            fold_in_below(volume, centroid, i_info, i->position, i->thickness * 0.5);
        }
        else
        {
            --temp;
            fold_in_below(
                volume, centroid,
                i_info,
                i->position,
                0.5 * (i->position - temp->position));
        }

        // Add upper half of slice thickness
        temp = i;
        ++temp;
        if (temp == end_m)
        {
            fold_in_above(volume, centroid, i_info, i->position, i->thickness * 0.5);
        }
        else
        {
            fold_in_above(
                volume, centroid,
                i_info,
                i->position,
                0.5 * (temp->position - i->position));
        }
    }

    if (volume != 0.)
    {
        centroid /= volume;
    }

    return std::make_pair(volume, centroid);
}

double get_volume(structure_geometry const& structure)
{
    return get_volume_and_centroid(structure).first;
}

vector3d get_centroid(
    structure_geometry const& structure)
{
    auto pair = get_volume_and_centroid(structure);
    if (pair.first == 0)
        throw exception("centroid requested for empty structure");
    return pair.second;
}

bool is_inside(structure_geometry const& structure, vector<3,double> const& p)
{
    auto s = get_slice(structure, p[2]);
    return is_inside(s, slice(p, 2));
}

slice_description_list
get_slice_descriptions(structure_geometry const& s)
{
    return s.master_slice_list;
}

bool
almost_equal(
    structure_geometry const& volume1,
    structure_geometry const& volume2,
    double tolerance)
{
    if (volume1.master_slice_list.size() != volume2.master_slice_list.size())
        return false;

    auto i1 = volume1.master_slice_list.begin();
    auto end1 = volume1.master_slice_list.end();
    auto i2 = volume2.master_slice_list.begin();

    auto const& slices1 = volume1.slices;
    auto const& slices2 = volume2.slices;

    for (; i1 != end1; ++i1, ++i2)
    {
        if (!almost_equal(i1->position, i2->position, tolerance) ||
            !almost_equal(i1->thickness, i2->thickness, tolerance))
        {
            return false;
        }
        auto r1 = find_slice_at_exact_position(slices1, i1->position, i1->thickness);
        auto r2 = find_slice_at_exact_position(slices2, i2->position, i2->thickness);
        if (!almost_equal(r1.region, r2.region, tolerance))
        {
            return false;
        }
    }

    return true;
}

bool almost_equal(structure_geometry const& a, structure_geometry const& b)
{
    return almost_equal(a, b, default_equality_tolerance<double>());
}

void do_set_operation(
    structure_geometry* result,
    set_operation op,
    structure_geometry const& structure1,
    structure_geometry const& structure2)
{
    if (structure1.master_slice_list != structure2.master_slice_list)
    {
        throw exception("structure set_operation requires matching master lists");
    }

    auto const& master_slices = structure1.master_slice_list;

    result->master_slice_list = master_slices;
    result->slices.clear();
    for (auto const& i : master_slices)
    {
        auto s1 =
            find_slice_at_exact_position(structure1.slices, i.position, i.thickness);
        auto s2 =
            find_slice_at_exact_position(structure2.slices, i.position, i.thickness);

        if (!is_empty(s1.region) || !is_empty(s2.region))
        {
            polyset region;
            do_set_operation(&region, op, s1.region, s2.region);
            if (!is_empty(region))
            {
                result->slices[i.position] = region;
            }
        }
    }
}

structure_geometry
structure_combination(
    set_operation op,
    std::vector<structure_geometry> const& structures)
{
    if (structures.size() < 2)
    {
        throw exception("structure_combination requires at least two structures");
    }
    structure_geometry tmp[2];
    int output_index = 0;
    structure_geometry const* input = &structures.front();
    auto i = structures.begin();
    ++i;
    for (; i != structures.end(); ++i)
    {
        output_index = 1 - output_index;
        auto output = &tmp[output_index];
        do_set_operation(output, op, *input, *i);
        input = output;
    }
    return tmp[output_index];
}

std::vector<double>
slice_position_list(structure_geometry const& structure)
{
    return map(
        [](slice_description const& slice)
        {
            return slice.position;
        },
        structure.master_slice_list);
}

void expand_in_2d(
    structure_geometry* result,
    structure_geometry const& structure,
    double amount)
{
    auto slice_descriptions = get_slice_descriptions(structure);
    reset_structure_to_slice_list(result, slice_descriptions);
    for (auto const& slice : structure.slices)
    {
        polyset p;
        expand(&p, slice.second, amount);
        result->slices[slice.first] = p;
    }
}

box<2,double>
bounding_box(polygon2 const& poly)
{
    optional<box<2,double> > box;
    compute_bounding_box(box, poly);
    // If there is no box, then the polygon is empty, so just return any box.
    return box ? box.get() :
        make_box(make_vector(0., 0.), make_vector(0., 0.));
}
void compute_bounding_box(
    optional<box<2,double> >& box,
    polygon2 const& poly)
{
    vertex2_array::const_iterator
        i = poly.vertices.begin(),
        end = poly.vertices.end();
    if (i == end)
        return;

    vector<2,double> min, max;
    if (box)
    {
        min = get_low_corner(box.get());
        max = get_high_corner(box.get());
    }
    else
    {
        min = *i;
        max = *i;
        ++i;
    }

    for (; i != end; ++i)
    {
        for (unsigned j = 0; j != 2; ++j)
        {
            if ((*i)[j] < min[j])
                min[j] = (*i)[j];
            if ((*i)[j] > max[j])
                max[j] = (*i)[j];
        }
    }

    box = cradle::box<2,double>(min, max - min);
}

// for a polyset
box<2,double> bounding_box(polyset const& region)
{
    optional<box<2,double> > box;
    compute_bounding_box(box, region);
    // If there is no box, then the polyset is empty, so just return any box.
    return box ? box.get() :
        make_box(make_vector(0., 0.), make_vector(0., 0.));
}
void compute_bounding_box(optional<box<2,double> >& box, polyset const& region)
{
    for (auto const& i : region.polygons)
        compute_bounding_box(box, i);
}

// for a structure_geometry
box<3,double> bounding_box(structure_geometry const& structure)
{
    optional<box<3,double> > box;
    compute_bounding_box(box, structure);
    // If there is no box, then the structure is empty, so just return any box.
    return box ? box.get() :
        make_box(make_vector(0., 0., 0.), make_vector(0., 0., 0.));
}
void compute_bounding_box(optional<box<3,double> >& box,
    structure_geometry const& structure)
{
    optional<cradle::box<2,double> > xy_box;
    if (box)
    {
        xy_box = slice(box.get(), 2);
    }

    double zmin = 1.0e100;
    double zmax = -1.0e100;
    auto master_slices = get_slice_descriptions(structure);
    for (auto const& m_slice : master_slices)
    {
        auto slice =
            find_slice_at_exact_position(
                structure.slices,
                m_slice.position,
                m_slice.thickness);
        if (!is_empty(slice.region))
        {
            compute_bounding_box(xy_box, slice.region);

            if (slice.position - 0.5 * slice.thickness < zmin)
            {
                zmin = slice.position - 0.5 * slice.thickness;
            }
            if (slice.position + 0.5 * slice.thickness > zmax)
            {
                zmax = slice.position + 0.5 * slice.thickness;
            }
        }
    }

    if (!structure.slices.empty() && xy_box)
    {
        //double z_min = structure.slices.begin()->position -
        //    structure.slices.begin()->thickness / 2;
        //auto last = structure.slices.end();
        //--last;
        //double z_max = last->position + last->thickness / 2;

        vector<2,double> const& xy_corner = xy_box.get().corner;
        vector<2,double> const& xy_size = xy_box.get().size;

        box = cradle::box<3,double>(
            make_vector(xy_corner[0], xy_corner[1], zmin),
            make_vector(xy_size[0], xy_size[1], zmax - zmin));
    }
}

bool
overlapping(
    box3d const& box,
    structure_geometry const& sg,
    unsigned structure_axis,
    optional<box3d> const& sg_bounds)
{
    if (sg_bounds)
    {
        if (!overlapping(box, get(sg_bounds)))
        {
            return false;
        }
    }

    // Get slices that the box contains
    auto slices =
        get_structure_slices(
            sg,
            box.corner[structure_axis],
            get_high_corner(box)[structure_axis]);

    // Check for overlap on each slice
    for (auto const& s : slices)
    {
        if (s.region.polygons.size() == 0)
        {
            continue;
        }

        auto const box2 = slice(box, structure_axis);

        // Check center of voxel as this may be a fast "short-circuit" for many cases
        if (point_in_polyset(get_center(box2), s.region))
        {
            return true;
        }
        else
        {
            // Check all points in the polyset
            // Note this isn't 100% accurate because holes can actually make a polygon
            // vertex outside and we don't catch that case
            for (auto const& p : s.region.polygons)
            {
                for (auto const& v : p.vertices)
                {
                    if (contains(box2, v))
                    {
                        return true;
                    }
                }
            }
        }
    }

    return false;
}

bool almost_equal(polygon2 const& a, polygon2 const& b, double tolerance)
{
    polyset region_a, region_b, xor_region;
    create_polyset(&region_a, a);
    create_polyset(&region_b, b);
    do_set_operation(&xor_region, set_operation::XOR, region_a, region_b);
    return almost_equal(get_area(xor_region), 0., tolerance);
}
bool almost_equal(polygon2 const& a, polygon2 const& b)
{
    polyset region_a, region_b, xor_region;
    create_polyset(&region_a, a);
    create_polyset(&region_b, b);
    do_set_operation(&xor_region, set_operation::XOR, region_a, region_b);
    return almost_equal(get_area(xor_region), 0.);
}

}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

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

 /*!

    @ingroup group-trigonometric
    Function object implementing cscd capabilities

    cosecante in degree.

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = cscd(x);
    @endcode

    is similar to:

    @code
    T r = rec(sind(x));
    @endcode

    As most other trigonometric function cscd can be called with a second optional parameter
    which is a tag on speed and accuracy (see @ref cos for further details)

    @see csc, cscpi,

  **/
  const boost::dispatch::functor<tag::cscd_> cscd = {};
} }
#endif

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

#endif


/*
* Bundle adjustment using ceres
*/

//#define V3DLIB_ENABLE_SUITESPARSE
#include "BAHandler.h"

// #include "ba/v3d_linear.h"
// #include "ba/v3d_vrmlio.h"
// #include "ba/v3d_metricbundle.h"
// #include "ba/v3d_stereobundle.h"

#include "Camera.h"
// #include "ceres/EigenQuaternionParameterization.h"
#include "datastructs/Data.h"

#include <opencv2/features2d/features2d.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <ceres/rotation.h>

#include <Eigen/Eigen>

// using namespace V3D;
using namespace std;
using namespace cv;

// namespace
// {
// 	inline double
// 	showErrorStatistics(double const f0,
// 						StdDistortionFunction const& distortion,
// 						vector<CameraMatrix> const& cams,
// 						vector<Vector3d> const& Xs,
// 						vector<Vector2d> const& measurements,
// 						vector<int> const& correspondingView,
// 						vector<int> const& correspondingPoint)
// 	{
// 		int const K = measurements.size();

// 		double meanReprojectionError = 0.0;
// 		for (int k = 0; k < K; ++k)
// 		{
// 			int const i = correspondingView[k];
// 			int const j = correspondingPoint[k];
// 			Vector2d p = cams[i].projectPoint(distortion, Xs[j]);

// 			double reprojectionError = norm_L2(f0 * (p - measurements[k]));
// 			meanReprojectionError += reprojectionError;
// 		}
// 		//cout << "mean reprojection error (in pixels): " << meanReprojectionError/K << endl;

// 		return meanReprojectionError/K;
// 	}
// }

// void BAHandler::adjustBundle(){

// 	cout<<"Bundle Adjust Method: no copy"<<endl;
// 	Data 		&data 	= Data::GetInstance();
// 	Camera 		&camera	= Camera::GetInstance();
// 	//important
// 	data.deleteTrashes();

// 	Mat &cam_matrix 				= camera.camMat;
// 	Mat &distortion_coefficients	= camera.distortionMat;

// 	vector<int>			imgIdxs;
// 	vector<int>			pt3DIdxs;
// 	double ppx,ppy;
// 	double 	meanErrorBefore, meanErrorAfter;

// 	int N = data.countFrames(), M = data.countLandMarks(), K = data.countMeasurements();
// 	cout << "N (cams) = " << N << " M (points) = " << M << " K (measurements) = " << K << endl;

// 	StdDistortionFunction distortion;
// 	distortion.k1 = distortion_coefficients.at<double>(0,0);
// 	distortion.k2 = distortion_coefficients.at<double>(0,1);
// 	distortion.p1 = distortion_coefficients.at<double>(0,2);
// 	distortion.p2 = distortion_coefficients.at<double>(0,3);

// 	//convert camera intrinsics to BA datastructs
// 	Matrix3x3d KMat;
// 	makeIdentityMatrix(KMat);
// 	KMat[0][0] = cam_matrix.at<double>(0,0); //fx
// 	KMat[1][1] = cam_matrix.at<double>(1,1); //fy
// 	KMat[0][1] = cam_matrix.at<double>(0,1); //skew
// 	KMat[0][2] = cam_matrix.at<double>(0,2); //ppx
// 	KMat[1][2] = cam_matrix.at<double>(1,2); //ppy

// 	ppx = KMat[0][2];
// 	ppy = KMat[1][2];

// 	double const f0 = KMat[0][0];
// 	double const f0_inv = 1.0/f0;

// 	//cout << "Cam = "<<endl;
// 	//displayMatrix(KMat);

// 	Matrix3x3d Knorm = KMat;
// 	// Normalize the intrinsic to have unit focal length.
// 	scaleMatrixIP(f0_inv, Knorm);
// 	Knorm[2][2] = 1.0;

// 	//convert 3D point cloud to BA datastructs
// 	const vector<LandMark::Ptr> &lms = data.getLandMarks();
// 	map<LandMark::Ptr, int, Data::LandMarkPtrCompare> lm2idx;
// 	vector<Vector3d > Xs;
// 	int idx = 0;
// 	Xs.reserve(lms.size());
// 	for (vector<LandMark::Ptr>::const_iterator it = lms.begin(); it!=lms.end(); ++it, ++idx)
// 	{
// 		assert(!(*it)->deleted);
// 		Xs.push_back(Vector3d((*it)->pt[0], (*it)->pt[1], (*it)->pt[2]));
// 		lm2idx[*it] = idx;
// 	}

// 	//convert cameras to BA datastructs
// 	const vector<Frame::Ptr> &frames = data.getFrames();
// 	map<Frame::Ptr, int, Data::FramePtrCompare> frame2idx;
// 	vector<CameraMatrix> cams;
// 	cams.reserve(frames.size());
// 	idx = 0;
// 	for (vector<Frame::Ptr>::const_iterator it = frames.begin(); it!=frames.end(); ++it, ++idx)
// 	{
// 		Matrix3x3d R;
// 		Vector3d T;

// 		Matx34d P = (*it)->getCVTransform();

// 		R[0][0] = P(0,0); R[0][1] = P(0,1); R[0][2] = P(0,2); T[0] = P(0,3);
// 		R[1][0] = P(1,0); R[1][1] = P(1,1); R[1][2] = P(1,2); T[1] = P(1,3);
// 		R[2][0] = P(2,0); R[2][1] = P(2,1); R[2][2] = P(2,2); T[2] = P(2,3);

// 		CameraMatrix cam;
// 		cam.setIntrinsic(Knorm);
// 		cam.setRotation(R);
// 		cam.setTranslation(T);
// 		cams.push_back(cam);
// 		frame2idx[*it] = idx;
// 	}

// 	const vector<Measurement::Ptr> &ms = data.getMeasurements();
// 	vector<Vector2d > measurements;
// 	vector<int> correspondingView; 		//corresponding camera mat idx
// 	vector<int> correspondingPoint;		//corresponding 3d point idx
// 	measurements.reserve(K);
// 	correspondingView.reserve(K);
// 	correspondingPoint.reserve(K);

// 	//convert 2D measurements to BA datastructs
// 	for (vector<Measurement::Ptr>::const_iterator it = ms.begin(); it!=ms.end(); ++it){
// 		assert(!(*it)->deleted);
// 		LandMark::Ptr &lm = (*it)->landmark;
// 		Frame::Ptr &frame = (*it)->frame;
// 		Point2f &xy		  = frame->kpts[(*it)->featureIdx].pt;
// 		measurements.push_back(Vector2d(xy.x*f0_inv, xy.y*f0_inv));
// 		correspondingView.push_back(frame2idx[frame]);
// 		correspondingPoint.push_back(lm2idx[lm]);
// 	}

// 	meanErrorBefore = showErrorStatistics(f0, distortion, cams, Xs, measurements, correspondingView, correspondingPoint);

// 	double const inlierThreshold = 2.0 / fabs(f0);

// 	Matrix3x3d K0 = cams[0].getIntrinsic();
// 	//cout << "K0 = "; displayMatrix(K0);

// 	bool good_adjustment = false;
// 	{
// 		ScopedBundleExtrinsicNormalizer extNorm(cams, Xs);
// 		ScopedBundleIntrinsicNormalizer intNorm(cams,measurements,correspondingView);
// 		CommonInternalsMetricBundleOptimizer opt(V3D::FULL_BUNDLE_FOCAL_LENGTH_PP, inlierThreshold, K0, distortion, cams, Xs,
// 												 measurements, correspondingView, correspondingPoint);

// 		opt.tau = 1e-3;
// 		opt.maxIterations = 50;
// 		opt.minimize();

// 		cout << "optimizer status = " << opt.status << endl;

// 		good_adjustment = (opt.status != 2);
// 	}

// 	//cout << "refined K = "; displayMatrix(K0);

// 	for (int i = 0; i < N; ++i) cams[i].setIntrinsic(K0);

// 	Matrix3x3d Knew = K0;
// 	scaleMatrixIP(f0, Knew);
// 	Knew[2][2] = 1.0;

// 	//cout << "Cam new = "<<endl;
// 	//displayMatrix(Knew);

// 	meanErrorAfter = showErrorStatistics(f0, distortion, cams, Xs, measurements, correspondingView, correspondingPoint);

// 	if(good_adjustment) {

// 		//extract 3D points
// 		for (unsigned int j = 0; j < Xs.size(); ++j)
// 		{
// 			lms[j]->pt[0] = Xs[j][0];
// 			lms[j]->pt[1] = Xs[j][1];
// 			lms[j]->pt[2] = Xs[j][2];
// 		}

// 		//extract adjusted cameras
// 		for (unsigned int i = 0; i < N; ++i)
// 		{
// 			Matrix3x3d R = cams[i].getRotation();
// 			Vector3d T = cams[i].getTranslation();

// 			Eigen::Matrix3d R_eigen;
// 			Eigen::Vector3d t_eigen;
// 			for(unsigned int r = 0; r<3; r++){
// 				for(unsigned int c = 0; c<3; c++){
// 					R_eigen(r,c) = R[r][c];
// 				}
// 				t_eigen(r) = T[r];
// 			}
// 			frames[i]->rotation = Eigen::Quaterniond(R_eigen);
// 			frames[i]->position = -R_eigen.transpose()*t_eigen;
// 		}

// 		cam_matrix.at<double>(0,0) = Knew[0][0];
// 		cam_matrix.at<double>(0,1) = Knew[0][1];
// 		cam_matrix.at<double>(0,2) = Knew[0][2];
// 		cam_matrix.at<double>(1,1) = Knew[1][1];
// 		cam_matrix.at<double>(1,2) = Knew[1][2];
// 	}

// 	cout<<"focal :"<<f0<<" -> "<<Knew[0][0]<<endl;
// 	cout<<"center:("<<ppx<<","<<ppy<<") -> ("<<Knew[0][2]<<","<<Knew[1][2]<<")"<<endl;
// 	cout<<"error :"<<meanErrorBefore<<" -> "<<meanErrorAfter<<endl;
// }

struct ReprojectionError {
	// (u, v): the position of the observation with respect to the image top left corner
	  // u is rightward positive and v is downward positive
	ReprojectionError(double observed_x, double observed_y, double f, double ppx, double ppy)
	:observed_x(observed_x)
	,observed_y(observed_y)
	,f(f)
	,ppx(ppx)
	,ppy(ppy)
	{

	}

	  template <typename T>
	  bool operator()(const T* const camera_rotation,
			  	  	  const T* const camera_translation,
	                  const T* const point,
	                  T* residuals) const {

		  //XXX: quaternion must be normalized before!
		  // Map the T* array to an Eigen Quaternion (no copy)
		  Eigen::Quaternion<T> q = Eigen::Map<const Eigen::Quaternion<T>>(camera_rotation);

		  // Map T* to Eigen Vector3 (no copy)
		  Eigen::Matrix<T,3,1> t = Eigen::Map<const Eigen::Matrix<T,3,1>>(camera_translation);

		  //copy point data
		  Eigen::Matrix<T,3,1> p;
		  p << T(point[0]), T(point[1]), T(point[2]);

		  //transform point to camera view space
		  p = q*(p-t);	//q is rotation of camera back to world axis, t is position of camera in world (opengv convention)
		  //p = q*p+t		//q is rotation of camera back to world axis, t is translation of camera before rotation (opencv convention)

	    // Compute the center of distortion. The sign change comes from
	    // the camera model that Noah Snavely's Bundler assumes, whereby
	    // the camera coordinate system has a negative z axis.
	    const T xp = p[0] / p[2];
	    const T yp = p[1] / p[2];

	    // Compute final projected point position.
	    const T predicted_x = f * xp+ppx;
	    const T predicted_y = f * yp+ppy;
	    //const T predicted_x = -f * xp+ppx;
	    //const T predicted_y = f * yp+ppy;

	    // The error is the difference between the predicted and observed position.
	    residuals[0] = predicted_x - observed_x;
	    residuals[1] = predicted_y - observed_y;

	    return true;
	  }

	  // Factory to hide the construction of the CostFunction object from
	  // the client code.
	  static ceres::CostFunction* Create(const double observed_x,
	                                     const double observed_y,
										 const double f,
										 const double ppx,
										 const double ppy) {
	    return (new ceres::AutoDiffCostFunction<
	    		ReprojectionError, 2, 4, 3, 3>(
	                new ReprojectionError(observed_x,
	                                      observed_y,
										  f,
										  ppx,
										  ppy)));
	  }

	  double observed_x;
	  double observed_y;
	  //since we fix camera paramters, use them as residule rather than parameter
	  //this avoids unnecessary jacobian calculation which speeds up a lot
	  double f;
	  double ppx;
	  double ppy;
};

// struct OctaveAwareReprojectionError {
// 	// (u, v): the position of the observation with respect to the image top left corner
// 	  // u is rightward positive and v is downward positive
// 	OctaveAwareReprojectionError(double observed_x, double observed_y, double f, double ppx, double ppy, double octave)
// 	:observed_x(observed_x)
// 	,observed_y(observed_y)
// 	,f(f)
// 	,ppx(ppx)
// 	,ppy(ppy)
// 	,octave(octave)
// 	{

// 	}

// 	  template <typename T>
// 	  bool operator()(const T* const camera_rotation,
// 			  	  	  const T* const camera_translation,
// 	                  const T* const point,
// 	                  T* residuals) const {

// 		  //XXX: quaternion must be normalized before!
// 		  // Map the T* array to an Eigen Quaternion (no copy)
// 		  Eigen::Quaternion<T> q = Eigen::Map<const Eigen::Quaternion<T>>(camera_rotation);

// 		  // Map T* to Eigen Vector3 (no copy)
// 		  Eigen::Matrix<T,3,1> t = Eigen::Map<const Eigen::Matrix<T,3,1>>(camera_translation);

// 		  //copy point data
// 		  Eigen::Matrix<T,3,1> p;
// 		  p << T(point[0]), T(point[1]), T(point[2]);

// 		  //transform point to camera view space
// 		  p = q*(p-t);	//q is rotation of camera back to world axis, t is position of camera in world (opengv convention)
// 		  //p = q*p+t		//q is rotation of camera back to world axis, t is translation of camera before rotation (opencv convention)

// 	    // Compute the center of distortion. The sign change comes from
// 	    // the camera model that Noah Snavely's Bundler assumes, whereby
// 	    // the camera coordinate system has a negative z axis.
// 	    const T xp = p[0] / p[2];
// 	    const T yp = p[1] / p[2];

// 	    // Compute final projected point position.
// 	    const T predicted_x = f * xp+ppx;
// 	    const T predicted_y = f * yp+ppy;
// 	    //const T predicted_x = -f * xp+ppx;
// 	    //const T predicted_y = f * yp+ppy;

// 	    // The error is the difference between the predicted and observed position.
// 	    residuals[0] = (predicted_x - observed_x)/octave;
// 	    residuals[1] = (predicted_y - observed_y)/octave;

// 	    return true;
// 	  }

// 	  // Factory to hide the construction of the CostFunction object from
// 	  // the client code.
// 	  static ceres::CostFunction* Create(const double observed_x,
// 	                                     const double observed_y,
// 										 const double f,
// 										 const double ppx,
// 										 const double ppy,
// 										 const double octave) {
// 	    return (new ceres::AutoDiffCostFunction<
// 	    		OctaveAwareReprojectionError, 2, 4, 3, 3>(
// 	                new OctaveAwareReprojectionError(observed_x,
// 	                                      observed_y,
// 										  f,
// 										  ppx,
// 										  ppy,
// 										  octave)));
// 	  }

// 	  double observed_x;
// 	  double observed_y;
// 	  //since we fix camera paramters, use them as residule rather than parameter
// 	  //this avoids unnecessary jacobian calculation which speeds up a lot
// 	  double f;
// 	  double ppx;
// 	  double ppy;
// 	  double octave;
// };

void BAHandler::adjustBundle_ceres_nocopy(){
	cout<<"Bundle Adjust Method: no copy"<<endl;

	clock_t				time;
	double				t_preprocess;

	time				= clock();
	Data 		&data 	= Data::GetInstance();
	Camera 		&camera	= Camera::GetInstance();
	//important
	data.deleteTrashes();

	double intrinsics[3];
	intrinsics[0] = camera.getCamFocal();
	intrinsics[1] = camera.getCamPrinciple().x;
	intrinsics[2] = camera.getCamPrinciple().y;

	ceres::Problem problem;
	ceres::LocalParameterization *eigenQuaternionParameterization = new ceres::EigenQuaternionParameterization;

	const vector<Measurement::Ptr> &ms = data.getMeasurements();
	for(vector<Measurement::Ptr>::const_iterator it = ms.begin(); it!=ms.end(); ++it){

		assert(!((*it)->deleted));

		Frame::Ptr 		&f 	= (*it)->frame;
		LandMark::Ptr 	&p 	= (*it)->landmark;
		int			&feIdx	= (*it)->featureIdx;

		Point2f &pt2D 	= f->kpts[feIdx].pt;
		double *q		= f->rotation.coeffs().data();
		double *t		= f->position.data();
		double *point	= p->pt.data();

		ceres::CostFunction* cost_function = ReprojectionError::Create( pt2D.x, pt2D.y, intrinsics[0], intrinsics[1], intrinsics[2]);
		problem.AddResidualBlock(cost_function, NULL, q, t, point);
		problem.SetParameterization(q,eigenQuaternionParameterization);
		if(f->fixed){
			//fix first camera transformation
			problem.SetParameterBlockConstant(q);
			problem.SetParameterBlockConstant(t);
		}
	}

	t_preprocess = double(clock()-time) / CLOCKS_PER_SEC;

	// Set a few options
	ceres::Solver::Options options;
	options.use_nonmonotonic_steps = true;
	options.preconditioner_type = ceres::SCHUR_JACOBI;
	options.linear_solver_type = ceres::SPARSE_SCHUR; //ceres::ITERATIVE_SCHUR; //
	options.max_num_iterations = 100;
	//options.max_solver_time_in_seconds = 0.015;

	ceres::Solver::Summary summary;
	ceres::Solve(options, &problem, &summary);

	//std::cout << "Final report:\n" << summary.FullReport();
	std::cout << "Time(s) preprocess :"<<t_preprocess<<endl;
	std::cout << "Time(s) BA :"<<summary.total_time_in_seconds<<endl;

}

void BAHandler::adjustBundle_ceres_local_nocopy(){

	cout<<"Bundle Adjust Method: local no copy"<<endl;

	clock_t				time;
	double				t_preprocess;

	time				= clock();
	Data 		&data 	= Data::GetInstance();
	Camera 		&camera	= Camera::GetInstance();
	//important
	data.deleteTrashes();

	double intrinsics[3];
	intrinsics[0] = camera.getCamFocal();
	intrinsics[1] = camera.getCamPrinciple().x;
	intrinsics[2] = camera.getCamPrinciple().y;

	ceres::Problem problem;
	ceres::LocalParameterization *eigenQuaternionParameterization = new ceres::EigenQuaternionParameterization;

	if(data.countFrames()<1) return;
	//fix all frames other than the last frame
	data.fixAllFrames();
	Frame::Ptr lastAddedFrame = data.getFrames().back();
	lastAddedFrame -> fixed = false;

	//for all measures in last added frame
	const vector<Measurement::Ptr> ms = data.getMeasurements(lastAddedFrame);
	for(vector<Measurement::Ptr>::const_iterator it = ms.begin(); it!=ms.end(); ++it){
		LandMark::Ptr &lmk = (*it)->landmark;
		//for all measurements tied to this landmark
		vector<Measurement::Ptr> lmkms = data.getMeasurements(lmk);
		for(vector<Measurement::Ptr>::const_iterator jt = lmkms.begin(); jt!=lmkms.end(); ++jt){
			assert(!((*jt)->deleted));

			Frame::Ptr 		&f 	= (*jt)->frame;
			LandMark::Ptr 	&p 	= (*jt)->landmark;
			int			&feIdx	= (*jt)->featureIdx;

			Point2f &pt2D 	= f->kpts[feIdx].pt;
			double *q		= f->rotation.coeffs().data();
			double *t		= f->position.data();
			double *point	= p->pt.data();

			ceres::CostFunction* cost_function = ReprojectionError::Create( pt2D.x, pt2D.y, intrinsics[0], intrinsics[1], intrinsics[2]);
			problem.AddResidualBlock(cost_function, NULL, q, t, point);
			problem.SetParameterization(q,eigenQuaternionParameterization);
			if(f->fixed){
				problem.SetParameterBlockConstant(q);
				problem.SetParameterBlockConstant(t);
			}
		}
	}

	t_preprocess = double(clock()-time) / CLOCKS_PER_SEC;

	// Set a few options
	ceres::Solver::Options options;
	options.use_nonmonotonic_steps = true;
	options.preconditioner_type = ceres::SCHUR_JACOBI;
	options.linear_solver_type = ceres::SPARSE_SCHUR; //ceres::ITERATIVE_SCHUR; //
	options.max_num_iterations = 100;
	//options.max_solver_time_in_seconds = 0.015;

	ceres::Solver::Summary summary;
	ceres::Solve(options, &problem, &summary);

	//std::cout << "Final report:\n" << summary.FullReport();
	std::cout << "Time(s) preprocess :"<<t_preprocess<<endl;
	std::cout << "Time(s) BA :"<<summary.total_time_in_seconds<<endl;
}

void BAHandler::adjustBundle_ceres_local_nocopy(vector<Measurement::Ptr> &ms){

	cout<<"Bundle Adjust Method: local no copy"<<endl;

	clock_t				time;
	double				t_preprocess;

	time				= clock();
	Data 		&data 	= Data::GetInstance();
	Camera 		&camera	= Camera::GetInstance();
	//important
	data.deleteTrashes();

	double intrinsics[3];
	intrinsics[0] = camera.getCamFocal();
	intrinsics[1] = camera.getCamPrinciple().x;
	intrinsics[2] = camera.getCamPrinciple().y;

	ceres::Problem problem;
	ceres::LocalParameterization *eigenQuaternionParameterization = new ceres::EigenQuaternionParameterization;

	if(data.countFrames()<1) return;
	if(ms.empty()) return;
	//fix all frames, the provided measurements should all generate from a frame that is not in data
	data.fixAllFrames();
	//get measures already added to data if exists
	Frame::Ptr &mFrame = ms[0]->frame;
	//unfix this measure frame
	mFrame->fixed = false;

	//for all given measures
	for(vector<Measurement::Ptr>::const_iterator it = ms.begin(); it!=ms.end(); ++it){
		assert((*it)->frame.get() == mFrame.get());
		LandMark::Ptr &lmk = (*it)->landmark;
		//for all measurements tied to this landmark
		vector<Measurement::Ptr> lmkms = data.getMeasurements(lmk);
		for(vector<Measurement::Ptr>::iterator jt = lmkms.begin(); jt!=lmkms.end(); ++jt){
			assert(!((*jt)->deleted));

			Frame::Ptr 		&f 	= (*jt)->frame;
			LandMark::Ptr 	&p 	= (*jt)->landmark;
			int			&feIdx	= (*jt)->featureIdx;

			Point2f &pt2D 	= f->kpts[feIdx].pt;
			double *q		= f->rotation.coeffs().data();
			double *t		= f->position.data();
			double *point	= p->pt.data();

			ceres::CostFunction* cost_function = ReprojectionError::Create( pt2D.x, pt2D.y, intrinsics[0], intrinsics[1], intrinsics[2]);
			problem.AddResidualBlock(cost_function, NULL, q, t, point);
			problem.SetParameterization(q,eigenQuaternionParameterization);
			if(f->fixed){
				problem.SetParameterBlockConstant(q);
				problem.SetParameterBlockConstant(t);
			}
		}
	}

	//if mFrame was already added to data, we add these measures as well for bundle adjustment
	vector<Measurement::Ptr> dataMs = data.getMeasurements(mFrame);
	for(vector<Measurement::Ptr>::const_iterator it = dataMs.begin(); it!=dataMs.end(); ++it){
		assert((*it)->frame.get() == mFrame.get());
		LandMark::Ptr &lmk = (*it)->landmark;
		//for all measurements tied to this landmark
		vector<Measurement::Ptr> lmkms = data.getMeasurements(lmk);
		for(vector<Measurement::Ptr>::iterator jt = lmkms.begin(); jt!=lmkms.end(); ++jt){
			assert(!((*jt)->deleted));

			Frame::Ptr 		&f 	= (*jt)->frame;
			LandMark::Ptr 	&p 	= (*jt)->landmark;
			int			&feIdx	= (*jt)->featureIdx;

			Point2f &pt2D 	= f->kpts[feIdx].pt;
			double *q		= f->rotation.coeffs().data();
			double *t		= f->position.data();
			double *point	= p->pt.data();

			ceres::CostFunction* cost_function = ReprojectionError::Create( pt2D.x, pt2D.y, intrinsics[0], intrinsics[1], intrinsics[2]);
			problem.AddResidualBlock(cost_function, NULL, q, t, point);
			problem.SetParameterization(q,eigenQuaternionParameterization);
			if(f->fixed){
				problem.SetParameterBlockConstant(q);
				problem.SetParameterBlockConstant(t);
			}
		}
	}

	t_preprocess = double(clock()-time) / CLOCKS_PER_SEC;

	// Set a few options
	ceres::Solver::Options options;
	options.use_nonmonotonic_steps = true;
	options.preconditioner_type = ceres::SCHUR_JACOBI;
	options.linear_solver_type = ceres::SPARSE_SCHUR; //ceres::ITERATIVE_SCHUR; //
	options.max_num_iterations = 100;
	//options.max_solver_time_in_seconds = 0.015;

	ceres::Solver::Summary summary;
	ceres::Solve(options, &problem, &summary);

	//std::cout << "Final report:\n" << summary.FullReport();
	std::cout << "Time(s) preprocess :"<<t_preprocess<<endl;
	std::cout << "Time(s) BA :"<<summary.total_time_in_seconds<<endl;
}

// void BAHandler::adjustBundle_ceres_local_fixPoints_nocopy(vector<Measurement::Ptr> &ms){

// 	cout<<"Bundle Adjust Method: local fix points no copy"<<endl;

// 	clock_t				time;
// 	double				t_preprocess;

// 	time				= clock();
// 	Data 		&data 	= Data::GetInstance();
// 	Camera 		&camera	= Camera::GetInstance();
// 	//important
// 	data.deleteTrashes();

// 	double intrinsics[3];
// 	intrinsics[0] = camera.getCamFocal();
// 	intrinsics[1] = camera.getCamPrinciple().x;
// 	intrinsics[2] = camera.getCamPrinciple().y;

// 	ceres::Problem problem;
// 	ceres::LocalParameterization *eigenQuaternionParameterization = new ceres::EigenQuaternionParameterization;

// 	if(data.countFrames()<1) return;
// 	if(ms.empty()) return;

// 	//for all given measures
// 	for(vector<Measurement::Ptr>::const_iterator it = ms.begin(); it!=ms.end(); ++it){

// 		Frame::Ptr 		&f 	= (*it)->frame;
// 		LandMark::Ptr 	&p 	= (*it)->landmark;
// 		int			&feIdx	= (*it)->featureIdx;
// 		double 		oct		= (double) (f->kpts[feIdx].octave + 1);

// 		Point2f &pt2D 	= f->kpts[feIdx].pt;
// 		double *q		= f->rotation.coeffs().data();
// 		double *t		= f->position.data();
// 		double *point	= p->pt.data();

// 		ceres::CostFunction* cost_function = OctaveAwareReprojectionError::Create( pt2D.x, pt2D.y, intrinsics[0], intrinsics[1], intrinsics[2], oct);
// 		problem.AddResidualBlock(cost_function, NULL, q, t, point);
// 		problem.SetParameterization(q,eigenQuaternionParameterization);
// 		problem.SetParameterBlockConstant(point);

// 	}

// /*

// 	//fix all frames, the provided measurements should all generate from a frame that is not in data
// 	data.fixAllFrames();
// 	//get measures already added to data if exists
// 	Frame::Ptr &mFrame = ms[0]->frame;
// 	//unfix this measure frame
// 	mFrame->fixed = false;

// 	//for all given measures
// 	for(vector<Measurement::Ptr>::const_iterator it = ms.begin(); it!=ms.end(); ++it){
// 		assert((*it)->frame.get() == mFrame.get());
// 		LandMark::Ptr &lmk = (*it)->landmark;
// 		//for all measurements tied to this landmark
// 		vector<Measurement::Ptr> lmkms = data.getMeasurements(lmk);
// 		for(vector<Measurement::Ptr>::iterator jt = lmkms.begin(); jt!=lmkms.end(); ++jt){
// 			assert(!((*jt)->deleted));

// 			Frame::Ptr 		&f 	= (*jt)->frame;
// 			LandMark::Ptr 	&p 	= (*jt)->landmark;
// 			int			&feIdx	= (*jt)->featureIdx;

// 			Point2f &pt2D 	= f->kpts[feIdx].pt;
// 			double *q		= f->rotation.coeffs().data();
// 			double *t		= f->position.data();
// 			double *point	= p->pt.data();

// 			ceres::CostFunction* cost_function = ReprojectionError::Create( pt2D.x, pt2D.y, intrinsics[0], intrinsics[1], intrinsics[2]);
// 			problem.AddResidualBlock(cost_function, NULL, q, t, point);
// 			problem.SetParameterization(q,eigenQuaternionParameterization);
// 			problem.SetParameterBlockConstant(point);
// 			if(f->fixed){
// 				problem.SetParameterBlockConstant(q);
// 				problem.SetParameterBlockConstant(t);
// 			}
// 		}
// 	}

// 	//if mFrame was already added to data, we add these measures as well for bundle adjustment
// 	vector<Measurement::Ptr> dataMs = data.getMeasurements(mFrame);
// 	for(vector<Measurement::Ptr>::const_iterator it = dataMs.begin(); it!=dataMs.end(); ++it){
// 		assert((*it)->frame.get() == mFrame.get());
// 		LandMark::Ptr &lmk = (*it)->landmark;
// 		//for all measurements tied to this landmark
// 		vector<Measurement::Ptr> lmkms = data.getMeasurements(lmk);
// 		for(vector<Measurement::Ptr>::iterator jt = lmkms.begin(); jt!=lmkms.end(); ++jt){
// 			assert(!((*jt)->deleted));

// 			Frame::Ptr 		&f 	= (*jt)->frame;
// 			LandMark::Ptr 	&p 	= (*jt)->landmark;
// 			int			&feIdx	= (*jt)->featureIdx;

// 			Point2f &pt2D 	= f->kpts[feIdx].pt;
// 			double *q		= f->rotation.coeffs().data();
// 			double *t		= f->position.data();
// 			double *point	= p->pt.data();

// 			ceres::CostFunction* cost_function = ReprojectionError::Create( pt2D.x, pt2D.y, intrinsics[0], intrinsics[1], intrinsics[2]);
// 			problem.AddResidualBlock(cost_function, NULL, q, t, point);
// 			problem.SetParameterization(q,eigenQuaternionParameterization);
// 			problem.SetParameterBlockConstant(point);
// 			if(f->fixed){
// 				problem.SetParameterBlockConstant(q);
// 				problem.SetParameterBlockConstant(t);
// 			}
// 		}
// 	}
// */

// 	t_preprocess = double(clock()-time) / CLOCKS_PER_SEC;

// 	// Set a few options
// 	ceres::Solver::Options options;
// 	options.use_nonmonotonic_steps = true;
// 	options.preconditioner_type = ceres::SCHUR_JACOBI;
// 	options.linear_solver_type = ceres::SPARSE_SCHUR; //ceres::ITERATIVE_SCHUR; //
// 	options.max_num_iterations = 100;
// 	//options.max_solver_time_in_seconds = 0.015;

// 	ceres::Solver::Summary summary;
// 	ceres::Solve(options, &problem, &summary);

// 	std::cout << "Final report:\n" << summary.FullReport();
// 	std::cout << "Time(s) preprocess :"<<t_preprocess<<endl;
// 	std::cout << "Time(s) BA :"<<summary.total_time_in_seconds<<endl;
// }