/**
    @file bayes_classifier.cpp

    @author Terence Henriod

    Project 1: Bayesian Minimum Error Classification

    @brief Class implementations for the StrictGaussianClassifier defined in
           bayes_classifier.h.

    @version Original Code 1.00 (3/8/2014) - T. Henriod
*/

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
                   HEADER FILES / NAMESPACES
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

// Class Declaration
#include "strict_gaussian_classifier.h"

// Other Dependencies
#include <cassert>
#include <iostream>
#include <fstream>

#include "bayes_classifier.h"
#include <Eigen/Dense>  // -I /home/thenriod/Desktop/cpp_libs/Eigen_lib

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
================================================================================
                   CLASS FUNCTION IMPLEMENTATIONS
================================================================================
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
                   CONSTRUCTOR(S) / DESTRUCTOR
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

/**
StrictGaussianClassifier

Description

@pre
-# The GameState object is given an appropriate identifier.

@post
-# A new, empty GameState will be initialized.

@code
@endcode
*/
StrictGaussianClassifier::StrictGaussianClassifier()
{
  // variables
  int ndx = 0;
  Eigen::Matrix2d temp_matrix;
    temp_matrix << 1, 0,
                   0, 1;

  // initialize all members
  class_name_ = "Give me a name!";
  mean_vector_ << 1, 1;
  set_covariance( temp_matrix );
  decision_threshold_ = 0.5;

  // no return - constructor
}

StrictGaussianClassifier::StrictGaussianClassifier(
    const StrictGaussianClassifier& other )
{
  // no return - copy constructor
}

StrictGaussianClassifier& StrictGaussianClassifier::operator=(
    const StrictGaussianClassifier& other )
{
  // return *this
  return *this;
}

StrictGaussianClassifier::~StrictGaussianClassifier()
{
  // currently nothing to destruct

  // no return - destructor
}

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
                   MUTATORS
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

void StrictGaussianClassifier::clear()
{
  // no return - void
}

void StrictGaussianClassifier::set_mean( const Eigen::Vector2d& new_mean_vector )
{
  // set the appropriate mean vector
  mean_vector_ = new_mean_vector;

  // no return - void
}

void StrictGaussianClassifier::set_mean( const vector<DataItem>& data )
{
  // variables
  int i = 0;
  int num_data = 0;

  // reset the mean vector
  mean_vector_( 0 ) = 0;
  mean_vector_( 1 ) = 0;

  // sum the values of the features over all of the data
  for( i = 0; i < data.size(); i++ )
  {
    // case: the data is of the desired class
    if( data[i].actual_class == class_name_ )
    {
      // add the data to the sum
      mean_vector_( 0 ) += data[i].feature_vector( 0 );
      mean_vector_( 1 ) += data[i].feature_vector( 1 );
      num_data++;
    }
  }

  // scale the data
  mean_vector_( 0 ) /= num_data;
  mean_vector_( 1 ) /= num_data;

  // no return - void
}

void StrictGaussianClassifier::set_covariance(
    const Eigen::Matrix2d& new_covariance_matrix )
{
  // set the appropriate covariance matrix
  covariance_matrix_ = new_covariance_matrix;

  // update the other covariance related members
  inverse_covariance_matrix_ = covariance_matrix_.inverse();
  covariance_determinant_ = covariance_matrix_.determinant();

  // no return - void
}

void StrictGaussianClassifier::set_covariance( const vector<DataItem>& data,
                                      const Eigen::Vector2d& mean )
{
  // variables
  int i = 0;
  int num_data = 0;

  // reset the covariance matrix
  covariance_matrix_ << 0, 0,
                        0, 0;

  // sum the values of the features over all of the data
  for( num_data = 0, i = 0; i < data.size(); i++ )
  {
    // case: the data is of the desired class
    if( data[i].actual_class == class_name_ )
    {
      // add the data to the sums
      covariance_matrix_( 0, 0 ) +=
          ( data[i].feature_vector( 0 ) - mean( 0 ) ) *
          ( data[i].feature_vector( 0 ) - mean( 0 ) );
      covariance_matrix_( 1, 0 ) +=
          ( data[i].feature_vector( 1 ) - mean( 1 ) ) *
          ( data[i].feature_vector( 0 ) - mean( 0 ) );
      covariance_matrix_( 1, 1 ) +=
          ( data[i].feature_vector( 1 ) - mean( 1 ) ) *
          ( data[i].feature_vector( 1 ) - mean( 1 ) );
      num_data++;
    }
  }

  // set the covariance above the diagonal
  covariance_matrix_( 0, 1 ) = covariance_matrix_( 1, 0 );

  // scale the result
  covariance_matrix_ = ( 1.0 / ( (double) num_data - 1.0) ) *
                       covariance_matrix_;

  // update the other covariance related members
  inverse_covariance_matrix_ = covariance_matrix_.inverse();
  covariance_determinant_ = covariance_matrix_.determinant();

  // no return - void
}

void StrictGaussianClassifier::set_class_name( const string& new_name )
{
  // set the class name member
  class_name_ = new_name;
}

void StrictGaussianClassifier::set_decision_threshold(
    const double new_threshold )
{
  // set the class decision threshold member
  decision_threshold_ = new_threshold;
}

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
                   ACCESSORS
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

Eigen::Vector2d StrictGaussianClassifier::mean_vector() const
{
  // return the prior mean feature vector of the class
  return mean_vector_;
}

Eigen::Matrix2d StrictGaussianClassifier::covariance_matrix() const
{
  // return the covariance matrix of the class
  return covariance_matrix_;
}

Eigen::Matrix2d StrictGaussianClassifier::inverse_covariance_matrix() const
{
  // return the inverse of the covariance matrix of the class
  return inverse_covariance_matrix_;
}

double StrictGaussianClassifier::covariance_determinant() const
{
  // return the determinant of the covariance matrix of the class
  return covariance_determinant_;
}

string StrictGaussianClassifier::class_name() const
{
  // return the class name
  return class_name_;
}

double StrictGaussianClassifier::decision_threshold() const
{
  // return the decision threshold value
  return decision_threshold_;
}

void StrictGaussianClassifier::reportClassifierInfo()
{
  // variables
    // none

  // report the class name
  cout << "Classifier for class " << class_name_ << endl;

  // report the trained mean
  printf( "The training data mean is:\r\n" );
  cout << mean_vector_ << endl;

  // report the trained covariance
  printf( "The training data covariance is:\r\n" );
  cout << covariance_matrix_ << endl;

  // no return - void
}

bool StrictGaussianClassifier::objectIsInThisClass( Eigen::Vector2d& test_vector )
{
  // return the decision that the object is in this class
  return ( getGaussianProbability( test_vector ) > decision_threshold_ );
}

double StrictGaussianClassifier::getGaussianProbability(
    Eigen::Vector2d& test_vector )
{
  // variables
  double gaussian_probability_density = 0;
  double fractional_part = 1;
  double exponent_part = 0;
  Eigen::Vector2d test_mean_difference;
  Eigen::Vector2d intermediate_vector;

/*
    DON'T KNOW WHY, BUT THE SCALE FACTOR IS RUINING THINGS - IS DATA ALREADY
    NORMALIZED/SCALED SOMEHOW?

  // compute the normalizing/scale factor
  fractional_part = sqrt( 2 * PI );
  fractional_part = pow( fractional_part, DIMENSIONALITY );
  fractional_part *= sqrt( covariance_determinant_ );
  fractional_part = pow( fractional_part, -1 );
*/

  // compute the expontent part
  test_mean_difference = test_vector - mean_vector_;

  intermediate_vector = test_mean_difference.transpose() *
                        inverse_covariance_matrix_;
  exponent_part = -0.5 * ( intermediate_vector.dot( test_mean_difference ) );

  // compute the whole thing
  gaussian_probability_density = fractional_part * exp( exponent_part );

  // return the result
  return gaussian_probability_density;
}


/**
 * @file calc-characteristic.cpp
 *
 * @brief calculate characteristic polynomial.
 *
 * @author Mutsuo Saito (Hiroshima University)
 * @author Makoto Matsumoto (The University of Tokyo)
 *
 * Copyright (C) 2012 Mutsuo Saito, Makoto Matsumoto,
 * Hiroshima University and The University of Tokyo.
 * All rights reserved.
 *
 * The 3-clause BSD License is applied to this software, see
 * LICENSE.txt
 */
#include <mpi.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <inttypes.h>
#include <stdint.h>
#include "dSFMText.hpp"
#include "dSFMT-calc-jump.hpp"
#include <NTL/GF2X.h>
#include <NTL/vec_GF2.h>
#include <NTL/GF2XFactoring.h>

using namespace dsfmt;
using namespace NTL;
using namespace std;
static void get_lcm_sub(GF2X& lcmpoly, dSFMText& dsfmt);

void calc_minimal(GF2X& minimal, int maxdegree, w128_t outseq[], int bitpos)
{
    uint64_t mask;
    vec_GF2 seq;
    seq.SetLength(2 * maxdegree);
    int idx;
    if (bitpos >= 64) {
	idx = 1;
	mask = UINT64_C(1) << (bitpos - 64);
    } else {
	idx = 0;
	mask = UINT64_C(1) << bitpos;
    }
    for (int i = 0; i < 2 * maxdegree; i++) {
	if (outseq[i].u[idx] & mask) {
	    seq[i] = 1;
	} else {
	    seq[i] = 0;
	}
    }
    MinPolySeq(minimal, seq, maxdegree);
}

void LCM(GF2X& lcm, const GF2X& x, const GF2X& y) {
    GF2X gcd;
    mul(lcm, x, y);
    GCD(gcd, x, y);
    lcm /= gcd;
}

void get_lcm(int rank, int num_process, GF2X& lcmpoly, dSFMText& dsfmt) {
    int maxdegree = dsfmt.get_mamaxdegree();
    dsfmt.seeding(1234);
    get_lcm_sub(lcmpoly, dsfmt);
    int unit = maxdegree / num_process;
    int start = rank * num_process;
    for(int i = start; i < start + unit; i++) {
	dsfmt.init_basis(i);
	get_lcm_sub(lcmpoly, dsfmt);
    }
}

static void get_lcm_sub(GF2X& lcmpoly, dSFMText& dsfmt) {
    GF2X tmp;
    int mamaxdegree = dsfmt.get_mamaxdegree();
    w128_t out_seq[2 * mamaxdegree];
    int i, bitpos;

    for (int i = 0; i < 2 * mamaxdegree; i++) {
	out_seq[i] = dsfmt.next();
    }

    GF2X minimal;
    for (bitpos = 0; bitpos < 128; bitpos++) {
	calc_minimal(minimal, mamaxdegree, out_seq, bitpos);
	LCM(tmp, lcmpoly, minimal);
	lcmpoly = tmp;
    }
}

#if defined(IRRE_CHECK)
static int has_large_irreducible(GF2X& fpoly, int degree) {
    static const GF2X t2(2, 1);
    static const GF2X t1(1, 1);
    GF2X t2m;
    GF2X t;
    GF2X alpha;
    int m;

    t2m = t2;
    if (deg(fpoly) < degree) {
	return 0;
    }
    t = t1;
    t += t2m;

    for (m = 1; deg(fpoly) > degree; m++) {
	for(;;) {
	    GCD(alpha, fpoly, t);
	    if (IsOne(alpha)) {
		break;
	    }
	    fpoly /= alpha;
	    if (deg(fpoly) < degree) {
		return 0;
	    }
	}
	t2m *= t2m;
	t2m %= fpoly;
	add(t, t2m, t1);
    }
    if (deg(fpoly) != degree) {
	return 0;
    }
    return IterIrredTest(fpoly);
}
#endif

int main(int argc, char *argv[]) {
    int rank;
    int num_process;
    int MPI_Status status;
    MPI_Init(&argc, &argv);
    MPI_Comm_rank(MPI_COMM_WORLD, &rank);
    MPI_Comm_size(MPI_COMM_WORLD, &num_process);
    if (argc < 6) {
	cout << argv[0] << " mexp pos1 sl1 mask1 mask2" << endl;
	MPI_Finalize();
	return -1;
    }
    int mexp = strtol(argv[1], NULL, 10);
    int pos1 = strtol(argv[2], NULL, 10);
    int sl1 = strtol(argv[3], NULL, 10);
    uint64_t mask[2];
    mask[0] = strtoull(argv[4], NULL, 16);
    mask[1] = strtoull(argv[5], NULL, 16);
    stringstream ss;
    ss << "lcm." << dec << mexp << "." << rank << ".txt";
    string fname;
    ss >> fname;
    //ofstream fout("/tmp/results.txt", ios::trunc );
    ofstream fout(fname.c_str());

    dSFMText dsfmt(mexp, pos1, sl1, mask[0], mask[1]);
    GF2X characteristic(0,1);
    get_lcm(rank, num_process, characteristic, dsfmt);
    GF2X work;
    work = characteristic;
#if defined(IRRE_CHECK)
    if (!has_large_irreducible(characteristic, mexp)) {
        fout << "error? does not have large irreducible" << endl;
	MPI_Finalize();
        return -1;
    }
#endif
    string x;
    polytostring(x, work);
    fout << "#" << dec << mexp;
    fout << "," << dec << pos1;
    fout << "," << dec << sl1;
    fout << "," << hex << mask[0];
    fout << "," << hex << mask[1];
    fout << dec << endl;
    fout << x << endl;
    fout << dec << flush;
    MPI_Finalize();
}


#include <adept.h>
#include <benchmark/benchmark.h>
#include <ceres/autodiff_cost_function.h>
#include <game/vsr/cga_op.h>
#include <glog/logging.h>
#include <hep/ga.hpp>
#include <vahlen/vahlen.h>

using namespace vsr::cga;

adept::Stack g_stack;

double g_vector[3] = {1.0, 2.0, 3.0};
double g_bivector[3] = {1.0, 2.0, 3.0};
double g_motor[8] = {1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
double g_point[5] = {1.0, 2.0, 3.0, 1.0, 7.0};
double g_point_spin_motor[5];
double g_vec_ip_biv[3] = {0.0, 0.0, 0.0};

template <typename T>
void InnerProductVectorBivector(const T *vec, const T *biv, T *res) {
  Vector<T> vector(vec);
  Bivector<T> bivector(biv);
  Vector<T> result = vector <= bivector;
  for (int i = 0; i < 3; ++i)
    res[i] = result[i];
}

struct InnerProductVectorBivectorFunctor {
  template <typename T>
  bool operator()(const T *vec, const T *biv, T *res) const {
    InnerProductVectorBivector(vec, biv, res);
    return true;
  }
};

template <typename T>
void InnerProductVectorBivector2(const T *a, const T *b, T *res) {
  res[0] = -a[2] * b[1] - a[1] * b[0];
  res[1] = -a[2] * b[2] + a[0] * b[0];
  res[2] = a[1] * b[2] + a[0] * b[1];
}

struct InnerProductVectorBivectorFunctor2 {
  template <typename T>
  bool operator()(const T *vec, const T *biv, T *res) const {
    InnerProductVectorBivector2(vec, biv, res);
    return true;
  }
};

template <typename T>
void GaalopCalculateMotorSpinPoint(const T m1, const T m2, const T m3,
                                   const T m4, const T m5, const T m6,
                                   const T m7, const T m8, const T p1,
                                   const T p2, const T p3, const T p4,
                                   const T p5, T *q) {

  q[0] = ((-(2.0 * m1 * m5)) - 2.0 * m2 * m6 - 2.0 * m3 * m7 - 2.0 * m4 * m8) *
             p4 +
         (2.0 * m1 * m3 + 2.0 * m2 * m4) * p3 +
         (2.0 * m1 * m2 - 2.0 * m3 * m4) * p2 +
         (m1 * m1 - m2 * m2 - m3 * m3 + m4 * m4) * p1; // e1
  q[1] = (2.0 * m2 * m5 - 2.0 * m1 * m6 - 2.0 * m4 * m7 + 2.0 * m3 * m8) * p4 +
         (2.0 * m1 * m4 - 2.0 * m2 * m3) * p3 +
         ((m1 * m1 - m2 * m2 + m3 * m3) - m4 * m4) * p2 +
         ((-(2.0 * m1 * m2)) - 2.0 * m3 * m4) * p1; // e2
  q[2] =
      ((2.0 * m3 * m5 + 2.0 * m4 * m6) - 2.0 * m1 * m7 - 2.0 * m2 * m8) * p4 +
      ((m1 * m1 + m2 * m2) - m3 * m3 - m4 * m4) * p3 +
      ((-(2.0 * m2 * m3)) - 2.0 * m1 * m4) * p2 +
      (2.0 * m2 * m4 - 2.0 * m1 * m3) * p1;            // e3
  q[3] = (m4 * m4 + m3 * m3 + m2 * m2 + m1 * m1) * p4; // e0
  q[4] =
      (m1 * m1 + m2 * m2 + m3 * m3 + m4 * m4) * p5 +
      (2.0 * m5 * m5 + 2.0 * m6 * m6 + 2.0 * m7 * m7 + 2.0 * m8 * m8) * p4 +
      ((-(2.0 * m3 * m5)) - 2.0 * m4 * m6 - 2.0 * m1 * m7 - 2.0 * m2 * m8) *
          p3 +
      ((-(2.0 * m2 * m5)) - 2.0 * m1 * m6 + 2.0 * m4 * m7 + 2.0 * m3 * m8) *
          p2 +
      (((-(2.0 * m1 * m5)) + 2.0 * m2 * m6 + 2.0 * m3 * m7) - 2.0 * m4 * m8) *
          p1; // einf
}

template <typename T>
void GaalopMotorSpinPoint(const T *mot, const T *pnt, T *res) {
  GaalopCalculateMotorSpinPoint(mot[0], mot[1], mot[2], mot[3], mot[4], mot[5],
                                mot[6], mot[7], pnt[0], pnt[1], pnt[2], pnt[3],
                                pnt[4], res);
}

struct GaalopMotorSpinPointFunctor {
  template <typename T>
  bool operator()(const T *mot, const T *pnt, T *res) const {
    GaalopMotorSpinPoint(mot, pnt, res);
    return true;
  }
};

template <typename T>
void VahlenMotorSpinPoint(const T *mot, const T *pnt, T *res) {
  using Mat = vahlen::Matrix<T>;
  Mat motor = vahlen::Motor<T>(mot);
  Mat point = vahlen::Point<T>(pnt);
  Mat result = motor * point * vahlen::Reverse(motor);
}

struct VahlenMotorSpinPointFunctor {
  template <typename T>
  bool operator()(const T *mot, const T *pnt, T *res) const {
    VahlenMotorSpinPoint(mot, pnt, res);
    return true;
  }
};

template <typename T> void MotorSpinPoint(const T *mot, const T *pnt, T *res) {
  Motor<T> motor(mot);
  Point<T> point(pnt);
  Point<T> result = point.spin(motor);
  for (int i = 0; i < 5; ++i)
    res[i] = result[i];
}

struct MotorSpinPointFunctor {
  template <typename T>
  bool operator()(const T *mot, const T *pnt, T *res) const {
    MotorSpinPoint(mot, pnt, res);
    return true;
  }
};

template <typename T> void RotorSpinPoint(const T *rot, const T *pnt, T *res) {
  Rotor<T> rotor(rot);
  Vector<T> point(pnt);
  Vector<T> result = point.spin(rotor);
  for (int i = 0; i < 3; ++i)
    res[i] = result[i];
}

struct RotorSpinPointFunctor {
  template <typename T>
  bool operator()(const T *rot, const T *pnt, T *res) const {
    RotorSpinPoint(rot, pnt, res);
    return true;
  }
};

static void BM_InnerProductVectorBivector(benchmark::State &state) {
  while (state.KeepRunning()) {
    InnerProductVectorBivector(g_vector, g_bivector, g_vec_ip_biv);
  }
}

static void BM_InnerProductVectorBivector2(benchmark::State &state) {
  while (state.KeepRunning()) {
    InnerProductVectorBivector2(g_vector, g_bivector, g_vec_ip_biv);
  }
}

static void BM_AdeptJacobianForward(benchmark::State &state) {
  while (state.KeepRunning()) {
    double jac[9];
    adept::adouble vector[3];
    adept::set_values(vector, 3, g_vector);
    adept::adouble bivector[3];
    adept::set_values(bivector, 3, g_bivector);
    g_stack.new_recording();
    adept::adouble vec_ip_biv[3] = {0.0, 0.0, 0.0};
    InnerProductVectorBivector(vector, bivector, vec_ip_biv);

    g_stack.independent(vector, 3);
    g_stack.dependent(vec_ip_biv, 3);
    g_stack.jacobian_forward(jac);
  }
}

static void BM_AdeptJacobianForward2(benchmark::State &state) {
  while (state.KeepRunning()) {
    double jac[9];
    adept::adouble vector[3];
    adept::set_values(vector, 3, g_vector);
    adept::adouble bivector[3];
    adept::set_values(bivector, 3, g_bivector);
    g_stack.new_recording();
    adept::adouble vec_ip_biv[3] = {0.0, 0.0, 0.0};
    InnerProductVectorBivector2(vector, bivector, vec_ip_biv);

    g_stack.independent(vector, 3);
    g_stack.dependent(vec_ip_biv, 3);
    g_stack.jacobian_forward(jac);
  }
}

static void BM_AdeptJacobianReverse(benchmark::State &state) {
  while (state.KeepRunning()) {
    double jac[9];
    adept::adouble vector[3];
    adept::set_values(vector, 3, g_vector);
    adept::adouble bivector[3];
    adept::set_values(bivector, 3, g_bivector);
    g_stack.new_recording();
    adept::adouble vec_ip_biv[3] = {0.0, 0.0, 0.0};
    InnerProductVectorBivector(vector, bivector, vec_ip_biv);

    g_stack.independent(vector, 3);
    g_stack.dependent(vec_ip_biv, 3);
    g_stack.jacobian_reverse(jac);
  }
}

static void BM_AdeptJacobianReverse2(benchmark::State &state) {
  while (state.KeepRunning()) {
    double jac[9];
    adept::adouble vector[3];
    adept::set_values(vector, 3, g_vector);
    adept::adouble bivector[3];
    adept::set_values(bivector, 3, g_bivector);
    g_stack.new_recording();
    adept::adouble vec_ip_biv[3] = {0.0, 0.0, 0.0};
    InnerProductVectorBivector2(vector, bivector, vec_ip_biv);

    g_stack.independent(vector, 3);
    g_stack.dependent(vec_ip_biv, 3);
    g_stack.jacobian_reverse(jac);
  }
}

static void BM_CeresJacobian(benchmark::State &state) {
  while (state.KeepRunning()) {
    double jac[9];
    double vector[3] = {1.0, 2.0, 3.0};
    double bivector[3] = {1.0, 2.0, 3.0};
    double vec_ip_biv[3] = {0.0, 0.0, 0.0};
    const double *parameters[2] = {&vector[0], &bivector[0]};
    double *jacobians[2] = {jac, nullptr};

    ceres::AutoDiffCostFunction<InnerProductVectorBivectorFunctor, 3, 3, 3>(
        new InnerProductVectorBivectorFunctor())
        .Evaluate(parameters, &vec_ip_biv[0], jacobians);
  }
}

static void BM_CeresJacobian2(benchmark::State &state) {
  while (state.KeepRunning()) {
    double jac[9];
    double vector[3] = {1.0, 2.0, 3.0};
    double bivector[3] = {1.0, 2.0, 3.0};
    double vec_ip_biv[3] = {0.0, 0.0, 0.0};
    const double *parameters[2] = {&vector[0], &bivector[0]};
    double *jacobians[2] = {jac, nullptr};

    ceres::AutoDiffCostFunction<InnerProductVectorBivectorFunctor2, 3, 3, 3>(
        new InnerProductVectorBivectorFunctor2())
        .Evaluate(parameters, &vec_ip_biv[0], jacobians);
  }
}

static void BM_MotorSpinPoint(benchmark::State &state) {
  while (state.KeepRunning()) {
    MotorSpinPoint(g_motor, g_point, g_point_spin_motor);
  }
}

static void BM_VahlenMotorSpinPoint(benchmark::State &state) {
  while (state.KeepRunning()) {
    VahlenMotorSpinPoint(g_motor, g_point, g_point_spin_motor);
  }
}

static void BM_GaalopMotorSpinPoint(benchmark::State &state) {
  while (state.KeepRunning()) {
    GaalopMotorSpinPoint(g_motor, g_point, g_point_spin_motor);
  }
}

static void BM_AdeptMotorSpinPointJacobianReverse(benchmark::State &state) {
  while (state.KeepRunning()) {
    double jac[5 * 8];
    adept::adouble motor[8];
    adept::set_values(motor, 8, g_motor);
    adept::adouble point[5];
    adept::set_values(point, 5, g_point);
    g_stack.new_recording();
    adept::adouble res[5] = {0.0, 0.0, 0.0, 0.0, 0.0};
    MotorSpinPoint(motor, point, res);

    g_stack.independent(motor, 8);
    g_stack.dependent(res, 5);
    g_stack.jacobian_reverse(jac);
  }
}

static void BM_AdeptMotorSpinPointJacobianForward(benchmark::State &state) {
  while (state.KeepRunning()) {
    double jac[5 * 8];
    adept::adouble motor[8];
    adept::set_values(motor, 8, g_motor);
    adept::adouble point[5];
    adept::set_values(point, 5, g_point);
    g_stack.new_recording();
    adept::adouble res[5] = {0.0, 0.0, 0.0, 0.0, 0.0};
    MotorSpinPoint(motor, point, res);

    g_stack.independent(motor, 8);
    g_stack.dependent(res, 5);
    g_stack.jacobian_forward(jac);
  }
}

static void BM_AdeptRotorSpinPointJacobianForward(benchmark::State &state) {
  while (state.KeepRunning()) {
    double jac[3 * 4];
    adept::adouble motor[4];
    adept::set_values(motor, 4, g_motor);
    adept::adouble point[3];
    adept::set_values(point, 3, g_point);
    g_stack.new_recording();
    adept::adouble res[3] = {0.0, 0.0, 0.0};
    RotorSpinPoint(motor, point, res);

    g_stack.independent(motor, 4);
    g_stack.dependent(res, 3);
    g_stack.jacobian_reverse(jac, true);
  }
}

static void BM_CeresMotorSpinPointJacobian(benchmark::State &state) {
  while (state.KeepRunning()) {
    double jac[5 * 8];
    const double *parameters[2] = {&g_motor[0], &g_point[0]};
    double *jacobians[2] = {jac, nullptr};

    ceres::AutoDiffCostFunction<MotorSpinPointFunctor, 5, 8, 5>(
        new MotorSpinPointFunctor())
        .Evaluate(parameters, &g_point_spin_motor[0], jacobians);
  }
}

static void BM_CeresGaalopMotorSpinPointJacobian(benchmark::State &state) {
  while (state.KeepRunning()) {
    double jac[5 * 8];
    const double *parameters[2] = {&g_motor[0], &g_point[0]};
    double *jacobians[2] = {jac, nullptr};

    ceres::AutoDiffCostFunction<GaalopMotorSpinPointFunctor, 5, 8, 5>(
        new GaalopMotorSpinPointFunctor())
        .Evaluate(parameters, &g_point_spin_motor[0], jacobians);
  }
}

static void BM_CeresRotorSpinPointJacobian(benchmark::State &state) {
  double jac[3 * 4];
  const double *parameters[2] = {&g_motor[0], &g_point[0]};
  double *jacobians[2] = {jac, nullptr};
  while (state.KeepRunning()) {
    ceres::AutoDiffCostFunction<RotorSpinPointFunctor, 3, 4, 3>(
        new RotorSpinPointFunctor())
        .Evaluate(parameters, &g_point_spin_motor[0], jacobians);
  }
}

template <typename T> using Matrix4 = Eigen::Matrix<T, 4, 4>;

template <typename T> inline static Matrix4<T> s() {
  Matrix4<T> m;
  m << T(1), T(0), T(0), T(0), T(0), T(1), T(0), T(0), T(0), T(0), T(1), T(0),
      T(0), T(0), T(0), T(1);
  return m;
}

template <typename T> inline static Matrix4<T> e1() {
  Matrix4<T> m;
  m << T(0), T(0), T(0), T(1), T(0), T(0), T(1), T(0), T(0), T(1), T(0), T(0),
      T(1), T(0), T(0), T(0);
  return m;
}

template <typename T> inline static Matrix4<T> e2() {
  Matrix4<T> m;
  m << T(0), T(0), T(1), T(0), T(0), T(0), T(0), T(-1), T(1), T(0), T(0), T(0),
      T(0), T(-1), T(0), T(0);
  return m;
}

template <typename T> inline static Matrix4<T> e3() {
  Matrix4<T> m;
  m << T(1), T(0), T(0), T(0), T(0), T(1), T(0), T(0), T(0), T(0), T(-1), T(0),
      T(0), T(0), T(0), T(-1);
  return m;
}

struct DiffRotorMatrixFunctor {
  template <typename T> bool operator()(const T *th, const T *a, T *b) const {
    Matrix4<T> rotor =
        cos(T(0.5) * th[0]) * s<T>() - sin(T(0.5) * th[0]) * e1<T>() * e2<T>();
    Matrix4<T> rotor_inv =
        cos(T(0.5) * th[0]) * s<T>() + sin(T(0.5) * th[0]) * e1<T>() * e2<T>();
    Matrix4<T> vec_a = a[0] * e1<T>() + a[1] * e2<T>() + a[2] * e3<T>();
    Matrix4<T> vec_b = rotor * vec_a * rotor_inv;
    b[0] = vec_b(0, 3); // e1
    b[1] = vec_b(0, 2); // e2
    b[2] = vec_b(0, 0); // e3
    return true;
  }
};

static void BM_CeresRotorMatrixJacobian(benchmark::State &state) {
  double theta = 0.5;
  double jac[3];
  const double *parameters[2] = {&theta, &g_point[0]};
  double *jacobians[2] = {jac, nullptr};
  while (state.KeepRunning()) {
    ceres::AutoDiffCostFunction<DiffRotorMatrixFunctor, 3, 1, 3>(
        new DiffRotorMatrixFunctor())
        .Evaluate(parameters, &g_point_spin_motor[0], jacobians);
  }
}

struct DiffRotorVersorFunctor {
  template <typename T> bool operator()(const T *th, const T *a, T *b) const {
    Rotor<T> rotor{cos(T(0.5) * th[0]), -sin(T(0.5) * th[0]), T(0.0), T(0.0)};
    Vector<T> vec_a{a[0], a[1], a[2]};
    Vector<T> vec_b = vec_a.spin(rotor);
    for (int i = 0; i < 3; ++i)
      b[i] = vec_b[i];
    return true;
  }
};

static void BM_CeresRotorVersorJacobian(benchmark::State &state) {
  double theta = 0.5;
  double jac[3];
  const double *parameters[2] = {&theta, &g_point[0]};
  double *jacobians[2] = {jac, nullptr};
  while (state.KeepRunning()) {
    ceres::AutoDiffCostFunction<DiffRotorVersorFunctor, 3, 1, 3>(
        new DiffRotorVersorFunctor())
        .Evaluate(parameters, &g_point_spin_motor[0], jacobians);
  }
}

struct DiffRotorHepGAFunctor {
  template <typename T> bool operator()(const T *th, const T *a, T *b) const {
    using Algebra = hep::algebra<T, 3, 0>;
    using Rotor = hep::multi_vector<Algebra, hep::list<0, 3, 5, 6>>;
    using Vector = hep::multi_vector<Algebra, hep::list<1, 2, 4>>;
    Rotor rotor{cos(T(0.5) * th[0]), -sin(T(0.5) * th[0]), T(0.0), T(0.0)};
    Vector pnt_a{a[0], a[1], a[2]};
    Vector pnt_b = hep::grade<1>(rotor * pnt_a * ~rotor);
    for (int i = 0; i < 3; ++i)
      b[i] = pnt_b[i];
    return true;
  }
};

static void BM_CeresRotorHepGAJacobian(benchmark::State &state) {
  double theta = 0.5;
  double jac[3];
  const double *parameters[2] = {&theta, &g_point[0]};
  double *jacobians[2] = {jac, nullptr};
  while (state.KeepRunning()) {
    ceres::AutoDiffCostFunction<DiffRotorHepGAFunctor, 3, 1, 3>(
        new DiffRotorHepGAFunctor())
        .Evaluate(parameters, &g_point_spin_motor[0], jacobians);
  }
}

static void BM_AdeptRotorHepGAJacobianForward(benchmark::State &state) {
  double jac[3];
  adept::adouble theta{0.5};
  adept::adouble point[3];
  adept::set_values(point, 3, g_point);
  while (state.KeepRunning()) {
    g_stack.new_recording();
    adept::adouble res[3] = {0.0, 0.0, 0.0};
    DiffRotorHepGAFunctor()(&theta, &point[0], &res[0]);
    g_stack.independent(&theta, 1);
    g_stack.dependent(res, 3);
    g_stack.jacobian_forward(jac, true);
  }
}

static void BM_AdeptRotorHepGAJacobianReverse(benchmark::State &state) {
  double jac[3];
  adept::adouble theta{0.5};
  adept::adouble point[3];
  adept::set_values(point, 3, g_point);
  while (state.KeepRunning()) {
    g_stack.new_recording();
    adept::adouble res[3] = {0.0, 0.0, 0.0};
    DiffRotorHepGAFunctor()(&theta, &point[0], &res[0]);
    g_stack.independent(&theta, 1);
    g_stack.dependent(res, 3);
    g_stack.jacobian_reverse(jac, true);
  }
}

static void BM_AdeptRotorVersorJacobianForward(benchmark::State &state) {
  double jac[3];
  adept::adouble theta{0.5};
  adept::adouble point[3];
  adept::set_values(point, 3, g_point);
  while (state.KeepRunning()) {
    g_stack.new_recording();
    adept::adouble res[3] = {0.0, 0.0, 0.0};
    DiffRotorVersorFunctor()(&theta, &point[0], &res[0]);
    g_stack.independent(&theta, 1);
    g_stack.dependent(res, 3);
    g_stack.jacobian_forward(jac, true);
  }
}

static void BM_AdeptRotorVersorJacobianReverse(benchmark::State &state) {
  double jac[3];
  adept::adouble theta{0.5};
  adept::adouble point[3];
  adept::set_values(point, 3, g_point);
  while (state.KeepRunning()) {
    g_stack.new_recording();
    adept::adouble res[3] = {0.0, 0.0, 0.0};
    DiffRotorVersorFunctor()(&theta, &point[0], &res[0]);
    g_stack.independent(&theta, 1);
    g_stack.dependent(res, 3);
    g_stack.jacobian_reverse(jac, true);
  }
}

static void BM_AdeptRotorMatrixJacobianForward(benchmark::State &state) {
  double jac[3];
  adept::adouble theta{0.5};
  adept::adouble point[3];
  adept::set_values(point, 3, g_point);
  while (state.KeepRunning()) {
    g_stack.new_recording();
    adept::adouble res[3] = {0.0, 0.0, 0.0};
    DiffRotorMatrixFunctor()(&theta, &point[0], &res[0]);
    g_stack.independent(&theta, 1);
    g_stack.dependent(res, 3);
    g_stack.jacobian_forward(jac, true);
  }
}

static void BM_AdeptRotorMatrixJacobianReverse(benchmark::State &state) {
  double jac[3];
  adept::adouble theta{0.5};
  adept::adouble point[3];
  adept::set_values(point, 3, g_point);
  while (state.KeepRunning()) {
    g_stack.new_recording();
    adept::adouble res[3] = {0.0, 0.0, 0.0};
    DiffRotorMatrixFunctor()(&theta, &point[0], &res[0]);
    g_stack.independent(&theta, 1);
    g_stack.dependent(res, 3);
    g_stack.jacobian_reverse(jac, true);
  }
}

struct DiffRotorHandFunctor {
  template <typename T> bool operator()(const T *th, const T *a, T *b) const {
    T st = sin(th[0] / 2.0);
    T ct = cos(th[0] / 2.0);
    T stst = st * st;
    T ctct = ct * ct;
    T ctst = ct * st;
    b[0] = (-(a[0] * stst)) - 2.0 * a[1] * ctst + a[0] * ctct; // e1
    b[1] = (-(a[1] * stst)) + 2.0 * a[0] * ctst + a[1] * ctct; // e2
    b[2] = a[2] * stst + a[2] * ctct;                          // e3
    return true;
  }
};

struct DiffRotorGaalopFunctor {
  template <typename T> bool operator()(const T *th, const T *a, T *b) const {
    b[0] = (-(a[0] * sin(th[0] / 2.0) * sin(th[0] / 2.0))) -
           2.0 * a[1] * cos(th[0] / 2.0) * sin(th[0] / 2.0) +
           a[0] * cos(th[0] / 2.0) * cos(th[0] / 2.0); // e1
    b[1] = (-(a[1] * sin(th[0] / 2.0) * sin(th[0] / 2.0))) +
           2.0 * a[0] * cos(th[0] / 2.0) * sin(th[0] / 2.0) +
           a[1] * cos(th[0] / 2.0) * cos(th[0] / 2.0); // e2
    b[2] = a[2] * sin(th[0] / 2.0) * sin(th[0] / 2.0) +
           a[2] * cos(th[0] / 2.0) * cos(th[0] / 2.0); // e3
    return true;
  }
};

static void BM_AdeptRotorGaalopJacobianReverse(benchmark::State &state) {
  double jac[3];
  adept::adouble theta{0.5};
  adept::adouble point[3];
  adept::set_values(point, 3, g_point);
  while (state.KeepRunning()) {
    g_stack.new_recording();
    adept::adouble res[3] = {0.0, 0.0, 0.0};
    DiffRotorGaalopFunctor()(&theta, &point[0], &res[0]);
    g_stack.independent(&theta, 1);
    g_stack.dependent(res, 3);
    g_stack.jacobian_reverse(jac, true);
  }
}

static void BM_RotorMatrix(benchmark::State &state) {
  double theta{0.5};
  double point[3] = {1.0, 2.0, 3.0};
  double res[3] = {0.0, 0.0, 0.0};
  while (state.KeepRunning()) {
    DiffRotorHandFunctor()(&theta, &point[0], &res[0]);
  }
}
static void BM_RotorHand(benchmark::State &state) {
  double theta{0.5};
  double point[3] = {1.0, 2.0, 3.0};
  double res[3] = {0.0, 0.0, 0.0};
  while (state.KeepRunning()) {
    DiffRotorHandFunctor()(&theta, &point[0], &res[0]);
  }
}
static void BM_RotorGaalop(benchmark::State &state) {
  double theta{0.5};
  double point[3] = {1.0, 2.0, 3.0};
  double res[3] = {0.0, 0.0, 0.0};
  while (state.KeepRunning()) {
    DiffRotorGaalopFunctor()(&theta, &point[0], &res[0]);
  }
}

static void BM_RotorVersor(benchmark::State &state) {
  double theta{0.5};
  double point[3] = {1.0, 2.0, 3.0};
  double res[3] = {0.0, 0.0, 0.0};
  while (state.KeepRunning()) {
    DiffRotorVersorFunctor()(&theta, &point[0], &res[0]);
  }
}

static void BM_RotorHepGA(benchmark::State &state) {
  double theta{0.5};
  double point[3] = {1.0, 2.0, 3.0};
  double res[3] = {0.0, 0.0, 0.0};
  while (state.KeepRunning()) {
    DiffRotorHepGAFunctor()(&theta, &point[0], &res[0]);
  }
}

static void BM_AdeptRotorGaalopJacobianForward(benchmark::State &state) {
  double jac[3];
  adept::adouble theta{0.5};
  adept::adouble point[3];
  adept::set_values(point, 3, g_point);
  while (state.KeepRunning()) {
    g_stack.new_recording();
    adept::adouble res[3] = {0.0, 0.0, 0.0};
    DiffRotorGaalopFunctor()(&theta, &point[0], &res[0]);
    g_stack.independent(&theta, 1);
    g_stack.dependent(res, 3);
    g_stack.jacobian_forward(jac, true);
  }
}

static void BM_AdeptRotorHandJacobianReverse(benchmark::State &state) {
  double jac[3];
  adept::adouble theta{0.5};
  adept::adouble point[3];
  adept::set_values(point, 3, g_point);
  while (state.KeepRunning()) {
    g_stack.new_recording();
    adept::adouble res[3] = {0.0, 0.0, 0.0};
    DiffRotorHandFunctor()(&theta, &point[0], &res[0]);
    g_stack.independent(&theta, 1);
    g_stack.dependent(res, 3);
    g_stack.jacobian_reverse(jac, true);
  }
}

static void BM_AdeptRotorHandJacobianForward(benchmark::State &state) {
  double jac[3];
  adept::adouble theta{0.5};
  adept::adouble point[3];
  adept::set_values(point, 3, g_point);
  while (state.KeepRunning()) {
    g_stack.new_recording();
    adept::adouble res[3] = {0.0, 0.0, 0.0};
    DiffRotorHandFunctor()(&theta, &point[0], &res[0]);
    g_stack.set_max_jacobian_threads(3);
    g_stack.independent(&theta, 1);
    g_stack.dependent(res, 3);
    // g_stack.jacobian_forward_openmp(jac, true);
    g_stack.jacobian_forward(jac, true);
  }
}

static void BM_CeresRotorHandJacobian(benchmark::State &state) {
  double theta = 0.5;
  double jac[3];
  const double *parameters[2] = {&theta, &g_point[0]};
  double *jacobians[2] = {jac, nullptr};
  while (state.KeepRunning()) {
    ceres::AutoDiffCostFunction<DiffRotorHandFunctor, 3, 1, 3>(
        new DiffRotorHandFunctor())
        .Evaluate(parameters, &g_point_spin_motor[0], jacobians);
  }
}
static void BM_CeresRotorGaalopJacobian(benchmark::State &state) {
  double theta = 0.5;
  double jac[3];
  const double *parameters[2] = {&theta, &g_point[0]};
  double *jacobians[2] = {jac, nullptr};
  while (state.KeepRunning()) {
    ceres::AutoDiffCostFunction<DiffRotorGaalopFunctor, 3, 1, 3>(
        new DiffRotorGaalopFunctor())
        .Evaluate(parameters, &g_point_spin_motor[0], jacobians);
  }
}

BENCHMARK(BM_InnerProductVectorBivector);
BENCHMARK(BM_AdeptJacobianForward);
BENCHMARK(BM_AdeptJacobianReverse);
BENCHMARK(BM_CeresJacobian);
BENCHMARK(BM_InnerProductVectorBivector2);
BENCHMARK(BM_AdeptJacobianForward2);
BENCHMARK(BM_AdeptJacobianReverse2);
BENCHMARK(BM_CeresJacobian2);
BENCHMARK(BM_MotorSpinPoint);
BENCHMARK(BM_VahlenMotorSpinPoint);
BENCHMARK(BM_GaalopMotorSpinPoint);
BENCHMARK(BM_AdeptMotorSpinPointJacobianForward);
BENCHMARK(BM_AdeptMotorSpinPointJacobianReverse);
BENCHMARK(BM_CeresGaalopMotorSpinPointJacobian);
BENCHMARK(BM_CeresMotorSpinPointJacobian);
BENCHMARK(BM_CeresRotorSpinPointJacobian);
BENCHMARK(BM_AdeptRotorSpinPointJacobianForward);

// AMDO paper
BENCHMARK(BM_CeresRotorMatrixJacobian);
BENCHMARK(BM_CeresRotorVersorJacobian);
BENCHMARK(BM_CeresRotorHepGAJacobian);
BENCHMARK(BM_CeresRotorGaalopJacobian);
BENCHMARK(BM_CeresRotorHandJacobian);
BENCHMARK(BM_AdeptRotorMatrixJacobianForward);
BENCHMARK(BM_AdeptRotorMatrixJacobianReverse);
BENCHMARK(BM_AdeptRotorVersorJacobianForward);
BENCHMARK(BM_AdeptRotorVersorJacobianReverse);
BENCHMARK(BM_AdeptRotorHepGAJacobianForward);
BENCHMARK(BM_AdeptRotorHepGAJacobianReverse);
BENCHMARK(BM_AdeptRotorGaalopJacobianReverse);
BENCHMARK(BM_AdeptRotorGaalopJacobianForward);
BENCHMARK(BM_AdeptRotorHandJacobianReverse);
BENCHMARK(BM_AdeptRotorHandJacobianForward);

BENCHMARK(BM_RotorHand);
BENCHMARK(BM_RotorGaalop);
BENCHMARK(BM_RotorVersor);
BENCHMARK(BM_RotorMatrix);
BENCHMARK(BM_RotorHepGA);

BENCHMARK_MAIN()


// This file is part of the dune-xt project:
//   https://github.com/dune-community/dune-xt
// Copyright 2009-2018 dune-xt developers and contributors. All rights reserved.
// License: Dual licensed as BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)
//      or  GPL-2.0+ (http://opensource.org/licenses/gpl-license)
//          with "runtime exception" (http://www.dune-project.org/license.html)
// Authors:
//   René Fritze    (2018 - 2019)
//   Tobias Leibner (2018)

#ifndef DUNE_XT_DATA_COORDINATES_HH
#define DUNE_XT_DATA_COORDINATES_HH

#include <boost/geometry.hpp>

#include <dune/common/fvector.hh>

namespace Dune::XT::Data {

/** Converts from (x, y, z) to (theta, phi) on the unit sphere s.t.
 * (x, y, z) = (sin(theta) cos(phi), sin(theta) sin(phi), cos(theta))
 * with 0 \leq \theta \leq \pi and 0 \leq \varphi < 2\pi. **/
template <class DomainFieldType>
class CoordinateConverter
{
  using BoostCartesianCoordType =
      typename boost::geometry::model::point<DomainFieldType, 3, typename boost::geometry::cs::cartesian>;
  using BoostSphericalCoordType = typename boost::geometry::model::
      point<DomainFieldType, 2, typename boost::geometry::cs::spherical<boost::geometry::radian>>;

public:
  using CartesianCoordType = FieldVector<DomainFieldType, 3>;
  using SphericalCoordType = FieldVector<DomainFieldType, 2>;

  static SphericalCoordType to_spherical(const CartesianCoordType& x)
  {
    BoostCartesianCoordType x_boost(x[0], x[1], x[2]);
    BoostSphericalCoordType x_spherical_boost;
    boost::geometry::transform(x_boost, x_spherical_boost);
    return SphericalCoordType{boost::geometry::get<1>(x_spherical_boost), boost::geometry::get<0>(x_spherical_boost)};
  }

  static CartesianCoordType to_cartesian(const SphericalCoordType& x_spherical, bool first_is_cosine = false)
  {
    // if first_is_cosine, the first coordinate is not theta but rather cos(theta)
    if (first_is_cosine) {
      const auto& mu = x_spherical[0];
      const auto& phi = x_spherical[1];
      return CartesianCoordType{
          std::sqrt(1 - std::pow(mu, 2)) * std::cos(phi), std::sqrt(1 - std::pow(mu, 2)) * std::sin(phi), mu};
    }
    BoostSphericalCoordType x_spherical_boost(x_spherical[1], x_spherical[0]);
    BoostCartesianCoordType x_boost;
    boost::geometry::transform(x_spherical_boost, x_boost);
    return CartesianCoordType{
        boost::geometry::get<0>(x_boost), boost::geometry::get<1>(x_boost), boost::geometry::get<2>(x_boost)};
  }
};

} // namespace Dune::XT::Data

#endif // DUNE_XT_DATA_COORDINATES_HH


/*
 *  Copyright (c) 2009, Rene Wagner
 *  All rights reserved.
 *
 *  Author: Rene Wagner <rw@nelianur.org>
 *
 *  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 Rene Wagner nor the names of any
 *     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 OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef __UKFOM_LAPACK_CHOLESKY_HPP__
#define __UKFOM_LAPACK_CHOLESKY_HPP__

#include "lapack.h"

#include <Eigen/Core>

namespace ukfom {
namespace lapack {
using namespace Eigen;

template<size_t M>
class cholesky
{
public:
	cholesky(const Matrix<double, M, M> &m)
	{
		L_ = m;

		char UPLO = 'L';
		int N = L_.cols();
		int LDA = L_.stride();
		int INFO;

		dpotrf_(&UPLO, &N, L_.data(), &LDA, &INFO);

		spd_ = INFO == 0;

		// clear everything but the lower triangular matrix
		for (int j = 1; j < L_.cols(); ++j)
			for (int i = 0; i < j; ++i)
				L_(i,j) = 0;
	}

	const Matrix<double, M, M> &getL() const
	{
		if (!spd_)
			throw "not SPD";
		return L_;
	}

	bool isSPD() const
	{
		return spd_;
	}

private:
	Matrix<double, M, M> L_;
	bool spd_;
};

} // namespace lapack
} // namespace ukfom

#endif // __UKFOM_LAPACK_CHOLESKY_HPP__


/**
 * \file      rigid-body-kinematics.hpp
 * \author    Mehdi Benallegue
 * \date       2013
 * \brief      Implements integrators for the kinematics, in terms or rotations
 *             and translations.
 *
 * \details
 *
 *
 */

#ifndef StATEOBSERVATIONRIGIDBODYKINEMATICS_H
#define StATEOBSERVATIONRIGIDBODYKINEMATICS_H

#include <Eigen/SVD>

#include <state-observation/api.h>
#include <state-observation/tools/definitions.hpp>
#include <state-observation/tools/miscellaneous-algorithms.hpp>
#include <state-observation/tools/probability-law-simulation.hpp>

namespace stateObservation
{
namespace kine
{
inline void integrateKinematics(Vector3 & position, const Vector3 & velocity, double dt);

inline void integrateKinematics(Vector3 & position, Vector3 & velocity, const Vector3 & acceleration, double dt);

inline void integrateKinematics(Matrix3 & orientation, const Vector3 & rotationVelocity, double dt);

inline void integrateKinematics(Matrix3 & orientation,
                                Vector3 & rotationVelocity,
                                const Vector3 & rotationAcceleration,
                                double dt);

inline void integrateKinematics(Quaternion & orientation, const Vector3 & rotationVelocity, double dt);

inline void integrateKinematics(Quaternion & orientation,
                                Vector3 & rotationVelocity,
                                const Vector3 & rotationAcceleration,
                                double dt);

/// integrates the position/orientation and their time derivatives, given the
/// accelerations, and initial velocities and positions. The rotations are
/// expressed by rotation matrix
inline void integrateKinematics(Vector3 & position,
                                Vector3 & velocity,
                                const Vector3 & acceleration,
                                Matrix3 & orientation,
                                Vector3 & rotationVelocity,
                                const Vector3 & rotationAcceleration,
                                double dt);

/// integrates the position/orientation and their time derivatives, given the
/// accelerations, and initial velocities and positions. The orientations are
/// expressed by quaternions
inline void integrateKinematics(Vector3 & position,
                                Vector3 & velocity,
                                const Vector3 & acceleration,
                                Quaternion & orientation,
                                Vector3 & rotationVelocity,
                                const Vector3 & rotationAcceleration,
                                double dt);

/// integrates the postition/orientation given the velocities
inline void integrateKinematics(Vector3 & position,
                                const Vector3 & velocity,
                                Matrix3 & orientation,
                                const Vector3 & rotationVelocity,
                                double dt);

/// integrates the postition/orientation given the velocities
inline void integrateKinematics(Vector3 & position,
                                const Vector3 & velocity,
                                Quaternion & orientation,
                                const Vector3 & rotationVelocity,
                                double dt);

/// Puts the orientation vector norm between 0 and Pi if it
/// gets close to 2pi
inline Vector regulateRotationVector(const Vector3 & v);

/// Transform the rotation vector into angle axis
inline AngleAxis rotationVectorToAngleAxis(const Vector3 & v);

/// Tranbsform the rotation vector into rotation matrix
inline Matrix3 rotationVectorToRotationMatrix(const Vector3 & v);

/// Tranbsform the rotation vector into quaternion
inline Quaternion rotationVectorToQuaternion(const Vector3 & v);

/// Tranbsform the rotation matrix into rotation vector
inline Vector3 rotationMatrixToRotationVector(const Matrix3 & R);

/// Tranbsform a quaternion into rotation vector
inline Vector3 quaternionToRotationVector(const Quaternion & q);

/// Tranbsform a quaternion into rotation vector
inline Vector3 quaternionToRotationVector(const Vector4 & v);

/// scalar component of a quaternion
inline double scalarComponent(const Quaternion & q);

/// vector part of the quaternion
inline Vector3 vectorComponent(const Quaternion & q);

/// Transform the rotation matrix into roll pitch yaw
///(decompose R into Ry*Rp*Rr)
inline Vector3 rotationMatrixToRollPitchYaw(const Matrix3 & R, Vector3 & v);

inline Vector3 rotationMatrixToRollPitchYaw(const Matrix3 & R);

/// Transform the roll pitch yaw into rotation matrix
///( R = Ry*Rp*Rr)
inline Matrix3 rollPitchYawToRotationMatrix(double roll, double pitch, double yaw);

inline Matrix3 rollPitchYawToRotationMatrix(const Vector3 & rpy);

/// Transform the roll pitch yaw into rotation matrix
///( R = Ry*Rp*Rr)
inline Quaternion rollPitchYawToQuaternion(double roll, double pitch, double yaw);

inline Quaternion rollPitchYawToQuaternion(const Vector3 & rpy);

/// Projects the Matrix to so(3)
inline Matrix3 orthogonalizeRotationMatrix(const Matrix3 & M);

/// transform a 3d vector into a skew symmetric 3x3 matrix
inline Matrix3 skewSymmetric(const Vector3 & v, Matrix3 & R);

/// transform a 3d vector into a skew symmetric 3x3 matrix
inline Matrix3 skewSymmetric(const Vector3 & v);

/// transform a 3d vector into a squared skew symmetric 3x3 matrix
inline Matrix3 skewSymmetric2(const Vector3 & v, Matrix3 & R);

/// transform a 3d vector into a squared skew symmetric 3x3 matrix
inline Matrix3 skewSymmetric2(const Vector3 & v);

/// transforms a homogeneous matrix into 6d vector (position theta mu)
inline Vector6 homogeneousMatrixToVector6(const Matrix4 & M);

/// transforms a 6d vector (position theta mu) into a homogeneous matrix
inline Matrix4 vector6ToHomogeneousMatrix(const Vector6 & v);

/// @brief Builds the smallest angle matrix allowing to get from a NORMALIZED vector v1 to its imahe Rv1
/// This is based on Rodrigues formula
///
/// @param v1 the NORMALIZED vector
/// @param Rv1 the NORMALIZED image of this vector by the rotation matrix R
/// @return Matrix3 the rotation matrix R
inline Matrix3 twoVectorsToRotationMatrix(const Vector3 & v1, const Vector3 Rv1);

/// @brief checks if this matrix is a pure yaw matrix or not
///
/// @param R the rotation matrix
/// @return true is pure yaw
/// @return false is not pure yaw
inline bool isPureYaw(const Matrix3 & R);

/// @brief Gets a vector that remains horizontal with this rotation. This vector is NOT normalized
/// @details There is a general version in getInvariantOrthogonalVector(). This can be used to extract yaw angle from
/// a rotation matrix without needing to specify an order in the tils (e.g. roll then pich).
///
/// @param R the input rotation
/// @return Vector3 the output horizontal vector
inline Vector3 getInvariantHorizontalVector(const Matrix3 & R);

/// @brief Gets a vector \f$v\f$ that is orthogonal to \f$e_z\f$ and such that \f$\hat{R}^T e_z\f$ is orthogonal to
/// the tilt \f$R^T e_z\f$. This vector is NOT normalized.
/// @details This is a generalization of getInvariantHorizontalVector() which corresponds to no tilt
/// \f$\hat{R}^T e_z=e_z\f$. This function is useful to merge the yaw from the rotation matrix with the tilt.
///
/// @param Rhat the input rotation matrix \f$\hat{R}^T\f$
/// @param Rtez the input tilt \f$\hat{R}^T e_z\f$
/// @return Vector3 the output horizontal vector
inline Vector3 getInvariantOrthogonalVector(const Matrix3 & Rhat, const Vector3 & Rtez);

/// @brief Merge the roll and pitch from the tilt (R^T e_z) with the yaw from a rotation matrix (minimizes the
/// deviation of the v vector)
/// @details throws exception when the orientation is singlular (likely gimbal lock)
/// to avoid these issues, we recommend to use mergeTiltWithYawAxisAgnostic()
/// @param Rtez the tilt \f$R_1^T e_z\f$ (the local image of \f$e_z\f$ unit vector)
/// @param R2 is the second rotation matrix from which the "yaw" needs to be extracted
/// @param v is the vector to use as reference it must be horizontal and normalized (for a traditional yaw v is by
/// deftault \f$e_x\f$)
/// @return Matrix3 the merged rotation matrix
inline Matrix3 mergeTiltWithYaw(const Vector3 & Rtez,
                                const Matrix3 & R2,
                                const Vector3 & v = Vector3::UnitX()) noexcept(false);

/// @brief Merge the roll and pitch with the yaw from a rotation matrix (minimizes the deviation of the v vector)
///
/// @param R1 is the first rotation to get the roll and pitch
/// @param R2 is the second rotation matrix from which the "yaw" needs to be extracted
/// @param v is the vector to use as reference (for a traditional yaw v is initialized to \f$e_x\f$)
/// @return Matrix3 the merged rotation matrix
inline Matrix3 mergeRoll1Pitch1WithYaw2(const Matrix3 & R1, const Matrix3 & R2, const Vector3 & v = Vector3::UnitX());

/// @brief Merge the roll and pitch from the tilt (R^T e_z) with the yaw from a rotation matrix (minimizes the deviation
/// of the v vector)
/// @param Rtez the tilt \f$R_1^T e_z\f$ (the local image of \f$e_z\f$ unit vector)
/// @param R2 is the second rotation matrix from which the "yaw" needs to be extracted
/// @param v is the vector to use as reference (for a traditional yaw v is initialized to \f$e_x\f$)
/// @return Matrix3 the merged rotation matrix
inline Matrix3 mergeTiltWithYawAxisAgnostic(const Vector3 & Rtez, const Matrix3 & R2);

/// @brief Merge the roll and pitch with the yaw from a rotation matrix with optimal reference vector
///
/// @param R1 is the first rotation to get the roll and pitch
/// @param R2 is the second rotation matrix from which the "yaw" needs to be extracted
/// @param v is the vector to use as reference (for a traditional yaw v is initialized to \f$e_x\f$)
/// @return Matrix3 the merged rotation matrix
inline Matrix3 mergeRoll1Pitch1WithYaw2AxisAgnostic(const Matrix3 & R1, const Matrix3 & R2);

/// @brief take 3x3 matrix represeting a rotation and gives the angle that vector v turns around the axis with this
/// rotation
/// @param rotation The 3x3 rotation matrix
/// @param axis the axis of rotation (must be normalized)
/// @param v the vector that is rotated with the rotation (must be orthogonal to axis and normalized)
/// @return double the angle
inline double rotationMatrixToAngle(const Matrix3 & rotation, const Vector3 & axis, const Vector3 & v);

/// @brief take 3x3 matrix represeting a rotation and gives the angle that vector v turns around the upward vertical
/// axis with this rotation
/// @details this is a generalization of yaw extraction (yaw is equivalent to v = Matrix3::UnitX(), but it is more
/// efficiant to calll the dedicated  rotationMatrixToYaw() without vector parameter).
/// @param rotation The 3x3 rotation matrix
/// @param v the rotated vector (expressed in the horizontal plane, must be normalized)
/// @return double the angle
inline double rotationMatrixToYaw(const Matrix3 & rotation, const Vector2 & v);

/// @brief take 3x3 matrix represeting a rotation and gives the yaw angle from roll pitch yaw representation
/// @param rotation The 3x3 rotation matrix
/// @return double the angle
inline double rotationMatrixToYaw(const Matrix3 & rotation);

/// @brief take 3x3 matrix represeting a rotation and gives a corresponding angle around upward vertical axis
/// @details This is similar to yaw angle but here we identify a horizontal vector that stays horizontal after rotation.
/// this can be called axis agnostic yaw extraction.
/// and get the angle between them
/// @param rotation The 3x3 rotation matrix
/// @return double the angle
inline double rotationMatrixToYawAxisAgnostic(const Matrix3 & rotation);

/// @brief Get the Identity Quaternion
///
/// @return Quaternion
inline Quaternion zeroRotationQuaternion();

/// @brief Get a uniformly random Quaternion
///
/// @return Quaternion
inline Quaternion randomRotationQuaternion();

/// @brief get a randomAngle between -pi and pu
///
/// @return double the random angle
inline double randomAngle();

/// @brief Checks if it is a rotation matrix (right-hand orthonormal) or not
/// @param precision the absolute precision of the test
/// @return true when it is a rotation matrix
/// @return false when not
inline bool isRotationMatrix(const Matrix3 &, double precision = 2 * cst::epsilon1);

/// transforms a rotation into translation given a constraint of a fixed point
inline void fixedPointRotationToTranslation(const Matrix3 & R,
                                            const Vector3 & rotationVelocity,
                                            const Vector3 & rotationAcceleration,
                                            const Vector3 & fixedPoint,
                                            Vector3 & outputTranslation,
                                            Vector3 & outputLinearVelocity,
                                            Vector3 & outputLinearAcceleration);

/// derivates a quaternion using finite difference to get a angular velocity vector
inline Vector3 derivateRotationFD(const Quaternion & q1, const Quaternion & q2, double dt);

/// derivates a rotation vector using finite difference to get a angular velocity vector
inline Vector3 derivateRotationFD(const Vector3 & o1, const Vector3 & o2, double dt);

inline Vector6 derivateHomogeneousMatrixFD(const Matrix4 & m1, const Matrix4 & m2, double dt);

inline Vector6 derivatePoseThetaUFD(const Vector6 & v1, const Vector6 & v2, double dt);

/// Computes the "multiplicative Jacobian" for
/// Kalman filtering for example
/// orientation is the current orientation
/// dR is the rotation delta between the current orientation and the orientation
/// at the next step.
/// dRdR is the "multiplicative" Jacobian with regard to variations of orientation
/// dRddeltaR is the "multiplicative" Jacobian with regard to variations of deltaR
inline void derivateRotationMultiplicative(const Vector3 & deltaR, Matrix3 & dRdR, Matrix3 & dRddeltaR);

/// Computes the "multiplicative Jacobian" for
/// a function R^T.v giving a vector v expressed in a local frame
/// with regard to Rotations of this local frame
inline Matrix3 derivateRtvMultiplicative(const Matrix3 & R, const Vector3 & v);

/// uses the derivation to reconstruct the velocities and accelerations given
/// trajectories in positions and orientations only
inline IndexedVectorArray reconstructStateTrajectory(const IndexedVectorArray & positionOrientation, double dt);

inline Vector invertState(const Vector & state);

inline Matrix4 invertHomoMatrix(const Matrix4 & m);

enum rotationType
{
  matrix = 0,
  rotationVector = 1,
  quaternion = 2,
  angleaxis = 3
};

template<rotationType = rotationVector>
struct indexes
{
};

template<>
struct indexes<rotationVector>
{
  /// indexes of the different components of a vector of the kinematic state
  /// when the orientation is represented using a 3D rotation vector
  static const unsigned pos = 0;
  static const unsigned ori = 3;
  static const unsigned linVel = 6;
  static const unsigned angVel = 9;
  static const unsigned linAcc = 12;
  static const unsigned angAcc = 15;
  static const unsigned size = 18;
};

template<>
struct indexes<quaternion>
{
  /// indexes of the different components of a vector of the kinematic state
  /// when the orientation is represented using a quaternion
  static const unsigned pos = 0;
  static const unsigned ori = 3;
  static const unsigned linVel = 7;
  static const unsigned angVel = 10;
  static const unsigned linAcc = 13;
  static const unsigned angAcc = 16;
  static const unsigned size = 19;
};

/// relative tolereance to the square of quaternion norm.
constexpr double quatNormTol = 1e-6;

class Orientation
{
public:
  /// The parameter initialize should be set to true except when it is
  /// certain that the initial value will not be used
  /// And that the first operation would be to set its value
  explicit Orientation(bool initialize = true);

  /// this is the rotation vector and NOT Euler angles
  explicit Orientation(const Vector3 & v);

  explicit Orientation(const Quaternion & q);

  explicit Orientation(const Matrix3 & m);

  explicit Orientation(const AngleAxis & aa);

  Orientation(const Quaternion & q, const Matrix3 & m);

  Orientation(const double & roll, const double & pitch, const double & yaw);

  Orientation(const Orientation & multiplier1, const Orientation & multiplier2);

  inline Orientation & operator=(const Vector3 & v);

  inline Orientation & operator=(const Quaternion & q);

  inline Orientation & operator=(const Matrix3 & m);

  inline Orientation & operator=(const AngleAxis & aa);

  inline Orientation & setValue(const Quaternion & q, const Matrix3 & m);

  inline Orientation & fromVector4(const Vector4 & v);

  inline Orientation & setRandom();

  template<typename t>
  inline Orientation & setZeroRotation();

  inline Orientation & setZeroRotation();

  /// get a const reference on the matrix or the quaternion
  inline const Matrix3 & toMatrix3() const;
  inline const Quaternion & toQuaternion() const;

  inline operator const Matrix3 &() const;
  inline operator const Quaternion &() const;

  inline Vector4 toVector4() const;

  inline Vector3 toRotationVector() const;
  inline Vector3 toRollPitchYaw() const;
  inline AngleAxis toAngleAxis() const;

  /// Multiply the rotation (orientation) by another rotation R2
  /// the non const versions allow to use more optimized methods

  inline Orientation operator*(const Orientation & R2) const;

  /// Noalias versions of the operator*
  inline const Orientation & setToProductNoAlias(const Orientation & R1, const Orientation & R2);

  inline Orientation inverse() const;

  /// use the vector dt_x_omega as the increment of rotation expressed in the
  /// world frame. Which gives R_{k+1}=\exp(S(dtxomega))R_k
  inline const Orientation & integrate(Vector3 dt_x_omega);

  /// gives the log (rotation vector) of the difference of orientation
  /// gives log of (*this).inverse()*R_k1
  inline Vector3 differentiate(Orientation R_k1) const;

  /// Rotate a vector
  inline Vector3 operator*(const Vector3 & v) const;

  inline bool isSet() const;
  inline void reset();

  inline bool isMatrixSet() const;
  inline bool isQuaternionSet() const;

  /// switch the state of the Matrix or quaternion to set or not
  /// this can be used for forward initialization
  inline void setMatrix(bool b = true);
  inline void setQuaternion(bool b = true);

  /// no checks are performed for these functions, use with caution

  inline CheckedMatrix3 & getMatrixRefUnsafe();
  inline CheckedQuaternion & getQuaternionRefUnsafe();

  /// synchronizes the representations (quaternion and rotation matrix)
  inline void synchronize();

  /// retruns a zero rotation
  static inline Orientation zeroRotation();

  /// Returns a uniformly distributed random rotation
  static inline Orientation randomRotation();

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private:
  void check_() const;

  inline const Matrix3 & quaternionToMatrix_() const;
  inline const Quaternion & matrixToQuaternion_() const;

  mutable CheckedQuaternion q_;
  mutable CheckedMatrix3 m_;
};

struct Kinematics
{
  struct Flags
  {
    typedef unsigned char Byte;

    static const Byte position = BOOST_BINARY(000001);
    static const Byte orientation = BOOST_BINARY(000010);
    static const Byte linVel = BOOST_BINARY(000100);
    static const Byte angVel = BOOST_BINARY(001000);
    static const Byte linAcc = BOOST_BINARY(010000);
    static const Byte angAcc = BOOST_BINARY(100000);

    static const Byte all = position | orientation | linVel | angVel | linAcc | angAcc;
  };

  Kinematics() {}

  /// Constructor from a vector
  /// the flags show which parts of the kinematics to be loaded from the vector
  /// the order of the vector is
  /// position orientation (quaternion) linevel angvel linAcc angAcc
  /// use the flags to define the structure of the vector
  Kinematics(const Vector & v, Flags::Byte = Flags::all);

  Kinematics(const Kinematics & multiplier1, const Kinematics & multiplier2);

  /// Fills from vector
  /// the flags show which parts of the kinematics to be loaded from the vector
  /// the order of the vector is
  /// position orientation (quaternion) linevel angvel linAcc angAcc
  /// use the flags to define the structure of the vector
  Kinematics & fromVector(const Vector & v, Flags::Byte = Flags::all);

  /// initializes at zero all the flagged fields
  /// the typename allows to set if the prefered type for rotation
  /// is a Matrix3 or a Quaternion (Quaternion by default)
  template<typename t>
  Kinematics & setZero(Flags::Byte = Flags::all);

  Kinematics & setZero(Flags::Byte = Flags::all);

  inline const Kinematics & integrate(double dt);

  inline const Kinematics & update(const Kinematics & newValue, double dt, Flags::Byte = Flags::all);

  inline Kinematics getInverse() const;

  /// converts the object to a vector
  /// the order of the vector is
  /// position orientation (quaternion) linevel angvel linAcc angAcc
  /// use the flags to define the structure of the vector
  inline Vector toVector(Flags::Byte) const;
  inline Vector toVector() const;

  /// composition of transformation
  inline Kinematics operator*(const Kinematics &)const;

  inline Kinematics setToProductNoAlias(const Kinematics & operand1, const Kinematics & operand2);

  inline void reset();

  CheckedVector3 position;
  Orientation orientation;

  CheckedVector3 linVel;
  CheckedVector3 angVel;

  CheckedVector3 linAcc;
  CheckedVector3 angAcc;

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

protected:
  inline const Kinematics & update_deprecated(const Kinematics & newValue, double dt, Flags::Byte = Flags::all);

  Vector3 tempVec_;
};

} // namespace kine
} // namespace stateObservation

inline std::ostream & operator<<(std::ostream & os, const stateObservation::kine::Kinematics & k);

#include <state-observation/tools/rigid-body-kinematics.hxx>

#endif // StATEOBSERVATIONRIGIDBODYKINEMATICS_H


// Copyright Abel Sinkovics (abel@sinkovics.hu)  2011.
// 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 <boost/config.hpp>

#ifdef BOOST_NO_CXX11_CONSTEXPR
#include <iostream>

int main()
{
  std::cout << "Please use a compiler that supports constexpr" << std::endl;
}
#else

#define BOOST_MPL_LIMIT_STRING_SIZE 64
#define BOOST_METAPARSE_LIMIT_STRING_SIZE BOOST_MPL_LIMIT_STRING_SIZE

#include <boost/metaparse/grammar.hpp>
#include <boost/metaparse/entire_input.hpp>
#include <boost/metaparse/build_parser.hpp>
#include <boost/metaparse/token.hpp>
#include <boost/metaparse/string.hpp>
#include <boost/metaparse/util/digit_to_int.hpp>

#include <boost/mpl/apply_wrap.hpp>
#include <boost/mpl/fold.hpp>
#include <boost/mpl/front.hpp>
#include <boost/mpl/back.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/times.hpp>
#include <boost/mpl/divides.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/eval_if.hpp>
#include <boost/mpl/lambda.hpp>
#include <boost/mpl/char.hpp>
#include <boost/mpl/int.hpp>

using boost::metaparse::build_parser;
using boost::metaparse::entire_input;
using boost::metaparse::token;
using boost::metaparse::grammar;

using boost::metaparse::util::digit_to_int;

using boost::mpl::apply_wrap1;
using boost::mpl::fold;
using boost::mpl::front;
using boost::mpl::back;
using boost::mpl::plus;
using boost::mpl::minus;
using boost::mpl::times;
using boost::mpl::divides;
using boost::mpl::eval_if;
using boost::mpl::equal_to;
using boost::mpl::_1;
using boost::mpl::_2;
using boost::mpl::char_;
using boost::mpl::lambda;
using boost::mpl::int_;

#ifdef _STR
  #error _STR already defined
#endif
#define _STR BOOST_METAPARSE_STRING

template <class A, class B>
struct lazy_plus : plus<typename A::type, typename B::type> {};

template <class A, class B>
struct lazy_minus : minus<typename A::type, typename B::type> {};

template <class A, class B>
struct lazy_times : times<typename A::type, typename B::type> {};

template <class A, class B>
struct lazy_divides : divides<typename A::type, typename B::type> {};

template <class C, class T, class F>
struct lazy_eval_if : eval_if<typename C::type, T, F> {};

template <class A, class B>
struct lazy_equal_to : equal_to<typename A::type, typename B::type> {};

template <class Sequence, class State, class ForwardOp>
struct lazy_fold :
  fold<typename Sequence::type, typename State::type, typename ForwardOp::type>
{};

typedef
  lazy_fold<
    back<_1>,
    front<_1>,
    lambda<
      lazy_eval_if<
        lazy_equal_to<front<_2>, char_<'*'>>,
        lazy_times<_1, back<_2>>,
        lazy_divides<_1, back<_2>>
      >
    >::type
  >
  prod_action;

typedef
  lazy_fold<
    back<_1>,
    front<_1>,
    lambda<
      lazy_eval_if<
        lazy_equal_to<front<_2>, char_<'+'>>,
        lazy_plus<_1, back<_2>>,
        lazy_minus<_1, back<_2>>
      >
    >::type
  >
  plus_action;

typedef
  lambda<
    lazy_fold<
      _1,
      int_<0>,
      lambda<
        lazy_plus<lazy_times<_1, int_<10>>, apply_wrap1<digit_to_int<>, _2>>
      >::type
    >
  >::type
  int_action;

typedef
  grammar<_STR("plus_exp")>

    ::rule<_STR("int ::= ('0'|'1'|'2'|'3'|'4'|'5'|'6'|'7'|'8'|'9')+"), int_action>::type
    ::rule<_STR("ws ::= (' ' | '\n' | '\r' | '\t')*")>::type
    ::rule<_STR("int_token ::= int ws"), front<_1>>::type
    ::rule<_STR("plus_token ::= '+' ws"), front<_1>>::type
    ::rule<_STR("minus_token ::= '-' ws"), front<_1>>::type
    ::rule<_STR("mult_token ::= '*' ws"), front<_1>>::type
    ::rule<_STR("div_token ::= '/' ws"), front<_1>>::type
    ::rule<_STR("plus_token ::= '+' ws")>::type
    ::rule<_STR("plus_exp ::= prod_exp ((plus_token | minus_token) prod_exp)*"), plus_action>::type
    ::rule<_STR("prod_exp ::= int_token ((mult_token | div_token) int_token)*"), prod_action>::type
  expression;

typedef build_parser<entire_input<expression>> calculator_parser;

int main()
{
  using std::cout;
  using std::endl;

  cout
    << apply_wrap1<calculator_parser, _STR("13")>::type::value << endl
    << apply_wrap1<calculator_parser, _STR("1+ 2*4-6/2")>::type::value << endl
    ;
}
#endif


// smooth_feedback: Control theory on Lie groups
// https://github.com/pettni/smooth_feedback
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include <boost/numeric/odeint.hpp>
#include <smooth/compat/odeint.hpp>
#include <smooth/feedback/mpc.hpp>

#include <chrono>

#ifdef ENABLE_PLOTTING
#include <matplot/matplot.h>
#endif

using namespace std::chrono_literals;
using namespace boost::numeric::odeint;

using Time = std::chrono::duration<double>;

template<typename T>
using X = Eigen::Vector2<T>;
template<typename T>
using U = Eigen::Matrix<T, 1, 1>;

using Gd = X<double>;
using Ud = U<double>;

int main()
{
  using std::sin;
  std::srand(5);

  // system variables
  Gd g = Gd::Random();
  Ud u;

  // dynamics
  auto f = []<typename S>(const X<S> & x, const U<S> u) -> smooth::Tangent<X<S>> {
    return {x(1), u(0)};
  };

  // running constraints
  auto cr = []<typename S>(const X<S> &, const U<S> & u) -> Eigen::Vector<S, 1> { return u; };
  Eigen::Vector<double, 1> crl{-0.5}, cru{0.5};

  // create MPC object and set input bounds, and desired trajectories
  smooth::feedback::MPC<Time, Gd, Ud, decltype(f), decltype(cr)> mpc{
    f,
    cr,
    crl,
    cru,
    {
      .K  = 20,
      .tf = 5,
      .qp = {.scaling = false, .polish = false},
    },
  };

  mpc.set_weights({
    .Q   = Eigen::Matrix2d::Identity(),
    .Qtf = 0.1 * Eigen::Matrix2d::Identity(),
    .R   = 0.1 * Eigen::Matrix<double, 1, 1>::Identity(),
  });
  mpc.set_xdes_rel([]<typename T>(T t) -> X<T> { return X<T>{-0.5 * sin(0.3 * t), 0}; });
  mpc.set_udes_rel([]<typename T>(T) -> U<T> { return U<T>::Zero(); });

  // prepare for integrating the closed-loop system
  runge_kutta4<Gd, double, smooth::Tangent<Gd>, double, vector_space_algebra> stepper{};
  const auto ode = [&f, &u](const Gd & x, smooth::Tangent<Gd> & d, double) { d = f(x, u); };
  std::vector<double> tvec, xvec, vvec, uvec;

  // integrate closed-loop system
  const auto t0 = std::chrono::high_resolution_clock::now();

  for (std::chrono::milliseconds t = 0s; t < 60s; t += 50ms) {
    // compute MPC input
    auto [u_mpc, code] = mpc(t, g);
    u                  = u_mpc;
    if (code != smooth::feedback::QPSolutionStatus::Optimal) {
      std::cerr << "Solver failed with code " << static_cast<int>(code) << std::endl;
    }

    // store data
    tvec.push_back(duration_cast<Time>(t).count());
    xvec.push_back(g.x());
    vvec.push_back(g.y());
    uvec.push_back(u(0));

    // step dynamics
    stepper.do_step(ode, g, 0, 0.05);
  }

  const auto tf = std::chrono::high_resolution_clock::now();

  std::cout << "MPC loop time: "
            << std::chrono::duration_cast<std::chrono::microseconds>(tf - t0).count() << "us\n";

#if ENABLE_PLOTTING
  matplot::figure();
  matplot::hold(matplot::on);

  matplot::plot(tvec, xvec)->line_width(2);
  matplot::plot(tvec, matplot::transform(tvec, [](auto t) { return -0.5 * sin(0.3 * t); }), "k--")
    ->line_width(2);
  matplot::plot(tvec, vvec)->line_width(2);
  matplot::plot(tvec, uvec)->line_width(2);
  matplot::legend({"x", "x_{des}", "v", "u"});

  matplot::show();
#else
  std::cout << "TRAJECTORY:" << std::endl;
  for (auto i = 0u; i != tvec.size(); ++i) {
    std::cout << "t=" << tvec[i] << ": x=" << xvec[i] << ", v=" << vvec[i] << std::endl;
  }
#endif
}


#pragma once

#include "Faddeeva/Faddeeva.hh"
#include "util.hpp"
#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <iostream>
#include <memory>
#include <random>

#include <fstream>
#include <sstream>

namespace myFM {
template <typename Real> struct OprobitSampler {

  using DenseVector = types::Vector<Real>;
  using DenseMatrix = types::DenseMatrix<Real>;
  using IntVector = Eigen::Matrix<int, Eigen::Dynamic, 1>;
  static constexpr Real SQRT2 = 1.4142135623730951;
  static constexpr Real SQRTPI = 1.7724538509055159;
  static constexpr Real SQRT2PI = SQRT2 * SQRTPI;
  static constexpr Real PI = 3.141592653589793;

  OprobitSampler(DenseVector &x, const DenseVector &y, int K,
                 const std::vector<size_t> &indices, std::mt19937 &rng,
                 Real reg, Real nu)
      : x_(x), y_(y), K(K), indices_(indices), reg(reg), nu(nu), rng(rng),
        zmins(K), zmaxs(K), histogram(K), accept_count(0) {
    this->alpha_now = DenseVector::Zero(K - 1);
    this->gamma_now = DenseVector::Zero(K - 1);
    this->alpha_to_gamma(gamma_now, alpha_now);
    this->H = DenseMatrix::Zero(K - 1, K - 1);
    for (auto i : indices_) {
      int y_label = static_cast<int>(y_(i));
      if (std::abs(y_label - y(i)) > 1e-3) {
        throw std::invalid_argument("y has a floating-point element.");
      }
      if (y_label < 0) {
        throw std::invalid_argument("y has a negative element.");
      }
      if (y_label >= K) {
        std::stringstream ss;
        ss << "y[ " << i << "] is greater than " << (K - 1) << ".";
        throw std::invalid_argument(ss.str());
      }
      histogram[y_label]++;
    }
  }

  inline Real log_p_mvt(const DenseMatrix &SigmaInverse, const DenseVector mu,
                        Real nu, const DenseVector &x) {
    Real log_p = (x - mu).transpose() * SigmaInverse * (x - mu);
    return std::log(1 + log_p / nu) * (-nu - SigmaInverse.rows()) / 2;
  }

  inline DenseVector sample_mvt(const DenseMatrix &SigmaInverse, Real nu) {
    /*Sample From multivariate t-distribution*/
    DenseVector result(SigmaInverse.rows());
    std::normal_distribution<Real> base_dist(0, 1);
    std::gamma_distribution<Real> chi_gen(nu / 2);
    for (int i = 0; i < result.rows(); i++) {
      result(i) = base_dist(rng);
    }
    Eigen::LLT<DenseMatrix, Eigen::Upper> L(SigmaInverse);
    result = L.matrixU().solve(result);
    result /= std::sqrt(chi_gen(rng) * 2 / nu);
    if (fix_gamma0) {
      result(0) = 0;
    }
    return result;
  }

  static inline void jacobian_dgamma_dalpha(DenseMatrix &J,
                                            const DenseVector &alpha) {
    /*
    J_{ij} with i=> alpha, j=>gamma
    */
    J.array() = 0;
    J(0, 0) = 1;
    if (!fix_gamma0) {
      for (int j = 1; j < alpha.rows(); j++) {
        J(0, j) = 1;
      }
    }
    for (int i = 1; i < alpha.rows(); i++) {
      Real ed = std::exp(alpha(i));
      for (int j = i; j < alpha.rows(); j++) {
        J(i, j) = ed;
      }
    }
    // d f / d alpha_0 = (df / d gamma_i) (d gamma_i / d alpha_0 )
  }

  static inline void alpha_to_gamma(DenseVector &target,
                                    const DenseVector &alpha) {
    target(0) = alpha(0);
    for (int i = 1; i < alpha.rows(); i++) {
      target(i) = target(i - 1) + std::exp(alpha(i));
    }
  }

  static inline void gamma_to_alpha(DenseVector &target,
                                    const DenseVector &gamma) {
    target(0) = gamma(0);
    for (int i = 1; i < gamma.rows(); i++) {
      target(i) = std::log(gamma(i) - gamma(i - 1));
    }
  }

  static inline void safe_ldiff(Real x, Real y, Real &loss, Real &dx, Real &dy,
                                DenseMatrix *HessianTarget = nullptr,
                                int label = 0) {
    // assert(x >= y);
    Real denominator;
    Real exp_factor;
    if (y > 0) {
      // both positive
      // erfcy = erfc * exp( y**2 / 2)
      exp_factor = std::exp((y * y - x * x) / 2);
      denominator =
          Faddeeva::erfcx(y / SQRT2) - exp_factor * Faddeeva::erfcx(x / SQRT2);

      loss -= y * y / 2;
      loss += std::log(denominator / 2);
      dx += (2 / SQRT2PI) * exp_factor / denominator;
      dy -= (2 / SQRT2PI) / denominator;
      if (HessianTarget != nullptr) {
        (*HessianTarget)(label, label) +=
            -(SQRT2PI * x * denominator * std::exp((y * y - x * x) / 2) +
              2 * std::exp(y * y - x * x)) /
            denominator / denominator / PI;
        (*HessianTarget)(label - 1, label - 1) +=
            (SQRT2PI * y * denominator - 2) / denominator / denominator / PI;
        Real off_diag =
            2 * std::exp((y * y - x * x) / 2) / PI / denominator / denominator;
        (*HessianTarget)(label, label - 1) += off_diag;
        (*HessianTarget)(label - 1, label) += off_diag;
      }
    } else if (x < 0) {
      // both negative
      loss -= x * x / 2;

      exp_factor = std::exp((x * x - y * y) / 2);
      denominator = Faddeeva::erfcx(-x / SQRT2) -
                    exp_factor * Faddeeva::erfcx(-y / SQRT2);
      loss += std::log(denominator / 2);
      dx += (2 / SQRT2PI) / denominator;
      dy -= (2 / SQRT2PI) * exp_factor / denominator;
      if (HessianTarget != nullptr) {
        (*HessianTarget)(label, label) +=
            -(SQRT2PI * x * denominator + 2) / PI / denominator / denominator;
        (*HessianTarget)(label - 1, label - 1) +=
            (SQRT2PI * y * exp_factor * denominator -
             2 * (exp_factor * exp_factor)) /
            PI / denominator / denominator;
        Real off_diag = 2 * exp_factor / PI / denominator / denominator;
        (*HessianTarget)(label, label - 1) += off_diag;
        (*HessianTarget)(label - 1, label) += off_diag;
      }
    } else {
      // x positive, y negative. safe to use erf
      denominator = Faddeeva::erf(x / SQRT2) - Faddeeva::erf(y / SQRT2);
      Real expxx = std::exp(-x * x / 2);
      Real expyy = std::exp(-y * y / 2);
      dx += 2 * expxx / denominator / SQRT2PI;
      dy -= 2 * expyy / denominator / SQRT2PI;
      loss += std::log(denominator / 2);
      if (HessianTarget != nullptr) {
        (*HessianTarget)(label, label) +=
            -(SQRT2PI * x * denominator * expxx + 2 * expxx * expxx) / PI /
            denominator / denominator;
        (*HessianTarget)(label - 1, label - 1) +=
            -(-SQRT2PI * y * denominator * expyy + 2 * expyy * expyy) / PI /
            denominator / denominator;
        Real off_diag = 2 * expxx * expyy / PI / denominator / denominator;
        (*HessianTarget)(label, label - 1) += off_diag;
        (*HessianTarget)(label - 1, label) += off_diag;
      }
    }
  }

  static inline void safe_lcdf(Real x, Real &loss, Real &dx,
                               DenseMatrix *HessianTarget = nullptr,
                               int label = 0) {
    Real denominator;
    Real exp_factor;
    if (x > 1) {
      exp_factor = std::exp(-x * x / 2);
      denominator = 1 + Faddeeva::erf(x / SQRT2);
      dx += (2 / SQRT2PI) * exp_factor / denominator;
      loss += std::log(denominator / 2);
      if (HessianTarget != nullptr) {
        (*HessianTarget)(label, label) +=
            -(SQRT2PI * x * denominator * exp_factor +
              2 * exp_factor * exp_factor) /
            PI / denominator / denominator;
      }
    } else {
      denominator = Faddeeva::erfcx(-x / SQRT2);
      dx += (2 / SQRT2PI) / denominator;
      loss -= x * x / 2;
      loss += std::log(denominator / 2);
      if (HessianTarget != nullptr) {
        (*HessianTarget)(label, label) +=
            -(SQRT2PI * x * denominator + 2) / PI / denominator / denominator;
      }
    }
  }

  inline void safe_lccdf(Real x, Real &loss, Real &dx,
                         DenseMatrix *HessianTarget, int label = 0) {
    Real denominator;
    if (x > -1) {
      denominator = Faddeeva::erfcx(x / SQRT2);
      dx -= (2 / SQRT2PI) / denominator;
      loss += std::log(denominator / 2);
      loss -= x * x / 2;
      if (HessianTarget != nullptr) {
        (*HessianTarget)(label - 1, label - 1) +=
            (SQRT2PI * x * denominator - 2) / denominator / denominator / PI;
      }
    } else {
      // safe to use erf
      denominator = 1 - Faddeeva::erf(x / SQRT2);
      dx -= (2 / SQRT2PI) * std::exp(-x * x / 2) / denominator;
      loss += std::log(denominator / 2);
      if (HessianTarget != nullptr) {
        Real exp_factor = std::exp(-(x * x) / 2);
        (*HessianTarget)(label - 1, label - 1) +=
            -(-SQRT2PI * x * denominator * exp_factor +
              2 * exp_factor * exp_factor) /
            PI / denominator / denominator;
      }
    }
  }

  inline void sample_z_given_cutpoint() {
    zmins.array() = std::numeric_limits<Real>::max();
    zmaxs.array() = std::numeric_limits<Real>::lowest();
    Real deviation = 1;

    for (int train_data_index : indices_) {
      int class_index = static_cast<int>(y_(train_data_index));
      Real pred_score = x_(train_data_index);
      Real z_new;

      if (class_index == 0) {
        z_new = deviation * sample_truncated_normal_right(
                                rng, (gamma_now(class_index) - pred_score) /
                                         deviation) +
                pred_score;
        zmaxs(0) = std::max(zmaxs(0), z_new);
      } else if (class_index == (K - 1)) {
        z_new =
            deviation * sample_truncated_normal_left(
                            rng, (gamma_now(K - 2) - pred_score) / deviation) +
            pred_score;
        zmins(K - 1) = std::min(zmins(K - 1), z_new);
      } else {
        z_new =
            deviation *
                sample_truncated_normal_twoside(
                    rng, (gamma_now(class_index - 1) - pred_score) / deviation,
                    (gamma_now(class_index) - pred_score) / deviation) +
            pred_score;
        zmins(class_index) = std::min(zmins(class_index), z_new);
        zmaxs(class_index) = std::max(zmaxs(class_index), z_new);
      }
      x_(train_data_index) -= z_new;
    }
  }

  inline void start_sample() {
    DenseVector alpha_hat = DenseVector::Zero(K - 1);
    find_minimum(alpha_hat);
    alpha_now = alpha_hat;
    alpha_to_gamma(gamma_now, alpha_now);
  }

  inline void sample_cutpoint_given_z() {
    for (int i = 1; i <= (K - 3); i++) {
      Real lower = zmaxs(i);
      Real upper = zmins(i + 1);
      gamma_now(i) = std::uniform_real_distribution<Real>(lower, upper)(rng);
    }
  }

  inline void find_minimum(DenseVector &alpha_hat, bool verbose = false) {
    int max_iter = 10000;
    Real epsilon = 1e-5;
    Real epsilon_rel = 1e-5;
    Real delta = 1e-5;
    int past = 3;
    DenseVector history(past);
    DenseVector alpha_new(alpha_hat);
    DenseVector dalpha(alpha_hat);
    DenseVector direction(alpha_hat);
    Real ll_current;
    bool first = true;
    int i = 0;
    while (true) {
      if (first) {
        ll_current = (*this)(alpha_hat, dalpha, &H);
        if (verbose) {
          print_to_stream(std::cout, "ll_current = ", ll_current,
                          "\ndalpha = ", dalpha);
          std::cout << std::endl;
        }
      }
      {

        Real alpha2 = alpha_hat.norm();
        Real dalpha2 = dalpha.norm();
        if (verbose) {
          print_to_stream(std::cout, "ll = ", ll_current,
                          "\nalpha_hat =", alpha_hat);
          std::cout << std::endl;

          print_to_stream(std::cout, "dalpha2 = ", dalpha2);
          std::cout << std::endl;
        }

        if (dalpha2 < epsilon || dalpha2 < epsilon_rel * alpha2) {
          break;
        }
      }

      direction = -H.llt().solve(dalpha);
      if (verbose) {
        print_to_stream(std::cout, "H = ", H);
        std::cout << std::endl;

        print_to_stream(std::cout, "direction = ", direction);
        std::cout << std::endl;
      }

      Real step_size = 1;
      int lsc = 0;
      while (true) {
        alpha_new = alpha_hat + step_size * direction;
        Real ll_new;
        try {
          ll_new = (*this)(alpha_new, dalpha, &H);
        } catch (std::runtime_error) {
          step_size /= 2;
          continue;
        }

        if (ll_new >= (ll_current * (1 + delta))) {
          step_size /= 2;
        } else {
          alpha_hat = alpha_new;
          ll_current = ll_new;
          break;
        }
        if (++lsc > 1000)
          break;
      }
      first = false;
      if (i >= past) {
        Real past_loss = history(i % past);
        if (std::abs(past_loss - ll_current) <=
            delta *
                std::max(std::max(abs(ll_current), abs(past_loss)), Real(1))) {
          break;
        }
      }
      history(i % past) = ll_current;
      i++;
      if (i >= max_iter)
        break;
    }
    if (i == max_iter) {
      throw std::runtime_error("Failed to converge. See fail-log.txt");
    }
  }

  inline bool step(bool verbose = false) {
    DenseVector alpha_hat = alpha_now;
    DenseVector gamma(alpha_hat);
    find_minimum(alpha_hat, verbose);
    DenseVector alpha_candidate = sample_mvt(H, nu) + alpha_hat;

    Real ll_candidate, ll_old;
    try {
      ll_candidate = -(*this)(alpha_candidate, gamma);
      ll_old = -(*this)(alpha_now, gamma);
    } catch (std::runtime_error e) {
      // should be NaN encounter
      return false;
    }
    Real log_p_transition_candidate =
        log_p_mvt(H, alpha_hat, nu, alpha_candidate);
    Real log_p_transition_old = log_p_mvt(H, alpha_hat, nu, alpha_now);
    Real test_ratio = std::exp(ll_candidate - log_p_transition_candidate -
                               ll_old + log_p_transition_old);
    Real u = std::uniform_real_distribution<Real>{0, 1}(rng);
    if (u < test_ratio) {
      alpha_now = alpha_candidate;
      alpha_to_gamma(gamma_now, alpha_now);
      accept_count++;
      return true;
    } else {
      return false;
    }
  }

  inline Real operator()(const DenseVector &alpha, DenseVector &dalpha,
                         DenseMatrix *HessianTarget = nullptr) {
    DenseVector gamma = DenseVector::Zero(alpha.rows());
    dalpha.array() = 0;
    alpha_to_gamma(gamma, alpha);

    DenseMatrix dGammadAlpha = DenseMatrix(alpha.rows(), alpha.rows());
    jacobian_dgamma_dalpha(dGammadAlpha, alpha);
    Real ll = 0;
    if (HessianTarget != nullptr) {

      (*HessianTarget).array() = 0;
    }
    for (auto i : indices_) {
      int label = y_(i);
      if (label == 0) {
        safe_lcdf(gamma(0) - x_(i), ll, dalpha(0), HessianTarget, label);
      } else if (label == (K - 1)) {
        safe_lccdf(gamma(K - 2) - x_(i), ll, dalpha(K - 2), HessianTarget,
                   label);
      } else {
        safe_ldiff(gamma(label) - x_(i), gamma(label - 1) - x_(i), ll,
                   dalpha(label), dalpha(label - 1), HessianTarget, label);
      }
    }

    if (HessianTarget != nullptr) {
      DenseMatrix &H = (*HessianTarget);
      DenseVector expAlpha(alpha.array().exp().matrix());
      H = dGammadAlpha * H * dGammadAlpha.transpose();
      {
        // m = 0
        // gamma 0 = alpha_0 does not contribute
        // since \partial^2 gamma_0 / \partial alpha_i \partial alpha_j = 0 for
        // all gamma_m = alpha_0 + \sum _{s=1}^{m}(exp\alpha_s)
        for (int m = 1; m < (K - 1); m++) {
          { // i =0, j > 0
            for (int j = 1; j <= m; j++) {
              H(j, j) += dalpha(m) * expAlpha(j);
            }
          }
        }
      }
      H(0, 0) -= reg;
      for (int m = 1; m < (K - 1); m++) {
        H(m, m) -= reg;
      }
      H.array() *= -1;
      if (H.hasNaN()) {
        fail_dump();
        throw std::runtime_error(print_to_string(
            __FILE__, ":", __LINE__, " H has NaN, alpha = ", alpha));
      }
    }
    if (fix_gamma0) {
      dalpha(0) = 0;
      if (HessianTarget != nullptr) {
        (*HessianTarget).row(0).array() = 0;
        (*HessianTarget).col(0).array() = 0;
        (*HessianTarget)(0, 0) = 1;
      }
    }
    dalpha = -dGammadAlpha * dalpha;
    if (dalpha.hasNaN()) {
      fail_dump();
      throw std::runtime_error(print_to_string(
          __FILE__, ":", __LINE__, " dalpha has NaN, alpha = ", alpha));
    }

    dalpha(0) += reg * alpha(0);
    ll -= 0.5 * reg * alpha(0) * alpha(0);
    for (int m = 1; m < (K - 1); m++) {
      dalpha(m) += reg * alpha(m);
      ll -= 0.5 * reg * alpha(m) * alpha(m);
    }
    return -ll;
  }

  template <class ostype> inline void show_info(ostype &os) {
    os << "{\"xs\": [";
    bool first = true;
    for (auto i : indices_) {
      if (!first)
        os << ", ";
      os << x_[i];
      first = false;
    }
    os << "], \"ys\":[";
    first = true;
    for (auto i : indices_) {
      if (!first)
        os << ", ";
      os << y_[i];
      first = false;
    }
    os << "]}";
  }

  inline void fail_dump() {
    std::ofstream fail_log("fail-log.json");
    show_info(fail_log);
  }

  DenseVector &x_;
  const DenseVector &y_;

  int K;
  const std::vector<size_t> indices_;
  Real tune = 1;
  Real reg;
  Real nu;
  std::mt19937 &rng;
  DenseVector alpha_now;
  DenseVector gamma_now;
  DenseMatrix H;
  static constexpr bool fix_gamma0 = false;
  DenseVector zmins, zmaxs;
  std::vector<size_t> histogram;
  size_t accept_count;
};

} // namespace myFM