/******************************************************************************
* Author:   Steffen Urban                                              *
* Contact:  urbste@gmail.com                                          *
* License:  Copyright (c) 2016 Steffen Urban, ANU. All rights reserved.      *
*                                                                            *
* Redistribution and use in source and binary forms, with or without         *
* modification, are permitted provided that the following conditions         *
* are met:                                                                   *
* * Redistributions of source code must retain the above copyright           *
*   notice, this list of conditions and the following disclaimer.            *
* * Redistributions in binary form must reproduce the above copyright        *
*   notice, this list of conditions and the following disclaimer in the      *
*   documentation and/or other materials provided with the distribution.     *
* * Neither the name of ANU 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 ANU OR THE 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.                                                               *
******************************************************************************/

/* if you use MLPnP consider citing our paper:
@INPROCEEDINGS {mlpnp2016,
	title={MLPNP - A REAL-TIME MAXIMUM LIKELIHOOD SOLUTION TO THE PERSPECTIVE-N-POINT PROBLEM},
	author={Urban, Steffen and Leitloff, Jens and Hinz, Stefan},
	booktitle={ISPRS Annals of Photogrammetry, Remote Sensing \& Spatial Information Sciences},
	pages={131-138},
	year={2016},
	volume={3}
}

29.06.2016 Steffen Urban
*/

#include <opengv/absolute_pose/modules/mlpnp.hpp>
#include <opengv/math/cayley.hpp>
#include <opengv/math/rodrigues.hpp>
#include <Eigen/Sparse>
#include <iostream>

void opengv::absolute_pose::modules::mlpnp::mlpnp_residuals_and_jacs(
	const Eigen::VectorXd &x,
	const points_t &pts,
	const std::vector<Eigen::MatrixXd> &nullspaces,
	Eigen::VectorXd &r,
	Eigen::MatrixXd &fjac,
	bool getJacs)
{
	rodrigues_t w(x[0], x[1], x[2]);
	translation_t T(x[3], x[4], x[5]);

	//rotation_t R = math::cayley2rot(c);
	rotation_t R = math::rodrigues2rot(w);
	int ii = 0;

	Eigen::MatrixXd jacs(2, 6);

	for (int i = 0; i < pts.size(); ++i)
	{
		Eigen::Vector3d ptCam = R * pts[i] + T;
		ptCam /= ptCam.norm();

		r[ii] = nullspaces[i].col(0).transpose() * ptCam;
		r[ii + 1] = nullspaces[i].col(1).transpose() * ptCam;
		if (getJacs)
		{
			// jacs
			modules::mlpnp::mlpnpJacs(pts[i],
									  nullspaces[i].col(0), nullspaces[i].col(1),
									  w, T,
									  jacs);

			// r
			fjac(ii, 0) = jacs(0, 0);
			fjac(ii, 1) = jacs(0, 1);
			fjac(ii, 2) = jacs(0, 2);

			fjac(ii, 3) = jacs(0, 3);
			fjac(ii, 4) = jacs(0, 4);
			fjac(ii, 5) = jacs(0, 5);
			// s
			fjac(ii + 1, 0) = jacs(1, 0);
			fjac(ii + 1, 1) = jacs(1, 1);
			fjac(ii + 1, 2) = jacs(1, 2);

			fjac(ii + 1, 3) = jacs(1, 3);
			fjac(ii + 1, 4) = jacs(1, 4);
			fjac(ii + 1, 5) = jacs(1, 5);
		}
		ii += 2;
	}
}

void opengv::absolute_pose::modules::mlpnp::mlpnp_lm(
	Eigen::VectorXd &x,
	const points_t &pts,
	const std::vector<Eigen::MatrixXd> &nullspaces,
	const Eigen::SparseMatrix<double> Kll,
	bool use_cov)
{
	const int numObservations = pts.size();
	const int numUnknowns = 6;
	// check redundancy
	assert((2 * numObservations - 6) > 0);

	// =============
	// set all matrices up
	// =============

	Eigen::VectorXd r(2 * numObservations);
	Eigen::VectorXd rd(2 * numObservations);
	Eigen::MatrixXd Jac(2 * numObservations, numUnknowns);
	Eigen::VectorXd g(numUnknowns, 1);
	Eigen::VectorXd dx(numUnknowns, 1); // result vector
	Eigen::MatrixXd eyeMat(numUnknowns, numUnknowns);

	eyeMat.setIdentity();
	Jac.setZero();
	r.setZero();
	dx.setZero();
	g.setZero();

	int it_cnt = 0;
	bool stop = false;
	const int maxIt = 3;
	const double tau = 1e-3;
	double mu = 0.0, nu = 2.0;
	double epsP = 1e-6;
	Eigen::MatrixXd JacTSKll;
	Eigen::MatrixXd A;
	while (it_cnt < maxIt && !stop)
	{
		mlpnp_residuals_and_jacs(x, pts, nullspaces,
								 r, Jac, true);

		if (use_cov)
			JacTSKll = Jac.transpose() * Kll;
		else
			JacTSKll = Jac.transpose();

		A = JacTSKll * Jac;

		// initalize mu
		if (it_cnt == 0 && !stop)
			mu = tau * sqrt(A.diagonal().maxCoeff());
		// get system matrix
		g = JacTSKll * r;

		A += mu * eyeMat;
		// solve
		Eigen::LDLT<Eigen::MatrixXd> chol(A);
		dx = chol.solve(g);
		//dx = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(g);
		// observation update
		Eigen::MatrixXd dl = Jac * dx;
		if (dl.array().abs().maxCoeff() < epsP || mu < 1e-10)
		{
			if (mu == 0.0)
			{
				it_cnt -= 2;
				stop = true;
				x = x - dx;
				break;
			}
			else
				mu = 0.0;
		}
		else
		{
			Eigen::VectorXd xnew = x - dx;

			rd.setZero();
			mlpnp_residuals_and_jacs(xnew, pts, nullspaces,
									 rd, Jac, false);

			// levenberg marquardt
			double epsP = 0.0;
			double Sd = 0.0;
			double dS = 1e-15;
			if (use_cov)
			{
				epsP = r.transpose() * Kll * r;
				Sd = rd.transpose() * Kll * rd;
			}
			else
			{
				epsP = r.transpose() * r;
				Sd = rd.transpose() * rd; //Kll missing!!!
			}

			Eigen::VectorXd dS1 = dx;
			Eigen::VectorXd dS2 = (mu * dS1 + g);

			dS = (dS1.transpose() * dS2);

			double rho = (epsP - Sd) / dS;
			if (rho > 0.0)
			{
				double val1 = 1.0 - pow((2 * rho - 1), 3.0);
				double val2 = 1.0 / 3.0;

				if (val1 > val2)
					mu *= val1;
				else
					mu *= val2;

				nu = 2.0;
				x = xnew;
			}
			else
			{
				mu = mu * nu;
				nu = 2 * nu;
			}
		}
		++it_cnt;
	} //while
	  // result
}

void opengv::absolute_pose::modules::mlpnp::mlpnp_gn(
	Eigen::VectorXd &x,
	const points_t &pts,
	const std::vector<Eigen::MatrixXd> &nullspaces,
	const Eigen::SparseMatrix<double> Kll,
	bool use_cov)
{
	const int numObservations = pts.size();
	const int numUnknowns = 6;
	// check redundancy
	assert((2 * numObservations - numUnknowns) > 0);

	// =============
	// set all matrices up
	// =============

	Eigen::VectorXd r(2 * numObservations);
	Eigen::VectorXd rd(2 * numObservations);
	Eigen::MatrixXd Jac(2 * numObservations, numUnknowns);
	Eigen::VectorXd g(numUnknowns, 1);
	Eigen::VectorXd dx(numUnknowns, 1); // result vector

	Jac.setZero();
	r.setZero();
	dx.setZero();
	g.setZero();

	int it_cnt = 0;
	bool stop = false;
	const int maxIt = 5;
	double epsP = 1e-5;

	Eigen::MatrixXd JacTSKll;
	Eigen::MatrixXd A;
	// solve simple gradient descent
	while (it_cnt < maxIt && !stop)
	{
		mlpnp_residuals_and_jacs(x, pts,
								 nullspaces,
								 r, Jac, true);

		if (use_cov)
		{
			JacTSKll = Jac.transpose() * Kll;
		}
		else
			JacTSKll = Jac.transpose();

		A = JacTSKll * Jac;

		// get system matrix
		g = JacTSKll * r;

		// solve
		Eigen::LDLT<Eigen::MatrixXd> chol(A);
		dx = chol.solve(g);
		//dx = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(g);
		// this is to prevent the solution from falling into a wrong minimum
		// if the linear estimate is spurious
		if (dx.array().abs().maxCoeff() > 5.0 || dx.array().abs().minCoeff() > 1.0)
			break;
		// observation update
		Eigen::MatrixXd dl = Jac * dx;
		if (dl.array().abs().maxCoeff() < epsP)
		{
			stop = true;
			x = x - dx;
			break;
		}
		else
			x = x - dx;

		++it_cnt;
	} //while
	std::cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << std::endl;
	std::cout << A.inverse() << std::endl;
	std::cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << std::endl;
	// result
}

void opengv::absolute_pose::modules::mlpnp::mlpnp_gn(
	Eigen::VectorXd &x,
	const points_t &pts,
	const std::vector<Eigen::MatrixXd> &nullspaces,
	const Eigen::SparseMatrix<double> Kll,
	// 適当で良い↓
	Eigen::MatrixXd &Qxx,
	Eigen::MatrixXd &Qldld,
	bool use_cov)
{
	const int numObservations = pts.size();
	const int numUnknowns = 6;
	// check redundancy
	assert((2 * numObservations - 6) > 0);

	// =============
	// set all matrices up
	// =============

	Eigen::VectorXd r(2 * numObservations);
	Eigen::VectorXd rd(2 * numObservations);
	Eigen::MatrixXd Jac(2 * numObservations, numUnknowns);
	Eigen::VectorXd g(numUnknowns, 1);
	Eigen::VectorXd dx(numUnknowns, 1); // result vector

	Jac.setZero();
	r.setZero();
	dx.setZero();
	g.setZero();

	int it_cnt = 0;
	bool stop = false;
	const int maxIt = 5;
	double epsP = 1e-6;

	Eigen::MatrixXd JacTSKll;
	Eigen::MatrixXd A;
	// solve simple gradient descent
	while (it_cnt < maxIt && !stop)
	{
		mlpnp_residuals_and_jacs(x, pts,
								 nullspaces,
								 r, Jac, true);

		if (use_cov)
		{
			JacTSKll = Jac.transpose() * Kll;
		}
		else
			JacTSKll = Jac.transpose();

		A = JacTSKll * Jac;

		// get system matrix
		g = JacTSKll * r;

		// solve
		Eigen::LDLT<Eigen::MatrixXd> chol(A);
		dx = chol.solve(g);
		// this is to prevent the solution from falling into a wrong minimum
		// if the linear estimate is spurious
		if (dx.array().abs().maxCoeff() > 1.0 || dx.array().abs().minCoeff() > 1.0)
			break;
		// observation update
		Eigen::MatrixXd dl = Jac * dx;
		if (dl.array().abs().maxCoeff() < epsP)
		{
			stop = true;
			x = x - dx;
			break;
		}
		else
			x = x - dx;

		++it_cnt;
	} //while
	// statistics
	Qxx = A.inverse();
	Qldld = Jac * Qxx * Jac.transpose();
}


// -*- tab-width: 2; indent-tabs-mode: nil; coding: utf-8-with-signature -*-
//-----------------------------------------------------------------------------
// Copyright 2000-2021 CEA (www.cea.fr) IFPEN (www.ifpenergiesnouvelles.com)
// See the top-level COPYRIGHT file for details.
// SPDX-License-Identifier: Apache-2.0
//-----------------------------------------------------------------------------

/* Author : havep at Wed Nov 14 13:29:24 2007
 * Generated by createNew
 */
#include <iostream>
#include <iomanip>

#include <boost/shared_ptr.hpp>

#include <arcane/BasicUnitTest.h>

#include <arcane/tests/ArcaneTestGlobal.h>

#include <arcane/geometry/IGeometry.h>
#include <arcane/geometry/IGeometryMng.h>
#include <arcane/discreteoperator/IDivKGradDiscreteOperator.h>

#include <arcane/IMainFactory.h>
#include <arcane/IMeshModifier.h>
#include <arcane/ITimeLoopMng.h>
#include <arcane/MeshStats.h>
#include "arcane/solvers/ILinearSolver.h"
#include "arcane/solvers/ILinearSystemOneStepBuilder.h"
#include "arcane/solvers/impl/LinearSystemOneStepBuilder.h"

using namespace Arcane::Numerics;
#include "arcane/tests/discreteoperator/DiscreteOperatorAMRUnitTest_axl.h"

/*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*/

ARCANETEST_BEGIN_NAMESPACE

using namespace Arcane;

class DiscreteOperatorAMRUnitTest
  : public ArcaneDiscreteOperatorAMRUnitTestObject
{
public:
  /** Constructeur de la classe */
  DiscreteOperatorAMRUnitTest(const ServiceBuildInfo& cb)
    : ArcaneDiscreteOperatorAMRUnitTestObject(cb)
  {
    ;
  }

  /** Destructeur de la classe */
  virtual ~DiscreteOperatorAMRUnitTest() {}

public:
  virtual void initializeTest();
  virtual void executeTest();

  /** Retourne le numéro de version du module */
  virtual Arcane::VersionInfo versionInfo() const { return Arcane::VersionInfo(1,0,0); }

private:
  // Discrete operator
  IDivKGradDiscreteOperator * m_op;

  // Geometric service
  IGeometryMng * m_geom;

  // Permeabilities
  boost::shared_ptr<VariableCellReal> m_k;

  // Face groupss
  boost::shared_ptr<FaceGroup> m_internal_faces;
  boost::shared_ptr<FaceGroup> m_boundary_faces;

  IMesh * m_mesh;

  // Private functions
  void _refine(Integer nb_to_refine);

};

void
DiscreteOperatorAMRUnitTest::
initializeTest()
{
//! AMR
  _refine(1);

  // Initialize mesh
  m_mesh = subDomain()->mesh();

  // Mesh statistics
  MeshStats stats(traceMng(),m_mesh,subDomain()->parallelMng());
  stats.dumpStats();

  // Initialize services
  debug() << "Initializing services";
  m_geom = options()->geometryService();
  m_op = options()->op();

  m_geom->init();
  m_op->init();

  m_op->setGeometryService(m_geom);

  // Initialize diffusion
  debug() << "Initializing diffusion coefficient";
  VariableBuildInfo vb(m_mesh, "k", IVariable::PPrivate|IVariable::PTemporary);
  m_k.reset(new VariableCellReal(vb));

  Real kT = 1;
  ENUMERATE_CELL(iT, m_mesh->allCells()) {
    (*m_k)[*iT] = kT;
  }

  // Create face groups
  debug() << "Creating face groups";
  m_internal_faces.reset(new FaceGroup(m_mesh->allCells().innerActiveFaceGroup()));
  m_boundary_faces.reset(new FaceGroup(m_mesh->allCells().outerActiveFaceGroup()));

}

/*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*/

void
DiscreteOperatorAMRUnitTest::
executeTest()
{
//! AMR
  // Prepare coefficient containers
  debug() << "Preparing coefficient containers";
  CoefficientArrayT<Cell> cell_coefficients(m_mesh->allActiveFaces(),
                                            m_mesh->allActiveCells());
  CoefficientArrayT<Face> face_coefficients(m_mesh->allActiveFaces(),
                                            *m_boundary_faces);

  // Prepare containers
  FaceGroup c_internal_faces  =
    m_internal_faces->itemFamily()->createGroup("C_INTERNAL_FACES");
  FaceGroup cf_internal_faces =
    m_internal_faces->itemFamily()->createGroup("CF_INTERNAL_FACES");

  // Prepare the operator
  debug() << "Preparing discrete operator";
  m_op->prepare(*m_internal_faces,
                *m_boundary_faces,
                c_internal_faces,
                cf_internal_faces,
                &cell_coefficients,
                &face_coefficients);

  // Compute relevant geometric properties
  debug() << "Computing geometric data";
  m_geom->addItemGroupProperty(m_op->cells(),
                               m_op->getCellGeometricProperties() | IGeometryProperty::PMeasure);
  m_geom->addItemGroupProperty(m_op->faces(), m_op->getFaceGeometricProperties());
  m_geom->update();

  // Print relevant geometric properties
  const IGeometryMng::Real3GroupMap & face_center =
    m_geom->getReal3GroupMapProperty(m_op->faces(), IGeometryProperty::PCenter);
  const IGeometryMng::Real3GroupMap & face_normal =
    m_geom->getReal3GroupMapProperty(m_op->faces(), IGeometryProperty::PNormal);

  debug() << std::setw(20) << "Face"
          << std::setw(20) << "Normal"
          << std::setw(20) << "Center";
  ENUMERATE_FACE(iF, m_op->faces()) {
    const Face & F = *iF;

    std::ostringstream center, normal;
    face_normal[F].printXyz(normal);
    face_center[F].printXyz(center);

    debug() << std::setw(20) << iF->localId()
            << std::setw(20) << normal.str()
            << std::setw(20) << center.str();
  }

  const IGeometryMng::Real3GroupMap & cell_center =
    m_geom->getReal3GroupMapProperty(m_op->cells(), IGeometryProperty::PCenter);
  const IGeometryMng::RealGroupMap & cell_measure =
    m_geom->getRealGroupMapProperty(m_op->cells(), IGeometryProperty::PMeasure);

  debug() << std::setw(20) << "Cell"
          << std::setw(20) << "Center"
          << std::setw(20) << "Measure";
  ENUMERATE_CELL(iT, m_op->cells()) {
    const Cell & T = *iT;

    std::ostringstream center;
    cell_center[T].printXyz(center);

    debug() << std::setw(20) << iT->localId()
            << std::setw(20) << center.str()
            << std::setw(20) << cell_measure[T];
  }

  // Scalar diffusivity
  debug() << "Forming discrete operator";
  m_op->formDiscreteOperator(*m_k);

  // Print coefficients
  debug() << "Cell coefficients";
  cell_coefficients.print(debug());
  debug() << "Face coefficients";
  face_coefficients.print(debug());

  // Build and solve linear system
  ILinearSolver * solver = options()->linearSolver();
  LinearSystemOneStepBuilder builder;
  solver->setLinearSystemBuilder(&builder);
  solver->init();

  // Define entry and equation
  const IIndexManager::Entry entry =
    builder.getIndexManager()->buildVariableEntry(m_pressure.variable(), IIndexManager::Direct);
  const IIndexManager::Equation equation =
    builder.getIndexManager()->buildEquation("Equation", entry);

  debug() << "Defining entries end equations";
  ENUMERATE_CELL(icell, m_mesh->allActiveCells()) {
    const Cell & T = *icell;
    debug() << "Define entry " << T.uniqueId();
    builder.getIndexManager()->defineEntryIndex(entry, T);
  }
  ENUMERATE_CELL(icell, m_mesh->ownActiveCells()) {
    const Cell & T = *icell;
    debug() << "Define equation " << T.uniqueId();
    builder.getIndexManager()->defineEquationIndex(equation, T);
  }

  // Initialize pressure (this is needed since mesh has been refined)
  ENUMERATE_CELL(icell, m_mesh->ownActiveCells()) {
    const Cell & T = *icell;
    m_pressure[T] = 0.;
  }
  m_pressure.synchronize();

  // Assemble linear system
  debug() << "Assembling linear system";
  solver->start();

  // Right-hand side
  ENUMERATE_CELL(iT, m_mesh->ownActiveCells()) { // m_op->cells().own()) {
    const Cell& T = *iT;
    debug() << "Assembling RHS for cell" << T.uniqueId();
    ARCANE_ASSERT(T.isActive(), ("Inactive cell found"));
    Real mT = cell_measure[T];
    if(T.isActive())
      builder.addRHSData(builder.getIndexManager()->getEquationIndex(equation, T), mT);
  }

  // Internal faces
  debug() << "-- Internal faces";
  ENUMERATE_FACE(iface, m_op->internalFaces())
    {
      const Face & F = *iface;

      debug() << std::setw(20) << "Internal face" << F.localId();

      const Cell & T0  = F.backCell();  // back cell
      const Cell & T1  = F.frontCell(); // front cell

      ARCANE_ASSERT(T0.isActive() && T1.isActive(), ("Inactive cell found"));

      UniqueArray<Integer> entry_indices(cell_coefficients.getStencilSize(F));
      builder.getIndexManager()->getEntryIndex(entry,
                                               cell_coefficients.getStencil(F),
                                               entry_indices);
      if (T0.isOwn() || T1.isOwn()) {
        UniqueArray<Integer> entry_indices(cell_coefficients.getStencilSize(F));
        builder.getIndexManager()->getEntryIndex(entry,
                                                 cell_coefficients.getStencil(F),
                                                 entry_indices);
        if(T0.isOwn())
          builder.addData(builder.getIndexManager()->getEquationIndex(equation, T0),
                          1.,
                          entry_indices,
                          cell_coefficients.getCoefficients(F));
        if(T1.isOwn())
          builder.addData(builder.getIndexManager()->getEquationIndex(equation, T1),
                          -1.,
                          entry_indices,
                          cell_coefficients.getCoefficients(F));
      }
    }

  // Boundary faces
  debug() << "-- Boundary faces";
  ENUMERATE_FACE(iface, m_op->boundaryFaces())
    {
      const Face & F = *iface;

      debug() << std::setw(20) << "Boundary face" << F.localId();

      const Cell & T0  = F.boundaryCell();  // back cell

      ARCANE_ASSERT(T0.isActive(), ("Inactive cell found"));

      UniqueArray<Integer> entry_indices(cell_coefficients.getStencilSize(F));
      builder.getIndexManager()->getEntryIndex(entry,
                                               cell_coefficients.getStencil(F),
                                               entry_indices);
      if(T0.isOwn())
        builder.addData(builder.getIndexManager()->getEquationIndex(equation, T0),
                        1.,
                        entry_indices,
                        cell_coefficients.getCoefficients(F));
    }

  // Solve the system
  debug() << "Solving linear system";
  const bool success = solver->solve();
  if (!success)
    fatal() << "Solver Convergence failed";

  // Retrieve solution
  debug() << "Retrieving solution";
  solver->getSolution() ;
  solver->end();
  builder.end();

  // Exit the loop
  subDomain()->timeLoopMng()->stopComputeLoop(true);
}

/*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*/

void
DiscreteOperatorAMRUnitTest::
_refine(Integer nb_to_refine)
{
//! AMR
  // Recherche les nb_to_refine premières mailles de type IT_Hexaedron8
  ENUMERATE_CELL(icell, subDomain()->mesh()->ownActiveCells())
    {
      Cell cell = *icell;
      ItemInternal* iitem = cell.internal();
      if (cell.type()==IT_Hexaedron8){
        Integer f = iitem->flags();
        f |= ItemInternal::II_Refine;
        iitem->setFlags(f);
        nb_to_refine--;
        if (nb_to_refine == 0)
          break;
      }
    }

  subDomain()->mesh()->modifier()->refineItems();
}

/*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*/

ARCANE_REGISTER_SERVICE_DISCRETEOPERATORAMRUNITTEST(DiscreteOperatorAMRUnitTest,DiscreteOperatorAMRUnitTest);

ARCANETEST_END_NAMESPACE


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

#include "tests/Unit/TestingFramework.hpp"

#include <array>
#include <boost/range/combine.hpp>
#include <boost/tuple/tuple.hpp>
#include <cstddef>
#include <string>
#include <tuple>
#include <type_traits>
#include <utility>

#include "DataStructures/DataBox/DataBoxTag.hpp"
#include "DataStructures/DataVector.hpp"
#include "DataStructures/Index.hpp"
#include "DataStructures/Tensor/Tensor.hpp"
#include "DataStructures/Variables.hpp"
#include "DataStructures/VariablesHelpers.hpp"
#include "ErrorHandling/Error.hpp"
#include "Parallel/PupStlCpp11.hpp"
#include "Utilities/GetOutput.hpp"
#include "Utilities/Gsl.hpp"
#include "Utilities/Literals.hpp"
#include "Utilities/StdHelpers.hpp"
#include "Utilities/TMPL.hpp"
#include "Utilities/TaggedTuple.hpp"
#include "tests/Unit/TestHelpers.hpp"

// IWYU pragma: no_forward_declare Variables

namespace VariablesTestTags_detail {
/// [simple_variables_tag]
struct vector : db::SimpleTag {
  static std::string name() noexcept { return "vector"; }
  using type = tnsr::I<DataVector, 3, Frame::Grid>;
};
/// [simple_variables_tag]

struct scalar : db::SimpleTag {
  static std::string name() noexcept { return "scalar"; }
  using type = Scalar<DataVector>;
};

struct scalar2 : db::SimpleTag {
  static std::string name() noexcept { return "scalar2"; }
  using type = Scalar<DataVector>;
};

/// [prefix_variables_tag]
template <class Tag>
struct PrefixTag0 : db::PrefixTag, db::SimpleTag {
  using type = db::item_type<Tag>;
  using tag = Tag;
  static std::string name() noexcept { return "PrefixTag0"; }
};
/// [prefix_variables_tag]

template <class Tag>
struct PrefixTag1 : db::PrefixTag, db::SimpleTag {
  using type = db::item_type<Tag>;
  using tag = Tag;
  static std::string name() noexcept { return "PrefixTag1"; }
};

template <class Tag>
struct PrefixTag2 : db::PrefixTag, db::SimpleTag {
  using type = db::item_type<Tag>;
  using tag = Tag;
  static std::string name() noexcept { return "PrefixTag2"; }
};

template <class Tag>
struct PrefixTag3 : db::PrefixTag, db::SimpleTag {
  using type = db::item_type<Tag>;
  using tag = Tag;
  static std::string name() noexcept { return "PrefixTag3"; }
};
}  // namespace VariablesTestTags_detail

static_assert(
    std::is_nothrow_move_constructible<
        Variables<tmpl::list<VariablesTestTags_detail::scalar,
                             VariablesTestTags_detail::vector>>>::value,
    "Missing move semantics in Variables.");

SPECTRE_TEST_CASE("Unit.DataStructures.Variables", "[DataStructures][Unit]") {
  Variables<tmpl::list<VariablesTestTags_detail::vector,
                       VariablesTestTags_detail::scalar,
                       VariablesTestTags_detail::scalar2>>
      v(1, -3.0);
  decltype(v) v_initialize;
  v_initialize.initialize(1, -3.0);
  CHECK(v == v_initialize);

  CHECK(v.size() ==
        v.number_of_grid_points() * v.number_of_independent_components);

  CHECK(1 == v.number_of_grid_points());
  CHECK(5 == v.size());

  auto& vector_in_v = get<VariablesTestTags_detail::vector>(v);
  // clang-tidy: do not use pointer arithmetic
  CHECK(-3.0 == v.data()[0]);  // NOLINT
  CHECK(-3.0 == vector_in_v.get(0)[0]);

  tnsr::I<DataVector, 3, Frame::Grid> another_vector(1_st, -5.0);

  CHECK(-5.0 == another_vector.get(0)[0]);

  vector_in_v = another_vector;

  // clang-tidy: do not use pointer arithmetic
  CHECK(-5.0 == v.data()[0]);  // NOLINT
  CHECK(-5.0 == vector_in_v.get(0)[0]);

  vector_in_v = tnsr::I<DataVector, 3, Frame::Grid>{1_st, -4.0};

  // clang-tidy: do not use pointer arithmetic
  CHECK(-4.0 == v.data()[0]);  // NOLINT
  CHECK(-4.0 == v.data()[1]);  // NOLINT
  CHECK(-4.0 == v.data()[2]);  // NOLINT
  CHECK(-4.0 == vector_in_v.get(0)[0]);
  CHECK(-4.0 == vector_in_v.get(1)[0]);
  CHECK(-4.0 == vector_in_v.get(2)[0]);

  const auto& kvector_in_v = get<VariablesTestTags_detail::vector>(v);
  CHECK(kvector_in_v.get(0)[0] == -4.0);

  Variables<tmpl::list<VariablesTestTags_detail::vector,
                       VariablesTestTags_detail::scalar,
                       VariablesTestTags_detail::scalar2>>
      v2(1, -3.0);
  CHECK(v != v2);
  v2 = v;
  CHECK(v2 == v);

  Variables<tmpl::list<VariablesTestTags_detail::vector,
                       VariablesTestTags_detail::scalar,
                       VariablesTestTags_detail::scalar2>>
      v3;
  CHECK(v3.size() == 0);
  CHECK(v3.number_of_grid_points() == 0);
  v3 = v2;
  CHECK(v2 == v3);

  const std::string expected_output =
      "vector:\n"
      "T(0)=(-4)\n"
      "T(1)=(-4)\n"
      "T(2)=(-4)\n\n"
      "scalar:\n"
      "T()=(-3)\n\n"
      "scalar2:\n"
      "T()=(-3)";
  CHECK(get_output(v) == expected_output);

  Variables<tmpl::list<>> empty_vars;
  CHECK(get_output(empty_vars) == "Variables is empty!");

  // Check self-assignment
#ifndef __APPLE__
#if defined(__clang__) && __clang_major__ > 6
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wself-assign-overloaded"
#endif  // defined(__clang__) && __clang_major__ > 6
#endif  // ! __APPLE__
  v = v;
#ifndef __APPLE__
#if defined(__clang__) && __clang_major__ > 6
#pragma GCC diagnostic pop
#endif  // defined(__clang__) && __clang_major__ > 6
#endif  // ! __APPLE__
  CHECK(v == v2);

  CHECK(
      Tags::Variables<tmpl::list<VariablesTestTags_detail::vector,
                                 VariablesTestTags_detail::scalar,
                                 VariablesTestTags_detail::scalar2>>::name() ==
      "Variables(vector,scalar,scalar2)");
}

// [[OutputRegex, Must copy into same size]]
[[noreturn]] SPECTRE_TEST_CASE("Unit.DataStructures.Variables.BadCopy",
                               "[DataStructures][Unit]") {
  ASSERTION_TEST();
#ifdef SPECTRE_DEBUG
  Variables<tmpl::list<VariablesTestTags_detail::vector,
                       VariablesTestTags_detail::scalar,
                       VariablesTestTags_detail::scalar2>>
      v(1, -3.0);
  auto& vector_in_v = get<VariablesTestTags_detail::vector>(v);
  vector_in_v = tnsr::I<DataVector, 3, Frame::Grid>{10_st, -4.0};
  ERROR("Failed to trigger ASSERT in an assertion test");
#endif
}

SPECTRE_TEST_CASE("Unit.DataStructures.Variables.Move",
                  "[DataStructures][Unit]") {
  Variables<tmpl::list<VariablesTestTags_detail::vector>> x(1, -2.0),
      z(2, -3.0);
  CHECK(&get<VariablesTestTags_detail::vector>(z)[0][0] == z.data());
  Variables<tmpl::list<VariablesTestTags_detail::vector>> y = std::move(x);
  x = std::move(z);
  CHECK(
      (x == Variables<tmpl::list<VariablesTestTags_detail::vector>>{2, -3.0}));
  CHECK(&get<VariablesTestTags_detail::vector>(x)[0][0] == x.data());

// Intentionally testing self-move
#ifdef __clang__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wself-move"
#endif  // defined(__clang__)
  x = std::move(x);
#ifdef __clang__
#pragma GCC diagnostic pop
#endif  // defined(__clang__)
  // clang-tidy: false positive 'x' used after it was moved
  CHECK((x ==  // NOLINT
         Variables<tmpl::list<VariablesTestTags_detail::vector>>{2, -3.0}));
  CHECK(&get<VariablesTestTags_detail::vector>(x)[0][0] == x.data());
}

namespace {
template <typename T1, typename VT, bool VF>
void check_vectors(const Variables<T1>& t1, const blaze::Vector<VT, VF>& t2) {
  CHECK(t1.size() == (~t2).size());
  for (size_t i = 0; i < t1.size(); ++i) {
    // We've removed the subscript operator so people don't try to use that
    // and as a result we need to use the data() member function
    // clang-tidy: do not use pointer arithmetic
    CHECK(t1.data()[i] == approx((~t2)[i]));  // NOLINT
  }
}

template <typename T1, typename T2>
void check_vectors(const Variables<T1>& t1, const Variables<T2>& t2) {
  CHECK(t1.size() == t2.size());
  for (size_t i = 0; i < t1.size(); ++i) {
    // We've removed the subscript operator so people don't try to use that
    // and as a result we need to use the data() member function
    // clang-tidy: do not use pointer arithmetic
    CHECK(t1.data()[i] == approx(t2.data()[i]));  // NOLINT
  }
}
}  // namespace

SPECTRE_TEST_CASE("Unit.DataStructures.Variables.Math",
                  "[DataStructures][Unit]") {
  using test_variable_type =
      Variables<tmpl::list<VariablesTestTags_detail::vector,
                           VariablesTestTags_detail::scalar,
                           VariablesTestTags_detail::scalar2>>;
  test_variable_type three(1, 3.0);
  check_vectors(test_variable_type{1, 6}, 2.0 * three);
  check_vectors(test_variable_type{1, 6}, three * 2.0);
  check_vectors(test_variable_type{1, 1.5}, three / 2.0);
  check_vectors(test_variable_type{1, 12}, three + three + three + three);
  check_vectors(test_variable_type{1, 9}, three + (three + three));

  // clang-tidy: point sides of overloaded operator are equivalent
  check_vectors(test_variable_type{1, -6},
                three - three - three - three);                      // NOLINT
  check_vectors(test_variable_type{1, 3}, three - (three - three));  // NOLINT
  check_vectors(test_variable_type{1, 3}, three - three + three);    // NOLINT
  check_vectors(test_variable_type{1, 3}, three + three - three);

  test_variable_type test_assignment(three * 1.0);
  test_assignment += test_variable_type{1, 3};
  check_vectors(test_variable_type{1, 6}, test_assignment);
  test_assignment -= test_variable_type{1, 2};
  check_vectors(test_variable_type{1, 4}, test_assignment);
  test_assignment *= 0.25;
  check_vectors(test_variable_type{1, 1.0}, test_assignment);
  test_assignment /= 0.1;
  check_vectors(test_variable_type{1, 10.0}, test_assignment);

  test_assignment += test_variable_type{1, 3} * 3.0;
  check_vectors(test_variable_type{1, 19.0}, test_assignment);
  test_assignment -= test_variable_type{1, 3} * 2.0;
  check_vectors(test_variable_type{1, 13.0}, test_assignment);

  test_variable_type test_assignment2(1, 0.0);
  test_assignment2 = test_assignment * 1.0;
  CHECK(test_assignment2 == test_assignment);

  const auto check_components = [](const auto& variables,
                                   const DataVector& datavector) {
    tmpl::for_each<typename std::decay_t<decltype(variables)>::tags_list>(
        [&variables, &datavector](auto tag) {
          using Tag = tmpl::type_from<decltype(tag)>;
          for (const auto& component : get<Tag>(variables)) {
            CHECK(component == datavector);
          }
        });
  };
  const DataVector dv{1., 2., 3., 4.};
  test_variable_type test_datavector_math(4, 2.);
  test_datavector_math *= dv;
  check_components(test_datavector_math, {2., 4., 6., 8.});
  check_components(test_datavector_math * dv, {2., 8., 18., 32.});
  check_components(dv * test_datavector_math, {2., 8., 18., 32.});
  test_datavector_math *= dv;
  check_components(test_datavector_math, {2., 8., 18., 32.});
  check_components(test_datavector_math / dv, {2., 4., 6., 8.});
  test_datavector_math /= dv;
  check_components(test_datavector_math, {2., 4., 6., 8.});
}

SPECTRE_TEST_CASE("Unit.DataStructures.Variables.PrefixSemantics",
                  "[DataStructures][Unit]") {
  using variable_type =
      Variables<tmpl::list<VariablesTestTags_detail::vector,
                           VariablesTestTags_detail::scalar,
                           VariablesTestTags_detail::scalar2>>;
  using prefix_variable_type = Variables<tmpl::list<
      VariablesTestTags_detail::PrefixTag0<VariablesTestTags_detail::vector>,
      VariablesTestTags_detail::PrefixTag0<VariablesTestTags_detail::scalar>,
      VariablesTestTags_detail::PrefixTag0<VariablesTestTags_detail::scalar2>>>;
  prefix_variable_type prefix_vars_0(4, 8.);
  prefix_variable_type prefix_vars_1(4, 6.);
  variable_type vars_0(prefix_vars_0);
  variable_type vars_1(std::move(prefix_vars_1));

  const auto check_variables = [](const auto& lvars_0, const auto& lvars_1) {
    tmpl::for_each<typename std::decay_t<decltype(lvars_0)>::tags_list>(
        [&lvars_0, &lvars_1](auto tag) {
          using Tag = tmpl::type_from<decltype(tag)>;
          for (const auto& component :
               boost::combine(get<Tag>(lvars_0), get<Tag>(lvars_1))) {
            CHECK(boost::get<0>(component) == boost::get<1>(component));
          }
        });
  };

  check_variables(vars_0, variable_type(4, 8.0));
  check_variables(vars_1, variable_type(4, 6.0));

  prefix_variable_type prefix_vars_2(4, 4.);
  prefix_variable_type prefix_vars_3(4, 2.);
  vars_0 = prefix_vars_2;
  check_variables(vars_0, variable_type(4, 4.0));
  vars_0 = std::move(prefix_vars_3);
  check_variables(vars_0, variable_type(4, 2.0));
}

SPECTRE_TEST_CASE("Unit.DataStructures.Variables.PrefixMath",
                  "[DataStructures][Unit]") {
  using prefix0_variable_type = Variables<tmpl::list<
      VariablesTestTags_detail::PrefixTag0<VariablesTestTags_detail::vector>,
      VariablesTestTags_detail::PrefixTag0<VariablesTestTags_detail::scalar>,
      VariablesTestTags_detail::PrefixTag0<VariablesTestTags_detail::scalar2>>>;
  using prefix1_variable_type = Variables<tmpl::list<
      VariablesTestTags_detail::PrefixTag1<VariablesTestTags_detail::vector>,
      VariablesTestTags_detail::PrefixTag1<VariablesTestTags_detail::scalar>,
      VariablesTestTags_detail::PrefixTag1<VariablesTestTags_detail::scalar2>>>;
  using prefix2_variable_type = Variables<tmpl::list<
      VariablesTestTags_detail::PrefixTag2<VariablesTestTags_detail::vector>,
      VariablesTestTags_detail::PrefixTag2<VariablesTestTags_detail::scalar>,
      VariablesTestTags_detail::PrefixTag2<VariablesTestTags_detail::scalar2>>>;
  using prefix3_variable_type = Variables<tmpl::list<
      VariablesTestTags_detail::PrefixTag3<VariablesTestTags_detail::PrefixTag2<
          VariablesTestTags_detail::vector>>,
      VariablesTestTags_detail::PrefixTag3<VariablesTestTags_detail::PrefixTag2<
          VariablesTestTags_detail::scalar>>,
      VariablesTestTags_detail::PrefixTag3<VariablesTestTags_detail::PrefixTag2<
          VariablesTestTags_detail::scalar2>>>>;

  prefix0_variable_type three_0(1, 3.0);
  prefix1_variable_type three_1(1, 3.0);
  prefix2_variable_type three_2(1, 3.0);
  prefix3_variable_type three_3(1, 3.0);

  check_vectors(prefix3_variable_type{1, 6.0}, 2.0 * three_0);
  check_vectors(prefix3_variable_type{1, 6.0}, three_0 * 2.0);
  check_vectors(prefix3_variable_type{1, 1.5}, three_0 / 2.0);
  check_vectors(prefix3_variable_type{1, 12.0},
                three_0 + three_1 + three_2 + three_3);
  check_vectors(prefix3_variable_type{1, 12.0},
                (three_0 + three_1) + (three_2 + three_3));
  check_vectors(prefix3_variable_type{1, 9.0}, three_0 + (three_1 + three_2));

  // clang-tidy: point sides of overloaded operator are equivalent
  check_vectors(prefix3_variable_type{1, -6.0},
                three_0 - three_1 - three_2 - three_3);  // NOLINT
  check_vectors(prefix3_variable_type{1, 6.0},
                three_0 - (three_1 - three_2 - three_3));  // NOLINT
  check_vectors(prefix3_variable_type{1, 0.0},
                (three_0 - three_1) - (three_2 - three_3));  // NOLINT
  check_vectors(prefix3_variable_type{1, 3.0},
                three_0 - (three_1 - three_2));  // NOLINT
  check_vectors(prefix3_variable_type{1, 3.0},
                three_0 - three_1 + three_2);  // NOLINT
  check_vectors(prefix3_variable_type{1, 3.0}, three_0 + three_1 - three_2);

  prefix0_variable_type test_assignment(three_0 * 1.0);
  test_assignment += prefix1_variable_type{1, 3};
  check_vectors(prefix0_variable_type{1, 6}, test_assignment);
  test_assignment -= prefix1_variable_type{1, 2};
  check_vectors(prefix0_variable_type{1, 4}, test_assignment);
  test_assignment *= 0.25;
  check_vectors(prefix0_variable_type{1, 1.0}, test_assignment);
  test_assignment /= 0.1;
  check_vectors(prefix0_variable_type{1, 10.0}, test_assignment);

  test_assignment += prefix1_variable_type{1, 3} * 3.0;
  check_vectors(prefix0_variable_type{1, 19.0}, test_assignment);
  test_assignment -= prefix1_variable_type{1, 3} * 2.0;
  check_vectors(prefix0_variable_type{1, 13.0}, test_assignment);
}

SPECTRE_TEST_CASE("Unit.DataStructures.Variables.Serialization",
                  "[DataStructures][Unit]") {
  Variables<tmpl::list<VariablesTestTags_detail::vector>> v(1, -3.0);
  test_serialization(v);
  auto tuple_of_v = std::make_tuple(
      Variables<tmpl::list<VariablesTestTags_detail::vector>>{1, -4.0});
  test_serialization(tuple_of_v);
}

SPECTRE_TEST_CASE("Unit.DataStructures.Variables.assign_subset",
                  "[DataStructures][Unit]") {
  constexpr size_t size = 3;
  const auto first_test = [&size](const auto& vars_subset0,
                                  const auto& vars_subset1) noexcept {
    Variables<tmpl::list<VariablesTestTags_detail::vector,
                         VariablesTestTags_detail::scalar>>
        vars_set0{size, 3.0};
    CHECK(get<VariablesTestTags_detail::vector>(vars_set0) ==
          db::item_type<VariablesTestTags_detail::vector>(size, 3.0));
    CHECK(get<VariablesTestTags_detail::scalar>(vars_set0) ==
          db::item_type<VariablesTestTags_detail::scalar>(size, 3.0));
    vars_set0.assign_subset(vars_subset0);
    CHECK(get<VariablesTestTags_detail::vector>(vars_set0) ==
          db::item_type<VariablesTestTags_detail::vector>(size, 8.0));
    CHECK(get<VariablesTestTags_detail::scalar>(vars_set0) ==
          db::item_type<VariablesTestTags_detail::scalar>(size, 3.0));
    vars_set0.assign_subset(vars_subset1);
    CHECK(get<VariablesTestTags_detail::vector>(vars_set0) ==
          db::item_type<VariablesTestTags_detail::vector>(size, 8.0));
    CHECK(get<VariablesTestTags_detail::scalar>(vars_set0) ==
          db::item_type<VariablesTestTags_detail::scalar>(size, 4.0));
  };

  first_test(
      Variables<tmpl::list<VariablesTestTags_detail::vector>>(size, 8.0),
      Variables<tmpl::list<VariablesTestTags_detail::scalar>>(size, 4.0));
  first_test(tuples::TaggedTuple<VariablesTestTags_detail::vector>(
                 VariablesTestTags_detail::vector::type{size, 8.0}),
             tuples::TaggedTuple<VariablesTestTags_detail::scalar>(
                 VariablesTestTags_detail::scalar::type{size, 4.0}));

  const auto second_test = [&size](const auto& vars_subset0,
                                   const auto& vars_subset1) noexcept {
    Variables<tmpl::list<VariablesTestTags_detail::vector,
                         VariablesTestTags_detail::scalar,
                         VariablesTestTags_detail::scalar2>>
        vars_set1(size, 3.0);
    CHECK(get<VariablesTestTags_detail::vector>(vars_set1) ==
          db::item_type<VariablesTestTags_detail::vector>(size, 3.0));
    CHECK(get<VariablesTestTags_detail::scalar>(vars_set1) ==
          db::item_type<VariablesTestTags_detail::scalar>(size, 3.0));
    CHECK(get<VariablesTestTags_detail::scalar2>(vars_set1) ==
          db::item_type<VariablesTestTags_detail::scalar2>(size, 3.0));
    vars_set1.assign_subset(vars_subset0);
    CHECK(get<VariablesTestTags_detail::vector>(vars_set1) ==
          db::item_type<VariablesTestTags_detail::vector>(size, 8.0));
    CHECK(get<VariablesTestTags_detail::scalar>(vars_set1) ==
          db::item_type<VariablesTestTags_detail::scalar>(size, 3.0));
    CHECK(get<VariablesTestTags_detail::scalar2>(vars_set1) ==
          db::item_type<VariablesTestTags_detail::scalar2>(size, 3.0));
    vars_set1.assign_subset(vars_subset1);
    CHECK(get<VariablesTestTags_detail::vector>(vars_set1) ==
          db::item_type<VariablesTestTags_detail::vector>(size, 8.0));
    CHECK(get<VariablesTestTags_detail::scalar>(vars_set1) ==
          db::item_type<VariablesTestTags_detail::scalar>(size, 4.0));
    CHECK(get<VariablesTestTags_detail::scalar2>(vars_set1) ==
          db::item_type<VariablesTestTags_detail::scalar2>(size, 3.0));
  };

  second_test(
      Variables<tmpl::list<VariablesTestTags_detail::vector>>(size, 8.0),
      Variables<tmpl::list<VariablesTestTags_detail::scalar>>(size, 4.0));
  second_test(tuples::TaggedTuple<VariablesTestTags_detail::vector>(
                  VariablesTestTags_detail::vector::type{size, 8.0}),
              tuples::TaggedTuple<VariablesTestTags_detail::scalar>(
                  VariablesTestTags_detail::scalar::type{size, 4.0}));

  const auto third_test = [&size](const auto& vars_subset0) noexcept {
    Variables<tmpl::list<VariablesTestTags_detail::vector>> vars_set2(size,
                                                                      -7.0);
    CHECK(get<VariablesTestTags_detail::vector>(vars_set2) ==
          db::item_type<VariablesTestTags_detail::vector>(size, -7.0));
    vars_set2.assign_subset(vars_subset0);
    CHECK(get<VariablesTestTags_detail::vector>(vars_set2) ==
          db::item_type<VariablesTestTags_detail::vector>(size, 8.0));
  };

  third_test(
      Variables<tmpl::list<VariablesTestTags_detail::vector>>(size, 8.0));
  third_test(tuples::TaggedTuple<VariablesTestTags_detail::vector>(
      VariablesTestTags_detail::vector::type{size, 8.0}));
}

SPECTRE_TEST_CASE("Unit.DataStructures.Variables.SliceVariables",
                  "[DataStructures][Unit]") {
  Variables<tmpl::list<VariablesTestTags_detail::vector>> vars(24, 0.);
  const size_t x_extents = 2, y_extents = 3, z_extents = 4,
               vec_size = VariablesTestTags_detail::vector::type::size();
  Index<3> extents(x_extents, y_extents, z_extents);
  for (size_t s = 0; s < vars.size(); ++s) {
    // clang-tidy: do not use pointer arithmetic
    vars.data()[s] = s;  // NOLINT
  }
  Variables<tmpl::list<VariablesTestTags_detail::vector>>
      expected_vars_sliced_in_x(y_extents * z_extents, 0.),
      expected_vars_sliced_in_y(x_extents * z_extents, 0.),
      expected_vars_sliced_in_z(x_extents * y_extents, 0.);
  const size_t x_offset = 1, y_offset = 2, z_offset = 1;

  for (size_t s = 0; s < expected_vars_sliced_in_x.size(); ++s) {
    // clang-tidy: do not use pointer arithmetic
    expected_vars_sliced_in_x.data()[s] = x_offset + s * x_extents;  // NOLINT
  }
  for (size_t i = 0; i < vec_size; ++i) {
    for (size_t x = 0; x < x_extents; ++x) {
      for (size_t z = 0; z < z_extents; ++z) {
        // clang-tidy: do not use pointer arithmetic
        expected_vars_sliced_in_y
            .data()[x + x_extents * (z + z_extents * i)] =  // NOLINT
            i * extents.product() + x + x_extents * (y_offset + z * y_extents);
      }
    }
  }
  for (size_t i = 0; i < vec_size; ++i) {
    for (size_t x = 0; x < x_extents; ++x) {
      for (size_t y = 0; y < y_extents; ++y) {
        // clang-tidy: do not use pointer arithmetic
        expected_vars_sliced_in_z
            .data()[x + x_extents * (y + y_extents * i)] =  // NOLINT
            i * extents.product() + x + x_extents * (y + y_extents * z_offset);
      }
    }
  }
  CHECK(data_on_slice(vars, extents, 0, x_offset) == expected_vars_sliced_in_x);
  CHECK(data_on_slice(vars, extents, 1, y_offset) == expected_vars_sliced_in_y);
  CHECK(data_on_slice(vars, extents, 2, z_offset) == expected_vars_sliced_in_z);

  CHECK(
      data_on_slice<VariablesTestTags_detail::vector>(
          extents, 0, x_offset, get<VariablesTestTags_detail::vector>(vars)) ==
      expected_vars_sliced_in_x);
  CHECK(
      data_on_slice<VariablesTestTags_detail::vector>(
          extents, 1, y_offset, get<VariablesTestTags_detail::vector>(vars)) ==
      expected_vars_sliced_in_y);
  CHECK(
      data_on_slice<VariablesTestTags_detail::vector>(
          extents, 2, z_offset, get<VariablesTestTags_detail::vector>(vars)) ==
      expected_vars_sliced_in_z);
}

SPECTRE_TEST_CASE("Unit.DataStructures.Variables.add_slice_to_data",
                  "[DataStructures][Unit]") {
  using Vector = VariablesTestTags_detail::vector::type;
  const Index<2> extents{{{4, 2}}};
  Variables<tmpl::list<VariablesTestTags_detail::vector>> vars(
      extents.product());
  get<VariablesTestTags_detail::vector>(vars) =
      Vector{{{{1110000., 1120000., 1130000., 1140000., 1210000., 1220000.,
                1230000., 1240000.},
               {2110000., 2120000., 2130000., 2140000., 2210000., 2220000.,
                2230000., 2240000.},
               {3110000., 3120000., 3130000., 3140000., 3210000., 3220000.,
                3230000., 3240000.}}}};

  {
    const auto slice_extents = extents.slice_away(0);
    Variables<tmpl::list<VariablesTestTags_detail::vector>> slice(
        slice_extents.product(), 0.);
    get<VariablesTestTags_detail::vector>(slice) =
        Vector{{{{1100., 1200.}, {2100., 2200.}, {3100., 3200.}}}};
    add_slice_to_data(make_not_null(&vars), slice, extents, 0, 2);
  }
  {
    const auto slice_extents = extents.slice_away(1);
    Variables<tmpl::list<VariablesTestTags_detail::vector>> slice(
        slice_extents.product(), 0.);
    get<VariablesTestTags_detail::vector>(slice) = Vector{
        {{{11., 12., 13., 14.}, {21., 22., 23., 24.}, {31., 32., 33., 34.}}}};
    add_slice_to_data(make_not_null(&vars), slice, extents, 1, 1);
  }

  CHECK((Vector{{{{1110000., 1120000., 1131100., 1140000., 1210011., 1220012.,
                   1231213., 1240014.},
                  {2110000., 2120000., 2132100., 2140000., 2210021., 2220022.,
                   2232223., 2240024.},
                  {3110000., 3120000., 3133100., 3140000., 3210031., 3220032.,
                   3233233., 3240034.}}}}) ==
        get<VariablesTestTags_detail::vector>(vars));
}

// [[OutputRegex, Must copy into same size]]
[[noreturn]] SPECTRE_TEST_CASE(
    "Unit.DataStructures.Variables.assign_to_default",
    "[DataStructures][Unit]") {
  ASSERTION_TEST();
  #ifdef SPECTRE_DEBUG
  Variables<tmpl::list<VariablesTestTags_detail::scalar>> vars;
  get<VariablesTestTags_detail::scalar>(vars) = Scalar<DataVector>{{{{0.}}}};
  ERROR("Failed to trigger ASSERT in an assertion test");
#endif
}

// [[OutputRegex, volume_vars has wrong number of grid points.
//  Expected 8, got 10]]
[[noreturn]] SPECTRE_TEST_CASE(
    "Unit.DataStructures.Variables.add_slice_to_data.BadSize.volume",
    "[DataStructures][Unit]") {
  ASSERTION_TEST();
#ifdef SPECTRE_DEBUG
  Variables<tmpl::list<VariablesTestTags_detail::vector>> vars(10, 0.);
  const Variables<tmpl::list<VariablesTestTags_detail::vector>> slice(2, 0.);
  add_slice_to_data(make_not_null(&vars), slice, Index<2>{{{4, 2}}}, 0, 0);
  ERROR("Failed to trigger ASSERT in an assertion test");
#endif
}

// [[OutputRegex, vars_on_slice has wrong number of grid points.
//  Expected 2, got 5]]
[[noreturn]] SPECTRE_TEST_CASE(
    "Unit.DataStructures.Variables.add_slice_to_data.BadSize.slice",
    "[DataStructures][Unit]") {
  ASSERTION_TEST();
#ifdef SPECTRE_DEBUG
  Variables<tmpl::list<VariablesTestTags_detail::vector>> vars(8, 0.);
  const Variables<tmpl::list<VariablesTestTags_detail::vector>> slice(5, 0.);
  add_slice_to_data(make_not_null(&vars), slice, Index<2>{{{4, 2}}}, 0, 0);
  ERROR("Failed to trigger ASSERT in an assertion test");
#endif
}


/* Copyright 2017 The sfcpp Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
==============================================================================*/

#ifndef SRC_GEO_COORDINATESYSTEMTREENODE_HPP_
#define SRC_GEO_COORDINATESYSTEMTREENODE_HPP_

#include <Eigen/Dense>

#include <memory>
#include <vector>

namespace sfcpp {
namespace geo {

/**
 * It has to be assured that the world coordinate system lives at least as long
 * as the other coordinate systems are used.
 */
class CoordinateSystemTreeNode {
  CoordinateSystemTreeNode *parent;
  std::vector<std::shared_ptr<CoordinateSystemTreeNode>> children;

  Eigen::Matrix4d fromParent;
  Eigen::Matrix4d toParent;

  Eigen::Matrix4d fromWorld;
  Eigen::Matrix4d toWorld;

  void parentUpdated();
  void updateChildren();

  /**
   * This method can't set itself as a child of the parent because it does not
   * have a shared_ptr to itself.
   * But in can update its own parent variable.
   */
  void setParent(CoordinateSystemTreeNode *parent);
  void removeChild(CoordinateSystemTreeNode *child);

  CoordinateSystemTreeNode(CoordinateSystemTreeNode *parent,
                           Eigen::Matrix4d toParent);
  CoordinateSystemTreeNode(CoordinateSystemTreeNode *parent,
                           Eigen::Matrix4d toParent,
                           Eigen::Matrix4d fromParent);

 public:
  CoordinateSystemTreeNode();  // create world coordinate system

  /**
   * If the child already has a parent, the child is removed from the other
   * parent.
   */
  void addChild(std::shared_ptr<CoordinateSystemTreeNode> child);
  std::shared_ptr<CoordinateSystemTreeNode> createChild(
      Eigen::Matrix4d toParent);
  std::shared_ptr<CoordinateSystemTreeNode> createChild(
      Eigen::Matrix4d toParent, Eigen::Matrix4d fromParent);

  void setTransformationToParent(Eigen::Matrix4d toParent);
  void setTransformationFromParent(Eigen::Matrix4d fromParent);

  Eigen::Matrix4d getTransformationTo(
      std::shared_ptr<CoordinateSystemTreeNode> other) const;
  Eigen::Vector4d transform(Eigen::Vector4d const &vec,
                      std::shared_ptr<CoordinateSystemTreeNode> other) const;
  Eigen::Vector4d toWorldCoordinates(Eigen::Vector4d const &vec) const;
  Eigen::Vector4d fromWorldCoordinates(Eigen::Vector4d const &vec) const;
};

} /* namespace geo */
} /* namespace utils */

#endif /* SRC_GEO_COORDINATESYSTEMTREENODE_HPP_ */


// Copyright (C) 2014 The Regents of the University of California (Regents)
// and Google, Inc. All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California, Google,
//       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 HOLDERS 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 of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu), John Flynn (jflynn@google.com)

#include "theia/sfm/pose/three_point_relative_pose_partial_rotation.h"

#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <Eigen/SVD>
#include <Eigen/Geometry>
#include <math.h>

#include <limits>

namespace theia {

using Eigen::AngleAxisd;
using Eigen::EigenSolver;
using Eigen::JacobiSVD;
using Eigen::Map;
using Eigen::Matrix3d;
using Eigen::Matrix;
using Eigen::Quaterniond;
using Eigen::Vector3d;

namespace {

bool SolveQEP(const Matrix3d& M, const Matrix3d& C, const Matrix3d& K,
              std::vector<double>* eigenvalues,
              std::vector<Vector3d>* eigenvectors) {
  // Solves the quadratic eigenvalue problem:
  //
  //   Q(s)x = 0
  //
  // where:
  //
  //   Q(s) = s^2*M + s*C + K
  //
  // Returns true if the problem could be solved, false otherwise.
  //
  // This is converted to a generalized eigenvalue as described in:
  //   http://en.wikipedia.org/wiki/Quadratic_eigenvalue_problem
  //
  // The generalized eigenvalue problem is in this form:
  //
  // [ C  K ]z = s[ -M  0  ]z
  // [-I  0 ]     [ 0   -I ]
  //
  // With eigenvector z = [ sx ] and s is the eigenvalue.
  //                      [  x ]
  //
  // The eigenvector of the quadratic eigenvalue problem can be extracted from
  // z.
  //
  // Where z is the eigenvector, s is the eigenvalue.
  // This generalized eigenvalue can be converted to a standard eigenvalue
  // problem by multiplying by the inverse of the RHS matrix. The inverse of
  // the RHS matrix is particularly simple:
  //
  // [ -inv(M)  0 ]
  // [ 0       -I ]
  //
  // So the generalized eigenvalue problem reduces to a standard eigenvalue
  // problem on the constraint matrix:
  //
  // [ -inv(M)C  -inv(M)K ]z = sz
  // [ I         0        ]

  Matrix3d inv_M;
  static const double kDeterminantThreshold = 1e-12;
  bool invert_success;
  // Check that determinant of M is larger than threshold. This threshold only
  // seems to be reached when there is no rotation. TODO(jflynn): verify.
  M.computeInverseWithCheck(inv_M, invert_success, kDeterminantThreshold);
  if (!invert_success) {
    return false;
  }

  // Negate inverse of M.
  inv_M = -1.0 * inv_M;

  // Set up constraint matrix.
  Matrix<double, 6, 6> constraint = Matrix<double, 6, 6>::Zero();
  // Set upper-left to - inv(M) * C.
  constraint.block<3, 3>(0, 0) = inv_M * C;
  // Set upper-right to -inv(M) * K.
  constraint.block<3, 3>(0, 3) = inv_M * K;
  // Set lower-left to identity.
  constraint.block<3, 3>(3, 0) = Matrix3d::Identity();
  // Extract the left eigenvectors and values from the constraint matrix.
  EigenSolver<Matrix<double, 6, 6> > eig_solver(constraint);
  const double kImagEigenValueTolerance = 1e-12;

  for (int i = 0; i < eig_solver.eigenvalues().size(); i++) {
    // Ignore roots corresponding to s^2 + 1.
    if (fabs(eig_solver.eigenvalues()[i].imag() - 1) <
            kImagEigenValueTolerance ||
        fabs(eig_solver.eigenvalues()[i].imag() + 1) <
            kImagEigenValueTolerance) {
      continue;
    }
    // Only consider the real eigenvalues and corresponding eigenvectors.
    eigenvalues->push_back(eig_solver.eigenvalues()[i].real());
    eigenvectors->push_back(
        Vector3d(eig_solver.eigenvectors().col(i).tail<3>().real()));
  }
  return true;
}

}  // namespace

void ThreePointRelativePosePartialRotation(
    const Vector3d& axis,
    const Vector3d image_1_rays[3],
    const Vector3d image_2_rays[3],
    std::vector<Quaterniond>* soln_rotations,
    std::vector<Vector3d>* soln_translations) {
  // CHECK_DOUBLE_EQ(axis.squaredNorm(), 1.0);
  // CHECK_NOTNULL(soln_rotations)->clear();
  // CHECK_NOTNULL(soln_translations)->clear();
  soln_rotations->clear();
  soln_translations->clear();

  // Each correspondence gives another constraint and the constraints can
  // be stacked to create a constraint matrix.
  //
  // The rotation matrix R can be parameterized, up to scale factor, as:
  //
  //   R ~ 2 * (v * v' + s[v]x) + (s^2 - 1)I
  //
  //   I = Identity matrix.
  //
  // where is v is the known (unit length) axis and s is related to the
  // unknown angle of rotation.
  //
  // The epipolar constraint holds if the rotation matrix is scaled, so the
  // rotation matrix parameterization above can be used. Each row of the
  // constraint matrix is thus a function of s^2 and s, so the constraint can be
  // written as:
  //
  //   [ M * s^2 + C *s + k ] * [ t ] = 0
  //                            [ 1 ]
  //
  // This is standard quadratic eigenvalue problem (QEP) and is solved using
  // standard methods.

  // Creates the matrices for the QEP problem.
  Matrix3d M;
  Matrix3d C;
  Matrix3d K;
  for (int i = 0; i < 3; ++i) {
    const Vector3d& q1(image_1_rays[i]);
    const Vector3d& q2(image_2_rays[i]);
    M.row(i) = q2.cross(q1);
    C.row(i) = 2.0 * q2.cross(axis.cross(q1));
    K.row(i) = 2.0 * q1.dot(axis) * q2.cross(axis) - q2.cross(q1);
  }

  std::vector<double> eigenvalues;
  std::vector<Vector3d> eigenvectors;
  if (SolveQEP(M, C, K, &eigenvalues, &eigenvectors)) {
    // Extracts the translations and rotations from the eigenvalues and
    // eigenvectors of the QEP problem.
    for (int i = 0; i < eigenvalues.size(); ++i) {
      Quaterniond quat(eigenvalues[i], axis[0], axis[1], axis[2]);
      quat.normalize();

      soln_rotations->push_back(quat);
      soln_translations->push_back(eigenvectors[i]);
      soln_rotations->push_back(quat);
      soln_translations->push_back(-eigenvectors[i]);
    }
  } else {
    // When there is zero rotation the vector part of the quaternion disappears
    // and it becomes ([0], 1) (where [0] is the zero vector) and the SolveQEP
    // method cannot be used.
    // However from the equations for M, C, and K above we can see that the
    // C and K matrices contain the axis, which is 0 assuming zero rotation,
    // so to solve for the translation we can directly extract the null space
    // of M.
    // Alternatively this can be derived directly from the epipolar
    // constraint, after substituting identity for the rotation matrix.
    eigenvectors.clear();

    JacobiSVD<Matrix3d> svd = M.jacobiSvd(Eigen::ComputeFullV);
    const Vector3d eigenvector(svd.matrixV().col(2));

    soln_rotations->push_back(Quaterniond(AngleAxisd(0.0, axis)));
    soln_translations->push_back(eigenvector);
    soln_rotations->push_back(Quaterniond(AngleAxisd(0.0, axis)));
    soln_translations->push_back(-eigenvector);
  }
}

}  // namespace theia


#include <fstream>
#include <iostream>

#include "define.h"

#include <Eigen/SparseCholesky>
#include <Eigen/CholmodSupport>
#include <Eigen/Geometry>

#include <pcl/common/transforms.h>
#include <pcl/common/time.h>
#include <pcl/io/io.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/pcd_io.h>

#include <boost/thread/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/filesystem.hpp>

#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/factory.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/slam3d/types_slam3d.h>

#include "Optimizer.h"
#include "OptimizerParameter.h"

class SparseMatFiller {
public:
    SparseMatFiller(TriContainer &tri) : m_tripletToFill(tri) {}

    void fill(int row, int col, const Eigen::Matrix<double, 6, 1> &vec0, const Eigen::Matrix<double, 6, 1> &vec1) {
        fillBlock(row, row, vec0 * vec0.transpose());
        fillBlock(row, col, vec0 * vec1.transpose());
        fillBlock(col, row, vec1 * vec0.transpose());
        fillBlock(col, col, vec1 * vec1.transpose());
    }
    inline void add(int r, int c, double v) {
        m_tripletToFill.push_back(Tri(r, c, v));
        /*
        if (!m_indexMap.count(r)) {
            m_indexMap[r] = std::map<int, size_t>();
        }
        if (!m_indexMap[r].count(c)) {
            m_indexMap[r][c] = m_tripletToFill.size();
            m_tripletToFill.push_back(Tri(r, c, v));
        }
        else {
            m_tripletToFill[m_indexMap[r][c]].m_value += v;
        }
         */
    }
private:
    inline void fillBlock(int block_r, int block_c, const Eigen::Matrix<double, 6, 6> &mat) {
#pragma unroll 6
        for (int i = 0; i < 6; ++i) {
#pragma unroll 6
            for (int j = 0; j < 6; ++j) {
                add(block_r * 6 + i, block_c * 6 + j, mat(i, j));
            }
        }
    }

private:
    TriContainer &m_tripletToFill;
    std::map<int, std::map<int, size_t> > m_indexMap;
};

Optimizer::Optimizer(OptimizerParameter &params)
: m_params(params)
{
}

Optimizer::~Optimizer()
{
}

using namespace g2o;

G2O_USE_TYPE_GROUP(slam2d);
G2O_USE_TYPE_GROUP(slam3d);

void Optimizer::optimizeUseG2O()
{

    // create the linear solver
    BlockSolverX::LinearSolverType * linearSolver = new LinearSolverCSparse<BlockSolverX::PoseMatrixType>();

    // create the block solver on top of the linear solver
    BlockSolverX* blockSolver = new BlockSolverX(linearSolver);

    // create the algorithm to carry out the optimization
    //OptimizationAlgorithmGaussNewton* optimizationAlgorithm = new OptimizationAlgorithmGaussNewton(blockSolver);
    OptimizationAlgorithmLevenberg* optimizationAlgorithm = new OptimizationAlgorithmLevenberg(blockSolver);

    // NOTE: We skip to fix a variable here, either this is stored in the file
    // itself or Levenberg will handle it.

    // create the optimizer to load the data and carry out the optimization
    SparseOptimizer optimizer;
    SparseOptimizer::initMultiThreading();
    optimizer.setVerbose(true);
    optimizer.setAlgorithm(optimizationAlgorithm);

    {
        pcl::ScopeTime time("G2O setup Graph vertices");
        for (size_t cloud_count = 0; cloud_count < m_pointClouds.size(); ++cloud_count)
        {
            VertexSE3 *vertex = new VertexSE3;
            vertex->setId(cloud_count);
            Isometry3D affine = Isometry3D::Identity();
            affine.linear() = m_pointClouds[cloud_count]->sensor_orientation_.toRotationMatrix().cast<Isometry3D::Scalar>();
            affine.translation() = m_pointClouds[cloud_count]->sensor_origin_.block<3, 1>(0, 0).cast<Isometry3D::Scalar>();
            vertex->setEstimate(affine);
            optimizer.addVertex(vertex);
        }
        optimizer.vertex(0)->setFixed(true);
    }

    {
        pcl::ScopeTime time("G2O setup Graph edges");
        double trans_noise = 0.5, rot_noise = 0.5235;
        EdgeSE3::InformationType infomation = EdgeSE3::InformationType::Zero();
        infomation.block<3, 3>(0, 0) << trans_noise * trans_noise, 0, 0,
                                        0, trans_noise * trans_noise, 0,
                                        0, 0, trans_noise * trans_noise;
        infomation.block<3, 3>(3, 3) << rot_noise * rot_noise, 0, 0,
                                        0, rot_noise * rot_noise, 0,
                                        0, 0, rot_noise * rot_noise;
        for (size_t pair_count = 0; pair_count < m_cloudPairs.size(); ++pair_count)
        {
            CloudPair pair = m_cloudPairs[pair_count];
		    int from = pair.corresIdx.first;
		    int to = pair.corresIdx.second;
            EdgeSE3 *edge = new EdgeSE3;
		    edge->vertices()[0] = optimizer.vertex(from);
		    edge->vertices()[1] = optimizer.vertex(to);

            Eigen::Matrix<double, 6, 6> ATA = Eigen::Matrix<double, 6, 6>::Zero();
            Eigen::Matrix<double, 6, 1> ATb = Eigen::Matrix<double, 6, 1>::Zero();
#pragma unroll 8
            for (size_t point_count = 0; point_count < pair.corresPointIdx.size(); ++point_count) {
                int point_p = pair.corresPointIdx[point_count].first;
                int point_q = pair.corresPointIdx[point_count].second;
                PointType P = m_pointClouds[from]->points[point_p];
                PointType Q = m_pointClouds[to]->points[point_q];

                Eigen::Vector3d p = P.getVector3fMap().cast<double>();
                Eigen::Vector3d q = Q.getVector3fMap().cast<double>();
                Eigen::Vector3d Np = P.getNormalVector3fMap().cast<double>();

                double b = (p - q).dot(Np);

                Eigen::Matrix<double, 6, 1> A_p;
                A_p.block<3, 1>(0, 0) = p.cross(Np);
                A_p.block<3, 1>(3, 0) = Np;

                ATA += A_p * A_p.transpose();
                ATb += A_p * b;
            }

            Eigen::Matrix<double, 6, 1> X = ATA.ldlt().solve(ATb);
            Isometry3D measure = Isometry3D::Identity();
            float beta = X[0];
            float gammar = X[1];
            float alpha = X[2];
            measure.linear() = (Eigen::Matrix3d)Eigen::AngleAxisd(alpha, Eigen::Vector3d::UnitZ()) *
                Eigen::AngleAxisd(gammar, Eigen::Vector3d::UnitY()) *
                Eigen::AngleAxisd(beta, Eigen::Vector3d::UnitX());
            measure.translation() = X.block<3, 1>(3, 0);

            edge->setMeasurement(measure);

		    edge->setInformation(infomation);

            optimizer.addEdge(edge);
        }
    }

    optimizer.save("debug_preOpt.g2o");
    {
        pcl::ScopeTime time("g2o optimizing");
        optimizer.initializeOptimization();
        optimizer.optimize(30);
    }
    optimizer.save("debug_postOpt.g2o");

    for (size_t cloud_count = 0; cloud_count < m_pointClouds.size(); ++cloud_count)
    {
        CloudTypePtr tmp(new CloudType);
        Isometry3D trans = ((VertexSE3 *)optimizer.vertices()[cloud_count])->estimate();
        Eigen::Affine3d affine;
        affine.linear() = trans.rotation();
        affine.translation() = trans.translation();
        pcl::transformPointCloudWithNormals(*m_pointClouds[cloud_count], *tmp, affine.cast<float>());
        pcl::copyPointCloud(*tmp, *m_pointClouds[cloud_count]);
    }

    PCL_WARN("Opitimization DONE!!!!\n");

    if (m_params.saveDirectory.length()) {
        if (boost::filesystem::exists(m_params.saveDirectory) && !boost::filesystem::is_directory(m_params.saveDirectory)) {
            boost::filesystem::remove(m_params.saveDirectory);
        }
        boost::filesystem::create_directories(m_params.saveDirectory);

        char filename[1024] = { 0 };
        for (size_t i = 0; i < m_pointClouds.size(); ++i) {
            sprintf(filename, "%s/cloud_%d.ply", m_params.saveDirectory.c_str(), i);
            pcl::io::savePLYFileBinary(filename, *m_pointClouds[i]);
        }
    }
}

void Optimizer::optimizeRigid()
{
    size_t num_clouds = m_pointClouds.size();
    size_t matrix_size = num_clouds * 6;

    ATb.resize(matrix_size);
    ATA.resize(matrix_size, matrix_size);

    for (int iter_count = 0; iter_count < m_params.maxIteration; ++iter_count)
    {
        PCL_INFO("Iteration : %d\n", iter_count);
        pcl::ScopeTime time("optimizing");

        ATb.setZero();
        ATA.setZero();

        align_error = 0.0;

        PCL_INFO("\tProcessing %d cloud pairs\n", m_cloudPairs.size());

        int cpu_cores = std::max(boost::thread::hardware_concurrency(), (unsigned)2);
        int count = 0;
        boost::thread_group group;
        for (size_t pair_count = 0; pair_count < m_cloudPairs.size(); ++pair_count) {
            if (count == cpu_cores) {
                count = 0;
                group.join_all();
            }
            group.create_thread(boost::bind(&Optimizer::eachCloudPair, this, boost::ref(m_cloudPairs[pair_count])));
            count++;
        }
        group.join_all();

        /*
        for (size_t pair_count = 0; pair_count < m_cloudPairs.size(); ++pair_count) {
            eachCloudPair(m_cloudPairs[pair_count]);
        }*/

        PCL_WARN("\tDone!\n");
        PCL_WARN("\tAlign error : %.6f\n", align_error);

        Eigen::IOFormat format(Eigen::FullPrecision, 0);
        std::ofstream("debug_matrix.txt", std::ios::out) << ATA;
        std::ofstream("debug_b.txt", std::ios::out) << ATb.format(format);

        Vec X;
        if (m_params.useCholmod) {
            pcl::ScopeTime mat_solve("\t\tMatrix solve");
            Eigen::CholmodSupernodalLLT<Mat> solver;
            solver.compute(ATA);
            X = solver.solve(ATb);
        }
        else {
            pcl::ScopeTime mat_solve("\t\tMatrix solve");
            Eigen::SimplicialCholesky<Mat> solver(ATA);
            X = solver.solve(ATb);
        }

        std::ofstream("debug_result.txt", std::ios::out) << X.format(format);
        //std::cout << "Result :\n\n" << X << std::endl << std::endl;

#pragma unroll 8
        for (size_t i = 0; i < num_clouds; ++i) {
            CloudTransform trans;
            float beta = X.block<6, 1>(i * 6, 0)[0];
            float gammar = X.block<6, 1>(i * 6, 0)[1];
            float alpha = X.block<6, 1>(i * 6, 0)[2];
            trans.linear() = (Eigen::Matrix3f)Eigen::AngleAxisf(alpha, Eigen::Vector3f::UnitZ()) *
                Eigen::AngleAxisf(gammar, Eigen::Vector3f::UnitY()) *
                Eigen::AngleAxisf(beta, Eigen::Vector3f::UnitX());
            trans.translation() = X.block<6, 1>(i * 6, 0).block<3, 1>(3, 0).cast<float>();
            CloudTypePtr transformed(new CloudType);
            pcl::transformPointCloudWithNormals(*m_pointClouds[i], *transformed, trans);
            m_pointClouds[i] = transformed;
        }
    }

    PCL_WARN("Opitimization DONE!!!!\n");

    if (m_params.saveDirectory.length()) {
        if (boost::filesystem::exists(m_params.saveDirectory) && !boost::filesystem::is_directory(m_params.saveDirectory)) {
            boost::filesystem::remove(m_params.saveDirectory);
        }
        boost::filesystem::create_directories(m_params.saveDirectory);

        char filename[1024] = { 0 };
        for (size_t i = 0; i < m_pointClouds.size(); ++i) {
            sprintf(filename, "%s/cloud_%d.ply", m_params.saveDirectory.c_str(), i);
            pcl::io::savePLYFileBinary(filename, *m_pointClouds[i]);
        }
    }
}

void Optimizer::eachCloudPair(CloudPair &pair)
{
    int cloud0 = pair.corresIdx.first;
    int cloud1 = pair.corresIdx.second;

    size_t matrix_size = m_pointClouds.size() * 6;

    TriContainer mat_elem;
    mat_elem.reserve(matrix_size * matrix_size / 5);
    SparseMatFiller filler(mat_elem);

    for (int i = 0; i < 6; ++i) {
        filler.add(i, i, 1.0);
    }

    Vec atb(matrix_size);
    Mat ata(matrix_size, matrix_size);
    atb.setZero(), ata.setZero();

    double score = 0.0;
    {
        //pcl::ScopeTime time("calculate LSE matrix");

#pragma unroll 8
        for (size_t point_count = 0; point_count < pair.corresPointIdx.size(); ++point_count) {
            int point_p = pair.corresPointIdx[point_count].first;
            int point_q = pair.corresPointIdx[point_count].second;
            PointType P = m_pointClouds[cloud0]->points[point_p];
            PointType Q = m_pointClouds[cloud1]->points[point_q];

            Eigen::Vector3d p = P.getVector3fMap().cast<double>();
            Eigen::Vector3d q = Q.getVector3fMap().cast<double>();
            Eigen::Vector3d Np = P.getNormalVector3fMap().cast<double>();

            double b = -(p - q).dot(Np);
            score += b * b;
            Eigen::Matrix<double, 6, 1> A_p, A_q;
            A_p.block<3, 1>(0, 0) = p.cross(Np);
            A_p.block<3, 1>(3, 0) = Np;
            A_q.block<3, 1>(0, 0) = -q.cross(Np);
            A_q.block<3, 1>(3, 0) = -Np;

            filler.fill(cloud0, cloud1, A_p, A_q);
            atb.block<6, 1>(cloud0 * 6, 0) += A_p * b;
            atb.block<6, 1>(cloud1 * 6, 0) += A_q * b;
        }
        ata.setFromTriplets(mat_elem.begin(), mat_elem.end());
    }

    {
        //pcl::ScopeTime time("Fill sparse matrix");
        boost::mutex::scoped_lock lock(m_cloudPairMutex);
        //std::cout << "\tcurrent thread : " << boost::this_thread::get_id() << std::endl;
        //PCL_INFO("\tPair <%d, %d> alignment Score : %.6f\n", cloud0, cloud1, score);
        ATA += ata;
        ATb += atb;
        align_error += score;
    }
}

void Optimizer::operator()(std::vector<CloudTypePtr> &pointClouds, std::vector<CloudPair> &cloudPaire)
{
    m_pointClouds = pointClouds;
    m_cloudPairs = cloudPaire;

    if (m_params.g2oOptimize) {
        optimizeUseG2O();
    }
    else {
        optimizeRigid();
    }
}

void Optimizer::mergeClouds()
{
    CloudTypePtr all_cloud(new CloudType);
    for (size_t i = 0; i < m_pointClouds.size(); ++i) {
        //all_cloud += m_pointClouds[i];
    }
}


/*
 * py_bvh.cpp
 *
 *  Created on: Apr 18, 2018
 *      Author: Maxime Lemonnier
 */

#include <atomic>
#include <thread>
#include <mutex>
#include <algorithm>
#include <functional>
#include <iostream>
#include <numeric>
#include <tbb/parallel_for.h>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <pybind11/eigen.h>
#include <pybind11/functional.h>
#include <Eigen/Dense>
#include <unsupported/Eigen/BVH>
#include "BVHWrapper.h"
#include "RayPointsQuery.h"
#include "RayTrianglesQuery.h"

namespace py = pybind11;

using namespace Eigen;
class PyTrianglesBVH
{
public:
    typedef BVHWrapper<int, float,3, 3> Wrapper;
    typedef KdBVH<float, 3, size_t> BVH;
    typedef RayTrianglesQuery<BVH, Wrapper> Query;

    PyTrianglesBVH(const Ref<const Wrapper::Indices> triangles, const Ref<const Wrapper::Points> vertices) :
        _triangles(triangles), _vertices(vertices), _wrapper(_triangles, _vertices)
        , _tree(_wrapper.begin(), _wrapper.end(), _wrapper.boxes_begin(), _wrapper.boxes_end())
    {

    }

    decltype(auto) intersect_ray(const Eigen::Ref<const Query::Point> origin, const Eigen::Ref<const Query::Point> direction, bool keep_closest_only = false)
    {
        Query query(_wrapper, origin, direction);

        BVIntersect(_tree, query);

        auto results = query.sorted();

        auto n_intersections = (keep_closest_only ?
			std::min(results.size(), size_t(1))
			: results.size());

        Matrix<int, Dynamic, 1u> ids;
        Matrix<float, Dynamic, 3u> tuvs;

        ids.resize(n_intersections, 1);
        tuvs.resize(n_intersections, 3);

        for(size_t i = 0; i < n_intersections; i++)
        {
                ids[i] = results[i].id;
                tuvs.row(i) = results[i].tuv;
        }
        return std::make_tuple(ids, tuvs);
    }

    decltype(auto) intersect_rays(const Ref<const Wrapper::Points> origins, const Ref<const Wrapper::Points> directions, float threshold = 0.f, bool keep_closest_only = false)
    {
        size_t n_rays = origins.rows();

        std::vector<Query::Intersections> results(n_rays);
        tbb::parallel_for(size_t(0), n_rays, [&](auto i)
        //for(size_t i = 0; i < n_rays; i++)
        {
            Query query(_wrapper, origins.row(i), directions.row(i));
            BVIntersect(_tree, query);

            if(threshold > 0 && query.intersections.empty())
            {
                if(BVMinimize(_tree, query) < threshold)
                    results[i].emplace_back(Query::Intersection{query.minimum.id, query.minimum.tuv});
            }
            else
                results[i] = std::move(query.sorted());

        }
        );

        Matrix<int, Dynamic, 1u> offsets;
        Matrix<int, Dynamic, 1u> ids;
        Matrix<float, Dynamic, 3u> tuvs;

		using namespace std;

        size_t n_intersections = std::accumulate(results.begin()
		, results.end()
		, size_t(0u)
		, [&](const size_t & s, const Query::Intersections & result)
		{
			return s + (keep_closest_only ?
			min(result.size(), size_t(1))
			: result.size());
		});

        offsets.resize(n_rays+1, 1);
        ids.resize(n_intersections, 1);
        tuvs.resize(n_intersections, 3);

        size_t ray_i = 0;
        size_t i = 0;
        offsets[0] = 0;
        std::for_each(results.begin(), results.end(), [&](const Query::Intersections & result)
        {
            for(size_t r = 0; r < result.size(); r++)
            {

                ids[i] = result[r].id;
                tuvs.row(i) = result[r].tuv;
                i++;
                if(keep_closest_only)
                    break;
            }
            offsets[++ray_i] = i;
        });
        return std::make_tuple(offsets, ids, tuvs);
    }

    decltype(auto) ray_distance(const Eigen::Ref<const Query::Point> origin, const Eigen::Ref<const Query::Point> direction)
    {
        Query query(_wrapper, origin, direction);

        BVMinimize(_tree, query);

        return std::make_tuple(query.minimum.id, query.minimum.distance, query.minimum.tuv);
    }

    decltype(auto) rays_distances(const Ref<const Wrapper::Points> origins, const Ref<const Wrapper::Points> directions)
    {
        size_t n_rays = origins.rows();
        Matrix<int, Dynamic, 1u> ids;
        Matrix<float, Dynamic, 1u> distances;
        Matrix<float, Dynamic, 3u> tuvs;

        ids.resize(n_rays, 1);
        distances.resize(n_rays, 1);
        tuvs.resize(n_rays, 3);
        tbb::parallel_for(size_t(0), n_rays, [&](auto i)
        //for(size_t i = 0; i < n_rays; i++)
        {
            Query query(_wrapper, origins.row(i), directions.row(i));
            BVMinimize(_tree, query);
            ids[i] = query.minimum.id;
            distances[i] = query.minimum.distance;
            tuvs.row(i) = query.minimum.tuv;
        }
        );

        return std::make_tuple(ids, distances, tuvs);
    }
    const Wrapper::Indices _triangles; //TODO avoid copy
    const Wrapper::Points _vertices;
    Wrapper _wrapper;
    BVH _tree;
};

class PyPointsBVH
{
public:
    typedef BVHWrapper<int, float, 1, 3> Wrapper;
    typedef KdBVH<float, 3, size_t> BVH;
    typedef RayPointsQuery<BVH, Wrapper> Query;

    PyPointsBVH(const Ref<const Wrapper::Indices> indices, const Ref<const Wrapper::Points> vertices) :
        _indices(indices), _vertices(vertices), _wrapper(_indices, _vertices)
        , _tree(_wrapper.begin(), _wrapper.end(), _wrapper.boxes_begin(), _wrapper.boxes_end())
    {
    }

    decltype(auto) ray_distance(const Eigen::Ref<const Query::Point> origin, const Eigen::Ref<const Query::Point> direction)
    {
        Query query(_wrapper, origin, direction);

        BVMinimize(_tree, query);

        return std::make_tuple(query.minimum.id, query.minimum.distance, query.minimum.t);
    }

    decltype(auto) rays_distances(const Ref<const Wrapper::Points> origins, const Ref<const Wrapper::Points> directions)
    {
        size_t n_rays = origins.rows();
        Matrix<int, Dynamic, 1u> ids;
        Matrix<float, Dynamic, 1u> distances;
        Matrix<float, Dynamic, 1u> t;

        ids.resize(n_rays, 1);
        distances.resize(n_rays, 1);
        t.resize(n_rays, 3);
        tbb::parallel_for(size_t(0), n_rays, [&](auto i)
        //for(size_t i = 0; i < n_rays; i++)
        {
            Query query(_wrapper, origins.row(i), directions.row(i));
            BVMinimize(_tree, query);
            ids[i] = query.minimum.id;
            distances[i] = query.minimum.distance;
            t[i] = query.minimum.t;
        }
        );

        return std::make_tuple(ids, distances, t);
    }
    const Wrapper::Indices _indices; //TODO avoid copy
    const Wrapper::Points _vertices;
    Wrapper _wrapper;
    BVH _tree;
};

PYBIND11_MODULE(PyBVH, m) {
    py::class_<PyTrianglesBVH>(m, "BVH")
        .def(py::init<const Ref<const PyTrianglesBVH::Wrapper::Indices>, const Ref<const PyTrianglesBVH::Wrapper::Points>>())
        .def("intersect_ray", &PyTrianglesBVH::intersect_ray)
        .def("intersect_rays", &PyTrianglesBVH::intersect_rays)
        .def("ray_distance", &PyTrianglesBVH::ray_distance)
        .def("rays_distances", &PyTrianglesBVH::rays_distances)
        .def_readonly("triangles", &PyTrianglesBVH::_triangles)
        .def_readonly("vertices", &PyTrianglesBVH::_vertices)
        ;

    py::class_<PyPointsBVH>(m, "PointsBVH")
        .def(py::init<const Ref<const PyPointsBVH::Wrapper::Indices>, const Ref<const PyPointsBVH::Wrapper::Points> >())
        .def("ray_distance", &PyPointsBVH::ray_distance)
        .def("rays_distances", &PyPointsBVH::rays_distances)
        .def_readonly("vertices", &PyPointsBVH::_vertices)
        ;

#ifdef VERSION_INFO
    m.attr("__version__") = VERSION_INFO;
#else
    m.attr("__version__") = "dev";
#endif
}


/* Copyright 2017 The sfcpp Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
==============================================================================*/

#ifndef SRC_GEO_POINT3D_HPP_
#define SRC_GEO_POINT3D_HPP_

#include <geo/CoordinateSystem.hpp>
#include <Eigen/Dense>
#include "Vector3D.hpp"

#include <memory>

namespace sfcpp {
namespace geo {

class Point3D {
	Eigen::Vector3d vec;
	std::shared_ptr<CoordinateSystem> coordinateSystem;

	Eigen::Vector4d getVec4D() const;
public:
	Point3D(Eigen::Vector3d const &vec, std::shared_ptr<CoordinateSystem> const &coordinateSystem);
	Point3D(Point3D const &other);

	Eigen::Vector3d getCoordinates(std::shared_ptr<CoordinateSystem> const &targetSystem) const;
	Eigen::Vector3d getWorldCoordinates() const;

	Point3D &operator+=(Vector3D const &vector);

	friend Point3D operator+(Point3D const &first, Vector3D const &second);
	friend Vector3D operator-(Point3D const &first, Point3D const &second);
};

} /* namespace geo */
} /* namespace utils */

#endif /* SRC_GEO_POINT3D_HPP_ */


#ifndef CGTOPAIR_HH
#define CGTOPAIR_HH

/*!
 * \file CGTOPair.hh
 * \brief Definition of the CGTOPair class
 */

#include <typeinfo>
#include <Eigen/Core>
#include "AbstractBFPair.hh"
#include "CGTO.hh"
#include "MultiArray.hh"
#include "constants.hh"

/*!
 * \brief Class for pairs of contracted GTOs
 *
 * Class CGTOPair holds a pair of contracted Gaussian type orbitals, and
 * defines a set of operations on this pair, like computing the overlap
 * or kinetic energy integral.
 */
class CGTOPair: public AbstractBFPair
{
public:
	//! Unique class ID, used in looking up integral calculation functions
	static const size_t cid;

	//! Constructor
	CGTOPair(const CGTO& f, const CGTO& g):
		AbstractBFPair(cid, f, g),
		_shell_pair(CGTOShellList::singleton().pair(f.shell(), g.shell()))
		{}

	//! Return the first orbital in the pair
	const CGTO& f() const
	{
		return static_cast< const CGTO& >(AbstractBFPair::f());
	}
	//! Return the second orbital in the pair
	const CGTO& g() const
	{
		return static_cast< const CGTO& >(AbstractBFPair::g());
	}

	//! Return the pair of orbitals shells
	const CGTOShellPair& shellPair() const { return _shell_pair; }

	//! Compute the overlap between the two orbitals in this pair
	double overlap() const;
	//! Compute the kinetic energy integral between the two orbitals in this pair
	double kineticEnergy() const;
	/*!
	 * \brief Compute one-electron integrals
	 *
	 * Compute the overlap and kinetic energy integral between the two
	 * functions in this pair.
	 * \param S Place to store the overlap
	 * \param T Place to store the kinetic energy
	 */
	void oneElectron(double& S, double& T) const;
	/*!
	 * \brief Compute nuclear attraction integrals
	 *
	 * Compute the nuclear attraction integrals, due to the nuclei with
	 * positions \a nuc_pos and charges \a nuc_charge, between the functions
	 * in this pair.
	 * \param nuc_pos    The positions of the nuclei
	 * \param nuc_charge The nuclear charges
	 */
	double nuclearAttraction(const Eigen::MatrixXd& nuc_pos,
		const Eigen::VectorXd& nuc_charge) const;

	/*!
	 * \brief Return the total number of primitive pairs in this
	 *    pair of contractions
	 */
	int size() const
	{
		return f().size() * g().size();
	}
	//! Return the angular momentum in the \a i direction for the first orbital
	int lA(int i) const
	{
		return f().l(i);
	}
	//! Return the angular momentum in the \a i direction for the second orbital
	int lB(int i) const
	{
		return g().l(i);
	}
	//! Return the sum of angular momenta in the \a i direction for the two orbitals
	int lsum(int i) const
	{
		return lA(i) + lB(i);
	}
	/*!
	 * \brief Return the widths of the primitives in the first contraction,
	 *    for all primitives in the second contraction.
	 */
	const Eigen::ArrayXXd& widthsA() const
	{
		//return f().widths().replicate(1, g().size());
		return _shell_pair.widthsA();
	}
	/*!
	 * \brief Return the widths of the primitives in the second
	 *    contraction, for all primitives in the first contraction.
	 */
	const Eigen::ArrayXXd& widthsB() const
	{
		//return g().widths().transpose().replicate(f().size(), 1);
		return _shell_pair.widthsB();
	}
	//! The sums of primitive widths, equivalent to widthsA() + widthsB()
	const Eigen::ArrayXXd& widthsSum() const
	{
		//return widthsA() + widthsB();
		return _shell_pair.widthsSum();
	}
	//! The "reduced" primitive widths \f$\xi = \alpha\beta / (\alpha+\beta)\f$
	const Eigen::ArrayXXd& widthsReduced() const
	{
		return _shell_pair.widthsReduced();
	}
	/*!
	 * \brief \f\frac{$\exp(-\xi r^2)}{\alpha+\beta}\f$ with \f$r\f$ the
	 *    distance between the centers of the two orbitals.
	 */
	const Eigen::ArrayXXd& gaussReduced() const
	{
		return _shell_pair.gaussReduced();
	}
	//! Return half the inverse widths sums, \f$\frac{1}{2(\alpha+\beta)}\f$
	const Eigen::ArrayXXd& hInvWidths() const
	{
		return _shell_pair.hInvWidths();
	}
	//! Return the first orbital center
	const Eigen::Vector3d& centerA() const
	{
		return f().center();
	}
	//! Return the \a i coordinate of the first orbital center
	double centerA(int i) const
	{
		return f().center(i);
	}
	//! Return the second orbital center
	const Eigen::Vector3d& centerB() const
	{
		return g().center();
	}
	//! Return the \a i coordinate of the second orbital center
	double centerB(int i) const
	{
		return g().center(i);
	}
	//! Return the vector from the first to the second orbital center
	Eigen::Vector3d r() const
	{
		return g().center() - f().center();
	}
	//! Return the distance in the \a i direction between the two centers
	double r(int i) const
	{
		return g().center(i) - f().center(i);
	}
	/*!
	 * \brief Return the weighted average \a i coordinate, for each
	 *    combination of primitive widths.
	 */
	const Eigen::ArrayXXd& P(int i) const
	{
		return _shell_pair.P(i);
	}
	Eigen::ArrayXXd K() const
	{
		return Constants::sqrt_2_pi_5_4 * gaussReduced();
	}

	/*!
	 * \brief Create a new CGTOPair
	 *
	 * Create a new pairs of CGTOs. This pseudo-constructor provides a
	 * common interface that allows the Dispatcher to add pair creation
	 * functions for arbitrary pairs of basis function types.
	 * \param f The first orbital in the pair. Should be a CGTO.
	 * \param g The second orbital in the pair. Should be a CGTO.
	 */
	static AbstractBFPair *create(const AbstractBF& f, const AbstractBF& g)
	{
		try
		{
			const CGTO& ff = dynamic_cast< const CGTO& >(f);
			const CGTO& gg = dynamic_cast< const CGTO& >(g);
			return new CGTOPair(ff, gg);
		}
		catch (const std::bad_cast&)
		{
			throw Li::Exception("Invalid basis function type");
		}
	}

private:
	/*!
	 * Multiply the contributions to an integral \a C of each primitive pair
	 * with the weights of the primitives, and sum the results to compute
	 * the integral over the contraction.
	 * \param C the primitve integrals
	 */
	template <typename Derived>
	double mulWeights(const Eigen::ArrayBase<Derived>& C) const
	{
		return _shell_pair.mulWeights(C);
	}

	//! The shells for this pair of orbitals
	CGTOShellPair _shell_pair;

	//! Integrate the overlap matrix for this pair over dimension \a i.
	void overlapPrim1D(int i, Eigen::ArrayXXd& Sp) const;
	//! Integrate the overlap and kinetic energy matrices for this pair over dimension \a i.
	void oneElecPrim1D(int i, Eigen::ArrayXXd& Sp, Eigen::ArrayXXd& Tp) const;
	//! Integrate the coefficients for nuclear attraction integrals over dimension \a i.
	void nucAttrPrim1D(int i, const Eigen::ArrayXXd& theta,
		const Eigen::ArrayXXd& Pi, const Eigen::ArrayXXd& dPC,
		MultiArray& res) const;
};

#endif // CGTOPAIR_HH


#include "material_spectral_shapes.hpp"

#include <deal.II/lac/full_matrix.h>

namespace bart::acceleration::two_grid::spectral_shape {

MaterialSpectralShapes::MaterialSpectralShapes(std::unique_ptr<SpectralShapeCalculator> spectral_shape_calculator_ptr)
    : spectral_shape_calculator_ptr_(std::move(spectral_shape_calculator_ptr)) {
  AssertPointerNotNull(spectral_shape_calculator_ptr_.get(), "spectral shape calculator",
                       "MaterialSpectralShapesConstructor");
}
void MaterialSpectralShapes::CalculateMaterialSpectralShapes(std::shared_ptr<CrossSections> cross_sections_ptr) {
  using DealiiMatrix = dealii::FullMatrix<double>;
  const auto sigma_s_matrix_map{ cross_sections_ptr->sigma_s() };
  const auto sigma_t_vector_map{ cross_sections_ptr->sigma_t() };
  const int n_materials = sigma_t_vector_map.size();
  const int n_groups = sigma_t_vector_map.begin()->second.size();

  for (const auto& [material_id, sigma_t_vector] : sigma_t_vector_map) {
    DealiiMatrix sigma_t_matrix(n_groups, n_groups);
    for (int group = 0; group < n_groups; ++group) {
      sigma_t_matrix(group, group) = sigma_t_vector.at(group);
    }
    material_spectral_shapes_[material_id] =
        this->spectral_shape_calculator_ptr_->CalculateSpectralShape(sigma_t_matrix, sigma_s_matrix_map.at(material_id));
  }
}

} // namespace bart::acceleration::two_grid::spectral_shape


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

#include <boost/simd/arithmetic/functions/divs.hpp>
#include <boost/simd/include/functions/scalar/genmask.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/valmin.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/dispatch/meta/as_unsigned.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable: 4723) // potential divide by 0
#endif

  BOOST_DISPATCH_IMPLEMENT          ( divs_, tag::cpu_
                                    , (A0)
                                    , (scalar_< floating_<A0> >)
                                      (scalar_< floating_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return a0/a1;
    }
  };

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

  BOOST_DISPATCH_IMPLEMENT          ( divs_, tag::cpu_
                                    , (A0)
                                    , (scalar_< int_<A0> >)
                                      (scalar_< int_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename dispatch::meta::as_unsigned<A0>::type utype;
      A0 const aa0 = a0 + !((a1 + One<A0>()) | ((utype)a0 + Valmin<A0>()));
      if (a1)
        return aa0/a1;
      else if (a0)
        return Valmax<A0>() + ((utype)a0 >> (sizeof(A0)*CHAR_BIT-1));
      else
        return Zero<A0>();
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( divs_, tag::cpu_
                                    , (A0)
                                    , (scalar_< uint_<A0> >)
                                      (scalar_< uint_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return a1 ? a0/a1 : genmask(a0);
    }
  };
} } }

#endif