/*
 * Author: Benoit Sklenard benoit.sklenard@cea.fr
 *
 * 3D Poisson solver.
 * Neumann and Dirichlet boundary conditions are supported and read from Kernel::Mesh class.
 *
 * Matrix operations rely on boost::numeric::ublas library. Atlas library should be more appropriate.
 *
 * Conjugate gradient is used to solve linear system Ax = b
 * Newton-Raphson method is used to get self-consistent Thomas-Fermi/Poisson solution
 *
 * Copyright 2014 IMDEA Materials Institute, Getafe, Madrid, Spain
 *
 * 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.
 */

#include <iomanip>

#include "kernel/Domain.h"
#include "kernel/Mesh.h"
#include "kernel/Constants.h"

#include "io/Diagnostic.h"
#include "io/ParameterManager.h"
#include "io/FileParameters.h"
#include "io/Parameters.h"

#include "okmc/MobileParticleParam.h"

#include "lkmc/LatticeDiamondParam.h"

#include "FermiDirac.h"
#include <boost/lexical_cast.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <boost/timer.hpp>

#include "Poisson.h"

using namespace Kernel;
using namespace boost::numeric;
using namespace Electrostatics;

Poisson::Poisson(Tcl_Interp *pTcl, Domain *pDomain) {
	LOWMSG("Loading Poisson...");

	_pDomain            = pDomain;
	_pMesh              = pDomain->_pMesh;
	_pTcl               = pTcl;

	_T                  = 0;

	_nx                 = (_pMesh->getPeriodicX() ? _pMesh->getnx() - 1 : _pMesh->getnx());
	_ny                 = (_pMesh->getPeriodicY() ? _pMesh->getny() - 1 : _pMesh->getny());
	_nz                 = (_pMesh->getPeriodicZ() ? _pMesh->getnz() - 1 : _pMesh->getnz());

	_CG_tol             = Domains::global()->getFileParameters()->getFloat("MC/Electrostatic/cg.tolerance");       // 1e-6;
	_CG_maxIter         = Domains::global()->getFileParameters()->getInt("MC/Electrostatic/cg.max.iteration");     // 2000;
	_CG_verbose         = Domains::global()->getFileParameters()->getInt("MC/Electrostatic/cg.verbose");

	_NR_tol             = Domains::global()->getFileParameters()->getFloat("MC/Electrostatic/newton.tolerance");   // 1e-4;
	_NR_maxIter         = Domains::global()->getFileParameters()->getInt("MC/Electrostatic/newton.max.iteration"); // 50;
	_NR_verbose         = Domains::global()->getFileParameters()->getInt("MC/Electrostatic/newton.verbose");

	_FermiDirac         = Domains::global()->getFileParameters()->getBool("MC/Electrostatic/FermiDirac");
	_partialIonization  = Domains::global()->getFileParameters()->getBool("MC/Electrostatic/partial.ionization");
	_selfConsistentLKMC = Domains::global()->getFileParameters()->getBool("MC/Electrostatic/self.consistent.lkmc");

	_Qe                 = ublas::zero_vector<double>(_nx * _ny * _nz);
	_Qh                 = ublas::zero_vector<double>(_nx * _ny * _nz);
	_Qokmc              = ublas::zero_vector<double>(_nx * _ny * _nz);
	_psi                = ublas::zero_vector<double>(_nx * _ny * _nz);

	_Qlkmc_M            = ublas::zero_vector<double>(_nx * _ny * _nz);
	_Qlkmc_P            = ublas::zero_vector<double>(_nx * _ny * _nz);

	_Eg                 = ublas::zero_vector<double>(_nx * _ny * _nz);
	_Eg0                = ublas::zero_vector<double>(_nx * _ny * _nz);
	_Nc                 = ublas::zero_vector<double>(_nx * _ny * _nz);
	_Nv                 = ublas::zero_vector<double>(_nx * _ny * _nz);
	_DEc                = ublas::zero_vector<double>(_nx * _ny * _nz);
	_DEv                = ublas::zero_vector<double>(_nx * _ny * _nz);

	const IO::ParameterManager      *pPM  = Domains::global()->PM();
	const OKMC::MobileParticleParam *pMPP = _pDomain->_pMPPar;
	const LKMC::LatticeParam             **pLP  = _pDomain->_pLaPar;

	for(M_TYPE mt = 0; mt < pPM->getNMaterials(); ++mt)
	{
		// OKMC
		for(P_TYPE pt = 0; pt < pPM->getNParticles(); ++pt)
		{
			if(!pPM->isParticleDefined(pt, mt))
				continue;
			if (pMPP->_mapToGrid[mt][pt])
				_Nokmc[mt][pt] = ublas::zero_vector<double>(_nx * _ny * _nz);
		}
		// LKMC
		if (pLP[mt] != NULL && pLP[mt]->_mapToGrid)
			_Nlkmc[mt] = ublas::zero_vector<double>(_nx * _ny * _nz);
	}
}

Poisson::~Poisson() {
}

void Poisson::computeElectronicParameters(double TKelvin) {

	MeshNode           ***pNode = _pMesh->getNodes();
	IO::FileParameters   *pPar  = Domains::global()->getFileParameters();
	IO::ParameterManager *pPM   = Domains::global()->PM();

	for (Kernel::M_TYPE mt = 0; mt < Domains::global()->PM()->getNMaterials(); ++mt) {
		std::string path = pPM->getMaterialName(mt) + "/ElectronicStructure";

		if (!pPar->specified(path)) {
			WARNINGMSG(path << " does not exist!");
			_matParam[mt]._Nc  = 0;
			_matParam[mt]._Nv  = 0;
			_matParam[mt]._Eg  = 0;
			_matParam[mt]._Eg0 = 0;

			continue ;
		}
		std::string eDOSPath           = path + "/eDOSMass";
		std::string hDOSPath           = path + "/hDOSMass";
		std::string EgPath             = path + "/Bandgap";
		std::string EcDilatationalPath = path + "/Ec.dilatational";
		std::string EvDilatationalPath = path + "/Ev.dilatational";
		std::string EcDeviatoricPath   = path + "/Ec.deviatoric";
		std::string EvDeviatoricPath   = path + "/Ev.deviatoric";

		std::string T  = boost::lexical_cast<std::string>(_T);
		std::string T0 = boost::lexical_cast<std::string>(300);

		pPar->loadProcedure(_pTcl, eDOSPath, 1);
		pPar->loadProcedure(_pTcl, hDOSPath, 1);
		pPar->loadProcedure(_pTcl, EgPath, 1);

		double mdc = pPar->getFloatProc(_pTcl, eDOSPath, T);
		double mdv = pPar->getFloatProc(_pTcl, hDOSPath, T);

		_matParam[mt]._Nc  = 2. * pow(M0 * mdc * KB * _T / (2 * M_PI * pow(PLANCK_BAR, 2)), 3./2.);  // m3
		_matParam[mt]._Nv  = 2. * pow(M0 * mdv * KB * _T / (2 * M_PI * pow(PLANCK_BAR, 2)), 3./2.);  // m3
		_matParam[mt]._Eg  = ELECTRONVOLT_TO_HARTREE(pPar->getFloatProc(_pTcl, EgPath, T));     // Ha
		_matParam[mt]._Eg0 = ELECTRONVOLT_TO_HARTREE(pPar->getFloatProc(_pTcl, EgPath, T0));    // Ha

		if (pPar->specified(EcDilatationalPath) && pPar->specified(EvDilatationalPath)) {
			_matParam[mt]._Dc  = ublas::zero_vector<double>(3);
			_matParam[mt]._Dcx = ublas::zero_vector<double>(3);
			_matParam[mt]._Dcy = ublas::zero_vector<double>(3);
			_matParam[mt]._Dcz = ublas::zero_vector<double>(3);
			_matParam[mt]._Dv  = ublas::zero_vector<double>(2);
			_matParam[mt]._Dvb = ublas::zero_vector<double>(2);
			_matParam[mt]._Dvd = ublas::zero_vector<double>(2);

			std::map<std::string, float> Dc  = pPar->getFloatMap(EcDilatationalPath);
			std::map<std::string, float> Dcx = pPar->getFloatMap(EcDeviatoricPath + "(1)");
			std::map<std::string, float> Dcy = pPar->getFloatMap(EcDeviatoricPath + "(2)");
			std::map<std::string, float> Dcz = pPar->getFloatMap(EcDeviatoricPath + "(3)");

			std::map<std::string, float> Dv  = pPar->getFloatMap(EvDilatationalPath);
			std::map<std::string, float> Dvb = pPar->getFloatMap(EvDeviatoricPath + "(1)");
			std::map<std::string, float> Dvd = pPar->getFloatMap(EvDeviatoricPath + "(2)");

			for (unsigned i = 0; i < 3; ++i) {
				_matParam[mt]._Dc(i)  = Dc[boost::lexical_cast<std::string>(i + 1)];
				_matParam[mt]._Dcx(i) = Dcx[boost::lexical_cast<std::string>(i + 1)];
				_matParam[mt]._Dcy(i) = Dcy[boost::lexical_cast<std::string>(i + 1)];
				_matParam[mt]._Dcz(i) = Dcz[boost::lexical_cast<std::string>(i + 1)];
			}

			for (unsigned i = 0; i < 2; ++i) {
				_matParam[mt]._Dv(i)  = Dv[boost::lexical_cast<std::string>(i + 1)];
				_matParam[mt]._Dvb(i) = Dvb[boost::lexical_cast<std::string>(i + 1)];
				_matParam[mt]._Dvd(i) = Dvd[boost::lexical_cast<std::string>(i + 1)];
			}
			LOWMSG("got stress data for " << pPM->getMaterialName(mt));
		}
	}

	// initialize electronic parameters for the structure
	for (unsigned ix = 0; ix < _nx; ++ix) {
		for (unsigned iy = 0; iy < _ny; ++iy) {
			for (unsigned iz = 0; iz < _nz; ++iz) {
				if (pNode[ix][iy][iz]._active) {
					const unsigned idx = pNode[ix][iy][iz]._index;

					_Nc(idx)  = pNode[ix][iy][iz]._volume * 1e-27 * getNc(&pNode[ix][iy][iz]);
					_Nv(idx)  = pNode[ix][iy][iz]._volume * 1e-27 * getNv(&pNode[ix][iy][iz]);

					_Eg(idx)  = getEg(&pNode[ix][iy][iz]);
					_Eg0(idx) = getEg0(&pNode[ix][iy][iz]);

					if (_Eg(pNode[ix][iy][iz]._index) <= 0. || _Eg0(pNode[ix][iy][iz]._index) <= 0.)
						ERRORMSG("Poisson::computeElectronicParameters: band gap cannot be zero!");
				}
			}
		}
	}
}

void Poisson::setFirstGuess() {
	MeshNode ***pNode                     = _pMesh->getNodes();
	const IO::ParameterManager *pPM       = Domains::global()->PM();
	const OKMC::MobileParticleParam *pMPP = _pDomain->_pMPPar;

	for (size_t ix = 0; ix < _nx; ++ix) {
		for (size_t iy = 0; iy < _ny; ++iy) {
			for (size_t iz = 0; iz < _nz; ++iz) {
				if (pNode[ix][iy][iz]._active) {
					const unsigned i = pNode[ix][iy][iz]._index;
					double n = 0;

					for(M_TYPE mt = 0; mt < pPM->getNMaterials(); ++mt)
						for(P_TYPE pt = pPM->getNFamilies(); pt < pPM->getNParticles(); ++pt)
						{
							P_TYPE fam = pPM->getFamily(pt);
							P_POS  pos = pPM->getPPos(pt);
							if(fam == V_TYPE || fam == pPM->getMaterial(mt)._pt[0] || fam == pPM->getMaterial(mt)._pt[1] ||
								pos == POS_I || pos == POS_V)
								continue;
							if(!_Nokmc[mt][pt].empty() && _Nokmc[mt][pt](i) > 0)
								n += pMPP->_state2charge[mt][pt][0]; //impurities and dopants only have state 0!
						}

					if (n > 0.)
						_psi(i) = 0.5 * getEg(&pNode[ix][iy][iz]);
					else if (n < 0.)
						_psi(i) = - 0.5 * getEg(&pNode[ix][iy][iz]);
					else
						_psi(pNode[ix][iy][iz]._index) = 0;

					for(M_TYPE mt = 0; mt < pPM->getNMaterials(); ++mt) {
						if (!_Nlkmc[mt].empty() && _Nlkmc[mt](i) > 0) {
							_psi(i) = 0;
						}
					}
				}
			}
		}
	}
}

void Poisson::setT(double T)
{
	if (_T != T)
	{
		_T = T;
		MEDMSG("Building vector for OKMC particles...");
		buildOKMCVector();

		if (_selfConsistentLKMC)
			buildLKMCVector();

		MEDMSG("Building Laplacian matrix...");
		buildLaplacianMatrix();
		computeElectronicParameters(T);
		setFirstGuess();
	}
}

bool Poisson::compute() {
	MEDMSG("Building vector for OKMC particles...");
	buildOKMCVector();

	if (_selfConsistentLKMC)
		buildLKMCVector();

	MEDMSG("Solving non-linear Poisson equation...");

	if (! NewtonRaphson())
		ERRORMSG("Poisson::NewtonRaphson did not converged!");

	map2Nodes();
	map2Elements();

	return true;
}

double Poisson::getDEc(MeshNode *pNode) {
	const double kT = KB / Q * _T;

	std::set<MeshElement *> s;

	double DEc = 0;
	_pMesh->getElementsFromNode(pNode, s);

	for (std::set<MeshElement *>::const_iterator it = s.begin(); it != s.end(); ++it) {
		const M_TYPE mt = (*it)->getMaterial();

		// ublas::vector<double> DEci = ublas::zero_vector<double>(3);

		double tmp = 0;
		for (unsigned i = 0; i < 3; ++i) {
			// const ublas::vector<double> E = (*it)->strain();
			ublas::vector<double> E = ublas::zero_vector<double>(3);

			const double Dc         = (_matParam[mt]._Dc.size()  > 0 ? _matParam[mt]._Dc(i)  : 0);
			const double Dcx        = (_matParam[mt]._Dcx.size() > 0 ? _matParam[mt]._Dcx(i) : 0);
			const double Dcy        = (_matParam[mt]._Dcy.size() > 0 ? _matParam[mt]._Dcy(i) : 0);
			const double Dcz        = (_matParam[mt]._Dcz.size() > 0 ? _matParam[mt]._Dcz(i) : 0);

			tmp += exp(- (Dc * (E(0) + E(1) + E(2)) + Dcx * E(0) + Dcy * E(1) + Dcz * E(2))/ kT);
		}

		DEc += ELECTRONVOLT_TO_HARTREE(-kT * log(1./3. * tmp));

		if (Domains::global()->PM()->getMaterialName(mt) == "Silicon")
			LOWMSG("DEc:" << DEc / static_cast<double>(s.size()));
	}

	return (DEc / static_cast<double>(s.size()));
}

double Poisson::getNc(MeshNode *pNode) {
	std::set<MeshElement *> s;
	double Nc = 0;
	_pMesh->getElementsFromNode(pNode, s);

	for (std::set<MeshElement *>::const_iterator it = s.begin(); it != s.end(); ++it)
		Nc += _matParam[(*it)->getMaterial()]._Nc;

	return (Nc / static_cast<double>(s.size()));
}

double Poisson::getNv(MeshNode *pNode) {
	std::set<MeshElement *> s;
	double Nv = 0;
	_pMesh->getElementsFromNode(pNode, s);

	for (std::set<MeshElement *>::const_iterator it = s.begin(); it != s.end(); ++it)
		Nv += _matParam[(*it)->getMaterial()]._Nv;

	return (Nv / static_cast<double>(s.size()));
}

double Poisson::getEg(MeshNode *pNode) {
	std::set<MeshElement *> s;
	double Eg = 0;
	_pMesh->getElementsFromNode(pNode, s);

	for (std::set<MeshElement *>::const_iterator it = s.begin(); it != s.end(); ++it)
		Eg += _matParam[(*it)->getMaterial()]._Eg;

	return (Eg / static_cast<double>(s.size()));
}

double Poisson::getEg0(MeshNode *pNode) {
	std::set<MeshElement *> s;
	double Eg0 = 0;
	_pMesh->getElementsFromNode(pNode, s);

	for (std::set<MeshElement *>::const_iterator it = s.begin(); it != s.end(); ++it)
		Eg0 += _matParam[(*it)->getMaterial()]._Eg0;

	return (Eg0 / static_cast<double>(s.size()));
}

// OBSOLETE METHOD
// Do NOT call!
/* double Poisson::getLA(MeshNode *pNode) {
	double w = 0.;

	for (std::map<LKMC::LatticeAtom *, double>::iterator it = pNode->_mLA.begin(); it !=pNode->_mLA.end(); ++it)
		w += it->second;

	return w;
} */

void Poisson::map2Nodes() {
	MeshNode ***pNode = _pMesh->getNodes();

	for (unsigned ix = 0; ix < _nx; ++ix) {
		for (unsigned iy = 0; iy < _ny; ++iy) {
			for (unsigned iz = 0; iz < _nz; ++iz) {
				if (pNode[ix][iy][iz]._active) {
					pNode[ix][iy][iz]._V        = HARTREE_TO_ELECTRONVOLT(_psi(pNode[ix][iy][iz]._index));
					pNode[ix][iy][iz]._Eg       = HARTREE_TO_ELECTRONVOLT(_Eg(pNode[ix][iy][iz]._index));
					pNode[ix][iy][iz]._Eg0      = HARTREE_TO_ELECTRONVOLT(_Eg0(pNode[ix][iy][iz]._index));
					pNode[ix][iy][iz]._Ec       = HARTREE_TO_ELECTRONVOLT(0.5 * getEg(&pNode[ix][iy][iz]) - _psi(pNode[ix][iy][iz]._index));
					pNode[ix][iy][iz]._Ev       = HARTREE_TO_ELECTRONVOLT(- 0.5 * getEg(&pNode[ix][iy][iz]) - _psi(pNode[ix][iy][iz]._index));
					pNode[ix][iy][iz]._eDensity = _Qe(pNode[ix][iy][iz]._index) / (pNode[ix][iy][iz]._volume * 1e-21);
					pNode[ix][iy][iz]._hDensity = _Qh(pNode[ix][iy][iz]._index) / (pNode[ix][iy][iz]._volume * 1e-21);

					// pNode[ix][iy][iz]._N_okmc   = _Qokmc(pNode[ix][iy][iz]._index) / (pNode[ix][iy][iz]._volume * 1e-27); // FIXME

					if (ix > 0 && ix < (_nx - 1) && iy > 0 && iy < (_ny - 1) && iz > 0 && iz < (_nz - 1)) {
						if (pNode[ix-1][iy][iz]._index < 0)
							continue ;
						if (pNode[ix+1][iy][iz]._index < 0)
							continue ;
						if (pNode[ix][iy-1][iz]._index < 0)
							continue ;
						if (pNode[ix][iy+1][iz]._index < 0)
							continue ;
						if (pNode[ix][iy][iz-1]._index < 0)
							continue ;
						if (pNode[ix][iy][iz+1]._index < 0)
							continue ;

						const double dxp = pNode[ix+1][iy][iz]._coord._x - pNode[ix][iy][iz]._coord._x;
						const double dxm = pNode[ix][iy][iz]._coord._x - pNode[ix-1][iy][iz]._coord._x;
						const double dyp = pNode[ix][iy+1][iz]._coord._y - pNode[ix][iy][iz]._coord._y;
						const double dym = pNode[ix][iy][iz]._coord._y - pNode[ix][iy-1][iz]._coord._y;
						const double dzp = pNode[ix][iy][iz+1]._coord._z - pNode[ix][iy][iz]._coord._z;
						const double dzm = pNode[ix][iy][iz]._coord._z - pNode[ix][iy][iz-1]._coord._z;

						const double Vxp = HARTREE_TO_ELECTRONVOLT(_psi(pNode[ix+1][iy][iz]._index));
						const double Vxm = HARTREE_TO_ELECTRONVOLT(_psi(pNode[ix-1][iy][iz]._index));
						const double Vyp = HARTREE_TO_ELECTRONVOLT(_psi(pNode[ix][iy+1][iz]._index));
						const double Vym = HARTREE_TO_ELECTRONVOLT(_psi(pNode[ix][iy-1][iz]._index));
						const double Vzp = HARTREE_TO_ELECTRONVOLT(_psi(pNode[ix][iy][iz+1]._index));
						const double Vzm = HARTREE_TO_ELECTRONVOLT(_psi(pNode[ix][iy][iz-1]._index));

						const double V   = HARTREE_TO_ELECTRONVOLT(_psi(pNode[ix][iy][iz]._index));

						pNode[ix][iy][iz]._E    = ublas::zero_vector<double>(3); // electric field in eV/nm
						pNode[ix][iy][iz]._E(0) = - 0.5 * ((Vxp - V) / dxp + (V  - Vxm) / dxm);
						pNode[ix][iy][iz]._E(1) = - 0.5 * ((Vyp - V) / dyp + (V  - Vym) / dym);
						pNode[ix][iy][iz]._E(2) = - 0.5 * ((Vzp - V) / dzp + (V  - Vzm) / dzm);
					}
				}
			}
		}
	}
}

void Poisson::map2Elements() {
	for(Kernel::Mesh::iterator it = _pMesh->begin(); it != _pMesh->end(); ++it)
	{
		std::set<MeshNode *> s;
		const M_TYPE mt = it->getMaterial();

		_pMesh->getNodesFromElement(&(*it), s);

		if (s.empty())
			ERRORMSG("Poisson::map2Elements: cannot find nodes of element " << it->getIndex());

		double V  = 0;
		for (std::set<MeshNode *>::const_iterator i = s.begin(); i != s.end(); ++i)
			V  += (*i)->_V;

		it->electrostaticPotential() = V / static_cast<double>(s.size());
		it->bandGap() = HARTREE_TO_ELECTRONVOLT(_matParam[mt]._Eg);
	}
}

bool Poisson::NewtonRaphson() {
	MeshNode ***pNode                     = _pMesh->getNodes();
	const double kT                       = ELECTRONVOLT_TO_HARTREE(KB / Q * _T);
	const IO::ParameterManager *pPM       = Domains::global()->PM();
	const OKMC::MobileParticleParam *pMPP = _pDomain->_pMPPar;
	bool         ret                      = false;

	ublas::vector<double> deltaPsi  = ublas::zero_vector<double>(_nx * _ny * _nz);
	ublas::vector<double> err       = ublas::zero_vector<double>(_nx * _ny * _nz);
	ublas::vector<double> totCharge = ublas::zero_vector<double>(_nx * _ny * _nz);

	// ublas::vector<double> Qtmp      = ublas::zero_vector<double>(_nx * _ny * _nz);

	boost::timer t;
	t.restart();

	for (size_t it = 0; it < _NR_maxIter; ++it) {
		ublas::compressed_matrix<double> J = _laplacian;

		for (size_t ix = 0; ix < _nx; ++ix) {
			for (size_t iy = 0; iy < _ny; ++iy) {
				for (size_t iz = 0; iz < _nz; ++iz) {
					if (pNode[ix][iy][iz]._active) {
						const size_t i   = pNode[ix][iy][iz]._index;

						double dQe        = 0.;
						double dQh        = 0.;
						double dQokmc     = 0;
						double dQlkmc_M   = 0.;
						double dQlkmc_P   = 0.;

						const double Ef = 0;
						const double CBM = 0.5 * _Eg(i) - _psi(i);
						const double VBM = - 0.5 * _Eg(i) - _psi(i);

						const double EfEc = Ef - CBM;
						const double EvEf = VBM - Ef;
						// const double EfEc = _psi(i) - 0.5 * _Eg(i);
						// const double EvEf = - _psi(i) - 0.5 * _Eg(i);

						_Qokmc(i)   = 0;
						_Qlkmc_M(i) = 0.;
						_Qlkmc_P(i) = 0.;

						if (_FermiDirac) {
							dQe    = _Nc(i) / kT * FermiDirac::mhalf(EfEc / kT);
							dQh    = - _Nv(i) / kT * FermiDirac::mhalf(EvEf / kT);
							_Qe(i) = _Nc(i) * FermiDirac::phalf(EfEc / kT);
							_Qh(i) = _Nv(i) * FermiDirac::phalf(EvEf / kT);
						}
						else {
							dQe    = _Nc(i) / kT * exp(EfEc / kT);
							dQh    = - _Nv(i) / kT * exp(EvEf / kT);
							_Qe(i) = _Nc(i) * exp(EfEc / kT);
							_Qh(i) = _Nv(i) * exp(EvEf / kT);
						}

						for(M_TYPE mt = 0; mt < pPM->getNMaterials(); ++mt)
						{// OKMC
							for(P_TYPE pt = pPM->getNFamilies(); pt < pPM->getNParticles(); ++pt)
							{
								P_TYPE fam = pPM->getFamily(pt);
								P_POS  pos = pPM->getPPos(pt);
								if(fam == V_TYPE || fam == pPM->getMaterial(mt)._pt[0] || fam == pPM->getMaterial(mt)._pt[1] ||
										pos == POS_I || pos == POS_V)
									continue;
								if (! _Nokmc[mt][pt].empty() && _Nokmc[mt][pt](i) > 0)
								{
									if (pMPP->_state2charge[mt][pt][0] == 1) { //DONOR
										if (_partialIonization) {
											// const double E = _psi(i) - 0.5 * _Eg(i) + _Eg(i) / _Eg0(i) * ELECTRONVOLT_TO_HARTREE(pMPP->_stateEnergy[mt][pt]);
											const double E = Ef - (CBM - _Eg(i) / _Eg0(i) * ELECTRONVOLT_TO_HARTREE(pMPP->_stateEnergy[mt][pt]));
											_Qokmc(i) += _Nokmc[mt][pt](i) / (1. + pMPP->_stateDegeneracy[mt][pt] * exp(E / kT));
											dQokmc    += - _Nokmc[mt][pt](i) * pMPP->_stateDegeneracy[mt][pt] / kT * exp(E / kT) / pow(1. + pMPP->_stateDegeneracy[mt][pt] * exp(E / kT), 2);
										}
										else {
											_Qokmc(i) += _Nokmc[mt][pt](i);
										}
									}
									else if (pMPP->_state2charge[mt][pt][0] == -1) { //ACCEPTOR
										if (_partialIonization) {
											// const double E = - _psi(i) - 0.5 * _Eg(i) + _Eg(i) / _Eg0(i) * ELECTRONVOLT_TO_HARTREE(pMPP->_stateEnergy[mt][pt]);
											const double E = (VBM + _Eg(i) / _Eg0(i) * ELECTRONVOLT_TO_HARTREE(pMPP->_stateEnergy[mt][pt])) - Ef;
											_Qokmc(i) -= _Nokmc[mt][pt](i) / (1. + pMPP->_stateDegeneracy[mt][pt] * exp(E / kT));
											dQokmc    -= _Nokmc[mt][pt](i) * pMPP->_stateDegeneracy[mt][pt] / kT * exp(E / kT) / pow(1. + pMPP->_stateDegeneracy[mt][pt] * exp(E / kT), 2);
										}
										else {
											_Qokmc(i) -= _Nokmc[mt][pt](i);
										}
									}
								}
							}

							// LKMC
							/*
							if (_selfConsistentGFLS && _Nlkmc[mt].empty() == false && _Nlkmc[mt](i) > 0.) {
								if (_pDomain->_pLat[mt]->getType() != LKMC::LatticeParam::DIAMOND_1) {
									ERRORMSG("Poisson::NewtonRaphson does not support " << pPM->getMaterialName(mt) << " lattice.");
								}
								LKMC::LatticeDiamondParam *pLDP = static_cast<LKMC::LatticeDiamondParam *>(_pDomain->_pLaPar[mt]);

								const double E_lkmc_M = _psi(i) - 0.5 * _Eg(i) + _Eg(i) / _Eg0(i) * ELECTRONVOLT_TO_HARTREE(pLDP->_E_M0);   // Ef - E(0,-)
								const double E_lkmc_P = - _psi(i) - 0.5 * _Eg(i) + _Eg(i) / _Eg0(i) * ELECTRONVOLT_TO_HARTREE(pLDP->_E_P0); // E(+,0) - Ef

								const double g_M = pLDP->_g_M;
								const double g_0 = pLDP->_g_0;
								const double g_P = pLDP->_g_P;

								dQlkmc_M    +=   _Nc(i) / _Nlkmc[mt](i) * 1. / kT * g_M / g_0 * exp(E_lkmc_M / kT);
								dQlkmc_P    += - _Nv(i) / _Nlkmc[mt](i) * 1. / kT * g_P / g_0 * exp(E_lkmc_P / kT);
								_Qlkmc_M(i) += _Nc(i) / _Nlkmc[mt](i) * g_M / g_0 * exp(E_lkmc_M / kT);
								_Qlkmc_P(i) += _Nv(i) / _Nlkmc[mt](i) * g_P / g_0 * exp(E_lkmc_P / kT);
							}

							if (_selfConsistentGFLS2 && _Nlkmc[mt].empty() == false && _Nlkmc[mt](i) > 0.) {
								if (_pDomain->_pLat[mt]->getType() != LKMC::LatticeParam::DIAMOND_1) {
									ERRORMSG("Poisson::NewtonRaphson does not support " << pPM->getMaterialName(mt) << " lattice.");
								}
								LKMC::LatticeDiamondParam *pLDP = static_cast<LKMC::LatticeDiamondParam *>(_pDomain->_pLaPar[mt]);

								// const double Ef  = 0;
								// const double VBM = - V - 0.5 * Eg;

							    // Note: defect level scales with bandgap
								const double E_lkmc_M    = Ef - (VBM + _Eg(i) / _Eg0(i) * ELECTRONVOLT_TO_HARTREE(pLDP->_E_M0));      // Ef  - E(0,-)
								const double E_lkmc_P    = (VBM + _Eg(i) / _Eg0(i) * ELECTRONVOLT_TO_HARTREE(pLDP->_E_P0)) - Ef; // Ef  - E(0,-)

								const double g_M = pLDP->_g_M;
								const double g_0 = pLDP->_g_0;
								const double g_P = pLDP->_g_P;

								const double den_M = 1. + (g_0 / g_M) * exp( - E_lkmc_M / kT) + (g_P / g_M) * exp((E_lkmc_P - E_lkmc_M) / kT);
								const double den_P = 1. + (g_0 / g_P) * exp( - E_lkmc_P / kT) + (g_M / g_P) * exp((E_lkmc_M - E_lkmc_P) / kT);

								dQlkmc_M    +=   _Nlkmc[mt](i) * (1. / kT * (g_0 / g_M) * exp( - E_lkmc_M / kT) + 2. / kT * (g_P / g_M) * exp((E_lkmc_P - E_lkmc_M)/ kT)) / (den_M * den_M);
								dQlkmc_P    += - _Nlkmc[mt](i) * (1. / kT * (g_0 / g_P) * exp( - E_lkmc_P / kT) + 2. / kT * (g_M / g_P) * exp((E_lkmc_M - E_lkmc_P)/ kT)) / (den_P * den_P);
								_Qlkmc_M(i) += _Nlkmc[mt](i) / den_M;
								_Qlkmc_P(i) += _Nlkmc[mt](i) / den_P;
							}*/

							if (_selfConsistentLKMC && _Nlkmc[mt].empty() == false && _Nlkmc[mt](i) > 0.) {
								if (_pDomain->_pLat[mt]->getType() != LKMC::LatticeParam::DIAMOND) {
									ERRORMSG("Poisson::NewtonRaphson does not support " << pPM->getMaterialName(mt) << " lattice.");
								}
								const LKMC::LatticeDiamondParam *pLDP = static_cast<const LKMC::LatticeDiamondParam *>(_pDomain->_pLaPar[mt]);

								const double E_lkmc_M = _psi(i) - 0.5 * _Eg(i) + _Eg(i) / _Eg0(i) * ELECTRONVOLT_TO_HARTREE(pLDP->_E_M0);   // Ef - E(0,-)
								const double E_lkmc_P = - _psi(i) - 0.5 * _Eg(i) + _Eg(i) / _Eg0(i) * ELECTRONVOLT_TO_HARTREE(pLDP->_E_P0); // E(+,0) - Ef

								const double g_M = pLDP->_g_M;
								const double g_0 = pLDP->_g_0;
								const double g_P = pLDP->_g_P;

								const double den_M = 1. + (g_0 / g_M) * exp( - E_lkmc_M / kT) + (g_P / g_M) * exp((E_lkmc_P - E_lkmc_M) / kT);
								const double den_P = 1. + (g_0 / g_P) * exp( - E_lkmc_P / kT) + (g_M / g_P) * exp((E_lkmc_M - E_lkmc_P) / kT);

								dQlkmc_M    +=   _Nlkmc[mt](i) * (1. / kT * (g_0 / g_M) * exp( - E_lkmc_M / kT) + 2. / kT * (g_P / g_M) * exp((E_lkmc_P - E_lkmc_M)/ kT)) / (den_M * den_M);
								dQlkmc_P    += - _Nlkmc[mt](i) * (1. / kT * (g_0 / g_P) * exp( - E_lkmc_P / kT) + 2. / kT * (g_M / g_P) * exp((E_lkmc_M - E_lkmc_P)/ kT)) / (den_P * den_P);
								_Qlkmc_M(i) += _Nlkmc[mt](i) / den_M;
								_Qlkmc_P(i) += _Nlkmc[mt](i) / den_P;
							}
						}

						J(i,i) -= 4. * M_PI * (dQh - dQe + dQokmc + dQlkmc_P - dQlkmc_M);
					}
				}
			}
		}

		ublas::noalias(totCharge) = _Qh - _Qe + _Qokmc + _Qeff + _Qlkmc_P - _Qlkmc_M;
		ublas::noalias(err)       = - ublas::prod(_laplacian, _psi) + 4 * M_PI * totCharge;

		if (ublas::norm_2(err) < _NR_tol) {
			ret = true;
			if (_NR_verbose > 0)
				LOWMSG("Poisson::NewtonRaphson converged in " << it << " iterations (" << t.elapsed() << " s)");

			break ;
		}
		if (_NR_verbose > 1)
			LOWMSG("Poisson::NewtonRaphson iteration " << it << " error= " << ublas::norm_2(err) << " (" << t.elapsed() << " s)");

		deltaPsi = ublas::zero_vector<double>(_nx * _ny * _nz);

		if (! ConjugateGradient(J, deltaPsi, err))
			ERRORMSG("Poisson::CG did not converged");

		_psi += deltaPsi;
	}

	return ret;
}

bool Poisson::ConjugateGradient(const ublas::compressed_matrix<double> &A, ublas::vector<double> &x, const ublas::vector<double> &b) {
	boost::timer t;
	t.restart();

	ublas::vector<double> a;

	bool                  ret = false;
	ublas::vector<double> p   = ublas::zero_vector<double>(x.size());
	ublas::vector<double> r   = ublas::zero_vector<double>(x.size());

	ublas::noalias(r) = b - ublas::prod(A, x);
	ublas::noalias(p) = r;

	for (size_t it = 0; it < _CG_maxIter; ++it) {
		if (ublas::norm_2(r) < _CG_tol) {

			if (_CG_verbose > 0)
				LOWMSG("Poisson::CG solved in " << it << " iterations (" << t.elapsed() << " s)");

			ret = true;
			break ;
		}
		if (_CG_verbose > 1) {
			LOWMSG("Poisson::CG iteration " << it << " error= " << ublas::norm_2(r) << " (" << t.elapsed() << " s)");
		}

		a                 = ublas::zero_vector<double>(x.size());
		ublas::noalias(a) = ublas::prod(A, p);

		double  lambda = ublas::inner_prod(r, p) / ublas::inner_prod(a, p);

		x += lambda * p;
		r -= lambda * a;

		p = r - (ublas::inner_prod(r, a) / ublas::inner_prod(a, p)) * p;
	}

	return (ret);
}

// This method is for test purpose only
void Poisson::testPN() {
	MeshNode ***pNode = _pMesh->getNodes();

	for (size_t ix = 0; ix < _nx; ++ix)
		for (size_t iy = 0; iy < _ny; ++iy)
			for (size_t iz = 0; iz < _nz; ++iz)
				if (pNode[ix][iy][iz]._active) {
					if (ix < _nx / 2)
						_Qokmc(pNode[ix][iy][iz]._index) = .01;
					else
						_Qokmc(pNode[ix][iy][iz]._index) = -.005;

					if (ix == 0 || ix == _nx - 1 || iy == 0 || iy == _ny - 1 || iz == 0 || iz == _nz - 1)
						_Qokmc(pNode[ix][iy][iz]._index) *= .5;
				}
}

void Poisson::buildOKMCVector() {
	MeshNode ***pNode = _pMesh->getNodes();
	const IO::ParameterManager *pPM       = Domains::global()->PM();

	for(M_TYPE mt = 0; mt < pPM->getNMaterials(); ++mt) {
		for(P_TYPE pt = 0; pt < pPM->getNParticles(); ++pt) {
			if (!_Nokmc[mt][pt].empty()) {
				_Nokmc[mt][pt] = ublas::zero_vector<double>(_nx * _ny * _nz);
			}
		}
	}

	for (size_t ix = 0; ix < _nx; ++ix) {
		for (size_t iy = 0; iy < _ny; ++iy) {
			for (size_t iz = 0; iz < _nz; ++iz) {
				if (pNode[ix][iy][iz]._active && !pNode[ix][iy][iz]._mPart.empty()) {
					for (std::map<OKMC::Particle *, double>::const_iterator it = pNode[ix][iy][iz]._mPart.begin(); it != pNode[ix][iy][iz]._mPart.end(); ++it) {
						const OKMC::Particle * pPart = it->first;
						const M_TYPE mt        = pPart->getElement()->getMaterial();
						const P_TYPE pt        = pPart->getPType();
						if(_Nokmc[mt][pt].empty()) //particle not mapped.
							continue;
						_Nokmc[mt][pt](pNode[ix][iy][iz]._index) += it->second;
					}
				}
			}
		}
	}
}

void Poisson::buildLKMCVector() {
	MeshNode ***pNode = _pMesh->getNodes();
	const IO::ParameterManager *pPM       = Domains::global()->PM();

	for(M_TYPE mt = 0; mt < pPM->getNMaterials(); ++mt)
		if (!_Nlkmc[mt].empty())
		{
			_Nlkmc[mt] = ublas::zero_vector<double>(_nx * _ny * _nz);
			for (size_t ix = 0; ix < _nx; ++ix)
				for (size_t iy = 0; iy < _ny; ++iy)
					for (size_t iz = 0; iz < _nz; ++iz)
						if (pNode[ix][iy][iz]._active && !pNode[ix][iy][iz]._mLA.empty())
							for (std::map<LKMC::LatticeAtom *, double>::const_iterator it = pNode[ix][iy][iz]._mLA.begin(); it != pNode[ix][iy][iz]._mLA.end(); ++it)
								if (/*!it->first->getDefective() && */!it->first->getPerformed())
									_Nlkmc[mt](pNode[ix][iy][iz]._index) += it->second;
		}
}

double Poisson::getPermittivity(MeshNode *n1, MeshNode *n2, MeshNode *n3) {
	double eps = 0.;

	std::set<MeshElement *> s1;
	std::set<MeshElement *> s2;
	std::set<MeshElement *> s3;

	_pMesh->getElementsFromNode(n1, s1);
	_pMesh->getElementsFromNode(n2, s2);
	_pMesh->getElementsFromNode(n3, s3);

	for(std::set<MeshElement *>::const_iterator it = s1.begin(); it != s1.end(); ++it)
		if (s2.find(*it) != s2.end() && s3.find(*it) != s3.end()) {
			// eps = Domains::global()->PM()->getPermittivity((*it)->getMaterial());
			eps = Domains::global()->PM()->getMaterial((*it)->getMaterial())._permittivity;
			break ;
		}

	return eps;
}

void Poisson::buildLaplacianMatrix() {
	double dxm;
	double dxp;
	double dym;
	double dyp;
	double dzm;
	double dzp;

	MeshNode ***pNode = _pMesh->getNodes();

	_laplacian = ublas::compressed_matrix<double> (_nx * _ny * _nz, _nx * _ny * _nz);
	_Qeff      = ublas::zero_vector<double> (_nx * _ny * _nz);

	for (unsigned ix = 0; ix < _pMesh->getnx(); ++ix) {
		for (unsigned iy = 0; iy < _pMesh->getny(); ++iy) {
			for (unsigned iz = 0; iz < _pMesh->getnz(); ++iz) {

				if (pNode[ix][iy][iz]._active) {
					double a[6] = {};

					unsigned ixm = ix - 1;
					unsigned ixp = ix + 1;
					unsigned iym = iy - 1;
					unsigned iyp = iy + 1;
					unsigned izm = iz - 1;
					unsigned izp = iz + 1;

					// xm
					if (ix == 0) {
						ixm = (_pMesh->getPeriodicX() ? _nx-1 : ix);
						dxm = (_pMesh->getPeriodicX() ? pNode[_nx][iy][iz]._coord._x - pNode[_nx-1][iy][iz]._coord._x : 0);
					} else
						dxm = pNode[ix][iy][iz]._coord._x   - pNode[ixm][iy][iz]._coord._x;
					// xp
					if (ix == _nx - 1) {
						ixp = (_pMesh->getPeriodicX() ? 0 : _nx-1);
						dxp = (_pMesh->getPeriodicX() ? pNode[_nx][iy][iz]._coord._x - pNode[_nx-1][iy][iz]._coord._x : 0);
					}
					else
						dxp = pNode[ixp][iy][iz]._coord._x - pNode[ix][iy][iz]._coord._x;
					// ym
					if (iy == 0) {
						iym = (_pMesh->getPeriodicY() ? _ny-1 : iy);
						dym = (_pMesh->getPeriodicY() ? pNode[ix][_ny][iz]._coord._y - pNode[ix][_ny-1][iz]._coord._y : 0);
					} else
						dym = pNode[ix][iy][iz]._coord._y   - pNode[ix][iym][iz]._coord._y;
					// yp
					if (iy == _ny - 1) {
						iyp = (_pMesh->getPeriodicY() ? 0 : _ny-1);
						dyp = (_pMesh->getPeriodicY() ? pNode[ix][_ny][iz]._coord._y - pNode[ix][_ny-1][iz]._coord._y : 0);
					}
					else
						dyp = pNode[ix][iyp][iz]._coord._y - pNode[ix][iy][iz]._coord._y;
					// zm
					if (iz == 0) {
						izm = (_pMesh->getPeriodicZ() ? _nz-1 : iz);
						dzm = (_pMesh->getPeriodicZ() ? pNode[ix][iy][_nz]._coord._z - pNode[ix][iy][_nz-1]._coord._z : 0);
					} else
						dzm = pNode[ix][iy][iz]._coord._z   - pNode[ix][iy][izm]._coord._z;
					// zp
					if (iz == _nz - 1) {
						izp = (_pMesh->getPeriodicZ() ? 0 : _nz-1);
						dzp = (_pMesh->getPeriodicZ() ? pNode[ix][iy][_nz]._coord._z - pNode[ix][iy][_nz-1]._coord._z : 0);
					}
					else
						dzp = pNode[ix][iy][izp]._coord._z - pNode[ix][iy][iz]._coord._z;

					dxm /= BOHR_RADIUS * 1e9;
					dxp /= BOHR_RADIUS * 1e9;
					dym /= BOHR_RADIUS * 1e9;
					dyp /= BOHR_RADIUS * 1e9;
					dzm /= BOHR_RADIUS * 1e9;
					dzp /= BOHR_RADIUS * 1e9;

					if (ix != ixp) {
						const double eps1 = getPermittivity(&pNode[ixp][iy][iz], &pNode[ix][iyp][iz], &pNode[ix][iy][izp]);
						const double eps2 = getPermittivity(&pNode[ixp][iy][iz], &pNode[ix][iym][iz], &pNode[ix][iy][izp]);
						const double eps3 = getPermittivity(&pNode[ixp][iy][iz], &pNode[ix][iyp][iz], &pNode[ix][iy][izm]);
						const double eps4 = getPermittivity(&pNode[ixp][iy][iz], &pNode[ix][iym][iz], &pNode[ix][iy][izm]);

						a[0] = 1. / (4. * dxp) * (eps1 * dyp * dzp + eps2 * dym * dzp + eps3 * dyp * dzm + eps4 * dym * dzm);
					}
					if (ix != ixm) {
						const double eps1 = getPermittivity(&pNode[ixm][iy][iz], &pNode[ix][iyp][iz], &pNode[ix][iy][izp]);
						const double eps2 = getPermittivity(&pNode[ixm][iy][iz], &pNode[ix][iym][iz], &pNode[ix][iy][izp]);
						const double eps3 = getPermittivity(&pNode[ixm][iy][iz], &pNode[ix][iyp][iz], &pNode[ix][iy][izm]);
						const double eps4 = getPermittivity(&pNode[ixm][iy][iz], &pNode[ix][iym][iz], &pNode[ix][iy][izm]);

						a[1] = 1. / (4. * dxm) * (eps1 * dyp * dzp + eps2 * dym * dzp + eps3 * dyp * dzm + eps4 * dym * dzm);
					}
					if (iy != iyp) {
						const double eps1 = getPermittivity(&pNode[ix][iyp][iz], &pNode[ixp][iy][iz], &pNode[ix][iy][izp]);
						const double eps2 = getPermittivity(&pNode[ix][iyp][iz], &pNode[ixm][iy][iz], &pNode[ix][iy][izp]);
						const double eps3 = getPermittivity(&pNode[ix][iyp][iz], &pNode[ixp][iy][iz], &pNode[ix][iy][izm]);
						const double eps4 = getPermittivity(&pNode[ix][iyp][iz], &pNode[ixm][iy][iz], &pNode[ix][iy][izm]);

						a[2] = 1. / (4. * dyp) * (eps1 * dxp * dzp + eps2 * dxm * dzp + eps3 * dxp * dzm + eps4 * dxm * dzm);
					}
					if (iy != iym) {
						const double eps1 = getPermittivity(&pNode[ix][iym][iz], &pNode[ixp][iy][iz], &pNode[ix][iy][izp]);
						const double eps2 = getPermittivity(&pNode[ix][iym][iz], &pNode[ixm][iy][iz], &pNode[ix][iy][izp]);
						const double eps3 = getPermittivity(&pNode[ix][iym][iz], &pNode[ixp][iy][iz], &pNode[ix][iy][izm]);
						const double eps4 = getPermittivity(&pNode[ix][iym][iz], &pNode[ixm][iy][iz], &pNode[ix][iy][izm]);

						a[3] = 1. / (4. * dym) * (eps1 * dxp * dzp + eps2 * dxm * dzp + eps3 * dxp * dzm + eps4 * dxm * dzm);
					}
					if (iz != izp) {
						const double eps1 = getPermittivity(&pNode[ix][iy][izp], &pNode[ixp][iy][iz], &pNode[ix][iyp][iz]);
						const double eps2 = getPermittivity(&pNode[ix][iy][izp], &pNode[ixp][iy][iz], &pNode[ix][iym][iz]);
						const double eps3 = getPermittivity(&pNode[ix][iy][izp], &pNode[ixm][iy][iz], &pNode[ix][iyp][iz]);
						const double eps4 = getPermittivity(&pNode[ix][iy][izp], &pNode[ixm][iy][iz], &pNode[ix][iym][iz]);

						a[4] = 1. / (4. * dzp) * (eps1 * dyp * dxp + eps2 * dym * dxp + eps3 * dyp * dxm + eps4 * dym * dxm);
					}
					if (iz != izm) {
						const double eps1 = getPermittivity(&pNode[ix][iy][izm], &pNode[ixp][iy][iz], &pNode[ix][iyp][iz]);
						const double eps2 = getPermittivity(&pNode[ix][iy][izm], &pNode[ixp][iy][iz], &pNode[ix][iym][iz]);
						const double eps3 = getPermittivity(&pNode[ix][iy][izm], &pNode[ixm][iy][iz], &pNode[ix][iyp][iz]);
						const double eps4 = getPermittivity(&pNode[ix][iy][izm], &pNode[ixm][iy][iz], &pNode[ix][iym][iz]);

						a[5] = 1. / (4. * dzm) * (eps1 * dyp * dxp + eps2 * dym * dxp + eps3 * dyp * dxm + eps4 * dym * dxm);
					}

					if (!pNode[ixp][iy][iz]._active && _pMesh->isDirichletX())
						_Qeff(pNode[ix][iy][iz]._index) += a[0] * ELECTRONVOLT_TO_HARTREE(pNode[ixp][iy][iz]._V) / (4. * M_PI);
					if (!pNode[ixm][iy][iz]._active && _pMesh->isDirichletX())
						_Qeff(pNode[ix][iy][iz]._index) += a[1] * ELECTRONVOLT_TO_HARTREE(pNode[ixm][iy][iz]._V) / (4. * M_PI);

					if (!pNode[ix][iyp][iz]._active && _pMesh->isDirichletY())
						_Qeff(pNode[ix][iy][iz]._index) += a[2] * ELECTRONVOLT_TO_HARTREE(pNode[ix][iyp][iz]._V) / (4. * M_PI);
					if (!pNode[ix][iym][iz]._active && _pMesh->isDirichletY())
						_Qeff(pNode[ix][iy][iz]._index) += a[3] * ELECTRONVOLT_TO_HARTREE(pNode[ix][iym][iz]._V) / (4. * M_PI);

					if (!pNode[ix][iy][izm]._active && _pMesh->isDirichletZ())
						_Qeff(pNode[ix][iy][iz]._index) += a[4] * ELECTRONVOLT_TO_HARTREE(pNode[ix][iy][izp]._V) / (4. * M_PI);
					if (!pNode[ix][iy][izp]._active && _pMesh->isDirichletZ())
						_Qeff(pNode[ix][iy][iz]._index) += a[5] * ELECTRONVOLT_TO_HARTREE(pNode[ix][iy][izm]._V) / (4. * M_PI);

					_laplacian(pNode[ix][iy][iz]._index, pNode[ix][iy][iz]._index)      = a[0] + a[1] + a[2] + a[3] + a[4] + a[5];
					if (ix != ixp)
						_laplacian(pNode[ix][iy][iz]._index, pNode[ixp][iy][iz]._index) = - a[0];
					if (ix != ixm)
						_laplacian(pNode[ix][iy][iz]._index, pNode[ixm][iy][iz]._index) = - a[1];
					if (iy != iyp)
						_laplacian(pNode[ix][iy][iz]._index, pNode[ix][iyp][iz]._index) = - a[2];
					if (iy != iym)
						_laplacian(pNode[ix][iy][iz]._index, pNode[ix][iym][iz]._index) = - a[3];
					if (iz != izp)
						_laplacian(pNode[ix][iy][iz]._index, pNode[ix][iy][izp]._index) = - a[4];
					if (iz != izm)
						_laplacian(pNode[ix][iy][iz]._index, pNode[ix][iy][izm]._index) = - a[5];
				}
			}
		}
	}
}


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

    @copyright 2016 NumScale SAS

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

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

  /*!
    @ingroup group-trigonometric
    This function object returns the secant of the angle in degree:
    \f$1/\cos(\pi x/180)\f$.

    @see cosd, sec, secpi

    @par Header <boost/simd/function/secd.hpp>

    @par Example:

      @snippet secd.cpp secd

    @par Possible output:

      @snippet secd.txt secd

  **/
  IEEEValue secd(IEEEValue const& x);
} }
#endif

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

#endif


/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#ifndef PIC_FEATURES_MATCHING_SUSAN_CORNER_DETECTOR_HPP
#define PIC_FEATURES_MATCHING_SUSAN_CORNER_DETECTOR_HPP

#include "../util/vec.hpp"
#include "../util/std_util.hpp"
#include "../image.hpp"
#include "../filtering/filter_luminance.hpp"
#include "../filtering/filter_gaussian_2d.hpp"

#include "../features_matching/general_corner_detector.hpp"

#ifndef PIC_DISABLE_EIGEN

#ifndef PIC_EIGEN_NOT_BUNDLED
    #include "../externals/Eigen/Dense"
#else
    #include <Eigen/Dense>
#endif

#endif

namespace pic {

#ifndef PIC_DISABLE_EIGEN

/**
 * @brief The SusanCornerDetector class
 */
class SusanCornerDetector: public GeneralCornerDetector
{
protected:
    Image *lum_flt;
    bool  bComputeThreshold;

    float sigma, threshold;
    int   radius, radius_maxima;

    void release()
    {
        lum = delete_s(lum);
        lum_flt = delete_s(lum_flt);
    }

public:
    /**
     * @brief SusanCornerDetector
     */
    SusanCornerDetector() : GeneralCornerDetector()
    {
        lum_flt = NULL;

        bComputeThreshold = true;
        update();
    }

    ~SusanCornerDetector()
    {
        release();
    }

    /**
     * @brief update
     * @param sigma
     * @param radius_maxima
     * @param radius
     * @param threshold
     */
    void update(float sigma = 1.0f, int radius_maxima = 5, int radius = 3, float threshold = 0.001f)
    {
        this->sigma = sigma > 0.0f ? sigma : 1.0f;
        this->radius = radius > 0 ? radius : 3;
        this->threshold = threshold > 0.0f ? threshold : 0.001f;
        this->radius_maxima = radius_maxima > 0 ? radius_maxima : 5;
    }

    /**
     * @brief execute
     * @param img
     * @param corners
     */
    void execute(Image *img, std::vector< Eigen::Vector2f > *corners)
    {
        if(img == NULL || corners == NULL) {
            return;
        }

        if(img->channels == 1) {
            bLum = false;
            lum = img;
        } else {
            bLum = true;
            lum = FilterLuminance::execute(img, lum, LT_CIE_LUMINANCE);
        }

        corners->clear();

        std::vector< Eigen::Vector3f > corners_w_quality;

        //filter the input image
        FilterGaussian2D flt(sigma);
        lum_flt = flt.Process(Single(lum), NULL);

        //"rasterizing" a circle
        std::vector< int > x, y;
        int radius_sq = radius * radius;
        for(int i=-radius; i<=radius; i++) {
            int i_sq = i * i;
            for(int j=-radius; j<=radius; j++) {

                if((j == 0) && (i == 0)) {
                    continue;
                }

                int r_sq = i_sq + j * j;
                if(r_sq <= radius_sq){
                    x.push_back(j);
                    y.push_back(i);
                }
            }
        }

        int width  = lum_flt->width;
        int height = lum_flt->height;

        float C = float(x.size());

        float t = 0.05f; //depends on image noise

        float g = C * 0.5f; //geometric constant for determing corners

        Image R(1,width, height, 1);
        R.setZero();
        for(int i = radius; i < (height - radius - 1); i++) {
            for(int j = radius; j < (width - radius - 1); j++) {

                int ind = i * width + j;

                float sum = 0.0f;

                for(unsigned int k = 0; k < x.size(); k++) {
                    int ind_c = (i + y[k]) * width + (j + x[k]);

                    float diff = (lum_flt->data[ind_c] - lum_flt->data[ind]) / t;
                    float diff_2 = diff * diff;
                    float diff_4 = diff_2 * diff_2;
                    float diff_6 = diff_4 * diff_2;

                    sum += expf(-diff_6);
                }

                if(sum < g) {
                    R.data[ind] = g - sum;
                }
            }
        }

        //non-maximal supression
        int side = radius_maxima * 2 + 1;
        int *indices = new int [side * side];

        for(int i = radius_maxima; i< (height - radius_maxima - 1); i++) {

            int tmp = i * width;

            for(int j = radius_maxima; j < (width - radius_maxima - 1); j++) {
                int ind = tmp + j;

                if(R.data[ind] <= 0.0f) {
                    continue;
                }

                indices[0] = ind;
                int counter = 1;

                for(int k = -radius_maxima; k <= radius_maxima; k++) {
                    int yy = CLAMP(i + k, height);

                    for(int l = -radius_maxima; l <= radius_maxima; l++) {

                        if((l == 0) && (k == 0)) {
                            continue;
                        }

                        int xx = CLAMP(j + l, width);

                        ind = yy * width + xx;

                        if(R.data[ind]>0.0f){
                            indices[counter] = ind;
                            counter++;
                        }

                    }
                }

                //are other corners near-by?
                if(counter > 1) {
                    //find the maximum value
                    float R_value = R.data[indices[0]];
                    int index = 0;

                    for(int k = 1; k < counter; k++){
                        if(R.data[indices[k]] > R_value) {
                            R_value = R.data[indices[k]];
                            index = k;
                        }
                    }

                    if(index == 0){
                        corners_w_quality.push_back(Eigen::Vector3f (float(j), float(i), 1.0f) );
                    }
                } else {
                    corners_w_quality.push_back(Eigen::Vector3f (float(j), float(i), 1.0f) );
                }
            }
        }

        sortCornersAndTransfer(&corners_w_quality, corners);

        if(indices != NULL) {
            delete[] indices;
            indices = NULL;
        }
    }
};

#endif

} // end namespace pic

#endif /* PIC_FEATURES_MATCHING_SUSAN_CORNER_DETECTOR_HPP */


#include <fftw3.h>
#include <boost/timer/timer.hpp>
#include <boost/chrono.hpp>
#include <cmath>
#include <iostream>
#include <complex>
#include <Eigen/Dense>
#include <vector>
#include <boost/program_options.hpp>
#include <chirpz.h>

/*
  This is a simple stand-alone C++ benchmark of the C++
  library to show how you can use it without python
  and to provide a better framework for microbenchmarking.
 */

using namespace boost::program_options;
using namespace Eigen;

typedef std::complex<float> fc32_t;

const float PI = 3.1415926535;
const fc32_t J(0, 1);

int chirpz_benchmark(int argc, const char *argv[]) {
    options_description desc{"Options"};
    desc.add_options()
        ("help,h", "Help screen")
        ("N", value<int>()->default_value(256), "input point number")
        ("M", value<int>()->default_value(256), "output point number")
        ("iters", value<int>()->default_value(1000), "Number of transforms to do ")
        ;

    variables_map vm;
    store(parse_command_line(argc, argv, desc), vm);

    int N = vm["N"].as<int>();
    int M = vm["M"].as<int>();

    int iters = vm["iters"].as<int>();
    std::cout << "Running for " << iters << " iterations" << std::endl;

    float theta_start = -PI/2.0;
    float theta_delta = PI / (N)/2.0;

    fc32_t A = std::exp(J * theta_start);
    fc32_t W = std::exp(-J * theta_delta);

    boost::timer::cpu_timer setup_timer;
    chirpz::ChirpZ2d<chirpz::c32_t> cz(N, M, A, W) ;
    auto setup_ns = boost::chrono::nanoseconds(setup_timer.elapsed().wall);
    auto setup_s = boost::chrono::duration_cast<boost::chrono::seconds>(setup_ns);
    std::cout << setup_s << " for setup" << std::endl;

    {
        MatrixXcf x = MatrixXcf::Random(M, M);
        boost::timer::cpu_timer timer;
        for(int i = 0; i < iters; ++i) {
            auto y = cz.compute(x);
        }
        auto ns = boost::chrono::nanoseconds(timer.elapsed().wall);
        auto ms = boost::chrono::duration_cast<boost::chrono::milliseconds>(ns);

        std::cout << ms / double(iters) << " per eval" << std::endl;
    }

}

int main(int argc, const char *argv[]) {
    //spherical_bench();
    //fft_benchmark(argc, argv);

    chirpz_benchmark(argc, argv);

}


/**
 * \copyright
 * Copyright (c) 2012-2017, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 */

#include "LocalLinearLeastSquaresExtrapolator.h"

#include <Eigen/SVD>
#include <logog/include/logog.hpp>

#include "MathLib/LinAlg/Eigen/EigenMapTools.h"
#include "MathLib/LinAlg/LinAlg.h"
#include "MathLib/LinAlg/MatrixVectorTraits.h"
#include "NumLib/Assembler/SerialExecutor.h"
#include "NumLib/Function/Interpolation.h"
#include "ExtrapolatableElementCollection.h"

namespace NumLib
{
LocalLinearLeastSquaresExtrapolator::LocalLinearLeastSquaresExtrapolator(
    NumLib::LocalToGlobalIndexMap const& dof_table)
    : _nodal_values(NumLib::GlobalVectorProvider::provider.getVector(
          MathLib::MatrixSpecifications(dof_table.dofSizeWithoutGhosts(),
                                        dof_table.dofSizeWithoutGhosts(),
                                        &dof_table.getGhostIndices(),
                                        nullptr)))
#ifndef USE_PETSC
    , _residuals(dof_table.size())
#else
    , _residuals(dof_table.size(), false)
#endif
    , _local_to_global(dof_table)
{
    /* Note in case the following assertion fails:
     * If you copied the extrapolation code, for your processes from
     * somewhere, note that the code from the groundwater flow process might
     * not suit your needs: It is a special case and is therefore most
     * likely too simplistic. You better adapt the extrapolation code from
     * some more advanced process, like the TES process.
     */
    assert(dof_table.getNumberOfComponents() == 1 &&
           "The d.o.f. table passed must be for one variable that has "
           "only one component!");
}

void LocalLinearLeastSquaresExtrapolator::extrapolate(
    ExtrapolatableElementCollection const& extrapolatables)
{
    _nodal_values.setZero();

    // counts the writes to each nodal value, i.e., the summands in order to
    // compute the average afterwards
    auto counts =
        MathLib::MatrixVectorTraits<GlobalVector>::newInstance(_nodal_values);
    counts->setZero();  // TODO BLAS?

    auto const size = extrapolatables.size();
    for (std::size_t i=0; i<size; ++i) {
        extrapolateElement(i, extrapolatables, *counts);
    }

    MathLib::LinAlg::componentwiseDivide(_nodal_values, _nodal_values, *counts);
}

void LocalLinearLeastSquaresExtrapolator::calculateResiduals(
    ExtrapolatableElementCollection const& extrapolatables)
{
    assert(static_cast<std::size_t>(_residuals.size()) ==
           extrapolatables.size());

    auto const size = extrapolatables.size();
    for (std::size_t i=0; i<size; ++i) {
        calculateResidualElement(i, extrapolatables);
    }
}

void LocalLinearLeastSquaresExtrapolator::extrapolateElement(
    std::size_t const element_index,
    ExtrapolatableElementCollection const& extrapolatables,
    GlobalVector& counts)
{
    auto const& integration_point_values =
        extrapolatables.getIntegrationPointValues(
            element_index, _integration_point_values_cache);

    auto const& N_0 = extrapolatables.getShapeMatrix(element_index, 0);
    const unsigned num_nodes = static_cast<unsigned>(N_0.cols());
    const unsigned num_int_pts = integration_point_values.size();

    assert(num_int_pts >= num_nodes &&
           "Least squares is not possible if there are more nodes than"
           "integration points.");

    auto const pair_it_inserted = _qr_decomposition_cache.emplace(
        std::make_pair(num_nodes, num_int_pts), CachedData{});

    auto& cached_data = pair_it_inserted.first->second;
    if (pair_it_inserted.second)
    {
        DBUG("Computing new singular value decomposition");

        // interpolation_matrix * nodal_values = integration_point_values
        // We are going to pseudo-invert this relation now using singular value
        // decomposition.
        auto& interpolation_matrix = cached_data.A;
        interpolation_matrix.resize(num_int_pts, num_nodes);

        interpolation_matrix.row(0) = N_0;
        for (unsigned int_pt = 1; int_pt < num_int_pts; ++int_pt) {
            auto const& shp_mat =
                extrapolatables.getShapeMatrix(element_index, int_pt);
            assert(shp_mat.cols() == num_nodes);

            // copy shape matrix to extrapolation matrix row-wise
            interpolation_matrix.row(int_pt) = shp_mat;
        }

        // JacobiSVD is extremely reliable, but fast only for small matrices.
        // But we usually have small matrices and we don't compute very often.
        // Cf. http://eigen.tuxfamily.org/dox/group__TopicLinearAlgebraDecompositions.html
        //
        // Decomposes interpolation_matrix = U S V^T.
        Eigen::JacobiSVD<Eigen::MatrixXd> svd(
            interpolation_matrix, Eigen::ComputeThinU | Eigen::ComputeThinV);

        auto const& S = svd.singularValues();
        auto const& U = svd.matrixU();
        auto const& V = svd.matrixV();

        // Compute and save the pseudo inverse V * S^{-1} * U^T.
        auto const rank = svd.rank();
        assert(rank == num_nodes);

        // cf. http://eigen.tuxfamily.org/dox/JacobiSVD_8h_source.html
        cached_data.A_pinv.noalias() =
            V.leftCols(rank) *
            S.head(rank).asDiagonal().inverse() *
            U.leftCols(rank).transpose();
    }
    else if (cached_data.A.row(0) != N_0) {
        OGS_FATAL("The cached and the passed shapematrices differ.");
    }

    // TODO make gp_vals an Eigen::VectorXd const& ?
    auto const integration_point_values_vec =
        MathLib::toVector(integration_point_values);

    // Apply the pre-computed pseudo-inverse.
    Eigen::VectorXd const nodal_values =
        cached_data.A_pinv * integration_point_values_vec;

    // TODO: for now always zeroth component is used. This has to be extended if
    // multi-component properties shall be extrapolated
    auto const& global_indices = _local_to_global(element_index, 0).rows;

    // TODO does that give rise to PETSc problems?
    _nodal_values.add(global_indices, nodal_values);
    counts.add(global_indices, std::vector<double>(global_indices.size(), 1.0));
}

void LocalLinearLeastSquaresExtrapolator::calculateResidualElement(
    std::size_t const element_index,
    ExtrapolatableElementCollection const& extrapolatables)
{
    auto const& int_pt_vals = extrapolatables.getIntegrationPointValues(
        element_index, _integration_point_values_cache);

    // TODO: for now always zeroth component is used
    const auto& global_indices = _local_to_global(element_index, 0).rows;

    const unsigned num_int_pts = int_pt_vals.size();
    const unsigned num_nodes = global_indices.size();

    // filter nodal values of the current element
    Eigen::VectorXd nodal_vals_element(num_nodes);
    for (unsigned i = 0; i < num_nodes; ++i) {
        // TODO PETSc negative indices?
        nodal_vals_element[i] = _nodal_values[global_indices[i]];
    }

    auto const& interpolation_matrix =
        _qr_decomposition_cache.find({num_nodes, num_int_pts})->second.A;

    double const residual = (interpolation_matrix * nodal_vals_element -
                             MathLib::toVector(int_pt_vals))
                                .squaredNorm();

    _residuals.set(element_index, std::sqrt(residual / num_int_pts));
}

}  // namespace NumLib


#include <Python.h>
#include <iostream>
#include <Eigen/Dense>
#include <math.h>
#include <numpy/arrayobject.h>
#include "ID_Daw.h"

/*
This module uses IDDAW.cpp to find the energy of a set of interlayer dislocations
It can be built using setup_ID_DAW.py
and tested using test_IDDAW.py

https://dfm.io/posts/python-c-extensions/
*/

static char module_docstring[] =
    "This module provides an interface for calculating the Continuum interlayer energy.";
static char Energyf_sp_docstring[] =
    "Calculate the Energy of a set of interlayer Dislocations using Daw's formalism.";
static char visDuftot_docstring[] =
    "Calculate the Distortion field of a set of interlayer Dislocations using Daw's formalism.";
static char visuftot_docstring[] =
    "Calculate the in-plane displacement field of a set of interlayer Dislocations using Daw's formalism with stacking.";

static PyObject *IDDAW_Energyf_sp(PyObject *self, PyObject *args);
static PyObject *IDDAW_visDuftot(PyObject *self, PyObject *args);
static PyObject *IDDAW_visuftot(PyObject *self, PyObject *args);

MatrixXd unpacklocbur(double* , int);

static PyMethodDef module_methods[] = {
    {"_Energyf_sp", IDDAW_Energyf_sp, METH_VARARGS, Energyf_sp_docstring},
    {"_visDuftot", IDDAW_visDuftot, METH_VARARGS, visDuftot_docstring},
    {"_visuftot", IDDAW_visuftot, METH_VARARGS, visuftot_docstring},
    {NULL, NULL, 0, NULL}
};

PyMODINIT_FUNC init_IDDAW(void)
{
    PyObject *m = Py_InitModule3("_IDDAW", module_methods, module_docstring);
    if (m == NULL)
        return;

    /* Load `numpy` functionality. */
    import_array();
}

static PyObject *IDDAW_Energyf_sp(PyObject *self, PyObject *args)
{
    double rc, kap, c33, z0, alpha;
    int ndisl, ndislout, pmax, qmax, ncoor;
    PyObject *Cijkl_obj, *Lxy_obj, *a1_obj, *a2_obj, *loc_obj, *burgers_obj, *fG1_obj, *fG2_obj, *locout_obj, *dirout_obj, *burgersout_obj, *rcout_obj, *M_obj, *X_obj;

    /* Parse the input tuple */
    if (!PyArg_ParseTuple(args, "OOOOOOdiOOiiOOOOidddOiOd", &Cijkl_obj, &Lxy_obj, &a1_obj, &a2_obj,
        &loc_obj, &burgers_obj, &rc, &ndisl, &fG1_obj, &fG2_obj, &pmax, &qmax,
        &locout_obj, &dirout_obj, &burgersout_obj, &rcout_obj, &ndislout, &kap, &c33, &z0, &M_obj, &ncoor, &X_obj, &alpha))
        return NULL;

    /* Interpret the input objects as numpy arrays. */
    PyObject *Cijkl_array = PyArray_FROM_OTF(Cijkl_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *Lxy_array = PyArray_FROM_OTF(Lxy_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *a1_array = PyArray_FROM_OTF(a1_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *a2_array = PyArray_FROM_OTF(a2_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *loc_array = PyArray_FROM_OTF(loc_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *burgers_array = PyArray_FROM_OTF(burgers_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *fG1_array = PyArray_FROM_OTF(fG1_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *fG2_array = PyArray_FROM_OTF(fG2_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *locout_array = PyArray_FROM_OTF(locout_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *dirout_array = PyArray_FROM_OTF(dirout_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *burgersout_array = PyArray_FROM_OTF(burgersout_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *rcout_array = PyArray_FROM_OTF(rcout_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *M_array = PyArray_FROM_OTF(M_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *X_array = PyArray_FROM_OTF(X_obj, NPY_DOUBLE, NPY_IN_ARRAY);

    /* If that didn't work, throw an exception. */
    if (Cijkl_array == NULL || Lxy_array == NULL || a1_array == NULL ||
        a2_array == NULL || loc_array == NULL || burgers_array == NULL ||
        fG1_array == NULL || fG2_array == NULL || locout_array == NULL ||
        burgersout_array == NULL || dirout_array == NULL || M_array == NULL || X_array == NULL) {
        Py_XDECREF(Cijkl_array);
        Py_XDECREF(Lxy_array);
        Py_XDECREF(a1_array);
        Py_XDECREF(a2_array);
        Py_XDECREF(loc_array);
        Py_XDECREF(burgers_array);
        Py_XDECREF(fG1_array);
        Py_XDECREF(fG2_array);
        Py_XDECREF(locout_array);
        Py_XDECREF(dirout_array);
        Py_XDECREF(burgersout_array);
        Py_XDECREF(rcout_array);
        Py_XDECREF(M_array);
        Py_XDECREF(X_array);
        return NULL;
    }

    /* Get pointers to the data as C-types. */
    double *Cijklp    = (double*)PyArray_DATA(Cijkl_array);
    double *Lxyp    = (double*)PyArray_DATA(Lxy_array);
    double *a1p = (double*)PyArray_DATA(a1_array);
    double *a2p = (double*)PyArray_DATA(a2_array);
    double *locp = (double*)PyArray_DATA(loc_array);
    double *burgersp = (double*)PyArray_DATA(burgers_array);
    double *fG1p = (double*)PyArray_DATA(fG1_array);
    double *fG2p = (double*)PyArray_DATA(fG2_array);
    double *locoutp = (double*)PyArray_DATA(locout_array);
    double *diroutp = (double*)PyArray_DATA(dirout_array);
    double *burgersoutp = (double*)PyArray_DATA(burgersout_array);
    double *rcoutp = (double*)PyArray_DATA(rcout_array);
    double *Mp = (double*)PyArray_DATA(M_array);
    double *Xp = (double*)PyArray_DATA(X_array);

    cout << "pmax" << endl << pmax << endl;
    cout << "qmax" << endl << qmax << endl;

    //Convert Pointers into C Data Types
    MatrixXd C(2,8);
    C(0,0) = Cijklp[0]; C(0,1) = Cijklp[1]; C(0,2) = Cijklp[2]; C(0,3) = Cijklp[3];
    C(0,4) = Cijklp[4]; C(0,5) = Cijklp[5]; C(0,6) = Cijklp[6]; C(0,7) = Cijklp[7];
    C(1,0) = Cijklp[8]; C(1,1) = Cijklp[9]; C(1,2) = Cijklp[10]; C(1,3) = Cijklp[11];
    C(1,4) = Cijklp[12]; C(1,5) = Cijklp[13]; C(1,6) = Cijklp[14]; C(1,7) = Cijklp[15];

    MatrixXd burgers(ndisl,2);
    MatrixXd loc(ndisl,2);

    burgers = unpacklocbur(burgersp,ndisl);
    loc = unpacklocbur(locp,ndisl);

    MatrixXd burgersout(ndislout,2);
    MatrixXd locout(ndislout,2);
    MatrixXd dirout(ndislout,2);

    burgersout = unpacklocbur(burgersoutp,ndislout);
    locout = unpacklocbur(locoutp,ndislout);
    dirout = unpacklocbur(diroutp,ndislout);
    VectorXd rcout(2*ndislout);
    for (int k = 0; k<2*ndislout; k++){
        rcout(k) = rcoutp[k];
    }

    Vector2d Lxy(Lxyp[0],Lxyp[1]);
    Vector2d a1(a1p[0],a1p[1]);
    Vector2d a2(a2p[0],a2p[1]);

    Vector2d M(Mp[0],Mp[1]);

    MatrixXd fG1 = Map<Matrix<double,Dynamic,Dynamic,RowMajor> >(fG1p,2*pmax+1,2*qmax+1);
    MatrixXd fG2 = Map<Matrix<double,Dynamic,Dynamic,RowMajor> >(fG2p,2*pmax+1,2*qmax+1);

    MatrixXd X = Map<Matrix<double,Dynamic,Dynamic,RowMajor> >(Xp,ncoor,2);

    /*
    cout << "Cijkl" << endl << C << endl;
    cout << "kap" << endl << kap << endl;
    cout << "c33" << endl << c33 << endl;
    cout << "Lxy" << endl << Lxy << endl;
    cout << "a1" << endl << a1 << endl;
    cout << "a2" << endl << a2 << endl;
    cout << "fG1" << endl << fG1 << endl;
    cout << "fG2" << endl << fG2 << endl;
    cout << "loc" << endl << loc << endl;
    cout << "burgers" << endl << burgers << endl;
    cout << "rc" << endl << rc << endl;
    cout << "loc_out" << endl << locout << endl;
    cout << "dir_out" << endl << dirout << endl;
    cout << "burgers_out" << endl << burgersout << endl;
    cout << "rc_out" << endl << rcout << endl;
    cout << "pmax" << endl << pmax << endl;
    cout << "qmax" << endl << qmax << endl;
    cout << "z0" << endl << z0 << endl;
    cout << "M" << endl << M << endl;
    */
    Vector3d res = energyf_sp(C, kap, c33, Lxy, a1, a2, fG1, fG2, loc, burgers, rc, locout, dirout, burgersout, rcout, pmax, qmax, z0, M, ncoor, X, alpha);

    //double value = res[0];
    double ret_array[3];
    ret_array[0] = res[0];
    ret_array[1] = res[1];
    ret_array[2] = res[2];

    /* Clean up. */
    Py_XDECREF(Cijkl_array);
    Py_XDECREF(Lxy_array);
    Py_XDECREF(a1_array);
    Py_XDECREF(a2_array);
    Py_XDECREF(loc_array);
    Py_XDECREF(burgers_array);
    Py_XDECREF(fG1_array);
    Py_XDECREF(fG2_array);
    Py_XDECREF(locout_array);
    Py_XDECREF(dirout_array);
    Py_XDECREF(burgersout_array);
    Py_XDECREF(rcout_array);
    Py_XDECREF(M_array);
    Py_XDECREF(X_array);

    npy_intp dims[1] = {3};
    PyObject *ret = PyArray_SimpleNew(1, dims, NPY_DOUBLE);
    memcpy(PyArray_DATA(ret), ret_array, sizeof(ret_array));

    /* Build the output tuple */
    //PyObject *ret = Py_BuildValue("d", value);
    return ret;
}

static PyObject *IDDAW_visDuftot(PyObject *self, PyObject *args)
{
    double rc, kap, c33, z0, alpha;
    int ndisl, ndislout, pmax, qmax, ncoor;
    PyObject *Cijkl_obj, *Lxy_obj, *a1_obj, *a2_obj, *loc_obj, *burgers_obj, *fG1_obj, *fG2_obj, *locout_obj, *dirout_obj, *burgersout_obj, *rcout_obj, *M_obj, *X_obj;

    /* Parse the input tuple */
    if (!PyArg_ParseTuple(args, "OOOOOOdiOOiiOOOOidddOiOd", &Cijkl_obj, &Lxy_obj, &a1_obj, &a2_obj,
        &loc_obj, &burgers_obj, &rc, &ndisl, &fG1_obj, &fG2_obj, &pmax, &qmax,
        &locout_obj, &dirout_obj, &burgersout_obj, &rcout_obj, &ndislout, &kap, &c33, &z0, &M_obj, &ncoor, &X_obj, &alpha))
        return NULL;

    /* Interpret the input objects as numpy arrays. */
    PyObject *Cijkl_array = PyArray_FROM_OTF(Cijkl_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *Lxy_array = PyArray_FROM_OTF(Lxy_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *a1_array = PyArray_FROM_OTF(a1_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *a2_array = PyArray_FROM_OTF(a2_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *loc_array = PyArray_FROM_OTF(loc_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *burgers_array = PyArray_FROM_OTF(burgers_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *fG1_array = PyArray_FROM_OTF(fG1_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *fG2_array = PyArray_FROM_OTF(fG2_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *locout_array = PyArray_FROM_OTF(locout_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *dirout_array = PyArray_FROM_OTF(dirout_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *burgersout_array = PyArray_FROM_OTF(burgersout_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *rcout_array = PyArray_FROM_OTF(rcout_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *M_array = PyArray_FROM_OTF(M_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *X_array = PyArray_FROM_OTF(X_obj, NPY_DOUBLE, NPY_IN_ARRAY);

    /* If that didn't work, throw an exception. */
    if (Cijkl_array == NULL || Lxy_array == NULL || a1_array == NULL ||
        a2_array == NULL || loc_array == NULL || burgers_array == NULL ||
        fG1_array == NULL || fG2_array == NULL || locout_array == NULL ||
        burgersout_array == NULL || dirout_array == NULL || M_array == NULL || X_array == NULL) {
        Py_XDECREF(Cijkl_array);
        Py_XDECREF(Lxy_array);
        Py_XDECREF(a1_array);
        Py_XDECREF(a2_array);
        Py_XDECREF(loc_array);
        Py_XDECREF(burgers_array);
        Py_XDECREF(fG1_array);
        Py_XDECREF(fG2_array);
        Py_XDECREF(locout_array);
        Py_XDECREF(dirout_array);
        Py_XDECREF(burgersout_array);
        Py_XDECREF(rcout_array);
        Py_XDECREF(M_array);
        Py_XDECREF(X_array);
        return NULL;
    }

    /* Get pointers to the data as C-types. */
    double *Cijklp    = (double*)PyArray_DATA(Cijkl_array);
    double *Lxyp    = (double*)PyArray_DATA(Lxy_array);
    double *a1p = (double*)PyArray_DATA(a1_array);
    double *a2p = (double*)PyArray_DATA(a2_array);
    double *locp = (double*)PyArray_DATA(loc_array);
    double *burgersp = (double*)PyArray_DATA(burgers_array);
    double *fG1p = (double*)PyArray_DATA(fG1_array);
    double *fG2p = (double*)PyArray_DATA(fG2_array);
    double *locoutp = (double*)PyArray_DATA(locout_array);
    double *diroutp = (double*)PyArray_DATA(dirout_array);
    double *burgersoutp = (double*)PyArray_DATA(burgersout_array);
    double *rcoutp = (double*)PyArray_DATA(rcout_array);
    double *Mp = (double*)PyArray_DATA(M_array);
    double *Xp = (double*)PyArray_DATA(X_array);

    cout << "pmax" << endl << pmax << endl;
    cout << "qmax" << endl << qmax << endl;

    //Convert Pointers into C Data Types
    MatrixXd C(2,8);
    C(0,0) = Cijklp[0]; C(0,1) = Cijklp[1]; C(0,2) = Cijklp[2]; C(0,3) = Cijklp[3];
    C(0,4) = Cijklp[4]; C(0,5) = Cijklp[5]; C(0,6) = Cijklp[6]; C(0,7) = Cijklp[7];
    C(1,0) = Cijklp[8]; C(1,1) = Cijklp[9]; C(1,2) = Cijklp[10]; C(1,3) = Cijklp[11];
    C(1,4) = Cijklp[12]; C(1,5) = Cijklp[13]; C(1,6) = Cijklp[14]; C(1,7) = Cijklp[15];

    MatrixXd burgers(ndisl,2);
    MatrixXd loc(ndisl,2);

    burgers = unpacklocbur(burgersp,ndisl);
    loc = unpacklocbur(locp,ndisl);

    MatrixXd burgersout(ndislout,2);
    MatrixXd locout(ndislout,2);
    MatrixXd dirout(ndislout,2);

    burgersout = unpacklocbur(burgersoutp,ndislout);
    locout = unpacklocbur(locoutp,ndislout);
    dirout = unpacklocbur(diroutp,ndislout);
    VectorXd rcout(2*ndislout);
    for (int k = 0; k<2*ndislout; k++){
        rcout(k) = rcoutp[k];
    }

    Vector2d Lxy(Lxyp[0],Lxyp[1]);
    Vector2d a1(a1p[0],a1p[1]);
    Vector2d a2(a2p[0],a2p[1]);

    Vector2d M(Mp[0],Mp[1]);

    MatrixXd fG1 = Map<Matrix<double,Dynamic,Dynamic,RowMajor> >(fG1p,2*pmax+1,2*qmax+1);
    MatrixXd fG2 = Map<Matrix<double,Dynamic,Dynamic,RowMajor> >(fG2p,2*pmax+1,2*qmax+1);

    MatrixXd X = Map<Matrix<double,Dynamic,Dynamic,RowMajor> >(Xp,ncoor,2);

    /*
    cout << "Cijkl" << endl << C << endl;
    cout << "kap" << endl << kap << endl;
    cout << "c33" << endl << c33 << endl;
    cout << "Lxy" << endl << Lxy << endl;
    cout << "a1" << endl << a1 << endl;
    cout << "a2" << endl << a2 << endl;
    cout << "fG1" << endl << fG1 << endl;
    cout << "fG2" << endl << fG2 << endl;
    cout << "loc" << endl << loc << endl;
    cout << "burgers" << endl << burgers << endl;
    cout << "rc" << endl << rc << endl;
    cout << "loc_out" << endl << locout << endl;
    cout << "dir_out" << endl << dirout << endl;
    cout << "burgers_out" << endl << burgersout << endl;
    cout << "rc_out" << endl << rcout << endl;
    cout << "pmax" << endl << pmax << endl;
    cout << "qmax" << endl << qmax << endl;
    cout << "z0" << endl << z0 << endl;
    cout << "M" << endl << M << endl;
    */
    MatrixXd res = visD_uftot(C, kap, c33, Lxy, a1, a2, fG1, fG2, loc, burgers, rc, locout, dirout, burgersout, rcout, pmax, qmax, z0, M, ncoor, X, alpha);

    //cout << "Return: " << res << endl;
    //double value = res[0];

    double ret_array[8*ncoor];

    for (int i = 0; i < ncoor; i++){
        for (int j = 0; j < 8; j++){
            ret_array[8*i+j] = res(i,j);
        }
    }

    /* Clean up. */
    Py_XDECREF(Cijkl_array);
    Py_XDECREF(Lxy_array);
    Py_XDECREF(a1_array);
    Py_XDECREF(a2_array);
    Py_XDECREF(loc_array);
    Py_XDECREF(burgers_array);
    Py_XDECREF(fG1_array);
    Py_XDECREF(fG2_array);
    Py_XDECREF(locout_array);
    Py_XDECREF(dirout_array);
    Py_XDECREF(burgersout_array);
    Py_XDECREF(rcout_array);
    Py_XDECREF(M_array);
    Py_XDECREF(X_array);

    //cout << "Return: " << ret_array << endl;

    npy_intp dims[1] = {8*ncoor};
    PyObject *ret = PyArray_SimpleNew(1, dims, NPY_DOUBLE);
    memcpy(PyArray_DATA(ret), ret_array, sizeof(ret_array));

    /* Build the output tuple */
    //PyObject *ret = Py_BuildValue("d", value);
    return ret;
}

static PyObject *IDDAW_visuftot(PyObject *self, PyObject *args)
{
    double rc, kap, c33, z0, alpha;
    int ndisl, ndislout, pmax, qmax, ncoor;
    PyObject *Cijkl_obj, *Lxy_obj, *a1_obj, *a2_obj, *loc_obj, *burgers_obj, *fG1_obj, *fG2_obj, *locout_obj, *dirout_obj, *burgersout_obj, *rcout_obj, *M_obj, *X_obj;

    /* Parse the input tuple */
    if (!PyArg_ParseTuple(args, "OOOOOOdiOOiiOOOOidddOiOd", &Cijkl_obj, &Lxy_obj, &a1_obj, &a2_obj,
        &loc_obj, &burgers_obj, &rc, &ndisl, &fG1_obj, &fG2_obj, &pmax, &qmax,
        &locout_obj, &dirout_obj, &burgersout_obj, &rcout_obj, &ndislout, &kap, &c33, &z0, &M_obj, &ncoor, &X_obj, &alpha))
        return NULL;

    /* Interpret the input objects as numpy arrays. */
    PyObject *Cijkl_array = PyArray_FROM_OTF(Cijkl_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *Lxy_array = PyArray_FROM_OTF(Lxy_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *a1_array = PyArray_FROM_OTF(a1_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *a2_array = PyArray_FROM_OTF(a2_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *loc_array = PyArray_FROM_OTF(loc_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *burgers_array = PyArray_FROM_OTF(burgers_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *fG1_array = PyArray_FROM_OTF(fG1_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *fG2_array = PyArray_FROM_OTF(fG2_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *locout_array = PyArray_FROM_OTF(locout_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *dirout_array = PyArray_FROM_OTF(dirout_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *burgersout_array = PyArray_FROM_OTF(burgersout_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *rcout_array = PyArray_FROM_OTF(rcout_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *M_array = PyArray_FROM_OTF(M_obj, NPY_DOUBLE, NPY_IN_ARRAY);
    PyObject *X_array = PyArray_FROM_OTF(X_obj, NPY_DOUBLE, NPY_IN_ARRAY);

    /* If that didn't work, throw an exception. */
    if (Cijkl_array == NULL || Lxy_array == NULL || a1_array == NULL ||
        a2_array == NULL || loc_array == NULL || burgers_array == NULL ||
        fG1_array == NULL || fG2_array == NULL || locout_array == NULL ||
        burgersout_array == NULL || dirout_array == NULL || M_array == NULL || X_array == NULL) {
        Py_XDECREF(Cijkl_array);
        Py_XDECREF(Lxy_array);
        Py_XDECREF(a1_array);
        Py_XDECREF(a2_array);
        Py_XDECREF(loc_array);
        Py_XDECREF(burgers_array);
        Py_XDECREF(fG1_array);
        Py_XDECREF(fG2_array);
        Py_XDECREF(locout_array);
        Py_XDECREF(dirout_array);
        Py_XDECREF(burgersout_array);
        Py_XDECREF(rcout_array);
        Py_XDECREF(M_array);
        Py_XDECREF(X_array);
        return NULL;
    }

    /* Get pointers to the data as C-types. */
    double *Cijklp    = (double*)PyArray_DATA(Cijkl_array);
    double *Lxyp    = (double*)PyArray_DATA(Lxy_array);
    double *a1p = (double*)PyArray_DATA(a1_array);
    double *a2p = (double*)PyArray_DATA(a2_array);
    double *locp = (double*)PyArray_DATA(loc_array);
    double *burgersp = (double*)PyArray_DATA(burgers_array);
    double *fG1p = (double*)PyArray_DATA(fG1_array);
    double *fG2p = (double*)PyArray_DATA(fG2_array);
    double *locoutp = (double*)PyArray_DATA(locout_array);
    double *diroutp = (double*)PyArray_DATA(dirout_array);
    double *burgersoutp = (double*)PyArray_DATA(burgersout_array);
    double *rcoutp = (double*)PyArray_DATA(rcout_array);
    double *Mp = (double*)PyArray_DATA(M_array);
    double *Xp = (double*)PyArray_DATA(X_array);

    cout << "pmax" << endl << pmax << endl;
    cout << "qmax" << endl << qmax << endl;

    //Convert Pointers into C Data Types
    MatrixXd C(2,8);
    C(0,0) = Cijklp[0]; C(0,1) = Cijklp[1]; C(0,2) = Cijklp[2]; C(0,3) = Cijklp[3];
    C(0,4) = Cijklp[4]; C(0,5) = Cijklp[5]; C(0,6) = Cijklp[6]; C(0,7) = Cijklp[7];
    C(1,0) = Cijklp[8]; C(1,1) = Cijklp[9]; C(1,2) = Cijklp[10]; C(1,3) = Cijklp[11];
    C(1,4) = Cijklp[12]; C(1,5) = Cijklp[13]; C(1,6) = Cijklp[14]; C(1,7) = Cijklp[15];

    MatrixXd burgers(ndisl,2);
    MatrixXd loc(ndisl,2);

    burgers = unpacklocbur(burgersp,ndisl);
    loc = unpacklocbur(locp,ndisl);

    MatrixXd burgersout(ndislout,2);
    MatrixXd locout(ndislout,2);
    MatrixXd dirout(ndislout,2);

    burgersout = unpacklocbur(burgersoutp,ndislout);
    locout = unpacklocbur(locoutp,ndislout);
    dirout = unpacklocbur(diroutp,ndislout);
    VectorXd rcout(2*ndislout);
    for (int k = 0; k<2*ndislout; k++){
        rcout(k) = rcoutp[k];
    }

    Vector2d Lxy(Lxyp[0],Lxyp[1]);
    Vector2d a1(a1p[0],a1p[1]);
    Vector2d a2(a2p[0],a2p[1]);

    Vector2d M(Mp[0],Mp[1]);

    MatrixXd fG1 = Map<Matrix<double,Dynamic,Dynamic,RowMajor> >(fG1p,2*pmax+1,2*qmax+1);
    MatrixXd fG2 = Map<Matrix<double,Dynamic,Dynamic,RowMajor> >(fG2p,2*pmax+1,2*qmax+1);

    MatrixXd X = Map<Matrix<double,Dynamic,Dynamic,RowMajor> >(Xp,ncoor,2);

    /*
    cout << "Cijkl" << endl << C << endl;
    cout << "kap" << endl << kap << endl;
    cout << "c33" << endl << c33 << endl;
    cout << "Lxy" << endl << Lxy << endl;
    cout << "a1" << endl << a1 << endl;
    cout << "a2" << endl << a2 << endl;
    cout << "fG1" << endl << fG1 << endl;
    cout << "fG2" << endl << fG2 << endl;
    cout << "loc" << endl << loc << endl;
    cout << "burgers" << endl << burgers << endl;
    cout << "rc" << endl << rc << endl;
    cout << "loc_out" << endl << locout << endl;
    cout << "dir_out" << endl << dirout << endl;
    cout << "burgers_out" << endl << burgersout << endl;

    cout << "pmax" << endl << pmax << endl;
    cout << "qmax" << endl << qmax << endl;
    cout << "z0" << endl << z0 << endl;
    cout << "M" << endl << M << endl;
    */
    cout << "rc_out" << endl << rcout << endl;
    MatrixXd res = visuftot(C, kap, c33, Lxy, a1, a2, fG1, fG2, loc, burgers, rc, locout, dirout, burgersout, rcout, pmax, qmax, z0, M, ncoor, X, alpha);

    //cout << "Return: " << res << endl;
    //double value = res[0];

    double ret_array[4*ncoor];

    for (int i = 0; i < ncoor; i++){
        for (int j = 0; j < 4; j++){
            ret_array[4*i+j] = res(i,j);
        }
    }

    /* Clean up. */
    Py_XDECREF(Cijkl_array);
    Py_XDECREF(Lxy_array);
    Py_XDECREF(a1_array);
    Py_XDECREF(a2_array);
    Py_XDECREF(loc_array);
    Py_XDECREF(burgers_array);
    Py_XDECREF(fG1_array);
    Py_XDECREF(fG2_array);
    Py_XDECREF(locout_array);
    Py_XDECREF(dirout_array);
    Py_XDECREF(burgersout_array);
    Py_XDECREF(rcout_array);
    Py_XDECREF(M_array);
    Py_XDECREF(X_array);

    //cout << "Return: " << ret_array << endl;

    npy_intp dims[1] = {4*ncoor};
    PyObject *ret = PyArray_SimpleNew(1, dims, NPY_DOUBLE);
    memcpy(PyArray_DATA(ret), ret_array, sizeof(ret_array));

    /* Build the output tuple */
    //PyObject *ret = Py_BuildValue("d", value);
    return ret;
}

MatrixXd unpacklocbur(double* point, int ndisl){
	MatrixXd out(ndisl,2);
	for (int i=0;i<ndisl;i++){
		out(i,0) = point[i*2];
		out(i,1) = point[i*2+1];
	}

	return out;
}


//
// Copyright (c) 2015-2019 CNRS INRIA
//

#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/centroidal.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"

#include "pinocchio/parsers/sample-models.hpp"

#include "pinocchio/utils/timer.hpp"

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

template<typename JointModel>
static void addJointAndBody(pinocchio::Model & model,
                            const pinocchio::JointModelBase<JointModel> & joint,
                            const std::string & parent_name,
                            const std::string & name,
                            const pinocchio::SE3 placement = pinocchio::SE3::Random(),
                            bool setRandomLimits = true)
{
  using namespace pinocchio;
  typedef typename JointModel::ConfigVector_t CV;
  typedef typename JointModel::TangentVector_t TV;

  Model::JointIndex idx;

  if(setRandomLimits)
    idx = model.addJoint(model.getJointId(parent_name),joint,
                         SE3::Random(),
                         name + "_joint",
                         TV::Random() + TV::Constant(1),
                         TV::Random() + TV::Constant(1),
                         CV::Random() - CV::Constant(1),
                         CV::Random() + CV::Constant(1)
                         );
  else
    idx = model.addJoint(model.getJointId(parent_name),joint,
                         placement, name + "_joint");

  model.addJointFrame(idx);

  model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity());
  model.addBodyFrame(name + "_body", idx);
}

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

BOOST_AUTO_TEST_CASE(test_ccrba)
{
  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);
  pinocchio::Data data(model), data_ref(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill( 1.);
  Eigen::VectorXd q = randomConfiguration(model);
  Eigen::VectorXd v = Eigen::VectorXd::Ones(model.nv);

  crba(model,data_ref,q);
  data_ref.M.triangularView<Eigen::StrictlyLower>() = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();
  data_ref.Ycrb[0] = data_ref.liMi[1].act(data_ref.Ycrb[1]);

  pinocchio::SE3 cMo (pinocchio::SE3::Matrix3::Identity(), -getComFromCrba(model, data_ref));

  ccrba(model, data, q, v);
  BOOST_CHECK(data.com[0].isApprox(-cMo.translation(),1e-12));
  BOOST_CHECK(data.oYcrb[0].matrix().isApprox(data_ref.Ycrb[0].matrix(),1e-12));

  pinocchio::Inertia Ig_ref (cMo.act(data.oYcrb[0]));
  BOOST_CHECK(data.Ig.matrix().isApprox(Ig_ref.matrix(),1e-12));

  pinocchio::SE3 oM1 (data_ref.liMi[1]);
  pinocchio::SE3 cM1 (cMo * oM1);

  pinocchio::Data::Matrix6x Ag_ref (cM1.inverse().toActionMatrix().transpose() * data_ref.M.topRows <6> ());
  BOOST_CHECK(data.Ag.isApprox(Ag_ref,1e-12));
}

BOOST_AUTO_TEST_CASE(test_centroidal_mapping)
{
  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);
  pinocchio::Data data(model), data_ref(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill( 1.);
  Eigen::VectorXd q = randomConfiguration(model);
  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);

  computeCentroidalMap(model, data, q);
  ccrba(model,data_ref,q,v);

  BOOST_CHECK(data.J.isApprox(data_ref.J));
  BOOST_CHECK(data.Ag.isApprox(data_ref.Ag));

  computeJointJacobians(model,data_ref,q);

  BOOST_CHECK(data.J.isApprox(data_ref.J));
}

BOOST_AUTO_TEST_CASE(test_dccrb)
{
  using namespace pinocchio;
  Model model;
  buildModels::humanoidRandom(model);
  addJointAndBody(model,JointModelSpherical(),"larm6_joint","larm7");
  Data data(model), data_ref(model);

  model.lowerPositionLimit.head<7>().fill(-1.);
  model.upperPositionLimit.head<7>().fill(1.);

  Eigen::VectorXd q = randomConfiguration(model,model.lowerPositionLimit,model.upperPositionLimit);
  Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
  Eigen::VectorXd a = Eigen::VectorXd::Random(model.nv);

  const Eigen::VectorXd g = rnea(model,data_ref,q,0*v,0*a);
  rnea(model,data_ref,q,v,a);

  crba(model,data_ref,q);
  data_ref.M.triangularView<Eigen::StrictlyLower>() = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();

  SE3 cMo(SE3::Identity());
  cMo.translation() = -getComFromCrba(model, data_ref);

  SE3 oM1 (data_ref.liMi[1]);
  SE3 cM1 (cMo * oM1);
  Data::Matrix6x Ag_ref (cM1.toDualActionMatrix() * data_ref.M.topRows <6> ());

  Force hdot_ref(cM1.act(Force(data_ref.tau.head<6>() - g.head<6>())));

  ccrba(model,data_ref,q,v);
  dccrba(model,data,q,v);
  BOOST_CHECK(data.Ag.isApprox(Ag_ref));
  BOOST_CHECK(data.Ag.isApprox(data_ref.Ag));
  BOOST_CHECK(data.hg.isApprox(data_ref.hg));

  centerOfMass(model,data_ref,q,v,a);
  BOOST_CHECK(data_ref.vcom[0].isApprox(data.hg.linear()/data_ref.M(0,0)));
  BOOST_CHECK(data_ref.vcom[0].isApprox(data.vcom[0]));
  BOOST_CHECK(data_ref.acom[0].isApprox(hdot_ref.linear()/data_ref.M(0,0)));

  Force hdot(data.Ag * a + data.dAg * v);
  BOOST_CHECK(hdot.isApprox(hdot_ref));

  dccrba(model,data,q,0*v);
  BOOST_CHECK(data.dAg.isZero());

  // Check that dYcrb is equal to doYcrb
  {
    // Compute dYcrb
    Data data_ref(model), data_ref_plus(model), data(model);

    const double alpha = 1e-8;
    Eigen::VectorXd q_plus(model.nq);
    q_plus = integrate(model,q,alpha*v);

    forwardKinematics(model,data_ref,q);
    crba(model,data_ref,q);
    crba(model,data_ref_plus,q_plus);
    forwardKinematics(model,data_ref_plus,q_plus);
    dccrba(model,data,q,v);

    for(size_t i = 1; i < (size_t)model.njoints; ++i)
    {
      Inertia::Matrix6 dYcrb = (data_ref_plus.oMi[i].act(data_ref_plus.Ycrb[i]).matrix() -
                                data_ref.oMi[i].act(data_ref.Ycrb[i]).matrix())/alpha;
      BOOST_CHECK(data.doYcrb[i].isApprox(dYcrb,sqrt(alpha)));
    }
  }

  {
    Data data(model);
    ccrba(model,data_ref,q,v);
    SE3 oMc_ref(SE3::Identity());
    oMc_ref.translation() = data_ref.com[0];
    const Data::Matrix6x Ag_ref = oMc_ref.toDualActionMatrix() * data_ref.Ag;
    crba(model,data_ref,q);
    const Data::Matrix6x Ag_ref_from_M = data_ref.oMi[1].toDualActionMatrix() * data_ref.M.topRows<6>();

    const double alpha = 1e-8;
    Eigen::VectorXd q_plus(model.nq);
    q_plus = integrate(model,q,alpha*v);
    ccrba(model,data_ref,q_plus,v);
    SE3 oMc_ref_plus(SE3::Identity());
    oMc_ref_plus.translation() = data_ref.com[0];
    const Data::Matrix6x Ag_plus_ref = oMc_ref_plus.toDualActionMatrix() * data_ref.Ag;
    crba(model,data_ref,q_plus);
    const Data::Matrix6x Ag_plus_ref_from_M = data_ref.oMi[1].toDualActionMatrix() * data_ref.M.topRows<6>();
    const Data::Matrix6x dAg_ref = (Ag_plus_ref - Ag_ref)/alpha;
    const Data::Matrix6x dAg_ref_from_M = (Ag_plus_ref_from_M - Ag_ref_from_M)/alpha;

    dccrba(model, data, q, v);
    SE3 oMc(SE3::Identity());
    oMc.translation() = data.com[0];
    Data::Matrix6x dAg = oMc.toDualActionMatrix() * data.dAg;
    BOOST_CHECK(oMc.isApprox(oMc_ref));
    BOOST_CHECK(dAg.isApprox(dAg_ref,sqrt(alpha)));
    BOOST_CHECK(dAg.isApprox(dAg_ref_from_M,sqrt(alpha)));
  }

  // Compute tensor dAg/dq
  {
    std::vector<Data::Matrix6x> dAgdq((size_t)model.nv,Data::Matrix6x::Zero(6,model.nv));
    Data data(model), data_fd(model);
    Eigen::VectorXd v_fd(Eigen::VectorXd::Zero(model.nv));
    ccrba(model,data_fd,q,v);
    SE3 oMc_ref(SE3::Identity());
    oMc_ref.translation() = data_fd.com[0];

    Data::Matrix6x Ag0 = oMc_ref.toDualActionMatrix() * data_fd.Ag;
    const Force hg0 = oMc_ref.act(data_fd.hg);

    Data::Matrix6x Ag_fd(6,model.nv);
    Force hg_fd;
    const double alpha = 1e-8;
    Eigen::VectorXd q_plus(model.nq);
    Data::Matrix6x dhdq(6,model.nv);
    for(int k = 0; k < model.nv; ++k)
    {
      v_fd[k] = alpha;
      q_plus = integrate(model,q,v_fd);
      ccrba(model,data_fd,q_plus,v);
      SE3 oMc_fd(SE3::Identity());
      oMc_fd.translation() = data_fd.com[0];
      Ag_fd = oMc_fd.toDualActionMatrix() * data_fd.Ag;
      hg_fd = oMc_fd.act(data_fd.hg);
      dAgdq[(size_t)k] = (Ag_fd - Ag0)/alpha;
      dhdq.col(k) = (hg_fd - hg0).toVector()/alpha;
      v_fd[k] = 0.;
    }

    Data::Matrix6x dAg_ref(6,model.nv); dAg_ref.setZero();
    for(int k = 0; k < model.nv; ++k)
    {
      dAg_ref += dAgdq[(size_t)k] * v[k];
    }

    Data::Matrix6x dAg_ref_bis(6,model.nv); dAg_ref_bis.setZero();
    for(int k = 0; k < model.nv; ++k)
    {
      dAg_ref_bis.col(k) = dAgdq[(size_t)k] * v;
    }

    dccrba(model, data, q, v);
    SE3 oMc(SE3::Identity());
    oMc.translation() = data.com[0];
    Data::Matrix6x dAg = oMc.toDualActionMatrix() * data.dAg;
    BOOST_CHECK(dAg.isApprox(dAg_ref,sqrt(alpha)));
    BOOST_CHECK(dhdq.isApprox(dAg_ref_bis,sqrt(alpha)));
    BOOST_CHECK((dAg*v).isApprox(dhdq*v,sqrt(alpha)));

  }
}

BOOST_AUTO_TEST_CASE(test_centroidal_mapping_time_derivative)
{
  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);
  pinocchio::Data data(model), data_ref(model);

  model.lowerPositionLimit.head<3>().fill(-1.);
  model.upperPositionLimit.head<3>().fill( 1.);
  Eigen::VectorXd q = randomConfiguration(model);
  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);

  computeCentroidalMapTimeVariation(model, data, q, v);
  dccrba(model,data_ref,q,v);

  BOOST_CHECK(data.J.isApprox(data_ref.J));
  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
  BOOST_CHECK(data.Ag.isApprox(data_ref.Ag));
  BOOST_CHECK(data.dAg.isApprox(data_ref.dAg));

  computeJointJacobiansTimeVariation(model,data_ref,q,v);

  BOOST_CHECK(data.J.isApprox(data_ref.J));
  BOOST_CHECK(data.dJ.isApprox(data_ref.dJ));
}

BOOST_AUTO_TEST_CASE(test_computeCentroidalMomentum_computeCentroidalMomentumTimeVariation)
{
  using namespace pinocchio;
  Model model;
  buildModels::humanoidRandom(model);
  addJointAndBody(model,JointModelSpherical(),"larm6_joint","larm7");
  Data data(model), data_fk1(model), data_fk2(model), data_ref(model);

  model.lowerPositionLimit.head<7>().fill(-1.);
  model.upperPositionLimit.head<7>().fill( 1.);

  Eigen::VectorXd q = randomConfiguration(model,model.lowerPositionLimit,model.upperPositionLimit);
  Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv);
  Eigen::VectorXd a = Eigen::VectorXd::Random(model.nv);

  ccrba(model,data_ref,q,v);
  forwardKinematics(model,data_ref,q,v);
  centerOfMass(model,data_ref,q,v,false);
  computeCentroidalMomentum(model,data,q,v);

  BOOST_CHECK(data.mass[0] == data_ref.mass[0]);
  BOOST_CHECK(data.com[0].isApprox(data_ref.com[0]));
  BOOST_CHECK(data.hg.isApprox(data_ref.hg));
  for(size_t k = 1; k < (size_t)model.njoints; ++k)
  {
    BOOST_CHECK(data.mass[k] == data_ref.mass[k]);
    BOOST_CHECK(data.com[k].isApprox(data_ref.com[k]));
    BOOST_CHECK(data.v[k].isApprox(data_ref.v[k]));
  }

  // Check other signature
  forwardKinematics(model,data_fk1,q,v);
  computeCentroidalMomentum(model,data_fk1);

  BOOST_CHECK(data_fk1.hg.isApprox(data.hg));

  computeCentroidalMomentumTimeVariation(model,data,q,v,a);
  model.gravity.setZero();
  rnea(model,data_ref,q,v,a);
  dccrba(model,data_ref,q,v);
  const Force hgdot(data_ref.Ag * a + data_ref.dAg * v);

  BOOST_CHECK(data.mass[0] == data_ref.mass[0]);
  BOOST_CHECK(data.com[0].isApprox(data_ref.com[0]));
  BOOST_CHECK(data.hg.isApprox(data_ref.hg));
  BOOST_CHECK(data.dhg.isApprox(hgdot));
  for(size_t k = 1; k < (size_t)model.njoints; ++k)
  {
    BOOST_CHECK(data.mass[k] == data_ref.mass[k]);
    BOOST_CHECK(data.com[k].isApprox(data_ref.com[k]));
    BOOST_CHECK(data.v[k].isApprox(data_ref.v[k]));
    BOOST_CHECK(data.a[k].isApprox(data_ref.a_gf[k]));
    BOOST_CHECK(data.f[k].isApprox(data_ref.f[k]));
  }

  // Check other signature
  forwardKinematics(model,data_fk2,q,v,a);
  computeCentroidalMomentumTimeVariation(model,data_fk2);

  BOOST_CHECK(data_fk2.hg.isApprox(data.hg));
  BOOST_CHECK(data_fk2.dhg.isApprox(data.dhg));

  // Check against finite differences
  Data data_fd(model);
  const double eps = 1e-8;
  Eigen::VectorXd v_plus = v + eps * a;
  Eigen::VectorXd q_plus = integrate(model,q,eps*v);

  const Force hg = computeCentroidalMomentum(model,data_fd,q,v);
  const SE3::Vector3 com = data_fd.com[0];
  const Force hg_plus = computeCentroidalMomentum(model,data_fd,q_plus,v_plus);
  const SE3::Vector3 com_plus = data_fd.com[0];

  SE3 transform(SE3::Identity());
  transform.translation() = com_plus - com;
  Force dhg_ref = (transform.act(hg_plus) - hg)/eps;

  BOOST_CHECK(data.dhg.isApprox(dhg_ref,sqrt(eps)));
}

BOOST_AUTO_TEST_SUITE_END()


#include <NTL/ZZ.h>
#include <NTL/ZZ_pXFactoring.h>
#include "nsgen.h"
#include "ring.h"

void print_poly (ZZX& f) {
  long d = deg(f);
  if (d == -1) {
    cout << "0" << endl;
  }
  else {
    for (long i = d; i >= 0; --i) {
      if (f[i] != 0)
	cout << f[i] << "*" << "X^" << i << " + ";
    }
    cout << endl;
  }
}

/***********************************************************************/
ZZX ring::apply_conj (ZZX& a, long i) {
  long d = deg(a);
  if (d == -1) return ZZX(0);

  ZZX phi = (ZZX(INIT_MONO, mR)-1)/(ZZX(INIT_MONO, 1)-1);
  ZZX w = ZZX(0);

  for (long j = 0; j <= d; j++) {
    if (a[j] != 0) {
      long t = (j*i) % mR;
      ZZX z = ZZX(INIT_MONO, t);
      rem(z, z, phi);
      z *= ZZX(a[j]);
      w += z;
    }
  }

  return w;
}

ZZX ring::trace (ZZX& w) {
  ZZX r = ZZX(0);

  for (long a = 0; a < dR; ++a) {
    ZZX v = apply_conj(w, GZ(a));
    r += v;
  }
  return r;
}

ZZX ring::eta (long j) {
  ZZX z = ZZX(INIT_MONO, 1);
  ZZX phi = (ZZX(INIT_MONO, mR)-1)/(ZZX(INIT_MONO, 1)-1);

  long t = GofZ(j);
  ZZX w = ZZX(INIT_MONO, t);  // w = X^t
  rem(w, w, phi);

  return trace(w);
}

ZZ_pX ring::delta (long j) {
  return conv<ZZ_pX>(eta(j) - dR);
}

ZZ ring::scalar_quo (ZZX& a, ZZX& b, ZZ& p) {
  long d = max(deg(a), deg(b));
  a.SetLength(d+1);
  b.SetLength(d+1);

  // ZZ_pContext context;
  // context.save();
  ZZ_p::init(p);

  ZZ_pX a1, b1;
  a1.SetLength(d+1);
  a1 = conv<ZZ_pX>(a);
  b1.SetLength(d+1);
  b1 = conv<ZZ_pX>(b);

  long i;
  ZZ w;
  for (i = 0; i <= d; ++i) {
    if (GCD(b[i]%p, p) == 1) {
      w = (a[i] * InvMod(b[i]%p, p)) % p;
      break;
    }
  }

  // ZZ_pX c1 = conv<ZZ_pX>(w) * b1;
  // if (c1 != a1) {
  //   cout << "Error: unexpected behaviour in scalar_quo" << endl;
  //   cout << "i = " << i << ", p = " << p << endl;
  //   // cout << "a1 = " << a1 << endl;
  //   // cout << "b1 = " << b1 << endl;
  //   // cout << "c1 = " << c1 << endl;
  //   exit(-1);
  // }

  //context.restore();
  return w;
}

/***********************************************************************/
ZZ compose_integer (const ZZ& r1, const ZZ& q1, const ZZ& r2, const ZZ& q2) {
  ZZ a, b, c;
  XGCD(a, b, c, q1, q2);  // 1 = a = b*q1 + c*q2
  ZZ w1 = c * q2;
  ZZ w2 = b * q1;
  return (r1*w1 + r2*w2) % (q1*q2);
}

void compose_vector (vec_ZZ& omega, vec_ZZ& omega1, ZZ& q1, vec_ZZ& omega2, ZZ& q2) {
  for (long i = 0; i < omega1.length(); i++)
    omega[i] = compose_integer(omega1[i], q1, omega2[i], q2);
}

void compose (ZZX& res, ring& rg, ZZX& res0, ZZ& q0, ZZX& res1, ZZ& q1) {
  vec_ZZ v0 = VectorCopy(res0, rg.mR-1);
  vec_ZZ v1 = VectorCopy(res1, rg.mR-1);
  vec_ZZ v; v.SetLength(rg.mR-1);
  compose_vector(v, v0, q0, v1, q1);
  res.SetLength(rg.mR-1);
  for (long i = 0; i < rg.mR-1; i++) SetCoeff(res, i, v[i]);
}

// assumes that 'm' is prime
// modulus of the resolution is p^r
void ring::resolution_of_one (ZZX& res, const long m, const ZZ& p) {
  ZZ_pContext context;
  context.save();
  ZZ_p::init(p);

  ZZ_pX phi = (ZZ_pX(INIT_MONO, m)-1)/(ZZ_pX(INIT_MONO, 1)-1);

  cout << "." << flush;

  vec_ZZ_pX factors;
  //SFBerlekamp(factors, phi);
  if (p==ZZ(2))
    SFCanZass(factors, phi);
  else
    RootEDF(factors, phi);

  ZZ_pX G = ZZ_pX(1);
  for (long i = 1; i < factors.length(); ++i) G *= factors[i];
  ZZ_pX a, b, c;
  XGCD(a, b, c, factors[0], G); // 1 = a = b * factors[0] + c * G
  ZZ_pX res1 = c*G;

  // ZZ_pX res2 = (res1*res1) % phi;
  // if (res1 != res2) {
  //   cout << "Error in resolution_of_one" << endl;
  //   cout << "res   = " << res1 << endl;
  //   cout << "res2  = " << res2 << endl;
  //   exit(-1);
  // }

  res = conv<ZZX>(res1);
  context.restore();
}

void ring::lift_resolution (ZZX& res, const long m, const ZZ& r, const long l) {
  ZZ_p::init(power(r, l));
  ZZ_pX phi = (ZZ_pX(INIT_MONO, m)-1)/(ZZ_pX(INIT_MONO, 1)-1);

  ZZ_pX x = conv<ZZ_pX>(-res);
  for (long j = 0; j < l-1; ++j) {
    if ((j % 100) == 0) cout << "." << flush;
    x = (power(x+1, conv<long>(r)) - 1) % phi;
  }
  res = conv<ZZX>(-x);
}

void ring::canonical_resolution_of_one (ZZX& res, long prec) {
  resolution_of_one(res, mR, ZZ(pR));
  lift_resolution(res, mR, ZZ(pR), prec);
}

void ring::init_resolution (long prec) {
  if (type == prime) {
    canonical_resolution_of_one(res, prec);
  }
  else { // ring is composite
    if (left->initialized == false)
      left->init_resolution(prec);
    if (right->initialized == false)
      right->init_resolution(prec);
  }
}

void ring::init (long prec, noise& ns) {
  if (initialized == true) return;

  cout << "Initializing ring " << label << flush;

  init_resolution(prec);

  cout << "computing omega..." << flush;
  init_omega(power(ZZ(2), prec));

  depth = get_subrings(subrings);

  initialized = true;
  cout << "done" << endl;
}

/***********************************************************************/
// Transpose a (m*n)-dim. vector 'a'
// into a m x n matrix (m-dim vector of n-dim vector)'w'
void mat (Vec<double*>& aa, const double* a, const long n, const long m) {
  for (long j = 0; j < m; ++j)
    for (long i = 0; i < n; ++i)
      aa[j][i] = a[i + n*j];
}

void transpose (Vec<double*>& b, Vec<double*>& a) {
  long m = a.length();
  long n = b.length();

  for (long i = 0; i < m; ++i)
    for (long j = 0; j < n; ++j)
      b[j][i] = a[i][j];
}

void vec (double* b, const Vec<double*>& a, long n) {
  long m = a.length();

  for (long i = 0; i < m; ++i)
    for (long j = 0; j < n; ++j)
      b[i*n+j] = a[i][j];
}

/***************************************************************************/
// (Omega)_{i,j} = (rho_{t_i}(eta_j))
void ring::comp_omega (vec_ZZ& o, ZZ& q) {
  ZZX phi = (ZZX(INIT_MONO, mR)-1)/(ZZX(INIT_MONO, 1)-1);

  ZZ_p::init(q);

  o.SetLength(gR);
  for (long i = 0; i < gR; ++i) {
    if ((i % 100) == 0) cout << "." << flush;
    ZZ_pX e = conv<ZZ_pX>(eta(i));
    ZZ_pX a = (e * conv<ZZ_pX>(res)) % conv<ZZ_pX>(phi);
    ZZX aa = conv<ZZX>(a);
    o[i] = scalar_quo(aa, res, q);
  }
}

void ring::comp_omega_inv (vec_ZZ& o, const vec_ZZ& _omega, const ZZ& q) {
  o.SetLength(gR);

  for (long i = 0; i < gR; ++i) {
    ZZ w, conj_eta;
    if ((dR % 2) == 1) conj_eta = _omega[(i+gR/2) % gR];
    else conj_eta = _omega[i];
    w = conj_eta - dR;
    w = (w * InvMod(ZZ(mR)%q, q)) % q;
    o[i] = w;
  }
}

// (Gamma)_{i,j} = (rho_{t_i}(eta_j - d))
void ring::comp_gamma (vec_ZZ& o, const vec_ZZ& _omega, const ZZ& q) {
  o.SetLength(gR);

  for (long i = 0; i < gR; ++i) {
    o[i] = (omega[i] - dR) % q;
  }
}

void ring::comp_gamma_inv (vec_ZZ& o, const vec_ZZ& _omega, const ZZ& q) {
  o.SetLength(gR);

  for (long i = 0; i < gR; ++i) {
    ZZ conj_eta;
    if ((dR % 2) == 1) conj_eta = _omega[(i+gR/2) % gR];
    else conj_eta = _omega[i];
    o[i] = (conj_eta * InvMod(ZZ(mR)%q, q)) % q;
  }
}

void ring::init_omega (ZZ q) {
  if (type == prime) {
    comp_omega(omega, q);
    comp_omega_inv(omega_inv, omega, q);
  }
  else { // ring type is composite
    if (left->initialized == false)
      left->init_omega(q);
    if (right->initialized == false)
      right->init_omega(q);
  }
}

void param::init (long prec) {
  rg.init(prec, ns);
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2014 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "harmonic.h"
#include "cotmatrix.h"
#include "massmatrix.h"
#include "invert_diag.h"
#include "min_quad_with_fixed.h"
#include <Eigen/Sparse>

template <
  typename DerivedV,
  typename DerivedF,
  typename Derivedb,
  typename Derivedbc,
  typename DerivedW>
IGL_INLINE bool igl::harmonic(
  const Eigen::PlainObjectBase<DerivedV> & V,
  const Eigen::PlainObjectBase<DerivedF> & F,
  const Eigen::PlainObjectBase<Derivedb> & b,
  const Eigen::PlainObjectBase<Derivedbc> & bc,
  const int k,
  Eigen::PlainObjectBase<DerivedW> & W)
{
  using namespace Eigen;
  typedef typename DerivedV::Scalar Scalar;
  typedef Matrix<Scalar,Dynamic,1> VectorXS;
  SparseMatrix<Scalar> L,M,Mi;
  cotmatrix(V,F,L);
  switch(F.cols())
  {
    case 3:
      massmatrix(V,F,MASSMATRIX_TYPE_VORONOI,M);
      break;
    case 4:
    default:
      massmatrix(V,F,MASSMATRIX_TYPE_BARYCENTRIC,M);
      break;
  }
  invert_diag(M,Mi);
  SparseMatrix<Scalar> Q = -L;
  for(int p = 1;p<k;p++)
  {
    Q = (Q*Mi*-L).eval();
  }
  const VectorXS B = VectorXS::Zero(V.rows(),1);
  min_quad_with_fixed_data<Scalar> data;
  min_quad_with_fixed_precompute(Q,b,SparseMatrix<Scalar>(),true,data);
  W.resize(V.rows(),bc.cols());
  for(int w = 0;w<bc.cols();w++)
  {
    const VectorXS bcw = bc.col(w);
    VectorXS Ww;
    if(!min_quad_with_fixed_solve(data,B,bcw,VectorXS(),Ww))
    {
      return false;
    }
    W.col(w) = Ww;
  }
  return true;
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template specialization
template bool igl::harmonic<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, 1, 0, -1, 1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> > const&, int, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, 1, 0, -1, 1> >&);
template bool igl::harmonic<Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, -1, 0, -1, -1>, Eigen::Matrix<int, -1, 1, 0, -1, 1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, -1, 0, -1, -1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<int, -1, 1, 0, -1, 1> > const&, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, int, Eigen::PlainObjectBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);
#endif