/* Newton nonlinear solver class header/implementation file.

   D.R. Reynolds
   Math 6321 @ SMU
   Fall 2020  */

#ifndef NEWTON_DEFINED__
#define NEWTON_DEFINED__

// Inclusions
#include <cmath>
#include <armadillo>
#include <iomanip>

// Declare abstract base classes for residual and Jacobian to
// define what the Newton solver expects from each.

//   Residual function abstract base class; derived classes
//   must at least implement the Evaluate() routine
class ResidualFunction {
 public:
  virtual int Evaluate(arma::vec& y, arma::vec& r) = 0;
};

//   Residual Jacobian function abstract base class; derived classes
//   must at least implement the Evaluate() routine
class ResidualJacobian {
 public:
  virtual int Evaluate(arma::vec& y, arma::mat& J) = 0;
};

// Newton solver class
class NewtonSolver {

 private:

  // private reusable data
  arma::vec f;      // stores nonlinear residual vector
  arma::vec s;      // stores Newton update vector
  arma::mat J;      // stores nonlinear residual Jacobian matrix

  // private pointers to problem-defining function objects
  ResidualFunction *fres;   // nonlinear residual function pointer
  ResidualJacobian *Jres;   // nonlinear residual Jacobian function pointer

  // private solver parameters
  const arma::vec *w;       // pointer to desired error weight vector

  // private statistics
  int iters;                // iteration counter (reset in each solve)
  double error_norm;        // most recent error estimate (in error-weight max norm)

 public:

  // public solver parameters
  double tol;               // desired tolerance (in error-weight max norm)
  int maxit;                // maximum desired Newton iterations
  bool show_iterates;       // flag to output iteration information

  // Constructor
  //
  // Inputs:  fres_  -- the ResidualFunction to use
  //          Jres_  -- the JacobianFunction to use
  //          tol_   -- the desired solution tolerance
  //          w_     -- the error weight vector to use
  //          maxit_ -- the maximum allowed number of iterations
  //          y      -- template solution vector (only used to clone)
  //          show_iterates_ -- enable/disable printing iterate info
  NewtonSolver(ResidualFunction& fres_, ResidualJacobian& Jres_,
               const double tol_, const arma::vec& w_, const int maxit_,
               const arma::vec& y, const bool show_iterates_) {

    // set pointers to problem-defining function objects
    fres = &fres_;
    Jres = &Jres_;

    // set error weight vector pointer, tolerance
    tol = tol_;
    w = &w_;

    // set remaining solver parameters
    show_iterates = show_iterates_;
    maxit = maxit_;

    // create reusable solver objects (clone off of y)
    f = arma::vec(y);
    s = arma::vec(y);
    J = arma::mat(y.size(), y.size());

    // initialize statistics
    iters = 0;
    error_norm = 0.0;
  }

  // Utility routine to ensure that the Newton solver object has the current
  // fres, Jres and w pointers
  void UpdatePointers(ResidualFunction& fres_,
                      ResidualJacobian& Jres_,
                      const arma::vec& w_) {
    fres = &fres_;
    Jres = &Jres_;
    w = &w_;
  };

  // Error-weight max norm utility routine for convergence tests
  //   max_i | w_i*e_i |
  // where w is the error-weight vector stored in the NewtonSolver
  // object, and e is the input vector.
  double EWTNorm(const arma::vec& e) {
    double nrm = 0.0;
    for (size_t i=0; i<e.size(); i++) {
      double we = (*w)(i) * e(i);
      nrm = std::max(nrm, std::abs(we));
    }
    return nrm;
  }

  // Newton solver routine
  //
  // Input:   y  -- the initial guess
  // Outputs: y  -- the computed solution
  //
  // The return value is one of:
  //          0 => successful solve
  //         -1 => bad function call or input
  //          1 => non-convergent iteration
  int Solve(arma::vec& y) {

    // set initial residual value
    if (fres->Evaluate(y, f) != 0) {
      std::cerr << "NewtonSolver::Solve error: residual function failure\n";
      return -1;
    }

    // perform iterations
    for (iters=1; iters<=maxit; iters++) {

      // evaluate Jacobian
      if (Jres->Evaluate(y, J) != 0) {
        std::cerr << "NewtonSolver::Solve error: Jacobian function failure\n";
        return -1;
      }

      // compute Newton update, norm
      if (arma::solve(s, J, f) == false) {
        std::cerr << "NewtonSolver::Solve error: linear solver failure\n";
        return -1;
      }
      error_norm = EWTNorm(s);

      // perform update
      y -= s;

      // update residual
      if (fres->Evaluate(y, f) != 0) {
        std::cerr << "NewtonSolver::Solve error: residual function failure\n";
        return -1;
      }

      // output convergence information
      if (show_iterates)
        printf("   iter %3i, ||s*w||_inf = %7.2e, ||f(x)*w||_inf = %7.2e\n",
               iters, error_norm, EWTNorm(f));

      // check for convergence, return if successful
      if (error_norm < tol)  return 0;

    }

    // if we've made it here, Newton did not converge, output warning and return
    std::cerr << "\nNewtonSolver::Solve WARNING: nonconvergence after " << maxit
              << " iterations (||s|| = " << error_norm << ")\n";
    return 1;
  }

  // Parameter update & statistics accessor routines
  void ResetIters() { iters = 0; };
  const int GetIters() { return iters; };
  const double GetErrorNorm() { return error_norm; };

};

#endif


#include <iostream>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <manifold/SO3.h>
#include <manifold/S.h>

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

  uint32_t N = 10;
  uint32_t K = 2;

  double theta = 0.*M_PI/180.;
  Eigen::Matrix3d Rmu_;
  Rmu_ << 1, 0, 0,
         0, cos(theta), sin(theta),
         0, -sin(theta), cos(theta);
  SO3d Rmu(Rmu_);
  SO3d R = Rmu;
  double tau_R = 10.;

  std::vector<S3d> mus;
  mus.push_back(S3d(Eigen::Vector3d(0.,cos(theta),-sin(theta))));
  mus.push_back(S3d(Eigen::Vector3d(0.,sin(theta),cos(theta))));
  std::vector<double> taus;
  taus.push_back(10.);
  taus.push_back(10.);

  theta = 15.*M_PI/180.;
  std::vector<S3d> ns;
  std::vector<uint32_t> zs;
  for (uint32_t i=0; i<N/2; ++i) {
    ns.push_back(S3d(Eigen::Vector3d(0.,cos(theta),-sin(theta))));
    ns.push_back(S3d(Eigen::Vector3d(0.,sin(theta),cos(theta))));
    zs.push_back(0);
    zs.push_back(1);
  }

  std::cout << "Using SO(3) formulation derived from the Stiefel manifold formulation by Absil" << std::endl;

  double delta = 0.01;
  double f_prev = 1e99;
  double f = -tau_R*(Rmu.Inverse() + R).matrix().trace();
  for (uint32_t i=0; i<N; ++i)
    f -= taus[zs[i]]*mus[zs[i]].vector().transpose()*R.matrix()*ns[i].vector();
  std::cout << "f=" << f << std::endl;
  for (uint32_t it=0; it<100; ++it) {
    Eigen::Matrix3d J = -0.5*tau_R*(Rmu.matrix() - (R+Rmu.Inverse()+R).matrix());
    for (uint32_t i=0; i<N; ++i) {
      J -= 0.5*(taus[zs[i]]*(mus[zs[i]].vector()*ns[i].vector().transpose()
         - R.matrix()*(ns[i].vector()*mus[zs[i]].vector().transpose()*R.matrix())));
    }
    Eigen::Vector3d Jw = SO3d::vee(R.Inverse().matrix()*J);
    R += -delta*Jw;
    f_prev = f;
    f = -tau_R*(Rmu.Inverse() + R).matrix().trace();
    for (uint32_t i=0; i<N; ++i)
      f -= taus[zs[i]]*mus[zs[i]].vector().transpose()*R.matrix()*ns[i].vector();
    std::cout << "@" << it << ": f=" << f << " df/f=" << (f_prev - f)/fabs(f)
      << std::endl;
    if ((f_prev - f)/fabs(f) < 1e-9)
      break;
  }
//  std::cout << std::endl << Rmu << std::endl;
  std::cout << std::endl << R << std::endl;
  std::cout << acos(R.matrix()(1,1))*180/M_PI << std::endl;

  std::cout << "Using SO(3) formulation first order" << std::endl;

  R = Rmu;
  delta = 0.01;
  f_prev = 1e99;
  f = -tau_R*(Rmu.Inverse() + R).matrix().trace();
  for (uint32_t i=0; i<N; ++i)
    f -= taus[zs[i]]*mus[zs[i]].vector().transpose()*R.matrix()*ns[i].vector();
  std::cout << "f=" << f << std::endl;
  for (uint32_t it=0; it<200; ++it) {
    Eigen::Vector3d J;
    for (uint32_t l=0; l<3; ++l) {
      J(l) = -tau_R*(Rmu.Inverse().matrix()*SO3d::G(l)*R.matrix()).trace();
//      std::cout << -tau_R*(Rmu.Inverse().matrix()*R.matrix()*SO3d::G(l)).trace()
//        << " " << -tau_R*(Rmu.Inverse().matrix()*SO3d::G(l)*R.matrix()).trace()
//        << std::endl;
//      J(l) = -tau_R*(Rmu.Inverse().matrix()*R.matrix()*SO3d::G(l)).trace();
    }
    for (uint32_t i=0; i<N; ++i) {
      J -= -taus[zs[i]]*mus[zs[i]].vector().transpose()*SO3d::invVee(R.matrix()*ns[i].vector());
    }
    R += -delta*J;
    f_prev = f;
    f = -tau_R*(Rmu.Inverse() + R).matrix().trace();
    for (uint32_t i=0; i<N; ++i)
      f -= taus[zs[i]]*mus[zs[i]].vector().transpose()*R.matrix()*ns[i].vector();
    std::cout << "@" << it << ": f=" << f << " df/f=" << (f_prev - f)/fabs(f) << std::endl;
    if ((f_prev - f)/fabs(f) < 1e-9)
      break;
  }
//  std::cout << std::endl << Rmu << std::endl;
  std::cout << std::endl << R << std::endl;
  std::cout << acos(R.matrix()(1,1))*180/M_PI << std::endl;

  std::cout << "Using SO(3) formulation second order" << std::endl;

  R = Rmu;
  delta = 0.9;
  f_prev = 1e99;
  f = -tau_R*(Rmu.Inverse() + R).matrix().trace();
  for (uint32_t i=0; i<N; ++i)
    f -= taus[zs[i]]*mus[zs[i]].vector().transpose()*R.matrix()*ns[i].vector();
  std::cout << "f=" << f << std::endl;
  for (uint32_t it=0; it<100; ++it) {
    Eigen::Vector3d J;
    for (uint32_t l=0; l<3; ++l)
      J(l) = -tau_R*(Rmu.Inverse().matrix()*SO3d::G(l)*R.matrix()).trace();
    for (uint32_t i=0; i<N; ++i) {
      J -= -taus[zs[i]]*mus[zs[i]].vector().transpose()*SO3d::invVee(R.matrix()*ns[i].vector());
    }
    Eigen::Matrix3d H;
    for (uint32_t l=0; l<3; ++l)
      for (uint32_t m=0; m<3; ++m) {
        Eigen::Matrix3d Glmml = 0.5*(SO3d::G(l)*SO3d::G(m)+SO3d::G(m)*SO3d::G(l));
        H(l,m) = -tau_R*(Rmu.Inverse().matrix()*R.matrix()*Glmml).trace();
      }
    for (uint32_t i=0; i<N; ++i) {
      for (uint32_t l=0; l<3; ++l)
        for (uint32_t m=0; m<3; ++m) {
          Eigen::Matrix3d Glmml = 0.5*(SO3d::G(l)*SO3d::G(m)+SO3d::G(m)*SO3d::G(l));
          H(l,m) += -taus[zs[i]]*mus[zs[i]].vector().transpose()*R.matrix()*Glmml*ns[i].vector();
        }
    }
    Eigen::Vector3d xi = - H.lu().solve(J);
    R += delta*xi;
    f_prev = f;
    f = -tau_R*(Rmu.Inverse() + R).matrix().trace();
    for (uint32_t i=0; i<N; ++i)
      f -= taus[zs[i]]*mus[zs[i]].vector().transpose()*R.matrix()*ns[i].vector();
    std::cout << "@" << it << ": f=" << f << " df/f=" << (f_prev - f)/fabs(f) << std::endl;
    if ((f_prev - f)/fabs(f) < 1e-9)
      break;
  }
//  std::cout << std::endl << Rmu << std::endl;
  std::cout << std::endl << R << std::endl;
  std::cout << acos(R.matrix()(1,1))*180/M_PI << std::endl;

}


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

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

#include "mgc.h"
#include "../structures/molecule.h"

CHARGEFW2_METHOD(MGC)

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

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

    Eigen::MatrixXd S = Eigen::MatrixXd::Zero(n, n);
    Eigen::VectorXd X0 = Eigen::VectorXd::Zero(n);

    double log_sum = 0;

    for (const auto &atom: molecule.atoms()) {
        auto i = atom.index();
        S(i, i) = 1;
        X0(i) = atom.element().electronegativity();
        log_sum += log(X0(i));
    }

    for (const auto &bond: molecule.bonds()) {
        auto i1 = bond.first().index();
        auto i2 = bond.second().index();
        auto order = bond.order();
        S(i1, i1) += order;
        S(i2, i2) += order;
        S(i1, i2) -= order;
        S(i2, i1) -= order;
    }

    Eigen::VectorXd chi = S.partialPivLu().solve(X0);
    for (size_t i = 0; i < n; i++) {
        chi(i) -= molecule.atoms()[i].element().electronegativity();
    }
    chi /= exp(log_sum / n);

    return std::vector<double>(chi.data(), chi.data() + chi.size());
}


/*
 Copyright 2010-2012 Karsten Ahnert
 Copyright 2011-2013 Mario Mulansky
 Copyright 2013 Pascal Germroth

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

#include <iostream>
#include <vector>

#include <boost/numeric/odeint.hpp>

//[ rhs_function
/* The type of container used to hold the state vector */
typedef std::vector< double > state_type;

const double gam = 0.15;

/* The rhs of x' = f(x) */
void harmonic_oscillator( const state_type &x , state_type &dxdt , const double /* t */ )
{
    dxdt[0] = x[1];
    dxdt[1] = -x[0] - gam*x[1];
}
//]

//[ rhs_class
/* The rhs of x' = f(x) defined as a class */
class harm_osc {

    double m_gam;

public:
    harm_osc( double gam ) : m_gam(gam) { }

    void operator() ( const state_type &x , state_type &dxdt , const double /* t */ )
    {
        dxdt[0] = x[1];
        dxdt[1] = -x[0] - m_gam*x[1];
    }
};
//]

//[ integrate_observer
struct push_back_state_and_time
{
    std::vector< state_type >& m_states;
    std::vector< double >& m_times;

    push_back_state_and_time( std::vector< state_type > &states , std::vector< double > &times )
    : m_states( states ) , m_times( times ) { }

    void operator()( const state_type &x , double t )
    {
        m_states.push_back( x );
        m_times.push_back( t );
    }
};
//]

struct write_state
{
    void operator()( const state_type &x ) const
    {
        std::cout << x[0] << "\t" << x[1] << "\n";
    }
};

int main(int /* argc */ , char** /* argv */ )
{
    using namespace std;
    using namespace boost::numeric::odeint;

    //[ state_initialization
    state_type x(2);
    x[0] = 1.0; // start at x=1.0, p=0.0
    x[1] = 0.0;
    //]

    //[ integration
    size_t steps = integrate( harmonic_oscillator ,
            x , 0.0 , 10.0 , 0.1 );
    //]

    //[ integration_class
    harm_osc ho(0.15);
    steps = integrate( ho ,
            x , 0.0 , 10.0 , 0.1 );
    //]

    //[ integrate_observ
    vector<state_type> x_vec;
    vector<double> times;

    steps = integrate( harmonic_oscillator ,
            x , 0.0 , 10.0 , 0.1 ,
            push_back_state_and_time( x_vec , times ) );

    /* output */
    for( size_t i=0; i<=steps; i++ )
    {
        cout << times[i] << '\t' << x_vec[i][0] << '\t' << x_vec[i][1] << '\n';
    }
    //]

    //[ define_const_stepper
    runge_kutta4< state_type > stepper;
    integrate_const( stepper , harmonic_oscillator , x , 0.0 , 10.0 , 0.01 );
    //]

    //[ integrate_const_loop
    const double dt = 0.01;
    for( double t=0.0 ; t<10.0 ; t+= dt )
        stepper.do_step( harmonic_oscillator , x , t , dt );
    //]

    //[ define_adapt_stepper
    typedef runge_kutta_cash_karp54< state_type > error_stepper_type;
    //]

    //[ integrate_adapt
    typedef controlled_runge_kutta< error_stepper_type > controlled_stepper_type;
    controlled_stepper_type controlled_stepper;
    integrate_adaptive( controlled_stepper , harmonic_oscillator , x , 0.0 , 10.0 , 0.01 );
    //]

    {
    //[integrate_adapt_full
    double abs_err = 1.0e-10 , rel_err = 1.0e-6 , a_x = 1.0 , a_dxdt = 1.0;
    controlled_stepper_type controlled_stepper(
        default_error_checker< double , range_algebra , default_operations >( abs_err , rel_err , a_x , a_dxdt ) );
    integrate_adaptive( controlled_stepper , harmonic_oscillator , x , 0.0 , 10.0 , 0.01 );
    //]
    }

    //[integrate_adapt_make_controlled
    integrate_adaptive( make_controlled< error_stepper_type >( 1.0e-10 , 1.0e-6 ) ,
                        harmonic_oscillator , x , 0.0 , 10.0 , 0.01 );
    //]

    //[integrate_adapt_make_controlled_alternative
    integrate_adaptive( make_controlled( 1.0e-10 , 1.0e-6 , error_stepper_type() ) ,
                        harmonic_oscillator , x , 0.0 , 10.0 , 0.01 );
    //]

    #ifdef BOOST_NUMERIC_ODEINT_CXX11
    //[ define_const_stepper_cpp11
    {
    runge_kutta4< state_type > stepper;
    integrate_const( stepper , []( const state_type &x , state_type &dxdt , double t ) {
            dxdt[0] = x[1]; dxdt[1] = -x[0] - gam*x[1]; }
        , x , 0.0 , 10.0 , 0.01 );
    }
    //]

    //[ harm_iterator_const_step]
    std::for_each( make_const_step_time_iterator_begin( stepper , harmonic_oscillator, x , 0.0 , 0.1 , 10.0 ) ,
                   make_const_step_time_iterator_end( stepper , harmonic_oscillator, x ) ,
                   []( std::pair< const state_type & , const double & > x ) {
                       cout << x.second << " " << x.first[0] << " " << x.first[1] << "\n"; } );
    //]
    #endif

}


#ifndef HOPS_EXPECTEDSQUAREDJUMPDISTANCE_HPP
#define HOPS_EXPECTEDSQUAREDJUMPDISTANCE_HPP

#include <hops/Statistics/Covariance.hpp>

#include <Eigen/Core>
#include <Eigen/Cholesky>

#include <string>
#include <stdexcept>
#include <vector>
#include <cmath>
#include <cassert>
#include <memory>

namespace hops {
    /*
     * Compute Expected Squared Jump Distance incrementally on a single vector of draws.
     * The Expected Squared Jump Distance is defined as
     * \[ ESJD = \frac{1}{N-1} \sum_{n=1}^(N-1) \| \theta_{n+1} - \theta_n \|^2_{\Sigma} \]
     */
    template<typename StateType, typename MatrixType>
    double computeExpectedSquaredJumpDistance(const std::vector<StateType>& draws,
                                              unsigned long numUnseen,
                                              double esjdSeen,
                                              unsigned long numSeen,
                                              const MatrixType& sqrtCovariance) {
        size_t numDraws = draws.size(),
               correction = 0;
        // account for missing jump between two batches of samples
        if (numSeen > 0 && draws.size() > numUnseen) {
            correction = 1;
        }

        // in order to guarantee eta to be 1, we have to set it to 1.
        if (numSeen == 0) {
            ++numSeen;
        }

        double esjd = 0,
               eta = 1.0 * (numSeen - 1) / (numSeen + numUnseen - 1),
               squaredDistance;
        for (unsigned long i = numDraws - numUnseen - correction; i < numDraws - 1; ++i) {
            StateType distance = sqrtCovariance.template triangularView<Eigen::Lower>().solve(draws[i] - draws[i+1]);
            distance = sqrtCovariance.template triangularView<Eigen::Lower>().transpose().solve(distance);
            squaredDistance = (draws[i] - draws[i+1]).transpose() * distance;
            esjd += squaredDistance;
        }
        esjd /=  numUnseen - 1 + correction;
        return eta * esjdSeen + (1 - eta) * esjd;
    }

    /*
     * Compute Expected Squared Jump Distance non-incrementally on all draws passed.
     */
    template<typename StateType, typename MatrixType>
    double computeExpectedSquaredJumpDistance(const std::vector<StateType>& draws, const MatrixType& sqrtCovariance) {
        return computeExpectedSquaredJumpDistance<StateType, MatrixType>(draws, draws.size(), 0, 0, sqrtCovariance);
    }

    /*
     * Compute Expected Squared Jump Distance non-incrementally on all draws passed.
     */
    template<typename StateType, typename MatrixType>
    double computeExpectedSquaredJumpDistance(const std::vector<StateType>& draws) {
        MatrixType covariance = computeCovariance<StateType, MatrixType>(draws);
        MatrixType sqrtCovariance = covariance.llt().matrixL();
        return computeExpectedSquaredJumpDistance<StateType, MatrixType>(draws, sqrtCovariance);
    }

    /*
     * Compute Expected Squared Jump Distance for every chain in \c chains incrementally.
     */
    template<typename StateType, typename MatrixType>
    std::vector<double> computeExpectedSquaredJumpDistance(const std::vector<std::vector<StateType>>& chains,
                                                           unsigned long numUnseen,
                                                           std::vector<double> esjdSeen,
                                                           unsigned long numSeen,
                                                           const MatrixType& sqrtCovariance) {
        std::vector<double> esjds(chains.size());
        for (size_t i = 0; i < chains.size(); ++i) {
            esjds[i] = computeExpectedSquaredJumpDistance<StateType, MatrixType>(chains[i], numUnseen, esjdSeen[i], numSeen, sqrtCovariance);
        }
        return esjds;
    }

    /*
     * Compute Expected Squared Jump Distance non-incrementally for every chain in \c chains.
     */
    template<typename StateType, typename MatrixType>
    std::vector<double> computeExpectedSquaredJumpDistance(const std::vector<std::vector<StateType>>& chains, const MatrixType& sqrtCovariance) {
        return computeExpectedSquaredJumpDistance<StateType, MatrixType>(chains, chains[0].size(), std::vector<double>(chains.size()), 0, sqrtCovariance);
    }

    /*
     * Compute Expected Squared Jump Distance non-incrementally for every chain in \c chains.
     */
    template<typename StateType, typename MatrixType>
    std::vector<double> computeExpectedSquaredJumpDistance(const std::vector<std::vector<StateType>>& chains) {
        MatrixType covariance = computeCovariance<StateType, MatrixType>(chains);
        MatrixType sqrtCovariance = covariance.llt().matrixL();
        return computeExpectedSquaredJumpDistance<StateType, MatrixType>(chains, sqrtCovariance);
    }

    /*
     * Compute Expected Squared Jump Distance for every chain in \c chains incrementally.
     * \c chains is supposed to be a vector of pointers to the actual chains.
     */
    template<typename StateType, typename MatrixType>
    std::vector<double> computeExpectedSquaredJumpDistance(const std::vector<const std::vector<StateType>*>& chains,
                                                           unsigned long numUnseen,
                                                           std::vector<double> esjdSeen,
                                                           unsigned long numSeen,
                                                           const MatrixType& sqrtCovariance) {
        std::vector<double> esjds(chains.size());
        for (size_t i = 0; i < chains.size(); ++i) {
            esjds[i] = computeExpectedSquaredJumpDistance<StateType, MatrixType>(*chains[i], numUnseen, esjdSeen[i], numSeen, sqrtCovariance);
        }
        return esjds;
    }

    /*
     * Compute Expected Squared Jump Distance non-incrementally for every chain in \c chains.
     * \c chains is supposed to be a vector of pointers to the actual chains.
     */
    template<typename StateType, typename MatrixType>
    std::vector<double> computeExpectedSquaredJumpDistance(const std::vector<const std::vector<StateType>*>& chains,
                                                           const MatrixType& sqrtCovariance) {
        return computeExpectedSquaredJumpDistance<StateType, MatrixType>(chains, chains[0]->size(), std::vector<double>(chains.size()), 0, sqrtCovariance);
    }

    /*
     * Compute Expected Squared Jump Distance non-incrementally for every chain in \c chains.
     * \c chains is supposed to be a vector of pointers to the actual chains.
     */
    template<typename StateType, typename MatrixType>
    std::vector<double> computeExpectedSquaredJumpDistance(const std::vector<const std::vector<StateType>*>& chains) {
        MatrixType covariance = computeCovariance<StateType, MatrixType>(chains);
        MatrixType sqrtCovariance = covariance.llt().matrixL();
        return computeExpectedSquaredJumpDistance<StateType, MatrixType>(chains, sqrtCovariance);
    }
}

#endif //HOPS_EXPECTEDSQUAREDJUMPDISTANCE_HPP


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

#include <boost/math/quadrature/gauss_kronrod.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>

template <class T>
void print_gauss_constants(const char* suffix, int prec, int tag)
{
   auto ab = T::abscissa();
   auto w = T::weights();
   std::cout << std::setprecision(prec) << std::scientific;
   std::size_t order = (ab[0] == 0) ? (ab.size() * 2) - 1 : ab.size() * 2;
   std::cout <<
      "template <class T>\n"
      "class gauss_detail<T, " << order << ", " << tag << ">\n"
      "   {\n"
      "   public:\n"
      "      static " << (prec > 40 ? " " : "constexpr ") << "std::array<T, " << ab.size() << "> const & abscissa()\n"
      "      {\n"
      "         static " << (prec > 40 ? " " : "constexpr ") << "std::array<T, " << ab.size() << "> data = {\n";
   for (unsigned i = 0; i < ab.size(); ++i)
      std::cout << "            " << (prec > 40 ? "BOOST_MATH_HUGE_CONSTANT(T, 0, " : "") << ab[i] << (prec > 40 ? ")" : suffix) << ",\n";
   std::cout <<
      "};\n"
      "         return data;\n"
      "      }\n"
      "      static " << (prec > 40 ? " " : "constexpr ") << "std::array<T, " << w.size() << "> const & weights()\n"
      "      {\n"
      "         static " << (prec > 40 ? " " : "constexpr ") << "std::array<T, " << w.size() << "> data = {\n";
   for (unsigned i = 0; i < w.size(); ++i)
      std::cout << "            " << (prec > 40 ? "BOOST_MATH_HUGE_CONSTANT(T, 0, " : "") << w[i] << (prec > 40 ? ")" : suffix) << ",\n";

   std::cout << "         };\n"
      "         return data;\n"
      "      }\n"
      "   };\n\n";
}

template <class T>
void print_gauss_kronrod_constants(const char* suffix, int prec, int tag)
{
   auto ab = T::abscissa();
   auto w = T::weights();
   std::cout << std::setprecision(prec) << std::scientific;
   std::size_t order = (ab.size() * 2) - 1;
   std::cout <<
      "   template <class T>\n"
      "   class gauss_kronrod_detail<T, " << order << ", " << tag << ">\n"
      "   {\n"
      "   public:\n"
      "      static " << (prec > 40 ? " " : "constexpr ") << "std::array<T, " << ab.size() << "> const & abscissa()\n"
      "      {\n"
      "         static " << (prec > 40 ? " " : "constexpr ") << "std::array<T, " << ab.size() << "> data = {\n";

   for (unsigned i = 0; i < ab.size(); ++i)
      std::cout << "            " << (prec > 40 ? "BOOST_MATH_HUGE_CONSTANT(T, 0, " : "") << ab[i] << (prec > 40 ? ")" : suffix) << ",\n";

   std::cout << "         };\n"
      "         return data;\n"
      "      }\n"
      "      static " << (prec > 40 ? " " : "constexpr ") << "std::array<T, " << w.size() << "> const & weights()\n"
      "      {\n"
      "         static " << (prec > 40 ? " " : "constexpr ") << "std::array<T, " << w.size() << "> data = {\n";

   for (unsigned i = 0; i < w.size(); ++i)
      std::cout << "            " << (prec > 40 ? "BOOST_MATH_HUGE_CONSTANT(T, 0, " : "") << w[i] << (prec > 40 ? ")" : suffix) << ",\n";

   std::cout << "         };\n"
      "         return data;\n"
      "      }\n"
      "   };\n\n";
}

int main()
{
   typedef boost::multiprecision::number<boost::multiprecision::cpp_bin_float<250> > mp_type;

   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 7> >("f", 9, 0);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 7> >("", 17, 1);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 7> >("L", 35, 2);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 7> >("Q", 35, 3);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 7> >("", 115, 4);

   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 10> >("f", 9, 0);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 10> >("", 17, 1);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 10> >("L", 35, 2);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 10> >("Q", 35, 3);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 10> >("", 115, 4);

   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 15> >("f", 9, 0);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 15> >("", 17, 1);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 15> >("L", 35, 2);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 15> >("Q", 35, 3);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 15> >("", 115, 4);

   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 20> >("f", 9, 0);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 20> >("", 17, 1);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 20> >("L", 35, 2);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 20> >("Q", 35, 3);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 20> >("", 115, 4);

   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 25> >("f", 9, 0);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 25> >("", 17, 1);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 25> >("L", 35, 2);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 25> >("Q", 35, 3);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 25> >("", 115, 4);

   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 30> >("f", 9, 0);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 30> >("", 17, 1);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 30> >("L", 35, 2);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 30> >("Q", 35, 3);
   print_gauss_constants<boost::math::quadrature::gauss<mp_type, 30> >("", 115, 4);

   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 15> >("f", 9, 0);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 15> >("", 17, 1);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 15> >("L", 35, 2);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 15> >("Q", 35, 3);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 15> >("", 115, 4);

   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 21> >("f", 9, 0);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 21> >("", 17, 1);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 21> >("L", 35, 2);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 21> >("Q", 35, 3);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 21> >("", 115, 4);

   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 31> >("f", 9, 0);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 31> >("", 17, 1);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 31> >("L", 35, 2);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 31> >("Q", 35, 3);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 31> >("", 115, 4);

   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 41> >("f", 9, 0);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 41> >("", 17, 1);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 41> >("L", 35, 2);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 41> >("Q", 35, 3);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 41> >("", 115, 4);

   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 51> >("f", 9, 0);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 51> >("", 17, 1);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 51> >("L", 35, 2);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 51> >("Q", 35, 3);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 51> >("", 115, 4);

   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 61> >("f", 9, 0);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 61> >("", 17, 1);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 61> >("L", 35, 2);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 61> >("Q", 35, 3);
   print_gauss_kronrod_constants<boost::math::quadrature::gauss_kronrod<mp_type, 61> >("", 115, 4);

   return 0;
}


/*
Utilize the result of eye-in-hand calibration to transform (picking) point
coordinates from the camera frame to the robot base frame.
*/

#include <Eigen/Core>
#include <opencv2/core/core.hpp>

#include <cmath>
#include <iostream>

Eigen::MatrixXd cvToEigen(const cv::Mat &);
cv::Mat readTransform(const std::string &);

int main()
{
    try
    {
        // define (picking) point in camera frame
        const Eigen::Vector4d pointInCameraFrame(81.2, 18.0, 594.6, 1);
        std::cout << "Point coordinates in camera frame: " << pointInCameraFrame.segment(0, 3).transpose() << std::endl;

        // Read camera pose in end-effector frame (result of eye-in-hand calibration)
        const auto eyeInHandTransformation = readTransform("handEyeTransform.yaml");

        // Read end-effector pose in robot base frame
        const auto endEffectorPose = readTransform("robotTransform.yaml");

        // convert to Eigen matrices for easier computation
        const auto transformEndEffectorToCamera = cvToEigen(eyeInHandTransformation);
        const auto transformBaseToEndEffector = cvToEigen(endEffectorPose);

        // Compute camera pose in robot base frame
        const auto transform_base_to_camera = transformBaseToEndEffector * transformEndEffectorToCamera;

        // compute (picking) point in robot base frame
        const auto pointInBaseFrame = transform_base_to_camera * pointInCameraFrame;
        std::cout << "Point coordinates in robot base frame: " << pointInBaseFrame.segment(0, 3).transpose()
                  << std::endl;
    }

    catch(const std::exception &e)
    {
        std::cerr << "Error: " << e.what() << std::endl;
        return EXIT_FAILURE;
    }
}

Eigen::MatrixXd cvToEigen(const cv::Mat &cvMat)
{
    if(cvMat.dims > 2)
    {
        throw std::invalid_argument("Invalid matrix dimensions. Expected 2D.");
    }

    Eigen::MatrixXd eigenMat(cvMat.rows, cvMat.cols);

    for(int i = 0; i < cvMat.rows; i++)
    {
        for(int j = 0; j < cvMat.cols; j++)
        {
            eigenMat(i, j) = cvMat.at<double>(i, j);
        }
    }

    return eigenMat;
}

cv::Mat readTransform(const std::string &file_name)
{
    auto fileStorage = cv::FileStorage();

    if(!fileStorage.open(file_name, cv::FileStorage::Mode::READ))
    {
        throw std::invalid_argument("Could not open " + file_name);
    }
    try
    {
        const auto poseStateNode = fileStorage["PoseState"];

        if(poseStateNode.empty())
        {
            throw std::invalid_argument("PoseState not found in file " + file_name);
        }

        const auto rows = poseStateNode.mat().rows;
        const auto cols = poseStateNode.mat().cols;
        if(rows != 4 || cols != 4)
        {
            throw std::invalid_argument("Expected 4x4 matrix in " + file_name + ", but got " + std::to_string(cols)
                                        + "x" + std::to_string(rows));
        }

        const auto poseState = poseStateNode.mat();
        fileStorage.release();
        return poseState;
    }
    catch(...)
    {
        fileStorage.release();
        throw;
    }
}


#include "aux/eigen2hdf.hpp"
#include "aux/message.hpp"
#include "aux/timer.hpp"
#include "post_processing/mass.hpp"
#include "post_processing/momentum.hpp"
#include "quadrature/qhermite.hpp"
#include "spectral/basis/spectral_basis.hpp"
#include "spectral/basis/spectral_basis_factory_hermite.hpp"
#include "spectral/basis/spectral_basis_factory_ks.hpp"
#include "spectral/basis/spectral_elem.hpp"
#include "spectral/basis/spectral_elem_accessor.hpp"
#include "spectral/basis/spectral_function/hermite_polynomial.hpp"
#include "spectral/basis/toolbox/spectral_basis.hpp"

#include "spectral/polar_to_hermite.hpp"
#include "spectral/shift_hermite_2d.hpp"

#include <Eigen/Sparse>
#include <boost/program_options.hpp>
#include <fstream>
#include <iomanip>
#include <iostream>

#define PI 3.141592653589793238462643383279502884197

using namespace std;
using namespace boltzmann;

namespace po = boost::program_options;

#ifdef EXTENDED_PRECISION
typedef long double numeric_t;
#else
typedef double numeric_t;
#endif
const int nrep = 1000;

int main(int argc, char *argv[])
{
  Timer<> timer;
  po::options_description options("options");
  options.add_options()
      ("help", "show help message");
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, options), vm);
  po::notify(vm);

  if (vm.count("help")) {
    cout << options << "\n";
    return 0;
  }

  // read polar basis from file
  typedef typename SpectralBasisFactoryKS::basis_type polar_basis_t;
  polar_basis_t polar_basis;
  SpectralBasisFactoryKS::create(polar_basis, "spectral_basis.desc");
  //  SpectralBasisFactoryKS::write_basis_descriptor(polar_basis, "spectral_basis.desc");

  int max_deg = spectral::get_max_k(polar_basis);
  const unsigned int K = max_deg + 1;
  // create corresponding Hermite basis
  typedef typename SpectralBasisFactoryHN::basis_type hermite_basis_t;
  hermite_basis_t hermite_basis;
  SpectralBasisFactoryHN::create(hermite_basis, max_deg + 1, 2);
  SpectralBasisFactoryHN::write_basis_descriptor(hermite_basis, "hermite_basis.desc");

  if (hermite_basis.n_dofs() != polar_basis.n_dofs()) {
    throw runtime_error("Hermite basis does not match!");
    return 1;
  }

  cout << "size(polar basis) = " << polar_basis.n_dofs() << endl
       << "size(hermite basis) = " << hermite_basis.n_dofs();

  cout << "\n--------------------\n";
  cout << "Test 2: (P->H) -> (H->P) show coefficients\n";
  timer.start();
  Polar2Hermite<polar_basis_t, hermite_basis_t> P2H(polar_basis, hermite_basis);
  print_timer(timer.stop(), "init P2H");

  /*
   * load coefficients (polar basis) from HDF5
   */
  const unsigned int N = polar_basis.n_dofs();
  Eigen::VectorXd coeffs(N);
  hid_t h5_init = H5Fopen("init.h5", H5F_ACC_RDONLY, H5P_DEFAULT);
  eigen2hdf::load(h5_init, "coeffs", coeffs);
  H5Fclose(h5_init);

  // compute bulk velocity
  Mass mass;
  mass.init(polar_basis);
  Momentum momentum;
  momentum.init(polar_basis);

  const double m = mass.compute(coeffs.data());
  auto u = momentum.compute(coeffs.data()) / m;
  cout << scientific << setprecision(8) << "mass: " << m << endl
       << "momentum: " << u(0) << ", " << u(1) << endl;

  // compute hermite coefficients
  Eigen::VectorXd buf(N);

  // --------------------------------------------------
  // Transform to Hermite basis
  // --------------------------------------------------
  {
    timer.start();
    int nrep = 100000;
    for (int i = 0; i < nrep; ++i) {
      P2H.to_hermite(buf, coeffs);
    }
    double t = timer.stop();

    print_timer(t / nrep, "P2H.to_hermite");
  }

  if (sizeof(numeric_t) == 16) {
    cout << "Using *extended precision*  in ShiftHermite\n";
  } else if (sizeof(numeric_t) == 8) {
    cout << "Using double precision in ShiftHermite\n";
  }

  // use (extended/double) precision for shifting ...
  std::vector<numeric_t> cH(buf.data(), buf.data() + N);
  ShiftHermite2D<hermite_basis_t, numeric_t> shift_hermite(hermite_basis);
  shift_hermite.init();

  // --------------------------------------------------
  // Shift hermite coefficients
  // --------------------------------------------------
  timer.start();
  for (int i = 0; i < nrep; ++i) {
    shift_hermite.shift(cH.data(), u(0), u(1));
  }
  double t_shift_hermite = timer.stop();
  print_timer(t_shift_hermite / nrep, "shift Hermite coefficients");
  cout << "t_shift_hermite: " << scientific << setprecision(10) << t_shift_hermite << endl;

  // // transform coefficients back to double
  // std::transform(cH.begin(), cH.end(), buf.begin(), [](numeric_t x) { return double(x); });

  // // -> Polar coordinates
  // std::vector<double> Cc(N, 0.0);
  // P2H.to_polar(Cc, buf);

  // const double mc = mass.compute(Cc.data());
  // auto uc = momentum.compute(Cc.data())/mc;
  // cout << "centered:\n";
  // cout << "mass: " << scientific  << setprecision(8) << mc << "\t(diff = " << std::abs(m-mc) <<
  // ")"
  //      << endl
  //      << "momentum: " << uc(0) << ", " << uc(1) << endl;

  // // write new coefficients to disk
  // hid_t h5_shifted = H5Fcreate("shifted.h5", H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
  // Eigen::Map< Eigen::VectorXd> Cc_eigen(Cc.data(), Cc.size());
  // eigen2hdf::save(h5_shifted, "coeffs", Cc_eigen);
  // // export hermite coefficients
  // Eigen::Map< Eigen::VectorXd> cH_eigen(buf.data(), buf.size());
  // eigen2hdf::save(h5_shifted, "coeffs_hermite", cH_eigen);
  // H5Fclose(h5_shifted);

  // // do some cheap scattering
  // // ...

  // // Move back to original position
  // timer.start();
  // shift_hermite.shift(cH.data(), -u(0), -u(1));
  // print_timer(timer.stop(), "shift Hermite coefficients (back)");

  // // go back to polar coordinates
  // std::transform(cH.begin(), cH.end(), buf.begin(), [](numeric_t x) { return double(x); });
  // std::vector<double> Cc2(N, 0.0);
  // P2H.to_polar(Cc2, buf);

  // const double m1 = mass.compute(Cc2.data());
  // auto u1 = momentum.compute(Cc2.data())/m1;

  // cout << "move back:\n";
  // cout << "mass: " << scientific  << setprecision(8) << m1 << "\t(diff = " << std::abs(m-m1) <<
  // ")"
  //      << endl
  //      << "momentum: " << scientific  << setprecision(8) << u1(0) << ", " << u1(1) << "\t(diff =
  //      " << (u-u1).squaredNorm() << ")" << endl;
  return 0;}


#include <Eigen/Dense>

using namespace Eigen;

extern "C" {
  void AmulB(double*, double*, double*, long, long, long);
  void AmulBt(double*, double*, double*, long, long, long);
  void AtmulB(double*, double*, double*, long, long, long);
  void AtmulBt(double*, double*, double*, long, long, long);
  void Amulvb(double*, double*, double*, long, long);
  void Atmulvb(double*, double*, double*, long, long);
  double dot(double*, double*, long);
  double selfdot(double*, long);
  double dot3(double*, double*, double*, long, long);
  void aplusBc(double*, double*, double*, double*, long, long);
  double OLSlp(double*, double*, double*, long, long);
  void AplusAt(double*, double*, long);
  double logdettriangle(double*, long);
}

typedef Map<MatrixXd> mMatrix;
typedef Map<VectorXd> mVector;

void AmulB(double* pC, double* pA, double* pB, long M, long K, long N){
  mMatrix A(pA, M, K);
  mMatrix B(pB, K, N);
  mMatrix C(pC, M, N);
  C.noalias() = A * B;
  return;
}
void AmulBt(double* pC, double* pA, double* pBt, long M, long K, long N){
  mMatrix A(pA, M, K);
  mMatrix Bt(pBt, N, K);
  mMatrix C(pC, M, N);
  C.noalias() = A * Bt.transpose();
  return;
}
void AtmulB(double* pC, double* pAt, double* pB, long M, long K, long N){
  mMatrix At(pAt, K, M);
  mMatrix B(pB, K, N);
  mMatrix C(pC, M, N);
  C.noalias() = At.transpose() * B;
  return;
}
void AtmulBt(double* pC, double* pAt, double* pBt, long M, long K, long N){
  mMatrix At(pAt, K, M);
  mMatrix Bt(pBt, N, K);
  mMatrix C(pC, M, N);
  C.noalias() = At.transpose() * Bt.transpose();
  return;
}

void Amulvb(double* px, double* pA, double* py, long M, long N){
  mVector x(px, M);
  mMatrix A(pA, M, N);
  mVector y(py, N);
  x.noalias() = A * y;
  return;
}

void Atmulvb(double* px, double* pAt, double* py, long M, long N){
  mVector x(px, M);
  mMatrix At(pAt, M, N);
  mVector y(py, N);
  x.noalias() = At.transpose() * y;
  return;
}

double dot(double* pa, double* pb, long N){
  mVector a(pa, N);
  mVector b(pb, N);
  return a.dot(b);
}

double selfdot(double* pa, long N){
  mVector a(pa, N);
  return a.dot(a);
}

double dot3(double* px, double* pA, double* py, long M, long N){
  mVector x(px, M);
  mMatrix A(pA, M, N);
  mVector y(py, N);
  return x.dot(A * y);
}

void aplusBc(double* pD, double* pa, double* pB, double* pc, long M, long N){
  mMatrix D(pD, M, N);
  mVector a(pa, M);
  mMatrix B(pB, M, N);
  mVector c(pc, N);
  // D = (a + (B * c.asDiagonal()).colwise());
  D.colwise() = a;
  D.noalias() += B * c.asDiagonal();
  return;
}

double OLSlp(double* py, double* pA, double* px, long M, long N){
  mVector y(py, M);
  mMatrix A(pA, M, N);
  mVector x(px, N);
  return (y - A * x).squaredNorm();
}

void AplusAt(double* pB, double* pA, long N){
  mMatrix B(pB, N, N);
  mMatrix A(pA, N, N);
  B = A + A.transpose();
  return;
}

// double logdettriangle(double* pA, long N){
//   mMatrix A(pA, N, N);
//   return log(A.diagonal()).sum();
// }


//Refer: https://github.com/gaoxiang12/g2o_ba_example
#include "common.h"

// for std
#include <iostream>
// for opencv
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <boost/concept_check.hpp>
// for g2o
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/types/slam3d/se3quat.h>
#include <g2o/types/sba/types_six_dof_expmap.h>

using namespace std;

// 寻找两个图像中的对应点，像素坐标系
// 输入：img1, img2 两张图像
// 输出：points1, points2, 两组对应的2D点
int findCorrespondingPoints(const cv::Mat &img1, const cv::Mat &img2, vector<cv::Point2f> &points1, vector<cv::Point2f> &points2);

// 相机内参
double cx = 325.5;
double cy = 253.5;
double fx = 518.0;
double fy = 519.0;

int main(int argc, char **argv)
{
    // 调用格式：命令 [第一个图] [第二个图]
    if (argc != 3)
    {
        cout << "Usage: ba_example img1, img2" << endl;
        exit(1);
    }

    // 读取图像
    cv::Mat img1 = cv::imread(argv[1]);
    cv::Mat img2 = cv::imread(argv[2]);

    // 找到对应点
    vector<cv::Point2f> pts1, pts2;
    if (findCorrespondingPoints(img1, img2, pts1, pts2) == false)
    {
        cout << "匹配点不够！" << endl;
        return 0;
    }
    cout << "找到了" << pts1.size() << "组对应特征点。" << endl;
    // 构造g2o中的图
    // 先构造求解器
    g2o::SparseOptimizer optimizer;
    // 使用Cholmod中的线性方程求解器
    std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver = g2o::make_unique<g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>>();

    // 6*3 的参数
    std::unique_ptr<g2o::BlockSolver_6_3> block_solver = g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver));
    // L-M 下降
    g2o::OptimizationAlgorithmLevenberg *algorithm = new g2o::OptimizationAlgorithmLevenberg(std::move(block_solver));

    optimizer.setAlgorithm(algorithm);
    optimizer.setVerbose(false);

    // 添加节点
    // 两个位姿节点
    for (int i = 0; i < 2; i++)
    {
        g2o::VertexSE3Expmap *v = new g2o::VertexSE3Expmap();
        v->setId(i);
        if (i == 0)
            v->setFixed(true); // 第一个点固定为零
        // 预设值为单位Pose，因为我们不知道任何信息
        v->setEstimate(g2o::SE3Quat());
        optimizer.addVertex(v);
    }
    // 很多个特征点的节点
    // 以第一帧为准
    for (size_t i = 0; i < pts1.size(); i++)
    {
        g2o::VertexSBAPointXYZ *v = new g2o::VertexSBAPointXYZ();
        v->setId(2 + i);
        // 由于深度不知道，只能把深度设置为1了
        double z = 1;
        double x = (pts1[i].x - cx) * z / fx;
        double y = (pts1[i].y - cy) * z / fy;
        v->setMarginalized(true);
        v->setEstimate(Eigen::Vector3d(x, y, z));
        optimizer.addVertex(v);
    }

    // 准备相机参数
    g2o::CameraParameters *camera = new g2o::CameraParameters(fx, Eigen::Vector2d(cx, cy), 0);
    camera->setId(0);
    optimizer.addParameter(camera);

    // 准备边
    // 第一帧
    vector<g2o::EdgeProjectXYZ2UV *> edges;
    for (size_t i = 0; i < pts1.size(); i++)
    {
        g2o::EdgeProjectXYZ2UV *edge = new g2o::EdgeProjectXYZ2UV();
        edge->setVertex(0, dynamic_cast<g2o::VertexSBAPointXYZ *>(optimizer.vertex(i + 2)));
        edge->setVertex(1, dynamic_cast<g2o::VertexSE3Expmap *>(optimizer.vertex(0)));
        edge->setMeasurement(Eigen::Vector2d(pts1[i].x, pts1[i].y));
        edge->setInformation(Eigen::Matrix2d::Identity());
        edge->setParameterId(0, 0);
        // 核函数
        edge->setRobustKernel(new g2o::RobustKernelHuber());
        optimizer.addEdge(edge);
        edges.push_back(edge);
    }
    // 第二帧
    for (size_t i = 0; i < pts2.size(); i++)
    {
        g2o::EdgeProjectXYZ2UV *edge = new g2o::EdgeProjectXYZ2UV();
        edge->setVertex(0, dynamic_cast<g2o::VertexSBAPointXYZ *>(optimizer.vertex(i + 2)));
        edge->setVertex(1, dynamic_cast<g2o::VertexSE3Expmap *>(optimizer.vertex(1)));
        edge->setMeasurement(Eigen::Vector2d(pts2[i].x, pts2[i].y));
        edge->setInformation(Eigen::Matrix2d::Identity());
        edge->setParameterId(0, 0);
        // 核函数
        edge->setRobustKernel(new g2o::RobustKernelHuber());
        optimizer.addEdge(edge);
        edges.push_back(edge);
    }

    cout << "开始优化" << endl;
    optimizer.setVerbose(true);
    optimizer.initializeOptimization();
    optimizer.optimize(10);
    cout << "优化完毕" << endl;

    //我们比较关心两帧之间的变换矩阵
    g2o::VertexSE3Expmap *v = dynamic_cast<g2o::VertexSE3Expmap *>(optimizer.vertex(1));
    Eigen::Isometry3d pose = v->estimate();
    cout << "Pose=" << endl
         << pose.matrix() << endl;

    // 以及所有特征点的位置
    for (size_t i = 0; i < pts1.size(); i++)
    {
        g2o::VertexSBAPointXYZ *v = dynamic_cast<g2o::VertexSBAPointXYZ *>(optimizer.vertex(i + 2));
        cout << "vertex id " << i + 2 << ", pos = ";
        Eigen::Vector3d pos = v->estimate();
        cout << pos(0) << "," << pos(1) << "," << pos(2) << endl;
    }

    // 估计inlier的个数
    int inliers = 0;
    for (auto e : edges)
    {
        e->computeError();
        // chi2 就是 error*\Omega*error, 如果这个数很大，说明此边的值与其他边很不相符
        if (e->chi2() > 1)
        {
            cout << "error = " << e->chi2() << endl;
        }
        else
        {
            inliers++;
        }
    }

    cout << "inliers in total points: " << inliers << "/" << pts1.size() + pts2.size() << endl;
    optimizer.save("ba.g2o");
    return 0;
}

int findCorrespondingPoints(const cv::Mat &img1, const cv::Mat &img2, vector<cv::Point2f> &points1, vector<cv::Point2f> &points2)
{

    cv::Ptr<cv::Feature2D> orb = cv::ORB::create(1000, 1.2, 8, 31, 0, 2, 0, 31, 20);
    vector<cv::KeyPoint> kp1, kp2;
    cv::Mat desp1, desp2;
    orb->detectAndCompute(img1, cv::Mat(), kp1, desp1);
    orb->detectAndCompute(img2, cv::Mat(), kp2, desp2);
    cout << "分别找到了" << kp1.size() << "和" << kp2.size() << "个特征点" << endl;

    cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");

    double knn_match_ratio = 0.8;
    vector<vector<cv::DMatch>> matches_knn;
    matcher->knnMatch(desp1, desp2, matches_knn, 2);
    vector<cv::DMatch> matches;
    for (size_t i = 0; i < matches_knn.size(); i++)
    {
        if (matches_knn[i][0].distance < knn_match_ratio * matches_knn[i][1].distance)
            matches.push_back(matches_knn[i][0]);
    }

    if (matches.size() <= 20) //匹配点太少
        return false;

    for (auto m : matches)
    {
        points1.push_back(kp1[m.queryIdx].pt);
        points2.push_back(kp2[m.trainIdx].pt);
    }

    return true;
}


#include <bits/stdc++.h>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
typedef long long ll;
typedef vector <int> vi;

int der[14];
int fac[14];

int main(){
    ios_base::sync_with_stdio(false);
    cin.tie(nullptr);
    freopen("in.txt", "r", stdin);
    freopen("out.txt", "w", stdout);
    der[0] = 1; der[1] = 0;
    int res = 1;
    fac[0] = fac[1] = 1;
    for(int i=2; i<13; i++){
        der[i] = (i-1) * (der[i-1] + der[i-2]);
        fac[i] = res = res * i;
    }
    int tc; cin >> tc;
    for(int cas = 1; cas <= tc; cas++){
        int n; cin >> n;
        cout << der[n] << "/" << fac[n] << "\n";
    }
    return 0;
}

Rare Topics/Rare Algorithms/Formulas or Theorems/Hats.cpp


// Copyright 2013 Velodyne Acoustics, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkVelodyneHDLReader.cxx

  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.

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

#include "vtkPlaneFitter.h"

#include "vtkObjectFactory.h"
#include "vtkPointSet.h"
#include "vtkSmartPointer.h"
#include "vtkDoubleArray.h"
#include "vtkThreshold.h"
#include "vtkUnstructuredGrid.h"
#include "vtkNew.h"

#include <Eigen/Dense>

//-----------------------------------------------------------------------------
vtkStandardNewMacro(vtkPlaneFitter);

//-----------------------------------------------------------------------------
vtkPlaneFitter::vtkPlaneFitter()
{
}

//-----------------------------------------------------------------------------
vtkPlaneFitter::~vtkPlaneFitter()
{
}

//-----------------------------------------------------------------------------
void vtkPlaneFitter::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);
}

//-----------------------------------------------------------------------------
void vtkPlaneFitter::PlaneFit(vtkPointSet* pts, double origin[3], double normal[3],
                              double &minDist, double &maxDist, double &stdDev,
                              double channelMean[32], double channelStdDev[32],
                              vtkIdType channelNpts[32])
{
  using namespace Eigen;

  vtkSmartPointer<vtkDoubleArray> ptdata = vtkSmartPointer<vtkDoubleArray>::New();
  ptdata->DeepCopy(pts->GetPoints()->GetData());

  const vtkIdType n = ptdata->GetNumberOfTuples();
  if(n < 1)
    {
    return;
    }

  assert(ptdata->GetNumberOfComponents() == 3);
  Map<MatrixXd> eigpointsraw(static_cast<double*>(ptdata->GetVoidPointer(0)),
                             ptdata->GetNumberOfComponents(),
                             ptdata->GetNumberOfTuples());

  MatrixXd eigpoints = eigpointsraw.transpose();

  VectorXd mean(3);

  mean = eigpoints.colwise().sum() / n;
  assert(mean.size() == 3);

  for(int i = 0; i < 3; ++i)
    {
    origin[i] = mean[i];
    }

  eigpoints.rowwise() -= mean.transpose();

  JacobiSVD<MatrixXd> svd(eigpoints, ComputeThinU | ComputeThinV);

  VectorXd enormal = svd.matrixV().col(2);
  assert(enormal.size() == 3);
  assert(std::fabs(enormal.norm() - 1.0) < 1.0e-8);

  for(int i = 0; i < 3; ++i)
    {
    normal[i] = enormal[i];
    }

  VectorXd distances = eigpoints * enormal;
  assert(distances.size() == n);

  minDist = distances.minCoeff();
  maxDist = distances.maxCoeff();

  stdDev = std::sqrt(distances.squaredNorm() / (n-1));

  for(int i = 0; i < 32; ++i)
    {
    vtkNew<vtkThreshold> threshold;
    threshold->ThresholdBetween(i,i);
    threshold->SetInputData(pts);
    threshold->SetInputArrayToProcess(0, 0, 0, vtkDataObject::FIELD_ASSOCIATION_POINTS, "laser_id");
    threshold->SetOutputPointsPrecision(vtkAlgorithm::DEFAULT_PRECISION);
    threshold->Update();

    channelNpts[i] = threshold->GetOutput()->GetNumberOfPoints();

    vtkSmartPointer<vtkDoubleArray> threshdata = vtkSmartPointer<vtkDoubleArray>::New();
    threshdata->DeepCopy(threshold->GetOutput()->GetPoints()->GetData());

    const vtkIdType n = threshdata->GetNumberOfTuples();
    if(n < 2)
      {
      channelMean[i] = 0.0;
      channelStdDev[i] = 0.0;
      continue;
      }

    assert(threshdata->GetNumberOfComponents() == 3);
    assert(threshdata->GetNumberOfTuples() >= 2);
    Map<MatrixXd> channelraw(static_cast<double*>(threshdata->GetVoidPointer(0)),
                             threshdata->GetNumberOfComponents(),
                             threshdata->GetNumberOfTuples());

    MatrixXd channelpts = channelraw.transpose();
    channelpts.rowwise() -= mean.transpose();

    VectorXd channelds = channelpts * enormal;

    double cmean = channelds.sum() / channelds.size();
    double cstddev = std::sqrt((channelds.array() - cmean).matrix().squaredNorm() / (channelds.size()-1));

    channelMean[i] = cmean;
    channelStdDev[i] = cstddev;
    }

}


//Authors: Dario Cattaruzza, Alessandro Abate, Peter Schrammel, Daniel Kroening
//University of Oxford 2016
//This code is based on Komei Fukuda's ccd implementation and as such supplied under the GPL license agreement (see license.txt)

/* dplex.c:  dual simplex method c-code
   written by Komei Fukuda, fukuda@ifor.math.ethz.ch
   Version 0.61, December 1, 1997
*/

/* dplex.c : C-Implementation of the dual simplex method for
   solving an LP: max/min  c^T x subject to  x in P, where
   P= {x :  b - A x >= 0}.
   Please read COPYING (GNU General Public Licence) and
   the manual cddman.tex for detail.
*/

#include "DualSimplex.h"

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>
#include <string.h>

#include <boost/timer.hpp>

namespace abstract {
template <class scalar>  DualSimplex<scalar>::DualSimplex(const int size,const int dimension)  :
    Tableau<scalar>(size,dimension),
    m_auxiliaryRow(1,m_dimension),
    m_orBlockSize(1)
{}

template <class scalar>
bool DualSimplex<scalar>::load(const MatrixS &faces,const MatrixS &supports,const bool transpose)
{
  this->Conversion=LPMax;
  if (!Tableau<scalar>::load (faces,supports,transpose)) return false;
  m_auxiliaryRow.resize(1,m_dimension);
  m_tableau.coeffRef(m_objectiveRow,0)=0;
  return true;
}

template <class scalar>
bool DualSimplex<scalar>::SelectDualSimplexPivot(const bool Phase1,pivot_t &pivot)
{ /* selects a dual simplex pivot (pivot.row, pivot.col) if the current
     basis is dual feasible and not optimal. If not dual feasible,
     the procedure returns false and m_status=LPSundecided.
     If Phase1=true, the RHS column will be considered as the negative
     of the column of the largest variable (==m_size).  For this case, it is assumed
     that the caller used the auxiliary row (with variable m_size) to make the current
     dictionary dual feasible before calling this routine so that the nonbasic
     column for m_size corresponds to the auxiliary variable.
  */
  refScalar maxrat=0,rat=0;
  scalar val=0;
  scalar rcost[m_dimension];

  pivot.col=0;
  m_status=eUndecided;
  for (int j=1; j<m_dimension; j++){//ignore RHSCol
    rcost[j]=entry(m_objectiveRow,j);
    if (func::isPositive(rcost[j])) {
      //The zero case may cause an overapproximation of an empty set to an m_zero^n size hypercube.
      return false;
    }
  }
  //Dual Feasible.
  pivot.row=Phase1 ? findMaxRow(m_basicVars[m_size]) : findMinRow(RHSCol);
  if (pivot.row<0) {
    m_status=eOptimal;
  }
  else {
    for (int j=1; j<m_dimension; j++){// ignore RHSCol
      val=entry(pivot.row,j);
      if (func::isPositive(val)) {
        //The zero case would result in a pivot move by a value inside the interval of the hyperplane
        rat=func::toUpper(rcost[j]/val);
        if ((pivot.col==0) || (rat > maxrat)){
          maxrat=rat;
          pivot.col=j;
        }
      }
    }
    if (pivot.col>0) return true;
    m_status=eInconsistent;
  }
  return false;
}

template <class scalar>
bool DualSimplex<scalar>::SelectOredPivot(const long rowmax, const Set &noPivotRow, const Set &noPivotCol, pivot_t &pivot)
/* Select a position (pivot) in the matrix X.T such that (X.T)[pivot.row][pivot.col] is nonzero
   The choice is feasible, i.e., not on NopivotRow and NopivotCol, and
   best with respect to the specified roworder
 */
{
  long rtemp;
  Set rowexcluded(noPivotRow);
  scalar Xtemp,Xtemp2;
  scalar Ftemp,Ftemp2;
  for (rtemp=rowmax;rtemp<m_size;rtemp++) {
    rowexcluded.add(rtemp);   /* cannot pivot on any row > rmax */
  }
  while(true) {
    rtemp=-1;
    for (int i=1;i<=m_size;i++) {
      if (!rowexcluded.member(m_tableau.zeroOrder(i))){
        rtemp=m_tableau.zeroOrder(i);
        break;
      }
    }
    if (rtemp>=0) {
      rtemp-=(rtemp%m_orBlockSize);
      pivot.row=rtemp;
      for (pivot.col=0;pivot.col < m_dimension;pivot.col++) {
        if (!noPivotCol.member(pivot.col)) {
          Xtemp=entry(pivot.row,pivot.col);
          Ftemp=Xtemp/entry(pivot.row,0);
          int offset=0;
          for (int j=1;j<m_orBlockSize;j++) {
            Xtemp2=entry(pivot.row+j,pivot.col);
            Ftemp2=Xtemp2/entry(pivot.row+j,0);
            if (func::isPositive(Ftemp2-Ftemp)) {
              offset=j;
              Xtemp=Xtemp2;
              Ftemp=Ftemp2;
            }
          }
          pivot.row+=offset;
          char sign=func::softSign(Xtemp);//hardSign(Xtemp);//Zero is ensured by check in entry
          if (sign<0) return true;
        }
      }
      for (int i=0;i<m_orBlockSize;i++) rowexcluded.add(rtemp+i);
    }
    else {
      pivot.row = -1;
      pivot.col = -1;
      return false;
    }
  }
  return true;
}

template <class scalar>
void DualSimplex<scalar>::AuxiliaryPivotAndUpdate(long col)
{
  MatrixS Rtemp=m_auxiliaryRow*m_basisInverse;
  char sign=func::hardSign(Rtemp.coeff(0,col));
  if (sign==0) return;
  scalar Xtemp;
  refScalar Xtemp0 = func::toCentre(Rtemp.coeff(0,col));
  for (int j = 0; j < m_dimension; j++) {
    if (j != col) {
      Xtemp = Rtemp.coeff(0,j) / Xtemp0;
      for (int j1 = 0; j1 < m_dimension; j1++)
        func::msub(m_basisInverse.coeffRef(j1,j),m_basisInverse.coeff(j1,col),Xtemp); //m_basisInverse.coeffRef(j1,j) -= m_basisInverse.coeff(j1,col) * Xtemp;
    }
  }
  m_basisInverse.col(col) /= Xtemp0;
  if (ms_trace_tableau>eTraceTableau) this->logBasis(m_size,col);
  long entering=m_nonBasicRow[col];
  m_basicVars[m_size]=col;              // the nonbasic variable r corresponds to column s
  m_nonBasicRow[col]=m_size;            // the nonbasic variable on s column is r
  if (entering>=0) m_basicVars[entering]=-1; // original variables have negative index and should not affect the row index
}

// Find the corresponding row with the minimum entry value for a given column
template <class scalar>
int DualSimplex<scalar>::findMinRow(const int col)
{
  int row=-1;
  refScalar minval=0;
  refScalar val;
  for (int i=0; i<m_objectiveRow; i++) {
    if (m_basicVars[i]<0) {  /* i is a basic variable */
      val=func::toUpper(entry(i,col)); // for dual Phase I (auxiliary row is non-basic)
      if (val < minval) {
        row=i;
        minval=val;
      }
    }
  }
  return row;
}

// Find the corresponding row with the minimum entry value for a given column
template <class scalar>
int DualSimplex<scalar>::findMaxRow(const int col)
{
  int row=-1;
  refScalar maxval=0;
  refScalar val;
  for (int i=0; i<m_objectiveRow; i++) {
    if (m_basicVars[i]<0) {  /* i is a basic variable */
      // for dual Phase I (auxiliary row is non-basic)
      val=func::toLower(entry(i,col));
      if (val > maxval) {
        row=i;
        maxval=val;
      }
    }
  }
  return row;
}

template <class scalar>
int DualSimplex<scalar>::FindDualFeasibleBasis()
{ /* Find a dual feasible basis using Phase I of Dual Simplex method.
     If the problem is dual feasible,
     the procedure returns m_status=LPSundecided and a dual feasible
     basis.   If the problem is dual infeasible, this returns
     m_status=DualInconsistent and the evidence column.
  */

  long rank=0;
  pivot_t pivot;

  m_status=eUndecided; this->m_evidenceCol=-1;
  if (ms_trace_tableau>eTraceTableau) this->logBasis(m_objectiveRow,-1);
  scalar maxcost=-1;
  int maxReducedCostCol=0;  /* ms will be the index of column which has the largest reduced cost */
  for (int col=1; col<m_tableau.cols(); col++){//ignore RHSCol
    scalar cost=entry(m_objectiveRow,col);
    if (func::toLower(cost) > func::toUpper(maxcost)) {maxcost=cost; maxReducedCostCol = col;}//TODO:might want to check for imprecision
  }
  if (ms_trace_tableau>=eTracePivots) {
    std::stringstream buffer;
    buffer << "Dual feasible Basis. cost=" << ms_logger.MakeNumber(maxcost) << ",c=" << maxReducedCostCol;
    ms_logger.logData(buffer.str());
  }
  if (!func::isPositive(maxcost)) return rank;//Dual feasible
  //The zero case above indicates we are somewhere on the hyperplane which is feasible (or an m_zero overapprox)
  m_auxiliaryRow=MatrixS::Zero(1,m_dimension);
  for (int k=1; k<m_dimension; k++) {
    if (m_nonBasicRow[k]>=0) {
      m_auxiliaryRow-=m_tableau.row(m_nonBasicRow[k]);/* To make the auxiliary row (0,-1,-1,...,-1).  */
    }
  }
  if (ms_trace_tableau>eTracePivots) {
    ms_logger.logData(m_auxiliaryRow,"Auxiliary Row:");
    if (ms_trace_tableau>=eTraceEntries) {
      this->logNonBasic();
      this->logBasic();
      MatrixS matrix=m_auxiliaryRow*m_basisInverse;
      ms_logger.logData(matrix,"Entries");
    }
  }

  /* Pivot on (m_auxiliaryRow, maxReducedCostCol) so that the dual basic solution becomes feasible */
  AuxiliaryPivotAndUpdate(maxReducedCostCol);
  rank++;

  m_status=eUndecided;/* Dual Simplex Phase I */
  while (SelectDualSimplexPivot(true, pivot))  {
    this->ColumnPivotAndUpdate(pivot);
    rank++;
    if (m_basicVars[m_size]<0) return rank;
  }
  /* The current dictionary is terminal.  There are two cases:
     TableauEntry(m_objectiveRow,maxReducedCostCol) is negative or zero.
     The first case implies dual infeasible,
     and the latter implies dual feasible but m_size is still in nonbasis.
     We must pivot in the auxiliary variable m_size. */

  pivot.row=findMinRow(maxReducedCostCol);
  pivot.col=maxReducedCostCol;
  if (pivot.row>=0) {
    this->ColumnPivotAndUpdate(pivot);
    rank++;
  }
  if (func::isNegative(entry(m_objectiveRow, pivot.col))) {
    m_status=eDualInconsistent;
    this->m_evidenceCol=maxReducedCostCol;
  }
  return rank;
}

template <class scalar>
scalar DualSimplex<scalar>::maximise(const std::vector<scalar> &vector,const ResetType_t resetType)
{
  if (vector.size()<this->getDimension()) return func::ms_nan;
  for (int col=1;col<m_tableau.cols();col++) {
    m_tableau.coeffRef(m_objectiveRow,col)=vector.at(col-1);//TODO: check the sign
  }
  return processMaximize(resetType);
}

template <class scalar>
scalar DualSimplex<scalar>::maximise(const MatrixS &vector,const ResetType_t resetType)
{
  m_tableau.block(m_objectiveRow,1,1,vector.cols())=vector;//TODO: check the sign
  return processMaximize(resetType);
}

template <class scalar>
bool DualSimplex<scalar>::maximiseAll(const MatrixS &vectors, MatrixS &supports,AproxType_t aprox)
{
  boost::timer timer;
  try {
    if (aprox==eOverAprox)       this->toOuter();
    else if (aprox==eUnderAprox) this->toInner();
    if ((supports.rows()!=vectors.cols()) || (supports.cols()!=1)) supports.resize(vectors.cols(),1);
    FindFeasBasis(eResetBasis);
    for (int i=0;i<vectors.cols();i++) {
      m_tableau.block(m_objectiveRow,1,1,vectors.rows())=vectors.block(0,i,vectors.rows(),1).transpose();//TODO: check sign
      supports.coeffRef(i,0)=processMaximize(eUseDefaultBasis);
    }
  }
  catch(std::string error) {
    ms_logger.logData(error);
    return false;
  }
  if (this->ms_trace_time) {
    int elapsed=timer.elapsed()*1000;
    if (elapsed>0) {
      ms_logger.logData(this->getName(),false);
      ms_logger.logData(elapsed," Maximise time",true);
    }
  }
  return true;
}

template <class scalar>
scalar DualSimplex<scalar>::processMaximize(const ResetType_t resetType)
/*
When LP is inconsistent then *re returns the evidence row.
When LP is dual-inconsistent then *se returns the evidence column.
*/
{
  boost::timer timer;
  long rank=0;
  long maxpivfactor=70;
  pivot_t pivot;
  this->Error=this->None;
  func::setZero(this->m_zero);
  long maxpivots=maxpivfactor*m_dimension;  // maximum pivots to be performed before cc pivot is applied.
  long rebasepivots=((func::getDefaultPrec()>>6)+1)*m_dimension;
  /* Initializing control variables. */

  this->m_evidenceRow=-1;
  this->m_evidenceCol=-1;

  if (resetType!=eRebaseBasis) m_iterations=0;

  if (ms_trace_tableau>=eTracePivots) {
    ms_logger.logData(m_tableau,"Maximise");
  }

  m_iterations+=this->FindFeasBasis(resetType);
  if (this->ms_trace_time && (ms_trace_tableau>=eTracePivots) && (resetType!=eUseDefaultBasis)) {
    logPivotCount(timer.elapsed()*1000,"Find Feasible:");
  }
  m_status=eUndecided;
  if (this->m_evidenceCol<0) m_iterations+=FindDualFeasibleBasis();
  if ((this->ms_trace_time) && (ms_trace_tableau>=eTracePivots)) {
    logPivotCount(timer.elapsed()*1000,"Find DualFeasible:");
  }

  if (this->m_evidenceCol>=0){
    if (m_status==eUndecided) m_status=eStrucDualInconsistent;// No LP basis is found, and thus Inconsistent.
    // else No dual feasible basis is found, and thus DualInconsistent.
    return entry( m_objectiveRow,RHSCol);
  }

  if (ms_trace_tableau>=eTracePivots) ms_logger.logData("LP max");

  /* Dual Simplex Method */
  while(true) {
    m_status=eUndecided;
    if (rank>rebasepivots) {// && (func::toWidth(entry(m_objectiveRow,RHSCol))*m_dimension*m_dimension>m_zero)) {
      return processMaximize(eRebaseBasis);
    }
    if ((rank<maxpivots) && SelectDualSimplexPivot(false, pivot)) {
      this->ColumnPivotAndUpdate(pivot);
      rank++;
    }
    else if ((m_status==eUndecided) && SelectCrissCrossPivot(pivot)) {
      /* In principle this should not be executed because we already have dual feasibility
         attained and dual simplex pivot should have been chosen.  This might occur
         under floating point computation, or the case of cycling.
      */
      this->ColumnPivotAndUpdate(pivot);
      maxpivots+=maxpivfactor*m_dimension;
      rank++;
    }
    else {
      switch (m_status) {
        case eInconsistent: this->m_evidenceRow=pivot.row;
        case eDualInconsistent: this->m_evidenceCol=pivot.col;
        default: break;
      }
      break;
    }
  }
  m_iterations+=rank;
  if ((this->ms_trace_time) && (ms_trace_tableau>=eTracePivots)) logPivotCount(timer.elapsed()*1000,"Find Support:");
  scalar result=entry( m_objectiveRow,RHSCol);
  if (func::isNan(result))
    return result;
  return result;
}

template <class scalar>
int DualSimplex<scalar>::FindFeasBasis(const ResetType_t resetType)
{
  m_tableau.coeffRef(m_objectiveRow,RHSCol)=0;
  if (resetType==eUseDefaultBasis) {
    m_basisInverse=m_feasBasisInverse;
    m_basicVars=m_feasBasicVars;
    m_nonBasicRow=m_feasNonBasicRow;
    if (ms_trace_tableau>=eTraceTableau) this->logBasis(-1,-1);
    return 0;
  }
  else if (resetType==eRebaseBasis) {
    return this->Rebase();
  }
  this->ComputeRowOrderVector(MinIndex);
  int result=this->FindLPBasis();//TODO: go to feasLP?
  m_feasBasisInverse=m_basisInverse;
  m_feasBasicVars=m_basicVars;
  m_feasNonBasicRow=m_nonBasicRow;
  return result;
}

template <class scalar>
int DualSimplex<scalar>::FindFeasOrBasis(int orBlockSize,const ResetType_t resetType)
{
  normalise(true);
  m_orBlockSize=orBlockSize;
  m_tableau.coeffRef(m_objectiveRow,RHSCol)=0;
  if (resetType==eUseDefaultBasis) {
    m_basisInverse=m_feasBasisInverse;
    m_basicVars=m_feasBasicVars;
    m_nonBasicRow=m_feasNonBasicRow;
    if (ms_trace_tableau>=eTraceTableau) this->logBasis(-1,-1);
    return 0;
  }
  else if (resetType==eRebaseBasis) {
    return this->Rebase();
  }
  this->ComputeRowOrderVector(MinIndex);
  int result=this->FindOrLPBasis();
  m_feasBasisInverse=m_basisInverse;
  m_feasBasicVars=m_basicVars;
  m_feasNonBasicRow=m_nonBasicRow;
  return result;
}

template <class scalar>
int DualSimplex<scalar>::FindOrLPBasis()
{ /* Find a LP basis using Gaussian pivots.
     If the problem has an LP basis,
     the procedure returns m_evidenceCol=-1 if LPSundecided and an LP basis.
     If the constraint matrix A (excluding the rhs and objective) is not
     column indepent, there are two cases.  If the dependency gives a dual
     inconsistency, this returns the evidence column m_evidenceCol.  Otherwise, this returns an LP basis of size less than n_size.  Columns j
     that do not belong to the basis (i.e. cannot be chosen as pivot because
     they are all zero) will be indicated in nbindex vector: nbindex[j] will
     be negative and set to -j.
  */
  if (ms_trace_tableau>=eTracePivots) ms_logger.logData("Ored Feasibility Basis");
  ResetTableau();
  Set RowSelected(m_size);
  Set ColSelected(m_dimension);
  RowSelected.add(m_objectiveRow);
  ColSelected.add(RHSCol);
  pivot_t pivot;
  int rank=m_dimension;
  m_evidenceCol=-1;
  for (int i=0;i<m_dimension;i++) {   /* Find a set of rows for a basis */
    if (!SelectOredPivot(m_size, RowSelected, ColSelected, pivot))
    {
      rank=i;
      for (int j=1;j<m_dimension; j++) {//Skip RHSCol
        if (m_nonBasicRow[j]<0){
          if (!func::isZero(entry(m_objectiveRow,j),m_zero)) {  /* dual inconsistent */
            m_evidenceCol=j;
            break;
          }
        }
      }
      /* dependent columns but not dual inconsistent. */
      break;
    }
    if (ms_trace_tableau>=eTraceEntries) {
      RowSelected.logSet("Available Rows:",true);
      ColSelected.logSet("Available Cols:",true);
    }
    RowSelected.add(pivot.row);
    ColSelected.add(pivot.col);
    ColumnPivotAndUpdate(pivot);
  }
  return rank;
}

template <class scalar>
bool DualSimplex<scalar>::SelectCrissCrossPivot(pivot_t &pivot)
{
  m_status=eUndecided;
  for (int i=0; i<m_size; i++) {
    if (i!=m_objectiveRow && m_basicVars[i]==-1) {  /* i is a basic variable */
      if (func::isNegative(entry(i,RHSCol))) {
        //The zero case above indicates we are somewhere on the hyperplane which is feasible (or an m_zero overapprox)
        pivot.row=i;
        for (int j=0; j<m_size; j++) {
          if (m_basicVars[j] >0) { /* i is nonbasic variable */
             if (func::isPositive(entry(pivot.row,m_basicVars[j]))) {
               pivot.col=m_basicVars[j];
               return true;
             }
          }
        }
        m_status=eInconsistent;
        return false;
      }

    }
    else if (m_basicVars[i] >0) { /* i is nonbasic variable */
      if (func::isPositive(entry(m_objectiveRow,m_basicVars[i]))) {
        //The zero case above indicates we are somewhere on the hyperplane which is feasible (or an m_zero overapprox)
        pivot.col=m_basicVars[i];
        for (int j=0; j<m_size; j++) {
          if (j!=m_objectiveRow && m_basicVars[j]==-1) {  /* i is a basic variable */
            if (func::isNegative(entry(j,pivot.col))) {
              pivot.row=j;
              return true;
            }
          }
        }
        m_status=eDualInconsistent;
        return false;
      }
    }
  }
  m_status=eOptimal;
  return false;
}

/// Normalises the directions of the faces
template <class scalar>
void DualSimplex<scalar>::normalise(bool reload)
{
  if (this->m_isNormalised || m_faces.rows()<=0) return;
  MatrixS norms=m_faces.rowwise().norm();
  for (int i=0;i<m_faces.rows();i++) {
    char sign=func::hardSign(norms.coeff(i));
    if (sign!=0) {
      for (int j=0;j<m_faces.cols();j++) m_faces.coeffRef(i,j)/=norms.coeff(i);
      m_supports.coeffRef(i,0)=m_supports.coeff(i,0)/norms.coeff(i);
    }
  }
  this->m_isNormalised=true;
  if (reload) load(m_faces,m_supports);
}

/// Clears redundant faces in the polyhedra (caused by intersections and reductions)
template <class scalar>
bool DualSimplex<scalar>::removeRedundancies()
{
  int rows=m_faces.rows();
  if (rows<=0) return false;
  bool isRedundant[rows];
  for (int i=0;i<m_faces.rows();i++) isRedundant[i] =true;
  int redundant=m_faces.rows();
  this->m_isNormalised=false;
  for (int row=0;row<m_faces.rows();row++) {
    for (int col=0;col<m_faces.cols();col++) {
      char sign=func::hardSign(m_faces.coeff(row,col));
      if (sign!=0) {
        isRedundant[row]=false;
        redundant--;
        break;
      }
    }
  }
  normalise(false);
  SortedMatrix<scalar> faces(m_faces.rows(),m_faces.cols());
  faces.block(0,0,m_faces.rows(),m_faces.cols())=m_faces;
  faces.ComputeRowOrderVector(LexMin);
  for (int i=1;i<=m_faces.rows();i++)
  {
    int row=faces.zeroOrder(i);
    if (isRedundant[row]) continue;
    int count=m_faces.rows();
    for (int j=i+1;j<=count;j++) {
      int row2=faces.zeroOrder(j);
      if (isRedundant[row2]) continue;
      MatrixS check=m_faces.row(row)-m_faces.row(row2);
      char sign=func::hardSign(check.norm());
      if (sign==0) {
        isRedundant[row2]=true;//TODO: should aggregate width(error) on non-redundant vector
        if (func::isNegative(m_supports.coeff(row2,0)-m_supports.coeff(row,0))) {
          isRedundant[row2]=false;
          isRedundant[row]=true;
          i=j;
          row=row2;
        }
        redundant++;
      }
      else break;
    }
  }

/*  for (int i=0;i<m_faces.rows();i++)
  {
    if (isRedundant[i]) continue;
    int count=m_faces.rows();
    for (int j=i+1;j<count;j++) {
      if (isRedundant[j]) continue;
      MatrixS check=m_faces.row(i)-m_faces.row(j);
      char sign=func::hardSign(check.norm());
      if (sign==0) isRedundant[j]=true;
      if (isRedundant[j]) {
        if (m_supports.coeff(j,0)<m_supports.coeff(i,0)) {
          isRedundant[j]=false;
          isRedundant[i++]=true;
          while ((i<count) && isRedundant[i]) i++;
          j=i+1;
        }
        redundant++;
      }
    }
  }*/
  if (redundant>0) {
    int pos=0;
    for (int i=0;i<m_faces.rows();i++) {
      if (isRedundant[i]) continue;
      m_faces.row(pos)=m_faces.row(i);
      m_supports.coeffRef(pos,0)=m_supports.coeff(i,0);
      pos++;
    }
    m_faces.conservativeResize(pos,m_faces.cols());
    m_supports.conservativeResize(pos,1);
    load(m_faces,m_supports);
    return true;
  }
  load(m_faces,m_supports);
  return false;
}

/// Saves the time and iteration count data for the given process
template <class scalar>
void DualSimplex<scalar>::logPivotCount(int time,std::string process)
{
  ms_logger.logData(time,process,true);
  ms_logger.logData(m_iterations,"Iterations:",true);
}

#ifdef USE_LDOUBLE
  #ifdef USE_SINGLES
    template class DualSimplex<long double>;
  #endif
  #ifdef USE_INTERVALS
    template class DualSimplex<ldinterval>;
  #endif
#endif
#ifdef USE_MPREAL
  #ifdef USE_SINGLES
    template class DualSimplex<mpfr::mpreal>;
  #endif
  #ifdef USE_INTERVALS
    template class DualSimplex<mpinterval>;
  #endif
#endif

}


#include <iostream>
#include <fstream>
#include <boost/config.hpp>
#include <boost/version.hpp>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Polygon_2.h>
#include <CGAL/Polygon_with_holes_2.h>
#include <CGAL/Constrained_Delaunay_triangulation_2.h>
#include <CGAL/Constrained_triangulation_plus_2.h>
#include <CGAL/Polyline_simplification_2/simplify.h>
#include <CGAL/IO/WKT.h>

namespace PS = CGAL::Polyline_simplification_2;

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Polygon_2<K>    Polygon_2;
typedef CGAL::Polygon_with_holes_2<K>    Polygon_with_holes_2;
typedef PS::Vertex_base_2<K>  Vb;
typedef CGAL::Constrained_triangulation_face_base_2<K> Fb;
typedef CGAL::Triangulation_data_structure_2<Vb, Fb> TDS;
typedef CGAL::Exact_predicates_tag                          Itag;
typedef CGAL::Constrained_Delaunay_triangulation_2<K,TDS, Itag> CDT;
typedef CGAL::Constrained_triangulation_plus_2<CDT>     CT;
typedef CT::Point                           Point;
typedef CT::Constraint_id                   Constraint_id;
typedef CT::Constraint_iterator             Constraint_iterator;
typedef CT::Vertices_in_constraint_iterator Vertices_in_constraint_iterator;
typedef CT::Points_in_constraint_iterator   Points_in_constraint_iterator;
typedef PS::Stop_below_count_ratio_threshold Stop;
typedef PS::Squared_distance_cost Cost;

void print(const CT& ct, Constraint_id cid)
{
  std::cout << "simplified polyline" <<std::endl;
  for(Vertices_in_constraint_iterator vit =
        ct.vertices_in_constraint_begin(cid);
      vit != ct.vertices_in_constraint_end(cid);
      ++vit){
    std::cout << (*vit)->point() << std::endl ;
  }

  std::cout << "original points" <<std::endl;
  for(Points_in_constraint_iterator pit =
        ct.points_in_constraint_begin(cid);
      pit != ct.points_in_constraint_end(cid);
      ++pit){
    std::cout << *pit << std::endl ;
  }

}

int main(int argc, char* argv[])
{
  std::ifstream ifs( (argc==1)?"data/polygon.wkt":argv[1]);
  const bool remove_points = false;
  CT ct;
  Polygon_with_holes_2 P;
  Constraint_id cid;
  std::size_t largest = 0;
  while(CGAL::IO::read_polygon_WKT(ifs, P)){
    const Polygon_2& poly = P.outer_boundary();
    Constraint_id cid2 = ct.insert_constraint(poly);
    if(poly.size() > largest){
      cid = cid2;
    }
  }

  PS::simplify(ct, cid, Cost(), Stop(0.5), remove_points);
  print(ct, cid);
  PS::simplify(ct, cid, Cost(), Stop(0.5), remove_points);
  ct.remove_points_without_corresponding_vertex(cid);
  print(ct, cid);
  return 0;
}


/*

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

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

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

*/

#ifndef PIC_COLORS_MATRIX_FORM_PRIMARIES_HPP
#define PIC_COLORS_MATRIX_FORM_PRIMARIES_HPP

#include "../base.hpp"

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

namespace pic {

/**
 * @brief createMatrixFromPrimaries computes a matrix for converting XYZ values into the
 * defined color space (i.e., by defining the three primaries: red, green, and blue).
 * @param red_XYZ is the XYZ values of the red primary
 * @param green_XYZ is the XYZ values of the green primary
 * @param blue_XYZ is the XYZ values of the blue primary
 * @param white_point_XYZ is the XYZ values of the white point primary
 * @return It returns a 3x3 matrix for converting XYZ values into the defined color space
 */
float *createMatrixFromPrimaries(float *red_XYZ,
                                 float *green_XYZ,
                                 float *blue_XYZ,
                                 float *white_point_XYZ,
                                 float *ret = NULL
                                 )
{
    if(red_XYZ == NULL || green_XYZ == NULL || blue_XYZ == NULL) {
        return ret;
    }

    if(ret == NULL) {
        ret = new float[9];
    }

#ifndef PIC_DISABLE_EIGEN

    int w = 0;
    if(white_point_XYZ != NULL) {
        w = 3;
    }

    //set up a liner system A x = b
    int nRow = 9 + w;
    Eigen::MatrixXf A(nRow, 9);
    Eigen::VectorXf b(nRow);

    //A matrix
    A.setZero();

    //red
    for(int j = 0; j < 3; j++) {
        for(int i = 0 ; i < 3; i++) {
            A(j, j * 3 + i) = red_XYZ[i];
        }
    }

    //green`
    for(int j = 0; j < 3; j++) {
        for(int i = 0 ; i < 3; i++) {
            A(j + 3, j * 3 + i) = green_XYZ[i];
        }
    }

    //blue`
    for(int j = 0; j < 3; j++) {
        for(int i = 0 ; i < 3; i++) {
            A(j + 6, j * 3 + i) = blue_XYZ[i];
        }
    }

    //white
    if(w == 3) {
        for(int j = 0; j < 3; j++) {
            for(int i = 0 ; i < 3; i++) {
                A(j + 9, j * 3 + i) = white_point_XYZ[i];
            }
        }
    }

    //b vector
    b(0) = 1.0f;
    b(1) = 0.0f;
    b(2) = 0.0f;

    b(3) = 0.0f;
    b(4) = 1.0f;
    b(5) = 0.0f;

    b(6) = 0.0f;
    b(7) = 0.0f;
    b(8) = 1.0f;

    if(w == 3) {
        b(9) = 1.0f;
        b(10) = 1.0f;
        b(11) = 1.0f;
    }

    //solve Ax=b
    Eigen::VectorXf x = A.colPivHouseholderQr().solve(b);

    for(int i = 0; i < 9; i++) {
        ret[i] = x(i);
    }
#endif
    return ret;
}

} // end namespace pic

#endif /* PIC_COLORS_MATRIX_FORM_PRIMARIES_HPP */


#include <iostream>
#include <stdlib.h>

#include <gmpxx.h>
#include <NTL/ZZ.h>

using namespace std;

void usage(char *progname) {
	cout << "This program finds m given m^e and e, as long as "
		"m^e < n." << endl;
	cout << "Usage: " << progname << " c e" << endl;
}

int main(int argc, char *argv[]) {
	if (argc != 3) { usage(argv[0]); return 3; }

	// NTL doesn't provide an integer root function, so we
	// have to use GMP integers
	mpz_class c, root;
	mpz_t r;
	mpz_init(r);
	if(c.set_str(argv[1],0)) {
		cerr << "Invalid message: " << argv[1] << endl;
		return 2;
	}

	unsigned long e;
	if(!(e = strtoul(argv[2], NULL, 0))) {
		cerr << "Invalid public exponent: " << argv[2] << endl;
		return 2;
	}

	// Find c^(1/e)
	if(!mpz_root(r, c.get_mpz_t(), e)) {
		cerr << "Error: m^e is greater than n!" << endl;
		return 1;
	} else {
		cout << mpz_class(r) << endl;
	}
}


// Copyright (C) 2016 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef STAT_NORMAL_DISTRIBUTION_HPP
#define STAT_NORMAL_DISTRIBUTION_HPP

#include <stdexcept>
#include <string>
#include <boost/lexical_cast.hpp>
#include <boost/throw_exception.hpp>
#include "power.hpp"
#include "moment.hpp"

namespace stat {

using math::p2;
using math::p3;
using math::p4;

class normal_distribution : public moment<normal_distribution> {
private:
  typedef moment<normal_distribution> super_type;
public:
  normal_distribution(double mu = 0, double sigma = 1) : super_type(*this), mu_(mu), sigma_(sigma) {
    if (sigma_ <= 0)
      boost::throw_exception(std::invalid_argument("stat::normal_distribution"));
  }
  std::string name() const {
    return "Normal Distribution: N(" + boost::lexical_cast<std::string>(mu_) + ","
      + boost::lexical_cast<std::string>(sigma_) + ")";
  }
  double moment1() const { return mu_; }
  double moment2() const { return p2(mu_) + p2(sigma_); }
  double moment3() const { return p3(mu_) + 3 * mu_ * p2(sigma_); }
  double moment4() const { return p4(mu_) + 6 * p2(mu_) * p2(sigma_) + 3 * p4(sigma_); }
private:
  double mu_, sigma_;
};

} // end namespace stat

#endif // STAT_NORMAL_DISTRIBUTION_HPP