/******************************************************************************

  Basic linear algebra subprograms.

  Copyright (c) 2011 - 2013
  Dzmitry Hlindzich <dzmitry.hlindzich@ziti.uni-heidelberg.de>
  Alexander Rukletsov <rukletsov@gmail.com>
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions
  are met:
  1.  Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
  2.  Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.

  THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS "AS IS" AND
  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  SUCH DAMAGE.

*******************************************************************************/

#ifndef BLAS_EXTENSIONS_HPP_F74A6974_6444_4C40_BFE7_75ADEC15B7E6_
#define BLAS_EXTENSIONS_HPP_F74A6974_6444_4C40_BFE7_75ADEC15B7E6_

#include <iostream>
#include <vector>
#include <cmath>
#include <boost/format.hpp>

// Suppress boost::numeric::ublas C4127 warning under MSVC.
#ifdef _MSC_VER
#   pragma warning(push)
#   pragma warning(disable:4127)
#endif // _MSC_VER

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>

#ifdef _MSC_VER
#   pragma warning(pop)
#endif // _MSC_VER

#include "bo/math/functions.hpp"

// Dirty hack: add stream operator for matrix<T> type into boost namespace.
namespace boost {
namespace numeric {
namespace ublas {

// Prints formatted matrix to the given stream. T must support operator<<.
template <typename T>
std::ostream& operator<<(std::ostream& os, const matrix<T>& obj)
{
    // Print full vertex info.
    os << boost::format("%1%x%2% boost::numeric::ublas::matrix, object %3$#x, %4% bytes: ")
          % obj.size1() % obj.size2() % &obj % sizeof(obj) << std::endl;

    for (std::size_t i = 0; i < obj.size1(); ++i)
    {
        for (std::size_t j = 0; j < obj.size2(); ++j)
        {
            os << "\t " << obj(i, j);
        }
        os << std::endl;
    }

    os << boost::format("end of object %1$#x.") % &obj << std::endl;

    return os;
}

} // namespace ublas
} // namespace numeric
} // namespace boost

namespace bo {
namespace math {

using namespace boost::numeric::ublas;

// Matrix inversion routine.
// Use lu_factorize and lu_substitute in uBLAS to invert a matrix.
template<class T>
bool invert_matrix(const matrix<T>& input, matrix<T>& inverse)
{
    // Create a working copy of the input
    matrix<T> A(input);

    // Create a permutation matrix for the LU-factorization
    permutation_matrix<std::size_t> pm(A.size1());

    // Perform LU-factorization
    size_t res = lu_factorize(A, pm);
    if (res != 0)
        return false;

    // Create identity matrix of "inverse"
    inverse.assign(identity_matrix<T> (A.size1()));

    // Backsubstitute to get the inverse
    lu_substitute(A, pm, inverse);

    return true;
}

// Define the sign of the determinant using the given permutation matrix.
inline
int determinant_sign(const permutation_matrix<std::size_t>& pm)
{
    int pm_sign=1;
    std::size_t size = pm.size();
    for (std::size_t i = 0; i < size; ++i)
    {
        if (i != pm(i))
        {
            // swap_rows would swap a pair of rows here, so we change sign
            pm_sign *= -1;
        }
    }

    return pm_sign;
}

// Calculate the determinant of the input matrix.
template<class T>
double determinant(const matrix<T>& input)
{
    // create a working copy of the input
    matrix<T> A(input);

    // create a permutation matrix for the LU-factorization
    permutation_matrix<std::size_t> pm(A.size1());

    // perform LU-factorization
    int res = lu_factorize(A, pm);

    double det=1.0;

    if (res != 0 )
    {
        det = 0.0;
    }
    else
    {
        // multiply by elements on diagonal
        for(int i = 0; i < A.size1(); ++i)
            det *= A(i,i);
        det = det * determinant_sign( pm );
    }

    return det;
}

// Eigenvector decomposition for real symmetric matrices. Returns a vector of the
// eigenvalues, sorted in non-decreasing order. The corresponding eigenvectors are
// stored in the columns of the matrix expr.
template <class E>
std::vector<typename E::value_type> eigen_symmetric(matrix_expression<E>& expr)
{
    // Cache the underlying matrix type.
    typedef typename E::value_type T;

    // Evaluate and cache the matrix_expression.
    typename E::closure_type A(expr());

    // Allocate the return vector.
    const int n(static_cast<int>(A.size1()));
    std::vector<T> d(n);

    // Initialize the return vector.
    for (int j = 0; j < n; ++j)
    {
        d[j] = A(n - 1, j);
    }

    BOOST_ASSERT(A.size2() == (unsigned)n);

    // Check for a square matrix.
    if (A.size2() != (unsigned)n)
    {
        return d;
    }

    std::vector<T> e(n, T(0));

    // Iterating.
    for (int i = n - 1; i > 0; --i)
    {
        T scale(0);

        for (int k = 0; k < i; ++k)
        {
            scale += std::abs(d[k]);
        }

        if (scale == T(0))
        {
            e[i] = d[i - 1];

            for (int j = 0; j < i; ++j)
            {
                d[j] = A(i - 1, j);
                A(i, j) = A(j, i) = T(0);
            }

            d[i] = T(0);
        }
        else
        {
            T h(0);
            T invscale = T(1.0 / scale);

            for (int k = 0; k < i; ++k)
            {
                d[k] *= invscale;
                h += square(d[k]);
            }

            T f = d[i - 1];
            T g = (f > 0) ? -std::sqrt(h) : std::sqrt(h);
            e[i] = scale * g;
            h -= f * g;
            d[i - 1] = f - g;

            for (int j = 0; j < i; ++j)
            {
                e[j] = T(0);
            }

            for (int j = 0; j < i; ++j)
            {
                f = d[j];
                A(j, i) = f;
                g = e[j] + f * A(j, j);

                for (int k = j+1; k < i; k++)
                {
                    g += A(k, j) * d[k];
                    e[k] += A(k, j) * f;
                }

                e[j] = g;
            }

            f = T(0);
            T invh = T(1.0 / h);

            for (int j = 0; j < i; ++j)
            {
                e[j] *= invh;
                f += e[j] * d[j];
            }

            T hh = f / (h + h);

            for (int j = 0; j < i; j++)
            {
                e[j] -= hh * d[j];
            }

            for (int j = 0; j < i; j++)
            {
                f = d[j];
                g = e[j];

                for (int k = j; k < i; k++)
                {
                    A(k, j) -= f * e[k] + g * d[k];
                }

                d[j] = A(i - 1, j);
                A(i, j) = T(0);
            }

            d[i] = h;
        }
    }

    // I put on my robe and wizard hat...
    for (int i = 0; i < n - 1; ++i)
    {
        A(n - 1, i) = A(i, i);
        A(i, i) = 1;

        T h = d[i+1];

        if (h != T(0))
        {
            T invh = T(1.0 / h);

            for (int k = 0; k <= i; ++k)
            {
                d[k] = A(k, i + 1) * invh;
            }

            for (int j = 0; j <= i; ++j)
            {
                T g(0);

                for (int k = 0; k <= i; ++k)
                {
                    g += A(k, i + 1) * A(k, j);
                }

                for (int k = 0; k <= i; ++k)
                {
                    A(k, j) -= g * d[k];
                }

            }
        }

        for (int k = 0; k <= i; ++k)
        {
            A(k, i + 1) = T(0);
        }
    }

    for (int j = 0; j < n; ++j)
    {
        d[j] = A(n - 1, j);
        A(n - 1, j) = T(0);
    }

    A(n - 1, n - 1) = 1;

    // QL.
    for (int i = 1; i < n; ++i)
    {
        e[i - 1] = e[i];
    }

    e[n - 1] = T(0);
    T f(0), tmp(0);
    const T eps = T(std::pow(T(2), -52));

    for (int l = 0; l < n; l++)
    {
        tmp = std::max(tmp, std::fabs(d[l]) + std::fabs(e[l]));
        int m = l;

        while (m < n)
        {
            if (std::fabs(e[m]) <= eps * tmp)
                break;
            ++m;
        }

        if (m > l)
        {
            do
            {
                T g = d[l];
                T p = (d[l + 1] - g) / (e[l] + e[l]);
                T r = T(std::sqrt(square(p) + T(1)));

                if (p < T(0))
                {
                    r = -r;
                }

                d[l] = e[l] / (p + r);
                d[l + 1] = e[l] * (p + r);
                T dl1 = d[l + 1];
                T h = g - d[l];

                for (int i = l + 2; i < n; ++i)
                {
                    d[i] -= h;
                }

                f += h;
                p = d[m];
                T c(1), c2(1), c3(1);

                T el1 = e[l + 1];
                T s(0), s2(0);

                for (int i = m - 1; i >= l; --i)
                {
                    c3 = c2;
                    c2 = c;
                    s2 = s;
                    g = c * e[i];
                    h = c * p;

                    r = T(std::sqrt(square(p) + square(e[i])));

                    e[i + 1] = s * r;
                    s = e[i] / r;
                    c = p / r;
                    p = c * d[i] - s * g;
                    d[i + 1] = h + s * (c * g + s * d[i]);

                    for (int k = 0; k < n; ++k)
                    {
                        h = A(k, i + 1);
                        A(k, i + 1) = s * A(k, i) + c * h;
                        A(k, i) = c * A(k, i) - s * h;
                    }
                }

                p = -s * s2 * c3 * el1 * e[l] / dl1;

                e[l] = s * p;
                d[l] = c * p;

            }
            while (std::fabs(e[l]) > eps * tmp);
        }

        d[l] += f;
        e[l] = T(0);
    }

    // Sort.
    for (int i = 0; i < n - 1; ++i)
    {
        int k = i;
        T p = d[i];

        for (int j = i + 1; j < n; ++j)
        {
            if (d[j] < p)
            {
                k = j;
                p = d[j];
            }
        }

        if (k == i) continue;

        d[k] = d[i];
        d[i] = p;

        for (int j = 0; j < n; ++j)
        {
            p = A(j, i);
            A(j, i) = A(j, k);
            A(j, k) = p;
        }
    }

    return d;
}

// Computes the L1 norm of matrix. Uses std::abs(T), which implies T is a built-in type
// or a custom type, for which abs() is provided in std namespace.
template <class T>
T l1_norm(const matrix<T>& A)
{
    T sum(0);

    for (std::size_t i = 0; i < A.size1(); ++i)
        for (std::size_t j = 0; j < A.size2(); ++j)
            sum += std::abs(A(i, j));

    return sum;
}

} // namespace math
} // namespace bo

#endif // BLAS_EXTENSIONS_HPP_F74A6974_6444_4C40_BFE7_75ADEC15B7E6_


/*
 * Copyright (c) 2015, The Regents of the University of California (Regents).
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *    1. Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *
 *    2. Redistributions in binary form must reproduce the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer in the documentation and/or other materials provided
 *       with the distribution.
 *
 *    3. Neither the name of the copyright holder nor the names of its
 *       contributors may be used to endorse or promote products derived
 *       from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS AS IS
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * Please contact the author(s) of this library if you have any questions.
 * Authors: David Fridovich-Keil   ( dfk@eecs.berkeley.edu )
 *          Erik Nelson            ( eanelson@eecs.berkeley.edu )
 */

///////////////////////////////////////////////////////////////////////////////
//
// This class defines the FundamentalMatrixRansacModel class, which
// is derived from the abstract base class RansacModel.
//
///////////////////////////////////////////////////////////////////////////////

#include <iostream>

#include <algorithm>
#include <Eigen/Core>
#include <gflags/gflags.h>
#include <vector>

#include "ransac_problem.h"
#include "fundamental_matrix_ransac_problem.h"
#include "../math/random_generator.h"
#include "../geometry/eight_point_algorithm_solver.h"
#include "../geometry/fundamental_matrix_solver_options.h"

namespace bsfm {

// ------------ FundamentalMatrixRansacModel methods ------------ //

// Default constructor.
FundamentalMatrixRansacModel::FundamentalMatrixRansacModel()
    : F_(Matrix3d::Identity()), error_(0.0) {}

FundamentalMatrixRansacModel::FundamentalMatrixRansacModel(
    const Matrix3d& F)
    : F_(F), error_(0.0) {}

// Destructor.
FundamentalMatrixRansacModel::~FundamentalMatrixRansacModel() {}

// Return model error.
double FundamentalMatrixRansacModel::Error() const {
  return error_;
}

// Evaluate model on a single data element and update error.
bool FundamentalMatrixRansacModel::IsGoodFit(
    const FeatureMatch& data_point,
    double error_tolerance) const {
  const double error = EvaluateEpipolarCondition(data_point);

  // Test squared error against the provided tolerance.
  if (error * error < error_tolerance) {
    return true;
  }
  return false;
}

double FundamentalMatrixRansacModel::EvaluateEpipolarCondition(
    const FeatureMatch& match) const {
  // Construct vectors for 2D keypoints in match.
  Vector3d kp1, kp2;
  kp1 << match.feature1_.u_, match.feature1_.v_, 1;
  kp2 << match.feature2_.u_, match.feature2_.v_, 1;

  // Compute deviation from the epipolar condition.
  const double epipolar_condition = kp2.transpose() * F_ * kp1;
  return epipolar_condition;
}

// ------------ FundamentalMatrixRansacProblem methods ------------ //

// RansacProblem constructor.
FundamentalMatrixRansacProblem::FundamentalMatrixRansacProblem() {}

// RansacProblem destructor.
FundamentalMatrixRansacProblem::~FundamentalMatrixRansacProblem() {}

// Subsample the data.
std::vector<FeatureMatch> FundamentalMatrixRansacProblem::SampleData(
    unsigned int num_samples) {
  // Randomly shuffle the entire dataset and take the first elements.
  std::random_shuffle(data_.begin(), data_.end());

  // Make sure we don't over step.

  if (static_cast<size_t>(num_samples) > data_.size()) {
    VLOG(1) << "Requested more RANSAC data samples than are available. "
               "Returning all data.";
    num_samples = data_.size();
  }

  // Get samples.
  std::vector<FeatureMatch> samples(
      data_.begin(), data_.begin() + static_cast<size_t>(num_samples));

  return samples;
}

// Return all data that was not sampled.
std::vector<FeatureMatch> FundamentalMatrixRansacProblem::RemainingData(
    unsigned int num_sampled_previously) const {
  // In Sample(), the data was shuffled and we took the first
  // 'num_sampled_previously' elements. Here, take the remaining elements.
  if (num_sampled_previously >= data_.size()) {
    VLOG(1) << "No remaining RANSAC data to sample.";
    return std::vector<FeatureMatch>();
  }

  return std::vector<FeatureMatch>(
      data_.begin() + num_sampled_previously, data_.end());
}

// Fit a model to the provided data using the 8-point algorithm.
FundamentalMatrixRansacModel FundamentalMatrixRansacProblem::FitModel(
    const std::vector<FeatureMatch>& input_data) const {
  // Create an empty fundamental matrix.
  Matrix3d F;

  // Run the 8-point algorithm with default options.
  EightPointAlgorithmSolver solver;
  FundamentalMatrixSolverOptions options;
  solver.SetOptions(options);

  if (solver.ComputeFundamentalMatrix(input_data, F)) {
    // Create a new RansacModel using the computed fundamental matrix.
    FundamentalMatrixRansacModel model_out(F);

    // Record sum of squared error over all matches.
    model_out.error_ = 0.0;
    for (const auto& feature_match : input_data) {
      const double error = model_out.EvaluateEpipolarCondition(feature_match);
      model_out.error_ += error * error;
    }

    return model_out;
  }
  // Set a large error - we didn't find a model that fits the data.
  FundamentalMatrixRansacModel model_out(Matrix3d::Identity());
  model_out.error_ = std::numeric_limits<double>::infinity();
  return model_out;
}

}  //\namespace bsfm


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef ITL_BICGSTAB_INCLUDE
#define ITL_BICGSTAB_INCLUDE

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/operation/resource.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

#include <boost/numeric/itl/utility/exception.hpp>
#include <boost/numeric/itl/krylov/base_solver.hpp>

namespace itl {

///  Bi-Conjugate Gradient Stabilized
template < class LinearOperator, class HilbertSpaceX, class HilbertSpaceB,
	   class Preconditioner, class Iteration >
int bicgstab(const LinearOperator& A, HilbertSpaceX& x, const HilbertSpaceB& b,
	     const Preconditioner& M, Iteration& iter)
{
  typedef typename mtl::Collection<HilbertSpaceX>::value_type Scalar;
  typedef HilbertSpaceX                                       Vector;
  mtl::vampir_trace<7004> tracer;

  Scalar     rho_1(0), rho_2(0), alpha(0), beta(0), gamma, omega(0);
  Vector     p(resource(x)), phat(resource(x)), s(resource(x)), shat(resource(x)),
             t(resource(x)), v(resource(x)), r(resource(x)), rtilde(resource(x));

  r = b - A * x;
  rtilde = r;

  while (! iter.finished(r)) {
    ++iter;
    rho_1 = dot(rtilde, r);
    MTL_THROW_IF(rho_1 == 0.0, unexpected_orthogonality());

    if (iter.first())
      p = r;
    else {
      MTL_THROW_IF(omega == 0.0, unexpected_orthogonality());
      beta = (rho_1 / rho_2) * (alpha / omega);
      p = r + beta * (p - omega * v);
    }
    phat = solve(M, p);
    v = A * phat;

    gamma = dot(rtilde, v);
    MTL_THROW_IF(gamma == 0.0, unexpected_orthogonality());

    alpha = rho_1 / gamma;
    s = r - alpha * v;

    if (iter.finished(s)) {
      x += alpha * phat;
      break;
    }
    shat = solve(M, s);
    t = A * shat;
    omega = dot(t, s) / dot(t, t);

    x += omega * shat + alpha * phat;
    r = s - omega * t;

    rho_2 = rho_1;
  }
  return iter;
}

/// Solver class for BiCGStab method; right preconditioner ignored (prints warning if not identity)
/** Methods inherited from \ref base_solver. **/
template < typename LinearOperator, typename Preconditioner= pc::identity<LinearOperator>,
	   typename RightPreconditioner= pc::identity<LinearOperator> >
class bicgstab_solver
  : public base_solver< bicgstab_solver<LinearOperator, Preconditioner, RightPreconditioner>, LinearOperator >
{
    typedef base_solver< bicgstab_solver<LinearOperator, Preconditioner, RightPreconditioner>, LinearOperator > base;
  public:
    /// Construct solver from a linear operator; generate (left) preconditioner from it
    explicit bicgstab_solver(const LinearOperator& A) : base(A), L(A)
    {
	if (!pc::static_is_identity<RightPreconditioner>::value)
	    std::cerr << "Right Preconditioner ignored!" << std::endl;
    }

    /// Construct solver from a linear operator and (left) preconditioner
    bicgstab_solver(const LinearOperator& A, const Preconditioner& L) : base(A), L(L)
    {
	if (!pc::static_is_identity<RightPreconditioner>::value)
	    std::cerr << "Right Preconditioner ignored!" << std::endl;
    }

    /// Solve linear system approximately as specified by \p iter
    template < typename HilbertSpaceX, typename HilbertSpaceB, typename Iteration >
    int solve(HilbertSpaceX& x, const HilbertSpaceB& b, Iteration& iter) const
    {
	return bicgstab(this->A, x, b, L, iter);
    }

  private:
    Preconditioner        L;
};

} // namespace itl

#endif // ITL_BICGSTAB_INCLUDE


#include <Eigen/Dense>
#include <vector>
#include <cassert>
#include <cmath>
#include "day_6.hpp"

// We will be using these a lot so for convenience
// make it so we don't have to put Eigen:: in front of them.
using Eigen::MatrixXd;
using Eigen::Vector3d;

/*! \brief Returns the value of the chi tensor for 3 orbital indices on the same atom
 */
double chi_on_atom(int o1, int o2, int o3, double model_dipole)
{
    if(o1 == o2 and o3 == 0)
        return 1.0;
    if(o1 == o3 && o3 > 0 && o2 == 0)
        return model_dipole;
    if(o2 == o3 && o3 > 0 && o1 == 0)
        return model_dipole;
    return 0.0;
}

/*! \brief Returns the atom index part of an atomic orbital index
 */
int atom(int ao_index)
{
    // Division of two integers always results in an integer in C++
    return ao_index / orbitals_per_atom;
}

/*! \brief Returns the atomic orbital index for a given atom index and orbital type
 */
int ao_index(int atom_p, int orb_p)
{
    return atom_p*orbitals_per_atom + orb_p;
}

/*! \brief Returns the orbital type of an atomic orbital index
 */
int orb(int ao_index)
{
    return ao_index % orbitals_per_atom;
}

MatrixXd calculate_fock_matrix_fast(MatrixXd hamiltonian_matrix,
                                    MatrixXd interaction_matrix,
                                    MatrixXd density_matrix,
                                    double model_dipole)
{
    // Number of degrees of freedon
    const size_t ndof = hamiltonian_matrix.rows();
    MatrixXd fock_matrix(hamiltonian_matrix); // Calls copy constructor

    // Potential term
    for(size_t p = 0; p < ndof; p++)
    {
        for(int orb_q = 0; orb_q < orbitals_per_atom; orb_q++)
        {
            int q = ao_index(atom(p), orb_q); // p & q on same atom

            for(int orb_t = 0; orb_t < orbitals_per_atom; orb_t++)
            {
                int t = ao_index(atom(p), orb_t); // p & t on same atom
                double chi_pqt = chi_on_atom(orb(p), orb_q, orb_t, model_dipole);

                for(size_t r = 0; r < ndof; r++)
                {
                    for(int orb_s = 0; orb_s < orbitals_per_atom; orb_s++)
                    {
                        int s = ao_index(atom(r), orb_s); // r & s on same atom
                        for(int orb_u = 0; orb_u < orbitals_per_atom; orb_u++)
                        {
                            int u = ao_index(atom(r), orb_u); // r & u on same atom
                            double chi_rsu = chi_on_atom(orb(r), orb_s, orb_u, model_dipole);
                            fock_matrix(p,q) += 2.0 * chi_pqt * chi_rsu * interaction_matrix(t,u) * density_matrix(r,s);
                        }
                    }
                }
            }
        }
    }

    // Exchange term
    for(size_t p = 0; p < ndof; p++)
    {
        for(int orb_s = 0; orb_s < orbitals_per_atom; orb_s++)
        {
            int s = ao_index(atom(p), orb_s); // p & s on same atom
            for(int orb_u = 0; orb_u < orbitals_per_atom; orb_u++)
            {
                int u = ao_index(atom(p), orb_u); // p & u on same atom
                double chi_psu = chi_on_atom(orb(p), orb_s, orb_u, model_dipole);

                for(size_t q = 0; q < ndof; q++)
                {
                    for(int orb_r = 0; orb_r < orbitals_per_atom; orb_r++)
                    {
                        int r = ao_index(atom(q), orb_r); // q & r on same atom
                        for(int orb_t = 0; orb_t < orbitals_per_atom; orb_t++)
                        {
                            int t = ao_index(atom(q), orb_t);
                            double chi_rqt = chi_on_atom(orb_r, orb(q), orb_t, model_dipole);
                            fock_matrix(p,q) -= chi_rqt * chi_psu * interaction_matrix(t,u) * density_matrix(r,s);
                        }
                    }
                }
            }
        }
    }

    return fock_matrix;
}

double calculate_energy_mp2(std::vector<double> E_occ, std::vector<double> E_virt, MatrixXd v_tilde_flat)
{
    size_t num_occ = E_occ.size();
    size_t num_virt = E_virt.size();

    double energy_mp2 = 0.0;

    // The v_tilde matrix has been flattened from 4 indices to 2 indices.
    // The dimensions of the flattened tensor should be nvirt*nocc x nvirt*nocc
    size_t nrow = v_tilde_flat.rows(); // should be nvirt*nocc
    size_t ncol = v_tilde_flat.cols(); // should be nvirt*nocc
    assert(nrow == num_virt*num_occ);
    assert(ncol == num_virt*num_occ);

    for(size_t a = 0; a < num_virt; a++)
    for(size_t i = 0; i < num_occ; i++)
    {
        size_t idx_1 = a*num_occ+i;
        for(size_t b = 0; b < num_virt; b++)
        for(size_t j = 0; j < num_occ; j++)
        {
            size_t idx_2 = b*num_occ+j;
            size_t idx_3 = a*num_occ+j;
            size_t idx_4 = b*num_occ+i;

            double numerator = 2.0 * pow(v_tilde_flat(idx_1, idx_2), 2) - v_tilde_flat(idx_1, idx_2)*v_tilde_flat(idx_3, idx_4);
            double denominator = E_virt[a] + E_virt[b] - E_occ[i] - E_occ[j];
            energy_mp2 -= numerator/denominator;
        }
    }

    return energy_mp2;
}


#include "PhysicsTools/Utilities/interface/Parameter.h"
#include "PhysicsTools/Utilities/interface/ZLineShape.h"
#include "PhysicsTools/Utilities/interface/Gaussian.h"
#include "PhysicsTools/Utilities/interface/Numerical.h"
#include "PhysicsTools/Utilities/interface/Exponential.h"
#include "PhysicsTools/Utilities/interface/Polynomial.h"
#include "PhysicsTools/Utilities/interface/Constant.h"
#include "PhysicsTools/Utilities/interface/Convolution.h"
#include "PhysicsTools/Utilities/interface/Operations.h"
#include "PhysicsTools/Utilities/interface/Integral.h"
#include "PhysicsTools/Utilities/interface/MultiHistoChiSquare.h"
#include "PhysicsTools/Utilities/interface/RootMinuit.h"
#include "PhysicsTools/Utilities/interface/RootMinuitCommands.h"
#include "PhysicsTools/Utilities/interface/FunctClone.h"
#include "PhysicsTools/Utilities/interface/rootPlot.h"
#include "TROOT.h"
#include "TH1.h"
#include "TFile.h"
#include <boost/program_options.hpp>
using namespace boost;
namespace po = boost::program_options;

#include <iostream>
#include <algorithm>
#include <exception>
#include <iterator>
#include <string>
#include <vector>
using namespace std;

// A helper function to simplify the main part.
template<class T>
ostream& operator<<(ostream& os, const vector<T>& v) {
  copy(v.begin(), v.end(), ostream_iterator<T>(cout, " "));
  return os;
}

//A function that sets istogram contents to 0
//if they are too small
void fix(TH1* histo) {
  for(int i = 1; i <= histo->GetNbinsX(); ++i) {
    if(histo->GetBinContent(i) < 0.1) {
      histo->SetBinContent(i, 0.0);
      histo->SetBinError(i, 0.0);
    }
  }
}

typedef funct::GaussIntegrator IntegratorConv;
typedef funct::GaussIntegrator IntegratorNorm;

typedef funct::Product<funct::Exponential,
		       funct::Convolution<funct::ZLineShape, funct::Gaussian, IntegratorConv>::type>::type ZPeak;

typedef funct::Master<ZPeak> SigPeak;
typedef funct::Slave<ZPeak> SigPeakClone;
typedef funct::Product<funct::Parameter, SigPeak>::type Sig1;
typedef funct::Product<funct::Parameter, SigPeakClone>::type Sig2;

typedef funct::Product<
            funct::Exponential,
            funct::Polynomial<2> >::type ExpPoly;

NUMERICAL_FUNCT_INTEGRAL(ExpPoly, GaussIntegrator);

typedef funct::DefIntegral<ExpPoly, funct::Constant, funct::Constant, IntegratorNorm> ExpPolyNormFactor;
typedef funct::Ratio<ExpPoly, ExpPolyNormFactor>::type ExpPolyNorm;

typedef funct::Product<
          funct::Parameter,
          funct::Difference<
            funct::Ratio<
              funct::Numerical<2>,
              funct::Parameter>::type,
            funct::Numerical<2> >::type>::type Coeff1;

typedef funct::Product<Coeff1, ExpPolyNorm>::type Bkg1;

typedef funct::Product<
          funct::Parameter,
          funct::Square<
            funct::Difference<
              funct::Ratio<
                funct::Numerical<1>,
                funct::Parameter>::type,
              funct::Numerical<1> >::type>::type>::type Coeff2;

typedef funct::Product<Coeff2, ExpPolyNorm>::type Bkg2;

NUMERICAL_FUNCT_INTEGRAL(Bkg1, GaussIntegrator);
NUMERICAL_FUNCT_INTEGRAL(Bkg2, GaussIntegrator);

typedef funct::Sum<Sig1, Bkg1>::type Fun1;
typedef funct::Sum<Sig2, Bkg2>::type Fun2;
typedef fit::MultiHistoChiSquare<Fun1, Fun2> ChiSquared;

int main(int ac, char *av[]) {
  gROOT->SetStyle("Plain");
  try {
    double fMin, fMax;
    string ext;
    po::options_description desc("Allowed options");
    desc.add_options()
      ("help,h", "produce help message")
      ("input-file,i", po::value< vector<string> >(), "input file")
      ("min,m", po::value<double>(&fMin)->default_value(60), "minimum value for fit range")
      ("max,M", po::value<double>(&fMax)->default_value(120), "maximum value for fit range")
      ("plot-format,p", po::value<string>(&ext)->default_value("ps"),
       "output plot format")
      ;

    po::positional_options_description p;
    p.add("input-file", -1);

    po::variables_map vm;
    po::store(po::command_line_parser(ac, av).
	    options(desc).positional(p).run(), vm);
    po::notify(vm);

    if (vm.count("help")) {
      cout << "Usage: options_description [options]\n";
      cout << desc;
      return 0;
      }

    fit::RootMinuitCommands<ChiSquared> commands("csa08IsoBkg.txt");

    if (vm.count("input-file")) {
      cout << "Input files are: "
	   << vm["input-file"].as< vector<string> >() << "\n";
      vector<string> v_file = vm["input-file"].as< vector<string> >();
      for(vector<string>::const_iterator it = v_file.begin();
	  it != v_file.end(); ++it) {
	TFile * root_file = new TFile(it->c_str(),"read");

	TH1D * histo1 = (TH1D*) root_file->Get("oneNonIsolatedZToMuMuPlots/zMass");
	fix(histo1);
	TH1D * histo2 = (TH1D*) root_file->Get("twoNonIsolatedZToMuMuPlots/zMass");
	fix(histo2);

	cout << ">>> histogram loaded\n";
	string f_string = *it;
	replace(f_string.begin(), f_string.end(), '.', '_');
	replace(f_string.begin(), f_string.end(), '/', '_');
	string plot_string = f_string + "." + ext;
	cout << ">>> Input files loaded\n";

	const char * kYieldZMuMu1 = "YieldZMuMu1";
	const char * kYieldZMuMu2 = "YieldZMuMu2";
	const char * kYieldBkg = "YieldBkg";
	const char * kEffBkg = "EffBkg";
	const char * kLambdaZMuMu = "LambdaZMuMu";
	const char * kMass = "Mass";
	const char * kGamma = "Gamma";
	const char * kPhotonFactorZMuMu = "PhotonFactorZMuMu";
	const char * kInterferenceFactorZMuMu = "InterferenceFactorZMuMu";
	const char * kMeanZMuMu = "MeanZMuMu";
	const char * kSigmaZMuMu = "SigmaZMuMu";
	const char * kAlpha = "Alpha";
	const char * kA0 = "A0";
	const char * kA1 = "A1";
	const char * kA2 = "A2";

	funct::Parameter lambdaZMuMu(kLambdaZMuMu, commands.par(kLambdaZMuMu));
	funct::Parameter mass(kMass, commands.par(kMass));
	funct::Parameter gamma(kGamma, commands.par(kGamma));
	funct::Parameter photonFactorZMuMu(kPhotonFactorZMuMu, commands.par(kPhotonFactorZMuMu));
	funct::Parameter interferenceFactorZMuMu(kInterferenceFactorZMuMu, commands.par(kInterferenceFactorZMuMu));
	funct::Parameter yieldZMuMu1(kYieldZMuMu1, commands.par(kYieldZMuMu1));
	funct::Parameter yieldZMuMu2(kYieldZMuMu2, commands.par(kYieldZMuMu2));
	funct::Parameter yieldBkg(kYieldBkg, commands.par(kYieldBkg));
	funct::Parameter effBkg(kEffBkg, commands.par(kEffBkg));
	funct::Parameter meanZMuMu(kMeanZMuMu, commands.par(kMeanZMuMu));
	funct::Parameter sigmaZMuMu(kSigmaZMuMu, commands.par(kSigmaZMuMu));
	funct::Parameter alpha(kAlpha, commands.par(kAlpha));
	funct::Parameter a0(kA0, commands.par(kA0));
	funct::Parameter a1(kA1, commands.par(kA1));
	funct::Parameter a2(kA2, commands.par(kA2));
	funct::Constant cFMin(fMin), cFMax(fMax);

	IntegratorConv integratorConv(1.e-4);
	IntegratorNorm integratorNorm(1.e-4);

	ZPeak zPeak = funct::Exponential(lambdaZMuMu) *
	  funct::conv(funct::ZLineShape(mass, gamma, photonFactorZMuMu, interferenceFactorZMuMu),
		      funct::Gaussian(meanZMuMu, sigmaZMuMu),
		      -3*sigmaZMuMu.value(), 3*sigmaZMuMu.value(), integratorConv);
	SigPeak sp = funct::master(zPeak);
	SigPeakClone spc = funct::slave(sp);
	Sig1 sig1 = yieldZMuMu1 * sp;
	Sig2 sig2 = yieldZMuMu2 * spc;
	funct::Numerical<1> _1;
	funct::Numerical<2> _2;
	ExpPoly ep = funct::Exponential(alpha) * funct::Polynomial<2>(a0, a1, a2);
	ExpPolyNorm epn = ep / ExpPolyNormFactor(ep, cFMin, cFMax, integratorNorm);
	Coeff1 c1 = yieldBkg * (_2 / effBkg - _2);
	Bkg1 bkg1 = c1 * epn;
	Coeff2 c2 = yieldBkg * ((_1 /effBkg - _1) ^ _2);
	Bkg2 bkg2 = c2 * epn;
	Fun1 f1 = sig1 + bkg1;
	Fun2 f2 = sig2 + bkg2;

	ChiSquared chi2(f1, histo1, f2, histo2, fMin, fMax);
	cout << "N. deg. of freedom: " << chi2.degreesOfFreedom() << endl;
	fit::RootMinuit<ChiSquared> minuit(chi2, true);
	commands.add(minuit, yieldZMuMu1);
	commands.add(minuit, yieldZMuMu2);
	commands.add(minuit, yieldBkg);
	commands.add(minuit, effBkg);
	commands.add(minuit, lambdaZMuMu);
	commands.add(minuit, mass);
	commands.add(minuit, gamma);
	commands.add(minuit, photonFactorZMuMu);
	commands.add(minuit, interferenceFactorZMuMu);
	commands.add(minuit, meanZMuMu);
	commands.add(minuit, sigmaZMuMu);
	commands.add(minuit, alpha);
	commands.add(minuit, a0);
	commands.add(minuit, a1);
	commands.add(minuit, a2);
	commands.run(minuit);
	const unsigned int nPar = 15;//WARNIG: this must be updated manually for now
	ROOT::Math::SMatrix<double, nPar, nPar, ROOT::Math::MatRepSym<double, nPar> > err;
	minuit.getErrorMatrix(err);
	std::cout << "error matrix:" << std::endl;
	for(unsigned int i = 0; i < nPar; ++i) {
	  for(unsigned int j = 0; j < nPar; ++j) {
	    std::cout << err(i, j) << "\t";
	  }
	  std::cout << std::endl;
	}
	minuit.printFitResults();

	funct::GaussIntegrator integrator(1.e-6);
	double nbkg1 = funct::integral_f(bkg1, fMin, fMax, integrator);
	double nbkg2 = funct::integral_f(bkg2, fMin, fMax, integrator);
	std::cout << "Background yields in [" << fMin <<", " << fMax << "]: "
		  << nbkg1 <<", " <<nbkg2 << std::endl;

	// binning is at 1 GeV, fortunately
	double i1 = histo1->Integral(int(fMin), int(fMax));
	double i2 = histo2->Integral(int(fMin), int(fMax));
	std::cout << "Histogram integrals in [" << fMin <<", " << fMax << "]: "
		  << i1 <<", " << i2 << std::endl;
	double s;
	s = 0;
	for(int i = 1; i <= histo1->GetNbinsX(); ++i)
	  s += histo1->GetBinContent(i);
	histo1->SetEntries(s);
	s = 0;
	for(int i = 1; i <= histo2->GetNbinsX(); ++i)
	  s += histo2->GetBinContent(i);
	histo2->SetEntries(s);

	double extrap = nbkg1*nbkg1 / nbkg2 /4;
	cout << "extrapolated background with no isolated muons:" << extrap << endl;

	string Plot1 = "OneIsolated_" + plot_string;
	root::plot<Fun1>(Plot1.c_str(), *histo1, f1, fMin, fMax,
			 yieldZMuMu1, lambdaZMuMu, mass, gamma, photonFactorZMuMu, interferenceFactorZMuMu,
			 meanZMuMu, sigmaZMuMu, yieldBkg, effBkg, alpha, a0, a1, a2,
			 kRed, 2, kDashed, 100,
			 "Z -> #mu #mu mass", "#mu #mu invariant mass (GeV/c^{2})",
			 "Events");
	string Plot2 = "TwoIsolated_" + plot_string;
	root::plot<Fun2>(Plot2.c_str(), *histo2, f2, fMin, fMax,
			 yieldZMuMu2, lambdaZMuMu, mass, gamma, photonFactorZMuMu, interferenceFactorZMuMu,
			 meanZMuMu, sigmaZMuMu, yieldBkg, effBkg, alpha, a0, a1, a2,
			 kRed, 2, kDashed, 100,
			 "Z -> #mu #mu mass", "#mu #mu invariant mass (GeV/c^{2})",
			 "Events");
      }
    }

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


#include <cmath>
#include <vector>
#include <string>
#include "multi_lidar_calib/common.h"
#include "multi_lidar_calib/tic_toc.h"
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/obj_io.h>
#include <pcl/PolygonMesh.h>
#include <pcl/point_cloud.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/sac_model_sphere.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/sample_consensus/sac_model_perpendicular_plane.h>
#include <pcl/filters/passthrough.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
 #include <ceres/ceres.h>
 #include "lidarFactor.hpp"
#include <pcl/features/vfh.h>
#include <pcl/features/normal_3d.h>
#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/console/parse.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <sstream>
#include<vector>

#include<mutex>

#define PI 3.1415926

using std::atan2;
using std::cos;
using std::sin;

double lidar_z_height = -1;

ros::Publisher  pubFittedPlane;

double parameters[7] = {0, 0, 0, 1, 0, 0, 0}; // 激光雷达间相对位姿关系

Eigen::Map<Eigen::Quaterniond> q_to_be_optimized(parameters);
Eigen::Map<Eigen::Vector3d> t_to_be_optimized(parameters + 4);

std::string extracted_path = " ";
int pcd_num_per_lidar = 0;
void planeFitting(pcl::PointCloud<pcl::PointXYZ>::Ptr extracted_cloud, pcl::ModelCoefficients::Ptr & coefficients)
{

    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    seg.setOptimizeCoefficients (true);
    seg.setModelType (pcl::SACMODEL_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setDistanceThreshold (0.01);
     seg.setInputCloud(extracted_cloud);
    seg.segment (*inliers, *coefficients);

    // // ax + by + cz + d = 0，其中法向量为(a, b, c)
    // std::cout<<"平面参数："<<std::endl;
    // std::cout<<"a："<<coefficients->values[0]<<std::endl;
    // std::cout<<"b："<<coefficients->values[1]<<std::endl;
    // std::cout<<"c："<<coefficients->values[2]<<std::endl;
    // std::cout<<"d："<<coefficients->values[3]<<std::endl;
    double a,b,c,d;
    a =   coefficients->values[0];
    b =  coefficients->values[1];
    c =  coefficients->values[2];
    d =  coefficients->values[3];
    // double numerator = fabs(  a*x_c+b*y_c+c*z_c+d  );
    // double denominator = std::sqrt(  a*a+b*b+c*c );
    // double distanceToArea = numerator/ denominator;
    pcl::PointCloud<pcl::PointXYZ>::Ptr fittedPlaneCloud(new pcl::PointCloud<pcl::PointXYZ>);
    for (int i=0; i<inliers->indices.size();i++)
    {
        int ind = inliers->indices[i];
         fittedPlaneCloud->points.push_back(  extracted_cloud->points[ind]  );
    }

    sensor_msgs::PointCloud2 fittedPlaneCloudMsg;
    pcl::toROSMsg(*fittedPlaneCloud, fittedPlaneCloudMsg);
    fittedPlaneCloudMsg.header.stamp = ros::Time::now();
    fittedPlaneCloudMsg.header.frame_id = "/rslidar";
    pubFittedPlane.publish(fittedPlaneCloudMsg);

}

pcl::PointCloud<pcl::PointXYZ>::Ptr planeFittingReturn(pcl::PointCloud<pcl::PointXYZ>::Ptr extracted_cloud, pcl::ModelCoefficients::Ptr & coefficients)
{

    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    seg.setOptimizeCoefficients (true);
    seg.setModelType (pcl::SACMODEL_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setDistanceThreshold (0.01);
     seg.setInputCloud(extracted_cloud);
    seg.segment (*inliers, *coefficients);

    // // ax + by + cz + d = 0，其中法向量为(a, b, c)
    // std::cout<<"平面参数："<<std::endl;
    // std::cout<<"a："<<coefficients->values[0]<<std::endl;
    // std::cout<<"b："<<coefficients->values[1]<<std::endl;
    // std::cout<<"c："<<coefficients->values[2]<<std::endl;
    // std::cout<<"d："<<coefficients->values[3]<<std::endl;
    double a,b,c,d;
    a =   coefficients->values[0];
    b =  coefficients->values[1];
    c =  coefficients->values[2];
    d =  coefficients->values[3];
    // double numerator = fabs(  a*x_c+b*y_c+c*z_c+d  );
    // double denominator = std::sqrt(  a*a+b*b+c*c );
    // double distanceToArea = numerator/ denominator;
    pcl::PointCloud<pcl::PointXYZ>::Ptr fittedPlaneCloud(new pcl::PointCloud<pcl::PointXYZ>);
    for (int i=0; i<inliers->indices.size();i++)
    {
        int ind = inliers->indices[i];
         fittedPlaneCloud->points.push_back(  extracted_cloud->points[ind]  );
    }

    sensor_msgs::PointCloud2 fittedPlaneCloudMsg;
    pcl::toROSMsg(*fittedPlaneCloud, fittedPlaneCloudMsg);
    fittedPlaneCloudMsg.header.stamp = ros::Time::now();
    fittedPlaneCloudMsg.header.frame_id = "/rslidar";
    pubFittedPlane.publish(fittedPlaneCloudMsg);

    return fittedPlaneCloud;

}

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

    ros::init(argc, argv, "multi_lidar_calibration");
	ros::NodeHandle nh;

    ros::param::get("~extracted_path", extracted_path);
    ros::param::get("~pcd_num_per_lidar", pcd_num_per_lidar);

    pubFittedPlane = nh.advertise<sensor_msgs::PointCloud2>("/fitted_plane", 100);

    for (int iterCount = 0; iterCount < 10; iterCount++)
    {

            ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
            ceres::LocalParameterization *q_parameterization =
                new ceres::EigenQuaternionParameterization();
            ceres::Problem::Options problem_options;

            ceres::Problem problem(problem_options);
            problem.AddParameterBlock(parameters, 4, q_parameterization);
            problem.AddParameterBlock(parameters + 4, 3);

            for (int k=0; k<pcd_num_per_lidar; k++)
            {

                std::stringstream ss;
                std::string filename_pt0 = extracted_path;
                std::string filename_pt1 = extracted_path;
                ss << k+1;
                std::string num = ss.str();
                filename_pt0.append(num);
                filename_pt0.append("_0.pcd");
                filename_pt1.append(num);
                filename_pt1.append("_1.pcd");

                std::cout<<"Extracting "<<k<<" pointcloud from "<<  filename_pt0<<std::endl;
                std::cout<<"Extracting "<<k<<" pointcloud from "<<  filename_pt1<<std::endl;
                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud0(new pcl::PointCloud<pcl::PointXYZ>);
                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>);

                if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename_pt0, *cloud0) == -1)
                    {
                    PCL_ERROR ("Couldn't read PCD file \n");
                    }
                if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename_pt1, *cloud1) == -1)
                    {
                    PCL_ERROR ("Couldn't read PCD file \n");
                    }

                pcl::ModelCoefficients::Ptr coefficients0 (new pcl::ModelCoefficients);
                planeFitting(cloud0, coefficients0);
                pcl::ModelCoefficients::Ptr coefficients1 (new pcl::ModelCoefficients);
                planeFitting(cloud1,coefficients1);

                // 两平面法线的夹角作为一个损失值
                Eigen::Vector3d norm0(coefficients0->values[0], coefficients0->values[1], coefficients0->values[2]);
                Eigen::Vector3d norm1(coefficients1->values[0], coefficients1->values[1], coefficients1->values[2]);
                ceres::CostFunction *cost_function;
                cost_function = LidarNormFactor::Create(norm0, norm1);
                problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);

                TicToc t_solver;
                ceres::Solver::Options options;
                options.linear_solver_type = ceres::DENSE_QR;
                options.max_num_iterations = 10;
                options.minimizer_progress_to_stdout = false;
                options.check_gradients = false;
                options.gradient_check_relative_precision = 1e-4;
                ceres::Solver::Summary summary;
                ceres::Solve(options, &problem, &summary);

                sleep(0.2);

            }

    }
    printf("result q %f %f %f %f result t %f %f %f\n", parameters[3], parameters[0], parameters[1], parameters[2],parameters[4], parameters[5], parameters[6]);
  // ====================================================== //=========================================================
  // 接下来标平移：（之所以分开标定是因为如果平移不准，起码保证旋转是准的，平移不会影响旋转。实验发现，这样标定的话平移也挺准）
    for (int iterCount = 0; iterCount < 10; iterCount++)
    {

            ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
            ceres::Problem::Options problem_options;
            ceres::Problem problem(problem_options);
            problem.AddParameterBlock(parameters + 4, 3);

            for (int k=0; k<pcd_num_per_lidar; k++)
            {

                std::stringstream ss;
                std::string filename_pt0 = extracted_path;
                std::string filename_pt1 = extracted_path;
                ss << k+1;
                std::string num = ss.str();
                filename_pt0.append(num);
                filename_pt0.append("_0.pcd");
                filename_pt1.append(num);
                filename_pt1.append("_1.pcd");

                std::cout<<"Extracting "<<k<<" pointcloud from "<<  filename_pt0<<std::endl;
                std::cout<<"Extracting "<<k<<" pointcloud from "<<  filename_pt1<<std::endl;
                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud0(new pcl::PointCloud<pcl::PointXYZ>);
                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>);
                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud0_plane(new pcl::PointCloud<pcl::PointXYZ>);
                pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1_plane(new pcl::PointCloud<pcl::PointXYZ>);

                if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename_pt0, *cloud0) == -1)
                    {
                    PCL_ERROR ("Couldn't read PCD file \n");
                    }
                if (pcl::io::loadPCDFile<pcl::PointXYZ> (filename_pt1, *cloud1) == -1)
                    {
                    PCL_ERROR ("Couldn't read PCD file \n");
                    }

                pcl::ModelCoefficients::Ptr coefficients0 (new pcl::ModelCoefficients);
                cloud0_plane = planeFittingReturn(cloud0, coefficients0);
                pcl::ModelCoefficients::Ptr coefficients1 (new pcl::ModelCoefficients);
                cloud1_plane = planeFittingReturn(cloud1,coefficients1);

                // 一共有三维损失值，分别代表：。。。。。。先加一维
                // 0：质心距离
                // 计算质心
                double center_x0 = 0;
                double center_y0 = 0;
                double center_z0 = 0;
                double center_x1 = 0;
                double center_y1 = 0;
                double center_z1 = 0;
                for (int i=0; i<cloud0_plane->points.size(); i++)
                {
                    center_x0 += cloud0_plane->points[i].x;
                    center_y0 += cloud0_plane->points[i].y;
                    center_z0 += cloud0_plane->points[i].z;
                }
                center_x0 /= cloud0_plane->points.size();
                center_y0 /= cloud0_plane->points.size();
                center_z0 /= cloud0_plane->points.size();

                for (int i=0; i<cloud1_plane->points.size(); i++)
                {
                    // 先将点用旋转转过去：
                    Eigen::Vector3d tmppoint(cloud1_plane->points[i].x,  cloud1_plane->points[i].y, cloud1_plane->points[i].z);
                    Eigen::Vector3d tmppoint_roted =  q_to_be_optimized.matrix() * tmppoint;
                    center_x1 += tmppoint_roted.x();
                    center_y1 +=tmppoint_roted.y();
                    center_z1 += tmppoint_roted.z();
                }
                center_x1 /= cloud1_plane->points.size();
                center_y1 /= cloud1_plane->points.size();
                center_z1 /= cloud1_plane->points.size();

                Eigen::Vector3d plane_center0(center_x0, center_y0, center_z0) ;
                Eigen::Vector3d plane_center1(center_x1, center_y1, center_z1) ;

                ceres::CostFunction *cost_function;
                cost_function = LidarCenterFactor::Create(plane_center0, plane_center1);
                problem.AddResidualBlock(cost_function, loss_function, parameters + 4);

                TicToc t_solver;
                ceres::Solver::Options options;
                options.linear_solver_type = ceres::DENSE_QR;
                options.max_num_iterations = 10;
                options.minimizer_progress_to_stdout = false;
                options.check_gradients = false;
                options.gradient_check_relative_precision = 1e-4;
                ceres::Solver::Summary summary;
                ceres::Solve(options, &problem, &summary);

                sleep(0.2);

            }

    }

    std::cout<<"======================最终标定结果======================"<<std::endl;
    printf("result q %f %f %f %f result t %f %f %f\n", parameters[3], parameters[0], parameters[1], parameters[2],parameters[4], parameters[5], parameters[6]);
    std::cout<<q_to_be_optimized.matrix()<<std::endl;

    return 0;
}


#include <iostream>
#include <vector>
#include <set>
#include <iterator>
#include <cmath>
#include <string>
#include <chrono>
#include <exception>
#include <queue>
#include <stack>
#include <bitset>
#include <random>
#include <set>
#include <boost/math/distributions/normal.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graphviz.hpp>
#include <sys/time.h>
#include "DataTypes.hpp"
#include "ATG.hpp"

std::ostream& operator<<(std::ostream &os, const NodeType  &et) {
  std::string state;
  switch(et.status) {
      case NotReady : state = "NotReady"; break;
      case Ready    : state = "Ready"; break;
      case Finished : state = "Finished"; break;
      case Active   : state = "Active"; break;
      default  : state = "None"; break;
  }
  os << "Task{id:"<<et.id<<",start:" << et.start_time << ",C:" <<  et.C << ",stop:"<<et.stop_time<<",alloc:"<<et.alloc<<",status:"<<state<<"}";
  return os;
}

/* Execution time characteristics of jth MT-task */
double AppTaskGraph::tau_j(int jid, int l) {
  NodeType *nd = &(this->gSS[this->vdMap[jid]]);
  double dj    = nd->dj;
  double tau1  = nd->tau1;
  double ret   = (tau1)*(pow(l,-dj));
  return ret;
}

/* Generate a random task graph */
AppTaskGraph::AppTaskGraph(int N, double p) : vdMap(N) {
    if (p >= 1 && p < 0) {
        throw std::invalid_argument("Wrong value of parameter p");
    }
    // initExecTimeChar(int N)
    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_real_distribution<> dis2(0.01, 0.99);
    std::uniform_real_distribution<> dis1(80,400);
    std::bernoulli_distribution d(p);

    int i, j, cnt = 0;

    this->wC = 0.0;
    /* Create the nodes */
    /*
     * -1 reflects unintialized values
     */
    for (i = 0; i < N; i++) {
        NodeType vi2       = {.status = NotReady, \
                              .estart = -1, \
                              .dj = dis2(gen), .tau1 = dis1(gen), .id = i, \
                              .alloc = -1, .start_time = -1, .stop_time = -1, .C = -1};
        this->vdMap[i]     = add_vertex(vi2,this->gSS);

        this->wC += this->tau_j(vi2.id,1);
    }
    this->deadline = 0.4*(this->wC);

    /* Connect the nodes */
    for (i = 0; i < N; i++) {
        for (j = i+1; j < N; j++) {
            if (d(gen)) {
                EdgeType edg = {.edgeNum = cnt++};
                add_edge(this->vdMap[i],this->vdMap[j],edg,this->gSS);
            }
        }
    }

    /* Initialize the ready set and schedule set to null */
    this->t = 0;
    this->computeNbdMap();
    this->initialized = true;
}

AppTaskGraph::AppTaskGraph() : vdMap(6) {
    /* Create a DAG as shown in buggy.dot */
    int N = 6;
    int i;

    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_real_distribution<> dis2(0.01, 0.99);
    std::uniform_real_distribution<> dis1(80,400);
    // std::bernoulli_distribution d(p);

    for (i = 0; i < N; i++) {
        NodeType vi2       = {.status = NotReady, \
                              .estart = -1, \
                              .dj = dis2(gen), .tau1 = dis1(gen), .id = i, \
                              .alloc = -1, .start_time = -1, .stop_time = -1, .C = -1};
        this->vdMap[i]     = add_vertex(vi2,this->gSS);
    }

    EdgeType edg = {.edgeNum = 8};
    add_edge(this->vdMap[0],this->vdMap[2],edg,this->gSS);
    add_edge(this->vdMap[0],this->vdMap[4],edg,this->gSS);
    add_edge(this->vdMap[0],this->vdMap[5],edg,this->gSS);
    add_edge(this->vdMap[1],this->vdMap[2],edg,this->gSS);
    add_edge(this->vdMap[1],this->vdMap[4],edg,this->gSS);
    add_edge(this->vdMap[1],this->vdMap[5],edg,this->gSS);
    add_edge(this->vdMap[2],this->vdMap[3],edg,this->gSS);
    add_edge(this->vdMap[2],this->vdMap[5],edg,this->gSS);
    add_edge(this->vdMap[3],this->vdMap[5],edg,this->gSS);
    add_edge(this->vdMap[4],this->vdMap[5],edg,this->gSS);

}

void AppTaskGraph::writeDot(int iter) {
    std::cout << "\n-- graphviz output START --" << std::endl;
    std::ofstream dotf;
    std::string dotFilename = "demo"+std::to_string(iter)+".dot";
    dotf.open(dotFilename,std::ios::out | std::ios::trunc);
    write_graphviz(dotf,this->gSS,DAGVertexPW(this->gSS),DAGEdgePW(this->gSS));
    dotf.close();
    std::cout << "\n-- graphviz output END --" << std::endl;
}

/* Compute the longest path to each node in DAG from source node */
void AppTaskGraph::computeHeight(bool schedule) {
    DAGInEdgeIterator  eiIS, eiIE;
    int N = this->vdMap.size();
    for (int i = 0; i < N; i++) {
        NodeType *nd = &(this->gSS[this->vdMap[i]]);
        nd->estart   = 0;

        int max = 0;
        for (boost::tie(eiIS,eiIE) = boost::in_edges(this->vdMap[i],this->gSS); eiIS != eiIE; eiIS++) {
            auto ndPrevId    = boost::source(*eiIS, this->gSS);
            NodeType *ndPrev = &(this->gSS[ndPrevId]);
            if (max < (ndPrev->estart + ndPrev->C))
                max = ndPrev->estart + ndPrev->C;
        }
        nd->estart = max;

        if (schedule) {
            nd->start_time = nd->estart;
            nd->stop_time  = nd->start_time + nd->C;
        }
    }
}

void AppTaskGraph::reset() {
    for (auto it = this->vdMap.begin(); it != this->vdMap.end(); it++) {
        NodeType *nd = &(this->gSS[*it]);

        nd->status = NotReady;
        nd->estart = -1;
        nd->start_time = -1;
        nd->stop_time = -1;
        nd->alloc  = -1;
    }
}

void AppTaskGraph::computeNbdMap() {
    int N = this->vdMap.size();
    DAGInEdgeIterator  eiIS, eiIE;
    DAGOutEdgeIterator  eiOS, eiOE;

    for (int i = 0; i < N; i++) {
        /* Insert predecessors */
        std::vector<DAGVertexType> preds;
        std::vector<DAGVertexType> succs;

        for (boost::tie(eiIS,eiIE) = boost::in_edges(this->vdMap[i],this->gSS); eiIS != eiIE; eiIS++) {
            auto ndPrevId    = boost::source(*eiIS, this->gSS);
            preds.push_back(ndPrevId);
        }

        /* Insert successors */
        for (boost::tie(eiOS,eiOE) = boost::out_edges(this->vdMap[i],this->gSS); eiOS != eiOE; eiOS++) {
            auto ndSuccId    = boost::target(*eiOS, this->gSS);
            succs.push_back(ndSuccId);
        }

        /* */
        this->predVdMap.insert({this->vdMap[i],preds});
        this->succVdMap.insert({this->vdMap[i],succs});
    }
}

/* Construct an ATG from a dot file */
AppTaskGraph::AppTaskGraph(std::string dotFile) {
    boost::dynamic_properties dp;
    std::ifstream dotF(dotFile);

    dp.property("dj", boost::get(&NodeType::dj,this->gSS));
    dp.property("tau1", boost::get(&NodeType::tau1,this->gSS));
    dp.property("node_id", boost::get(&NodeType::id,this->gSS));

    read_graphviz(dotF,this->gSS,dp);
}


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */

#include "FockDiis.h"
#include <Eigen/QR>
#include <algorithm>

namespace Scine {
namespace Utils {

FockDiis::FockDiis() {
  setSubspaceSize(5);
}

void FockDiis::setUnrestricted(bool b) {
  unrestricted_ = b;
  diisError_.setUnrestricted(b);
  restart();
}

void FockDiis::setSubspaceSize(int n) {
  bool resizeNeeded = n != subspaceSize_;

  subspaceSize_ = n;

  if (resizeNeeded) {
    resizeMembers();
  }
}

void FockDiis::setNAOs(int n) {
  bool resizeNeeded = n != nAOs_;

  nAOs_ = n;

  if (resizeNeeded) {
    resizeMembers();
  }
}

void FockDiis::resizeMembers() {
  fockMatrices.resize(subspaceSize_);
  diisError_.resize(subspaceSize_);
  diisStepErrors_.resize(subspaceSize_);

  overlap = Eigen::MatrixXd::Zero(nAOs_, nAOs_);

  B = Eigen::MatrixXd::Ones(subspaceSize_ + 1, subspaceSize_ + 1) * (-1);
  B(0, 0) = 0;

  rhs = Eigen::VectorXd::Zero(subspaceSize_ + 1);
  rhs(0) = -1;

  restart();
}

void FockDiis::setOverlapMatrix(const Eigen::MatrixXd& S) {
  overlap = S.selfadjointView<Eigen::Lower>();
  restart();
}

void FockDiis::restart() {
  C = Eigen::VectorXd::Zero(subspaceSize_ + 1);
  iterationNo_ = 0;
  index_ = 0;
}

void FockDiis::addMatrices(const SpinAdaptedMatrix& F, const DensityMatrix& P) {
  iterationNo_++;
  lastAdded_ = index_;

  fockMatrices[index_] = F;

  diisError_.setErrorFromMatrices(index_, F, P, overlap);
  diisStepErrors_[index_] = std::sqrt(diisError_.getError(index_, index_)) / nAOs_;

  updateBMatrix();

  index_ = (index_ + 1) % subspaceSize_;
}

void FockDiis::updateBMatrix() {
  int activeSize = iterationNo_ > subspaceSize_ ? subspaceSize_ : iterationNo_;

  // Bii element
  B(lastAdded_ + 1, lastAdded_ + 1) = diisError_.getError(lastAdded_, lastAdded_);

  // Bij elements
  for (int i = 1; i < activeSize + 1; i++) {
    if (i == lastAdded_ + 1) {
      continue;
    }
    B(lastAdded_ + 1, i) = diisError_.getError(lastAdded_, i - 1);
    B(i, lastAdded_ + 1) = B(lastAdded_ + 1, i);
  }
}

SpinAdaptedMatrix FockDiis::getMixedFockMatrix() {
  if (iterationNo_ > subspaceSize_) {
    iterationNo_ = subspaceSize_;
  }

  // If we have only one Fock matrix
  if (iterationNo_ < 2) {
    return fockMatrices[0];
  }

  C.head(iterationNo_ + 1) =
      B.block(0, 0, iterationNo_ + 1, iterationNo_ + 1).colPivHouseholderQr().solve(rhs.head(iterationNo_ + 1));

  return calculateLinearCombination();
}

SpinAdaptedMatrix FockDiis::calculateLinearCombination() {
  if (unrestricted_) {
    Eigen::MatrixXd FAlpha = Eigen::MatrixXd::Zero(nAOs_, nAOs_);
    Eigen::MatrixXd FBeta = Eigen::MatrixXd::Zero(nAOs_, nAOs_);
    for (int i = 0; i < iterationNo_; i++) {
      FAlpha += C(i + 1) * fockMatrices[i].alphaMatrix();
      FBeta += C(i + 1) * fockMatrices[i].betaMatrix();
    }
    return SpinAdaptedMatrix::createUnrestricted(std::move(FAlpha), std::move(FBeta));
  }
  Eigen::MatrixXd Fsol = Eigen::MatrixXd::Zero(nAOs_, nAOs_);
  for (int i = 0; i < iterationNo_; i++) {
    Fsol += C(i + 1) * fockMatrices[i].restrictedMatrix();
  }
  return SpinAdaptedMatrix::createRestricted(std::move(Fsol));
}

double FockDiis::getMaxError() const {
  int activeSize = iterationNo_ > subspaceSize_ ? subspaceSize_ : iterationNo_;
  auto maxIter = std::max_element(diisStepErrors_.begin(), diisStepErrors_.begin() + activeSize);
  return *maxIter;
}

double FockDiis::getMinError() const {
  int activeSize = iterationNo_ > subspaceSize_ ? subspaceSize_ : iterationNo_;
  auto minIter = std::min_element(diisStepErrors_.begin(), diisStepErrors_.begin() + activeSize);
  return *minIter;
}

double FockDiis::getLastError() const {
  return diisStepErrors_[lastAdded_];
}
} // namespace Utils
} // namespace Scine


// Boost.Geometry

// Copyright (c) 2015 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// 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_ANDOYER_INVERSE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_ANDOYER_INVERSE_HPP

#include <boost/math/constants/constants.hpp>

#include <boost/geometry/core/radius.hpp>
#include <boost/geometry/core/srs.hpp>

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

#include <boost/geometry/algorithms/detail/flattening.hpp>
#include <boost/geometry/algorithms/detail/result_inverse.hpp>

namespace boost { namespace geometry { namespace detail
{

/*!
\brief The solution of the inverse problem of geodesics on latlong coordinates,
       Forsyth-Andoyer-Lambert type approximation with first order terms.
\author See
    - Technical Report: PAUL D. THOMAS, MATHEMATICAL MODELS FOR NAVIGATION SYSTEMS, 1965
      http://www.dtic.mil/docs/citations/AD0627893
    - Technical Report: PAUL D. THOMAS, SPHEROIDAL GEODESICS, REFERENCE SYSTEMS, AND LOCAL GEOMETRY, 1970
      http://www.dtic.mil/docs/citations/AD703541
*/

template <typename CT, bool EnableDistance, bool EnableAzimuth>
struct andoyer_inverse
{
    typedef result_inverse<CT> result_type;

    template <typename T1, typename T2, typename Spheroid>
    static inline result_type apply(T1 const& lon1,
                                    T1 const& lat1,
                                    T2 const& lon2,
                                    T2 const& lat2,
                                    Spheroid const& spheroid)
    {
        result_type result;

        // coordinates in radians

        if ( math::equals(lon1, lon2)
          && math::equals(lat1, lat2) )
        {
            result.set(CT(0), CT(0));
            return result;
        }

        CT const pi_half = math::pi<CT>() / CT(2);

        if ( math::equals(math::abs(lat1), pi_half)
          && math::equals(math::abs(lat2), pi_half) )
        {
            result.set(CT(0), CT(0));
            return result;
        }

        CT const dlon = lon2 - lon1;
        CT const sin_dlon = sin(dlon);
        CT const cos_dlon = cos(dlon);
        CT const sin_lat1 = sin(lat1);
        CT const cos_lat1 = cos(lat1);
        CT const sin_lat2 = sin(lat2);
        CT const cos_lat2 = cos(lat2);

        // H,G,T = infinity if cos_d = 1 or cos_d = -1
        // lat1 == +-90 && lat2 == +-90
        // lat1 == lat2 && lon1 == lon2
        CT const cos_d = sin_lat1*sin_lat2 + cos_lat1*cos_lat2*cos_dlon;
        CT const d = acos(cos_d);
        CT const sin_d = sin(d);

        // just in case since above lat1 and lat2 is checked
        // the check below is equal to cos_d == 1 || cos_d == -1 || d == 0
        if ( math::equals(sin_d, CT(0)) )
        {
            result.set(CT(0), CT(0));
            return result;
        }

        // if the function returned before this place
        // and endpoints were on the poles +-90 deg
        // in this case the azimuth could either be 0 or +-pi

        CT const f = detail::flattening<CT>(spheroid);

        if ( BOOST_GEOMETRY_CONDITION(EnableDistance) )
        {
            CT const K = math::sqr(sin_lat1-sin_lat2);
            CT const L = math::sqr(sin_lat1+sin_lat2);
            CT const three_sin_d = CT(3) * sin_d;
            // H or G = infinity if cos_d = 1 or cos_d = -1
            CT const H = (d+three_sin_d)/(CT(1)-cos_d);
            CT const G = (d-three_sin_d)/(CT(1)+cos_d);

            // for e.g. lat1=-90 && lat2=90 here we have G*L=INF*0
            CT const dd = -(f/CT(4))*(H*K+G*L);

            CT const a = get_radius<0>(spheroid);

            result.distance = a * (d + dd);
        }
        else
        {
            result.distance = CT(0);
        }

        if ( BOOST_GEOMETRY_CONDITION(EnableAzimuth) )
        {
            CT A = CT(0);
            CT U = CT(0);
            if ( ! math::equals(cos_lat2, CT(0)) )
            {
                CT const tan_lat2 = sin_lat2/cos_lat2;
                CT const M = cos_lat1*tan_lat2-sin_lat1*cos_dlon;
                A = atan2(sin_dlon, M);
                CT const sin_2A = sin(CT(2)*A);
                U = (f/CT(2))*math::sqr(cos_lat1)*sin_2A;
            }

            CT V = CT(0);
            if ( ! math::equals(cos_lat1, CT(0)) )
            {
                CT const tan_lat1 = sin_lat1/cos_lat1;
                CT const N = cos_lat2*tan_lat1-sin_lat2*cos_dlon;
                CT const B = atan2(sin_dlon, N);
                CT const sin_2B = sin(CT(2)*B);
                V = (f/CT(2))*math::sqr(cos_lat2)*sin_2B;
            }

            // infinity if sin_d = 0, so cos_d = 1 or cos_d = -1
            CT const T = d / sin_d;
            CT const dA = V*T-U;

            result.azimuth = A - dA;
        }
        else
        {
            result.azimuth = CT(0);
        }

        return result;
    }
};

}}} // namespace boost::geometry::detail

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_ANDOYER_INVERSE_HPP


#include <iostream>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <boost/random.hpp>
#include <ctime>
#include <chrono>

#include <kv/matrix-inversion.hpp>

int main()
{
	boost::numeric::ublas::matrix<double> a(2, 2);
	boost::numeric::ublas::matrix<double> b;

	std::chrono::system_clock::time_point t;

	a(0, 0) = 1.; a(0, 1) = 2.;
	a(1, 0) = 3.; a(1, 1) = 4.;

	kv::invert(a, b);

	std::cout << b << "\n";

	int i, j, n;

	n = 2000;

	boost::numeric::ublas::matrix<double> c(n, n);
	using namespace boost;
	variate_generator< mt19937, uniform_real<double> > rand (mt19937(time(0)), uniform_real<double>(-1., 1.));

	for (i=0; i<n; i++) {
		for (j=0; j<n; j++) {
			c(i, j) = rand();
		}
	}

	t = std::chrono::system_clock::now();
	kv::invert(c, b);
	std::cout << std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - t).count() / 1e9 << " sec\n";

	boost::numeric::ublas::vector<double> d(n);

	t = std::chrono::system_clock::now();
	kv::linear_equation(c, d, d);
	std::cout << std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - t).count() / 1e9 << " sec\n";

	t = std::chrono::system_clock::now();
	kv::mm_mult(c, b, b);
	std::cout << std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - t).count() / 1e9 << " sec\n";
}


#include "get_nn.h"
#include "geometry.h"

#include <cmath>
#include <algorithm>
#include <iostream>
#include <unordered_map>
#include <Eigen/Geometry>
using namespace std;
using namespace Eigen;

namespace marvel {

spatial_hash::spatial_hash(const MatrixXd& points_, const size_t& nn_num_)
    : points(points_), points_num(points_.cols()), nn_num(nn_num_)
{
    //set hash parameter
    size_t table_size = size_t(floor(pow(points.cols(), 0.5)));
    //build hash_map
    points_hash = unordered_multimap<Vector3i, size_t>(table_size);
    hash_NNN();
}

int spatial_hash::get_shell(const Eigen::Vector3i& query, const int& radi, std::vector<Vector3i>& shell) const
{
    assert(radi > -1);
    //init
    shell.clear();
    int res = 0;

    auto loop = [&](const int& face, const int& face_axis, const int& radi_1, const int& radi_2) {
        if (face <= max_id(face_axis) && face >= min_id(face_axis))
        {
            int axis_1 = (face_axis + 1) % 3, axis_2 = (face_axis + 2) % 3;
            for (int j = query(axis_1) - radi_1; j < query(axis_1) + radi_1 + 1; ++j)
            {
                for (int k = query(axis_2) - radi_2; k < query(axis_2) + radi_2 + 1; ++k)
                {
                    Vector3i one_grid;
                    one_grid[face_axis] = face;
                    one_grid[axis_1]    = j;
                    one_grid[axis_2]    = k;
                    shell.push_back(one_grid);
                }
            }
            return 0;
        }
        else
        {
            return 1;
        }
    };
    if (radi > 0)
    {
        int touch_time = 0;
        for (size_t i = 0; i < 6; ++i)
        {

            touch_time += loop(i % 2 == 0 ? query(i / 2) - radi : query(i / 2) + radi, i / 2, i < 4 ? radi : radi - 1, i < 2 ? radi : radi - 1);
        }
        if (touch_time == 6)
            res = -1;
    }
    else
    {
        shell.push_back(query);
    }
    return res;
}

int spatial_hash::find_NN(const size_t& point_id, vector<pair_dis>& NN_cand)
{
    return std::move(find_NN(point_id, NN_cand, nn_num));
}

int spatial_hash::find_NN(const size_t& point_id, vector<pair_dis>& NN_cand, const size_t& nn_num_)
{

    size_t cand_num  = 0;
    int    grid_delt = 0;
    // bool once_more = false;
    int once_more = 0;
    int touch_bd  = 0;
    //count for grid_delt;
    do
    {
        vector<Vector3i> shell;
        touch_bd = get_shell(points_dis.col(point_id), grid_delt, shell);
        for (auto& grid : shell)
        {
            auto range = points_hash.equal_range(grid);
            if (range.first != range.second)
            {
                for_each(range.first, range.second, [&](decltype(points_hash)::value_type& one_point) {
                    double dis = (points.col(point_id) - points.col(one_point.second)).norm();
                    NN_cand.push_back({ one_point.second, dis });
                });
            }
        }
        ++grid_delt;
        if (NN_cand.size() > nn_num_ + 2)
            // once_more = !once_more;
            once_more++;
        if (touch_bd)
            break;

    } while (NN_cand.size() < nn_num_ + 2 || once_more < 2);
    return 0;
}

int spatial_hash::hash_NNN()
{
    //init
    NN.setZero(nn_num, points_num);
    points_hash.clear();
    //set parameter

    double   cell_num = pow(points.cols() / float(nn_num), 1.0 / 3);
    MatrixXd bdbox(3, 2);
    build_bdbox(points, bdbox);
    cell_size = (bdbox.col(1) - bdbox.col(0)) / cell_num;
    //generate discretized 3D position
    points_dis = MatrixXi(3, points_num);
    for (size_t i = 0; i < points.rows(); ++i)
    {
        points_dis.row(i) = floor(points.row(i).array() / cell_size(i)).cast<int>();
    }

    max_id = { points_dis.row(0).maxCoeff(), points_dis.row(1).maxCoeff(), points_dis.row(2).maxCoeff() };
    min_id = { points_dis.row(0).minCoeff(), points_dis.row(1).minCoeff(), points_dis.row(2).minCoeff() };

    //insert elements

    for (size_t i = 0; i < points_num; ++i)
    {
        points_hash.insert({ points_dis.col(i), i });
    }

    return 0;
}

const MatrixXi& spatial_hash::get_NN(const size_t& nn_num_)
{
    auto useless_sup_radi = get_sup_radi(nn_num_);

    return NN;
}
const MatrixXi& spatial_hash::get_NN()
{
    return std::move(get_NN(nn_num));
}

const VectorXi spatial_hash::get_NN(const Vector3d& query, const size_t& nn_num_)
{
    //init data
    assert(nn_num_ < points_num);
    VectorXi         near_nei = VectorXi::Zero(nn_num_);
    vector<pair_dis> NN_cand;

    size_t         cand_num    = 0;
    int            grid_delt   = 0;
    int            once_more   = 0;
    const Vector3i center_grid = floor(query.array() / cell_size.array()).cast<int>();
    do
    {
        vector<Vector3i> shell;
        get_shell(center_grid, grid_delt, shell);
        for (auto& grid : shell)
        {
            auto range = points_hash.equal_range(grid);
            if (range.first != range.second)
            {
                for_each(range.first, range.second, [&](decltype(points_hash)::value_type& one_point) {
                    double dis = (query - points.col(one_point.second)).norm();
                    NN_cand.push_back({ one_point.second, dis });
                });
            }
        }
        ++grid_delt;
        if (NN_cand.size() > nn_num_ + 2)
            once_more++;
    } while (NN_cand.size() < nn_num_ + 2 || once_more < 2);

    sort(NN_cand.begin(), NN_cand.end(), [](const pair_dis& a, const pair_dis& b) { return a.dis < b.dis; });

    for (size_t j = 0; j < nn_num_; ++j)
    {
        near_nei(j) = NN_cand[j].n;
    }
    return near_nei;
}

const Eigen::MatrixXi spatial_hash::get_four_noncoplanar_NN(const Eigen::MatrixXd& nods)
{
    MatrixXi near_nei = MatrixXi::Zero(4, nods.cols());
    // #pragma omp parallel for
    for (size_t i = 0; i < nods.cols(); ++i)
    {

        auto     ver_nn_num = 4;
        Vector2i two_index;
        two_index << 2, 3;
        bool is_co_line = true, is_co_plane = true;
        do
        {
            auto     NN_index = get_NN(nods.col(i), ver_nn_num);
            Vector3d V1       = (points.col(NN_index(1)) - points.col(NN_index(0))).normalized();
            Vector3d V2       = (points.col(NN_index(two_index(0))) - points.col(NN_index(0))).normalized();
            Vector3d cross_   = V1.cross(V2);
            if (cross_.squaredNorm() < 1e-5)
            {
                two_index(0)++;
                two_index(1)++;
                ver_nn_num++;
                is_co_line = true;
            }
            else
            {
                is_co_line = false;

                do
                {
                    Vector3d V3 = (points.col(NN_index(two_index(1))) - points.col(NN_index(0))).normalized();
                    if (fabs(cross_.dot(V3)) < 1e-5)
                    {
                        two_index(1)++;
                        ver_nn_num++;
                        NN_index = get_NN(nods.col(i), ver_nn_num);
                        // assert(ver_nn_num < 10);
                        is_co_plane = true;
                    }
                    else
                    {
                        is_co_plane    = false;
                        near_nei(0, i) = NN_index(0);
                        near_nei(1, i) = NN_index(1);
                        near_nei(2, i) = NN_index(two_index(0));
                        near_nei(3, i) = NN_index(two_index(1));
                        break;
                    }
                } while (is_co_plane);
            }
        } while (is_co_line);
    }
    return near_nei;
}

const VectorXd& spatial_hash::get_sup_radi()
{
    return std::move(get_sup_radi(nn_num));
}

const VectorXd& spatial_hash::get_sup_radi(const size_t& nn_num_)
{
    assert(points.cols() >= nn_num_);

    //init data

    sup_radi.setZero(points_num);

#pragma omp parallel for
    for (size_t i = 0; i < points_num; ++i)
    {
        vector<pair_dis> NN_cand;
        find_NN(i, NN_cand, nn_num_ + 2);
        sort(NN_cand.begin(), NN_cand.end(), [](const pair_dis& a, const pair_dis& b) { return a.dis < b.dis; });
        for (size_t j = 0; j < nn_num_; ++j)
        {
            NN(j, i) = NN_cand[j].n;
            sup_radi[i] += NN_cand[j].dis;
        }
    }
    sup_radi *= 3.0 / nn_num_;
    return sup_radi;
}
int spatial_hash::get_friends(const Vector3d& query, const double& sup_radi, vector<size_t>& friends, bool is_sample) const
{

    friends.clear();
    int            grid_delt   = static_cast<int>(ceil(sup_radi / cell_size.col(0).minCoeff())) + 1;
    const Vector3i center_grid = floor(query.array() / cell_size.array()).cast<int>();
    for (size_t i = 0; i < grid_delt; ++i)
    {
        vector<Vector3i> shell;
        get_shell(center_grid, i, shell);
        for (auto& one_grid : shell)
        {
            auto range = points_hash.equal_range(one_grid);
            if (range.first != range.second)
            {
                for_each(range.first, range.second, [&](const decltype(points_hash)::value_type& one_point) {
                    double dis = (points.col(one_point.second) - query).norm();
                    if (dis < sup_radi)
                        friends.push_back(one_point.second);
                });
            }
        }
    }
    assert(friends.size() > nn_num);
    return 0;
}
// int spatial_hash::update_points(const Eigen::MatrixXd &points_){
//   points = points_;
//   hash_NNN();
//   return 0;
// }

}  // namespace marvel