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

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

#include <Eigen/Core>
#include <Eigen/LU>
#include <glog/logging.h>
#include <vector>

namespace theia {

// We use the epipolar constraint to solve for the unknown relative position:
//
//   p1^t * [t]_x * p2 = 0
//
// By stacking this constraint for both correspondences, we can constraint the 2
// degrees of freedom in our unknown relative position and solve for the
// solution easily.
bool RelativePoseFromTwoPointsWithKnownRotation(
    const Eigen::Vector2d rotated_features1[2],
    const Eigen::Vector2d rotated_features2[2],
    Eigen::Vector3d* relative_position2) {
  CHECK_NOTNULL(relative_position2);

  // The epipolar constraint can be rewritten in terms of the rotated features p
  // an d q as:
  //
  // p2 * q1 * t3 - p2 * t1 - q1 * t2 + q2 * t1 - p1 * q2 * t3 + p1 * t2 = 0
  //
  // Note that p3 and q3 are both 1.0, so they do not appear in this
  // equation. These constraints are stacked together to form a linear system
  // whose null space reveals the relative position.
  Eigen::Matrix<double, 2, 3> epipolar_constraint;
  for (int i = 0; i < 2; i++) {
    epipolar_constraint(i, 0) =
        -rotated_features1[i].y() + rotated_features2[i].y();
    epipolar_constraint(i, 1) =
        -rotated_features2[i].x() + rotated_features1[i].x();
    epipolar_constraint(i, 2) =
        rotated_features1[i].y() * rotated_features2[i].x() -
        rotated_features1[i].x() * rotated_features2[i].y();
  }

  // Compute the null space of this matrix.
  Eigen::FullPivLU<Eigen::Matrix<double, 2, 3> > linear_solver(
      epipolar_constraint);
  // If the null space is greater than 1 dimension then the linear system is
  // ill-conditioned or otherwise degenerate and the relative position cannot be
  // solved for.
  if (linear_solver.dimensionOfKernel() != 1) {
    return false;
  }
  // Extract the relative position as the null space kernel.
  *relative_position2 = linear_solver.kernel().normalized();
  return true;
}

}  // namespace theia


#include "include/MMVII_all.h"
#include <boost/math/special_functions/fpclassify.hpp>

namespace MMVII
{

// return the variance of  exponential distribution of parameter "a" ( i.e proportiona to  "a^|x|")
double Sigma2FromFactExp(double a)
{
   return (2*a) / Square(1-a);
}

// return the value of "a" such that  exponential distribution of parameter "a" has variance S2
// i.e. this the "inverse" function of Sigma2FromFactExp

double FactExpFromSigma2(double aS2)
{
    return (aS2+1 - sqrt(Square(aS2+1)-Square(aS2))  ) / aS2 ;
}

/* *************************************** */
/*                                         */
/*        cComputeStdDev                   */
/*                                         */
/* *************************************** */

template <class Type> cComputeStdDev<Type>::cComputeStdDev() :
   mSomW   (0.0),
   mSomWV  (0.0),
   mSomWV2 (0.0)
{
}

template <class Type> void cComputeStdDev<Type>::Add(const Type & aW,const Type & aV)
{
    mSomW   += aW;
    mSomWV  += aW *aV;
    mSomWV2 += aW * Square(aV);
}

template <class Type> void cComputeStdDev<Type>::SelfNormalize()
{
    MMVII_ASSERT_INVERTIBLE_VALUE(mSomW);

    mSomWV  /= mSomW;
    mSomWV2  /= mSomW;
    mSomWV2 -= Square(mSomWV);
    mStdDev = std::sqrt(std::max(Type(0.0),mSomWV2));
}

template <class Type> Type cComputeStdDev<Type>::NormalizedVal(const Type & aVal)  const
{
    MMVII_ASSERT_INVERTIBLE_VALUE(mStdDev);
    return (aVal-mSomWV) / mStdDev;
}

template <class Type> cComputeStdDev<Type>  cComputeStdDev<Type>::Normalize() const
{
     cComputeStdDev<Type> aRes = *this;
     aRes.SelfNormalize();
     return aRes;
}

/* ============================================= */
/*      cMatIner2Var<Type>                       */
/* ============================================= */

template <class Type> cMatIner2Var<Type>::cMatIner2Var() :
   mS0  (0.0),
   mS1  (0.0),
   mS11 (0.0),
   mS2  (0.0),
   mS12 (0.0),
   mS22 (0.0)
{
}
template <class Type> void cMatIner2Var<Type>::Add(const double & aPds,const Type & aV1,const Type & aV2)
{
    mS0  += aPds;
    mS1  += aPds * aV1;
    mS11 += aPds * aV1 * aV1 ;
    mS2  += aPds * aV2;
    mS12 += aPds * aV1 * aV2 ;
    mS22 += aPds * aV2 * aV2 ;
}

template <class Type> void cMatIner2Var<Type>::Normalize()
{
     MMVII_ASSERT_INVERTIBLE_VALUE(mS0);

     mS1 /= mS0;
     mS2 /= mS0;
     mS11 /= mS0;
     mS12 /= mS0;
     mS22 /= mS0;
     mS11 -= Square(mS1);
     mS12 -= mS1 * mS2;
     mS22 -= mS2 * mS2;
}

template <class Type> cMatIner2Var<double> StatFromImageDist(const cDataIm2D<Type> & aIm)
{
    cMatIner2Var<double> aRes;
    for (const auto & aP : aIm)
    {
         aRes.Add(aIm.GetV(aP),aP.x(),aP.y());
    }
    aRes.Normalize();
    return aRes;
}

#define INSTANTIATE_MAT_INER(TYPE)\
template class cMatIner2Var<TYPE>;\
template  class cComputeStdDev<TYPE>;\
template  cMatIner2Var<double> StatFromImageDist(const cDataIm2D<TYPE> & aIm);

INSTANTIATE_MAT_INER(tREAL4)
INSTANTIATE_MAT_INER(tREAL8)
INSTANTIATE_MAT_INER(tREAL16)

/* *********************************************** */
/*                                                 */
/*        cUB_ComputeStdDev<Dim>                   */
/*                                                 */
/* *********************************************** */

template  <const int Dim> cUB_ComputeStdDev<Dim>::cUB_ComputeStdDev() :
    mSomW   (0.0),
    mSomWW  (0.0)
{
    for (int aD=0 ; aD<Dim ; aD++)
    {
        mSomWV[aD] = 0.0;
        mSomWVV[aD] = 0.0;
    }
}

template  <const int Dim> void cUB_ComputeStdDev<Dim>::Add(const  double *  aVal,const double & aPds)
{
    mSomW += aPds;
    mSomWW += Square(aPds);
    for (int aD=0 ; aD<Dim ; aD++)
    {
        mSomWV[aD] += aPds * aVal[aD];
        mSomWVV[aD] += aPds * Square(aVal[aD]);
    }
}
template  <const int Dim>  double cUB_ComputeStdDev<Dim>::DeBiasFactor() const
{
    MMVII_INTERNAL_ASSERT_strong(mSomW>0,"No value in DeBiasFactor");
    return   1 - mSomWW/Square(mSomW);
}

template  <const int Dim> bool    cUB_ComputeStdDev<Dim>::OkForUnBiasedVar() const
{
   return (mSomW>0)  && (DeBiasFactor()!=0);
}

template  <const int Dim> const double *   cUB_ComputeStdDev<Dim>::ComputeUnBiasedVar()
{
    /* At least, this formula is correct :
         - when all weight are equal => 1-1/N
         - when all but one weight are 0 => 0
         - and finally if all are equal or 0
    */
    double aDebias = DeBiasFactor();
    MMVII_INTERNAL_ASSERT_strong(aDebias>0,"No var can be computed in ComputeVar");

    for (int aD=0 ; aD<Dim ; aD++)
    {
        double aAver = mSomWV[aD] / mSomW;
        double aVar  = mSomWVV[aD] / mSomW;
        aVar = aVar - Square(aAver);
        mVar[aD] = std::max(0.0,aVar) /aDebias;
    }

   return mVar;
}

template  <const int Dim> const double *   cUB_ComputeStdDev<Dim>::ComputeBiasedVar()
{
    for (int aD=0 ; aD<Dim ; aD++)
    {
        double aAver = mSomWV[aD] / mSomW;
        double aVar  = mSomWVV[aD] / mSomW;
        aVar = aVar - Square(aAver);
        mBVar[aD] = std::max(0.0,aVar);
    }

   return mBVar;
}

template class cUB_ComputeStdDev<1>;

void BenchUnbiasedStdDev()
{
    for (int aNbTest = 0 ; aNbTest<1000 ; aNbTest++)
    {
         int aNbVar = 1 + RandUnif_N(3);
         int aNbTir = aNbVar;
         int aNbComb = pow(aNbVar,aNbTir);
         std::vector<double> aVecVals = VRandUnif_0_1(aNbVar);
         std::vector<double> aVecWeight = VRandUnif_0_1(aNbVar);
// aVecWeight =  std::vector<double> (aNbVar,1.0);

         // compute the average of all empirical variance
         double aMoyVar=0;
         for (int aFlag=0 ; aFlag < aNbComb ; aFlag++) // Explore all combinaison
         {
             cUB_ComputeStdDev<1> aUBS;
             for (int aVar=0 ; aVar < aNbVar ; aVar++) // All Variable of this realization
             {
                 int aNumVar = (aFlag / round_ni(pow(aNbVar,aVar))) % aNbVar; // "Majic" formula to exdtrac p-adic decomp
                 aUBS.Add(&(aVecVals.at(aNumVar)),aVecWeight.at(aNumVar));
             }
             aMoyVar += aUBS.ComputeBiasedVar()[0];
         }
         aMoyVar /= aNbComb;

         cUB_ComputeStdDev<1> aUBS;
StdOut() << "WWW=" ;
         for (int aK=0 ; aK<aNbVar ; aK++)
         {
StdOut() << " " << aVecWeight.at(aK) ;
            aUBS.Add(&(aVecVals.at(aK)),aVecWeight.at(aK));
         }
StdOut() << "\n" ;
         StdOut() << "MOYVAR " << aMoyVar <<  " " <<  aUBS.DeBiasFactor() *  aUBS.ComputeBiasedVar()[0]
                  << " DBF=" <<  aUBS.DeBiasFactor() << "\n";
getchar();
    }
}

/* *********************************************** */
/*                                                 */
/*            Test Exp Filtre                      */
/*                                                 */
/* *********************************************** */

// Test Sigma2FromFactExp
template <class Type> void TestVarFilterExp(cPt2di aP0,cPt2di aP1,const Type & aV0,double aFx,double aFy,int aNbIt)
{
   cPt2di aPMil = (aP0+aP1) / 2;
   cRect2 aRect2(aP0,aP1);

   cIm2D<Type> aIm(aP0,aP1,nullptr,eModeInitImage::eMIA_Null);
   cDataIm2D<Type> & aDIm = aIm.DIm();

   // 1- Make 1 iteration of expon filter on a Direc, check variance and aver
   aDIm.InitDirac(aPMil,aV0);

   ExponentialFilter(true,aDIm,aNbIt,aRect2,aFx,aFy);
   cMatIner2Var<double>  aMat = StatFromImageDist(aDIm);

   MMVII_INTERNAL_ASSERT_bench(std::abs(aMat.S1() - aPMil.x()) < 1e-5  ,"Average Exp")
   MMVII_INTERNAL_ASSERT_bench(std::abs(aMat.S2() - aPMil.y()) < 1e-5  ,"Average Exp")

   MMVII_INTERNAL_ASSERT_bench(std::abs(aMat.S11()-Sigma2FromFactExp(aFx) * aNbIt)<1e-5,"Var Exp");
   MMVII_INTERNAL_ASSERT_bench(std::abs(aMat.S22()-Sigma2FromFactExp(aFy) * aNbIt)<1e-5,"Var Exp");
}

template <class Type> void TestVarFilterExp(cPt2di aSz,double aStdDev,int aNbIter,double aEps)
{
   cIm2D<Type> aIm(cPt2di(0,0),aSz,nullptr,eModeInitImage::eMIA_Null);
   cDataIm2D<Type> & aDIm = aIm.DIm();
   cPt2di aPMil = aSz / 2;
   double aV0 = 2.0;

   // 1- Make 1 iteration of expon filter on a Direc, check variance and aver
   aDIm.InitDirac(aPMil,aV0);

   ExpFilterOfStdDev(aDIm,aNbIter,aStdDev);
   cMatIner2Var<double>  aMat = StatFromImageDist(aDIm);

   MMVII_INTERNAL_ASSERT_bench(std::abs(aMat.S11()-Square(aStdDev))<aEps,"Std dev");
   MMVII_INTERNAL_ASSERT_bench(std::abs(aMat.S22()-Square(aStdDev))<aEps,"Std dev");
}

void BenchStat()
{
   TestVarFilterExp<double>(cPt2di(-2,2),cPt2di(400,375),2.0,0.6,0.67,1);
   TestVarFilterExp<double>(cPt2di(-2,2),cPt2di(400,375),2.0,0.6,0.67,3);

   // Test Sigma2FromFactExp
   for (int aK=1 ; aK<100 ; aK++)
   {
        double aS2 = aK / 5.0;
        double aF = FactExpFromSigma2(aS2);
        {
           // Check both formula are inverse of each others
           double aCheckS2 = Sigma2FromFactExp(aF);
           MMVII_INTERNAL_ASSERT_bench(std::abs( aS2 - aCheckS2 ) < 1e-5  ,"Sigma2FromFactExp")

        }
        {
           // Check formula Sigma2FromFactExp on exponential filters
           TestVarFilterExp<double>(cPt2di(0,0),cPt2di(4000,3),2.0,aF,0,1);
        }
   }

   TestVarFilterExp<double>(cPt2di(300,300),2.0,2,1e-6);
   TestVarFilterExp<float>(cPt2di(300,300),5.0,3,1e-3);
}

};


#include <armadillo>
#include <vector>
#include <set>
#include <map>

using namespace std;
using namespace arma;

struct Hormiga {
    vector<int> camino;
    double costoCamino;
};

ostream& operator<<(ostream& os, const Hormiga& hormiga)
{
    os << "Recorrido: ";

    for (int ciudad : hormiga.camino)
        os << ciudad << ' ';

    os << "\nCosto del camino: " << hormiga.costoCamino << endl;

    return os;
}

class ColoniaHormigas {
public:
    ColoniaHormigas(string rutaArchivoDistancias,
                    int nHormigas,
                    int nEpocas,
                    double sigma_cero,
                    double alpha,
                    double beta,
                    double tasaEvaporacion,
                    double Q);

    double calcularCosto(const vector<int>& camino);
    Hormiga buscarCamino();
    int seleccionarVecino(int ciudadActual, set<int> vecinos);
    Hormiga encontrarSolucion();
    void depositarFeromonas();

private:
    vector<Hormiga> m_hormiguero;
    mat m_distancias;
    mat m_feromonas;
    const int m_nHormigas;
    const int m_nEpocas;
    const double m_alpha;
    const double m_beta;
    const double m_Q;
    const double m_tasaEvaporacion;
    unsigned int m_nCiudades;
    int m_nodoOrigen;
};

ColoniaHormigas::ColoniaHormigas(string rutaArchivoDistancias,
                                 int nHormigas,
                                 int nEpocas,
                                 double sigma_cero,
                                 double alpha,
                                 double beta,
                                 double tasaEvaporacion,
                                 double Q)
    : m_nHormigas{nHormigas}
    , m_nEpocas{nEpocas}
    , m_alpha{alpha}
    , m_beta{beta}
    , m_Q{Q}
    , m_tasaEvaporacion{tasaEvaporacion}
{
    m_distancias.load(rutaArchivoDistancias);

    if (!m_distancias.is_square())
        throw runtime_error("La matriz leída no es cuadrada");

    m_nCiudades = m_distancias.n_rows;
    m_nodoOrigen = randi(1, distr_param(1, m_nCiudades))(0);

    m_feromonas = randu(m_distancias.n_rows, m_distancias.n_cols) * sigma_cero;
    //    m_feromonas = mat(m_nCiudades, m_nCiudades);
    //    m_feromonas.fill(sigma_cero);
}

double ColoniaHormigas::calcularCosto(const vector<int>& camino)
{
    double costo = 0;

    for (unsigned int i = 0; i < camino.size() - 1; ++i) {
        const int ciudad1 = camino.at(i);
        const int ciudad2 = camino.at(i + 1);

        if (ciudad1 == ciudad2)
            throw runtime_error("Las ciudades son iguales");

        costo += m_distancias(ciudad1 - 1, ciudad2 - 1);
    }

    return costo;
}

Hormiga ColoniaHormigas::buscarCamino()
{
    Hormiga hormiga;
    hormiga.camino = {m_nodoOrigen};

    set<int> vecinos;
    for (unsigned int i = 1; i <= m_nCiudades; ++i)
        vecinos.insert(i);

    vecinos.erase(m_nodoOrigen);

    while (!vecinos.empty()) {
        const int ciudad = seleccionarVecino(hormiga.camino.back(), vecinos);
        hormiga.camino.push_back(ciudad);
        vecinos.erase(ciudad);
    }

    // Cuando salgo del bucle, pasé por todas las ciudades.
    // Solo falta volver a la ciudad inicial.
    hormiga.camino.push_back(m_nodoOrigen);

    return hormiga;
}

int ColoniaHormigas::seleccionarVecino(int ciudadActual,
                                       set<int> vecinos)
{
    map<int, double> probabilidadVecino;
    double sumaProbabilidades = 0;

    // Calculo el numerador y el denominador de la expresión de la diapositiva
    for (int vecino : vecinos) {
        const double probabilidad = pow(m_feromonas(ciudadActual - 1, vecino - 1), m_alpha)
                                    / pow(m_distancias(ciudadActual - 1, vecino - 1), m_beta);
        //        const double probabilidad = pow(m_feromonas(ciudadActual - 1, vecino - 1), m_alpha);
        probabilidadVecino[vecino] = probabilidad;
        sumaProbabilidades += probabilidad;
    }

    // Normalizo cada numerador por el denominador calculado
    for (auto& p : probabilidadVecino)
        p.second /= sumaProbabilidades;

    // Ahora que tengo las probabilidades normalizadas,
    // selecciono un vecino
    const double moneda = randu(1).eval()(0);
    double probAcumulada = 0;

    for (const auto& p : probabilidadVecino) {
        probAcumulada += p.second;

        if (moneda <= probAcumulada)
            return p.first;
    }

    throw runtime_error("Nunca se debería llegar hasta acá");

    //    pair<int, double> mejor{0, 0};

    //    for (const auto& p : probabilidadVecino) {
    //        if (p.second > mejor.second)
    //            mejor = p;
    //    }

    //    return mejor.first;
}

Hormiga ColoniaHormigas::encontrarSolucion()
{
    for (int epoca = 1; epoca <= m_nEpocas; ++epoca) {
        m_hormiguero.clear();

        // Para cada hormiga, busco un camino que recorra todas
        // las ciudades.
        for (int i = 0; i < m_nHormigas; ++i)
            m_hormiguero.push_back(buscarCamino());

        // Me fijo si todas las hormigas tienen el mismo camino
        bool todosIguales = true;
        for (const Hormiga& hormiga : m_hormiguero) {
            if (hormiga.camino != m_hormiguero.front().camino) {
                todosIguales = false;
                break;
            }
        }

        if (todosIguales) {
            Hormiga solucion = m_hormiguero.front();
            solucion.costoCamino = calcularCosto(solucion.camino);

            return solucion;
        }

        // Evaporar feromonas
        m_feromonas *= (1 - m_tasaEvaporacion);

        // Depositar feromonas
        depositarFeromonas();
    }

    // Si se llega hasta acá las hormigas no convergieron
    // a un único camino.
    throw runtime_error("No se encontró una solución");
}

void ColoniaHormigas::depositarFeromonas()
{
    if (m_hormiguero.empty())
        throw runtime_error("El hormiguero está vacío");

    for (Hormiga& hormiga : m_hormiguero) {
        hormiga.costoCamino = calcularCosto(hormiga.camino);
        const double deltaFeromonas = m_Q / hormiga.costoCamino;

        for (unsigned int i = 0; i < hormiga.camino.size() - 1; ++i) {
            const int ciudad1 = hormiga.camino.at(i);
            const int ciudad2 = hormiga.camino.at(i + 1);

            if (ciudad1 == ciudad2)
                throw runtime_error("Las ciudades son iguales");

            m_feromonas(ciudad1 - 1, ciudad2 - 1) += deltaFeromonas;
            //			m_feromonas(ciudad2 - 1, ciudad1 - 1) += deltaFeromonas;
        }
    }
}


#pragma once

#include <Eigen/Core>
using namespace Eigen;

#include <autodiff/forward.hpp>
#include <autodiff/forward/eigen.hpp>
using namespace autodiff;

#include <samson/so3.hpp>

namespace samson::robot
{
    using namespace samson::se3;

    auto fk(const Matrix4dual &M, const MatrixXdual &S, const VectorXdual &theta)
    {
        Matrix4dual T = M;
        for (int i = 0; i < S.rows(); ++i)
        {
            Vector6dual Vi = S.row(i);
            dual thetai = theta(i);
            Vi *= thetai;
            T *= exp(Vi);
        }
        return T;
    }

} // namespace samson::robot


/*=========================================================================

  Program:   Small Body Geophysical Analysis
  Module:    SBGATPolyhedronGravityModel.hpp

  Class derived from VTK's vtkPolyDataAlgorithm by Benjamin Bercovici

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/

/**
@file SBGATPolyhedronGravityModel.hpp
@class  SBGATPolyhedronGravityModel
@author Benjamin Bercovici
@date October 2018

@brief  Evaluation of potential, acceleration caused by a constant-density polyhedron
 @details Computes the potential, acceleration caused by a polyhedron
 of constant density by evaluating the so called Polyhedron Gravity Model as derived by Werner and Scheeres.
The input must be a topologically-closed polyhedron. This class will always use results expressed in `meters` as their distance unit (e.g accelerations in m/s^2, potentials in m^2/s^2,...) . Unit consistency is enforced through the use of the SetScaleMeters()
and SetScaleKiloMeters() method.

See Werner, R. A., & Scheeres, D. J. (1997). Exterior gravitation of a polyhedron derived and compared with harmonic and mascon gravitation representations of asteroid 4769 Castalia. Celestial Mechanics and Dynamical Astronomy, 65(3), 313–344. https://doi.org/10.1007/BF00053511
for further details. Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
@copyright MIT License, Benjamin Bercovici and Jay McMahon
*/

#ifndef SBGATPolyhedronGravityModel_hpp
#define SBGATPolyhedronGravityModel_hpp

#include <vtkFiltersCoreModule.h> // For export macro
#include <vtkPolyDataAlgorithm.h>
#include <armadillo>
#include <SBGATMassProperties.hpp>

class VTKFILTERSCORE_EXPORT SBGATPolyhedronGravityModel : public SBGATMassProperties{
public:

  /**
   * Constructs with initial values of zero.
   */

  static SBGATPolyhedronGravityModel *New();

  vtkTypeMacro(SBGATPolyhedronGravityModel,vtkPolyDataAlgorithm);
  void PrintSelf(std::ostream& os, vtkIndent indent) override;
  void PrintHeader(std::ostream& os, vtkIndent indent) override;
  void PrintTrailer(std::ostream& os, vtkIndent indent) override;

  /**
  Evaluates the Polyhedron Gravity Model potential at the specified point assuming
  a constant density
  @param point pointer to coordinates of queried point, expressed in the same frame as
  the polydata
  @return PGM potential evaluated at the queried point (m ^ 2/ s ^2)
  */
  double GetPotential(double const * point) const;

  /**
  Evaluates the Polyhedron Gravity Model potential at the specified point assuming
  a constant density
  @param point coordinates of queried point, expressed in the same frame as
  the polydata
  @return PGM potential evaluated at the queried point (m ^ 2 / s ^2)
  */
  double GetPotential(const arma::vec::fixed<3> & point) const;

  /**
  Evaluates the Polyhedron Gravity Model potential and acceleration at the specified point assuming
  a constant density
  @param point coordinates of queried point, expressed in the same frame as
  the polydata used to construct the PGM
  @param[out] potential PGM potential evaluated at the queried point (m ^ 2 / s ^2)
  @param[out] acc PGM acceleration evaluated at the queried point (m / s ^2)
  */
  void GetPotentialAcceleration(double const * point,double & potential,
    arma::vec::fixed<3> & acc) const;

  /**
  Evaluates the Polyhedron Gravity Model potential and acceleration at the specified point assuming
  a constant density
  @param point coordinates of queried point, expressed in the same frame as
  the polydata used to construct the PGM
  @param[out] potential PGM potential evaluated at the queried point (m ^ 2 / s ^2)
  @param[out] acc PGM acceleration evaluated at the queried point (m / s ^2)
  */
  void GetPotentialAcceleration(const arma::vec::fixed<3> & point,double & potential,
    arma::vec::fixed<3> & acc) const;

/**
Evaluates the Polyhedron Gravity Model potential, acceleration and gravity gradient matrix at the specified point assuming
a constant density
@param point coordinates of queried point, expressed in the same frame as
the polydata used to construct the PGM
@param[out] potential PGM potential evaluated at the queried point (m ^ 2 / s ^2)
@param[out] acc PGM acceleration evaluated at the queried point (m / s ^2)
@param[out] gravity_gradient_mat PGM gravity gradient matrix evaluated at the queried point (1 / s ^2)
*/
  void GetPotentialAccelerationGravityGradient(double const  * point,double & potential,
    arma::vec::fixed<3> & acc,arma::mat::fixed<3,3> & gravity_gradient_mat) const ;

/**
Get the body-fixed acceleration at the center of the specified facet
@param f facet index
@return body-fixed acceleration (m/s^2)
*/
  arma::vec::fixed<3> GetBodyFixedAccelerationf(const int & f) const;

/**
Evaluates the Polyhedron Gravity Model potential, acceleration and gravity gradient matrix at the specified point assuming
a constant density
@param point coordinates of queried point, expressed in the same frame as
the polydata used to construct the PGM
@param[out] potential PGM potential evaluated at the queried point (m ^ 2 / s ^2)
@param[out] acc PGM acceleration evaluated at the queried point (m / s ^2)
@param[out] gravity_gradient_mat PGM gravity gradient matrix evaluated at the queried point (1 / s ^2)
*/
  void GetPotentialAccelerationGravityGradient(const arma::vec::fixed<3> & point,double & potential,
    arma::vec::fixed<3> & acc,arma::mat::fixed<3,3> & gravity_gradient_mat) const ;

/**
Evaluates the Polyhedron Gravity Model gravity gradient matrix at the specified point assuming
a constant density
@param point coordinates of queried point, expressed in the same frame as
the polydata used to construct the PGM
@param[out] gravity_gradient_mat PGM gravity gradient matrix evaluated at the queried point (1 / s ^2)
*/
  arma::mat::fixed<3,3> GetGravityGradient(const arma::vec::fixed<3> & point) const ;

  /**
  Evaluates the Polyhedron Gravity Model acceleration at the specified point assuming
  a constant density
  @param point coordinates of queried point, expressed in the same frame as
  the polydata used to construct the PGM
  @return PGM acceleration evaluated at the queried point (m / s ^2)
  */
  arma::vec::fixed<3> GetAcceleration(const arma::vec::fixed<3> & point) const;

  /**
  Evaluates the Polyhedron Gravity Model acceleration at the specified point assuming
  a constant density
  @param point pointer to coordinates of queried point, expressed in the same frame as
  the input polydata
  @return PGM acceleration evaluated at the queried point (m / s ^2)
  */
  arma::vec::fixed<3> GetAcceleration(double const * point) const;

  /**
  Determines whether the provided point lies inside or outside the shape
  @param point coordinates of queried point, expressed in the same frame as
  the polydata
  @param tolerance
  @return true if the polydata contains the point, false otherwise
  */
  bool Contains(double const * point, double tol = 1e-8) const;

  /**
  Determines whether the provided point lies inside or outside the shape
  @param point coordinates of queried point, expressed in the same frame as
  the polydata
  @param tolerance
  @return true if the polydata contains the point, false otherwise
  */
  bool Contains(const arma::vec::fixed<3> & point,double tol = 1e-8) const;

  /**
  Computes the slope as the center of the designated facet
  @param f facet index
  @return slope (rad)
  */
  double GetSlope(const int & f ) const;

  /**
  Get polyhedron mass. Must have set the density and call
  @return mass (kg)
  */
  double GetMass() const{
    return this -> density * this -> GetVolume() * std::pow(this -> scaleFactor,3);
  }

  /**
  Return the performance factor of the f-th facet at the specified position
  @param pos position of field point
  @param f facet index
  @return omega_f
  */
  double GetPerformanceFactor(const arma::vec::fixed<3> & pos, const int & f) const;

  /**
  Return the performance factor of the f-th facet at the specified position
  @param pos position of field point
  @param f facet index
  @return omega_f
  */
  double GetPerformanceFactor( const double * pos, const int & f) const;

  /**
  Return the wire potential of the e-th edge at the specified position
  @param pos position of field point
  @param e edge index
  @return L_e
  */
  double GetLe(const arma::vec::fixed<3> & pos, const int & e) const;

  /**
  Return the wire potential of the e-th edge at the specified position
  @param pos position of field point
  @param e edge index
  @return L_e
  */
  double GetLe( const double * pos, const int & e) const;

  /**
  Evaluates the Polyhedron Gravity Model at the surface of the specified surface elements in the provided shape
  @param[in] selected_shape shape for which the surface polyhedron gravity model must be computed
  @param[in] queried_elements vector of elements indices where the polyhedron gravity model should be evaluated
  @param[in] is_in_meters true if the shape coordinates were expressed in meters, false if they were expressed in kilometers
  @param[in] density shape bulk density in kg/m^3
  @param[in] omega fixed angular velocity of shape expressed in rad/s
  @param[out] slopes vector storing the gravitational slopes (degrees) evaluated at the center of each queried element
  @param[out] inertial_potentials vector storing the inertial gravitational potentials (m^2/s^2) evaluated at the center of each queried element
  @param[out] body_fixed_potentials vector storing the inertial gravitational potentials (m^2/s^2) evaluated at the center of each queried element
  @param[out] inertial_acc_magnitudes vector storing the inertial gravitational acceleration magnitudes  (m/s^2) evaluated at the center of each queried element
  @param[out] body_fixed_acc_magnitudes vector storing the body-fixed gravitational acceleration magnitudes (m/s^2) evaluated at the center of each queried element
  */
  static void ComputeSurfacePGM(
    vtkSmartPointer<vtkPolyData> selected_shape,
    const std::vector<unsigned int> & queried_elements,
    bool is_in_meters,
    double density,
    const arma::vec::fixed<3> & omega,
    std::vector<double> & slopes,
    std::vector<double> & inertial_potentials,
    std::vector<double> & body_fixed_potentials,
    std::vector<double> & inertial_acc_magnitudes,
    std::vector<double> & body_fixed_acc_magnitudes);

  /**
  Saves the provided Polyhedron Gravity Model to a file
  @param[in] selected_shape shape for which the surface polyhedron gravity model must be computed
  @param[in] queried_elements shape indices of elements where the polyhedron gravity model should be evaluated
  @param[in] is_in_meters true if the shape coordinates were expressed in meters, false if they were expressed in kilometers
  @param[in] mass mass of shape model (kg)
  @param[in] omega fixed angular velocity of shape (rad/s)
  @param[in] slopes vector storing the gravitational slopes (degrees) evaluated at the center of each queried element
  @param[in] inertial_potentials vector storing the inertial gravitational potentials (m^2/s^2) evaluated at the center of each queried element
  @param[in] body_fixed_potentials vector storing the inertial gravitational potentials (m^2/s^2) evaluated at the center of each queried element
  @param[in] inertial_acc_magnitudes vector storing the inertial gravitational acceleration magnitudes  (m/s^2) evaluated at the center of each queried element
  @param[in] body_fixed_acc_magnitudes vector storing the body-fixed gravitational acceleration magnitudes (m/s^2) evaluated at the center of each queried element
  @param[in] path save path (ex: "pgm_surface.json")
  */
  static void SaveSurfacePGM(vtkSmartPointer<vtkPolyData> selected_shape,
    const std::vector<unsigned int> & queried_elements,
    bool is_in_meters,
    const double & mass,
    const arma::vec::fixed<3> & omega,
    const std::vector<double> & slopes,
    const std::vector<double> & inertial_potentials,
    const std::vector<double> & body_fixed_potentials,
    const std::vector<double> & inertial_acc_magnitudes,
    const std::vector<double> & body_fixed_acc_magnitudes,
    std::string path);

/**
  Saves the provided Polyhedron Gravity Model to a file
  @param[in] selected_shape shape for which the surface polyhedron gravity model must be computed
  @param[in] queried_elements shape indices of elements where the polyhedron gravity model should be evaluated
  @param[in] is_in_meters true if the shape coordinates were expressed in meters, false if they were expressed in kilometers
  @param[in] mass mass of shape model (kg)
  @param[in] omega fixed angular velocity of shape (rad/s)
  @param[in] slopes vector storing the gravitational slopes (degrees) evaluated at the center of each queried element
  @param[in] inertial_potentials vector storing the inertial gravitational potentials (m^2/s^2) evaluated at the center of each queried element
  @param[in] body_fixed_potentials vector storing the inertial gravitational potentials (m^2/s^2) evaluated at the center of each queried element
  @param[in] inertial_acc_magnitudes vector storing the inertial gravitational acceleration magnitudes  (m/s^2) evaluated at the center of each queried element
  @param[in] body_fixed_acc_magnitudes vector storing the body-fixed gravitational acceleration magnitudes (m/s^2) evaluated at the center of each queried element
  @param[in] slope_sds vector storing the standard deviation in the gravitational slopes (degrees) evaluated at the center of each queried element
  @param[in] path save path (ex: "pgm_surface.json")
  */
static void SaveSurfacePGM(vtkSmartPointer<vtkPolyData> selected_shape,
  const std::vector<unsigned int> & queried_elements,
  bool is_in_meters,
  const double & mass,
  const arma::vec::fixed<3> & omega,
  const std::vector<double> & slopes,
  const std::vector<double> & inertial_potentials,
  const std::vector<double> & body_fixed_potentials,
  const std::vector<double> & inertial_acc_magnitudes,
  const std::vector<double> & body_fixed_acc_magnitudes,
  const std::vector<double> & slope_sds,
  std::string path);

  /**
  Loads a previously computed surface Polyhedron Gravity Model from a file
  @param[out] mass mass of shape model (kg)
  @param[out] omega fixed angular velocity of shape (rad/s)
  @param[out] slopes vector storing the gravitational slopes (degrees) evaluated at the center of each queried element
  @param[out] inertial_potentials vector storing the inertial gravitational potentials (m^2/s^2) evaluated at the center of each queried element
  @param[out] body_fixed_potentials vector storing the body-fixed gravitational potentials (m^2/s^2) evaluated at the center of each queried element
  @param[out] inertial_acc_magnitudes vector storing the inertial gravitational acceleration magnitudes  (m/s^2) evaluated at the center of each queried element
  @param[out] body_fixed_acc_magnitudes vector storing the body-fixed gravitational acceleration magnitudes (m/s^2) evaluated at the center of each queried element
  @param[out] slope_sds standard deviation in the surface slopes (deg)
  @param[in] path load path (ex: "pgm_surface.json")
  */
  static void LoadSurfacePGM(double & mass,
    arma::vec::fixed<3> & omega,
    std::vector<double> & slopes,
    std::vector<double> & inertial_potentials,
    std::vector<double> & body_fixed_potentials,
    std::vector<double> & inertial_acc_magnitudes,
    std::vector<double> & body_fixed_acc_magnitudes,
    std::vector<double> & slope_sds,
    std::string path);

  /**
  Return the Xe^E vector holding the e-th edge dyadic factors (Le,r_ie_0^T,Ee^T)^T
  @param pos field point
  @param e edge index
  @return Xe^E vector holding the e-th edge dyadic factors
  */
  arma::vec::fixed<10> GetXe(const arma::vec::fixed<3> & pos,const int & e) const;

  /**
  Return the Xf^F vector holding the f-th facet dyadic factors (omega_f,r_if_0^T,Ff^T)^T
  @param pos field point
  @param f facet index
  @return Xf^F vector holding the f-th facet dyadic factors
  */
  arma::vec::fixed<10> GetXf(const arma::vec::fixed<3> & pos,const int & f) const;

  /**
  Return the parametrization of the designated edge dyad Ee. This dyad
  is stored in a flattened double container holding nine values and ordered like so

  E = {
    {E[0], E[1],  E[2]},
    {E[3], E[4],  E[5]},
    {E[6], E[7],  E[8]}
  };
  Due to the symmetric nature of E, its parametrization is
  {E[0],E[4],E[8],E[1],E[2],E[5]};

  @param e edge index
  @return Ee dyad parametrization
  */
  arma::vec::fixed<6> GetEeParam(const int & e) const;

  /**
  Return the parametrization of the designated facet dyad Ff. This dyad
  is stored in a flattened double container holding nine values and ordered like so

  F = {
    {F[0], F[1],  F[2]},
    {F[3], F[4],  F[5]},
    {F[6], F[7],  F[8]}
  };
  Due to the symmetric nature of E, its parametrization is
  {F[0],F[4],F[8],F[1],F[2],F[5]};

  @param f facet index
  @return Ff dyad parametrization
  */
  arma::vec::fixed<6> GetFfParam(const int & f) const;

  /**
  Return the contribution of a specific edge to the potential at a specified field point
  @param Xe vector of dyadic coefficients for edge e at the prescribed fieldpoint
  @return contribution to the potential of this specific edge at the prescribed fieldpoint
  */
  static double GetUe(const arma::vec::fixed<10> & Xe);

  /**
  Return the contribution of a specific facet to the potential at a specified field point
  @param Xf vector of dyadic coefficients for facet f at the prescribed fieldpoint
  @return contribution to the potential of this specific facet at the prescribed fieldpoint
  */
  static double GetUf(const arma::vec::fixed<10> & Xf);

  /**
  Return the contribution of a specific edge to the acceleration at a specified field point
  @param Xe vector of dyadic coefficients for edge e at the prescribed fieldpoint
  @return contribution to the potential of this specific edge at the prescribed fieldpoint
  */
  static arma::vec::fixed<3> GetAe(const arma::vec::fixed<10> & Xe);

  /**
  Return the contribution of a specific facet to the acceleration at a specified field point
  @param Xf vector of dyadic coefficients for facet f at the prescribed fieldpoint
  @return contribution to the potential of this specific facet at the prescribed fieldpoint
  */
  static arma::vec::fixed<3> GetAf(const arma::vec::fixed<10> & Xf);

  /**
  Set the angular velocity vector
  @param Omega angular velocity expressed in body-frame (rad/s)
  */
  void SetOmega(arma::vec::fixed<3> Omega){this -> Omega = Omega;}

  /**
  Return the angular velocity vector
  @return Omega angular velocity expressed in body-frame (rad/s)
  */
  arma::vec::fixed<3> GetOmega() const {return this -> Omega;}

protected:
  SBGATPolyhedronGravityModel();
  ~SBGATPolyhedronGravityModel() override;

  void Clear();

  int RequestData(vtkInformation* request,
    vtkInformationVector** inputVector,
    vtkInformationVector* outputVector) override;

  double ** facet_dyads;
  double ** edge_dyads;

  arma::vec::fixed<3> Omega;

private:
  SBGATPolyhedronGravityModel(const SBGATPolyhedronGravityModel&) = delete;
  void operator=(const SBGATPolyhedronGravityModel&) = delete;
};

#endif


#include "xpbd/green_strain_elastic_constraint.h"

#include <Eigen/Dense>
#include <eigen/SVD>

namespace xpbd {

green_strain_elastic_constraint_t::green_strain_elastic_constraint_t(
    std::initializer_list<index_type> indices,
    positions_type const& p,
    scalar_type young_modulus,
    scalar_type poisson_ratio,
    scalar_type const alpha)
    : base_type(indices, alpha), V0_{0.}, DmInv_{}, mu_{}, lambda_{}
{
    assert(indices.size() == 4u);

    auto const v1 = this->indices().at(0);
    auto const v2 = this->indices().at(1);
    auto const v3 = this->indices().at(2);
    auto const v4 = this->indices().at(3);

    auto const p1 = p.row(v1);
    auto const p2 = p.row(v2);
    auto const p3 = p.row(v3);
    auto const p4 = p.row(v4);

    Eigen::Matrix3d Dm;
    Dm.col(0) = (p1 - p4).transpose();
    Dm.col(1) = (p2 - p4).transpose();
    Dm.col(2) = (p3 - p4).transpose();

    V0_     = (1. / 6.) * Dm.determinant();
    DmInv_  = Dm.inverse();
    mu_     = (young_modulus) / (2. * (1 + poisson_ratio));
    lambda_ = (young_modulus * poisson_ratio) / ((1 + poisson_ratio) * (1 - 2 * poisson_ratio));
}

green_strain_elastic_constraint_t::scalar_type
green_strain_elastic_constraint_t::signed_volume(positions_type const& V) const
{
    Eigen::RowVector3d const p1 = V.row(indices()[0]);
    Eigen::RowVector3d const p2 = V.row(indices()[1]);
    Eigen::RowVector3d const p3 = V.row(indices()[2]);
    Eigen::RowVector3d const p4 = V.row(indices()[3]);

    Eigen::Matrix3d Ds;
    Ds.col(0)      = (p1 - p4).transpose();
    Ds.col(1)      = (p2 - p4).transpose();
    Ds.col(2)      = (p3 - p4).transpose();
    auto const vol = (1. / 6.) * Ds.determinant();
    return vol;
}

void green_strain_elastic_constraint_t::project(
    positions_type& p,
    masses_type const& m,
    scalar_type& lagrange,
    scalar_type const dt) const
{
    auto const v1 = this->indices().at(0);
    auto const v2 = this->indices().at(1);
    auto const v3 = this->indices().at(2);
    auto const v4 = this->indices().at(3);

    auto const p1 = p.row(v1);
    auto const p2 = p.row(v2);
    auto const p3 = p.row(v3);
    auto const p4 = p.row(v4);

    auto const w1 = 1. / m(v1);
    auto const w2 = 1. / m(v2);
    auto const w3 = 1. / m(v3);
    auto const w4 = 1. / m(v4);

    auto const Vsigned        = signed_volume(p);
    bool const is_V_positive  = Vsigned >= 0.;
    bool const is_V0_positive = V0_ >= 0.;
    bool const is_tet_inverted =
        (is_V_positive && !is_V0_positive) || (!is_V_positive && is_V0_positive);
    scalar_type constexpr epsilon = 1e-20;

    Eigen::Matrix3d Ds;
    Ds.col(0) = (p1 - p4).transpose();
    Ds.col(1) = (p2 - p4).transpose();
    Ds.col(2) = (p3 - p4).transpose();

    Eigen::Matrix3d const F = Ds * DmInv_;
    Eigen::Matrix3d const I = Eigen::Matrix3d::Identity();

    // TODO: Implement correct inversion handling described in
    // Irving, Geoffrey, Joseph Teran, and Ronald Fedkiw. "Invertible finite elements for robust
    // simulation of large deformation." Proceedings of the 2004 ACM SIGGRAPH/Eurographics symposium
    // on Computer animation. 2004.
    // scalar_type psi{};
    // Eigen::Matrix3d Piola;
    Eigen::JacobiSVD<Eigen::Matrix3d> UFhatV(F, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Vector3d const Fsigma = UFhatV.singularValues();
    Eigen::Matrix3d Fhat;
    Fhat.setZero();
    Fhat(0, 0) = Fsigma(0);
    Fhat(1, 1) = Fsigma(1);
    Fhat(2, 2) = Fsigma(2);

    Eigen::Matrix3d U       = UFhatV.matrixU();
    Eigen::Matrix3d const V = UFhatV.matrixV();

    if (is_tet_inverted)
    {
        Fhat(2, 2) = -Fhat(2, 2);
        U.col(2)   = -U.col(2);
    }

    // stress reaches maximum at 58% compression
    scalar_type constexpr min_singular_value = 0.577;
    Fhat(0, 0)                               = std::max(Fhat(0, 0), min_singular_value);
    Fhat(1, 1)                               = std::max(Fhat(1, 1), min_singular_value);
    Fhat(2, 2)                               = std::max(Fhat(2, 2), min_singular_value);

    Eigen::Matrix3d const Ehat     = 0.5 * (Fhat.transpose() * Fhat - I);
    scalar_type const EhatTrace    = Ehat.trace();
    Eigen::Matrix3d const Piolahat = Fhat * ((2. * mu_ * Ehat) + (lambda_ * EhatTrace * I));

    Eigen::Matrix3d const E  = U * Ehat * V.transpose();
    scalar_type const Etrace = E.trace();
    scalar_type const psi = mu_ * (E.array() * E.array()).sum() + 0.5 * lambda_ * Etrace * Etrace;

    Eigen::Matrix3d const Piola = U * Piolahat * V.transpose();

    // H is the negative gradient of the elastic potential
    scalar_type const V0     = std::abs(V0_);
    Eigen::Matrix3d const H  = -V0 * Piola * DmInv_.transpose();
    Eigen::Vector3d const f1 = H.col(0);
    Eigen::Vector3d const f2 = H.col(1);
    Eigen::Vector3d const f3 = H.col(2);
    Eigen::Vector3d const f4 = -(f1 + f2 + f3);

    // clang-format off
     auto const weighted_sum_of_gradients =
        w1 * f1.squaredNorm() +
        w2 * f2.squaredNorm() +
        w3 * f3.squaredNorm() +
        w4 * f4.squaredNorm();
    // clang-format on

    if (weighted_sum_of_gradients < epsilon)
        return;

    scalar_type const C           = V0 * psi;
    scalar_type const alpha_tilde = alpha_ / (dt * dt);
    scalar_type const delta_lagrange =
        -(C + alpha_tilde * lagrange) / (weighted_sum_of_gradients + alpha_tilde);

    lagrange += delta_lagrange;
    // because f = - grad(potential), then grad(potential) = -f and thus grad(C) = -f
    p.row(v1) += w1 * -f1 * delta_lagrange;
    p.row(v2) += w2 * -f2 * delta_lagrange;
    p.row(v3) += w3 * -f3 * delta_lagrange;
    p.row(v4) += w4 * -f4 * delta_lagrange;
}

green_strain_elastic_constraint_t::scalar_type
green_strain_elastic_constraint_t::evaluate(positions_type const& p, masses_type const& m) const
{
    auto const v1 = this->indices().at(0);
    auto const v2 = this->indices().at(1);
    auto const v3 = this->indices().at(2);
    auto const v4 = this->indices().at(3);

    auto const p1 = p.row(v1);
    auto const p2 = p.row(v2);
    auto const p3 = p.row(v3);
    auto const p4 = p.row(v4);

    auto const w1 = 1. / m(v1);
    auto const w2 = 1. / m(v2);
    auto const w3 = 1. / m(v3);
    auto const w4 = 1. / m(v4);

    auto const Vsigned        = signed_volume(p);
    bool const is_V_positive  = Vsigned >= 0.;
    bool const is_V0_positive = V0_ >= 0.;
    bool const is_tet_inverted =
        (is_V_positive && !is_V0_positive) || (!is_V_positive && is_V0_positive);
    scalar_type constexpr epsilon = 1e-20;

    Eigen::Matrix3d Ds;
    Ds.col(0) = (p1 - p4).transpose();
    Ds.col(1) = (p2 - p4).transpose();
    Ds.col(2) = (p3 - p4).transpose();

    Eigen::Matrix3d const F = Ds * DmInv_;
    Eigen::Matrix3d const I = Eigen::Matrix3d::Identity();

    // TODO: Implement correct inversion handling described in
    // Irving, Geoffrey, Joseph Teran, and Ronald Fedkiw. "Invertible finite elements for robust
    // simulation of large deformation." Proceedings of the 2004 ACM SIGGRAPH/Eurographics symposium
    // on Computer animation. 2004.
    // scalar_type psi{};
    // Eigen::Matrix3d Piola;
    Eigen::JacobiSVD<Eigen::Matrix3d> UFhatV(F, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Vector3d const Fsigma = UFhatV.singularValues();
    Eigen::Matrix3d Fhat;
    Fhat.setZero();
    Fhat(0, 0) = Fsigma(0);
    Fhat(1, 1) = Fsigma(1);
    Fhat(2, 2) = Fsigma(2);

    Eigen::Matrix3d U       = UFhatV.matrixU();
    Eigen::Matrix3d const V = UFhatV.matrixV();

    if (is_tet_inverted)
    {
        Fhat(2, 2) = -Fhat(2, 2);
        U.col(2)   = -U.col(2);
    }

    // stress reaches maximum at 58% compression
    scalar_type constexpr min_singular_value = 0.577;
    Fhat(0, 0)                               = std::max(Fhat(0, 0), min_singular_value);
    Fhat(1, 1)                               = std::max(Fhat(1, 1), min_singular_value);
    Fhat(2, 2)                               = std::max(Fhat(2, 2), min_singular_value);

    Eigen::Matrix3d const Ehat     = 0.5 * (Fhat.transpose() * Fhat - I);
    scalar_type const EhatTrace    = Ehat.trace();
    Eigen::Matrix3d const Piolahat = Fhat * ((2. * mu_ * Ehat) + (lambda_ * EhatTrace * I));

    Eigen::Matrix3d const E  = U * Ehat * V.transpose();
    scalar_type const Etrace = E.trace();
    scalar_type const psi = mu_ * (E.array() * E.array()).sum() + 0.5 * lambda_ * Etrace * Etrace;

    scalar_type const V0 = std::abs(V0_);
    scalar_type const C = V0 * psi;
    return C;
}

} // namespace xpbd


#include <iostream>
#include <cstdint>
#include <Eigen/Dense>

#include "hnf.hpp"

template<class Derived>
bool is_rowHNF(const Eigen::MatrixBase<Derived> &mat) noexcept
{
    for(int i = 0, j = 0; j < mat.cols() && i < mat.rows(); ++j) {
        for(int k = i+1; k < mat.rows(); ++k)
            if (mat(k,j) != 0)
                return false;
        if (mat(i,j) != 0) {
            for(int k = 0; k < i; ++k)
                if (mat(k,j) < 0 || mat(i,j) < mat(k,j))
                    return false;
            ++i;
        }
    }

    return true;
}

template<class Derived>
bool is_colHNF(const Eigen::MatrixBase<Derived> &mat) noexcept
{
    return is_rowHNF(mat.transpose().eval());
}

template<class Derived>
bool is_unimodular(const Eigen::MatrixBase<Derived> &mat) noexcept
{
    return std::abs(mat.determinant()) == 1;
}

int main(int argc, char*argv[])
{
    std::cout << "==========" << std::endl;
    std::cout << "Invalid size matrices should be rejected." << std::endl;
    std::cout << "==========" << std::endl;
    {
        using namespace khover;

        Eigen::Matrix<int,6,4> mat, a, b;
        Eigen::Matrix<int,4,6> c, d;
        Eigen::Matrix<int,3,3> e;

        auto fst = hnf_LLL<rowops>(mat,std::tie(b,c,d),std::tie(a,b));
        auto snd = hnf_LLL<rowops>(mat,std::tie(c,d),std::tie(a,b,c));
        auto trd = hnf_LLL<colops>(mat,std::tie(c,d,e),std::tie(a,b));
        auto fth = hnf_LLL<colops>(mat,std::tie(c,d),std::tie(e,a,b));

        if(fst || snd || trd || fth)
        {
            std::cout << "Failed to detect wrong size matrices" << std::endl;
            return -1;
        }
    }

    std::cout << "==========" << std::endl;
    std::cout << "Row HNF" << std::endl;
    std::cout << "==========" << std::endl;
    {
        Eigen::Matrix<std::int64_t, 3,4> mat0;
        mat0 <<
            1, 2, 3, 4,
            5, 6, 7, 8,
            9, 10, 11, 12;
        auto mat = mat0;
        auto u = Eigen::Matrix<std::int64_t,3,3>::Identity().eval();
        auto rk = khover::hnf_LLL<typename khover::rowops>(mat,std::tie(u),std::tuple<>{});

        if (!is_rowHNF(mat) || u*mat != mat0 || !is_unimodular(u) || !rk || *rk!=2)
        {
            std::cout << "mat=" << std::endl;
            std::cout << mat << std::endl;
            std::cout << std::endl;
            std::cout << "u=" << std::endl;
            std::cout << u << std::endl;
            std::cout << "rk=" << (rk ? *rk : -1) << std::endl;
            std::cout << "u*mat=" << std::endl;
            std::cout << u*mat << std::endl;
            return -1;
        }
    }
    {
        Eigen::Matrix<std::int64_t, 3,4> mat0;
        mat0 <<
            0, 1, 2, 3,
            4, 5, 6, 7,
            8, 9,10,11;
        auto mat = mat0;
        auto u = Eigen::Matrix<std::int64_t,3,3>::Identity().eval();
        auto rk = khover::hnf_LLL<typename khover::rowops>(mat,std::tie(u),std::tuple<>{});

        if (!is_rowHNF(mat) || u*mat != mat0 || !is_unimodular(u) || !rk || *rk!=2)
        {
            std::cout << "mat=" << std::endl;
            std::cout << mat << std::endl;
            std::cout << std::endl;
            std::cout << "u=" << std::endl;
            std::cout << u << std::endl;
            std::cout << "rk=" << (rk ? *rk : -1) << std::endl;
            std::cout << "u*mat=" << std::endl;
            std::cout << u*mat << std::endl;
            return -1;
        }
    }

    std::cout << "==========" << std::endl;
    std::cout << "Column HNF" << std::endl;
    std::cout << "==========" << std::endl;
    {
        Eigen::Matrix<std::int64_t, 5,4> mat0;
        mat0 <<
            1, 2, 4, 7,
            2, 3, 4, 5,
            6, 7, 8, 9,
            1, 1, 2, 3,
            5, 8, 13, 21;
        auto mat = mat0;
        Eigen::Matrix<std::int64_t,4,4> u = decltype(u)::Identity();
        Eigen::Matrix<std::int64_t,4,4> v = decltype(v)::Identity();
        auto rk = khover::hnf_LLL<typename khover::colops>(mat,std::tie(u),std::tie(v));

        if (!is_colHNF(mat) || mat*u != mat0
            || v*u != Eigen::Matrix<std::int64_t,4,4>::Identity()
            || !rk || *rk != 4)
        {
            std::cout << "mat=" << std::endl;
            std::cout << mat << std::endl;
            std::cout << std::endl;
            std::cout << "u=" << std::endl;
            std::cout << u << std::endl;
            std::cout << "rk=" << (rk ? *rk : -1) << std::endl;
            return -1;
        }
    }
    {
        Eigen::Matrix<std::int64_t,3,3> mat0;
        mat0 <<
            1, 1, 0,
           -1, 0, 1,
            0,-1,-1;

        Eigen::Matrix<std::int64_t,3,3> u0;
        u0 <<
            1, 0, 0,
           -1, 1, 0,
            1, 0, 1;
        decltype(mat0) mat = mat0;
        decltype(u0) u = u0;
        Eigen::Matrix<std::int64_t,3,3> v = decltype(v)::Identity();
        auto rk = khover::hnf_LLL<typename khover::colops>(
            mat,std::tie(u),std::tie(v));

        if (!is_colHNF(mat) || mat*u != mat0*u0
            || v*u != u0
            || !rk || *rk != 2)
        {
            ERR_MSG(
                "mat=\n" << mat << std::endl
                << "u=\n" << u << std::endl
                << "v=\n" << v << std::endl
                << "rk=" << (rk ? *rk : -1) << std::endl
                << "mat0*u0=\n" << mat0*u0 << std::endl
                << "mat*u=\n" << mat*u << std::endl
                << "v*u=\n" << v*u
                );
            return -1;
        }
    }
    {
        Eigen::Matrix<std::int64_t, 3,4> mat0;
        mat0 <<
            0, 1, 2, 3,
            4, 5, 6, 7,
            8, 9,10,11;
        auto mat = mat0;
        auto u = Eigen::Matrix<std::int64_t,4,4>::Identity().eval();
        auto rk = khover::hnf_LLL<typename khover::colops>(mat,std::tie(u),std::tuple<>{});

        if (!is_colHNF(mat) || mat*u != mat0 || !is_unimodular(u) || !rk || *rk!=2)
        {
            ERR_MSG(
                "mat=\n" << mat << std::endl
                << "u=\n" << u << std::endl
                << "rk=" << (rk ? *rk : -1) << std::endl
                << "mat*u=\n" << mat*u
                );
            return -1;
        }
    }
    {
        Eigen::Matrix<std::int64_t, 9,27> mat0;
        mat0 <<
            1, 0, 0, 0, 0, 1, 0, 1, 3, 0, 0, 1, 0, 0, 0,-1, 0, 0, 0, 1, 3,-1, 0, 0,-3, 0, 0,
            0, 1, 0, 1, 0,-3, 0,-3,-8, 1, 0,-3, 0, 0, 0, 0,-1, 0, 0,-3,-8, 0,-1, 0, 0,-3, 0,
            0, 0, 1, 0, 1, 3, 1, 3, 6, 0, 1, 3, 0, 0, 0, 0, 0,-1, 1, 3, 6, 0, 0,-1, 0, 0,-3,
            0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 3, 1, 3, 0, 0, 0, 3, 1, 3, 8, 0, 0,
            0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 0,-6, 0, 0,-8, 0, 0, 0, 0, 0,-8, 0, 8, 0,
            0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 6, 1, 3, 9, 0, 0, 0, 1, 3, 9, 0, 0, 8,
            0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,-1, 0, 0,-3, 0, 1, 0, 0, 0,-3, 0, 1,-6, 2, 6,
            0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,-1, 0, 1,-3,-3, 0, 0, 0, 1,-3,-3, 0,-12,-16,
            0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,-1, 0, 1, 0, 0, 0, 0, 0, 1, 0, 2, 6, 6;
        auto mat = mat0;
        auto u = Eigen::Matrix<std::int64_t,27,27>::Identity().eval();
        auto v = Eigen::Matrix<std::int64_t,27,27>::Identity().eval();
        auto rk = khover::hnf_LLL<khover::colops>(mat,std::tie(u),std::tie(v));

        if (!is_colHNF(mat)
            || mat*u != mat0
            || !(u*v).isIdentity() ||
            !rk || *rk!=7)
        {
            ERR_MSG(
                "mat=\n" << mat << std::endl
                << "u=\n" << u << std::endl
                << "rk=" << (rk ? *rk : -1) << std::endl
                << "mat*u=\n" << mat*u
                );
            return -1;
        }
        std::cout << mat.leftCols(1+*rk) << std::endl;
    }

    std::cout << "==========" << std::endl;
    std::cout << "HNF of zero matrices" << std::endl;
    std::cout << "==========" << std::endl;
    {
        using matrix_t = Eigen::Matrix<std::int64_t,Eigen::Dynamic,Eigen::Dynamic>;
        matrix_t zmat;

        for(std::size_t r = 0; r < 5; ++r) {
            for(std::size_t c = 0; c < 5; ++c) {
                // test row HNF
                zmat = matrix_t::Zero(r,c);
                auto rowrk = khover::hnf_LLL<khover::rowops>(zmat, {}, {});
                if (!rowrk || *rowrk != 0 || !zmat.isZero()) {
                    std::cout << "row HNF of zero is not zero!" << std::endl;
                    std::cout << "rk=" << (rowrk ? *rowrk : -1) << std::endl;
                    std::cout << zmat << std::endl;
                    return -1;
                }
                // test column HNF
                zmat.setZero();
                auto colrk = khover::hnf_LLL<khover::colops>(zmat, {}, {});
                if (!colrk || *colrk!=0 || !zmat.isZero()) {
                    std::cout << "column HNF of zero is not zero!" << std::endl;
                    std::cout << "rk=" << (colrk ? *colrk : -1) << std::endl;
                    std::cout << zmat << std::endl;
                    return -1;
                }
            }
        }
    }

    return 0;
}


#pragma once
#include <tuple>
#include <vector>
#include <Eigen/Dense>

typedef std::tuple<double, double, double, double, double> quintet;
typedef std::tuple<std::vector<double>, double, double, double, double>
        vquintet;

// user interface

namespace fine {
Eigen::VectorXd heston_formula(std::vector<double> strikes,
                               std::vector<double> maturities,
                               double underlying_price, double volatility,
                               double interest_rate, double kappa, double theta,
                               double nu, double rho);

Eigen::VectorXd heston_delta(std::vector<double> strikes,
                             std::vector<double> maturities,
                             double underlying_price, double volatility,
                             double interest_rate, double kappa, double theta,
                             double nu, double rho);

Eigen::VectorXd heston_vega(std::vector<double> strikes,
                            std::vector<double> maturities,
                            double underlying_price, double volatility,
                            double interest_rate, double kappa, double theta,
                            double nu, double rho);

quintet heston_calibration(std::vector<double> prices,
                           std::vector<double> strikes,
                           std::vector<double> maturities,
                           double underlying_price,
                           double interest_rate);

vquintet heston_calibration_ts(std::vector<double> prices,
                               std::vector<double> strikes,
                               std::vector<double> maturities,
                               std::vector<double> underlying_prices,
                               double interest_rate);

double heston_calibrate_vol(std::vector<double> prices,
                            std::vector<double> strikes,
                            std::vector<double> maturities,
                            double underlying_price, double interest_rate,
                            double kappa, double theta, double nu, double rho);
}


#ifndef SIMPLEMLP_HPP
#define SIMPLEMLP_HPP

#include <string>
#include <vector>
#include <Eigen/Dense>
#include <cstdlib>
#include "iostream"
#include <fstream>
#include <cmath>

namespace rai {

namespace FuncApprox {

enum class ActivationType {
  linear,
  relu,
  tanh,
  softsign
};

template<typename Dtype, ActivationType activationType>
struct Activation {
  inline void nonlinearity(Eigen::Matrix<Dtype, -1, -1> &output) {}
  inline void nonlinearity(Eigen::Matrix<Dtype, -1, 1> &output) {}
};

template<typename Dtype>
struct Activation<Dtype, ActivationType::relu> {
  inline void nonlinearity(Eigen::Matrix<Dtype, -1, -1> &output) {
    output = output.cwiseMax(0.0);
  }

  inline void nonlinearity(Eigen::Matrix<Dtype, -1, 1> &output) {
    output = output.cwiseMax(0.0);
  }
};

template<typename Dtype>
struct Activation<Dtype, ActivationType::tanh> {
  inline void nonlinearity(Eigen::Matrix<Dtype, -1, -1> &output) {
    output = output.array().tanh();
  }
  inline void nonlinearity(Eigen::Matrix<Dtype, -1, 1> &output) {
    output = output.array().tanh();
  }
};

template<typename Dtype>
struct Activation<Dtype, ActivationType::softsign> {
  inline void nonlinearity(Eigen::Matrix<Dtype, -1, -1> &output) {
    for (int i = 0; i < output.size(); i++) {
      output[i] = output[i] / (std::abs(output[i]) + 1.0);
    }
  }

  inline void nonlinearity(Eigen::Matrix<Dtype, -1, 1> &output) {
    for (int i = 0; i < output.size(); i++) {
      output[i] = output[i] / (std::abs(output[i]) + 1.0);
    }
  }
};

template<typename Dtype, int StateDim, int ActionDim, ActivationType activationType>
class MLP_fullyconnected {

 public:
  typedef Eigen::Matrix<Dtype, ActionDim, 1> Action;
  typedef Eigen::Matrix<Dtype, StateDim, 1> State;

  MLP_fullyconnected(std::vector<int> hiddensizes) {
    const static Eigen::IOFormat CSVFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", "\n");

    layersizes.push_back(StateDim);
    layersizes.reserve(layersizes.size() + hiddensizes.size());
    layersizes.insert(layersizes.end(), hiddensizes.begin(), hiddensizes.end());
    layersizes.push_back(ActionDim);
    ///[input hidden output]

    params.resize(2 * (layersizes.size() - 1));
    Ws.resize(layersizes.size() - 1);
    bs.resize(layersizes.size() - 1);
    lo.resize(layersizes.size());
    Stdev.resize(ActionDim);

    for (int i = 0; i < params.size(); i++) {
      int paramSize = 0;

      if (i % 2 == 0) ///W resize
      {
        Ws[i / 2].resize(layersizes[i / 2 + 1], layersizes[i / 2]);
        params[i].resize(layersizes[i / 2] * layersizes[i / 2 + 1]);
      }
      if (i % 2 == 1) ///b resize
      {
        bs[(i - 1) / 2].resize(layersizes[(i + 1) / 2]);
        params[i].resize(layersizes[(i + 1) / 2]);
      }
    }

  }

  bool load_eigen_from_binary(const std::string filename, Eigen::Matrix<Dtype, Eigen::Dynamic, Eigen::Dynamic> &data) {
    std::ifstream in(filename, std::ios::in | std::ios::binary);
    if (!in.is_open()) {
      return false;
    }
    size_t rows = 0;
    size_t cols = 0;
    in.read((char *) (&rows), sizeof(size_t));
    in.read((char *) (&cols), sizeof(size_t));
    data.resize(rows, cols);
    in.read((char *) data.data(), rows * cols * sizeof(Dtype));

    in.close();
    return true;
  }

  void updateParamFromBin(std::string fileName) {
    Eigen::Matrix<Dtype, -1, -1> temp;
    load_eigen_from_binary(fileName, temp);

    size_t pos = 0;

    for (int i = 0; i < params.size(); i++) {
      int paramSize = 0;
      for (size_t j = 0; j < params[i].size(); j++) {
        params[i](j) = temp.data()[pos++];
      }
      if (i % 2 == 0) ///W copy
      {
        memcpy(Ws[i / 2].data(), params[i].data(), sizeof(Dtype) * Ws[i / 2].size());
      }
      if (i % 2 == 1) ///b copy
      {
        memcpy(bs[(i - 1) / 2].data(), params[i].data(), sizeof(Dtype) * bs[(i - 1) / 2].size());
      }
    }
  }

  void updateParamFromBin_noesis(std::string fileName) {
    Eigen::Matrix<Dtype, -1, -1> temp;
    load_eigen_from_binary(fileName, temp);

    for (int i = 0; i < params.size(); i++) {
      int paramSize = 0;

      if (i % 2 == 0) ///W resize
      {
        Ws[i / 2].resize(layersizes[i / 2 + 1], layersizes[i / 2]);
        params[i].resize(layersizes[i / 2] * layersizes[i / 2 + 1]);
      }
      if (i % 2 == 1) ///b resize
      {
        bs[(i - 1) / 2].resize(layersizes[(i + 1) / 2]);
        params[i].resize(layersizes[(i + 1) / 2]);
      }
    }

    /// output layer

    size_t pos = 0;

    // Wo
    for (size_t j = 0; j <params.end()[-2].size(); j++) {
      params.end()[-2](j) = temp.data()[pos++];
    }

    // bo
    for (size_t j = 0; j <params.end()[-1].size(); j++) {
      params.end()[-1](j) = temp.data()[pos++];
    }

    memcpy(Ws.back().data(), params.end()[-2].data(), sizeof(Dtype) * Ws.back().size());
    memcpy(bs.back().data(), params.end()[-1].data(), sizeof(Dtype) * bs.back().size());

    for (int i = 0; i < params.size() - 2; i++) {
      int paramSize = 0;
      for (size_t j = 0; j < params[i].size(); j++) {
        params[i](j) = temp.data()[pos++];
      }
      if (i % 2 == 0) ///W copy
      {
        memcpy(Ws[i / 2].data(), params[i].data(), sizeof(Dtype) * Ws[i / 2].size());
      }
      if (i % 2 == 1) ///b copy
      {
        memcpy(bs[(i - 1) / 2].data(), params[i].data(), sizeof(Dtype) * bs[(i - 1) / 2].size());
      }
    }
  }

  void updateParamFromTxt(std::string fileName) {
    const static Eigen::IOFormat CSVFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", "\n");

    std::ifstream indata;
    indata.open(fileName);
    std::string line;
    getline(indata, line);
    std::stringstream lineStream(line);
    std::string cell;

    int totalN = 0;
    ///assign parameters
    for (int i = 0; i < params.size(); i++) {
      int paramSize = 0;

      while (std::getline(lineStream, cell, ',')) { ///Read param
        params[i](paramSize++) = std::stod(cell);
        if (paramSize == params[i].size()) break;
      }
      totalN += paramSize;
      if (i % 2 == 0) ///W copy
        memcpy(Ws[i / 2].data(), params[i].data(), sizeof(Dtype) * Ws[i / 2].size());
      if (i % 2 == 1) ///b copy
        memcpy(bs[(i - 1) / 2].data(), params[i].data(), sizeof(Dtype) * bs[(i - 1) / 2].size());
    }

  }

  inline Action forward(State &state) {

    lo[0] = state;

    for (int cnt = 0; cnt < Ws.size() - 1; cnt++) {
      lo[cnt + 1] = Ws[cnt] * lo[cnt] + bs[cnt];
      activation_.nonlinearity(lo[cnt + 1]);
    }

    lo[lo.size() - 1] = Ws[Ws.size() - 1] * lo[lo.size() - 2] + bs[bs.size() - 1]; /// output layer
    return lo.back();
  }

  inline Action forwardtemp(State &state) {
    Eigen::Matrix<Dtype, -1, 1> temp;
    temp = state;

    for (int cnt = 0; cnt < Ws.size() - 1; cnt++) {
      temp = Ws[cnt] * temp + bs[cnt];
      activation_.nonlinearity(temp);
    }
    temp = Ws.back() * temp + bs.back(); /// output layer
    return temp;
  }

 private:
  std::vector<Eigen::Matrix<Dtype, -1, 1>> params;
  std::vector<Eigen::Matrix<Dtype, -1, -1>> Ws;
  std::vector<Eigen::Matrix<Dtype, -1, 1>> bs;
  std::vector<Eigen::Matrix<Dtype, -1, 1>> lo;

  Activation<Dtype, activationType> activation_;
  Action Stdev;

  std::vector<int> layersizes;
  bool isTanh = false;
};

}

}

#endif //SIMPLEMLP_HPP


//
// Created by krab1k on 31/10/18.
//

#include <vector>
#include <cmath>
#include <Eigen/LU>

#include "kcm.h"
#include "../structures/molecule.h"
#include "../parameters.h"

CHARGEFW2_METHOD(KCM)

std::vector<double> KCM::calculate_charges(const Molecule &molecule) const {

    size_t n = molecule.atoms().size();
    size_t m = molecule.bonds().size();

    Eigen::MatrixXd W = Eigen::MatrixXd::Zero(m, m);
    Eigen::MatrixXd B = Eigen::MatrixXd::Zero(m, n);
    Eigen::VectorXd chi0 = Eigen::VectorXd::Zero(n);

    /* Compute
     *  q = (B.T @ W @ B + I)^-1 @ chi0 - chi0
     */

    for (size_t i = 0; i < n; i++) {
        chi0(i) = parameters_->atom()->parameter(atom::electronegativity)(molecule.atoms()[i]);
    }

    for (size_t i = 0; i < m; i++) {
        auto &bond = molecule.bonds()[i];
        auto &first = bond.first();
        auto &second = bond.second();

        W(i, i) = 1 / (parameters_->atom()->parameter(atom::hardness)(first) +
                            parameters_->atom()->parameter(atom::hardness)(second));

        B(i, first.index()) = 1;
        B(i, second.index()) = -1;
    }

    Eigen::VectorXd q = (B.transpose() * W * B + Eigen::MatrixXd::Identity(n, n)).partialPivLu().solve(chi0) - chi0;
    return std::vector<double>(q.data(), q.data() + q.size());
}


/* ----------------------------------------------------------------------------
 * Copyright 2022, Jeferson Lima
 * All Rights Reserved
 * See LICENSE for the license information
 * -------------------------------------------------------------------------- */

/**
 *  @file   examples/translation_ex.cpp
 *  @author Jeferson Lima
 *  @brief  Translation Matrix Example
 *  @date   Mar 11, 2022
 **/

#include <iostream>
#include <Eigen/Core>

void trans3dVec(const Eigen::Matrix<float, 4, 1>& v,
                Eigen::Matrix<float, 4, 1>& r,
		const Eigen::Matrix<float, 3, 1>& q)
{

  Eigen::Matrix<float, 4, 4> translation;
  translation << 1.0, 0.0, 0.0, q[0],
                 0.0, 1.0, 0.0, q[1],
		 0.0, 0.0, 1.0, q[2],
		 0.0, 0.0, 0.0, 1.0;

  std::cout << "v values: \n" << v << std::endl;
  std::cout << "translation values: \n" << translation << std::endl;

  r = translation * v;

}

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

  Eigen::Matrix<float, 4, 1> v;
  Eigen::Matrix<float, 4, 1> r;
  Eigen::Matrix<float, 3, 1> Q;

  // init matrix
  v << 0.5, 0.5, 0.0, 1.0;
  Q << 1.0, 1.0, 0.0;

  trans3dVec(v, r, Q);

  std::cout << "r:\n" << r << std::endl;

  return 0;
}


#include <dlib/optimization.h>

#include <bfgs/bfgs.h>
#include <bfgs/dval.h>

int _fun_call = 0;

// Rosenbrock's function.
class FRosen
{
private:
	const double _p1;

	template <typename T>
	T _f(const T& x1, const T& x2) const
	{
		++_fun_call;
		T v1 = x2 - x1 * x1;
		T v2 = _p1 - x1;
		T res = 100.0 * v1 * v1 + v2 * v2 + 1.0;
		return res;
	}

public:
	FRosen(double p1 = 1.0):
		_p1(p1)
	{
	}

	double operator()(const dlib::matrix<double,0,1>& m) const
	{
		return _f(m(0), m(1));
	}

	double operator()(const double* const x, int n) const
	{
		return _f(x[0], x[1]);
	}

	DVal<0> operator()(const DVal<0>* const x, uint32_t n) const
	{
		return _f(x[0], x[1]);
	}

	template <uint32_t N>
	DVal<N> operator()(const DVal<N>* const x, uint32_t n) const
	{
		return _f(x[0], x[1]);
	}
};

// Simple function.
class FSimple
{
private:
	const double _p1;

	template <typename T>
	T _f(const T& x1, const T& x2) const
	{
		++_fun_call;
		return _p1 * x1 * x1 + x2 * x2 + x1 + x2;
	}

public:
	FSimple(double p1 = 1.0):
		_p1(p1)
	{
	}

	double operator()(const dlib::matrix<double,0,1>& m) const
	{
		return _f(m(0), m(1));
	}

	double operator()(const double* const x, int n) const
	{
		return _f(x[0], x[1]);
	}

	DVal<0> operator()(const DVal<0>* const x, uint32_t n) const
	{
		return _f(x[0], x[1]);
	}

	template <uint32_t N>
	DVal<N> operator()(const DVal<N>* const x, uint32_t n) const
	{
		return _f(x[0], x[1]);
	}
};

template <typename fun>
void test(const fun& f)
{
	const int iter = 1000;
	// const int iter = 1;

	{
		dlib::matrix<double,0,1> dlib_point = {-1.0, -1.0};
		double dlib_y;

		auto beg = std::chrono::steady_clock::now();
		for (int i = 0; i < iter; ++i)
		{
			_fun_call = 0;
			dlib_point(0) = -1.0;
			dlib_point(1) = -1.0;
			dlib_y = dlib::find_min_using_approximate_derivatives(
					dlib::bfgs_search_strategy(),
					// dlib::gradient_norm_stop_strategy(1e-7, 1000),
					dlib::objective_delta_stop_strategy(1e-7, 1000),
					f,
					dlib_point,
					-1,
					1e-8);
		}

		auto end = std::chrono::steady_clock::now();
		double dt = std::chrono::duration_cast<std::chrono::nanoseconds>(end - beg).count() * 1e-9;

		std::cout << "\tdlib" << std::endl;
		std::cout << "fun_call: " << _fun_call << "\ndt (us): " << 1e6 * dt / iter << "\nsolution: " << dlib_y  << std::endl <<  dlib_point;
	}

	BFGS bfgs;
	bfgs.set_grad_eps(1e-8);
	bfgs.set_stop_grad_eps(1e-7);
	bfgs.set_stop_step_eps(1e-7);
	bfgs.set_max_iter(1000);
	bfgs.set_line_central_diff(false);

	{
		bfgs.set_lbfgs_m(0);
		const uint32_t n = 2;
		double bfgs_point[n] = {-1.0, -1.0};
		double bfgs_y = bfgs.find_min_num(f, bfgs_point, n);

		auto beg = std::chrono::steady_clock::now();
		for (int i = 0; i < iter; ++i)
		{
			_fun_call = 0;
			bfgs_point[0] = -1.0;
			bfgs_point[1] = -1.0;
			bfgs_y = bfgs.find_min_num(f, bfgs_point, n);
		}
		auto end = std::chrono::steady_clock::now();
		double dt = std::chrono::duration_cast<std::chrono::nanoseconds>(end - beg).count() * 1e-9;

		std::cout << "\tbfgs num" << std::endl;
		std::cout << "fun_call: " << _fun_call << "\ndt (us): " << 1e6 * dt / iter << "\nsolution: " << bfgs_y << std::endl;
		for (int i = 0; i < n; ++i)
			std::cout << bfgs_point[i] << std::endl;
	}

	{
		bfgs.set_lbfgs_m(5);
		const uint32_t n = 2;
		double bfgs_point[n] = {-1.0, -1.0};
		double bfgs_y = bfgs.find_min_num(f, bfgs_point, n);

		auto beg = std::chrono::steady_clock::now();
		for (int i = 0; i < iter; ++i)
		{
			_fun_call = 0;
			bfgs_point[0] = -1.0;
			bfgs_point[1] = -1.0;
			bfgs_y = bfgs.find_min_num(f, bfgs_point, n);
		}
		auto end = std::chrono::steady_clock::now();
		double dt = std::chrono::duration_cast<std::chrono::nanoseconds>(end - beg).count() * 1e-9;

		std::cout << "\tlbfgs num" << std::endl;
		std::cout << "fun_call: " << _fun_call << "\ndt (us): " << 1e6 * dt / iter << "\nsolution: " << bfgs_y << std::endl;
		for (int i = 0; i < n; ++i)
			std::cout << bfgs_point[i] << std::endl;
	}

	{
		bfgs.set_lbfgs_m(0);
		const uint32_t n = 2;
		double bfgs_point[n] = {-1.0, -1.0};
		double bfgs_y = bfgs.find_min_auto(f, bfgs_point, n);

		auto beg = std::chrono::steady_clock::now();
		for (int i = 0; i < iter; ++i)
		{
			_fun_call = 0;
			bfgs_point[0] = -1.0;
			bfgs_point[1] = -1.0;
			bfgs_y = bfgs.find_min_auto(f, bfgs_point, n);
		}
		auto end = std::chrono::steady_clock::now();
		double dt = std::chrono::duration_cast<std::chrono::nanoseconds>(end - beg).count() * 1e-9;

		std::cout << "\tbfgs auto dynamic" << std::endl;
		std::cout << "fun_call: " << _fun_call << "\ndt (us): " << 1e6 * dt / iter << "\nsolution: " << bfgs_y << std::endl;
		for (int i = 0; i < n; ++i)
			std::cout << bfgs_point[i] << std::endl;
	}

	{
		bfgs.set_lbfgs_m(0);
		const uint32_t n = 2;
		double bfgs_point[n] = {-1.0, -1.0};
		double bfgs_y = bfgs.find_min_auto<2>(f, bfgs_point, n);

		auto beg = std::chrono::steady_clock::now();
		for (int i = 0; i < iter; ++i)
		{
			_fun_call = 0;
			bfgs_point[0] = -1.0;
			bfgs_point[1] = -1.0;
			bfgs_y = bfgs.find_min_auto<2>(f, bfgs_point, n);
		}
		auto end = std::chrono::steady_clock::now();
		double dt = std::chrono::duration_cast<std::chrono::nanoseconds>(end - beg).count() * 1e-9;

		std::cout << "\tbfgs auto fixed" << std::endl;
		std::cout << "fun_call: " << _fun_call << "\ndt (us): " << 1e6 * dt / iter << "\nsolution: " << bfgs_y << std::endl;
		for (int i = 0; i < n; ++i)
			std::cout << bfgs_point[i] << std::endl;
	}

	std::cout << std::endl;
}

int main()
{
	std::cout.precision(9);

	for (int i = 1; i <= 100; i *= 10)
		test(FRosen(i));
	for (int i = 1; i <= 100; i *= 10)
		test(FSimple(i));

	return 0;
}


/**
 * @file gradientflow_main.cc
 * @brief NPDE homework GradientFlow code
 * @author Unknown, Oliver Rietmann
 * @date 04.04.2021
 * @copyright Developed at ETH Zurich
 */

#include <Eigen/Core>
#include <iostream>
#include <vector>

#include "gradientflow.h"

const static Eigen::IOFormat CSVFormat(Eigen::FullPrecision,
                                       Eigen::DontAlignCols, ", ", "\n");

int main() {
  // Parameters and initial condition of the Gradient Flow ODE
  double T = 0.1;
  double lambda = 10.0;
  Eigen::Vector2d d(1.0, 0.0);
  Eigen::Vector2d y0(1.0, 0.0);

  // Approximate exact solution using small timesteps
  int M_ref = 10000;
  std::cout << "T = " << T << ", lambda = " << lambda << std::endl;
  Eigen::Vector2d y_ref =
      GradientFlow::SolveGradientFlow(d, lambda, y0, T, M_ref).back();

  std::cout << "Final value (exact): " << y_ref.transpose().format(CSVFormat)
            << std::endl;

  // Compute error table
  Eigen::VectorXi M(6);
  M << 10, 20, 40, 80, 160, 320;
  std::cout << "Error table:\n";
  std::cout << "M\t error norm" << std::endl;
  for (int i = 0; i < M.size(); ++i) {
    Eigen::Vector2d y_approx =
        GradientFlow::SolveGradientFlow(d, lambda, y0, T, M(i)).back();
    std::cout << M(i) << "\t" << (y_approx - y_ref).norm() << "\t" << std::endl;
  }

  return 0;
}


#include <simpleuv/triangulate.h>
#include <Eigen/Dense>
#include <cmath>

namespace simpleuv
{

static Eigen::Vector3d norm(const Eigen::Vector3d &p1, const Eigen::Vector3d &p2, const Eigen::Vector3d &p3)
{
    auto side1 = p2 - p1;
    auto side2 = p3 - p1;
    auto perp = side1.cross(side2);
    return perp.normalized();
}

static float angle360(const Eigen::Vector3d &a, const Eigen::Vector3d &b, const Eigen::Vector3d &direct)
{
    auto angle = atan2((a.cross(b)).norm(), a.dot(b)) * 180.0 / 3.1415926;
    auto c = a.cross(b);
    if (c.dot(direct) < 0) {
        angle += 180;
    }
    return angle;
}

static Eigen::Vector3d vertexToEigenVector3d(const Vertex &vertex)
{
    return Eigen::Vector3d(vertex.xyz[0], vertex.xyz[1], vertex.xyz[2]);
}

static bool pointInTriangle(const Eigen::Vector3d &a, const Eigen::Vector3d &b, const Eigen::Vector3d &c, const Eigen::Vector3d &p)
{
    auto u = b - a;
    auto v = c - a;
    auto w = p - a;
    auto vXw = v.cross(w);
    auto vXu = v.cross(u);
    if (vXw.dot(vXu) < 0.0) {
        return false;
    }
    auto uXw = u.cross(w);
    auto uXv = u.cross(v);
    if (uXw.dot(uXv) < 0.0) {
        return false;
    }
    auto denom = uXv.norm();
    auto r = vXw.norm() / denom;
    auto t = uXw.norm() / denom;
    return r + t <= 1.0;
}

static Eigen::Vector3d ringNorm(const std::vector<Vertex> &vertices, const std::vector<size_t> &ring)
{
    Eigen::Vector3d normal;
    for (size_t i = 0; i < ring.size(); ++i) {
        auto j = (i + 1) % ring.size();
        auto k = (i + 2) % ring.size();
        const auto &enter = vertexToEigenVector3d(vertices[ring[i]]);
        const auto &cone = vertexToEigenVector3d(vertices[ring[j]]);
        const auto &leave = vertexToEigenVector3d(vertices[ring[k]]);
        normal += norm(enter, cone, leave);
    }
    return normal.normalized();
}

void triangulate(const std::vector<Vertex> &vertices, std::vector<Face> &faces, const std::vector<size_t> &ring)
{
    if (ring.size() < 3)
        return;
    std::vector<size_t> fillRing = ring;
    Eigen::Vector3d direct = ringNorm(vertices, fillRing);
    while (fillRing.size() > 3) {
        bool newFaceGenerated = false;
        for (decltype(fillRing.size()) i = 0; i < fillRing.size(); ++i) {
            auto j = (i + 1) % fillRing.size();
            auto k = (i + 2) % fillRing.size();
            const auto &enter = vertexToEigenVector3d(vertices[fillRing[i]]);
            const auto &cone = vertexToEigenVector3d(vertices[fillRing[j]]);
            const auto &leave = vertexToEigenVector3d(vertices[fillRing[k]]);
            auto angle = angle360(cone - enter, leave - cone, direct);
            if (angle >= 1.0 && angle <= 179.0) {
                bool isEar = true;
                for (size_t x = 0; x < fillRing.size() - 3; ++x) {
                    auto fourth = vertexToEigenVector3d(vertices[(i + 3 + k) % fillRing.size()]);
                    if (pointInTriangle(enter, cone, leave, fourth)) {
                        isEar = false;
                        break;
                    }
                }
                if (isEar) {
                    Face newFace;
                    newFace.indices[0] = fillRing[i];
                    newFace.indices[1] = fillRing[j];
                    newFace.indices[2] = fillRing[k];
                    faces.push_back(newFace);
                    fillRing.erase(fillRing.begin() + j);
                    newFaceGenerated = true;
                    break;
                }
            }
        }
        if (!newFaceGenerated)
            break;
    }
    if (fillRing.size() == 3) {
        Face newFace;
        newFace.indices[0] = fillRing[0];
        newFace.indices[1] = fillRing[1];
        newFace.indices[2] = fillRing[2];
        faces.push_back(newFace);
    }
}

}


/** @file 33.cpp Problem 33: Digit canceling fractions
 *
 * The fraction 49/98 is a curious fraction, as an inexperienced mathematician
 * in attempting to simplify it may incorrectly believe that 49/98 = 4/8, which
 * is correct, is obtained by cancelling the 9s.

 * We shall consider fractions like, 30/50 = 3/5, to be trivial examples.
 *
 * There are exactly four non-trivial examples of this type of fraction, less
 * than one in value, and containing two digits in the numerator and
 * denominator.
 *
 * If the product of these four fractions is given in its lowest common terms,
 * find the value of the denominator.
 */

/// @cond
#include <boost/multiprecision/cpp_int.hpp> // cpp_rational, denominator

#include <iostream> // cout
/// @endcond

using boost::multiprecision::cpp_rational;

bool equal(int n1, int d1, int n2, int d2)
{
    return d1 && d2 && cpp_rational(n1, d1) == cpp_rational(n2, d2);
}

int main()
{
    cpp_rational r = 1;
    for (int i = 10; i < 100; ++i) {
        for (int j = i + 1; j < 100; ++j) {
            if (i % 10 == 0 && j % 10 == 0)
                continue;   // Skip trivial examples.
            int a = i / 10, b = i % 10,
                c = j / 10, d = j % 10;
            if (       (b == d && equal(i, j, a, c))
                    || (b == c && equal(i, j, a, d))
                    || (a == d && equal(i, j, b, c))
                    || (a == c && equal(i, j, b, d)))
                r *= cpp_rational(i, j);
        }
    }
    std::cout << denominator(r) << std::endl;
}


// Copyright 2022 Eugen Hartmann. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include <chrono>
#include <iostream>
#include <format>

#include <boost/exception/diagnostic_information.hpp>
#include <boost/exception_ptr.hpp>

#include <openssl/bn.h>

#include "base-helpers.h"
#include "diffie-hellman-boost.h"

using cpp_int = boost::multiprecision::cpp_int;

static std::string
ConverCPPIntToHex(const boost::multiprecision::cpp_int& num) {
  std::stringstream ss;
  ss << std::uppercase << std::hex << num;
  std::string hex = ss.str();
  // Add the top 0 if necessary
  if (hex.size() % 2) {
    hex = "0" + hex;
  }
  return hex;
}

static boost::multiprecision::cpp_int
ConveryHexToCPPInt(std::string_view hex) {
  return boost::multiprecision::cpp_int{std::string("0x") + std::string(hex)};
}

DiffieHellmanBoost::DiffieHellmanBoost() {
}

DiffieHellmanBoost::~DiffieHellmanBoost() {
}

std::string_view DiffieHellmanBoost::GetImplementationName() const {
  return "boost cpp_int DH";
}

Result DiffieHellmanBoost::GenerateParameters(int primeLengthInBits, int generator) {
  if (generator <= 1) {
    return Result{Result::Fail, "Bad generator value."};
  }

  try {
    unsigned int minMillerRabinChecks = GetMillerRabinMinChecks(primeLengthInBits);

    // We must use different generators for the tests and prime generation,
    // otherwise we get false positives.
    // https://www.boost.org/doc/libs/1_60_0/libs/multiprecision/doc/html/boost_multiprecision/tut/primetest.html
    boost::random::mt11213b primeGenerator(clock());
    boost::random::mt19937 testGenerator(clock());

    std::array<std::uint16_t, Primes.size()> mods{};

    while (true) {
      cpp_int prime = ProbableSafePrime(primeLengthInBits, generator, primeGenerator);
      if (miller_rabin_test(prime, minMillerRabinChecks, testGenerator)) {
        // The value is probably prime, see if (prime - 1) / 2 is also prime.
        if (miller_rabin_test((prime - 1) / 2, minMillerRabinChecks, testGenerator)) {
          prime_ = std::move(prime);
          generator_ = cpp_int{generator};
          break;
        }
      }
    }
  } catch (const boost::exception& e) {
    return {Result::Fail, boost::diagnostic_information(e)};
  }

  return {Result::Success};
}

Result DiffieHellmanBoost::SetParameters(std::string_view hexPrime, std::string_view hexGenerator) {
  try {
    prime_ = ConveryHexToCPPInt(hexPrime);
    generator_ = ConveryHexToCPPInt(hexGenerator);

  } catch (const boost::exception& e) {
    return {Result::Fail, boost::diagnostic_information(e)};
  }

  return Result{Result::Success};
}
Result DiffieHellmanBoost::GetParameters(std::string* hexPrime, std::string* hexGenerator) const {
  try {
    *hexPrime = ConverCPPIntToHex(prime_);
    *hexGenerator = ConverCPPIntToHex(generator_);

  } catch (const boost::exception& e) {
    return {Result::Fail, boost::diagnostic_information(e)};
  }

  return Result{Result::Success};
}

Result DiffieHellmanBoost::GenerateKeys() {
  try {
    // It will return prime len - 1 anyway because we always set the top bit.
    // For example, 255 for a 256 bit prime.
    unsigned int len = boost::multiprecision::msb(prime_);

    // Configure the generator.
    cpp_int max = cpp_int{1} << len;
    boost::random::uniform_int_distribution<cpp_int> generatePrivateKey{0, max};
    boost::random::mt11213b privateKeyGenerator(clock());

    // Generate the private key.
    cpp_int privateKey = generatePrivateKey(privateKeyGenerator);

    // Derive the public key.
    cpp_int publicKey = boost::multiprecision::powm(generator_, privateKey, prime_);

    privateKey_ = std::move(privateKey);
    publicKey_ = std::move(publicKey);

  } catch (const boost::exception& e) {
    return {Result::Fail, boost::diagnostic_information(e)};
  }

  return {Result::Success};
}

Result DiffieHellmanBoost::GetPrivateKey(std::string* hexPrivateKey) const {
  try {
    hexPrivateKey->clear();
    hexPrivateKey->append(ConverCPPIntToHex(privateKey_));
    if (hexPrivateKey->empty()) {
      return {Result::Fail, "The private key is empty."};
    }
  } catch (const boost::exception& e) {
    return {Result::Fail, boost::diagnostic_information(e)};
  }

  return Result{Result::Success};
}

Result DiffieHellmanBoost::GetPublicKey(std::string* hexPublicKey) const {
  try {
    hexPublicKey->clear();
    hexPublicKey->append(ConverCPPIntToHex(publicKey_));
    if (hexPublicKey->empty()) {
      return {Result::Fail, "The public key is empty."};
    }
  } catch (const boost::exception& e) {
    return {Result::Fail, boost::diagnostic_information(e)};
  }

  return Result{Result::Success};
}

static Result CheckPublicKey(const boost::multiprecision::cpp_int& prime,
    const boost::multiprecision::cpp_int& publicKey) {
  try {
    if (publicKey <= 1) {
      return {Result::Fail, "The public key is too small."};
    }
    if (publicKey >= prime) {
      return {Result::Fail, "The public key is too large."};
    }

  } catch (const boost::exception& e) {
    return {Result::Fail, boost::diagnostic_information(e)};
  }

  return {Result::Success};
}

Result DiffieHellmanBoost::DeriveSharedSecret(std::string_view hexPeerPublicKey,
    std::string* hexSharedSecret) const {
  try {
    cpp_int peerPublicKey = ConveryHexToCPPInt(hexPeerPublicKey);

    if (Result result = CheckPublicKey(prime_, peerPublicKey); !result) {
      return result;
    }

    cpp_int sharedSecret = boost::multiprecision::powm(peerPublicKey, privateKey_, prime_);
    *hexSharedSecret = ConverCPPIntToHex(sharedSecret);

  } catch (const boost::exception& e) {
    return {Result::Fail, boost::diagnostic_information(e)};
  }
  return {Result::Success};
}

std::tuple<int, int> DiffieHellmanBoost::GetPrimeLength() const {
  try {
    // bytes = (bits - 1) / 8 + 1
    return {boost::multiprecision::msb(prime_) + 1,
      boost::multiprecision::msb(prime_) / 8 + 1};
  } catch (...) {
  }

  return {-1, -1};
}

std::tuple<int, int> DiffieHellmanBoost::GetPrivateKeyLength() const {
  try {
    return {boost::multiprecision::msb(privateKey_) + 1,
      boost::multiprecision::msb(privateKey_) / 8 + 1};
  } catch (...) {
  }

  return {-1, -1};
}

std::tuple<int, int> DiffieHellmanBoost::GetPublicKeyLength() const {
  try {
    return {boost::multiprecision::msb(publicKey_) + 1,
      boost::multiprecision::msb(publicKey_) / 8 + 1};
  } catch (...) {
  }

  return {-1, -1};
}

unsigned int DiffieHellmanBoost::GetMillerRabinMinChecks(int primeLengthInBits) {
  if (primeLengthInBits > 2048) {
    return 128;
  }
  return 64;
}

int DiffieHellmanBoost::GetTrivialDivisionNum(int primeLengthInBits) {
  if (primeLengthInBits <= 512) {
    return 64;
  } else if (primeLengthInBits <= 1024) {
    return 128;
  } else if (primeLengthInBits <= 2048) {
    return 384;
  } else if (primeLengthInBits <= 4096) {
    return 1024;
  }
  return Primes.size();
}

std::tuple<cpp_int, cpp_int> DiffieHellmanBoost::GetAddRem(int generator) {
  // See dh_builtin_genparams
  // libressl: crypto\dh\dh_gen.c
  boost::multiprecision::cpp_int add, rem;
  switch (generator) {
  case 2:
    add = 24;
    rem = 11;
    break;
  case 5:
    add = 10;
    rem = 3;
    break;
  default:
    add = 2;
    rem = 1;
  }
  return {add, rem};
}

cpp_int DiffieHellmanBoost::ProbableSafePrime(int primeLengthInBits,
    int generator, boost::random::mt11213b& primeGenerator) {
  int trivialDivisionNum = GetTrivialDivisionNum(primeLengthInBits);
  std::uint64_t maxDelta = 0xFFFFFFFFFFFFFFFFULL - Primes[trivialDivisionNum - 1];

  // Prepare cpp_int add/rem for the generator.
  auto [add, rem] = GetAddRem(generator);

  // Prepare the top bit.
  cpp_int topBit = cpp_int{0x80} << (primeLengthInBits - 8);

  // Prepare the bottom bit.
  cpp_int bottomBit{1};

  // Generation limits.
  cpp_int max = cpp_int{1} << primeLengthInBits;
  boost::random::uniform_int_distribution<cpp_int> generatePrime{0, max};

  cpp_int rnd;
  while (true) {
    rnd = generatePrime(primeGenerator);

    // Add top/bottom.
    rnd = rnd | topBit | bottomBit;

    // The prime should fulfill the condition prime % add == rem
    // in order to suit a given generator.
    cpp_int mod = rnd % add;
    rnd = rnd - mod + rem;

    // Probably the second condition is overhead?
    if (boost::multiprecision::msb(rnd) + 1 < primeLengthInBits || rnd < 5) {
      rnd += add;
    }

    auto [success, delta] = IncreaseProbabilityOfBeingPrime(primeLengthInBits, rnd, add);
    if (success) {
      rnd += delta;
      break;
    }
  }

  return rnd;
}

std::tuple<bool, std::uint64_t>
DiffieHellmanBoost::IncreaseProbabilityOfBeingPrime(int primeLengthInBits,
    cpp_int rnd, cpp_int add) {
  int trivialDivisionNum = GetTrivialDivisionNum(primeLengthInBits);

  std::uint64_t maxDelta = 0xFFFFFFFFFFFFFFFFULL - Primes[trivialDivisionNum - 1];
  std::uint64_t rnd64bit = rnd.convert_to<std::uint64_t>();
  std::uint64_t add64bit = add.convert_to<std::uint64_t>();
  std::uint64_t delta = 0;

  auto mods = std::make_unique<std::uint16_t[]>(Primes.size());

  for (int i = 1; i < trivialDivisionNum; ++i) {
    cpp_int mod = rnd % Primes[i];
    mods[i] = mod.convert_to<std::uint16_t>();
  }

  bool continueTesting = true;
  while (continueTesting) {
    continueTesting = false;
    for (int i = 1; i < trivialDivisionNum; ++i) {
      // Check if it is a prime.
      if (primeLengthInBits <= 31 && delta <= 0x7FFFFFFF) {
        std::uint64_t sq = static_cast<std::uint64_t>(Primes[i]) *
          static_cast<std::uint64_t>(Primes[i]);
        if (sq > rnd64bit + delta) {
          return {true, delta};
        }
      }
      // prime mod p == 1 implies q = (prime - 1) / 2 is divisible by p.
      if ((mods[i] + delta) % Primes[i] <= 1) {
        delta += add64bit;
        if (delta > maxDelta) {
          return {false, 0};
        }
        continueTesting = true;
        break;
      }
    }
  }

  return {true, delta};
}


//
// Created by kerail on 10.07.16.
//

#ifndef DAISU_SO3_HPP
#define DAISU_SO3_HPP

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <cmath>
#include <limits>

template<class T>
class SO3 {
 public:
  typedef Eigen::Matrix<T, 3, 1> Vector3;
  typedef Eigen::Matrix<T, 3, 3> Matrix33;
  typedef Eigen::Quaternion<T> Quaternion;

  SO3(const T &x, const T &y, const T &z) {
    m_coeffs = Eigen::Vector3d(x, y, z);
  }

  SO3(const Vector3 &v) {
    m_coeffs = v;
  }

  SO3(T angle, const Vector3 &v) {
    m_coeffs = v * angle;
  }

  SO3(const Quaternion &q_) {
    fromQuaternion(q_);
  }

  SO3(const Matrix33 &m) {
    assert(isRotationMatrix(m));
    //TODO: implement
  }

  Matrix33 getMatrix() const {
    Matrix33 m;
    const Vector3 &cfs = m_coeffs;

    T l2 = cfs.squaredNorm();
    T l = sqrt(l2);

    T sn_l, cs1_ll, cs;
    if (l == T(0)) {
      // if l is 0 sin(x)/x = 1
      sn_l = T(1);
    } else {
      sn_l = sin(l) / l;
    }

    cs = cos(l);
    static const T c_pi4096 = cos(M_PI / T(4096));
    if (cs > c_pi4096) {//fabs(l) < M_PI/T(4096)
      // when l is near nezo, we need to switch to more precise formula
      if (l2 == T(0)) {
        // when l2 is zero, we can precisely calculate limit
        cs1_ll = 1 / T(2);
      } else {
        // 1 - cos(x) = 2 * sin(x/2)^2
        T sn = sin(l / T(2));
        cs1_ll = T(2) * sn * sn / l2;
      }
    } else {
      // here l2 > 0 because abs(l) > pi/4096
      cs1_ll = (T(1) - cs) / l2;
    }

    Vector3 sn_ax = sn_l * m_coeffs;
    Vector3 cs1_l_ax = cs1_ll * m_coeffs;

    T tmp;
    tmp = cs1_l_ax.x() * m_coeffs.y();
    m.coeffRef(0, 1) = tmp - sn_ax.z();
    m.coeffRef(1, 0) = tmp + sn_ax.z();

    tmp = cs1_l_ax.x() * m_coeffs.z();
    m.coeffRef(0, 2) = tmp + sn_ax.y();
    m.coeffRef(2, 0) = tmp - sn_ax.y();

    tmp = cs1_l_ax.y() * m_coeffs.z();
    m.coeffRef(1, 2) = tmp - sn_ax.x();
    m.coeffRef(2, 1) = tmp + sn_ax.x();

    m.diagonal() = (cs1_l_ax.cwiseProduct(m_coeffs)).array() + cs;

//    Vector3 sq = cs1_l_ax.cwiseProduct(m_coeffs);
//    m.coeffRef(0, 0) = 1 + (-sq.y() - sq.z());
//    m.coeffRef(1, 1) = 1 + (-sq.x() - sq.z());
//    m.coeffRef(2, 2) = 1 + (-sq.x() - sq.y());

    // Rotation matrix checks
    assert(isRotationMatrix(m));
    return m;
  }

  Quaternion getQuaternion() const {
    const Vector3 cf2 = m_coeffs / T(2);
    T a = cf2.norm();
    if (a > T(0)) {
      T sn = sin(a) / a;
      return Quaternion(cos(a), cf2.x() * sn, cf2.y() * sn, cf2.z() * sn);
    } else {
      return Quaternion(T(1), cf2.x(), cf2.y(), cf2.z());
    }
  }

  Vector3 rotateVector(const Vector3 &v) const {
    T l2 = m_coeffs.squaredNorm();
    T l = sqrt(l2);

    T sa_l, ca, ca1_ll;
    if (l == T(0)) {
      sa_l = 1;
    } else {
      sa_l = sin(l) / l;
    }

    ca = cos(l);

    static const T c_pi4096 = cos(M_PI / T(4096));
    if (ca > c_pi4096) {//fabs(l) < M_PI/T(4096)
      // when l is near nezo, we need to switch to more precise formula
      if (l2 == T(0)) {
        // when l2 is zero, we can precisely calculate limit
        ca1_ll = 1 / T(2);
      } else {
        // 1 - cos(x) = 2 * sin(x/2)^2
        T sn = sin(l / T(2));
        ca1_ll = T(2) * sn * sn / l2;
      }
    } else {
      // here l2 > 0 because abs(l) > pi/4096
      ca1_ll = (T(1) - ca) / l2;
    }

    return v * ca + (m_coeffs * sa_l).cross(v) + m_coeffs * ((m_coeffs.dot(v)) * ca1_ll);
  }

  Vector3 coeffs() const {
    return m_coeffs;
  }

  T getAngle() const {
    return m_coeffs.norm();
  }

  Vector3 getAxis() const {
    T a = getAngle();
    return a > 0 ? (Vector3) (m_coeffs / a) : m_coeffs;
  }

  SO3<T> inverted() const {
    return SO3<T>(-m_coeffs);
  }

  SO3<T> operator *(const SO3<T> &r) {
    SO3<T> l(*this);
    return l *= r;
  }

  SO3<T> &operator *=(const SO3<T> &l) {
    fromQuaternion(getQuaternion() * l.getQuaternion());
    return *this;
  }

  SO3<T> operator ~() {
    return inverted();
  }

 private:
  Vector3 m_coeffs;

  void fromQuaternion(const Quaternion &q_) {
    const Quaternion &q = q_.w() >= T(0) ? q_ : Quaternion(-q_.coeffs());

    const Vector3 &qv = q.vec();
    T sinha = qv.norm();
    if (sinha > T(0)) {
      T angle = T(2) * atan2(sinha, q.w()); //NOTE: signed
      m_coeffs = qv * (angle / sinha);
    } else {
      // if l is too small, its norm can be equal 0 but norm_inf greater 0
      // probably w is much bigger that vec, use it as length
      m_coeffs = qv * (T(2) / q.w()); ////NOTE: signed
    }
  }
  bool isRotationMatrix(const Matrix33 &m) const {
    return ((m * m.transpose() - Matrix33::Identity()).norm() < 1e-10)
           && (fabs(fabs(m.determinant()) - 1) < 1e-10);
  }
};

typedef SO3<double> SO3d;
typedef SO3<float> SO3f;

#endif //DAISU_SO3_HPP