/**
 *  This test ensures that the sketch application (for CombBLAS matrices) is
 *  done correctly (on-the-fly matrix multiplication in the code is compared
 *  to true matrix multiplication).
 *  This test builds on the following assumptions:
 *
 *      - CombBLAS PSpGEMM returns the correct result, and
 *      - the random numbers in row_idx and row_value (see
 *        hash_transform_data_t) are drawn from the promised distributions.
 */

#include <vector>

#include <boost/mpi.hpp>
#include <boost/test/minimal.hpp>

#define SKYLARK_NO_ANY
#include "../../utility/distributions.hpp"
#include "../../base/sparse_matrix.hpp"
#include "../../sketch/sketch.hpp"

template < typename InputMatrixType,
           typename OutputMatrixType = InputMatrixType >
struct Dummy_t : public skylark::sketch::hash_transform_t<
    InputMatrixType, OutputMatrixType,
    boost::random::uniform_int_distribution,
    skylark::utility::rademacher_distribution_t > {

    typedef skylark::sketch::hash_transform_t<
        InputMatrixType, OutputMatrixType,
        boost::random::uniform_int_distribution,
        skylark::utility::rademacher_distribution_t >
            hash_t;

    Dummy_t(int N, int S, skylark::base::context_t& context)
        : skylark::sketch::hash_transform_t<InputMatrixType, OutputMatrixType,
          boost::random::uniform_int_distribution,
          skylark::utility::rademacher_distribution_t>(N, S, context)
    {}

    std::vector<size_t> getRowIdx() { return hash_t::row_idx; }
    std::vector<double> getRowValues() { return hash_t::row_value; }
};

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

    //////////////////////////////////////////////////////////////////////////
    //[> Parameters <]

    //FIXME: use random sizes?
    const size_t n   = 10;
    const size_t m   = 5;
    const size_t n_s = 6;
    const size_t m_s = 3;

    typedef skylark::base::sparse_matrix_t<double> Matrix_t;

    //////////////////////////////////////////////////////////////////////////
    //[> Setup test <]

    namespace mpi = boost::mpi;
    mpi::environment env(argc, argv);
    mpi::communicator world;
    const size_t rank = world.rank();

    skylark::base::context_t context (0);

    double count = 1.0;

    const int matrix_full = n * m;
    std::vector<int> colsf(m + 1);
    std::vector<int> rowsf(matrix_full);
    std::vector<double> valsf(matrix_full);

    for(size_t i = 0; i < m + 1; ++i)
        colsf[i] = i * n;

    for(size_t i = 0; i < matrix_full; ++i) {
        rowsf[i] = i % n;
        valsf[i] = count;
        count++;
    }

    Matrix_t A;
    A.attach(&colsf[0], &rowsf[0], &valsf[0], matrix_full, n, m, false);

    count = 1;
    const int* indptr = A.indptr();
    const int* indices = A.indices();
    const double* values = A.locked_values();

    for(int col = 0; col < A.width(); col++) {
        for(int idx = indptr[col]; idx < indptr[col + 1]; idx++) {
            BOOST_REQUIRE( values[idx] == count );
            count++;
        }
    }

    //////////////////////////////////////////////////////////////////////////
    //[> Test COO to CSC <]
    typename Matrix_t::coords_t coords_mat;
    count = 1;
    for(int col = 0; col < A.width(); col++) {
        for(int row = 0; row < A.height(); row++) {
            typename Matrix_t::coord_tuple_t new_entry(row, col, count);
            coords_mat.push_back(new_entry);
            count++;
        }
    }

    Matrix_t A_coord;
    A_coord.set(coords_mat);

    count = 1;
    indptr = A_coord.indptr();
    indices = A_coord.indices();
    values = A_coord.locked_values();

    for(int col = 0; col < A_coord.width(); col++) {
        for(int idx = indptr[col]; idx < indptr[col + 1]; idx++) {
            BOOST_REQUIRE( values[idx] == count );
            count++;
        }
    }

    //////////////////////////////////////////////////////////////////////////
    //[> Column wise application <]

    //[> 1. Create the sketching matrix <]
    Dummy_t<Matrix_t, Matrix_t> Sparse(n, n_s, context);
    std::vector<size_t> row_idx = Sparse.getRowIdx();
    std::vector<double> row_val = Sparse.getRowValues();

    // PI generated by random number gen
    int sketch_size = row_val.size();
    typename Matrix_t::coords_t coords;
    for(int i = 0; i < sketch_size; ++i) {
        typename Matrix_t::coord_tuple_t new_entry(row_idx[i], i, row_val[i]);
        coords.push_back(new_entry);
    }

    Matrix_t pi_sketch;
    pi_sketch.set(coords);

    //[> 2. Create sketched matrix <]
    Matrix_t sketch_A;

    //[> 3. Apply the transform <]
    Sparse.apply(A, sketch_A, skylark::sketch::columnwise_tag());

    BOOST_CHECK(sketch_A.height() == n_s);
    BOOST_CHECK(sketch_A.width()  == m);

    //[> 4. Build structure to compare: PI * A ?= sketch_A <]
    typename Matrix_t::coords_t coords_new;
    indptr = pi_sketch.indptr();
    indices = pi_sketch.indices();
    values = pi_sketch.locked_values();

    // multiply with vector where an entry has the value:
    //   col_idx * n + row_idx + 1.
    // See creation of A.
    for(int col = 0; col < pi_sketch.width(); col++) {
        for(int idx = indptr[col]; idx < indptr[col + 1]; idx++) {
            for(int ccol = 0; ccol < m; ++ccol) {
                typename Matrix_t::coord_tuple_t new_entry(indices[idx], ccol,
                    values[idx] * (ccol * n + col + 1));
                coords_new.push_back(new_entry);
            }
        }
    }

    Matrix_t expected_A;
    expected_A.set(coords_new, n_s, m);

    if (!static_cast<bool>(expected_A == sketch_A))
        BOOST_FAIL("Result of colwise application not as expected");

    //////////////////////////////////////////////////////////////////////////
    //[> Row wise application <]

    //[> 1. Create the sketching matrix <]
    Dummy_t<Matrix_t, Matrix_t> Sparse_row(m, m_s, context);
    row_idx = Sparse_row.getRowIdx();
    row_val = Sparse_row.getRowValues();

    // PI generated by random number gen
    sketch_size = row_val.size();
    coords.clear();
    for(int i = 0; i < sketch_size; ++i) {
        typename Matrix_t::coord_tuple_t new_entry(i, row_idx[i], row_val[i]);
        coords.push_back(new_entry);
    }

    Matrix_t pi_sketch_row;
    pi_sketch_row.set(coords);

    //[> 2. Create sketched matrix <]
    Matrix_t sketch_A_row;

    //[> 3. Apply the transform <]
    Sparse_row.apply(A, sketch_A_row, skylark::sketch::rowwise_tag());

    BOOST_CHECK(sketch_A_row.height() == n);
    BOOST_CHECK(sketch_A_row.width()  == m_s);

    //[> 4. Build structure to compare: A * PI ?= sketch_A <]
    coords_new.clear();
    indptr = pi_sketch_row.indptr();
    indices = pi_sketch_row.indices();
    values = pi_sketch_row.locked_values();

    // multiply with vector where an entry has the value:
    //   col_idx * n + row_idx + 1.
    // See creation of A.
    for(int col = 0; col < pi_sketch_row.width(); col++) {
        for(int idx = indptr[col]; idx < indptr[col + 1]; idx++) {
            for(int row = 0; row < n; ++row) {
                typename Matrix_t::coord_tuple_t new_entry(row, col,
                    values[idx] * (indices[idx] * n + row + 1));
                coords_new.push_back(new_entry);
            }
        }
    }

    Matrix_t expected_A_row;
    expected_A_row.set(coords_new, n, m_s);

    if (!static_cast<bool>(expected_A_row == sketch_A_row))
        BOOST_FAIL("Result of rowwise application not as expected");

    return 0;
}


//--C++--
//-- core_shell_cylinder 1D analytical = no orientation
//-- SANS

#include <boost/python.hpp>

#include "mccomponents/kernels/sample/AbstractSQ.h"
#include "mccomponents/kernels/sample/SQAdaptor.h"

extern "C" {
#include "sans/core_shell_cylinder.h"
}

namespace wrap_models{
struct Core_shell_cylinder{
	Core_shell_cylinder(double scale,double radius, double thickness, double length, double core_sld, double shell_sld, double solvent_sld, double background,double axis_theta, double axis_phi)
	{
    p.scale = scale;
    p.radius = radius;
	p.thickness = thickness;
	p.length = length;
	p.core_sld = core_sld;
	p.shell_sld = shell_sld;
	p.solvent_sld = solvent_sld;
	p.background = background;
    p.axis_theta = axis_theta;
	p.axis_phi = axis_phi;
	}

	double operator() (double q) const
	{
		return core_shell_cylinder_analytical_1D( const_cast<CoreShellCylinderParameters *>(&p), q);
	}
	CoreShellCylinderParameters p;
};

void wrap_core_shell_cylinder()
{
	using namespace boost::python;
	typedef Core_shell_cylinder w_t;
	class_<w_t>
		(
		"SANSModel_Core_shell_cylinder",
		init< double, double, double, double, double, double, double, double, double, double >()
			)
		;

	using namespace mccomponents::sample;
	typedef SQAdaptor<w_t> SQ;
	class_<SQ, bases<AbstractSQ> >
		(
		"SANSModel_Core_shell_cylinder_SQAdaptor",
		init< const w_t & >()
		[with_custodian_and_ward<1,2>()] //Jiao, what you mean by the numbers,<1,2>???
		)
		;
	}
}
//2009-06-02 jhcho
// End of file


#ifndef STAN_MATH_REV_MAT_FUN_TRACE_GEN_INV_QUAD_FORM_LDLT_HPP
#define STAN_MATH_REV_MAT_FUN_TRACE_GEN_INV_QUAD_FORM_LDLT_HPP

#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/rev/core.hpp>
#include <stan/math/prim/scal/meta/is_var.hpp>
#include <stan/math/rev/scal/meta/is_var.hpp>
#include <boost/utility/enable_if.hpp>
#include <stan/math/prim/mat/err/check_multiplicable.hpp>
#include <stan/math/prim/mat/err/check_square.hpp>
#include <stan/math/rev/mat/fun/trace_inv_quad_form_ldlt.hpp>

namespace stan {
  namespace math {

    /**
     * Compute the trace of an inverse quadratic form.  I.E., this computes
     *       trace(D B^T A^-1 B)
     * where D is a square matrix and the LDLT_factor of A is provided.
     **/
    template <typename T1, int R1, int C1, typename T2, int R2, int C2,
              typename T3, int R3, int C3>
    inline typename
    boost::enable_if_c<stan::is_var<T1>::value ||
    stan::is_var<T2>::value ||
    stan::is_var<T3>::value, var>::type
      trace_gen_inv_quad_form_ldlt(const Eigen::Matrix<T1, R1, C1> &D,
                                   const LDLT_factor<T2, R2, C2> &A,
                                   const Eigen::Matrix<T3, R3, C3> &B) {
      check_square("trace_gen_inv_quad_form_ldlt", "D", D);
      check_multiplicable("trace_gen_inv_quad_form_ldlt",
                          "A", A,
                          "B", B);
      check_multiplicable("trace_gen_inv_quad_form_ldlt",
                          "B", B,
                          "D", D);

      trace_inv_quad_form_ldlt_impl<T2, R2, C2, T3, R3, C3> *_impl
        = new trace_inv_quad_form_ldlt_impl<T2, R2, C2, T3, R3, C3>(D, A, B);

      return var(new trace_inv_quad_form_ldlt_vari<T2, R2, C2, T3, R3, C3>
                 (_impl));
    }

  }
}
#endif


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

#include "Domain/FunctionsOfTime/QuaternionFunctionOfTime.hpp"

// BoostMultiArray is used internally in odeint, so BoostMultiArray MUST be
// included before odeint
#include "DataStructures/BoostMultiArray.hpp"

#include <boost/numeric/odeint.hpp>
#include <ostream>
#include <pup_stl.h>

#include "Domain/FunctionsOfTime/QuaternionHelpers.hpp"
#include "NumericalAlgorithms/OdeIntegration/OdeIntegration.hpp"
#include "Utilities/ErrorHandling/Assert.hpp"
#include "Utilities/ErrorHandling/Error.hpp"
#include "Utilities/GenerateInstantiations.hpp"
#include "Utilities/MakeArray.hpp"
#include "Utilities/StdHelpers.hpp"

namespace domain::FunctionsOfTime {
template <size_t MaxDeriv>
QuaternionFunctionOfTime<MaxDeriv>::QuaternionFunctionOfTime(
    const double t, std::array<DataVector, 1> initial_quat_func,
    std::array<DataVector, MaxDeriv + 1> initial_angle_func,
    const double expiration_time)
    : stored_quaternions_and_times_{{t, std::move(initial_quat_func)}},
      angle_f_of_t_(t, std::move(initial_angle_func), expiration_time) {}

template <size_t MaxDeriv>
std::unique_ptr<FunctionOfTime> QuaternionFunctionOfTime<MaxDeriv>::get_clone()
    const {
  return std::make_unique<QuaternionFunctionOfTime>(*this);
}

template <size_t MaxDeriv>
void QuaternionFunctionOfTime<MaxDeriv>::pup(PUP::er& p) {
  FunctionOfTime::pup(p);
  p | stored_quaternions_and_times_;
  p | angle_f_of_t_;
}

template <size_t MaxDeriv>
void QuaternionFunctionOfTime<MaxDeriv>::update(
    const double time_of_update, DataVector updated_max_deriv,
    const double next_expiration_time) {
  angle_f_of_t_.update(time_of_update, std::move(updated_max_deriv),
                       next_expiration_time);
  update_stored_info();
}

template <size_t MaxDeriv>
void QuaternionFunctionOfTime<MaxDeriv>::update_stored_info() {
  const auto& angle_deriv_info = angle_f_of_t_.get_deriv_info();

  ASSERT(
      angle_deriv_info.size() == stored_quaternions_and_times_.size() + 1,
      "The number of stored quaternions is not one less than the number of "
      "stored angles (which it should be after an update). Currently there are "
          << angle_deriv_info.size() << " stored angles and "
          << stored_quaternions_and_times_.size() << " stored quaternions.");

  const size_t last_index = stored_quaternions_and_times_.size();
  // Final time, initial time, and quaternion
  const double t = angle_deriv_info[last_index].time;
  const double t0 = angle_deriv_info[last_index - 1].time;
  boost::math::quaternion<double> quaternion_to_integrate =
      datavector_to_quaternion(
          stored_quaternions_and_times_[last_index - 1].stored_quantities[0]);

  solve_quaternion_ode(make_not_null(&quaternion_to_integrate), t0, t);

  // normalize quaternion
  normalize_quaternion(make_not_null(&quaternion_to_integrate));

  stored_quaternions_and_times_.emplace_back(
      t, std::array<DataVector, 1>{
             quaternion_to_datavector(quaternion_to_integrate)});

  ASSERT(angle_deriv_info.size() == stored_quaternions_and_times_.size(),
         "The number of stored angles must be the same as the number of stored "
         "quaternions after updating the missing quaternion. Now there are "
             << angle_deriv_info.size() << " stored angles and "
             << stored_quaternions_and_times_.size() << " stored quaternions.");
}

template <size_t MaxDeriv>
void QuaternionFunctionOfTime<MaxDeriv>::solve_quaternion_ode(
    const gsl::not_null<boost::math::quaternion<double>*>
        quaternion_to_integrate,
    const double t0, const double t) const {
  // lambda that stores the internals of the ode
  const auto quaternion_ode_system =
      [this](const boost::math::quaternion<double>& state,
             boost::math::quaternion<double>& dt_state, const double time) {
        const boost::math::quaternion<double> omega =
            datavector_to_quaternion(angle_f_of_t_.func_and_deriv(time)[1]);
        dt_state = 0.5 * state * omega;
      };

  // Dense stepper
  auto dense_stepper = boost::numeric::odeint::make_dense_output(
      1.0e-12, 1.0e-14,
      boost::numeric::odeint::runge_kutta_dopri5<
          boost::math::quaternion<double>, double,
          boost::math::quaternion<double>, double,
          boost::numeric::odeint::vector_space_algebra>{});

  // Integrate from t0 to t, storing result in quaternion_to_integrate
  boost::numeric::odeint::integrate_adaptive(
      dense_stepper, quaternion_ode_system, *quaternion_to_integrate, t0, t,
      1e-4);
}

template <size_t MaxDeriv>
boost::math::quaternion<double> QuaternionFunctionOfTime<MaxDeriv>::setup_func(
    const double t) const {
  // Get quaternion and time at closest time before t
  const auto& stored_info_at_t0 =
      stored_info_from_upper_bound(t, stored_quaternions_and_times_);
  boost::math::quaternion<double> quat_to_integrate =
      datavector_to_quaternion(stored_info_at_t0.stored_quantities[0]);

  // Solve the ode and store the result in quat_to_integrate
  solve_quaternion_ode(make_not_null(&quat_to_integrate),
                       stored_info_at_t0.time, t);

  // Make unit quaternion
  normalize_quaternion(make_not_null(&quat_to_integrate));

  return quat_to_integrate;
}

template <size_t MaxDeriv>
std::array<DataVector, 1> QuaternionFunctionOfTime<MaxDeriv>::quat_func(
    const double t) const {
  return std::array<DataVector, 1>{quaternion_to_datavector(setup_func(t))};
}

template <size_t MaxDeriv>
std::array<DataVector, 2>
QuaternionFunctionOfTime<MaxDeriv>::quat_func_and_deriv(const double t) const {
  boost::math::quaternion<double> quat = setup_func(t);

  // Get angle and however many derivatives we need
  std::array<DataVector, 2> angle_and_deriv = angle_f_of_t_.func_and_deriv(t);

  boost::math::quaternion<double> omega =
      datavector_to_quaternion(angle_and_deriv[1]);

  boost::math::quaternion<double> dtquat = 0.5 * quat * omega;

  return std::array<DataVector, 2>{quaternion_to_datavector(quat),
                                   quaternion_to_datavector(dtquat)};
}

template <size_t MaxDeriv>
std::array<DataVector, 3>
QuaternionFunctionOfTime<MaxDeriv>::quat_func_and_2_derivs(
    const double t) const {
  boost::math::quaternion<double> quat = setup_func(t);

  // Get angle and however many derivatives we need
  std::array<DataVector, 3> angle_and_2_derivs =
      angle_f_of_t_.func_and_2_derivs(t);

  boost::math::quaternion<double> omega =
      datavector_to_quaternion(angle_and_2_derivs[1]);
  boost::math::quaternion<double> dtomega =
      datavector_to_quaternion(angle_and_2_derivs[2]);

  boost::math::quaternion<double> dtquat = 0.5 * quat * omega;
  boost::math::quaternion<double> dt2quat =
      0.5 * (dtquat * omega + quat * dtomega);

  return std::array<DataVector, 3>{quaternion_to_datavector(quat),
                                   quaternion_to_datavector(dtquat),
                                   quaternion_to_datavector(dt2quat)};
}

template <size_t MaxDeriv>
bool operator==(const QuaternionFunctionOfTime<MaxDeriv>& lhs,
                const QuaternionFunctionOfTime<MaxDeriv>& rhs) {
  return lhs.stored_quaternions_and_times_ ==
             rhs.stored_quaternions_and_times_ and
         lhs.angle_f_of_t_ == rhs.angle_f_of_t_;
}

template <size_t MaxDeriv>
bool operator!=(const QuaternionFunctionOfTime<MaxDeriv>& lhs,
                const QuaternionFunctionOfTime<MaxDeriv>& rhs) {
  return not(lhs == rhs);
}

template <size_t MaxDeriv>
std::ostream& operator<<(
    std::ostream& os,
    const QuaternionFunctionOfTime<MaxDeriv>& quaternion_f_of_t) {
  const size_t size = quaternion_f_of_t.stored_quaternions_and_times_.size();

  os << "Quaternion:\n";
  for (size_t i = 0; i < size; ++i) {
    os << quaternion_f_of_t.stored_quaternions_and_times_[i];
    os << "\n";
  }
  os << "Angle:\n";
  os << quaternion_f_of_t.angle_f_of_t_;
  return os;
}

// do explicit instantiation of MaxDeriv = {2,3,4}
#define DIM(data) BOOST_PP_TUPLE_ELEM(0, data)

#define INSTANTIATE(_, data)                                   \
  template class QuaternionFunctionOfTime<DIM(data)>;          \
  template bool operator==                                     \
      <DIM(data)>(const QuaternionFunctionOfTime<DIM(data)>&,  \
                  const QuaternionFunctionOfTime<DIM(data)>&); \
  template bool operator!=                                     \
      <DIM(data)>(const QuaternionFunctionOfTime<DIM(data)>&,  \
                  const QuaternionFunctionOfTime<DIM(data)>&); \
  template std::ostream& operator<<(                           \
      std::ostream& os, const QuaternionFunctionOfTime<DIM(data)>&);

GENERATE_INSTANTIATIONS(INSTANTIATE, (2, 3, 4))

#undef DIM
#undef INSTANTIATE
}  // namespace domain::FunctionsOfTime


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

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_GELSD_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_GELSD_HPP

#include <algorithm>

#include <boost/numeric/bindings/traits/type.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/traits/detail/array.hpp>
#include <boost/numeric/bindings/traits/detail/utils.hpp>
#include <boost/numeric/bindings/lapack/ilaenv.hpp>

// included to implicitly convert a vector to an nx1 matrix
// so that it is compatible with lapack binding
#include <boost/numeric/bindings/traits/ublas_vector2.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits.hpp>
#endif

namespace boost { namespace numeric { namespace bindings {

	namespace lapack {

		namespace detail {

			inline void gelsd(const int m, const int n, const int nrhs,
							  float *a, const int lda, float *b, const int ldb,
							  float *s, const float rcond, int *rank, float *work,
							  const int lwork, int *iwork, int *info)
			{
				LAPACK_SGELSD(&m, &n, &nrhs, a, &lda, b, &ldb, s,
							  &rcond, rank, work, &lwork, iwork, info);
			}

			inline void gelsd(const int m, const int n, const int nrhs,
							  double *a, const int lda, double *b, const int ldb,
							  double *s, const double rcond, int *rank, double *work,
							  const int lwork, int *iwork, int *info)
			{
				LAPACK_DGELSD(&m, &n, &nrhs, a, &lda, b, &ldb, s,
							  &rcond, rank, work, &lwork, iwork, info);
			}

			inline void gelsd(const int m, const int n, const int nrhs,
							  traits::complex_f *a, const int lda, traits::complex_f *b,
							  const int ldb, float *s, const float rcond, int *rank,
							  traits::complex_f *work, const int lwork, float *rwork,
							  int *iwork, int *info)
			{
				LAPACK_CGELSD(&m, &n, &nrhs, traits::complex_ptr(a),
							  &lda, traits::complex_ptr(b), &ldb, s,
							  &rcond, rank, traits::complex_ptr(work),
							  &lwork, rwork, iwork, info);
			}

			inline void gelsd(const int m, const int n, const int nrhs,
							  traits::complex_d *a, const int lda, traits::complex_d *b,
							  const int ldb, double *s, const double rcond, int *rank,
							  traits::complex_d *work, const int lwork, double *rwork,
							  int *iwork, int *info)
			{
				LAPACK_ZGELSD(&m, &n, &nrhs, traits::complex_ptr(a),
							  &lda, traits::complex_ptr(b), &ldb, s,
							  &rcond, rank, traits::complex_ptr(work),
							  &lwork, rwork, iwork, info);
			}

			// gelsd for real type
			template <typename MatrA, typename MatrB, typename VecS, typename Work>
			inline int gelsd(MatrA& A, MatrB& B, VecS& s, Work& work)
			{
				typedef typename MatrA::value_type val_t;
				typedef typename traits::type_traits<val_t>::real_type real_t;

				const int m = traits::matrix_size1(A);
				const int n = traits::matrix_size2(A);
				const int nrhs = traits::matrix_size2(B);
				const int maxmn = std::max(m, n);
				const int minmn = std::min(m, n);

				// sanity checks
				assert(m >= 0 && n >= 0);
				assert(nrhs >= 0);
				assert(traits::leading_dimension(A) >= std::max(1, m));
				assert(traits::leading_dimension(B) >= std::max(1, maxmn));
				assert(traits::vector_size(work) >= 1);
				assert(traits::vector_size(s) >= std::max(1, minmn));

				int info;
				const real_t rcond = -1;	// use machine precision
				int rank;

				// query for maximum size of subproblems
				const int smlsiz = ilaenv(9, "GELSD", "");
				const int nlvl = static_cast<int>(((std::log(static_cast<float>(minmn))/std::log(2.f))/ (smlsiz+1)) + 1);

				traits::detail::array<int> iwork(3*minmn*nlvl + 11*minmn);

				detail::gelsd(traits::matrix_size1(A),
							  traits::matrix_size2(A),
							  traits::matrix_size2(B),
							  traits::matrix_storage(A),
							  traits::leading_dimension(A),
							  traits::matrix_storage(B),
							  traits::leading_dimension(B),
							  traits::vector_storage(s),
							  rcond,
							  &rank,
							  traits::vector_storage(work),
							  traits::vector_size(work),
							  traits::vector_storage(iwork),
							  &info);

				return info;
			}

			// gelsd for complex type
			template <typename MatrA, typename MatrB, typename VecS,
						typename Work, typename RWork>
			inline int gelsd(MatrA& A, MatrB& B, VecS& s, Work& work, RWork& rwork)
			{
				typedef typename MatrA::value_type val_t;
				typedef typename traits::type_traits<val_t>::real_type real_t;

				const int m = traits::matrix_size1(A);
				const int n = traits::matrix_size2(A);
				const int nrhs = traits::matrix_size2(B);
				const int maxmn = std::max(m, n);
				const int minmn = std::min(m, n);

				// sanity checks
				assert(m >= 0 && n >= 0);
				assert(nrhs >= 0);
				assert(traits::leading_dimension(A) >= std::max(1, m));
				assert(traits::leading_dimension(B) >= std::max(1, maxmn));
				assert(traits::vector_size(work) >= 1);
				assert(traits::vector_size(s) >= std::max(1, minmn));

				int info;
				const real_t rcond = -1;	// use machine precision
				int rank;

				// query for maximum size of subproblems
				const int smlsiz = ilaenv(9, "GELSD", "");
				const int nlvl = static_cast<int>(((std::log(static_cast<float>(minmn))/std::log(2.f))/ (smlsiz+1)) + 1);

				traits::detail::array<int> iwork(3*minmn*nlvl + 11*minmn);

				detail::gelsd(traits::matrix_size1(A),
					traits::matrix_size2(A),
					traits::matrix_size2(B),
					traits::matrix_storage(A),
					traits::leading_dimension(A),
					traits::matrix_storage(B),
					traits::leading_dimension(B),
					traits::vector_storage(s),
					rcond,
					&rank,
					traits::vector_storage(work),
					traits::vector_size(work),
					traits::vector_storage(rwork),
					traits::vector_storage(iwork),
					&info);

				return info;
			}

			template <int N>
			struct Gelsd { };

			// specialization for gelsd real flavors (sgelsd, dgelsd)
			template <>
			struct Gelsd<1>
			{
				template <typename MatrA, typename MatrB, typename VecS>
				inline int operator() (MatrA& A, MatrB& B, VecS& s, minimal_workspace) const
				{
					typedef typename MatrA::value_type val_t;

					const int m = traits::matrix_size1(A);
					const int n = traits::matrix_size2(A);
					const int nrhs = traits::matrix_size2(B);

					const int minmn = std::min(m, n);			// minmn = m < n ? m : n
					const int maxmn = std::max(m, n);			// maxmn = m > n ? m : n
					const int maxmnr = std::max(maxmn, nrhs);	// maxmnr = maxmn > nrhs ? maxmn : nrhs

					// query for maximum size of subproblems
					const int smlsiz = ilaenv(9, "GELSD", "");
					const int nlvl = static_cast<int>(((std::log(static_cast<float>(minmn))/std::log(2.f))/ (smlsiz+1)) + 1);

					const int lwork = 12*minmn + 2*minmn*smlsiz + 8*minmn*nlvl +
							     	  minmn*nrhs + (smlsiz+1)*(smlsiz+1);

					traits::detail::array<val_t> work(lwork);

					return gelsd(A, B, s, work);
				}

				template <typename MatrA, typename MatrB, typename VecS>
				inline int operator() (MatrA& A, MatrB& B, VecS& s, optimal_workspace) const
				{
					typedef typename MatrA::value_type val_t;
					typedef typename traits::type_traits<val_t>::real_type real_t;

					const int m = traits::matrix_size1(A);
					const int n = traits::matrix_size2(A);
					const int nrhs = traits::matrix_size2(B);

					const int minmn = std::min(m, n);			// minmn = m < n ? m : n
					const int maxmn = std::max(m, n);			// maxmn = m > n ? m : n
					const int maxmnr = std::max(maxmn, nrhs);	// maxmnr = maxmn > nrhs ? maxmn : nrhs

					val_t temp_work;
					int temp_iwork;

					const real_t rcond = -1;
					int rank;
					int info;

					// query for optimal workspace size
					detail::gelsd(traits::matrix_size1(A),
								  traits::matrix_size2(A),
								  traits::matrix_size2(B),
								  traits::matrix_storage(A),
								  traits::leading_dimension(A),
								  traits::matrix_storage(B),
								  traits::leading_dimension(B),
								  traits::vector_storage(s),
								  rcond,
								  &rank,
								  &temp_work,	//traits::vector_storage(work),
								  -1,			//traits::vector_size(work),
								  &temp_iwork,
								  &info);

					assert(info == 0);

					const int lwork = traits::detail::to_int(temp_work);

					traits::detail::array<val_t> work(lwork);

					return gelsd(A, B, s, work);
				}

				template <typename MatrA, typename MatrB, typename VecS, typename Work>
				inline int operator() (MatrA& A, MatrB& B, VecS& s, detail::workspace1<Work>& workspace) const
				{
					const int m = traits::matrix_size1(A);
					const int n = traits::matrix_size2(A);
					const int nrhs = traits::matrix_size2(B);

					const int minmn = std::min(m, n);			// minmn = m < n ? m : n
					const int maxmn = std::max(m, n);			// maxmn = m > n ? m : n
					const int maxmnr = std::max(maxmn, nrhs);	// maxmnr = maxmn > nrhs ? maxmn : nrhs

					return gelsd(A, B, s, workspace.w_);
				}
			};

			// specialization for gelsd (cgelsd, zgelsd)
			template <>
			struct Gelsd<2>
			{
				template <typename MatrA, typename MatrB, typename VecS>
				inline int operator() (MatrA& A, MatrB& B, VecS& s, minimal_workspace) const
				{
					typedef typename MatrA::value_type val_t;
					typedef typename traits::type_traits<val_t>::real_type real_t;

					const int m = traits::matrix_size1(A);
					const int n = traits::matrix_size2(A);
					const int nrhs = traits::matrix_size2(B);

					const int minmn = std::min(m, n);			// minmn = m < n ? m : n
					const int maxmn = std::max(m, n);			// maxmn = m > n ? m : n
					const int maxmnr = std::max(maxmn, nrhs);	// maxmnr = maxmn > nrhs ? maxmn : nrhs

					// query for maximum size of subproblems
					const int smlsiz = ilaenv(9, "GELSD", "");
					const int nlvl = static_cast<int>(((std::log(static_cast<float>(minmn))/std::log(2.f))/ (smlsiz+1)) + 1);

					traits::detail::array<val_t> work(2*minmn + minmn*nrhs);

					const int rwork_size = 10*minmn + 2*minmn*smlsiz + 8*minmn*nlvl +
										   3*smlsiz*nrhs + (smlsiz+1)*(smlsiz+1);

					traits::detail::array<real_t> rwork(std::max(1, rwork_size));

					return gelsd(A, B, s, work, rwork);
				}

				template <typename MatrA, typename MatrB, typename VecS>
				inline int operator() (MatrA& A, MatrB& B, VecS& s, optimal_workspace) const
				{
					typedef typename MatrA::value_type val_t;
					typedef typename traits::type_traits<val_t>::real_type real_t;

					const int m = traits::matrix_size1(A);
					const int n = traits::matrix_size2(A);
					const int nrhs = traits::matrix_size2(B);

					const int minmn = std::min(m, n);			// minmn = m < n ? m : n
					const int maxmn = std::max(m, n);			// maxmn = m > n ? m : n
					const int maxmnr = std::max(maxmn, nrhs);	// maxmnr = maxmn > nrhs ? maxmn : nrhs

					val_t temp_work;
					real_t temp_rwork;
					int temp_iwork;

					const real_t rcond = -1;
					int rank;
					int info;

					// query for optimal workspace size
					detail::gelsd(traits::matrix_size1(A),
								  traits::matrix_size2(A),
								  traits::matrix_size2(B),
								  traits::matrix_storage(A),
								  traits::leading_dimension(A),
								  traits::matrix_storage(B),
								  traits::leading_dimension(B),
								  traits::vector_storage(s),
								  rcond,
								  &rank,
								  &temp_work,	//traits::vector_storage(work),
								  -1,			//traits::vector_size(work),
								  &temp_rwork,
								  &temp_iwork,
								  &info);

					assert(info == 0);

					const int lwork = traits::detail::to_int(temp_work);

					traits::detail::array<val_t> work(lwork);

					// query for maximum size of subproblems
					const int smlsiz = ilaenv(9, "GELSD", "");
					const int nlvl = static_cast<int>(((std::log(static_cast<float>(minmn))/std::log(2.f))/ (smlsiz+1)) + 1);

					const int rwork_size = 10*minmn + 2*minmn*smlsiz + 8*minmn*nlvl +
											3*smlsiz*nrhs + (smlsiz+1)*(smlsiz+1);

					traits::detail::array<real_t> rwork(std::max(1, rwork_size));

					return gelsd(A, B, s, work, rwork);
				}

				template <typename MatrA, typename MatrB, typename VecS, typename Work, typename RWork>
				inline int operator() (MatrA& A, MatrB& B, VecS& s, detail::workspace2<Work, RWork>& workspace) const
				{
					const int m = traits::matrix_size1(A);
					const int n = traits::matrix_size2(A);
					const int nrhs = traits::matrix_size2(B);

					const int minmn = std::min(m, n);			// minmn = m < n ? m : n
					const int maxmn = std::max(m, n);			// maxmn = m > n ? m : n
					const int maxmnr = std::max(maxmn, nrhs);	// maxmnr = maxmn > nrhs ? maxmn : nrhs

					return gelsd(A, B, s, workspace.w_, workspace.wr_);
				}
			};

		} // detail

		// gelsd
		// Parameters:
		//	A:			matrix of coefficients
		//	B:			matrix of solutions (stored column-wise)
		//	s:			vector to store singular values on output, length >= max(1, min(m,n))
		//  workspace:	either optimal, minimal, or user supplied
		//
		template <typename MatrA, typename MatrB, typename VecS, typename Work>
		inline int gelsd(MatrA& A, MatrB& B, VecS& s, Work& workspace)
		{
			typedef typename MatrA::value_type val_t;

			return detail::Gelsd<n_workspace_args<val_t>::value>() (A, B, s, workspace);
		}

		// gelsd, no singular values are returned
		// Parameters:
		//	A:			matrix of coefficients
		//	B:			matrix of solutions (stored column-wise)
		//	workspace:	either optimal, minimal, or user supplied
		//
		template <typename MatrA, typename MatrB, typename Work>
		inline int gelsd(MatrA& A, MatrB& B, Work& workspace)
		{
			typedef typename MatrA::value_type val_t;
			typedef typename traits::type_traits<val_t>::real_type real_t;

			const int m = traits::matrix_size1(A);
			const int n = traits::matrix_size2(A);

			const int s_size = std::max(1, std::min(m,n));
			traits::detail::array<real_t> s(s_size);

			return detail::Gelsd<n_workspace_args<val_t>::value>() (A, B, s, workspace);
		}

	} // lapack

}}}

#endif


#ifndef __BOXCOX_METHOD_H__
#define __BOXCOX_METHOD_H__

#include <string>
#include <vector>

#include <armadillo>

#include <glm/glm.hpp>
#include <besiq/method/method.hpp>
#include <besiq/stats/log_scale.hpp>
#include <besiq/model_matrix.hpp>

/**
 * This class for running the scale invariant method.
 */
class boxcox_method
: public method_type
{
public:
    /**
     * Constructor.
     *
     * Lambda goes lambda_start, lambda_start + lambda_step, ..., lambda_end.
     *
     * @param data Additional data required by all methods.
     * @param model_matrix The model matrix.
     * @param is_lm Is this a linear model.
     * @param lambda_start Start value of lambda.
     * @param lambda_end End value of lambda.
     * @param lambda_start Step value of lambda.
     * @param use_power_odds Use the power odds link family for normal data.
     */
    boxcox_method(method_data_ptr data, model_matrix &model_matrix, bool is_lm, float lambda_start, float lambda_end, float lambda_step, bool use_power_odds = false);

    /**
     * Destructor.
     */
    ~boxcox_method();

    /**
     * @see method_type::init.
     */
    virtual std::vector<std::string> init();

    /**
     * @see method_type::run.
     */
    virtual double run(const snp_row &row1, const snp_row &row2, float *output);

private:
    /**
     * The included models.
     */
    std::vector<glm_model *> m_model;

    /**
     * The model matrix.
     */
    model_matrix &m_model_matrix;

    /**
     * List of lambda for each link.
     */
    std::vector<float> m_lambda;

    /**
     * A possibly transformed phenotype.
     */
    arma::vec m_fixed_pheno;
};

#endif /* End of __BOXCOX_METHOD_H__ */


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

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

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_INTERSECTION_HPP
#define BOOST_GEOMETRY_ALGORITHMS_INTERSECTION_HPP

#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/algorithms/detail/overlay/intersection_insert.hpp>
#include <boost/geometry/algorithms/intersects.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace intersection
{

template
<
    typename Box1, typename Box2,
    typename BoxOut,
    typename Strategy,
    std::size_t Dimension, std::size_t DimensionCount
>
struct intersection_box_box
{
    static inline bool apply(Box1 const& box1,
            Box2 const& box2, BoxOut& box_out,
            Strategy const& strategy)
    {
        typedef typename coordinate_type<BoxOut>::type ct;

        ct min1 = get<min_corner, Dimension>(box1);
        ct min2 = get<min_corner, Dimension>(box2);
        ct max1 = get<max_corner, Dimension>(box1);
        ct max2 = get<max_corner, Dimension>(box2);

        if (max1 < min2 || max2 < min1)
        {
            return false;
        }
        // Set dimensions of output coordinate
        set<min_corner, Dimension>(box_out, min1 < min2 ? min2 : min1);
        set<max_corner, Dimension>(box_out, max1 > max2 ? max2 : max1);

        return intersection_box_box
            <
                Box1, Box2, BoxOut, Strategy,
                Dimension + 1, DimensionCount
            >::apply(box1, box2, box_out, strategy);
    }
};

template
<
    typename Box1, typename Box2,
    typename BoxOut,
    typename Strategy,
    std::size_t DimensionCount
>
struct intersection_box_box<Box1, Box2, BoxOut, Strategy, DimensionCount, DimensionCount>
{
    static inline bool apply(Box1 const&, Box2 const&, BoxOut&, Strategy const&)
    {
        return true;
    }
};

}} // namespace detail::intersection
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

// By default, all is forwarded to the intersection_insert-dispatcher
template
<
    typename Tag1, typename Tag2, typename TagOut,
    typename Geometry1, typename Geometry2,
    typename GeometryOut,
    typename Strategy
>
struct intersection
{
    typedef std::back_insert_iterator<GeometryOut> output_iterator;

    static inline bool apply(Geometry1 const& geometry1,
            Geometry2 const& geometry2,
            GeometryOut& geometry_out,
            Strategy const& strategy)
    {
        typedef typename boost::range_value<GeometryOut>::type OneOut;

        intersection_insert
        <
            Tag1, Tag2, typename geometry::tag<OneOut>::type,
            geometry::is_areal<Geometry1>::value,
            geometry::is_areal<Geometry2>::value,
            geometry::is_areal<OneOut>::value,
            Geometry1, Geometry2,
            detail::overlay::do_reverse<geometry::point_order<Geometry1>::value, false>::value,
            detail::overlay::do_reverse<geometry::point_order<Geometry2>::value, false>::value,
            detail::overlay::do_reverse<geometry::point_order<OneOut>::value>::value,
            output_iterator, OneOut,
            overlay_intersection,
            Strategy
        >::apply(geometry1, geometry2, std::back_inserter(geometry_out), strategy);

        return true;
    }

};

template
<
    typename Box1, typename Box2,
    typename BoxOut,
    typename Strategy
>
struct intersection
    <
        box_tag, box_tag, box_tag,
        Box1, Box2, BoxOut,
        Strategy
    > : public detail::intersection::intersection_box_box
            <
                Box1, Box2, BoxOut,
                Strategy,
                0, geometry::dimension<Box1>::value
            >
{};

template
<
    typename Tag1, typename Tag2, typename TagOut,
    typename Geometry1, typename Geometry2,
    typename GeometryOut,
    typename Strategy
>
struct intersection_reversed
{
    static inline bool apply(Geometry1 const& geometry1,
            Geometry2 const& geometry2,
            GeometryOut& geometry_out,
            Strategy const& strategy)
    {
        return intersection
            <
                Tag2, Tag1, TagOut,
                Geometry2, Geometry1,
                GeometryOut, Strategy
            >::apply(geometry2, geometry1, geometry_out, strategy);
    }
};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

/*!
\brief \brief_calc2{intersection}
\ingroup intersection
\details \details_calc2{intersection, spatial set theoretic intersection}.
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\tparam GeometryOut Collection of geometries (e.g. std::vector, std::deque, boost::geometry::multi*) of which
    the value_type fulfills a \p_l_or_c concept, or it is the output geometry (e.g. for a box)
\param geometry1 \param_geometry
\param geometry2 \param_geometry
\param geometry_out The output geometry, either a multi_point, multi_polygon,
    multi_linestring, or a box (for intersection of two boxes)

\qbk{[include reference/algorithms/intersection.qbk]}
*/
template
<
    typename Geometry1,
    typename Geometry2,
    typename GeometryOut
>
inline bool intersection(Geometry1 const& geometry1,
            Geometry2 const& geometry2,
            GeometryOut& geometry_out)
{
    concept::check<Geometry1 const>();
    concept::check<Geometry2 const>();

    typedef strategy_intersection
        <
            typename cs_tag<Geometry1>::type,
            Geometry1,
            Geometry2,
            typename geometry::point_type<Geometry1>::type
        > strategy;

    return boost::mpl::if_c
        <
            geometry::reverse_dispatch<Geometry1, Geometry2>::type::value,
            dispatch::intersection_reversed
            <
                    typename geometry::tag<Geometry1>::type,
                    typename geometry::tag<Geometry2>::type,
                    typename geometry::tag<GeometryOut>::type,
                    Geometry1, Geometry2, GeometryOut, strategy
            >,
            dispatch::intersection
            <
                    typename geometry::tag<Geometry1>::type,
                    typename geometry::tag<Geometry2>::type,
                    typename geometry::tag<GeometryOut>::type,
                    Geometry1, Geometry2, GeometryOut, strategy
            >
        >::type::apply(geometry1, geometry2, geometry_out, strategy());
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_INTERSECTION_HPP


#include <NDTMatcherF2F.hh>
#include <NDTMatcher.hh>
#include <NDTMap.hh>
#include <OctTree.hh>
#include <AdaptiveOctTree.hh>
#include <PointCloudUtils.hh>

#include "ros/ros.h"
#include "pcl/point_cloud.h"
#include "sensor_msgs/PointCloud2.h"
#include "pcl/io/pcd_io.h"
#include "pcl/features/feature.h"
#include <cstdio>
#include <Eigen/Eigen>
#include <fstream>
#include "message_filters/subscriber.h"
#include "tf/message_filter.h"
#include <tf/transform_broadcaster.h>
#include <tf_conversions/tf_eigen.h>
#include <boost/circular_buffer.hpp>

class NDTMatcherNode
{
protected:
    // Our NodeHandle
    ros::NodeHandle nh_;

    // Components for tf::MessageFilter
    ros::Subscriber points2_sub_;

    // Components for publishing
    tf::TransformBroadcaster tf_;
    ros::Publisher output_pub_;

    // Use the vector as a cyclic buffer (increment with std::rotate).
    std::vector<pcl::PointCloud<pcl::PointXYZ>,Eigen::aligned_allocator<pcl::PointCloud<pcl::PointXYZ> > > pcl_buffer_;
    unsigned int nb_added_clouds_;
    boost::mutex m;
    lslgeneric::NDTMatcherF2F *matcher;

    Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> incremental_pose_;
    void TransformEigenToTF(const Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &k, tf::Transform &t)
    {
        t.setOrigin(tf::Vector3(k.matrix()(0,3), k.matrix()(1,3), k.matrix()(2,3)));
        t.setBasis(btMatrix3x3(k.matrix()(0,0), k.matrix()(0,1),k.matrix()(0,2),k.matrix()(1,0), k.matrix()(1,1),k.matrix()(1,2),k.matrix()(2,0), k.matrix()(2,1),k.matrix()(2,2)));
    };

public:
    // Constructor
    NDTMatcherNode() : nb_added_clouds_(0)

    {
        double __res[] = {0.2, 0.4, 1, 2};
        std::vector<double> resolutions (__res, __res+sizeof(__res)/sizeof(double));
        matcher = new lslgeneric::NDTMatcherF2F(false, false, false, resolutions);
        points2_sub_ = nh_.subscribe("kinect_head/camera/rgb/points", 10, &NDTMatcherNode::points2Callback, this);
        pcl_buffer_.resize(2);
        incremental_pose_.setIdentity();
    }

    ~NDTMatcherNode()
    {
        delete matcher;
    }

    // Callback
    void points2Callback(const sensor_msgs::PointCloud2::ConstPtr& msg_in)
    {
        // Add to a queue

        ROS_INFO("Got points");
        m.lock ();
        pcl::fromROSMsg (*msg_in, pcl_buffer_[0]);
        m.unlock ();

        if (nb_added_clouds_ < pcl_buffer_.size())
        {
            nb_added_clouds_++;
            std::rotate(pcl_buffer_.begin(), pcl_buffer_.begin()+1, pcl_buffer_.end());
            return;
        }

        // The most recent cloud is in [0], the secound in [1], etc.
        Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> T;
        bool ret = matcher->match(pcl_buffer_[1], pcl_buffer_[0],T);
        if (!ret)
            ROS_INFO("Registration failed!");

        incremental_pose_ = T * incremental_pose_; // * T;

        // Publish the transformation...
        tf::Transform transform;
        TransformEigenToTF(T, transform);
        //tf::TransformEigenToTF(incremental_pose_, transform);
        tf_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "test"));

//      std::rotate(pcl_buffer_.begin(), pcl_buffer_.begin()+1, pcl_buffer_.end());
    }
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "ndt_matcher_node");

    NDTMatcherNode t;
    ros::spin();

    return 0;
}


//tomo3d.cpp
/*
 * IFBPJ algorithm implementation
 * (C) 2004-2005 olegabr. All rights reserved.
*/

#include <Magick++.h>

#include <iostream>
using std::cout; using std::endl;

#include <cmath>
using std::sqrt;

#include <boost/lexical_cast.hpp>
using boost::lexical_cast;

#include <tomo3d.h>
#include <object3d.h>
#include <shadow2d.h>
#include <filtered_shadow2d.h>
#include <transforms.h>

#include "transforms_ex.h"

namespace {
	void usage()
	{
		std::cout << "tomo3d start_angle end_angle increment images_directory\n";
	}
}

int main(int argc, char* argv[])
{
	if (argc < 5)
	{
		usage();
		return 0;
	}
	try {
		Magick::InitializeMagick(argv[0]);

		angle_t
			start    (lexical_cast<double>(argv[1])),
			end      (lexical_cast<double>(argv[2])),
			increment(lexical_cast<double>(argv[3]));
		angle_series_t series(start, end, increment);

		shadow2d exp_shadow(series);
		cout << "Loading projections..." << endl;
		exp_shadow.load_from_image(argv[argc-1]);

		normalize(exp_shadow);
		exp_shadow.save_to_image("normalized");

		align_to_gravity_center(exp_shadow);
		exp_shadow.save_to_image("aligned_to_gravity_center");

		cout << "Projections are loaded." << endl;

		tomo3d tomo(exp_shadow);
		tomo.IFBPJ(10, 1.0, 0.0001);
	}
	catch (boost::bad_lexical_cast &ex) {
		cout << ex.what() << endl;
	}
	catch (Magick::Exception &ex) {
		cout << ex.what() << endl;
	}
	catch (std::exception &e) {
		cout << e.what() << endl;
	}
	catch (...) {
		cout << "Unknown exception occured." << endl;
	}

	return 0;
}


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

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2015.
// Modifications copyright (c) 2015, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_ENVELOPE_LINESTRING_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_ENVELOPE_LINESTRING_HPP

#include <iterator>
#include <vector>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/core/coordinate_system.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/iterators/segment_iterator.hpp>

#include <boost/geometry/util/math.hpp>

#include <boost/geometry/algorithms/assign.hpp>
#include <boost/geometry/algorithms/is_empty.hpp>

#include <boost/geometry/algorithms/detail/envelope/range.hpp>
#include <boost/geometry/algorithms/detail/envelope/range_of_boxes.hpp>

#include <boost/geometry/algorithms/dispatch/envelope.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace envelope
{

struct envelope_linear_on_spheroid
{
    template <typename Units, typename Longitude, typename OutputIterator>
    static inline OutputIterator push_interval(Longitude const& lon1,
                                               Longitude const& lon2,
                                               OutputIterator oit)
    {
        typedef longitude_interval<Longitude> interval_type;

        typedef math::detail::constants_on_spheroid
            <
                Longitude, Units
            > constants;

        BOOST_GEOMETRY_ASSERT(! math::larger(lon1, lon2));
        BOOST_GEOMETRY_ASSERT(! math::larger(lon1, constants::max_longitude()));

        if (math::larger(lon2, constants::max_longitude()))
        {
            *oit++ = interval_type(lon1, constants::max_longitude());
            *oit++ = interval_type(constants::min_longitude(),
                                   lon2 - constants::period());
        }
        else
        {
            *oit++ = interval_type(lon1, lon2);
        }
        return oit;
    }

    template <typename Linear, typename Box>
    static inline void apply(Linear const& linear, Box& mbr)
    {
        typedef typename coordinate_type<Box>::type box_coordinate_type;
        typedef longitude_interval<box_coordinate_type> interval_type;

        typedef typename geometry::segment_iterator
            <
                Linear const
            > iterator_type;

        BOOST_GEOMETRY_ASSERT(! geometry::is_empty(linear));

        std::vector<interval_type> longitude_intervals;
        std::back_insert_iterator
            <
                std::vector<interval_type>
            > oit(longitude_intervals);

        box_coordinate_type lat_min = 0, lat_max = 0;
        bool first = true;
        for (iterator_type seg_it = geometry::segments_begin(linear);
             seg_it != geometry::segments_end(linear);
             ++seg_it, first = false)
        {
            Box segment_mbr;
            dispatch::envelope
                <
                    typename std::iterator_traits<iterator_type>::value_type
                >::apply(*seg_it, segment_mbr);

            oit = push_interval
                <
                    typename coordinate_system<Box>::type::units
                >(geometry::get<min_corner, 0>(segment_mbr),
                  geometry::get<max_corner, 0>(segment_mbr),
                  oit);

            if (first)
            {
                lat_min = geometry::get<min_corner, 1>(segment_mbr);
                lat_max = geometry::get<max_corner, 1>(segment_mbr);
            }

            // update min and max latitude, if needed
            if (math::smaller(geometry::get<min_corner, 1>(segment_mbr),
                              lat_min))
            {
                lat_min = geometry::get<min_corner, 1>(segment_mbr);
            }

            if (math::larger(geometry::get<max_corner, 1>(segment_mbr),
                             lat_max))
            {
                lat_max = geometry::get<max_corner, 1>(segment_mbr);
            }
        }

        box_coordinate_type lon_min = 0, lon_max = 0;
        envelope_range_of_longitudes
            <
                typename coordinate_system<Box>::type::units
            >::apply(longitude_intervals, lon_min, lon_max);

        assign_values(mbr, lon_min, lat_min, lon_max, lat_max);
    }
};

template <typename CSTag>
struct envelope_linestring
    : envelope_range<>
{};

template <>
struct envelope_linestring<spherical_equatorial_tag>
    : envelope_linear_on_spheroid
{};

}} // namespace detail::envelope
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename Linestring>
struct envelope<Linestring, linestring_tag>
    : detail::envelope::envelope_linestring<typename cs_tag<Linestring>::type>
{};

} // namespace dispatch
#endif

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_ENVELOPE_LINESTRING_HPP


#pragma once

#include <Eigen/Geometry>

class Camera {
public:
  enum CAMERA_PROJECTION_MODE { ORTHOGRAPHIC, PERSPECTIVE };

  Camera() {
    cameraProjectionMode = ORTHOGRAPHIC;
    displacement_ = std::vector<float>(3, 0);
    orientation_ = Eigen::Quaternion<float>::Identity();
  }

  CAMERA_PROJECTION_MODE getCameraProjectionMode() const {
    return cameraProjectionMode;
  }

  void setCameraProjectionMode(CAMERA_PROJECTION_MODE newCameraProjectionMode) {
    cameraProjectionMode = newCameraProjectionMode;
  }

  std::vector<float> getDisplacement() const { return displacement_; }

  void translate(const std::vector<float> &delta) {
    if (delta.size() != 3)
      throw std::runtime_error(
          "Failed to translate camera: the given delta must contain 3 "
          "dimensions");

    displacement_[0] += delta[0];
    displacement_[1] += delta[1];
    displacement_[2] += delta[2];
  }

  Eigen::Quaternion<float> getOrientation() const { return orientation_; }

  void rotate(const Eigen::Quaternion<float> &delta) {
    orientation_ = delta * orientation_;
    orientation_.normalize();
  }

  void reset() {
    displacement_ = std::vector<float>(3, 0);
    orientation_ = Eigen::Quaternion<float>::Identity();
  }

private:
  CAMERA_PROJECTION_MODE cameraProjectionMode;
  std::vector<float> displacement_;
  Eigen::Quaternion<float> orientation_;
};


#include "predeal.h"
#include "pairwiseICP.hpp"
#include "viz.hpp"
#include <boost/graph/graph_concepts.hpp>
#include<pcl/visualization/pcl_visualizer.h>
#include<pcl/registration/transforms.h>
#include<ctime>
#include<cmath>

using namespace std;
int main()
{

  //*************************
  //读取两个pose文件
  //************************
  string posedir="/home/shaoan/projects/slamEvaluation/data/pose.txt";
  vector<Eigen::Vector3f> v_pose;
  predeal::readposefile(posedir,v_pose);
    string maposedir="/home/shaoan/projects/slamEvaluation/data/optimizedpose.txt";
  vector<Eigen::Vector3f> v_mapose;
  predeal::readposefile(maposedir,v_mapose);

  if(v_mapose.size()!=v_pose.size())
  {
    cout<<"数据记录有误"<<endl;
    exit(0);
  }

  //**************************************************************
  //创建两个新文件，用来保存手动配准的位姿和slam算法给出的位姿
  //**************************************************************
ofstream rgsdPosefile, maPosefile;
  string rgsPosefileName="/home/shaoan/projects/slamEvaluation/data/rgspose.txt";
 rgsdPosefile.open(rgsPosefileName.c_str(),ios_base::out);
 string maPosefileName="/home/shaoan/projects/slamEvaluation/data/maPose.txt";
 maPosefile.open(maPosefileName.c_str(),ios_base::out);

  //************************************************
  //手动配准，并观察效果，决定是否记录
  //************************************************
  time_t begin,finish ;

 //const int numFile=v_mapose.size();
  const int numFile=9;
 for(int i=0;i<numFile-1;i++)
  {
    char target_pcddir[100];
    char source_pcddir[100];
    pcl::PointCloud<pcl::PointXYZ> source_pcd, target_pcd;
    string dir="/home/shaoan/projects/slamEvaluation/data/";
    sprintf(target_pcddir,"%sscan%.3d.txt",dir.c_str(),i);
    predeal::readpcdfile(target_pcddir,target_pcd);
    sprintf(source_pcddir,"%sscan%.3d.txt",dir.c_str(),i+1);
    predeal::readpcdfile(source_pcddir,source_pcd);

    /*
    Eigen::Vector3f tgt_pose,src_pose;
    tgt_pose=v_pose[i];
    src_pose=v_pose[i+1];
     src_pose(2)=2*M_PI+src_pose(2);
      tgt_pose(2)=2*M_PI+tgt_pose(2);
     Eigen::Quaternion<float> q_w_target(cos(tgt_pose(2)/2),0,0,sin(tgt_pose(2)/2));
    Eigen::Quaternion<float> q_w_source(cos(src_pose(2)/2),0,0,sin(src_pose(2)/2));
    Eigen::Vector3f t_w_target, t_w_source;
   t_w_target<<tgt_pose(0), tgt_pose(1),0.0f;
   t_w_source<<src_pose(0), src_pose(1),0.0f;
    Eigen::Matrix3f R_initial=q_w_target.toRotationMatrix().transpose()*q_w_source.toRotationMatrix();
    Eigen::Vector3f t_initial=q_w_target.toRotationMatrix().transpose()*(t_w_source-t_w_target);
    */

    Eigen::Matrix3f R_initial;
    Eigen::Vector3f t_initial;
      Eigen::Vector3f odomEdge=predeal::computeConstraint(v_pose[i],v_pose[i+1]);
    predeal::vector3ftoRationTtrans(odomEdge,R_initial,t_initial);
    Eigen::Vector3f maposeEdge;
    maposeEdge=predeal::computeConstraint(v_mapose[i],v_mapose[i+1]);
  //predeal::vector3ftoRationTtrans(maposeEdge,R_initial,t_initial);

   Eigen::Matrix3f R=R_initial;
   Eigen::Vector3f t=t_initial;

   map3d::ICP icp;
   begin=clock();
   icp.setInputCloud(source_pcd.makeShared(),target_pcd.makeShared());
   icp.setNum(100);
   icp.setParamsConvergence(200, 2, 0.02, 0.05);
   icp.setReJectThrehold(2, 0.1,0.01,0.01);
   icp.solve(R,t);
   finish=clock();
  cout<<R<<endl;
  cout<<t<<endl;

  Eigen::Matrix4f T=Eigen::Matrix4f::Identity();
  T.block(0,0,3,3)=R;T.block(0,3,3,1)=t;
  cout<<T<<endl;

  //**************************************************
   //可视化
   //***************************************************
  pcl::PointCloud<pcl::PointXYZ> transformedpcd;
  pcl::transformPointCloud(source_pcd,transformedpcd,T);
  assert(!source_pcd.empty());
 // vector<pcl::PointCloud<pcl::PointXYZ>::ConstPtr > v_pcd;
 // v_pcd.push_back(target_pcd.makeShared());v_pcd.push_back(transformedpcd.makeShared());
  boost::shared_ptr<pcl::visualization::PCLVisualizer > viewer(new pcl::visualization::PCLVisualizer);
  viewer=map3d::normalVis(target_pcd.makeShared(),transformedpcd.makeShared());
  while(!viewer->wasStopped())
  {
    viewer->spinOnce();
     boost::this_thread::sleep(boost::posix_time::microseconds(100000));
  }

  //***************************************
  //保存数据
  //***************************************
  cout<<"是否保存当前数据用于评估（y/n）"<<endl;
  char keyValue;
 L1: cin>>keyValue;
  if(keyValue=='y')
  {
    if(rgsdPosefile.good()&&maPosefile.good())
    {
      Eigen::Quaternion<float> q(R);
      float theta;
      float a1=acos(q.w());
      float a2=-acos(q.w());
      if(abs(sin(a1)-q.z())<0.001f)
	{
	  theta=2*a1;
	}
	else if(abs(sin(a2)-q.z())<0.001f)
	{
	  theta=2*a2;
	  //cout<<v(2)<<endl;
	}
      rgsdPosefile<<t(0)<<" "<<t(1)<<" "<<theta<<endl;
      maPosefile<<maposeEdge(0)<<" "<<maposeEdge(1)<<" "<<maposeEdge(2)<<endl;
    }
    else
    {
      cout<<"未能保存位姿"<<endl;
      exit(0);
    }
  }
  else if(keyValue=='n')
  {
    continue;
  }
  else
  {
    cout<<"未知输入，请重新输入"<<endl;
    goto L1;
  }

  }
  cout<<"ICP 花费的时间："<<double(finish-begin)/CLOCKS_PER_SEC<<endl;
  rgsdPosefile.close();
  maPosefile.close();

}